Torque 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.

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} \\ &\quad \tau_{min} - n \leq BPX \leq \tau_{max} - n \\ P &= [P_1, \ldots, P_{n_t}], \quad \ddot{q} = PX = \sum_i P_i \ddot{q}_i \\ \tau &= BPX^{opt} + n \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"
#include "trajectory_generation/trajectory_generation.h"

using namespace Qontrol;

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

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

void initController() override
{
  model =
      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
  
  torque_problem = std::make_shared<Qontrol::JointTorqueProblem>(model,ResolutionStrategy::Generalized);
  //tasks  (weights are useless)
  main_task = torque_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); 
  elbow_task = torque_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 = torque_problem->task_set->add("regularizationTask",14,14,1.0); //regularisation dimension = n_dof*n_tasks (other than regularisation)
  //constraints
  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");

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

  traj = new TrajectoryGeneration(resource_path+"trajectory.csv", m->opt.timestep);
  
  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);

  traj->update();
  pinocchio::SE3 traj_pose(traj->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);
  xdd_star_elbow[1]= 500.0 * error_elbow_y -2*std::sqrt(500.0)*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
  torque_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 torque problem
  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);

  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.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

Declaration

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

std::shared_ptr<Model::RobotModel> 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 tree tasks that will be updated every milliseconds.

The main task is expressed as a Cartesian Acceleration task.

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

We add the secondary task to control the elbow along y axis.

std::shared_ptr<Qontrol::Task::GenericTask> regularization_task;

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

Initialization

void initController() override
{
  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.

  torque_problem = std::make_shared<Qontrol::JointTorqueProblem>(model,ResolutionStrategy::Generalized);
  //tasks  (weights are useless)
  main_task = torque_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); 
  elbow_task = torque_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 = torque_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 torque_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 = 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 define the constraint set of torque_problem. Constraints of the robot are already pre-implemented and can be added as shown in this example.

  torque_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.

Update

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

  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 define the Cartesian goal for the end effector.

  pinocchio::SE3 traj_pose(traj->pose.matrix());

  //get desired cartesian acceleration for main task
  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(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);
  xdd_star_elbow[1]= 500.0 * error_elbow_y -2*std::sqrt(500.0)*model->getFrameVelocity("panda_link4").matrix()(1,3);

Similarly to the previous desired Cartesian acceleration computation, we compute the desired acceleration of the elbow along the y axis. Here the reference is either 0.15m, either -0.15m depending of the simulation time.

  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
  torque_problem->updateProjector(alphas);

Generalized Hierarchical Control is based on the projection P of the tasks on the other tasks’ nullspace. In order to compute the overall projection P, we need to create a vector of \(\alpha_i\) matrices defined such that:

\(\alpha_i = diag(\alpha_{i1} I_{m_1}, \ldots, \alpha_{ij} I_{m_j}, \ldots, \alpha_{i_{n_t}} I_{m_{n_t}})\)

where \(\alpha_i\) is a diagonal matrix, the main diagonal blocks of which are square matrices: \(\alpha_{ij} I_{m_j}\) . \(I_{m_j}\) is the \(m_j\) x \(m_j\) identity matrix, and \(\alpha_{ij} \in [0, 1]\). By convention, the coefficient \(\alpha_{ij}\) indicates the priority of task j with respect to task i. - \(\alpha_{ij} = 0\) corresponds to the case where task j has strict lower priority with respect to task i. - \(0 < \alpha_{ij} < 1\) corresponds to a soft (non-strict) priority between the two tasks: the greater the value of \(\alpha_{ij}\) , the higher the importance level of task j with respect to task i. - \(\alpha_{ij} = 1\) corresponds to the case where task j has a strict higher priority with respect to task i.

There is a particular case induced by the proposed formulation and corresponding to the influence of task i on itself. Even though not intuitive, this self-influence has to be interpreted in terms of task existence, modulated by \(\alpha_{ii}\) . If \(\alpha_{ii} = 1\) then task i is projected into its own null-space, i.e. it is basically canceled out. Decreasing \(\alpha_{ii}\) continuously to 0 is a simple and elegant way to introduce the task in the set of tasks. Conversely, increasing \(\alpha_{ii}\) continuously from 0 to 1 provides with a proper task deletion procedure.

