Acceleration Control GHC Interactive

Introduction

In this interactive example, the robot end-effector tracks a mocap target while the elbow oscillates along the Y-axis. The Generalized Hierarchical Control (GHC) strategy ensures that the secondary elbow task is projected into the null-space of the main end-effector task, allowing the elbow to move without disturbing the tracking accuracy of the robot tip.

The controller is built from:

• a main Cartesian acceleration task for end-effector tracking,

• a secondary Cartesian acceleration task on panda_link4,

• a regularization task,

• joint configuration and joint velocity constraints.

The example uses the interactive MuJoCo scene, so the target can be moved directly in the viewer.

This resolution strategy is based on the Generalized Hierarchical Control method published in this paper. Most existing techniques to handle strict task priorities in hierarchical control are based on null-space projectors or a sequence of quadratic programs; whereas non strict task priorities are usually handled by optimization based on a weighting strategy. This approach handles both strict and non-strict priorities of an arbitrary number of tasks, and achieves multiple priority rearrangements simultaneously. It relies on a generalized projector, which makes it possible to completely project a task into the null-space of a set of tasks, while partially projecting it into the null-space of some other tasks. Priority transitions are achieved by smooth variations of the generalized projector.

Formulation

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

\[\begin{split}\begin{aligned} X^T &= [\ddot{q}_1^T, \ldots, \ddot{q}_{n_t}^T] \\ X^{opt} &= \underset{X}{\mathrm{argmin}} \sum_{i=1}^{n_t} \left\| J_i S_{q_i} X + \dot{J}_i \dot{q} - a_i^* \right\|^2 + \left\| X \right\|^2 \\ &\textrm{s.t.} \\ &\quad 2 \left( \frac{q_{min} - q_k}{\Delta T^2} - \frac{\dot{q}_k}{\Delta T} \right) \leq PX \leq 2 \left( \frac{q_{max} - q_k}{\Delta T^2} - \frac{\dot{q}_k}{\Delta T} \right)\\ &\quad \frac{\dot{q}_{min} - \dot{q}_k}{\Delta T} \leq PX \leq \frac{\dot{q}_{max} - \dot{q}_k}{\Delta T} \\ P &= [P_1, \ldots, P_{n_t}], \quad \ddot{q} = PX = \sum_i P_i \ddot{q}_i \\ \end{aligned}\end{split}\]

Where P is the concatenation of the generalized projectors of each task.

For the i-th task we compute its projector \(P_i\) using its corresponding \(\alpha_i\) input: \(P_i(\alpha_i) = I_n − B_i(J_{s_i})^T \alpha_{i,r}^s(\alpha_i ,origin) B_i (J_{s_i} )\)

Where \(J_{s_i}\) is a matrix made of the rows of J sorted in descending order with respect to the values of the diagonal elements in \(\alpha_i\) .

The values in \(\alpha_i\) are sorted accordingly, leading to \(\alpha_{i}^s\).

\(B_i(J_{s_i})\) is a \(r\) x \(n\) matrix , (where \(r = rank(J_{s_i} )\)), such that the rows of \(B_i(J_{s_i})\) form an orthonormal basis of the joint space.

Finally, \(\alpha_{i,r}^s\) is a diagonal matrix of degree r. The vector \(origin\) is a vector of the row indexes of \(J_{s_i}\) selected during the construction of the orthonormal basis \(B_i\)

The robot main tasks consists in keeping the end effector at a defined pose in Cartesian space. A second task is added to control the position on the y axis of the elbow of the robot. The mujoco library is used to simulate the robot behaviour.

C++

Full Code

You can find the source code of this example here.

// 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"

using namespace Qontrol;

