Custom Constraint Interactive

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

\[\begin{split}\begin{array}{ccc}\boldsymbol{\dot{q}}^{opt} = & \underset{\boldsymbol{\dot{q}}}{\mathrm{argmin}} & ||J(\boldsymbol{q})\boldsymbol{\dot{q}} - \boldsymbol{v}^{target} || + \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}(\boldsymbol{\dot{q}}) \leq \boldsymbol{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. In this example the joint velocity constraint is defined as a custom constraint, meaning that the user as to update the constraint manually.

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"

#define MUJOCO_PLUGIN_DIR "mujoco_plugin"


using namespace Qontrol;

class MujocoQontrol : public MujocoSim 
{
public:
//------------------------------------------- simulation -------------------------------------------
std::shared_ptr<Qontrol::Model::RobotModel> model;
std::shared_ptr<Qontrol::JointVelocityProblem> velocity_problem;
std::shared_ptr<Qontrol::Task::CartesianVelocity<Qontrol::ControlOutput::JointVelocity>> main_task;
std::shared_ptr<Qontrol::Constraint::GenericConstraint> custom_constraint;
pinocchio::SE3 init_pose;

Qontrol::RobotState robot_state;

TrajectoryGeneration* traj;
std::string resource_path;
void initController() override
{
  model =
      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
  
  velocity_problem = std::make_shared<Qontrol::JointVelocityProblem>(model);  
  main_task = velocity_problem->task_set->add<Task::CartesianVelocity>("MainTask"); // <-- Based on the template given will implement correct task representation  
  auto regularisation_task = velocity_problem->task_set->add<Task::JointVelocity>("RegularisationTask",1e-5); // <-- Based on the template given will implement correct task representationn

  auto joint_configuration_constraint = velocity_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
  custom_constraint =
      velocity_problem->constraint_set->add("CustomConstraint",model->getNrOfDegreesOfFreedom());

  mju_copy(d->qpos, m->key_qpos, m->nu);
  robot_state.joint_position.resize(model->getNrOfDegreesOfFreedom());
  robot_state.joint_velocity.resize(model->getNrOfDegreesOfFreedom());
  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 = new TrajectoryGeneration(resource_path+"trajectory.csv", m->opt.timestep);
}

void updateController() override
{

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

  pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());
  traj->update();
  pinocchio::SE3 traj_pose(traj->pose.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 << 10,10,10,10,10,10;
  Eigen::Matrix<double,6,1> xd_star = p_gains.cwiseProduct(err);
  main_task->setTargetVelocity(xd_star);
  custom_constraint->setConstraintMatrix(Eigen::MatrixXd::Identity(
  model->getNrOfDegreesOfFreedom(), model->getNrOfDegreesOfFreedom()));
  custom_constraint->setUpperBounds(model->getJointVelocityLimits());
  custom_constraint->setLowerBounds(-model->getJointVelocityLimits());
  velocity_problem->update(m->opt.timestep);

  if (velocity_problem->solutionFound())
  {
    sendJointVelocity(velocity_problem->getJointVelocityCommand());    
  }    
}
 
};

int main(int argc, const char** argv) {
  MujocoQontrol mujoco_qontrol;
  Qontrol::Log::Logger::parseArgv(argc, argv);

  mjvScene scn;
  mjv_defaultScene(&scn);

  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, /* fully_managed = */ true
  );


  std::string robot = argv[1];
  const char* mujoco_model = "./resources/"+robot+"/scene.xml";
  mujoco_qontrol.resource_path = "./resources/"+robot+"/";

  // start physics thread
  std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_model);

  // 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<Qontrol::Model::RobotModel> model;

We use pinocchio for our model library.

std::shared_ptr<Qontrol::JointVelocityProblem> velocity_problem;

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

std::shared_ptr<Qontrol::Task::CartesianVelocity<Qontrol::ControlOutput::JointVelocity>> main_task;

The main task is expressed as a Cartesian velocity task.

std::shared_ptr<Qontrol::Constraint::GenericConstraint> custom_constraint;

Here we declare the custom constraint as a GenericConstraint.

Initialization

