Acceleration Qontrol Interactive#
Introduction#
This example demonstrates interactive acceleration control using Qontrol and MuJoCo. The user can control a robot by dragging a target (mocap body) in the simulation. The robot’s end effector tracks the target using a Cartesian acceleration task. The problem is formulated as a Quadratic Program (QP) and solved at the configuration level to map desired Cartesian accelerations to joint commands, considering robot constraints such as joint limits.
The robot main task consists in keeping the end effector at the target pose in Cartesian space. The mujoco library is used to simulate the robot behaviour.
Full Code:
You can find the source code of this example here.
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
18
19using namespace Qontrol;
20
21class MujocoQontrol : public MujocoSim
22{
23public:
24//------------------------------------------- simulation -------------------------------------------
25std::shared_ptr<Qontrol::Model::RobotModel> model;
26std::shared_ptr<Qontrol::JointAccelerationProblem> acceleration_problem;
27std::shared_ptr<Qontrol::Task::CartesianAcceleration<ControlOutput::JointAcceleration>> main_task;
28pinocchio::SE3 init_pose;
29
30Qontrol::RobotState robot_state;
31
32std::string resource_path;
33
34void initController() override
35{
36 model =
37 Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
38
39 acceleration_problem = std::make_shared<Qontrol::JointAccelerationProblem>(model);
40 main_task = acceleration_problem->task_set->add<Task::CartesianAcceleration>("MainTask");
41 auto regularisation_task = acceleration_problem->task_set->add<Task::JointAcceleration>("RegularisationTask",1e-5);
42
43 auto joint_configuration_constraint = acceleration_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
44 auto joint_velocity_constraint = acceleration_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");
45
46 robot_state.joint_position.resize(model->getNrOfDegreesOfFreedom());
47 robot_state.joint_velocity.resize(model->getNrOfDegreesOfFreedom());
48 mju_copy(d->qpos, m->key_qpos, m->nu);
49
50 for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
51 {
52 robot_state.joint_position[i] = d->qpos[i];
53 robot_state.joint_velocity[i] = d->qvel[i];
54 }
55
56 model->setRobotState(robot_state);
57 initialiseMocapPose(model->getFramePose(model->getTipFrameName()).matrix());
58}
59
60void updateController() override
61{
62 for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
63 {
64 std::cout << mj_id2name(m, mjtObj::mjOBJ_JOINT, i) << " " << d->qpos[i] << std::endl;
65 robot_state.joint_position[i] = d->qpos[i];
66 robot_state.joint_velocity[i] = d->qvel[i];
67 }
68
69 model->setRobotState(robot_state);
70 Eigen::Affine3d target_pose;
71 target_pose.translation() = Eigen::Vector3d(d->mocap_pos[0],d->mocap_pos[1],d->mocap_pos[2]);
72 target_pose.linear() = Eigen::Quaterniond(d->mocap_quat[0],d->mocap_quat[1],d->mocap_quat[2],d->mocap_quat[3]).toRotationMatrix();
73 pinocchio::SE3 traj_pose(target_pose.matrix());
74
75 pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());
76 const pinocchio::SE3 tipMdes = current_pose.actInv(traj_pose);
77 auto err = pinocchio::log6(tipMdes).toVector();
78 Eigen::Matrix<double, 6, 1> p_gains;
79 p_gains << 1000, 1000, 1000, 1000, 1000, 1000;
80 Eigen::Matrix<double, 6, 1> d_gains = 2.0 * p_gains.cwiseSqrt();
81 Eigen::Matrix<double, 6, 1> xdd_star =
82 p_gains.cwiseProduct(err) +
83 d_gains.cwiseProduct(-
84 model->getFrameVelocity(model->getTipFrameName()));
85
86 main_task->setTargetAcceleration(xdd_star);
87
88 acceleration_problem->update(m->opt.timestep);
89
90 if (acceleration_problem->solutionFound())
91 {
92 sendJointVelocity(acceleration_problem->getJointVelocityCommand());
93 }
94}
95
96};
97
98int main(int argc, const char** argv) {
99 MujocoQontrol mujoco_qontrol;
100 Qontrol::Log::Logger::parseArgv(argc, argv);
101
102 mjvCamera cam;
103 mjv_defaultCamera(&cam);
104
105 mjvOption opt;
106 mjv_defaultOption(&opt);
107
108 mjvPerturb pert;
109 mjv_defaultPerturb(&pert);
110
111 // simulate object encapsulates the UI
112 auto sim = std::make_unique<mj::Simulate>(
113 std::make_unique<mj::GlfwAdapter>(),
114 &cam, &opt, &pert, /* is_passive = */ false
115 );
116
117 std::string robot = argv[1];
118 std::string mujoco_model = "./resources/"+robot+"/scene_interactive.xml";
119 mujoco_qontrol.resource_path = "./resources/"+robot+"/";
120
121 // start physics thread
122 std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_model.c_str());
123
124 // start simulation UI loop (blocking call)
125 sim->RenderLoop();
126 physicsthreadhandle.join();
127
128 return 0;
129}
—
Explanation of the code
Declaration
First we declare all the objects that will be used to define our problem.
std::shared_ptr<Qontrol::Model::RobotModel> model;
We use pinocchio for our model library.
std::shared_ptr<Qontrol::JointAccelerationProblem> acceleration_problem;
The output of our qp controller is at the acceleration level, integrated to output velocity commands.
std::shared_ptr<Qontrol::Task::CartesianAcceleration<ControlOutput::JointAcceleration>> main_task;
We then declare a main task that will be updated every millisecond. The main task is expressed as a Cartesian Acceleration task.
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.
acceleration_problem = std::make_shared<Qontrol::JointAccelerationProblem>(model); main_task = acceleration_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); auto regularisation_task = acceleration_problem->task_set->add<Task::JointAcceleration>("RegularisationTask",1e-5);
We then fill the task set of acceleration_problem with the main task and the regularisation task. Each task is given a name and optionally a relative weight. By default the weight is 1.
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 acceleration_problem. Constraints of the robot are already pre-implemented and can be added as shown in this example.
Update
void updateController() override { for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i) { std::cout << mj_id2name(m, mjtObj::mjOBJ_JOINT, i) << " " << d->qpos[i] << std::endl; robot_state.joint_position[i] = d->qpos[i]; robot_state.joint_velocity[i] = d->qvel[i]; } model->setRobotState(robot_state);
The update function is called every millisecond. 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 from the dragging interactions.
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());
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.
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())); main_task->setTargetAcceleration(xdd_star);
We set the desired acceleration into the main task.
acceleration_problem->update(m->opt.timestep); if (acceleration_problem->solutionFound()) { sendJointVelocity(acceleration_problem->getJointVelocityCommand()); } } };
Once we updated the necessary tasks and constraints we can update the whole problem. If a solution to the problem exists 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_model = "./resources/"+robot+"/scene_interactive.xml"; mujoco_qontrol.resource_path = "./resources/"+robot+"/"; // start physics thread std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_model.c_str()); // start simulation UI loop (blocking call) sim->RenderLoop(); physicsthreadhandle.join(); return 0;
The main function fetches the robot name given in argv and starts the Mujoco simulation.
Full Code:
You can find the source code of this example here.
1#!/usr/bin/env python3
2"""
3Interactive Acceleration Control with MuJoCo and Qontrol
4
5This example demonstrates:
6- Loading a robot model in both MuJoCo and Qontrol
7- Joint velocity control via joint acceleration QP
8- Interactive mocap target control
9- Cartesian acceleration tracking with joint velocity commands
10
11Usage:
12 python acceleration_qontrol_interactive.py <robot_name>
13
14Example:
15 python acceleration_qontrol_interactive.py panda
16
17Controls:
18- Use the MuJoCo viewer to move the mocap body (red sphere)
19- The robot end-effector will track the mocap target
20- Press ESC to exit
21"""
22
23import numpy as np
24import qontrol
25import pinocchio as pin
26import argparse
27import os
28import time
29from mujoco_helper import MujocoSimulator, compute_log6_error
30
31
32class QontrolAccelerationController:
33 """Acceleration controller using Qontrol (MuJoCo-agnostic)"""
34
35 def __init__(self, robot_name: str, resource_path: str, mujoco_sim: MujocoSimulator):
36 """
37 Initialize the Qontrol acceleration controller
38
39 Args:
40 robot_name: Name of the robot (e.g., 'panda')
41 resource_path: Path to robot resources folder
42 mujoco_sim: MuJoCo simulator instance
43 """
44 self.robot_name = robot_name
45 self.resource_path = resource_path
46 self.sim = mujoco_sim
47
48 # =================================================================
49 # Qontrol Setup - Robot Model and QP Problem
50 # =================================================================
51 urdf_path = os.path.join(resource_path, "robot.urdf")
52 if not os.path.exists(urdf_path):
53 raise FileNotFoundError(f"URDF file not found: {urdf_path}")
54
55 print(f"Loading Qontrol model from: {urdf_path}")
56 self.qontrol_model = qontrol.RobotModel.load_from_file(urdf_path)
57 self.ndof = self.qontrol_model.get_nr_of_degrees_of_freedom()
58 print(f"Robot has {self.ndof} degrees of freedom")
59
60 # Create QP solver
61 self.solver = qontrol.create_qpmad_solver()
62
63 # Create acceleration-level problem (output: joint velocity commands)
64 self.acceleration_problem = qontrol.JointAccelerationProblem(self.qontrol_model, self.solver)
65
66 # Add Cartesian acceleration task for end-effector tracking
67 self.main_task = self.acceleration_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)
68
69 # Add joint acceleration regularization (minimize joint accelerations)
70 self.regularization_task = self.acceleration_problem.task_set.add_joint_acceleration("RegularizationTask", 1e-5)
71
72 # Add constraints
73 self.joint_config_constraint = self.acceleration_problem.constraint_set.add_joint_configuration("JointConfigurationConstraint")
74 self.joint_vel_constraint = self.acceleration_problem.constraint_set.add_joint_velocity("JointVelocityConstraint")
75
76 # Robot state object
77 self.robot_state = qontrol.RobotState()
78 self.robot_state.resize(self.ndof)
79
80 # Get tip frame name
81 self.tip_frame = self.qontrol_model.get_tip_frame_name()
82 print(f"Controlling frame: {self.tip_frame}")
83
84 # Get initial joint state and update model
85 qpos, qvel = self.sim.get_joint_state()
86 self.robot_state.joint_position = qpos
87 self.robot_state.joint_velocity = qvel
88 self.qontrol_model.set_robot_state(self.robot_state)
89
90 print("dt:", self.sim.dt)
91
92 # =================================================================
93 # Control Parameters
94 # =================================================================
95 self.p_gains = np.array([1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0]) # [position, orientation] gains
96 self.d_gains = 2.0 * np.sqrt(self.p_gains) # Critical damping
97
98 def update(self):
99 """Main control update: compute and apply joint velocity commands"""
100 # 1. Update Qontrol model with current state from simulation
101 qpos, qvel = self.sim.get_joint_state()
102 self.robot_state.joint_position = qpos
103 self.robot_state.joint_velocity = qvel
104 self.qontrol_model.set_robot_state(self.robot_state)
105
106 # 2. Get target and current poses
107 target_se3 = self.sim.get_mocap_pose_se3()
108 current_ee_matrix = self.qontrol_model.get_frame_pose(self.tip_frame)
109 current_se3 = pin.SE3(current_ee_matrix[:3, :3], current_ee_matrix[:3, 3])
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 velocities
121 self.main_task.set_target_acceleration(desired_acceleration)
122 self.acceleration_problem.update(self.sim.dt)
123
124 # 7. Apply velocities to simulation
125 if self.acceleration_problem.solution_found():
126 joint_velocities = self.acceleration_problem.get_joint_velocity_command()
127 self.sim.set_joint_velocities(joint_velocities)
128 else:
129 print("Warning: No QP solution found!")
130 self.sim.set_joint_velocities(np.zeros(self.ndof))
131
132
133def main():
134 """Main entry point"""
135 parser = argparse.ArgumentParser(
136 description="Interactive acceleration control with MuJoCo and Qontrol",
137 formatter_class=argparse.RawDescriptionHelpFormatter,
138 epilog="""
139Examples:
140 %(prog)s panda
141 %(prog)s ur5
142 """
143 )
144 parser.add_argument("robot", type=str, help="Robot name (e.g., panda)")
145
146 args = parser.parse_args()
147
148 # Get resource path
149 script_dir = os.path.dirname(os.path.abspath(__file__))
150 resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
151
152 if not os.path.exists(resource_path):
153 print(f"Error: Resource path not found: {resource_path}")
154 return 1
155
156 # MuJoCo scene path
157 scene_path = os.path.join(resource_path, "scene_interactive.xml")
158 if not os.path.exists(scene_path):
159 print(f"Error: Scene file not found: {scene_path}")
160 return 1
161
162 print(f"Loading MuJoCo scene from: {scene_path}")
163
164 # Initialize MuJoCo simulator
165 sim = MujocoSimulator(args.robot, resource_path)
166
167 # Initialize controller
168 controller = QontrolAccelerationController(args.robot, resource_path, sim)
169
170 # Initialize mocap to robot's end-effector pose
171 sim.init_mocap_to_frame(controller.tip_frame)
172
173 # Display initial mocap pose
174 mocap_pose = sim.get_mocap_pose_se3()
175 print(f"\nInitial mocap pose (at {controller.tip_frame}):")
176 print(f" Position: {mocap_pose.translation}")
177 quat = pin.Quaternion(mocap_pose.rotation)
178 print(f" Orientation (quaternion xyzw): [{quat.x:.6f}, {quat.y:.6f}, {quat.z:.6f}, {quat.w:.6f}]")
179 print(f" Orientation (quaternion wxyz): [{quat.w:.6f}, {quat.x:.6f}, {quat.y:.6f}, {quat.z:.6f}]")
180
181 print("\n" + "="*60)
182 print("Starting acceleration control simulation")
183 print("Move the red mocap target to control the robot")
184 print("Press ESC to exit")
185 print("="*60 + "\n")
186
187 try:
188 # Run simulation with controller
189 sim.run_interactive(controller.update, instructions="""Controls:
190 - Drag the red sphere (mocap body) to move the target
191 - The robot will track the mocap target
192 - Press ESC or close window to exit
193 - Double-click to select mocap body""")
194 except KeyboardInterrupt:
195 print("\nInterrupted by user")
196 except Exception as e:
197 print(f"\nError during simulation: {e}")
198 import traceback
199 traceback.print_exc()
200 return 1
201
202 return 0
203
204
205if __name__ == "__main__":
206 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.acceleration_problem = qontrol.JointAccelerationProblem(self.qontrol_model, self.solver)
The output of our qp controller is at the acceleration level.
self.main_task = self.acceleration_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)
We then declare a main task that will be updated every millisecond. The main task is expressed as a Cartesian Acceleration task.
Initialization
We initialize the problem by giving it the model. By default, the qpmad library is used.
We then fill the task set of acceleration_problem with the main task and the regularisation task.
self.regularization_task = self.acceleration_problem.task_set.add_joint_acceleration("RegularizationTask", 1e-5)
We then define the constraint set of acceleration_problem. Constraints of the robot are already pre-implemented and can be added as shown in this example.
self.joint_config_constraint = self.acceleration_problem.constraint_set.add_joint_configuration("JointConfigurationConstraint")
self.joint_vel_constraint = self.acceleration_problem.constraint_set.add_joint_velocity("JointVelocityConstraint")
Update
The update function is called every millisecond. 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 from the interactive marker.
def update(self):
"""Main control update: compute and apply joint velocity commands"""
# 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
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
cartesian_error = compute_log6_error(target_se3, current_se3)
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.
desired_acceleration = self.p_gains * cartesian_error - self.d_gains * ee_velocity
# 6. Solve QP for joint velocities
self.main_task.set_target_acceleration(desired_acceleration)
Once we updated the necessary tasks and constraints we can update the whole problem. If a solution to the problem exists we can then get it and send it to the simulated robot.
self.acceleration_problem.update(self.sim.dt)
# 7. Apply velocities to simulation
if self.acceleration_problem.solution_found():
joint_velocities = self.acceleration_problem.get_joint_velocity_command()
self.sim.set_joint_velocities(joint_velocities)
else:
print("Warning: No QP solution found!")
self.sim.set_joint_velocities(np.zeros(self.ndof))
Main function
The main function fetches the robot name given in argv and starts the Mujoco simulation.
def main():
"""Main entry point"""
parser = argparse.ArgumentParser(
description="Interactive acceleration 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 = QontrolAccelerationController(args.robot, resource_path, sim)
# Initialize mocap to robot's end-effector pose
sim.init_mocap_to_frame(controller.tip_frame)
# Display initial mocap pose
mocap_pose = sim.get_mocap_pose_se3()
print(f"\nInitial mocap pose (at {controller.tip_frame}):")
print(f" Position: {mocap_pose.translation}")
quat = pin.Quaternion(mocap_pose.rotation)
print(f" Orientation (quaternion xyzw): [{quat.x:.6f}, {quat.y:.6f}, {quat.z:.6f}, {quat.w:.6f}]")
print(f" Orientation (quaternion wxyz): [{quat.w:.6f}, {quat.x:.6f}, {quat.y:.6f}, {quat.z:.6f}]")
print("\n" + "="*60)
print("Starting acceleration control simulation")
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, instructions="""Controls:
- Drag the red sphere (mocap body) to move the target
- The robot will track the mocap target
- Press ESC or close window to exit
- Double-click to select mocap body""")
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