Constrained Manipulability

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

Features

• 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
• A ROS 2 interface that allows the above quantities to be used in IK optimization algorithms for collision-free trajectory optimization

Implementations for the following ROS distros are available on different git branches of this repository:

• ROS 1 Noetic on the noetic-devel branch,
• ROS 2 Humble on the default master branch.

The package was developed and tested on Ubuntu 20.04 for Noetic and Ubuntu 22.04 for Humble. Nevertheless, any operating systems supported by the ROS distros available to this package should also work.

We recommend using the default ROS 2 Humble implementation on master, as this continues to have ongoing support.

Installation

The following instructions will enable you to build the constrained_manipulability package within a ROS 2 workspace using colcon build (or catkin build if using ROS 1).

Dependencies

To install all the following dependencies, consider referring to the Dockerfile for a summary of commands:

• ROS 2 Humble
  • tf2_eigen
  • geometric_shapes
  • pcl_ros
  • kdl_parser
• Eigen 3
• robot_collision_checking
• eigen-cddlib
• octomap_filter

Install Instructions

Once all the library dependencies have been installed, you can clone and build the constrained_manipulability package into your ROS workspace’s src directory (set to the appropriate ROS distro implementation), e.g.:

mkdir -p ros2_ws/src
cd ros2_ws/src
git clone --branch <branch-name> https://github.com/philip-long/constrained_manipulability.git
cd ..
colcon build --symlink-install

Where <branch-name> is likely either master or noetic-devel.

The most tested route for installation is on your own local machine, however the next section describes an alternative using Docker.

Alternative - Docker Image

If you instead wish to explore the package in a Docker image, there is a Dockerfile available. After installing docker, simply clone the repository or download the Dockerfile and then run:

docker build --tag 'constrained_manipulability' . && docker run -it --rm --privileged --network host -e NVIDIA_VISIBLE_DEVICES=all -e NVIDIA_DRIVER_CAPABILITIES=all --env="DISPLAY" --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" 'constrained_manipulability' bash

The additional NVIDIA commands are to enable RViz to run properly (assuming you use NVIDIA graphics card drivers).

To launch examples in the Docker image that require a GUI, like those that launch an RViz display, you should also run this command in your local host’s terminal (i.e., not in the Docker image):

xhost +local:docker

Note: This command allows Docker containers to connect to the host’s X server, which is by default restricted for security reasons. As this command potentially opens the X server to unauthorized access, please also consider alternatives or reverting the changes after use:

xhost -local:docker

The Docker image is preconfigured with all the core libraries for constrained_manipulability (e.g., robot_collision_checking and its dependencies, octomap_filter, eigen-cddlib, etc.). After building the image and starting the container, a ROS workspace ros2_ws will have been created and built with all the necessary dependencies. The final step is to source ros2_ws before testing out the package (from within the docker):

source /ros2_ws/install/setup.bash 

If you wish to launch several sessions connected to the same container, run in separate terminals on your host machine:

docker run -it 'constrained_manipulability'

Note: Not all functionality of constrained_manipulability is guaranteed if using a Docker image. In particular, the RViz utilities of interacting with the package have only been briefly tested through this installation route.

Launch Examples

A template robot launch file can be run using abstract_robot.launch.py:

ros2 launch constrained_manipulability abstract_robot.launch.py root:=<your root link> tip:=<your end-effector link>  scene_config:=<your scene config stored in a .yaml file>

However, a robot state description will need to be provided/launched separately. For instance, there are several other complete example robot launches and scenes in config folder.

The launch file for the UR3e can be run as:

ros2 launch constrained_manipulability abstract_ur3e.launch.py

And for the Kinova Gen3:

ros2 launch constrained_manipulability abstract_gen3.launch.py

If using the Gen3 manipulator, refer to #10 for an explanation as to why you should build ros2_kortex from source.

Please note in the default RViz config file that appears, you should add the /visualization_marker topic to see the scene and polytopes. Additionally, the default configuration of the Kinova Gen3 robot launched by the above command will generate convex hull errors - please adjust the manipulator’s configuration (using the joint position slider GUI) to visualize the polytopes.

If you are running on real hardware: The above is mostly configured for use with RViz considering a simulated robot state. If running on real hardware, include in your workspace the necessary ROS 2 Universal Robot repository (Universal_Robots_ROS2_Driver) and/or Kinova Gen3 repository (ros2_kortex).

OctoMaps

The previous examples consider static scenes consisting of primitive geometric types as obstacles. However, the constrained_manipulability package can also handle dynamic obstacles, e.g., represented in the form of an OctoMap. The octomap_filter repository is necessary in these scenarios to remove the robot body from the OcTree representation.

Shrinking Polytopes

You can explore an example of shrinking polytopes (changing the linearization limits) by running this launch file:

