ros-industrial/reach

A tool that allows users to visualize and quantitatively evaluate the reach capability of a robot system for a given workpiece.

REACH

Robotic Evaluation And Comparison Heuristic

Table of Contents

• Description
• Installation
• ROS Integration
• Tips

Description

The REACH repository is a tool that allows users to visualize and quantitatively evaluate the reach capability of a robot system for a given workpiece. See the ROSCon 2019 presentation and video for a more detailed explanation of the reach study concept and approach.

Supported OS Distros

OS	Support
Ubuntu Focal	✓
Ubuntu Jammy	✓

Structure

reach is a ROS-independent package that provides the framework for the reach study process, defined in the diagram below:

The reach package also provides the interface definition for the required reach study functions:

1. TargetPoseGenerator
   • Generates Cartesian target poses that the robot should attempt to reach during the reach study
   • These target poses are expected to be relative to the kinematic base frame of the robot
   • The z-axis of the target poses is expected to oppose the z-axis of the robot kinematic tip frame
2. IKSolver
   • Calculates the inverse kinematics solution for the robot at an input 6 degree-of-freedom Cartesian target
3. Evaluator
   • Calculates a numerical "fitness" score of an IK solution (i.e., robot joint pose) at a given Cartesian target pose
   • Higher values indicate better reachability
   • Example numerical measures of reachability include manipulability, distance from closest collision, etc.
4. Display
   • Visualizes the robot/reach study environment, target Cartesian poses, IK solutions, and reach study results
5. Logger
   • Logs messages about the status and progress of the reach study

Plugins

The interfaces described above are exposed as plugins using the boost_plugin_loader library to support custom implementations.

Several default and dummy plugins have been created in the reach package. Many other ROS-based plugins have been implemented in the reach_ros and reach_ros2 packages. All of the plugins built in this project are discovered automatically by the plugin loader without additional manual steps.

The plugin loader class finds plugin libraries using two environment variables:

• LD_LIBRARY_PATH
  • The plugin loader searches for libraries containing plugins within the directories defined in the LD_LIBRARY_PATH environment variable.
  • When using a ROS-based build tool such as catkin or colcon this variable is set automatically to include both system level and workspace level folders by sourcing <devel|install>/setup.bash
• REACH_PLUGINS:
  • The plugin loader then looks for libraries with names defined by the environment variable REACH_PLUGINS within the directories specified by the environment variable LD_LIBRARY_PATH.
  • The names of these libraries should not include a prefix (e.g., lib) or a suffix (e.g., .so) and should be separated by a colon (:).
  • This variable must be set manually to specify plugin libraries not built in this project

If custom libraries created outside this project (for example libmy_custom_reach_plugins.so and libcool_reach_plugins.so) contain REACH plugins, make those plugin libraries visible to the plugin loader by setting the REACH_PLUGINS environment variable as follows:

export REACH_PLUGINS=my_custom_reach_plugins:cool_reach_plugins

Installation

Nominally, the reach package is ROS-independent, but it is convenient to use the ROS dependency management and build tools to build the package.

First, clone the repository into a catkin workspace

cd ~/reach_ws/src
git clone https://github.com/ros-industrial/reach.git
cd ..

Install the dependencies

vcs import src < src/reach/dependencies.repos
rosdep install --from-paths src --ignore-src -r -y

Build the repository

<catkin/colcon> build

ROS Integration

See the reach_ros and reach_ros2 repositories for ROS-based plugins, capability demos, and general usage instructions.

Tips

1. Ensure the object mesh and reach target position scales match and are correct (visualize in rviz). It is common to be off by a factor of 1000.
2. If a set of robot links are allowed to collide with the mesh, add their names to the touch_links field of the MoveItIKSolver plugin in the reach study configuration file.
3. The selection of IK solver is key to the performance of the reach study. Gradient-based solvers (such as KDL and TRAC-IK) are typically good choices.
   • Additional constraints (or lack thereof, such as orientation freedom about the tool z-axis) can also be incorporated into the IK solver (via parameters or source code changes) to produce different reach study results
   • For MoveIt-based plugins, the selection of IK solver is defined in the kinematics.yaml file
4. Reach study results are serialized to file and can be loaded using the API in reach for programmatic analysis or modification
5. You can specify a starting seed for the IK solver by providing a list of joint positions in the config. This be used to help the IK solver to find solutions for complex scenarios. It might also help to guide the solver to solutions that are closer to a certain configuration that you prefere. If no initial seed is provided, a pose with all joints at 0 position is used.

reach's Issues

Reach study results heat map

Integrate the code for creating a heat map on the target mesh using reach study results