class MujocoQontrol : public MujocoSim 
{
public:
//------------------------------------------- simulation -------------------------------------------
std::shared_ptr<Model::RobotModel> model;
std::shared_ptr<JointAccelerationProblem> acceleration_problem;
std::shared_ptr<Task::CartesianAcceleration<ControlOutput::JointAcceleration>> main_task;
std::shared_ptr<Task::CartesianAcceleration<ControlOutput::JointAcceleration>> elbow_task;
std::shared_ptr<Qontrol::Task::GenericTask> regularization_task;

Qontrol::RobotState robot_state;
std::string resource_path;
bool initialized = false;
double t = 0;

void initController() override
{
  model =
      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
  
  acceleration_problem = std::make_shared<Qontrol::JointAccelerationProblem>(model,ResolutionStrategy::Generalized);
  //tasks  (weights are useless)
  main_task = acceleration_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); 
  elbow_task = acceleration_problem->task_set->add<Task::CartesianAcceleration>("CommunicationTask");
  Eigen::MatrixXd selection_matrix = Eigen::MatrixXd::Zero(6,6);
  selection_matrix(1,1) = 1.0; //y axis control only
  elbow_task->setControledFrameName("panda_link4");
  elbow_task->setReferenceFrame(ReferenceFrame::LOCAL_WORLD_ALIGNED);
  elbow_task->setSelectionMatrix(selection_matrix);
  regularization_task = acceleration_problem->task_set->add("regularizationTask",14,14,1.0); //regularisation dimension = n_dof*n_tasks (other than regularisation)
  //constraints
  auto joint_configuration_constraint = acceleration_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
  auto joint_velocity_constraint = acceleration_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");

  joint_configuration_constraint->setHorizon(30);
  joint_velocity_constraint->setHorizon(30);

  //init() -> resizes properly some variables / to be called once the tasks and constraints are defined
  acceleration_problem->init();
  std::cout << "Model loaded with " << model->getNrOfDegreesOfFreedom() << " DoF" << std::endl;

  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);
  initialiseMocapPose(model->getFramePose(model->getTipFrameName()).matrix());
  
  initialized = true;
}

void updateController() override
{
  
  while (!initialized)
  {
    // init_pose = model->getFramePose(model->getTipFrameName());
    std::cout << "Waiting for initialization" << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  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);

  Eigen::Affine3d target_pose;
  target_pose.translation() = Eigen::Vector3d(d->mocap_pos[0],d->mocap_pos[1],d->mocap_pos[2]);
  target_pose.linear() = Eigen::Quaterniond(d->mocap_quat[0],d->mocap_quat[1],d->mocap_quat[2],d->mocap_quat[3]).toRotationMatrix();
  pinocchio::SE3 traj_pose(target_pose.matrix());

  //get desired cartesian acceleration for main task
  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(model->getFrameVelocity(model->getTipFrameName())) ;
  
  double y_des_elbow = 0.15;
  t+=m->opt.timestep;
  if(sin(0.5*t)<0){
    y_des_elbow = -0.15;
  }
  double current_y_elbow_pose = model->getFramePose("panda_link4").matrix()(1,3);
  double error_elbow_y =  y_des_elbow-current_y_elbow_pose;
  Eigen::VectorXd xdd_star_elbow = Eigen::VectorXd::Zero(6);
  double k_elbow = 125.0;
  xdd_star_elbow[1]= k_elbow * error_elbow_y -2.0*std::sqrt(k_elbow)*model->getFrameVelocity("panda_link4").matrix()(1,3);  

  Eigen::VectorXd alpha_1 = Eigen::VectorXd::Zero(7); //here absolute priority to main task
  Eigen::VectorXd alpha_2 = Eigen::VectorXd::Ones(7); //activation of the elbow task but should not impac tthe main task
  alpha_2[6] = 0.0;
  std::vector<Eigen::VectorXd> alphas;
  alphas.push_back(alpha_1);
  alphas.push_back(alpha_2);

  //update the projectors of GHC
  acceleration_problem->updateProjector(alphas);
  
  //TASKS UPDATE
  main_task->setTargetAcceleration(xdd_star);
  elbow_task->setTargetAcceleration(xdd_star_elbow);

  regularization_task->setE(0.00001*Eigen::MatrixXd::Identity(14,14)); //here 0.00001 is the weight of regularization task
  regularization_task->setf(Eigen::VectorXd::Zero(14));

  //update joint acceleration problem
  acceleration_problem->update(m->opt.timestep);

  if (acceleration_problem->solutionFound())
  {
    sendJointVelocity(acceleration_problem->getJointVelocityCommand());
  }
} 
};


