Velocity Qontrol Wall Constraint Interactive#

Introduction#

This example demonstrates how to use Qontrol to enforce cartesian plane constraints on a robot’s end-effector velocity. The QP problem is formulated to minimize the tracking error of a Cartesian trajectory while respecting joint velocity limits and a cartesian plane constraint (e.g., a virtual wall).

We define the robot base as the robot baselink frame \(\mathcal{R}_O\) (it can be changed with the root_frame_name, but this frame has to be in the robot URDF). In this example, we set the plane with the normal vector (pointing towards the constraint region) as \(u = -x_O\), and the point E (belonging to the plane) to 10 cm from the baselink \(OE = -0.1 x_O\).

The MuJoCo library is used to simulate the robot’s behavior.

Formulation#

The QP problem is expressed at the joint velocity level and is defined as:

\[\begin{split}\begin{array}{ccc} \boldsymbol{\dot{q}}^{opt} = & \underset{\boldsymbol{\dot{q}}}{\mathrm{argmin}} & ||J(\boldsymbol{q})\boldsymbol{\dot{q}} - \boldsymbol{v}^{target} ||^2 + \omega || \boldsymbol{\dot{q}} ||^2 \\ & \textrm{s.t.} & \boldsymbol{\dot{q}^{min}} \leq \boldsymbol{\dot{q}} \leq \boldsymbol{\dot{q}^{max}}, \\ & & \boldsymbol{q}^{min} \leq \boldsymbol{q} + \Delta t \cdot \boldsymbol{\dot{q}} \leq \boldsymbol{q}^{max}, \\ & & U \cdot J(\boldsymbol{q}) \boldsymbol{\dot{q}} \leq \frac{D - U \cdot \overrightarrow{OP}(\boldsymbol{q})}{\Delta t}. \end{array}\end{split}\]

  • The main task is to track a Cartesian velocity trajectory.

  • The regularization task minimizes joint velocities.

  • The cartesian plane constraint ensures that the end-effector does not cross a virtual wall.

—

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::JointVelocityProblem> velocity_problem;
 27std::shared_ptr<Qontrol::Task::CartesianVelocity<Qontrol::ControlOutput::JointVelocity>> main_task;
 28std::shared_ptr<Qontrol::Constraint::CartesianPlane<Qontrol::ControlOutput::JointVelocity>> wallConstraint;
 29
 30Qontrol::RobotState robot_state;
 31Eigen::Vector3d u_;
 32Eigen::Vector3d point_;
 33
 34std::string resource_path;
 35
 36void initController() override
 37{
 38  model =
 39      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
 40
 41  velocity_problem = std::make_shared<Qontrol::JointVelocityProblem>(model);  
 42  main_task = velocity_problem->task_set->add<Task::CartesianVelocity>("MainTask"); 
 43  auto regularisation_task = velocity_problem->task_set->add<Task::JointVelocity>("RegularisationTask",1e-5); 
 44
 45  auto joint_configuration_constraint = velocity_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
 46  auto joint_velocity_constraint = velocity_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");
 47
 48
 49  wallConstraint = velocity_problem->constraint_set->add<Constraint::CartesianPlane>("wallConstraint");
 50  
 51  robot_state.joint_position.resize(model->getNrOfDegreesOfFreedom());
 52  robot_state.joint_velocity.resize(model->getNrOfDegreesOfFreedom());
 53  mju_copy(d->qpos, m->key_qpos, m->nu);
 54
 55  for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
 56  {
 57    robot_state.joint_position[i] = d->qpos[i];
 58    robot_state.joint_velocity[i] = d->qvel[i];
 59  }
 60
 61  model->setRobotState(robot_state);
 62  initialiseMocapPose(model->getFramePose(model->getTipFrameName()).matrix());
 63
 64  // Set wall plane parameters from geom in scene
 65  int wall_geom_id = mj_name2id(m, mjOBJ_GEOM, "wall_plane");
 66  Eigen::Vector3d wall_pos(
 67    d->geom_xpos[3*wall_geom_id+0],
 68    d->geom_xpos[3*wall_geom_id+1],
 69    d->geom_xpos[3*wall_geom_id+2]
 70  );
 71  Eigen::Matrix3d wall_rot = Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>>(&d->geom_xmat[9*wall_geom_id]);
 72
 73  // Wall plane normal is x-axis of the geom frame (thin dimension)
 74  u_ = -wall_rot.col(0);
 75  point_ = wall_pos;
 76
 77  wallConstraint->setHorizon(10);
 78  wallConstraint->setPlane(u_,point_);
 79
 80}
 81
 82void updateController() override
 83{
 84  for (int i=0; i<model->getNrOfDegreesOfFreedom() ; ++i)
 85  {
 86    robot_state.joint_position[i] = d->qpos[i];
 87    robot_state.joint_velocity[i] = d->qvel[i];
 88  }
 89
 90  model->setRobotState(robot_state);
 91  Eigen::Affine3d target_pose;
 92  target_pose.translation() = Eigen::Vector3d(d->mocap_pos[0],d->mocap_pos[1],d->mocap_pos[2]);
 93  target_pose.linear() = Eigen::Quaterniond(d->mocap_quat[0],d->mocap_quat[1],d->mocap_quat[2],d->mocap_quat[3]).toRotationMatrix();
 94  pinocchio::SE3 traj_pose(target_pose.matrix());
 95  
 96  pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());
 97  const pinocchio::SE3 tipMdes = current_pose.actInv(traj_pose);
 98  auto err = pinocchio::log6(tipMdes).toVector();
 99  Eigen::Matrix<double,6,1> p_gains;
