Torque control

The following example solves a qp problem expressed at the torque level such that:

\begin{equation}\begin{array}{ccc}\boldsymbol{\tau}^{opt} = & \underset{\boldsymbol{\tau}}{\mathrm{argmin}} & ||\boldsymbol{\dot{v}}(\boldsymbol{\tau}) - \boldsymbol{\dot{v}}^{target} || + \omega || \boldsymbol{\tau} - (\boldsymbol{g} - \boldsymbol{\dot{q}})||^2_{M^{-1}}\\& \textrm{s.t.} & \boldsymbol{\tau^{min}} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau^{max}}. \\ & & \boldsymbol{q}^{min} \leq \boldsymbol{q}(\boldsymbol\tau) \leq \boldsymbol{q}^{max} \\ & & \boldsymbol{\dot{q}}^{min} \leq \boldsymbol{\dot{q}}(\boldsymbol\tau) \leq \boldsymbol{\dot{q}}^{max} \end{array} \end{equation}

.

The robot main tasks consists in following a simple trajectory defined in Cartesian space. The mujoco library is used to simulate the robot behaviour.

Simulation

To run this example run the following command from the build/examples directory:

./torqueQontrol robot_name

where robot_name can be either panda or universal_robots_ur5e

Full code

// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include "mujoco/mujoco_sim.h"
#include "Qontrol/Qontrol.hpp"
#include "trajectory_generation/trajectory_generation.h"
 
#define MUJOCO_PLUGIN_DIR "mujoco_plugin"
 
 
using namespace Qontrol;
 
class MujocoQontrol : public MujocoSim 
{
public:
//------------------------------------------- simulation -------------------------------------------
std::shared_ptr<Model::RobotModel<Model::RobotModelImplType::PINOCCHIO>> model;
std::shared_ptr<JointTorqueProblem> torque_problem;
std::shared_ptr<Task::CartesianAcceleration<ControlOutput::JointTorque>> main_task;
std::shared_ptr<Task::JointTorque<ControlOutput::JointTorque>> regularisation_task;
pinocchio::SE3 init_pose;
 
bool init = true;
Qontrol::RobotState robot_state;
TrajectoryGeneration* traj;
std::string resource_path;
 
void initController() override
{
  model =
      Model::RobotModel<Model::RobotModelImplType::PINOCCHIO>::loadModelFromFile(resource_path + "robot.urdf");
  
  torque_problem = std::make_shared<Qontrol::JointTorqueProblem>(model);  
  main_task = torque_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); 
  regularisation_task = torque_problem->task_set->add<Task::JointTorque>("RegularisationTask",1e-5); 
 
  auto joint_configuration_constraint = torque_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
  auto joint_velocity_constraint = torque_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");
  auto joint_torque_constraint = torque_problem->constraint_set->add<Constraint::JointTorque>("JointTorqueConstraint");
 
  mju_copy(d->qpos, m->key_qpos, m->nu  );
  robot_state.joint_position.resize(model->getNrOfDegreesOfFreedom());
  robot_state.joint_velocity.resize(model->getNrOfDegreesOfFreedom());
  
  for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
  {
    robot_state.joint_position[i] = d->qpos[i];
    robot_state.joint_velocity[i] = d->qvel[i];
  }
  model->setRobotState(robot_state);
 
  traj = new TrajectoryGeneration(resource_path+"trajectory.csv", m->opt.timestep);
}
 
