Torque Qontrol

# Torque control

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

\[\begin{split}\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{split}\]

.

The robot main tasks consists in following a simple trajectory defined in Cartesian space. 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::JointTorque<ControlOutput::JointTorque>> regularisation_task;

Qontrol::RobotState robot_state;
TrajectoryGeneration* traj;
std::string resource_path;

void initController() override
{
  model =
      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
  
  const int ndof = model->getNrOfDegreesOfFreedom();
  
  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(ndof);
  robot_state.joint_velocity.resize(ndof);

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

void updateController() override
{
  const int ndof = model->getNrOfDegreesOfFreedom();

 for (int i=0; i<ndof ; ++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);

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

  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.

  robot_state.joint_position.resize(ndof);
  robot_state.joint_velocity.resize(ndof);

We also create the robot state and resize it.

  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
{
  const int ndof = model->getNrOfDegreesOfFreedom();

 for (int i=0; i<ndof ; ++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.

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

  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
"""
Non-Interactive Torque Control with Trajectory Following

This example demonstrates:
- Loading a robot model in both MuJoCo and Qontrol
- Real-time torque-level inverse dynamics
- Following a pre-defined Cartesian trajectory from CSV file
- Cartesian acceleration tracking with joint torque commands

Mirrors the C++ example: torqueQontrol.cpp

Usage:
    python torque_qontrol.py <robot_name>
    
Example:
    python torque_qontrol.py panda
    python torque_qontrol.py panda --verbose
"""

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


class QontrolTorqueController:
    """Torque controller with trajectory following using Qontrol"""
    
    def __init__(self, robot_name: str, resource_path: str, mujoco_sim: MujocoSimulator, verbose: bool = False):
        """
        Initialize the Qontrol torque controller
        
        Args:
            robot_name: Name of the robot (e.g., 'panda')
            resource_path: Path to robot resources folder
            mujoco_sim: MuJoCo simulator instance
            verbose: Print detailed diagnostics each iteration (default: False)
        """
        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
        self.torque_problem = qontrol.JointTorqueProblem(self.qontrol_model, self.solver)
        
        # Add Cartesian acceleration task for end-effector tracking
        self.main_task = self.torque_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)
        
        # Add joint torque regularization (minimize joint torques)
        self.regularization_task = self.torque_problem.task_set.add_joint_torque("RegularizationTask", 1e-5)
        
        # Add constraints
        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")
        
        # Store verbose flag
        self.verbose = verbose

        # Robot state object
        self.robot_state = qontrol.RobotState()
        self.robot_state.resize(self.ndof)
        
        # Get tip frame name
        self.tip_frame = self.qontrol_model.get_tip_frame_name()
        print(f"Controlling frame: {self.tip_frame}")
        
        # 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)
        
        print("dt:", self.sim.dt)
       
        # =================================================================
        # Trajectory Setup
        # =================================================================
        trajectory_path = os.path.join(resource_path, "trajectory.csv")
        if not os.path.exists(trajectory_path):
            raise FileNotFoundError(f"Trajectory file not found: {trajectory_path}")
        
        print(f"Loading trajectory from: {trajectory_path}")
        self.trajectory = TrajectoryGeneration(trajectory_path, self.sim.dt)
        
        # =================================================================
        # Control Parameters
        # =================================================================
        # PD gains for Cartesian space
        self.p_gains = np.array([1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0])
        self.d_gains = 2.0 * np.sqrt(self.p_gains)  # Critical damping
        
        # Statistics tracking
        self.update_times = []
        self.iteration_count = 0
        self.last_stats_time = time.perf_counter()
    
    def update(self):
        """Main control update: compute and apply joint torques"""
        start_time = time.perf_counter()
        
        # 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. Update trajectory to get next target
        self.trajectory.update()
        target_pose = self.trajectory.get_pose()
        target_velocity = self.trajectory.get_velocity()
        target_acceleration = self.trajectory.get_acceleration()
        
        # 3. Get current end-effector pose
        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])
        
        # 4. Compute SE(3) log6 error
        cartesian_error = compute_log6_error(target_pose, current_se3)
        error_norm = np.linalg.norm(cartesian_error)
        
        # 5. Get current end-effector velocity
        ee_velocity = self.qontrol_model.get_frame_velocity(self.tip_frame)
        
        # 6. Compute desired Cartesian acceleration (PD control + feedforward)
        # xdd_star = Kp * err + Kd * (v_des - v_curr) + a_des
        desired_acceleration = (self.p_gains * cartesian_error + 
                              self.d_gains * (target_velocity - ee_velocity) + 
                              target_acceleration)
        
        # Set main task target
        self.main_task.set_target_acceleration(desired_acceleration)
        
        # 7. Set regularization task
        # Target torques: gravity compensation minus velocity damping
        g = self.qontrol_model.get_joint_gravity_torques()
        target_torques = g - qvel
        self.regularization_task.set_target_torque(target_torques)
        
        # Set weighting matrix (inverse of inertia matrix)
        M_inv = self.qontrol_model.get_inverse_joint_inertia_matrix()
        self.regularization_task.set_weighting_matrix(M_inv)
        
        # 8. 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
        
        # 9. Apply torques to simulation
        solution_found = self.torque_problem.solution_found()
        if solution_found:
            joint_torques = self.torque_problem.get_joint_torque_command()
            torque_norm = np.linalg.norm(joint_torques)
            self.sim.apply_torques(joint_torques)
        else:
            print("Warning: No QP solution found!")
            torque_norm = 0.0
            self.sim.apply_torques(np.zeros(self.ndof))
        
        # Statistics
        update_time = time.perf_counter() - start_time
        self.update_times.append(update_time)
        self.iteration_count += 1
        
        # Verbose diagnostics
        if self.verbose and self.iteration_count % 100 == 0:
            print(f"\nIteration {self.iteration_count} (t={self.trajectory.time:.2f}s):")
            print(f"  QP solve:       {t_qp*1000:.3f} ms")
            print(f"  Total:          {update_time*1000:.3f} ms")
            print(f"  Error norm:     {error_norm:.4f}")
            print(f"  Torque norm:    {torque_norm:.2f} Nm")
            print(f"  Solution:       {'FOUND' if solution_found else 'NOT FOUND'}")
            target_pos = target_pose.translation
            current_pos = current_se3.translation
            pos_error = np.linalg.norm(target_pos - current_pos)
            print(f"  Position error: {pos_error*1000:.2f} mm")
        
        # Print statistics every second
        if not self.verbose:
            current_time = time.perf_counter()
            if current_time - self.last_stats_time >= 1.0:
                avg_update_time = np.mean(self.update_times[-1000:])
                progress = (self.trajectory.time / self.trajectory.duration) * 100
                print(f"[Torque] t={self.trajectory.time:.1f}s/{self.trajectory.duration:.1f}s ({progress:.0f}%) | Compute: {avg_update_time*1000:.2f} ms | QP: {t_qp*1000:.2f} ms | Error: {error_norm:.4f}")
                self.last_stats_time = current_time
        
        # Check if trajectory is finished
        if self.trajectory.is_finished():
            return False  # Signal to stop simulation
        
        return True  # Continue simulation
    
    def print_statistics(self):
        """Print final statistics"""
        if self.update_times:
            avg_time = np.mean(self.update_times)
            update_rate = 1.0 / avg_time if avg_time > 0 else 0
            print(f"\nFinal Statistics:")
            print(f"Average update rate: {update_rate:.1f} Hz")
            print(f"Average update time: {avg_time*1000:.2f} ms")
            print(f"Max update time: {np.max(self.update_times)*1000:.2f} ms")
            print(f"Min update time: {np.min(self.update_times)*1000:.2f} ms")
            print(f"Total iterations: {self.iteration_count}")
            print(f"Trajectory duration: {self.trajectory.duration:.2f}s")


def main():
    """Main entry point"""
    parser = argparse.ArgumentParser(
        description="Non-interactive torque control with trajectory following",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Examples:
  %(prog)s panda
  %(prog)s panda --verbose
        """
    )
    parser.add_argument("robot", type=str, help="Robot name (e.g., panda)")
    parser.add_argument("--verbose", action="store_true", help="Print detailed diagnostics")
    
    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
    
    # Initialize MuJoCo simulator
    sim = MujocoSimulator(args.robot, resource_path, "scene.xml")
    
    # Initialize controller
    controller = QontrolTorqueController(args.robot, resource_path, sim, verbose=args.verbose)
    
    print("\n" + "="*60)
    print("Starting torque control with trajectory following")
    print("Press ESC to exit")
    print("="*60 + "\n")
    
    try:
        # Run simulation with controller (will stop when trajectory finishes)
        sim.run(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
    finally:
        # Print final statistics
        controller.print_statistics()
    
    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.

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

We use pinocchio for our model library.

        self.torque_problem = qontrol.JointTorqueProblem(self.qontrol_model, self.solver)

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

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

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

The main task is expressed as a Cartesian Acceleration task.

        self.regularization_task = self.torque_problem.task_set.add_joint_torque("RegularizationTask", 1e-5)

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

Initialization

During initialization we instantiate the model with the robot urdf.

        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)

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

        self.solver = qontrol.create_qpmad_solver()
        
        # Create torque-level problem
        self.torque_problem = qontrol.JointTorqueProblem(self.qontrol_model, self.solver)

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.

        self.main_task = self.torque_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)
        
        # Add joint torque regularization (minimize joint torques)
        self.regularization_task = self.torque_problem.task_set.add_joint_torque("RegularizationTask", 1e-5)

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.

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

We also create the robot state and resize it.

        self.robot_state = qontrol.RobotState()
        self.robot_state.resize(self.ndof)

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.

        trajectory_path = os.path.join(resource_path, "trajectory.csv")
        if not os.path.exists(trajectory_path):
            raise FileNotFoundError(f"Trajectory file not found: {trajectory_path}")
        
        print(f"Loading trajectory from: {trajectory_path}")
        self.trajectory = TrajectoryGeneration(trajectory_path, self.sim.dt)

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.

    def update(self):
        """Main control update: compute and apply joint torques"""
        start_time = time.perf_counter()
        
        # 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 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.

        self.trajectory.update()
        target_pose = self.trajectory.get_pose()
        target_velocity = self.trajectory.get_velocity()
        target_acceleration = self.trajectory.get_acceleration()
        
        # 3. Get current end-effector pose
        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])
        
        # 4. Compute SE(3) log6 error
        cartesian_error = compute_log6_error(target_pose, current_se3)
        error_norm = np.linalg.norm(cartesian_error)
        
        # 5. Get current end-effector velocity
        ee_velocity = self.qontrol_model.get_frame_velocity(self.tip_frame)
        
        # 6. Compute desired Cartesian acceleration (PD control + feedforward)
        # xdd_star = Kp * err + Kd * (v_des - v_curr) + a_des
        desired_acceleration = (self.p_gains * cartesian_error + 

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}}\)

        self.main_task.set_target_acceleration(desired_acceleration)
        
        # 7. Set regularization task
        # Target torques: gravity compensation minus velocity damping
        g = self.qontrol_model.get_joint_gravity_torques()
        target_torques = g - qvel
        self.regularization_task.set_target_torque(target_torques)
        
        # Set weighting matrix (inverse of inertia matrix)
        M_inv = self.qontrol_model.get_inverse_joint_inertia_matrix()
        self.regularization_task.set_weighting_matrix(M_inv)

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
        
        # 9. Apply torques to simulation
        solution_found = self.torque_problem.solution_found()
        if solution_found:
            joint_torques = self.torque_problem.get_joint_torque_command()
            torque_norm = np.linalg.norm(joint_torques)
            self.sim.apply_torques(joint_torques)

Main function

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

def main():
    """Main entry point"""
    parser = argparse.ArgumentParser(
        description="Non-interactive torque control with trajectory following",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Examples:
  %(prog)s panda
  %(prog)s panda --verbose
        """
    )
    parser.add_argument("robot", type=str, help="Robot name (e.g., panda)")
    parser.add_argument("--verbose", action="store_true", help="Print detailed diagnostics")
    
    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
    
    # Initialize MuJoCo simulator
    sim = MujocoSimulator(args.robot, resource_path, "scene.xml")
    
    # Initialize controller
    controller = QontrolTorqueController(args.robot, resource_path, sim, verbose=args.verbose)
    
    print("\n" + "="*60)
    print("Starting torque control with trajectory following")
    print("Press ESC to exit")
    print("="*60 + "\n")
    
    try:
        # Run simulation with controller (will stop when trajectory finishes)
        sim.run(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
    finally:
        # Print final statistics
        controller.print_statistics()
    
    return 0