This package contains the ROS1-based plugin implemenations of REACH kinematics, evaluation, and display interfaces
A simple demonstration of the capability of this repository is provided in the demo
sub-directory.
See the instructions for details on how to run the demo.
Use the following steps to run a reach study with a robot using the ROS1 infrastructure and plugins.
- Create a URDF of your robot system
- Create a launch file to load the URDF, SRDF, and other required parameters (e.g. related to kinematics, joint, limits) to the parameter server (see this demo example file)
- Create a mesh model of the workpiece
Note: the origin of this model should align with the kinematic base frame of the robot
- Create a point cloud of the target points on the workpiece
- This point cloud can be generated using a command line tool from PCL 1.8:
pcl_mesh_sampling <workpiece_mesh>.ply <output_cloud>.pcd -n_samples <number of samples> -leaf_size <leaf_size> -write_normals true
- This point cloud can be generated using a command line tool from PCL 1.8:
- Create a configuration YAML file defining the parameters of the reach study and the configuration of the interface plugins (see this demo example)
- Run the setup launch file
roslaunch reach_ros setup.launch robot:=<load_robot_parameters>.launch
- Run the reach study analysis
roslaunch reach_ros start.launch config_file:=<config_file.yaml> config_name:=<arbitrary_config>
This plugin uses MoveIt! to calculate the manipulability of a robot pose. Higher manipulability results in a higher pose score. Range: [0, inf)
Parameters:
planning_group
- The name of the planning group with which to evaluate the manipulability of a given robot pose
jacobian_row_subset
(optional)- The indices of the rows of the Jacobian to use when evaluating the manipulability. The row indices should be on [0, 6) and correspond to the output space [x, y, z, rx, ry, rz]
- Ex.
jacobian_row_subset: [0, 1, 2] # position manipulability only
This plugin uses MoveIt! to calculate the manipulability of a robot pose divided by the characteristic length of the motion group. The characteristic length is computed by walking from the base link to the tip link of the motion group and summing the distances between adjacent links. Higher scaled manipulability results in a higher pose score. Range: [0, inf)
Parameters:
planning_group
- The name of the planning group with which to evaluate the manipulability of a given robot pose
jacobian_row_subset
(optional)- The indices of the rows of the Jacobian to use when evaluating the manipulability. The row indices should be on [0, 6) and correspond to the output space [x, y, z, rx, ry, rz]
- Ex.
jacobian_row_subset: [0, 1, 2] # position manipulability only
excluded_links
(optional)- The names of links contained in the motion group that should not contribute to the characteristic length
This plugin uses MoveIt! to calculate the manipulability of a robot pose and evaluate a score. The score is calculated as the ratio of the smallest manipulability value to the largest manipulability value. The larger this ratio, the more uniform the dexterity and the higher the score. Range [0, 1]
Parameters:
planning_group
- The name of the planning group with which to evaluate the manipulability of a given robot pose
jacobian_row_subset
(optional)- The indices of the rows of the Jacobian to use when evaluating the manipulability. The row indices should be on [0, 6) and correspond to the output space [x, y, z, rx, ry, rz]
- Ex.
jacobian_row_subset: [0, 1, 2] # position manipulability only
This plugin uses the MoveIt! collision environment to calculate the distance to closest collision for a robot pose. That distance value is then used to score the robot pose. Larger distance to closest collision results in higher pose score. Range: [0, inf)
Parameters:
planning_group
- The name of the planning group to be used to solve the robot's inverse kinematics
distance_threshold
- The distance between 2 closest surfaces to collision under which an inverse kinematics solution will be considered invalid
collision_mesh_filename
- The filename (in ROS package URI format) of the reach object mesh to be used to do collision checking
- Example: `package://<your_package>//.stl
touch_links
- The names of the robot links with which the reach object mesh is allowed to collide
exponent
- score = (closest_distance_to_collision - distance_threshold)^exponent.
This plugin uses the MoveIt! robot model to calculate a robot pose score based on how much the pose deviates from the center of the joint range. Robot poses that are closer to the center of the joint range result in higher pose scores. Range: [0, 1]
Parameters:
planning_group
- The name of the planning_group with which to evaluate the joint penalty
This plugin uses MoveIt! kinematics solvers and collision checking to produce collision aware IK solutions
Parameters:
planning_group
- Name of the planning group
distance_threshold
- The distance from nearest collision at which to invalidate an IK solution. For example, if this parameter is set to 0.1m, then IK solutions whose distance to nearest collision is less than 0.1m will be invalidated
collision_mesh_filename
- The file path to the collision mesh model of the workpiece, in the
package://
or 'file://' URI format
- The file path to the collision mesh model of the workpiece, in the
touch_links
- The TF links that are allowed to be in contact with the collision mesh
evaluation_plugin
- The name (and parameters) of the evaluation plugin to be used to score IK solution poses
This plugin performs the same function as the MoveIt! IK solver plugin above, but calculates IK solutions for a target that has been discretized about its Z-axis by an input angle. The pose with the best score is returned.
Parameters:
planning_group
- Name of the planning group
distance_threshold
- The distance from nearest collision at which to invalidate an IK solution. For example, if this parameter is set to 0.1m, then IK solutions whose distance to nearest collision is less than 0.1m will be invalidated
collision_mesh_filename
- The file path to the collision mesh model of the workpiece, in the
package://
or 'file://' URI format
- The file path to the collision mesh model of the workpiece, in the
touch_links
- The TF links that are allowed to be in contact with the collision mesh
evaluation_plugin
- The name (and parameters) of the evaluation plugin to be used to score IK solution poses
discretization_angle
- The angle (between 0 and pi, in radians) with which to sample each target pose about the Z-axis
This plugin publishes joint state and visualization markers to display the results of the reach study. The markers are interactive:
- Left-clicking on a marker will change the robot position to the IK solution
- This works by publishing a
sensor_msgs/JointState
message to thereach_joints
topic, which is a source topic for thejoint_state_publisher
- This works by publishing a
- Right-clicking on the marker will bring up a context menu that displays the score of the reach target
The markers are colorized with a heat-map, where red represents the highest scores (i.e., hottest) and blue represents the lowest scores (i.e., coldest). Unreachable points are colorized black. There are two methods for computing the values for the heat map:
- By default, the values for the heat map are computed as the ratio of the individual scores to the maximum score. Thus if the scores of the reach study are all fairly consistent and close to the maximum, then the colors of the markers will be mostly red (with the target with the maximum score being the deepest hue of red). If the scores of the reach study targets differ greatly, more variation in color will be seen. Usually this method produces the most meaningful display.
- The values can also be computed using the "full color range", where the scores are fully normalized before colorization. The lowest score (regardless of value) is always displayed as the deepest hue of blue and the the highest score is always shown as the deepest hue of red. This is valuable for highlighting differences in reachability but can be misleading due to the normalization of the scores.
Parameters:
collision_mesh_filename
- The file path to the collision mesh model of the workpiece, in the
package://
or 'file://' URI format
- The file path to the collision mesh model of the workpiece, in the
kinematic_base_frame
- The base frame of the kinematic tree in which to display the interactive markers
marker_scale
- The length (in meters) of the arrow markers representing the target Cartesian points
use_full_color_range
(optional, default: False)- Colorize the heat map using the full range of colors (such that the target with the lowest score is the deepest hue of blue, and the target with the highest score is the deepest hue of red)