void updateController() override
{
  for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
  {
    robot_state.joint_position[i] = d->qpos[i];
    robot_state.joint_velocity[i] = d->qvel[i];
  }
  model->setRobotState(robot_state);
 
  traj->update();
  pinocchio::SE3 traj_pose(traj->pose.matrix());
  
  pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());
  const pinocchio::SE3 tipMdes = current_pose.actInv(traj_pose);
  auto err = pinocchio::log6(tipMdes).toVector();
 
  Eigen::Matrix<double, 6, 1> p_gains;
  p_gains << 1000, 1000, 1000, 1000, 1000, 1000;
 
  Eigen::Matrix<double, 6, 1> d_gains = 2.0 * p_gains.cwiseSqrt();
  Eigen::Matrix<double, 6, 1> xdd_star =
      p_gains.cwiseProduct(err) +
      d_gains.cwiseProduct(traj->velocity - model->getFrameVelocity(model->getTipFrameName())) + traj->acceleration;
 
  main_task->setTargetAcceleration(xdd_star);
  regularisation_task->setTargetTorque(model->getJointGravityTorques() -
                                      robot_state.joint_velocity);
  regularisation_task->setWeightingMatrix(
      model->getInverseJointInertiaMatrix());
  torque_problem->update(m->opt.timestep);
 
  if (torque_problem->solutionFound())
  {
    sendJointTorque(torque_problem->getJointTorqueCommand());
  }
}
};
 
int main(int argc, const char** argv) {
  MujocoQontrol mujoco_qontrol;
  Qontrol::Log::Logger::parseArgv(argc, argv);
 
  mjvScene scn;
  mjv_defaultScene(&scn);
 
  mjvCamera cam;
  mjv_defaultCamera(&cam);
 
  mjvOption opt;
  mjv_defaultOption(&opt);
 
  mjvPerturb pert;
  mjv_defaultPerturb(&pert);
 
  // simulate object encapsulates the UI
  auto sim = std::make_unique<mj::Simulate>(
      std::make_unique<mj::GlfwAdapter>(),
      &scn, &cam, &opt, &pert, /* fully_managed = */ true
  );
 
 
  std::string robot = argv[1];
  std::string mujoco_model = "./resources/"+robot+"/scene.xml";
  mujoco_qontrol.resource_path = "./resources/"+robot+"/";
 
  // start physics thread
  std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_model);
 
  // start simulation UI loop (blocking call)
  sim->RenderLoop();
  physicsthreadhandle.join();
 
  return 0;
}

Explanation of the code

Declaration

First we declare all the objects that will be used to define our problem.

std::shared_ptr<Model::RobotModel<Model::RobotModelImplType::PINOCCHIO>> model;

We use pinocchio for our model library.

std::shared_ptr<JointTorqueProblem> torque_problem;

The output of our qp controller is at the torque level.

std::shared_ptr<Task::CartesianAcceleration<ControlOutput::JointTorque>> main_task;

We then declare two tasks that will be updated every milliseconds.

The main task is expressed as a Cartesian Acceleration task.

std::shared_ptr<Task::JointTorque<ControlOutput::JointTorque>> regularisation_task;

And we add a regularisation task (also at the torque level).

Initialization

void initController() override
{
  model =
      Model::RobotModel<Model::RobotModelImplType::PINOCCHIO>::loadModelFromFile(resource_path + "robot.urdf");

During initialization we instantiate the model with the robot urdf.

We initialize the problem by giving it the model. By default, the qpmad library is used.

  
  torque_problem = std::make_shared<Qontrol::JointTorqueProblem>(model);  
  main_task = torque_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); 
  regularisation_task = torque_problem->task_set->add<Task::JointTorque>("RegularisationTask",1e-5); 

We then fill the task set of torque_problem with the main task and the regularisation task. Each tasks is given a name and a relative weight \( \omega \). This weight can be modified at any time.

 
  auto joint_configuration_constraint = torque_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
  auto joint_velocity_constraint = torque_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");
  auto joint_torque_constraint = torque_problem->constraint_set->add<Constraint::JointTorque>("JointTorqueConstraint");

We then fill the constraint set of torque_problem with the three pre-implemented constraints. Each constraint is given a name. These constraints will automatically be updated during the update of Qontrol.

 
  mju_copy(d->qpos, m->key_qpos, m->nu  );
  robot_state.joint_position.resize(model->getNrOfDegreesOfFreedom());
  robot_state.joint_velocity.resize(model->getNrOfDegreesOfFreedom());
  
  for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
  {
    robot_state.joint_position[i] = d->qpos[i];
    robot_state.joint_velocity[i] = d->qvel[i];
  }
  model->setRobotState(robot_state);

