RAMP

Vasileios Vasilopoulos, Suveer Garg, Pedro Piacenza, Jinwook Huh, Volkan Isler

This package includes the code for RAMP: Hierarchical Reactive Motion Planning for Manipulation Tasks Using Implicit Signed Distance Functions, developed by Samsung Research (Samsung AI Center - New York) and presented at IROS 2023.

Pipeline

For simulation demonstrations, we use the Isaac Gym physics simulation environment from NVIDIA, as well as modified environment generation code from SceneCollisionNet, included in the sim_env folder. For the computation of differentiable forward kinematics, the package uses pytorch-kinematics.

Installation and running

Environment setup

Create a python virtual environment inside the repo, source it and update pip:
```
python3 -m venv pyvenv
source pyvenv/bin/activate
pip install --upgrade pip
```
Install the requirements. When installing PyTorch, make sure that the PyTorch version matches your installed CUDA version by updating --extra-index-url, for example: https://download.pytorch.org/whl/cu118 for CUDA 11.8. You can check your CUDA version by running: nvcc --version.
```
pip install -r requirements.txt
```

Install PyTorch3D with GPU support:

pip install git+https://github.com/facebookresearch/pytorch3d.git@stable

Download Isaac Gym and copy the isaacgym folder into extern (Prerequisites:Ubuntu 18.04, or 20.04. Python 3.6, 3.7, or 3.8).

Setup all packages

Install all packages by running:

pip install -e .

Run without any arguments

Run the simulation:

python -m test_ramp_simulation

Run with optional arguments

Optionally, you can visualize the start (green) and goal (red) sphere markers with the --markers flag and/or you can specify an experiment to run with the --experiment flag. For demonstration purposes, we have included 5 static environment scenarios (numbered 0-4) and 5 dynamic environment scenarios (numbered 10-14). The full list of all available arguments is included near the top of the main script.

python -m test_ramp_simulation --markers True --experiment 10

Known Issues

If you see the error WARNING: lavapipe is not a conformant vulkan implementation, testing use only., try the following command:
```
export VK_ICD_FILENAMES=/usr/share/vulkan/icd.d/nvidia_icd.json
```

Use Outside of Isaac Gym Environment - API Description

In this package, RAMP is wrapped around an Isaac-Gym simulation environment. To use the algorithm for your own application:

You need to copy over to your project the mppi_planning (which includes the trajectory generator) and trajectory_following (which includes the trajectory follower) folders.

You need to instantiate a TrajectoryPlanner (see trajectory_planning) class, for example:

# Robot parameters
JOINT_LIMITS = [
    np.array([-2.8973, -1.7628, -2.8973, -
                3.0718, -2.8973, -0.0175, -2.8973]),
    np.array([2.8973, 1.7628, 2.8973, -
                0.0698, 2.8973, 3.7525, 2.8973])
]

LINK_FIXED = 'panda_link0'
LINK_EE = 'panda_hand'

LINK_SKELETON = [
    'panda_link1',
    'panda_link3',
    'panda_link4',
    'panda_link5',
    'panda_link7',
    'panda_hand',
]

robot_urdf_location = 'resources/panda/panda.urdf'
scene_urdf_location = 'resources/environment/environment.urdf'

# Instantiate trajectory planner
self.trajectory_planner = TrajectoryPlanner(
    joint_limits=JOINT_LIMITS,
    robot_urdf_location=robot_urdf_location,
    scene_urdf_location=scene_urdf_location,
    link_fixed=LINK_FIXED,
    link_ee=LINK_EE,
    link_skeleton=LINK_SKELETON,
)

You need to instantiate a TrajectoryFollower (see trajectory_following) class, for example:

# Trajectory Follower initialization
trajectory_follower = TrajectoryFollower(
    joint_limits = JOINT_LIMITS,
    robot_urdf_location = robot_urdf_location,
    link_fixed = LINK_FIXED,
    link_ee = LINK_EE,
    link_skeleton = LINK_SKELETON,
)

With the trajectory planner object, you can instantiate a motion planning problem for RAMP by calling the instantiate_mppi_ja_to_ja method of TrajectoryPlanner and passing the required parameters as well as the current and target joint angles, for example:
```
# MPPI parameters
N_JOINTS = 7
mppi_control_limits = [
    -0.05 * np.ones(N_JOINTS),
    0.05 * np.ones(N_JOINTS)
]
mppi_nsamples = 500
mppi_covariance = 0.005
mppi_lambda = 1.0

# Instantiate MPPI object
self.trajectory_planner.instantiate_mppi_ja_to_ja(
    current_joint_angles,
    target_joint_angles,
    mppi_control_limits=mppi_control_limits,
    mppi_nsamples=mppi_nsamples,
    mppi_covariance=mppi_covariance,
    mppi_lambda=mppi_lambda,
)
```
Then, we offer the following functionalities:
1. You can update the obstacle point cloud used for planning by calling the update_obstacle_pcd method of TrajectoryPlanner, for example:
```
self.trajectory_planner.update_obstacle_pcd(pcd=pcd)
```
2. You can run an MPC iteration to get the current trajectory by calling the get_mppi_rollout method of TrajectoryPlanner, for example:
```
trajectory = self.trajectory_planner.get_mppi_rollout(current_joint_angles)
```
3. You can update the current target without instantiating a new motion planning problem (the most recent trajectory will be used to warm-start the search) by calling the update_goal_ja method of TrajectoryPlanner, for example:
```
self.trajectory_planner.update_goal_ja(new_target_joint_angles)
```
With the trajectory follower object:
1. You can update the currently followed trajectory when needed with the update_trajectory method of TrajectoryFollower, for example:
```
trajectory_follower.update_trajectory(trajectory)
```
2. You can update the obstacle point cloud used for obstacle avoidance by calling the update_obstacle_pcd method of TrajectoryFollower, for example:
```
trajectory_follower.update_obstacle_pcd(new_pcd)
```
3. You can extract the commanded joint velocities at each control iteration by calling the follow_trajectory method of TrajectoryFollower, for example:
```
velocity_command = trajectory_follower.follow_trajectory(current_joint_angles)
```

License

This work is licensed under a Creative Commons Attribution-NonCommercial 4.0 International License (CC BY-NC).

Citation

If you find this work useful, please consider citing:

@inproceedings{vasilopoulos2023ramp,
  title={{RAMP: Hierarchical Reactive Motion Planning for Manipulation Tasks Using Implicit Signed Distance Functions}},
  author={Vasilopoulos, Vasileios and Garg, Suveer and Piacenza, Pedro and Huh, Jinwook and Isler, Volkan},
  booktitle={{IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}},
  year={2023}
}

SamsungLabs / RAMP

readme