int main(int argc, const char** argv) {
  MujocoQontrol mujoco_qontrol;
  Qontrol::Log::Logger::parseArgv(argc, argv);

  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>(),
      &cam, &opt, &pert, /* is_passive = */ false
  );

  std::string robot = argv[1];
  std::string mujoco_scene = "./resources/"+robot+"/scene_interactive.xml";
  mujoco_qontrol.resource_path = "./resources/"+robot+"/";

  // start physics thread
  std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_scene.c_str());

  // start simulation UI loop (blocking call)
  sim->RenderLoop();
  physicsthreadhandle.join();

  return 0;
}

Explanation of the code

Initialization

  model =
      Model::RobotModel::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.

  acceleration_problem = std::make_shared<Qontrol::JointAccelerationProblem>(model,ResolutionStrategy::Generalized);
  //tasks  (weights are useless)
  main_task = acceleration_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); 
  elbow_task = acceleration_problem->task_set->add<Task::CartesianAcceleration>("CommunicationTask");
  Eigen::MatrixXd selection_matrix = Eigen::MatrixXd::Zero(6,6);
  selection_matrix(1,1) = 1.0; //y axis control only
  elbow_task->setControledFrameName("panda_link4");
  elbow_task->setReferenceFrame(ReferenceFrame::LOCAL_WORLD_ALIGNED);
  elbow_task->setSelectionMatrix(selection_matrix);
  regularization_task = acceleration_problem->task_set->add("regularizationTask",14,14,1.0); //regularisation dimension = n_dof*n_tasks (other than regularisation)

We then fill the task set of acceleration_problem with the main task, the elbow task and the regularisation task. We define a controlled frame for each task. By default it is the end effector of the robot. Additionnaly, it is possible to define the reference frame considered (local,world, local_world_aligned). The default value is LOCAL. Finally, if the cartesian task dimension is lower than 6, a selection matrix can be defined. Each tasks is given a name and a relative weight \(\omega\). This weight is not considered when we use the Generalized approach.

  auto joint_configuration_constraint = acceleration_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
  auto joint_velocity_constraint = acceleration_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");

We then define the constraint set of torque_problem. Constraints of the robot are already pre-implemented and can be added as shown in this example.

  acceleration_problem->init();

Here we define that we are not using the default Weighting resolution strategy. It MUST be called after the tasks and constraints definition.

The problem is created in generalized mode:

  acceleration_problem = std::make_shared<Qontrol::JointAccelerationProblem>(model,ResolutionStrategy::Generalized);

The secondary elbow task is configured to control only the Y axis:

  Eigen::MatrixXd selection_matrix = Eigen::MatrixXd::Zero(6,6);
  selection_matrix(1,1) = 1.0; //y axis control only
  elbow_task->setControledFrameName("panda_link4");
  elbow_task->setReferenceFrame(ReferenceFrame::LOCAL_WORLD_ALIGNED);
  elbow_task->setSelectionMatrix(selection_matrix);

The mocap target is read from the interactive MuJoCo scene and converted to a Cartesian target pose:

  Eigen::Affine3d target_pose;
  target_pose.translation() = Eigen::Vector3d(d->mocap_pos[0],d->mocap_pos[1],d->mocap_pos[2]);
  target_pose.linear() = Eigen::Quaterniond(d->mocap_quat[0],d->mocap_quat[1],d->mocap_quat[2],d->mocap_quat[3]).toRotationMatrix();
  pinocchio::SE3 traj_pose(target_pose.matrix());

The GHC projector is updated before solving the acceleration problem:

  Eigen::VectorXd alpha_1 = Eigen::VectorXd::Zero(7); //here absolute priority to main task
  Eigen::VectorXd alpha_2 = Eigen::VectorXd::Ones(7); //activation of the elbow task but should not impac tthe main task
  alpha_2[6] = 0.0;
  std::vector<Eigen::VectorXd> alphas;
  alphas.push_back(alpha_1);
  alphas.push_back(alpha_2);

  //update the projectors of GHC
  acceleration_problem->updateProjector(alphas);

The executable loads the interactive scene:

  std::string mujoco_scene = "./resources/"+robot+"/scene_interactive.xml";

Python

A dedicated Python version of this GHC interactive acceleration example is not available yet in bindings/python/examples. For now, the closest interactive Python example is Acceleration Qontrol Interactive, which documents the non-GHC acceleration controller workflow.