100  p_gains << 10,10,10,10,10,10;
101  Eigen::Matrix<double,6,1> xd_star = p_gains.cwiseProduct(err);
102
103  main_task->setTargetVelocity(xd_star);
104  velocity_problem->update(m->opt.timestep);
105
106  if (velocity_problem->solutionFound())
107  {
108    sendJointVelocity(velocity_problem->getJointVelocityCommand());    
109  }
110}
111 
112};
113
114int main(int argc, const char** argv) {
115  MujocoQontrol mujoco_qontrol;
116  Qontrol::Log::Logger::parseArgv(argc, argv);
117
118  mjvCamera cam;
119  mjv_defaultCamera(&cam);
120
121  mjvOption opt;
122  mjv_defaultOption(&opt);
123
124  mjvPerturb pert;
125  mjv_defaultPerturb(&pert);
126
127  // simulate object encapsulates the UI
128  auto sim = std::make_unique<mj::Simulate>(
129      std::make_unique<mj::GlfwAdapter>(),
130      &cam, &opt, &pert, /* is_passive = */ false
131  );
132
133  std::string robot = argv[1];
134  std::string mujoco_model = "./resources/"+robot+"/scene_wallconstraint.xml";
135  mujoco_qontrol.resource_path = "./resources/"+robot+"/";
136
137  // start physics thread
138  std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_model.c_str());
139
140  // start simulation UI loop (blocking call)
141  sim->RenderLoop();
142  physicsthreadhandle.join();
143
144  return 0;
145}

—

Explanation of the code

Full Code:

