iROSA-lab / sampled_reachability_maps

Generate reachability and base placement maps for mobile manipulators using pytorch_kinematics

Sampled Reachability Maps

Reachability and Base Placement maps based on pytorch_kinematics (https://github.com/UM-ARM-Lab/pytorch_kinematics).

This repository is a part of the codebase of the paper: Robot Learning of Mobile Manipulation With Reachability Behavior Priors [1] [Paper] [Project site]

Usage

Clone repository somewhere, then pip3 install -e . to install in editable mode.

See scripts for samples. The samples create reachability maps for a Tiago++ mobile manipulator.

• Reachability maps can be created by either sampling joint positions and using forward kinematics (recommended) or by sampling end-effector poses and computing inverse kinematics (much slower)
• The maps store two metrics for computing the reachability of every 6D voxel:
  1. The maximum Manipulability (Yoshikawa metric) among all the joint configurations sampled that lead to the end-effector occupying the 6D voxel. This is the default metric.
  2. The number of joint configurations that lead to the end-effector occupying the 6D voxel (i.e. the no. of visitations of every 6D voxel).
• The script invert_reachability_map.py inverts the reachability map i.e. computes the inverse transform of all the end-effector poses
• The base placement map can then be obtained by transforming the inverted map based on a given goal pose and slicing the map to intersect with the ground (create_base_placement_map_for_goal_pose.py)
• For a detailed explanation on the computation of these maps, refer to Vahrenkamp et al. [2] (https://doi.org/10.1109/ICRA.2013.6630839)
• For an alternative, learned base placement map technique, refer to Jauhri at al. [1] (https://doi.org/10.1109/LRA.2022.3188109)

Visualization

To visualize the generated reachability and base placement maps, use the ROS package: reachability_map_visualizer (github.com/iROSA-lab/reachability_map_visualizer)

Credits

• pytorch_kinematics/transforms is extracted from pytorch3d with minor extensions. This was done instead of including pytorch3d as a dependency because it is hard to install and most of its code is unrelated. An important difference is that we use left hand multiplied transforms as is convention in robotics (T * pt) instead of their right hand multiplied transforms.
• pytorch_kinematics/urdf_parser_py, and pytorch_kinematics/mjcf_parser is extracted from kinpy, as well as the FK logic.
• pytorch_kinematics (https://github.com/UM-ARM-Lab/pytorch_kinematics) This repository contains add-ons, extensions and bug-fixes on top of pytorch_kinematics.

References

[1] S. Jauhri, J. Peters and G. Chalvatzaki, "Robot Learning of Mobile Manipulation With Reachability Behavior Priors", https://doi.org/10.1109/LRA.2022.3188109

[2] N. Vahrenkamp, T. Asfour and R. Dillmann, "Robot placement based on reachability inversion," 2013 IEEE International Conference on Robotics and Automation, 2013, pp. 1970-1975, https://doi.org/10.1109/ICRA.2013.6630839.