void initController() override
{
  model =
      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");

During initialization we instantiate the model with the robot urdf.

std::shared_ptr<Qontrol::JointVelocityProblem> velocity_problem;

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

std::shared_ptr<Qontrol::Task::CartesianVelocity<Qontrol::ControlOutput::JointVelocity>> main_task;
std::shared_ptr<Qontrol::Constraint::GenericConstraint> custom_constraint;
pinocchio::SE3 init_pose;

Qontrol::RobotState robot_state;

TrajectoryGeneration* traj;
std::string resource_path;
void initController() override
{
  model =
      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
  
  velocity_problem = std::make_shared<Qontrol::JointVelocityProblem>(model);  
  main_task = velocity_problem->task_set->add<Task::CartesianVelocity>("MainTask"); // <-- Based on the template given will implement correct task representation  
  auto regularisation_task = velocity_problem->task_set->add<Task::JointVelocity>("RegularisationTask",1e-5); // <-- Based on the template given will implement correct task representationn

We then fill the task set of velocity_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. In this example, the regularisation tasks is defined as a joint veloicty task. Its means that this task will minimize the overall robot joint veloicty.

  auto joint_configuration_constraint = velocity_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");

We then fill the constraint set of velocity_problem with the pre-implemented joint configuration constraint. Each constraint is given a name. This constraint will automatically be updated during the update of Qontrol.

      velocity_problem->constraint_set->add("CustomConstraint",model->getNrOfDegreesOfFreedom());

Then we add our custom constraint. We give it a name to identify it and also the size of the constraint. This size will be used by Qontrol to resize the constraint set.

  robot_state.joint_position.resize(model->getNrOfDegreesOfFreedom());
  robot_state.joint_velocity.resize(model->getNrOfDegreesOfFreedom());
  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);

We create the robot state and fill it with the simulated robot current state.

  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
{

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

  pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());
  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.

  auto err = pinocchio::log6(tipMdes).toVector();
  Eigen::Matrix<double,6,1> p_gains;
  p_gains << 10,10,10,10,10,10;
  Eigen::Matrix<double,6,1> xd_star = p_gains.cwiseProduct(err);

We then compute the desired Cartesian velocity using a simple proportionnal 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 are the proportionnal gains of the controller.

  main_task->setTargetVelocity(xd_star);

The desired Cartesian velocity is then fed to the main task.

  custom_constraint->setConstraintMatrix(Eigen::MatrixXd::Identity(
  model->getNrOfDegreesOfFreedom(), model->getNrOfDegreesOfFreedom()));
  custom_constraint->setUpperBounds(model->getJointVelocityLimits());
  custom_constraint->setLowerBounds(-model->getJointVelocityLimits());

Here we update the custom constraint. We first set the constraint matrix A to the identity matrix. Then we set the lower and upper bounds of the constraint with the robot joint velocity limits.

  velocity_problem->update(m->opt.timestep);

  if (velocity_problem->solutionFound())
  {
    sendJointVelocity(velocity_problem->getJointVelocityCommand());    
  }    
}
 
};

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

  mjvScene scn;
  mjv_defaultScene(&scn);

  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, /* fully_managed = */ true
  );


  std::string robot = argv[1];
  const char* mujoco_model = "./resources/"+robot+"/scene.xml";
  mujoco_qontrol.resource_path = "./resources/"+robot+"/";

  // start physics thread
  std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_model);

  // 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
"""
Custom Constraint Configuration Example

This example demonstrates how to manually configure constraints by setting
constraint matrix and bounds directly, while using pre-implemented tasks.

Compare with velocity_control_interactive.py which uses pre-implemented constraints.

Usage:
    python custom_constraint_interactive.py <robot_name>
    
Example:
    python custom_constraint_interactive.py panda
"""

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