The updateProjector method update the projector with the alphas. Note that alphas values can be modified over time to smoothly change the hierarchy between the tasks.

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

Finally, we set the E and f for each task, including the regularization task. Unlike the other tasks,the regularization task is weighted. In this example, the weight is added in the E matrix.

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

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

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

Python

Full Code:

You can find the source code of this example here.

#!/usr/bin/env python3
"""
Interactive Torque Control with GHC (Generalized Hierarchical Control)

This example demonstrates:
- Hierarchical task control using GHC projectors
- Multiple tasks with different priorities
- Main task: End-effector Cartesian tracking (highest priority)
- Secondary task: Elbow Y-position oscillation (projected in main task null space)
- Real-time torque-level inverse dynamics
- Interactive mocap target control

Mirrors the C++ example: torqueQontrol_GHC.cpp (adapted for interactive mode)

Usage:
    python torque_control_ghc_interactive.py <robot_name>
    
Example:
    python torque_control_ghc_interactive.py panda
    python torque_control_ghc_interactive.py panda --verbose
    
Controls:
- Use the MuJoCo viewer to move the mocap body (red sphere)
- The robot end-effector will track the mocap target
- The elbow oscillates in Y-direction in the null space of the main task
- Press ESC to exit
"""

import numpy as np
import qontrol
import pinocchio as pin
import argparse
import os
import time
from mujoco_helper import MujocoSimulator, compute_log6_error