ros2 launch constrained_manipulability shrinking_polytope.launch.py

Modify the limits using the sliding bar GUI that appears and again add the /visualization_marker topic to your RViz display.

IK Teleoperation

The following example illustrates how to perform IK teleoperation based on the polytope constraints computed in the constrained_manipulability package. Please first run the server with a UR3e configuration (as well as an OctoMap scene):

ros2 launch constrained_manipulability cm_server_example.launch.py

Then run the IK client node, which uses the convex constraints in an optimization routine:

ros2 run constrained_manipulability cm_client.py

If using a real robot’s joint states (default topic: /in_joint_states), or use the sim parameter if for visualization only:

ros2 run constrained_manipulability cm_client.py --ros-args -p sim:=true

We use cvxpy (pip install 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 and input geometry_msgs/Twist. The input Twist can be generated by a teleop command (recommended setting for speed is ~0.03 and turn ~0.014):

ros2 run teleop_twist_keyboard teleop_twist_keyboard

Computation

The ConstrainedManipulability node reads a kinematic chain from ROS 2 parameters, starting at the root of the robot and running until its tip or end-effector. The joint position and velocity limits are also read and are used to define the different polytopes. A collision world is also maintained, with a variety of objects that can be added/removed using shape_msgs and Eigen::Affine3d.

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 distances and is accessible via the interface package: robot_collision_checking. Polytopes in Cartesian space can be returned from getter functions, like:

Polytope getConstrainedAllowableMotionPolytope(const sensor_msgs::msg::JointState& joint_state,
                                               bool show_polytope, 
                                               Eigen::MatrixXd& AHrep,
                                               Eigen::VectorXd& bHrep,
                                               Eigen::Vector3d& offset_position,
                                               const std::vector<double>& color_pts = {0.0, 0.0, 0.5, 0.0}, 
                                               const std::vector<double>& color_line = {1.0, 0.0, 0.0, 0.4});

“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 via eigen-cddlib.

Different polytopes can be computed. More information about the allowable motion polytope is available in the “Optimization-Based Human-in-the-Loop Manipulation Using Joint Space Polytopes”, Long et al., 2019 work and more information about the constrained velocity polytope is available in “Evaluating Robot Manipulability in Constrained Environments by Velocity Polytope Reduction”, Long et al., 2018.

Applications

A video showing the applications of the constrained allowable motion polytope is available here. A video showing the utility of the constrained velocity polytope for humanoid robots can be found 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. As polytopes are returned via the constrained_manipulability library, they can instead be used for fast online IK solutions and guarded teleoperation use-cases.

2. Guarded Teleoperation

Polytopes are convex constraints that represent feasible configurations for the whole robot. By respecting these constraints, a guaranteed feasible IK solution can be obtained very quickly, which can be useful for generating virtual fixtures in teleoperation tasks. The polytope vizualized in red below shows an operator the Cartesian motions available at all times due to joint limits, kinematic constraints, and obstacles in the workspace. The original (allowable motion) polytope is shown below in blue.

3. Workspace Analysis

By evaluating the volume of the constrained manipulability polytope at points in the workspace, a reachability map can be obtained as shown in this video.

References

If you use this package, please cite either:

@inproceedings{Long2019Optimization,
  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)},
  year={2019},
  organization={IEEE}
}

or

@INPROCEEDINGS{Long2018Evaluating,
  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},
  year={2018},
  volume={},
  number={},
  pages={1-9},
  doi={10.1109/HUMANOIDS.2018.8624962},
  ISSN={2164-0580},
  month={Nov},}

and for the teleoperation use-case, especially alongside AR/VR, then please also cite:

@ARTICLE{Zolotas2021Motion,
  AUTHOR={Zolotas, Mark and Wonsick, Murphy and Long, Philip and Padır, Taşkın},   
  TITLE={Motion Polytopes in Virtual Reality for Shared Control in Remote Manipulation Applications},      
  JOURNAL={Frontiers in Robotics and AI},      
  VOLUME={8},           
  YEAR={2021},      	
  DOI={10.3389/frobt.2021.730433},      
  ISSN={2296-9144},
}

Contributing to constrained_manipulability

First off, thanks for taking the time to contribute! ❤️

All types of contributions are encouraged and valued. See the Table of Contents for different ways to help and details about how this project handles them. Please make sure to read the relevant section before making your contribution. It will make it a lot easier for us maintainers and smooth out the experience for all involved. The community looks forward to your contributions. 🎉

And if you like the project, but just don’t have time to contribute, that’s fine. There are other easy ways to support the project and show your appreciation, which we would also be very happy about:

• Star the project
• Tweet about it
• Refer this project in your project’s readme
• Mention the project at local meetups and tell your friends/colleagues

Table of Contents