We also create the robot state and fill it with the simulated robot current state.

 
  traj = new TrajectoryGeneration(resource_path+"trajectory.csv", m->opt.timestep);

We create a simple trajectory that has been precalculated and store in a csv file. This trajectory start at the robot current Cartesian pose and does a translation of (-0.1, -0,1, -0.1) m.

Update

void updateController() override
{
  for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
  {
    robot_state.joint_position[i] = d->qpos[i];
    robot_state.joint_velocity[i] = d->qvel[i];
  }
  model->setRobotState(robot_state);
 
  traj->update();

The update function is called every milliseconds. At the beginning of each update we fill the new robot state according to the simulated robot.

We also update the trajectory so that it gives the next Cartesian pose to reach in 1 ms.

  pinocchio::SE3 traj_pose(traj->pose.matrix());
  
  pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());

We then compute the desired Cartesian acceleration using a simple PD controller. Pinocchio is used to compute the error between the desired Cartesian pose and the current Cartesian pose. This is done by the log6 function. The p_gains and d_gains variables are the gains of the PD controller.

  const pinocchio::SE3 tipMdes = current_pose.actInv(traj_pose);
  auto err = pinocchio::log6(tipMdes).toVector();
 
  Eigen::Matrix<double, 6, 1> p_gains;
  p_gains << 1000, 1000, 1000, 1000, 1000, 1000;
 
  Eigen::Matrix<double, 6, 1> d_gains = 2.0 * p_gains.cwiseSqrt();
  Eigen::Matrix<double, 6, 1> xdd_star =
      p_gains.cwiseProduct(err) +
      d_gains.cwiseProduct(traj->velocity - model->getFrameVelocity(model->getTipFrameName())) + traj->acceleration;
 
  main_task->setTargetAcceleration(xdd_star);
  regularisation_task->setTargetTorque(model->getJointGravityTorques() -
                                      robot_state.joint_velocity);
  regularisation_task->setWeightingMatrix(
      model->getInverseJointInertiaMatrix());

The desired Cartesian acceleration is then fed to the main task. The regularisation task is also updated so that it compensate for gravity plus a damping term. The resulting regularisation task would be written : \( || \boldsymbol{\tau} - (\boldsymbol{g} - \boldsymbol{\dot{q}})||^2_{M^{-1}}\)

  torque_problem->update(m->opt.timestep);
 
  if (torque_problem->solutionFound())
  {
    sendJointTorque(torque_problem->getJointTorqueCommand());
  }
}
};

Once we updated the necassary tasks and constraints we can update the whole problem. If a solution to the problem exist we can then get it and send it to the simulated robot.

Main function

 
int main(int argc, const char** argv) {
  MujocoQontrol mujoco_qontrol;
  Qontrol::Log::Logger::parseArgv(argc, argv);
 
  mjvScene scn;
  mjv_defaultScene(&scn);
 
  mjvCamera cam;
  mjv_defaultCamera(&cam);
 
  mjvOption opt;
  mjv_defaultOption(&opt);
 
  mjvPerturb pert;
  mjv_defaultPerturb(&pert);
 
  // simulate object encapsulates the UI
  auto sim = std::make_unique<mj::Simulate>(
      std::make_unique<mj::GlfwAdapter>(),
      &scn, &cam, &opt, &pert, /* fully_managed = */ true
  );
 
 
  std::string robot = argv[1];
  std::string mujoco_model = "./resources/"+robot+"/scene.xml";
  mujoco_qontrol.resource_path = "./resources/"+robot+"/";
 
  // start physics thread
  std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_model);
 
  // start simulation UI loop (blocking call)
  sim->RenderLoop();
  physicsthreadhandle.join();
 
  return 0;

The main function function fetches the robot name given in argv and starts the Mujoco simulation.