class QontrolTorqueGHCController:
    """GHC Torque controller using Qontrol (MuJoCo-agnostic)"""
    
    def __init__(self, robot_name: str, resource_path: str, mujoco_sim: MujocoSimulator):
        """
        Initialize the Qontrol GHC torque controller
        
        Args:
            robot_name: Name of the robot (e.g., 'panda')
            resource_path: Path to robot resources folder
            mujoco_sim: MuJoCo simulator instance
        """
        self.robot_name = robot_name
        self.resource_path = resource_path
        self.sim = mujoco_sim
        
        # =================================================================
        # Qontrol Setup - Robot Model and QP Problem
        # =================================================================
        urdf_path = os.path.join(resource_path, "robot.urdf")
        if not os.path.exists(urdf_path):
            raise FileNotFoundError(f"URDF file not found: {urdf_path}")
        
        print(f"Loading Qontrol model from: {urdf_path}")
        self.qontrol_model = qontrol.RobotModel.load_from_file(urdf_path)
        self.ndof = self.qontrol_model.get_nr_of_degrees_of_freedom()
        print(f"Robot has {self.ndof} degrees of freedom")
        
        # Create QP solver
        self.solver = qontrol.create_qpmad_solver()
        
        # Create torque-level problem with Generalized resolution strategy (for GHC)
        print("Creating GHC torque problem with Generalized resolution strategy")
        self.torque_problem = qontrol.JointTorqueProblem(
            self.qontrol_model, 
            self.solver,
            qontrol.ResolutionStrategy.Generalized
        )
        
        # =================================================================
        # Task Setup with Hierarchical Control
        # =================================================================
        
        # Task 1: Main task - Cartesian acceleration for end-effector tracking
        # Weight is not used with GHC (priorities defined by alpha matrices)
        self.main_task = self.torque_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)
        
        # Task 2: Elbow task - Control elbow position in Y direction only
        self.elbow_task = self.torque_problem.task_set.add_cartesian_acceleration("ElbowTask", 1.0)
        
        # Configure elbow task to control only Y axis in LOCAL_WORLD_ALIGNED frame
        selection_matrix = np.zeros((6, 6))
        selection_matrix[1, 1] = 1.0  # Y axis control only
        self.elbow_task.set_selection_matrix(selection_matrix)
        self.elbow_task.set_controlled_frame_name("panda_link4")  # Elbow link
        self.elbow_task.set_reference_frame(qontrol.ReferenceFrame.LOCAL_WORLD_ALIGNED)
        print("Elbow task configured: panda_link4, Y-axis control only")
        
        # Task 3: Regularization task - For GHC mode, dimension = ndof * n_tasks (excluding regularization)
        # n_tasks = 2 (main + elbow), so regularization_dim = 7 * 2 = 14
        regularization_dim = self.ndof * 2
        self.regularization_task = self.torque_problem.task_set.add(
            "RegularizationTask", 
            regularization_dim, 
            regularization_dim,
            1.0  # Weight set during update
        )
        print(f"Regularization task dimension: {regularization_dim}")
        
        # =================================================================
        # Constraint Setup
        # =================================================================
        self.joint_config_constraint = self.torque_problem.constraint_set.add_joint_configuration("JointConfigurationConstraint")
        self.joint_vel_constraint = self.torque_problem.constraint_set.add_joint_velocity("JointVelocityConstraint")
        self.joint_torque_constraint = self.torque_problem.constraint_set.add_joint_torque("JointTorqueConstraint")
        
        # Set prediction horizons for constraints (like C++ example)
        self.joint_config_constraint.set_horizon(30)
        self.joint_vel_constraint.set_horizon(30)
        print("Constraint horizons set to 30")
        
        # Initialize problem (resizes tasks/constraints for GHC mode)
        print("Initializing GHC problem...")
        self.torque_problem.init()
        print("GHC problem initialized successfully")
        
        # =================================================================
        # Robot State
        # =================================================================
        self.robot_state = qontrol.RobotState()
        self.robot_state.resize(self.ndof)
        
        # Get initial joint state and update model
        qpos, qvel = self.sim.get_joint_state()
        self.robot_state.joint_position = qpos
        self.robot_state.joint_velocity = qvel
        self.qontrol_model.set_robot_state(self.robot_state)
        
        # Get tip frame name
        self.tip_frame = self.qontrol_model.get_tip_frame_name()
        print(f"Controlling frame: {self.tip_frame}")
        
        # Initialize mocap to current end-effector pose
        self.sim.init_mocap_to_frame(self.tip_frame)
        
        print("dt:", self.sim.dt)
       
        # =================================================================
        # Control Parameters
        # =================================================================
        # PD gains for main Cartesian task
        self.main_p_gains = np.array([1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0])
        self.main_d_gains = 2.0 * np.sqrt(self.main_p_gains)  # Critical damping
        
        # PD gains for elbow task (oscillation)
        self.elbow_p_gain = 1000.0
        self.elbow_d_gain = 2.0 * np.sqrt(self.elbow_p_gain)
        
        # Time counter for elbow oscillation
        self.t = 0.0
    
    def update(self):
        """Main control update: compute and apply joint torques"""
        # Update time
        self.t += self.sim.dt
        
        # 1. Update Qontrol model with current state from simulation
        qpos, qvel = self.sim.get_joint_state()
        self.robot_state.joint_position = qpos
        self.robot_state.joint_velocity = qvel
        self.qontrol_model.set_robot_state(self.robot_state)
        
        # 2. Get target and current poses for main task
        target_se3 = self.sim.get_mocap_pose_se3()
        current_ee_matrix = self.qontrol_model.get_frame_pose(self.tip_frame)
        current_se3 = pin.SE3(current_ee_matrix[:3, :3], current_ee_matrix[:3, 3])
        
        # 3. Compute SE(3) log6 error for main task
        cartesian_error = compute_log6_error(target_se3, current_se3)
        error_norm = np.linalg.norm(cartesian_error)
        
        # 4. Get current end-effector velocity
        ee_velocity = self.qontrol_model.get_frame_velocity(self.tip_frame)
        
        # 5. Compute desired Cartesian acceleration for main task (PD control)
        xdd_star = self.main_p_gains * cartesian_error - self.main_d_gains * ee_velocity
        
        # Set main task target
        self.main_task.set_target_acceleration(xdd_star)
        
        # 6. Compute elbow task (Y-axis oscillation)
        # Desired Y position oscillates: y_des = 0.15 * sign(sin(0.5*t))
        if np.sin(0.5 * self.t) < 0:
            y_des_elbow = -0.15
        else:
            y_des_elbow = 0.15
        
        # Get current elbow pose and velocity
        elbow_pose = self.qontrol_model.get_frame_pose("panda_link4")
        current_y_elbow = elbow_pose[1, 3]  # Y position
        
        elbow_velocity = self.qontrol_model.get_frame_velocity("panda_link4")
        
        # Compute error
        error_elbow_y = y_des_elbow - current_y_elbow
        
        # Compute desired acceleration (PD control, only Y axis)
        xdd_star_elbow = np.zeros(6)
        xdd_star_elbow[1] = (self.elbow_p_gain * error_elbow_y - 
                             self.elbow_d_gain * elbow_velocity[1])
        
        # Set elbow task target
        self.elbow_task.set_target_acceleration(xdd_star_elbow)
        
        # 7. Update GHC projectors with alpha matrices
        # Alpha for main task: absolute priority (all zeros)
        alpha_1 = np.zeros(self.ndof)
        
        # Alpha for elbow task: activated but should not impact main task
        alpha_2 = np.ones(self.ndof)
        alpha_2[6] = 0.0  # Last joint (gripper) not involved
        
        alphas = [alpha_1, alpha_2]
        self.torque_problem.update_projector(alphas)
        
        # 8. Set regularization task
        # Weight of regularization is 0.00001 (very small)
        reg_weight = 0.00001
        self.regularization_task.set_E(reg_weight * np.eye(self.ndof * 2))
        self.regularization_task.set_f(np.zeros(self.ndof * 2))
        
        # 9. Solve QP for joint torques
        t_qp_start = time.perf_counter()
        self.torque_problem.update(self.sim.dt)
        t_qp = time.perf_counter() - t_qp_start
        
        # 10. Apply torques to simulation
        if self.torque_problem.solution_found():
            joint_torques = self.torque_problem.get_joint_torque_command()
            self.sim.apply_torques(joint_torques)
        else:
            print("Warning: No QP solution found!")
            self.sim.apply_torques(np.zeros(self.ndof))