robot_reach_study_node crash if there are too much points in point cloud

I noticed that the robot_reach_study_node crashes if there are too much points in the point cloud. I thought it was a problem with my robot, but I was able to reproduce it with the demo.

The crash actually comes from the system that kills the process, because it consumes too much RAM (exit code -9)

The crash can occur either during the reach_study (+6000 points in the point cloud , my setup has 16GB of RAM) , or during the optimization loop (+4000 points in the point cloud).

I am not sure if it might be because of the design of the reach algorithm by itself or because of a memory leak your might have.

Note: probably related to the boost object that stores the results, because I was able to get this type of crash (too much RAM used) by loading reach.db.xml with a lot of markers inside (since the reach_study and optimization was already done).

Fix Python bindings for heat map generation

reach/src/python/python_bindings.cpp

Lines 298 to 299 in 5a3e4ca

	bp::def("computeHeatMapColors", &computeHeatMapColorsPython1);
	bp::def("computeHeatMapColors", &computeHeatMapColorsPython2);

The default values for the heat map generator functions are not being populated into the Python API. We need to add additional arguments to bp::def to define the default values. See this post for a reference solution

reach_demo refers to motoman model that is not part of repository

In this xacro a robot model is referenced that is not part of this repo (model/motoman_sia20d_macro.xacro).

Would it be an idea to make a minor change and let it refer to: this xacro in the ROS-Industrial Motoman repository.

I have tested this locally and (as far as I can oversee) it seems to work fine.

Does this tool require a mesh to perform a reach analysis or will a point set work?

Update Documentation

• Add documentation about the structure of a ReachDatabase/ReachResult/ReachRecord to the main README
• Add information on using the Python interface introduced in #38

Remove implicit 180 degree rotation of target pose

The reach study currently implicitly rotates each target pose by 180 degrees about the x-axis and makes the assumption that a robot's TCP frame has the z-axis pointing "out". This assumption and implict operation can be easily missed, causing reach studies to report many infeasible IK solutions, which is generally difficult to debug. We should remove the implicit rotation and state the assumption that the reach study will attempt to align the TCP frame directly with the target poses (per the operation of the underlying IK solver).

reach/src/reach_study.cpp

Line 111 in d89ddaf

const Eigen::Isometry3d& tgt_frame = target_poses_[i] * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());

Customizable creation of target points

Create a plugin (or ROS service?) interface for creating reach study target points, rather than relying on the user providing a point cloud file.

Normalize normal vector before including in pose matrix

reach/src/plugin_utils.cpp

Line 158 in d89ddaf

p.matrix().col(2).head<3>() = norm;

Is it possible to do a reachability study in a volume ?

Thank you for providing this package to the community, it is very powerful and useful.

I have a question regarding the possibilities offered by the package.

Do you think it might be possible to study reachability in a volume ? Instead of having a mesh and generating a point cloud on it, would it be possible to have just a point cloud in a volume and evaluate reachability in that point cloud ?
The idea I had to do it with a mesh is to have a vertical plane mesh, thin on the x axis, and doing the reachability study with this plane multiple times with a position further and further from the robot, and then compile all the results to make a volume.

Does it make sense according to you ?

One other question I had is a more tricky one. I would like to study the reachability of a mobile manipulator that has two arms. Do you think it's possible with the package, and how ? Otherwise, do you think it's possible to do some tricks with the code in order to make this study possible ?

Thank you for your help.

Target poses are not loaded in the right frame

The poses loaded from the pointcloud file are not correctly related to the kinematic_base_frame.

In a previous version of the code, the getSampledMesh function was loading the pointcloud and transforming it to the correct frame, doing something like:

geometry_msgs::TransformStamped tf = buffer.lookupTransform(req.fixed_frame, req.object_frame, ros::Time(0), ros::Duration(5.0));
transform = tf2::transformToEigen(tf.transform);

pcl::PointCloud<pcl::PointNormal> transformed_cloud;
pcl::transformPointCloudWithNormals(cloud, transformed_cloud, transform.matrix());

That part is lost in the current version.

However display of the markers is still correct because when adding the markers, the frame is actually used:

auto marker = utils::makeInteractiveMarker(id, db[i], kinematic_base_frame_, marker_scale_, heatmap_colors.row(i));

When doing the calculations, target_poses_ is used, which has not gone under any frame transformation. So even if the display of the poses is correct, the calculations are not.

Efficient Nearest Neighbors Search

The current "optimization" strategy relies on finding the nearest neighbors of a target point and re-solving IK using each neighbor as a seed point. Currently these neighbors are found by looping over all of the target points and measuring their distances to the current point. This could be improved significantly by implementing a KD-tree or other search tree. This has already been stubbed out using FLANN, but is still incomplete