class CustomConstraintController:
    """Velocity controller with custom manual constraint configuration"""
    
    def __init__(self, robot_name: str, resource_path: str, mujoco_sim: MujocoSimulator):
        """
        Initialize the controller
        
        Args:
            robot_name: Name of the robot
            resource_path: Path to robot resources
            mujoco_sim: MuJoCo simulator instance
        """
        self.robot_name = robot_name
        self.sim = mujoco_sim
        self.model = mujoco_sim.model
        self.data = mujoco_sim.data
        
        # Load robot model with Qontrol
        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 velocity-level QP problem
        self.velocity_problem = qontrol.JointVelocityProblem(self.qontrol_model, self.solver)
        
        # Create velocity-level QP problem
        self.velocity_problem = qontrol.JointVelocityProblem(self.qontrol_model, self.solver)
        
        # Add task for Cartesian velocity control (using pre-implemented task)
        self.main_task = self.velocity_problem.task_set.add_cartesian_velocity("MainTask", 1.0)
        
        # Add regularization task for joint velocity minimization (using pre-implemented task)
        self.regularization_task = self.velocity_problem.task_set.add_joint_velocity("Regularization", 1e-5)
        
        # Add configuration constraint using pre-implemented constraint
        self.joint_config_constraint = (
            self.velocity_problem.constraint_set.add_joint_configuration(
                "JointConfigurationConstraint"
            )
        )
        
        # Set horizon for position constraint (timesteps to joint limits)
        self.joint_config_constraint.set_horizon(15)
        
        # Add CUSTOM velocity constraint (manual configuration)
        # This replicates the joint velocity constraint but configured manually
        self.custom_velocity_constraint = self.velocity_problem.constraint_set.add(
            "CustomVelocityConstraint",
            self.ndof  # Dimension of the constraint
        )
        
        # Velocity limits will be set manually each iteration
        # (In practice, you would use add_joint_velocity, but this shows how to do it manually)
        
        # 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}")
        
        # Velocity limits (rad/s)
        self.velocity_max = 1.0
        
        # Control timestep
        self.dt = self.model.opt.timestep
        
        # Control gains (P controller)
        self.p_gains = np.array([10.0, 10.0, 10.0, 10.0, 10.0, 10.0])  # [position, orientation] gains
        
        print(f"Controller initialized for {robot_name}")
        print(f"  DOF: {self.ndof}")
        print(f"  Control timestep: {self.dt} s")
        print(f"  Using CUSTOM velocity constraint (manual configuration)")
    
    def update(self):
        """
        Compute control command
        
        Args:
            q: Joint positions
            dq: Joint velocities
            
        Returns:
            Joint velocity commands
        """
        # Update robot state
        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 end-effector pose
        current_se3 = self.sim.get_frame_pose_se3(self.tip_frame)
        
        # Get mocap target pose
        target_se3 = self.sim.get_mocap_pose_se3()
        
        # Compute error in SE(3) (log map)
        error = compute_log6_error(target_se3, current_se3)
        
        # Desired velocity (P controller in SE(3))
        desired_velocity = self.p_gains * error
        
        # Update pre-implemented tasks
        self.main_task.set_target_velocity(desired_velocity)
        self.regularization_task.set_target_velocity(np.zeros(self.ndof))
        
        # Update CUSTOM velocity constraint (MANUAL configuration)
        # This demonstrates the internal workings of constraints
        # The constraint is: lower_bound <= A * dq_cmd <= upper_bound
        # For velocity limits: -velocity_max <= dq_cmd <= velocity_max
        # So A = I (identity matrix)
        constraint_matrix = np.eye(self.ndof)
        lower_bounds = np.full(self.ndof, -self.velocity_max)
        upper_bounds = np.full(self.ndof, self.velocity_max)
        
        # Set constraint manually
        self.custom_velocity_constraint.set_constraint_matrix(constraint_matrix)
        self.custom_velocity_constraint.set_lower_bounds(lower_bounds)
        self.custom_velocity_constraint.set_upper_bounds(upper_bounds)

        self.velocity_problem.update(self.sim.dt)

        # Solve QP
        if self.velocity_problem.solution_found():
            joint_velocities = self.velocity_problem.get_joint_velocity_command()
            
            # Directly set joint velocities in simulation
            self.sim.set_joint_velocities(joint_velocities)
            
        else:
            print("Warning: No QP solution found!")
            # Fallback: set velocities to zero
            self.sim.set_joint_velocities(np.zeros(self.ndof))