def main():
    """Main entry point"""
    parser = argparse.ArgumentParser(
        description="Interactive GHC torque control with MuJoCo and Qontrol",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Examples:
  %(prog)s panda
  %(prog)s ur5
        """
    )
    parser.add_argument("robot", type=str, help="Robot name (e.g., panda)")
    
    args = parser.parse_args()
    
    # Get resource path
    script_dir = os.path.dirname(os.path.abspath(__file__))
    resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
    
    if not os.path.exists(resource_path):
        print(f"Error: Resource path not found: {resource_path}")
        return 1
    
    # MuJoCo scene path
    scene_path = os.path.join(resource_path, "scene_interactive.xml")
    if not os.path.exists(scene_path):
        print(f"Error: Scene file not found: {scene_path}")
        return 1
    
    print(f"Loading MuJoCo scene from: {scene_path}")
    
    # Initialize MuJoCo simulator
    sim = MujocoSimulator(args.robot, resource_path)
    
    # Initialize controller
    controller = QontrolTorqueGHCController(args.robot, resource_path, sim)
    
    print("\n" + "="*60)
    print("Starting GHC torque control simulation")
    print("Main task: End-effector tracking (highest priority)")
    print("Secondary task: Elbow Y-oscillation (null space)")
    print("Move the red mocap target to control the robot")
    print("Press ESC to exit")
    print("="*60 + "\n")
    
    try:
        # Run simulation with controller
        sim.run_interactive(controller.update)
    except KeyboardInterrupt:
        print("\nInterrupted by user")
    except Exception as e:
        print(f"\nError during simulation: {e}")
        import traceback
        traceback.print_exc()
        return 1
    
    return 0


if __name__ == "__main__":
    exit(main())

—

Explanation of the code

Note: The following explanation refers to the logic implemented in C++, but the Python API directly mirrors this structure.

Declaration

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

We use pinocchio for our model library.

        self.qontrol_model = qontrol.RobotModel.load_from_file(urdf_path)
        self.ndof = self.qontrol_model.get_nr_of_degrees_of_freedom()

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

        self.torque_problem = qontrol.JointTorqueProblem(
            self.qontrol_model, 
            self.solver,
            qontrol.ResolutionStrategy.Generalized

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

The main task is expressed as a Cartesian Acceleration task.

        self.main_task = self.torque_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)

We add the secondary task to control the elbow along y axis.

        self.elbow_task = self.torque_problem.task_set.add_cartesian_acceleration("ElbowTask", 1.0)

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

        self.regularization_task = self.torque_problem.task_set.add(
            "RegularizationTask", 
            regularization_dim, 
            regularization_dim,
            1.0  # Weight set during update

Initialization

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.

We then fill the task set of torque_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.

        selection_matrix = np.zeros((6, 6))
        selection_matrix[1, 1] = 1.0  # Y axis control only
        self.elbow_task.set_selection_matrix(selection_matrix)
        self.elbow_task.set_controlled_frame_name("panda_link4")  # Elbow link
        self.elbow_task.set_reference_frame(qontrol.ReferenceFrame.LOCAL_WORLD_ALIGNED)

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.

        self.joint_config_constraint = self.torque_problem.constraint_set.add_joint_configuration("JointConfigurationConstraint")
        self.joint_vel_constraint = self.torque_problem.constraint_set.add_joint_velocity("JointVelocityConstraint")
        self.joint_torque_constraint = self.torque_problem.constraint_set.add_joint_torque("JointTorqueConstraint")
        
        # Set prediction horizons for constraints (like C++ example)
        self.joint_config_constraint.set_horizon(30)
        self.joint_vel_constraint.set_horizon(30)

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

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.

    def update(self):
        """Main control update: compute and apply joint torques"""
        # Update time
        self.t += self.sim.dt
        
        # 1. Update Qontrol model with current state from simulation
        qpos, qvel = self.sim.get_joint_state()
        self.robot_state.joint_position = qpos
        self.robot_state.joint_velocity = qvel
        self.qontrol_model.set_robot_state(self.robot_state)

We also define the Cartesian goal for the end effector.

        target_se3 = self.sim.get_mocap_pose_se3()
        current_ee_matrix = self.qontrol_model.get_frame_pose(self.tip_frame)
        current_se3 = pin.SE3(current_ee_matrix[:3, :3], current_ee_matrix[:3, 3])

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.

        cartesian_error = compute_log6_error(target_se3, current_se3)
        error_norm = np.linalg.norm(cartesian_error)
        
        # 4. Get current end-effector velocity
        ee_velocity = self.qontrol_model.get_frame_velocity(self.tip_frame)
        
        # 5. Compute desired Cartesian acceleration for main task (PD control)
        xdd_star = self.main_p_gains * cartesian_error - self.main_d_gains * ee_velocity
        
        # Set main task target
        self.main_task.set_target_acceleration(xdd_star)

Similarly to the previous desired Cartesian acceleration computation, we compute the desired acceleration of the elbow along the y axis. Here the reference is either 0.15m, either -0.15m depending of the simulation time.

        if np.sin(0.5 * self.t) < 0:
            y_des_elbow = -0.15
        else:
            y_des_elbow = 0.15
        
        # Get current elbow pose and velocity
        elbow_pose = self.qontrol_model.get_frame_pose("panda_link4")
        current_y_elbow = elbow_pose[1, 3]  # Y position
        
        elbow_velocity = self.qontrol_model.get_frame_velocity("panda_link4")
        
        # Compute error
        error_elbow_y = y_des_elbow - current_y_elbow
        
        # Compute desired acceleration (PD control, only Y axis)
        xdd_star_elbow = np.zeros(6)
        xdd_star_elbow[1] = (self.elbow_p_gain * error_elbow_y - 
                             self.elbow_d_gain * elbow_velocity[1])
        
        # Set elbow task target
        self.elbow_task.set_target_acceleration(xdd_star_elbow)

Generalized Hierarchical Control is based on the projection P of the tasks on the other tasks’ nullspace. In order to compute the overall projection P, we need to create a vector of \(\alpha_i\) matrices defined such that:

\(\alpha_i = diag(\alpha_{i1} I_{m_1}, \ldots, \alpha_{ij} I_{m_j}, \ldots, \alpha_{i_{n_t}} I_{m_{n_t}})\)

where \(\alpha_i\) is a diagonal matrix, the main diagonal blocks of which are square matrices: \(\alpha_{ij} I_{m_j}\) . \(I_{m_j}\) is the \(m_j\) x \(m_j\) identity matrix, and \(\alpha_{ij} \in [0, 1]\). By convention, the coefficient \(\alpha_{ij}\) indicates the priority of task j with respect to task i. - \(\alpha_{ij} = 0\) corresponds to the case where task j has strict lower priority with respect to task i. - \(0 < \alpha_{ij} < 1\) corresponds to a soft (non-strict) priority between the two tasks: the greater the value of \(\alpha_{ij}\) , the higher the importance level of task j with respect to task i. - \(\alpha_{ij} = 1\) corresponds to the case where task j has a strict higher priority with respect to task i.

There is a particular case induced by the proposed formulation and corresponding to the influence of task i on itself. Even though not intuitive, this self-influence has to be interpreted in terms of task existence, modulated by \(\alpha_{ii}\) . If \(\alpha_{ii} = 1\) then task i is projected into its own null-space, i.e. it is basically canceled out. Decreasing \(\alpha_{ii}\) continuously to 0 is a simple and elegant way to introduce the task in the set of tasks.

        alpha_1 = np.zeros(self.ndof)
        
        # Alpha for elbow task: activated but should not impact main task
        alpha_2 = np.ones(self.ndof)
        alpha_2[6] = 0.0  # Last joint (gripper) not involved
        
        alphas = [alpha_1, alpha_2]
        self.torque_problem.update_projector(alphas)

Finally, we set the E and f for each task, including the regularization task. Unlike the other tasks,the regularization task is weighted. In this example, the weight is added in the E matrix.

        reg_weight = 0.00001
        self.regularization_task.set_E(reg_weight * np.eye(self.ndof * 2))
        self.regularization_task.set_f(np.zeros(self.ndof * 2))

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.

        self.torque_problem.update(self.sim.dt)
        t_qp = time.perf_counter() - t_qp_start
        
        # 10. Apply torques to simulation
        if self.torque_problem.solution_found():
            joint_torques = self.torque_problem.get_joint_torque_command()
            self.sim.apply_torques(joint_torques)
        else:
            print("Warning: No QP solution found!")
            self.sim.apply_torques(np.zeros(self.ndof))

Main function

def main():
    """Main entry point"""
    parser = argparse.ArgumentParser(
        description="Interactive GHC torque control with MuJoCo and Qontrol",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Examples:
  %(prog)s panda
  %(prog)s ur5
        """
    )
    parser.add_argument("robot", type=str, help="Robot name (e.g., panda)")
    
    args = parser.parse_args()
    
    # Get resource path
    script_dir = os.path.dirname(os.path.abspath(__file__))
    resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
    
    if not os.path.exists(resource_path):
        print(f"Error: Resource path not found: {resource_path}")
        return 1
    
    # MuJoCo scene path
    scene_path = os.path.join(resource_path, "scene_interactive.xml")
    if not os.path.exists(scene_path):
        print(f"Error: Scene file not found: {scene_path}")
        return 1
    
    print(f"Loading MuJoCo scene from: {scene_path}")
    
    # Initialize MuJoCo simulator
    sim = MujocoSimulator(args.robot, resource_path)
    
    # Initialize controller
    controller = QontrolTorqueGHCController(args.robot, resource_path, sim)
    
    print("\n" + "="*60)
    print("Starting GHC torque control simulation")
    print("Main task: End-effector tracking (highest priority)")
    print("Secondary task: Elbow Y-oscillation (null space)")
    print("Move the red mocap target to control the robot")
    print("Press ESC to exit")
    print("="*60 + "\n")
    
    try:
        # Run simulation with controller
        sim.run_interactive(controller.update)
    except KeyboardInterrupt:
        print("\nInterrupted by user")
    except Exception as e:
        print(f"\nError during simulation: {e}")
        import traceback
        traceback.print_exc()
        return 1
    
    return 0

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