You can find the source code of this example here.

  1#!/usr/bin/env python3
  2"""
  3Interactive Velocity Control with Wall Constraint and MuJoCo
  4
  5This example demonstrates:
  6- Loading a robot model in both MuJoCo and Qontrol
  7- Joint velocity control with Cartesian velocity tracking
  8- Cartesian plane constraint (wall constraint)
  9- Interactive mocap target control
 10
 11The robot will track the mocap target while respecting a plane constraint
 12that prevents the end-effector from crossing a virtual wall.
 13
 14Usage:
 15    python velocity_qontrol_wall_constraint_interactive.py <robot_name>
 16    
 17Example:
 18    python velocity_qontrol_wall_constraint_interactive.py panda
 19    
 20Controls:
 21- Use the MuJoCo viewer to move the mocap body (red sphere)
 22- The robot end-effector will track the mocap target
 23- The robot will avoid crossing the virtual wall plane
 24- Press ESC to exit
 25"""
 26
 27import numpy as np
 28import qontrol
 29import pinocchio as pin
 30import argparse
 31import os
 32import time
 33from mujoco_helper import MujocoSimulator, compute_log6_error
 34
 35
 36class QontrolVelocityControllerWithWall:
 37    """Velocity controller with wall constraint using Qontrol"""
 38    
 39    def __init__(self, robot_name: str, resource_path: str, mujoco_sim: MujocoSimulator):
 40        """
 41        Initialize the Qontrol velocity controller with wall constraint
 42        
 43        Args:
 44            robot_name: Name of the robot (e.g., 'panda')
 45            resource_path: Path to robot resources folder
 46            mujoco_sim: MuJoCo simulator instance
 47        """
 48        self.robot_name = robot_name
 49        self.resource_path = resource_path
 50        self.sim = mujoco_sim
 51        
 52        # =================================================================
 53        # Qontrol Setup - Robot Model and QP Problem
 54        # =================================================================
 55        urdf_path = os.path.join(resource_path, "robot.urdf")
 56        if not os.path.exists(urdf_path):
 57            raise FileNotFoundError(f"URDF file not found: {urdf_path}")
 58        
 59        print(f"Loading Qontrol model from: {urdf_path}")
 60        self.qontrol_model = qontrol.RobotModel.load_from_file(urdf_path)
 61        self.ndof = self.qontrol_model.get_nr_of_degrees_of_freedom()
 62        print(f"Robot has {self.ndof} degrees of freedom")
 63        
 64        # Create QP solver
 65        self.solver = qontrol.create_qpmad_solver()
 66        
 67        # Create velocity-level problem
 68        self.velocity_problem = qontrol.JointVelocityProblem(self.qontrol_model, self.solver)
 69        
 70        # Add Cartesian velocity task for end-effector tracking
 71        self.main_task = self.velocity_problem.task_set.add_cartesian_velocity("MainTask", 1.0)
 72        
 73        # Add joint velocity regularization (minimize joint velocities)
 74        self.regularization_task = self.velocity_problem.task_set.add_joint_velocity("RegularizationTask", 1e-5)
 75        
 76        # Add constraints
 77        self.joint_config_constraint = self.velocity_problem.constraint_set.add_joint_configuration("JointConfigurationConstraint")
 78        self.joint_vel_constraint = self.velocity_problem.constraint_set.add_joint_velocity("JointVelocityConstraint")
 79        
 80        # Add wall constraint (Cartesian plane constraint)
 81        self.wall_constraint = self.velocity_problem.constraint_set.add_cartesian_plane("WallConstraint")
 82        
 83        # Robot state object
 84        self.robot_state = qontrol.RobotState()
 85        self.robot_state.resize(self.ndof)
 86        
 87        # Get tip and root frame names
 88        self.tip_frame = self.qontrol_model.get_tip_frame_name()
 89        self.root_frame = self.qontrol_model.get_root_frame_name()
 90        print(f"Controlling frame: {self.tip_frame}")
 91        print(f"Root frame: {self.root_frame}")
 92        
 93        # Get initial joint state and update model
 94        qpos, qvel = self.sim.get_joint_state()
 95        self.robot_state.joint_position = qpos
 96        self.robot_state.joint_velocity = qvel
 97        self.qontrol_model.set_robot_state(self.robot_state)
 98        
 99        # =================================================================
