Skip to content
Switch branches/tags

Latest commit


Git stats


Failed to load latest commit information.
Latest commit message
Commit time

Constrained Manipulability

Constrained Manipulability is a library used to compute and vizualize a robot's constrained capacities.


  • Compute a robot's constrained allowable Cartesian motions due to collision avoidance constraints and joint limits
  • Compute a robot's constrained manipulability polytope due to dangerfield constraints
  • Static functions that allow the above quantities to be used in optimization algorithms for collision free trajectory optimization



Install instructions

  1. Clone repo into your current workspace

    cd catkin_ws/src
    git clone
    cd ..
    catkin build


Demos can be launched for a robot, using the provided test file

roslaunch constrained_manipulability abstract_robot.launch root_link:=<your root link> root_link:=<your end effector>  config:=<your scene stored in .yaml>

There are several example scenes in scene_config folder. This has been tested with Rethink's Sawyer, Universal robot (requires ur-description), and Frannka Panda (requires franka-description):

 roslaunch constrained_manipulability sawyer_test.launch
 roslaunch constrained_manipulability ur_test.launch
 roslaunch constrained_manipulability franka_test.launch

Launching shrinking, (changing the linearization limits) polytope tests

 roslaunch constrained_manipulability shrinking_polytope_test.launch "show_debug_statements":="true" "mp_display":="false"
 rosrun constrained_manipulability 

Launching a test using a sample octomap

roslaunch constrained_manipulability cm_octomap_test.launch

Launching a test using an ik example

First launch the octomap and the polytope server

roslaunch constrained_manipulability cm_server_test.launch

First launch sample IK file. This calls the polytope server then uses the convex constraints in an optimization routine

rosrun constrained_manipulability

We use cvxpy as the solver cost function

cost=cp.sum_squares(jacobian@dq - dx)

subject to

A@dq<= b

where dx is a desired change in position for instance and input twist. the input twist can be generated by a teleop command

rosrun teleop_twist_keyboard _speed:=0.03 _turn:=0.014


The polytopes are calculated by obtaining the minimum distance from each link on the robot to objects in the collision world. FCL is used to compute these distance and access via the interface package robot_collision_checking. The volume of the polytopes in Cartesian space is return from the get functions as follows:

    double getConstrainedAllowableMotionPolytope ( const sensor_msgs::JointState & joint_states,
            Eigen::MatrixXd & AHrep,
            Eigen::VectorXd & bhrep,
            bool show_polytope,
            std::vector<double>  color_pts= {0.0,0.0,0.5,1.0},
            std::vector<double>  color_line= {0.0,0.0,1.0,0.8} );

AHrep and bhrep represent the joint space polytope constraints i.e.

AHrep*dq <= bhrep

conversion from H-representation to V-representation is achieved using the Double Description method accessed via eigen-cddlib. Static functions are also available for all the different polytopes where, both the urdf model, the KDL chain and the object set must be explicity passed:

    static double getConstrainedAllowableMotionPolytope ( KDL::Chain &  chain,
            urdf::Model & model,
            FCLObjectSet objects,
            const sensor_msgs::JointState & joint_states,
            Eigen::MatrixXd & AHrep,
            Eigen::VectorXd & bhrep,
            double linearization_limit=0.1,
            double distance_threshold=0.3 );

Different Polytopes are available more information about allowable motion polytope is available here Optimization-Based Human-in-the-Loop Manipulation Using Joint Space Polytopes, Long et al 2019 more information about the constrained velocity polytope is available here Evaluating Robot Manipulability in Constrained Environments by Velocity Polytope Reduction Long et al 2018.


The main object is initialized as follows:

    ConstrainedManipulability ( ros::NodeHandle nh,
                                std::string root,
                                std::string tip,
				std::string robot_description="robot_description",
                                double distance_threshold=0.3,
                                double linearization_limit=0.1,
                                double dangerfield=10

It reads a kinematic chain from the parameter server starting a the root and running until the tip. The joint position and velocity limits are also read and are used to define the different polytopes. The collision model of the report is also pared. A collision world is maintained, objects can be added using ros_shape_msgs and Eigen::Affine3d

bool addCollisionObject ( const shape_msgs::SolidPrimitive & s1,
                              const  Eigen::Affine3d  & wT1,unsigned int object_id );
bool addCollisionObject ( const shape_msgs::Mesh & s1,
                              const  Eigen::Affine3d  & wT1,unsigned int object_id );

Objects are removed by id.

bool removeCollisionObject (unsigned int object_id );


Octomaps can now be used to as objects, the class subcribes to the "constrained_manipulability/octomap_full" topic

Octomaps as collision object


A video showing the applications of the constrained allowable motion polytope is available here. A video showing the uses of the constrained velocity polytope for humanoid robots can be seen here and here.

1. Motion planning

Planning collision free paths can be achieved by maximizing the volume of the allowable motion polytope, however since no analytical gradient is available this is typically slower than other motion planning algorithms. Nevertheless, since the polytopes are returned they can be used for fast on-line inverse kinematic solutions and guard teleoperation.

Planning collision free path by maximizing volume

2. Guarded tele-operation

The polytopes are convex constraints that represent feasible configuration for the whole robot. By respecting them a guaranteed feasible inverse kinematic solution can be obtained very quickly, this can be useful for generating virtual fixtures for teleoperation tasks. The polytope can be vizualized (in red below) showing an operator the Cartesian motions available at all times due to joint limits, kinematic constraints and obstacles in the workspace. The original polytope is shown below in blue/

Comparison of UR's allowable motions with and without constraints

3. Workspace Analysis

By evaluating the volume of the CMP at points in the workspace, a reachability map can be obtained see this video

Planar 2DOF workspace analysis Humanoid workspace analysis


If you use this package, please cite either :

  title={Optimization-Based Human-in-the-Loop Manipulation  Using Joint Space Polytopes},
  author={{Philip Long, Tar{\i}k Kele\c{s}temur, Aykut \"{O}zg\"{u}n \"{O}nol and Ta\c{s}k{\i}n Pad{\i}r },
  booktitle={2019 IEEE International Conference on Robotics and Automation (ICRA)},


author={P. {Long} and T. {Padir}},
booktitle={2018 IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids)},
title={Evaluating Robot Manipulability in Constrained Environments by Velocity Polytope Reduction},


  • Get working with Octomap


Constrained Manipulability is a library used to compute and visualize a robot's capacities in constrained environments.



No releases published


No packages published