• Code of Conduct
• I Have a Question
  • I Want To Contribute
  • Reporting Bugs
  • Suggesting Enhancements <!– - Your First Code Contribution
  • Improving The Documentation –> <!– - Styleguides
  • Commit Messages
• Join The Project Team –>

Code of Conduct

This project and everyone participating in it is governed by the constrained_manipulability Code of Conduct. By participating, you are expected to uphold this code. Please report unacceptable behavior to <>.

I Have a Question

If you want to ask a question, we assume that you have read the README.

Before you ask a question, it is best to search for existing Issues that might help you. In case you have found a suitable issue and still need clarification, you can write your question in this issue. It is also advisable to search the internet for answers first.

If you then still feel the need to ask a question and need clarification, we recommend the following:

• Open an Issue.
• Provide as much context as you can about what you’re running into.
• Provide project and platform versions (nodejs, npm, etc), depending on what seems relevant.

We will then take care of the issue as soon as possible.

I Want To Contribute

Legal Notice

When contributing to this project, you must agree that you have authored 100% of the content, that you have the necessary rights to the content and that the content you contribute may be provided under the project licence.

Reporting Bugs

Before Submitting a Bug Report

A good bug report shouldn’t leave others needing to chase you up for more information. Therefore, we ask you to investigate carefully, collect information and describe the issue in detail in your report. Please complete the following steps in advance to help us fix any potential bug as fast as possible.

• Make sure that you are using the latest version.
• Determine if your bug is really a bug and not an error on your side e.g. using incompatible environment components/versions (Make sure that you have read the README. If you are looking for support, you might want to check this section).
• To see if other users have experienced (and potentially already solved) the same issue you are having, check if there is not already a bug report existing for your bug or error in the bug tracker.
• Also make sure to search the internet (including Stack Overflow) to see if users outside of the GitHub community have discussed the issue.
• Collect information about the bug:
  • Stack trace (Traceback)
  • OS, Platform and Version (Windows, Linux, macOS, x86, ARM)
  • Version of the interpreter, compiler, SDK, runtime environment, package manager, depending on what seems relevant.
  • Possibly your input and the output
  • Can you reliably reproduce the issue? And can you also reproduce it with older versions?

How Do I Submit a Good Bug Report?

You must never report security related issues, vulnerabilities or bugs including sensitive information to the issue tracker, or elsewhere in public. Instead sensitive bugs must be sent by email to <>.

We use GitHub issues to track bugs and errors. If you run into an issue with the project:

• Open an Issue. (Since we can’t be sure at this point whether it is a bug or not, we ask you not to talk about a bug yet and not to label the issue.)
• Explain the behavior you would expect and the actual behavior.
• Please provide as much context as possible and describe the reproduction steps that someone else can follow to recreate the issue on their own. This usually includes your code. For good bug reports you should isolate the problem and create a reduced test case.
• Provide the information you collected in the previous section.

Once it’s filed:

• The project team will label the issue accordingly.
• A team member will try to reproduce the issue with your provided steps. If there are no reproduction steps or no obvious way to reproduce the issue, the team will ask you for those steps and mark the issue as needs-repro. Bugs with the needs-repro tag will not be addressed until they are reproduced.
• If the team is able to reproduce the issue, it will be marked needs-fix, as well as possibly other tags (such as critical), and the issue will be left to be implemented by someone.

Suggesting Enhancements

This section guides you through submitting an enhancement suggestion for constrained_manipulability, including completely new features and minor improvements to existing functionality. Following these guidelines will help maintainers and the community to understand your suggestion and find related suggestions.

Before Submitting an Enhancement

• Make sure that you are using the latest version.
• Read the README carefully and find out if the functionality is already covered, maybe by an individual configuration.
• Perform a search to see if the enhancement has already been suggested. If it has, add a comment to the existing issue instead of opening a new one.
• Find out whether your idea fits with the scope and aims of the project. It’s up to you to make a strong case to convince the project’s developers of the merits of this feature. Keep in mind that we want features that will be useful to the majority of our users and not just a small subset. If you’re just targeting a minority of users, consider writing an add-on/plugin library.

How Do I Submit a Good Enhancement Suggestion?

Enhancement suggestions are tracked as GitHub issues.

• Use a clear and descriptive title for the issue to identify the suggestion.
• Provide a step-by-step description of the suggested enhancement in as many details as possible.
• Describe the current behavior and explain which behavior you expected to see instead and why. At this point you can also tell which alternatives do not work for you.
• You may want to include screenshots or screen recordings which help you demonstrate the steps or point out the part which the suggestion is related to. You can use LICEcap to record GIFs on macOS and Windows, and the built-in screen recorder in GNOME or SimpleScreenRecorder on Linux.
• Explain why this enhancement would be useful to most constrained_manipulability users. You may also want to point out the other projects that solved it better and which could serve as inspiration.

Attribution

This guide is based on the contributing-gen. Make your own!