Slack Interactive#

This example demonstrates how to use slack variables.

  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<Model::RobotModel> model;
 26std::shared_ptr<JointTorqueProblem> torque_problem;
 27std::shared_ptr<Task::CartesianAcceleration<ControlOutput::JointTorque>> main_task;
 28std::shared_ptr<Task::JointTorque<ControlOutput::JointTorque>> regularisation_task;
 29std::shared_ptr<Constraint::JointTorque<ControlOutput::JointTorque>> joint_torque_constraint;
 30std::shared_ptr<Constraint::JointConfiguration<ControlOutput::JointTorque>> joint_configuration_constraint;
 31Qontrol::RobotState robot_state;
 32std::string resource_path;
 33
 34void initController() override
 35{
 36  model =
 37      Model::RobotModel::loadModelFromFile(resource_path+"robot.urdf");
 38  
 39  const int ndof = model->getNrOfDegreesOfFreedom();
 40  
 41  torque_problem = std::make_shared<Qontrol::JointTorqueProblem>(model);  
 42  main_task = torque_problem->task_set->add<Task::CartesianAcceleration>("MainTask"); 
 43  regularisation_task = torque_problem->task_set->add<Task::JointTorque>("RegularisationTask",1e-5); 
 44
 45  joint_configuration_constraint = torque_problem->constraint_set->add<Constraint::JointConfiguration>("JointConfigurationConstraint");
 46  auto joint_velocity_constraint = torque_problem->constraint_set->add<Constraint::JointVelocity>("JointVelocityConstraint");
 47  joint_torque_constraint = torque_problem->constraint_set->add<Constraint::JointTorque>("JointTorqueConstraint");
 48
 49  Eigen::VectorXd slack_max = 0.1 * Eigen::VectorXd::Ones(ndof);
 50  torque_problem->slack_set->add(joint_configuration_constraint,-slack_max,slack_max);
 51  torque_problem->slack_set->add(joint_torque_constraint, -slack_max, slack_max);
 52
 53  mju_copy(d->qpos, m->key_qpos, m->nu  );
 54  robot_state.joint_position.resize(ndof);
 55  robot_state.joint_velocity.resize(ndof);
 56  
 57  for (int i=0; i<ndof ; ++i)
 58  {
 59    robot_state.joint_position[i] = d->qpos[i];
 60    robot_state.joint_velocity[i] = d->qvel[i];
 61  }
 62  model->setRobotState(robot_state);
 63
 64  initialiseMocapPose(model->getFramePose(model->getTipFrameName()).matrix());
 65}
 66
 67void updateController() override
 68{
 69  const int ndof = model->getNrOfDegreesOfFreedom();
 70  
 71  for (int i=0; i<ndof ; ++i)
 72  {
 73    robot_state.joint_position[i] = d->qpos[i];
 74    robot_state.joint_velocity[i] = d->qvel[i];
 75  }
 76  model->setRobotState(robot_state);
 77
 78  Eigen::Affine3d target_pose;
 79  target_pose.translation() = Eigen::Vector3d(d->mocap_pos[0],d->mocap_pos[1],d->mocap_pos[2]);
 80  target_pose.linear() = Eigen::Quaterniond(d->mocap_quat[0],d->mocap_quat[1],d->mocap_quat[2],d->mocap_quat[3]).toRotationMatrix();
 81  pinocchio::SE3 traj_pose(target_pose.matrix());
 82  
 83  
 84  pinocchio::SE3 current_pose(model->getFramePose(model->getTipFrameName()).matrix());
 85  const pinocchio::SE3 tipMdes = current_pose.actInv(traj_pose);
 86  auto err = pinocchio::log6(tipMdes).toVector();
 87
 88  Eigen::Matrix<double, 6, 1> p_gains;
 89  p_gains << 1000, 1000, 1000, 1000, 1000, 1000;
 90
 91  Eigen::Matrix<double, 6, 1> d_gains = 2.0 * p_gains.cwiseSqrt();
 92  Eigen::Matrix<double, 6, 1> xdd_star =
 93      p_gains.cwiseProduct(err) +
 94      d_gains.cwiseProduct(- model->getFrameVelocity(model->getTipFrameName())) ;
 95
 96  main_task->setTargetAcceleration(xdd_star);
 97  regularisation_task->setTargetTorque(model->getJointGravityTorques() -
 98                                      robot_state.joint_velocity);
 99  regularisation_task->setWeightingMatrix(
100      model->getInverseJointInertiaMatrix());
101  torque_problem->update(m->opt.timestep);
102
103  if (torque_problem->solutionFound())
104  {
105    sendJointTorque(torque_problem->getJointTorqueCommand());
106  }
107}
108};
109
110int main(int argc, const char** argv) {
111  MujocoQontrol mujoco_qontrol;
112  Qontrol::Log::Logger::parseArgv(argc, argv);
113
114  mjvCamera cam;
115  mjv_defaultCamera(&cam);
116
117  mjvOption opt;
118  mjv_defaultOption(&opt);
119
120  mjvPerturb pert;
121  mjv_defaultPerturb(&pert);
122
123  // simulate object encapsulates the UI
124  auto sim = std::make_unique<mj::Simulate>(
125      std::make_unique<mj::GlfwAdapter>(),
126      &cam, &opt, &pert, /* is_passive = */ false
127  );
128
129  std::string robot = argv[1];
130  std::string mujoco_scene = "./resources/"+robot+"/scene_interactive.xml";
131  mujoco_qontrol.resource_path = "./resources/"+robot+"/";
132  // start physics thread
133  std::thread physicsthreadhandle( &MujocoQontrol::PhysicsThread, mujoco_qontrol, sim.get(), mujoco_scene.c_str());
134
135  // start simulation UI loop (blocking call)
136  sim->RenderLoop();
137  physicsthreadhandle.join();
138
139  return 0;
140}

  1#!/usr/bin/env python3
  2"""
  3Slack Variables Example
  4
  5This example demonstrates the use of slack variables to relax constraints
  6and achieve feasibility when the original QP problem would be infeasible.
  7
  8Based on C++ example: Qontrol_slack.cpp
  9
 10Usage:
 11    python slack_interactive.py <robot_name>
 12    
 13Example:
 14    python slack_interactive.py panda
 15"""
 16
 17import numpy as np
 18import qontrol
 19import pinocchio as pin
 20import argparse
 21import os
 22from mujoco_helper import MujocoSimulator, compute_log6_error
 23
 24
 25class SlackController:
 26    """Torque controller with slack variables on constraints"""
 27    
 28    def __init__(self, robot_name: str, resource_path: str, mujoco_sim: MujocoSimulator):
 29        """
 30        Initialize the controller
 31        
 32        Args:
 33            robot_name: Name of the robot
 34            resource_path: Path to robot resources
 35            mujoco_sim: MuJoCo simulator instance
 36        """
 37        self.robot_name = robot_name
 38        self.mujoco_sim = mujoco_sim
 39        self.sim = mujoco_sim  # Alias for convenience
 40        self.model = mujoco_sim.model
 41        self.data = mujoco_sim.data
 42        
 43        # Load robot model with Qontrol
 44        urdf_path = os.path.join(resource_path, "robot.urdf")
 45        if not os.path.exists(urdf_path):
 46            raise FileNotFoundError(f"URDF file not found: {urdf_path}")
 47        
 48        print(f"Loading Qontrol model from: {urdf_path}")
 49        self.qontrol_model = qontrol.RobotModel.load_from_file(urdf_path)
 50        self.ndof = self.qontrol_model.get_nr_of_degrees_of_freedom()
 51        print(f"Robot has {self.ndof} degrees of freedom")
 52        
 53        # Create QP solver
 54        self.solver = qontrol.create_qpmad_solver()
 55        
 56        # Create torque-level QP problem (for acceleration control)
 57        self.torque_problem = qontrol.JointTorqueProblem(self.qontrol_model, self.solver)
 58        
 59        # Create torque-level QP problem (for acceleration control)
 60        self.torque_problem = qontrol.JointTorqueProblem(self.qontrol_model, self.solver)
 61        
 62        # Add task for Cartesian acceleration control
 63        self.main_task = self.torque_problem.task_set.add_cartesian_acceleration("MainTask", 1.0)
 64        
 65        # Add regularization task for joint torque minimization
 66        self.regularization_task = self.torque_problem.task_set.add_joint_torque("TorqueRegularization", 1e-5)
 67        
 68        # Add configuration constraint
 69        self.joint_config_constraint = (
 70            self.torque_problem.constraint_set.add_joint_configuration(
 71                "JointConfigurationConstraint"
 72            )
 73        )
 74        
 75        self.joint_vel_constraint = self.torque_problem.constraint_set.add_joint_velocity("JointVelocityConstraint")
 76        
 77        # Add torque constraint
 78        self.joint_torque_constraint = (
 79            self.torque_problem.constraint_set.add_joint_torque(
 80                "JointTorqueConstraint"
 81            )
 82        )
 83        
 84        # Add SLACK VARIABLES to constraints
 85        # Slack variables allow small violations of constraints to maintain feasibility
 86        slack_max = 0.1  # Maximum allowed violation per joint
 87        
 88        # Create slack bounds as numpy arrays (dimension = ndof for both constraints)
 89        min_slack = -slack_max * np.ones(self.ndof)
 90        max_slack = slack_max * np.ones(self.ndof)
 91        
 92        self.torque_problem.slack_set.add(
 93            self.joint_config_constraint,
 94            min_slack,  # Lower slack bound (array)
 95            max_slack   # Upper slack bound (array)
 96        )
 97        
 98        self.torque_problem.slack_set.add(
 99            self.joint_torque_constraint,
100            min_slack,  # Lower slack bound (array)
101            max_slack   # Upper slack bound (array)
102        )
103        
104        print(f"Added slack variables to constraints (max violation: {slack_max})")
105        
106        # Robot state object
107        self.robot_state = qontrol.RobotState()
108        self.robot_state.resize(self.ndof)
109        
110        # Get tip frame name
111        self.tip_frame = self.qontrol_model.get_tip_frame_name()
112        print(f"Controlling frame: {self.tip_frame}")
113        
114        # PD gains for Cartesian acceleration control (arrays for element-wise multiplication)
115        self.kp = np.array([1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0])  # [position, orientation]
116        self.kd = 2.0 * np.sqrt(self.kp)  # Critical damping
117        
118        print(f"Controller initialized for {robot_name}")
119        print(f"  DOF: {self.ndof}")
120        print(f"  PD gains: Kp={self.kp}, Kd={self.kd}")
121        print(f"  Using slack variables for constraint relaxation")
122    
123    def update(self):
124        """
125        Main control update: compute and apply joint torques
126        """
127        # Update robot state
128        qpos, qvel = self.sim.get_joint_state()
129        self.robot_state.joint_position = qpos
130        self.robot_state.joint_velocity = qvel
131        self.qontrol_model.set_robot_state(self.robot_state)
132        
133        # Get end-effector pose and velocity
134        current_se3 = self.sim.get_frame_pose_se3(self.tip_frame)
135        ee_velocity = self.qontrol_model.get_frame_velocity(self.tip_frame)
136        
137        # Get mocap target pose
138        target_se3 = self.sim.get_mocap_pose_se3()
139        
140        # Compute error in SE(3) (log map)
141        error = compute_log6_error(target_se3, current_se3)
142        
143        # Desired acceleration (PD controller in SE(3))
144        desired_acceleration = self.kp * error - self.kd * ee_velocity
145        
146        # Update tasks
147        self.main_task.set_target_acceleration(desired_acceleration)
148             
149        # Set regularization task target torques
150        g = self.qontrol_model.get_joint_gravity_torques()
151        target_torques = g - qvel
152        self.regularization_task.set_target_torque(target_torques)
153        
154        # Set weighting matrix for regularization
155        M_inv = self.qontrol_model.get_inverse_joint_inertia_matrix()
156        self.regularization_task.set_weighting_matrix(M_inv)
157
158        # Solve QP (with slack variables)
159        self.torque_problem.update(self.sim.dt)
160        
161        if self.torque_problem.solution_found():
162            # Use get_control_solution() which returns only joint torques (ndof elements)
163            # without slack variables. This is the proper way to extract control commands.
164            joint_torques = self.torque_problem.get_control_solution()
165            self.sim.apply_torques(joint_torques)
166        else:
167            print("Warning: No QP solution found!")
168            self.sim.apply_torques(np.zeros(self.ndof))
169
170
171def main():
172    parser = argparse.ArgumentParser(description="Slack variables example")
173    parser.add_argument("robot", type=str, help="Robot name (e.g., panda, ur5)")
174    args = parser.parse_args()
175    
176    # Set up paths
177    script_dir = os.path.dirname(os.path.abspath(__file__))
178    resource_path = os.path.join(script_dir, "..", "..", "..", "examples", "resources", args.robot)
179    resource_path = os.path.abspath(resource_path)
180    
181    # Create simulator
182    sim = MujocoSimulator(args.robot, resource_path, "scene_interactive.xml")
183    
184    # Initialize mocap to end-effector pose
185    sim.init_mocap_to_frame("end_effector")
186    
187    # Create controller
188    controller = SlackController(args.robot, resource_path, sim)
189    
190    # Initialize mocap after controller is created
191    sim.init_mocap_to_frame(controller.tip_frame)
192    
193    # Run interactive simulation
194    print("\nStarting interactive simulation...")
195    print("Note: Slack variables allow small constraint violations for feasibility")
196    sim.run_interactive(
197        controller_callback=controller.update,
198        instructions="""Controls:
199  - Drag the red sphere (mocap body) to move the target
200  - The robot will track the mocap target using SLACK VARIABLES
201  - Press ESC or close window to exit
202  - Double-click to select mocap body
203  
204This example demonstrates SLACK VARIABLES for constraint relaxation."""
205    )
206    
207    print("Simulation ended")
208
209
210if __name__ == "__main__":
211    main()