Trouble creating initial database

I'm having a bit of trouble running this for the first time. My two launch commands are:

roslaunch reachability reachability_analysis.launch

This loads everything robot-related (joints, moveit stuff, fake trajectory execution, etc). I'm pretty sure this is set up correctly because I can plan/execute with the MoveIt GUI. It also loads the mesh that I want to study (as part of the URDF) and creates the tf frame for the mesh (the tf frame is called reach_object).

The second launch command is:

roslaunch reach_core start.launch config_file:=rospack find reachability/config/reachability_params.yaml config_name:=reach_study

What's the significance of config_name? Right now it's arbitrary, it doesn't match anything else.

The stack trace at this second launch command is:

ros.moveit_reach_plugins: Successfully initialized MoveItIKSolver plugin
ros.moveit_reach_plugins: Successfully initialized MoveItReachDisplay plugin
ros.reach_core: ------------------------------
ros.reach_core: No reach study database loaded
ros.reach_core: ------------------------------
================================================================================
REQUIRED process [robot_reach_study_node-1] has died!

^I'm confused here because I'm trying to create the initial database.

My reachability_params.yaml config file is below. I'm sure I have the wrong file name or something.

`fixed_frame: "base_link"
object_frame: "reach_object"
results_directory: "$(find reachability)/results" # I created a new package to hold reachability stuff
pcd_filename: "$(find reachability)/data/plane_pointcloud.pcd"
get_avg_neighbor_count: false
compare_dbs: []
visualize_results: true

optimization:
radius: 0.2
max_steps: 10
step_improvement_threshold: 0.01

ik_solver_config:
name: "moveit_reach_plugins/ik/MoveItIKSolver"
distance_threshold: 0.0
planning_group: "left_arm"
collision_mesh_filename: "package://reachability/data/plane.ply"
collision_mesh_frame: "reach_object"
touch_links: []
evaluation_plugin:
name: "reach_core/plugins/MultiplicativeFactory"
plugins:
- name: "moveit_reach_plugins/evaluation/ManipulabilityMoveIt"
planning_group: "left_arm"
- name: "moveit_reach_plugins/evaluation/DistancePenaltyMoveIt"
planning_group: "left_arm"
distance_threshold: 0.025
exponent: 2
collision_mesh_filename: "package://reach_demo/config/part.ply"
collision_mesh_frame: "reach_object"
touch_links: []

display_config:
name: "moveit_reach_plugins/display/MoveItReachDisplay"
planning_group: "left_arm"
collision_mesh_filename: "package://reachability/data/plane.ply"
collision_mesh_frame: reach_object
fixed_frame: "base_link"
marker_scale: 0.05`

PYTHONPATH does not include REACH Python interface with colcon build

Building with colcon and sourcing install/setup.bash doesn't seem to add install/reach/lib/python3/dist-packages to the PYTHONPATH environment variable, which makes the Python interface invisible to Python

Very simple custom plugin fails with segmentation fault

I am currently trying to create a custom reach plugin. However, even the most minimal plugin fails with a segmentation fault.
The segmentation fault happens at the fist call to the plugin. In my case it is an evaluator, therefore the first call to calculateScore fails. Further debugging revealed that calls to calculateScore still work as long as the factory object still exists, i.e. in the following code,

reach/src/reach_study.cpp

Lines 321 to 325 in 0d1d854

	reach::Evaluator::ConstPtr evaluator;
	{
	auto factory = loader.createInstance<EvaluatorFactory>(get<std::string>(eval_config, "name"));
	evaluator = factory->create(eval_config);
	}

calling calculateScore before line 325 works, after it, there is a segmentation fault.

Steps to reproduce and the corresponding source code can be found here: https://github.com/timonegk/reach_plugin_example

Reach ROS2 branch

I am working on a port of reach on ROS2 here. Can you open ros2 branch so I can create PR against it.

Could not find library plugin reach_core/plugins/MultiplicativeFactory

I've just cloned/built the package and attempted to run the demo. At this command:

roslaunch reach_core start.launch config_file:=rospack find reach_demo/config/params.yaml config_name:=reach_study

I get this error:

Could not find library corresponding to plugin reach_core/plugins/MultiplicativeFactory. Make sure the plugin description XML file has the correct name of the library and that the library actually exists.

Check list:

• I made sure the workspace was sourced correctly

• I tried both install and no install: catkin config --no-install & catkin config --install

• I ran rosdep install --from-paths src --ignore-src -r -y

• I'm on Ubuntu 18, Melodic