100        # Wall Constraint Setup
101        # =================================================================
102        # Define wall plane: u is the normal vector, point is a point on the plane
103        # Wall prevents end-effector from moving in the -x direction of the root frame
104        
105        # Get initial tip and root poses
106        initial_tip_matrix = self.qontrol_model.get_frame_pose(self.tip_frame)
107        initial_root_matrix = self.qontrol_model.get_frame_pose(self.root_frame)
108        
109        # Wall normal is -x direction of root frame
110        u_vector = -initial_root_matrix[:3, 0]  # -x axis (first column)
111        self.u = u_vector.reshape(1, 3)  # Shape (1, 3) for constraint
112        
113        # Wall is located 10cm in front of initial tip position in the u direction
114        initial_tip_pos = initial_tip_matrix[:3, 3]
115        self.point = (initial_tip_pos + 0.1 * u_vector).reshape(1, 3)
116        
117        print(f"Wall normal (u): {self.u}")
118        print(f"Wall point: {self.point}")
119        
120        # Set wall constraint parameters
121        self.wall_constraint.set_horizon(10)
122        self.wall_constraint.set_plane(self.u, self.point)
123        
124        # Initialize mocap to current end-effector pose
125        self.sim.init_mocap_to_frame(self.tip_frame)
126        
127        print("dt:", self.sim.dt)
128       
129        # =================================================================
130        # Control Parameters
131        # =================================================================
132        self.p_gains = np.array([10.0, 10.0, 10.0, 10.0, 10.0, 10.0])  # [position, orientation] gains
133    
134    def update(self):
135        """Main control update: compute and apply joint velocity commands"""
136        # 1. Update Qontrol model with current state from simulation
137        qpos, qvel = self.sim.get_joint_state()
138        self.robot_state.joint_position = qpos
139        self.robot_state.joint_velocity = qvel
140        self.qontrol_model.set_robot_state(self.robot_state)
141        
142        # 2. Get target and current poses
143        target_se3 = self.sim.get_mocap_pose_se3()
144        current_ee_matrix = self.qontrol_model.get_frame_pose(self.tip_frame)
145        current_se3 = pin.SE3(current_ee_matrix[:3, :3], current_ee_matrix[:3, 3])
146        
147        # 3. Compute SE(3) log6 error
148        cartesian_error = compute_log6_error(target_se3, current_se3)
149        
150        # 4. Compute desired Cartesian velocity (P control)
151        desired_velocity = self.p_gains * cartesian_error
152        
153        # 5. Solve QP for joint velocities
154        self.main_task.set_target_velocity(desired_velocity)
155        self.velocity_problem.update(self.sim.dt)
156        
157        # 6. Apply velocities to simulation
158        if self.velocity_problem.solution_found():
159            joint_velocities = self.velocity_problem.get_joint_velocity_command()
160            self.sim.set_joint_velocities(joint_velocities)
161        else:
162            print("Warning: No QP solution found!")
163            self.sim.set_joint_velocities(np.zeros(self.ndof))
164
165
166def main():
167    """Main entry point"""
168    parser = argparse.ArgumentParser(
169        description="Interactive velocity control with wall constraint using MuJoCo and Qontrol",
170        formatter_class=argparse.RawDescriptionHelpFormatter,
171        epilog="""
172Examples:
173  %(prog)s panda
174  %(prog)s ur5
175        """
176    )
177    parser.add_argument("robot", type=str, help="Robot name (e.g., panda)")
178    
179    args = parser.parse_args()
180    
181    # Get resource path
182    script_dir = os.path.dirname(os.path.abspath(__file__))
183    resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
184    
185    if not os.path.exists(resource_path):
186        print(f"Error: Resource path not found: {resource_path}")
187        return 1
188    
189    # MuJoCo scene path
190    scene_path = os.path.join(resource_path, "scene_interactive.xml")
191    if not os.path.exists(scene_path):
192        print(f"Error: Scene file not found: {scene_path}")
193        return 1
194    
195    print(f"Loading MuJoCo scene from: {scene_path}")
196    
197    # Initialize MuJoCo simulator
198    sim = MujocoSimulator(args.robot, resource_path)
199    
200    # Initialize controller
201    controller = QontrolVelocityControllerWithWall(args.robot, resource_path, sim)
202    
203    # Initialize mocap to robot's end-effector pose
204    sim.init_mocap_to_frame(controller.tip_frame)
205    
206    print("\n" + "="*60)
207    print("Starting velocity control with wall constraint simulation")
208    print("Move the red mocap target to control the robot")
209    print("The robot will avoid crossing the virtual wall plane")
210    print("Press ESC to exit")
211    print("="*60 + "\n")
212    
213    try:
214        # Run simulation with controller
215        sim.run_interactive(controller.update)
216    except KeyboardInterrupt:
217        print("\nInterrupted by user")
218    except Exception as e:
219        print(f"\nError during simulation: {e}")
220        import traceback
221        traceback.print_exc()
222        return 1
223    
224    return 0
225
226
227if __name__ == "__main__":
228    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.

On this page