Torque Control Interactive#
This example demonstrates interactive torque control.
1// Copyright 2021 DeepMind Technologies Limited
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15#include "mujoco/mujoco_sim.h"
16#include "Qontrol/Qontrol.hpp"
17#include <chrono>
18#include <numeric>
19#include <deque>
20
21using namespace Qontrol;
22
23class MujocoQontrol : public MujocoSim
24{
25public:
26 //------------------------------------------- simulation -------------------------------------------
27 std::shared_ptr<Model::RobotModel> model;
28 std::shared_ptr<JointTorqueProblem> torque_problem;
29 std::shared_ptr<Task::CartesianAcceleration<ControlOutput::JointTorque>> main_task;
30 std::shared_ptr<Task::JointTorque<ControlOutput::JointTorque>> regularisation_task;
31 std::shared_ptr<Constraint::JointTorque<ControlOutput::JointTorque>> joint_torque_constraint;
32 std::shared_ptr<Constraint::JointConfiguration<ControlOutput::JointTorque>> joint_configuration_constraint;
33 Qontrol::RobotState robot_state;
34 std::string resource_path;
35
36 // Timing and statistics
37 std::deque<double> update_times;
38 std::chrono::time_point<std::chrono::high_resolution_clock> last_stats_time;
39 int iteration_count = 0;
40 const int max_samples = 1000;
41 bool verbose = true;
42
43 void initController() override
44 {
45 model = Model::RobotModel::loadModelFromFile(resource_path + "robot.urdf");
46
47 torque_problem = std::make_shared<Qontrol::JointTorqueProblem>(model);
48 main_task = torque_problem->task_set->add<Task::CartesianAcceleration>("MainTask");
49 regularisation_task = torque_problem->task_set->add<Task::JointTorque>("RegularisationTask", 1e-5);
50
51 joint_configuration_constraint = torque_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
52 auto joint_velocity_constraint = torque_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");
53 joint_torque_constraint = torque_problem->constraint_set->add<Constraint::JointTorque>("JointTorqueConstraint");
54
55 mju_copy(d->qpos, m->key_qpos, m->nu);
56 robot_state.joint_position.resize(model->getNrOfDegreesOfFreedom());
57 robot_state.joint_velocity.resize(model->getNrOfDegreesOfFreedom());
58
59 for (int i = 0; i < model->getNrOfDegreesOfFreedom(); ++i)
60 {
61 robot_state.joint_position[i] = d->qpos[i];
62 robot_state.joint_velocity[i] = d->qvel[i];
63 }
64 model->setRobotState(robot_state);
65
66 initialiseMocapPose(model->getFramePose(model->getTipFrameName()).matrix());
67
68 // Initialize timing
69 last_stats_time = std::chrono::high_resolution_clock::now();
70 iteration_count = 0;
71 }
72
73 void updateController() override
74 {
75 auto start_time = std::chrono::high_resolution_clock::now();
76
77 // Update robot state
78 auto t_state_start = std::chrono::high_resolution_clock::now();
79 for (int i = 0; i < model->getNrOfDegreesOfFreedom(); ++i)
80 {
81 robot_state.joint_position[i] = d->qpos[i];
82 robot_state.joint_velocity[i] = d->qvel[i];
83 }
84 model->setRobotState(robot_state);
85 auto t_state = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t_state_start).count();
86
87 // Get target pose
88 auto t_pose_start = std::chrono::high_resolution_clock::now();
89 Eigen::Affine3d target_pose;
90 target_pose.translation() = Eigen::Vector3d(d->mocap_pos[0], d->mocap_pos[1], d->mocap_pos[2]);
91 target_pose.linear() = Eigen::Quaterniond(d->mocap_quat[0], d->mocap_quat[1], d->mocap_quat[2], d->mocap_quat[3]).toRotationMatrix();
92 pinocchio::SE3 traj_pose(target_pose.matrix());
93
94 pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());
95 auto t_pose = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t_pose_start).count();
96
97 // Compute error
98 auto t_error_start = std::chrono::high_resolution_clock::now();
99 const pinocchio::SE3 tipMdes = current_pose.actInv(traj_pose);
100 auto err = pinocchio::log6(tipMdes).toVector();
101 double error_norm = err.norm();
102 auto t_error = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t_error_start).count();
103
104 // Compute control
105 auto t_control_start = std::chrono::high_resolution_clock::now();
106 Eigen::Matrix<double, 6, 1> p_gains;
107 p_gains << 1000, 1000, 1000, 1000, 1000, 1000;
108
109 Eigen::Matrix<double, 6, 1> d_gains = 2.0 * p_gains.cwiseSqrt();
110 Eigen::Matrix<double, 6, 1> xdd_star =
111 p_gains.cwiseProduct(err) +
112 d_gains.cwiseProduct(-model->getFrameVelocity(model->getTipFrameName()));
113
114 main_task->setTargetAcceleration(xdd_star);
115 regularisation_task->setTargetTorque(model->getJointGravityTorques() -
116 robot_state.joint_velocity);
117 regularisation_task->setWeightingMatrix(
118 model->getInverseJointInertiaMatrix());
119 auto t_control = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t_control_start).count();
120
121 // Solve QP
122 auto t_qp_start = std::chrono::high_resolution_clock::now();
123 torque_problem->update(m->opt.timestep);
124 auto t_qp = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - t_qp_start).count();
125
126 // Apply torques
127 bool solution_found = torque_problem->solutionFound();
128 double torque_norm = 0.0;
129 if (solution_found)
130 {
131 auto torques = torque_problem->getJointTorqueCommand();
132 torque_norm = torques.norm();
133 sendJointTorque(torques);
134 }
135
136 // Timing statistics
137 auto update_time = std::chrono::duration<double>(std::chrono::high_resolution_clock::now() - start_time).count();
138 update_times.push_back(update_time);
139 if (update_times.size() > max_samples)
140 {
141 update_times.pop_front();
142 }
143 iteration_count++;
144
145 // Verbose diagnostics (every 10 iterations)
146 if (verbose && iteration_count % 10 == 0)
147 {
148 printf("\nIteration %d:\n", iteration_count);
149 printf(" State update: %.3f ms\n", t_state * 1000.0);
150 printf(" Pose retrieval: %.3f ms\n", t_pose * 1000.0);
151 printf(" Error compute: %.3f ms\n", t_error * 1000.0);
152 printf(" Control setup: %.3f ms\n", t_control * 1000.0);
153 printf(" QP solve: %.3f ms\n", t_qp * 1000.0);
154 printf(" Total: %.3f ms\n", update_time * 1000.0);
155 printf(" Error norm: %.4f\n", error_norm);
156 printf(" Torque norm: %.2f Nm\n", torque_norm);
157 printf(" Solution: %s\n", solution_found ? "FOUND" : "NOT FOUND");
158 }
159
160 // Print statistics every second (non-verbose mode)
161 if (!verbose)
162 {
163 auto current_time = std::chrono::high_resolution_clock::now();
164 auto elapsed = std::chrono::duration<double>(current_time - last_stats_time).count();
165 if (elapsed >= 1.0)
166 {
167 double avg_time = std::accumulate(update_times.begin(), update_times.end(), 0.0) / update_times.size();
168 double update_rate = 1.0 / avg_time;
169 double sim_freq = 1.0 / m->opt.timestep;
170
171 printf("[C++] Update: %.1f Hz | Time: %.2f ms | QP: %.2f ms | Sim freq: %.1f Hz | Error: %.4f\n",
172 update_rate, avg_time * 1000.0, t_qp * 1000.0, sim_freq, error_norm);
173
174 last_stats_time = current_time;
175 }
176 }
177 }
178};
179
180int main(int argc, const char **argv)
181{
182 MujocoQontrol mujoco_qontrol;
183 Qontrol::Log::Logger::parseArgv(argc, argv);
184
185 // Parse verbose flag
186 for (int i = 1; i < argc; i++)
187 {
188 if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v")
189 {
190 mujoco_qontrol.verbose = true;
191 std::cout << "Verbose mode enabled" << std::endl;
192 }
193 }
194
195 mjvCamera cam;
196 mjv_defaultCamera(&cam);
197
198 mjvOption opt;
199 mjv_defaultOption(&opt);
200
201 mjvPerturb pert;
202 mjv_defaultPerturb(&pert);
203
204 // simulate object encapsulates the UI
205 auto sim = std::make_unique<mj::Simulate>(
206 std::make_unique<mj::GlfwAdapter>(),
207 &cam, &opt, &pert, /* is_passive = */ false);
208
209 std::string robot = argv[1];
210 std::string mujoco_scene = "./resources/" + robot + "/scene_interactive.xml";
211 mujoco_qontrol.resource_path = "./resources/" + robot + "/";
212 // start physics thread
213 std::thread physicsthreadhandle(&MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_scene.c_str());
214
215 // start simulation UI loop (blocking call)
216 sim->RenderLoop();
217 physicsthreadhandle.join();
218
219 return 0;
220}
1#!/usr/bin/env python3
2"""
3Interactive Torque Control with MuJoCo and Qontrol (Clean Version)
4
5This example demonstrates:
6- Loading a robot model in both MuJoCo and Qontrol
7- Real-time torque-level inverse dynamics
8- Interactive mocap target control
9- Cartesian acceleration tracking with joint torque commands
10
11This version uses the MujocoSimulator helper class to reduce boilerplate.
12
13Usage:
14 python torque_control_interactive_clean.py <robot_name>
15
16Example:
17 python torque_control_interactive_clean.py panda
18
19Controls:
20- Use the MuJoCo viewer to move the mocap body (red sphere)
21- The robot end-effector will track the mocap target
22- Press ESC to exit
23"""
24
25import numpy as np
26import qontrol
27import pinocchio as pin
28import argparse
29import os
30import time
31import traceback
32from mujoco_helper import MujocoSimulator, compute_log6_error
33
34
35class QontrolTorqueController:
36 """Torque controller using Qontrol (MuJoCo-agnostic)"""
37
38 def __init__(self, robot_name: str, resource_path: str, mujoco_sim: MujocoSimulator):
39 """
40 Initialize the Qontrol torque controller
41
42 Args:
43 robot_name: Name of the robot (e.g., 'panda')
44 resource_path: Path to robot resources folder
45 mujoco_sim: MuJoCo simulator instance
46 """
47 self.robot_name = robot_name
48 self.resource_path = resource_path
49 self.sim = mujoco_sim
50
51 # =================================================================
52 # Qontrol Setup - Robot Model and QP Problem
53 # =================================================================
54 urdf_path = os.path.join(resource_path, "robot.urdf")
55 if not os.path.exists(urdf_path):
56 raise FileNotFoundError(f"URDF file not found: {urdf_path}")
57
58 print(f"Loading Qontrol model from: {urdf_path}")
59 self.qontrol_model = qontrol.RobotModel.load_from_file(urdf_path)
60 self.ndof = self.qontrol_model.get_nr_of_degrees_of_freedom()
61 print(f"Robot has {self.ndof} degrees of freedom")
62
63 # Create QP solver
64 self.solver = qontrol.create_qpmad_solver()
65
66 # Create torque-level problem
67 self.torque_problem = qontrol.JointTorqueProblem(self.qontrol_model, self.solver)
68
69 # Add Cartesian acceleration task for end-effector tracking
70 self.main_task = self.torque_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)
71
72 # Add joint torque regularization (minimize joint torques)
73 self.regularization_task = self.torque_problem.task_set.add_joint_torque("RegularizationTask", 1e-5)
74
75 # Add constraints
76 self.joint_config_constraint = self.torque_problem.constraint_set.add_joint_configuration("JointConfigurationConstraint")
77 self.joint_vel_constraint = self.torque_problem.constraint_set.add_joint_velocity("JointVelocityConstraint")
78 self.joint_torque_constraint = self.torque_problem.constraint_set.add_joint_torque("JointTorqueConstraint")
79
80 # Set constraint horizons
81 self.joint_config_constraint.set_horizon(15)
82 self.joint_vel_constraint.set_horizon(15)
83
84 # Robot state object
85 self.robot_state = qontrol.RobotState()
86 self.robot_state.resize(self.ndof)
87
88 # Get tip frame name
89 self.tip_frame = self.qontrol_model.get_tip_frame_name()
90 print(f"Controlling frame: {self.tip_frame}")
91
92 # =================================================================
93 # Control Parameters
94 # =================================================================
95 # PD gains for Cartesian space
96 self.p_gains = np.array([1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0]) # [position, orientation] gains
97 self.d_gains = 2.0 * np.sqrt(self.p_gains) # Critical damping
98
99 def update(self):
100 """Main control update: compute and apply joint torques"""
101 # 1. Update Qontrol model with current state from simulation
102 qpos, qvel = self.sim.get_joint_state()
103 self.robot_state.joint_position = qpos
104 self.robot_state.joint_velocity = qvel
105 self.qontrol_model.set_robot_state(self.robot_state)
106
107 # 2. Get target and current poses
108 target_se3 = self.sim.get_mocap_pose_se3()
109 current_se3 = self.sim.get_frame_pose_se3(self.tip_frame)
110
111 # 3. Compute SE(3) log6 error
112 cartesian_error = compute_log6_error(target_se3, current_se3)
113
114 # 4. Get current end-effector velocity
115 ee_velocity = self.qontrol_model.get_frame_velocity(self.tip_frame)
116
117 # 5. Compute desired Cartesian acceleration (PD control)
118 desired_acceleration = self.p_gains * cartesian_error - self.d_gains * ee_velocity
119
120 # 6. Solve QP for joint torques
121 self.main_task.set_target_acceleration(desired_acceleration)
122
123 # Set regularization task target torques
124 g = self.qontrol_model.get_joint_gravity_torques()
125 target_torques = g - qvel
126 self.regularization_task.set_target_torque(target_torques)
127
128 # Set weighting matrix for regularization
129 M_inv = self.qontrol_model.get_inverse_joint_inertia_matrix()
130 self.regularization_task.set_weighting_matrix(M_inv)
131
132 # Solve QP
133 self.torque_problem.update(self.sim.dt)
134
135 # 7. Apply torques to simulation
136 if self.torque_problem.solution_found():
137 joint_torques = self.torque_problem.get_joint_torque_command()
138 self.sim.apply_torques(joint_torques)
139 else:
140 print("Warning: No QP solution found!")
141 self.sim.apply_torques(np.zeros(self.ndof))
142
143def main():
144 """Main entry point"""
145 parser = argparse.ArgumentParser(
146 description="Interactive torque control with MuJoCo and Qontrol (Clean Version)",
147 formatter_class=argparse.RawDescriptionHelpFormatter,
148 epilog="""
149Examples:
150 %(prog)s panda
151 %(prog)s ur5
152
153The program expects the following files in examples/resources/<robot>/:
154 - scene_interactive.xml (MuJoCo scene with mocap body)
155 - robot.urdf (Robot description for Qontrol)
156"""
157 )
158
159 parser.add_argument(
160 "robot",
161 type=str,
162 help="Robot name (e.g., 'panda', 'ur5')"
163 )
164
165 parser.add_argument(
166 "--resources",
167 type=str,
168 default=None,
169 help="Path to resources directory (default: ../examples/resources/<robot>/)"
170 )
171
172 args = parser.parse_args()
173
174 # Determine resource path
175 if args.resources:
176 resource_path = args.resources
177 else:
178 # Default: ../../../examples/resources/<robot>/
179 # (from bindings/python/examples to examples/resources)
180 script_dir = os.path.dirname(os.path.abspath(__file__))
181 resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
182
183 resource_path = os.path.abspath(resource_path)
184
185 if not os.path.exists(resource_path):
186 print(f"Error: Resource path not found: {resource_path}")
187 print(f"\nExpected directory structure:")
188 print(f" {resource_path}/")
189 print(f" ├── scene_interactive.xml")
190 print(f" └── robot.urdf")
191 return 1
192
193 try:
194 # Create MuJoCo simulator
195 sim = MujocoSimulator(args.robot, resource_path)
196
197 # Create Qontrol controller
198 controller = QontrolTorqueController(
199 args.robot,
200 resource_path,
201 sim
202 )
203
204 # Initialize mocap to robot's end-effector pose
205 sim.init_mocap_to_frame(controller.tip_frame)
206
207 # Display initial mocap pose
208 mocap_pose = sim.get_mocap_pose_se3()
209 print(f"\nInitial mocap pose (at {controller.tip_frame}):")
210 print(f" Position: {mocap_pose.translation}")
211 quat = pin.Quaternion(mocap_pose.rotation)
212 print(f" Orientation (quaternion xyzw): [{quat.x:.6f}, {quat.y:.6f}, {quat.z:.6f}, {quat.w:.6f}]")
213 print(f" Orientation (quaternion wxyz): [{quat.w:.6f}, {quat.x:.6f}, {quat.y:.6f}, {quat.z:.6f}]")
214
215 # Run interactive simulation
216 sim.run_interactive(
217 controller_callback=controller.update,
218 instructions="""Controls:
219 - Drag the red sphere (mocap body) to move the target
220 - The robot will track the mocap target
221 - Press ESC or close window to exit
222 - Double-click to select mocap body"""
223 )
224
225 except KeyboardInterrupt:
226 print("\nInterrupted by user")
227 return 0
228 except Exception as e:
229 print(f"\nError: {e}")
230 traceback.print_exc()
231 return 1
232
233 return 0
234
235
236if __name__ == "__main__":
237 exit(main())