def main():
    parser = argparse.ArgumentParser(description="Custom constraint configuration example")
    parser.add_argument("robot", type=str, help="Robot name (e.g., panda, ur5)")
    args = parser.parse_args()
    
    # Set up paths
    script_dir = os.path.dirname(os.path.abspath(__file__))
    resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
    resource_path = os.path.abspath(resource_path)
    
    # Create simulator
    sim = MujocoSimulator(args.robot, resource_path)
    
    # Initialize mocap to end-effector pose
    
    # Create controller
    controller = CustomConstraintController(args.robot, resource_path, sim)

    sim.init_mocap_to_frame(controller.tip_frame)
    
    
    # Run interactive simulation
    sim.run_interactive(
        controller_callback=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  
        This example demonstrates CUSTOM CONSTRAINT configuration."""
        )
    
    print("Simulation ended")


if __name__ == "__main__":
    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.velocity_problem = qontrol.JointVelocityProblem(self.qontrol_model, self.solver)

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

        self.main_task = self.velocity_problem.task_set.add_cartesian_velocity("MainTask", 1.0)

The main task is expressed as a Cartesian velocity task.

        self.custom_velocity_constraint = self.velocity_problem.constraint_set.add(
            "CustomVelocityConstraint",
            self.ndof  # Dimension of the constraint

Here we declare the custom constraint as a GenericConstraint.

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 velocity-level QP problem
        self.velocity_problem = qontrol.JointVelocityProblem(self.qontrol_model, self.solver)

We then fill the task set of velocity_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. In this example, the regularisation tasks is defined as a joint veloicty task. Its means that this task will minimize the overall robot joint veloicty.

        self.main_task = self.velocity_problem.task_set.add_cartesian_velocity("MainTask", 1.0)
        
        # Add regularization task for joint velocity minimization (using pre-implemented task)
        self.regularization_task = self.velocity_problem.task_set.add_joint_velocity("Regularization", 1e-5)

We then fill the constraint set of velocity_problem with the pre-implemented joint configuration constraint. Each constraint is given a name. This constraint will automatically be updated during the update of Qontrol.

        self.joint_config_constraint = (
            self.velocity_problem.constraint_set.add_joint_configuration(
                "JointConfigurationConstraint"
            )
        )
        
        # Set horizon for position constraint (timesteps to joint limits)
        self.joint_config_constraint.set_horizon(15)

Then we add our custom constraint. We give it a name to identify it and also the size of the constraint. This size will be used by Qontrol to resize the constraint set.

        self.custom_velocity_constraint = self.velocity_problem.constraint_set.add(
            "CustomVelocityConstraint",
            self.ndof  # Dimension of the constraint

We create the robot state and fill it with the simulated robot current state.

        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.

        target_se3 = self.sim.get_mocap_pose_se3()

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):
        """
        Compute control command
        
        Args:
            q: Joint positions
            dq: Joint velocities
            
        Returns:
            Joint velocity commands
        """
        # Update robot state
        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 velocity using a simple proportionnal 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 are the proportionnal gains of the controller.

        error = compute_log6_error(target_se3, current_se3)
        
        # Desired velocity (P controller in SE(3))
        desired_velocity = self.p_gains * error

The desired Cartesian velocity is then fed to the main task.

        self.main_task.set_target_velocity(desired_velocity)

Here we update the custom constraint. We first set the constraint matrix A to the identity matrix. Then we set the lower and upper bounds of the constraint with the robot joint velocity limits.

        constraint_matrix = np.eye(self.ndof)
        lower_bounds = np.full(self.ndof, -self.velocity_max)
        upper_bounds = np.full(self.ndof, self.velocity_max)
        
        # Set constraint manually
        self.custom_velocity_constraint.set_constraint_matrix(constraint_matrix)
        self.custom_velocity_constraint.set_lower_bounds(lower_bounds)
        self.custom_velocity_constraint.set_upper_bounds(upper_bounds)

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.velocity_problem.update(self.sim.dt)

        # Solve QP
        if self.velocity_problem.solution_found():
            joint_velocities = self.velocity_problem.get_joint_velocity_command()
            
            # Directly set joint velocities in simulation
            self.sim.set_joint_velocities(joint_velocities)

Main function

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

def main():
    parser = argparse.ArgumentParser(description="Custom constraint configuration example")
    parser.add_argument("robot", type=str, help="Robot name (e.g., panda, ur5)")
    args = parser.parse_args()
    
    # Set up paths
    script_dir = os.path.dirname(os.path.abspath(__file__))
    resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
    resource_path = os.path.abspath(resource_path)
    
    # Create simulator
    sim = MujocoSimulator(args.robot, resource_path)
    
    # Initialize mocap to end-effector pose
    
    # Create controller
    controller = CustomConstraintController(args.robot, resource_path, sim)

    sim.init_mocap_to_frame(controller.tip_frame)
    
    
    # Run interactive simulation
    sim.run_interactive(
        controller_callback=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  
        This example demonstrates CUSTOM CONSTRAINT configuration."""
        )
    
    print("Simulation ended")