blackcoffeerobotics / ur5e_robotiq_85_mtc

A ROS1 package for Pick and Place using MoveIt! Task Constructor for the UR5e arm equipped with Robotiq-2f-85 grippers.

ur5e_robotiq_85_mtc

Overview

This package contains a custom MoveIt! configuration package for the UR5e arm equipped with a Robotiq 85 gripper.
It also contains a MoveIt! Task Constructor (MTC) package that can be used to generate plans for the robot to perform a pick and place task. All revelant parameters for MTC are stored in the config folder.

Tutorial

• Install all the dependencies for the rosinstall tool:

  sudo apt-get install \
   python3-vcstools \
   python3-rosinstall

• Install all the external dependencies (These include MoveIt Task Constructor, and our fork of MoveIt! for orientation constraints):

  cd third_party/
  rosinstall . /opt/ros/$ROS_DISTRO ur5e_robotiq_85_mtc.rosinstall
  ./modify_mtc.sh

• Install all the dependencies by running the following command in the root of your workspace:

  rosdep install --from-paths src --ignore-src -r -y

• Build the workspace and source the setup file:

  catkin build
  source devel/setup.bash

• Launch Gazebo along with support for MoveIt! and RViz:

  roslaunch ur5e_robotiq_85_mtc_pkg setup.launch start_rviz:=true

• Finally, run the pick and place pipeline:

  roslaunch ur5e_robotiq_85_mtc_pkg pick_and_place.launch

  pick_and_place_pipeline.mp4

Our Contributions to the MoveIt! Fork

1. Backporting the Joint Flip BugFix from MoveIt!2:

Following the amazing work of Jeroen De Maeyer during his GSoC 2020 project for MoveIt! Constrained Planning, most of the crucial fixes had been ported over to MoveIt2 instead of the former.

As a result, orientation constraints in MoveIt! do not work by default because of the specified bug.

We ported the relevant features of his PR to MoveIt!2 into MoveIt! in the following PR to the repository.

Therefore, to ensure that the constraints specified in the Connect Stage carry out the motion plan to the SerialContainer, use the main branch of our custom fork of MoveIt!.

2. Caching Constraints now takes Parameterization into account

If your environment remains static between runs, it might be a good idea to save aspects of the planning scene that do not vary. This helps us in creating a database of pre-computed constrained states. As a result, the planning time during the constrained motion is greatly reduced upon execution.

The format for saving constraints mainly involves setting the frame of reference, target orientation, tolerance, and parameterization. The parameterization field allows us to specify the target orientation using Euler angles or rotation vectors.

The default way you stored constraints earlier did not consider this field while generating the database. This is paramount as specifying constraints using Euler angles is not ideal in certain orientations, in which the end effector is stuck in a Gimbal lock. This is easily overcome by using rotation vectors instead of Euler angles in specifying target orientation.

Therefore, before you run your setup, store your constraints in your database using rotation vector representation instead and use the improve-constraint-manifold branch of our custom fork of MoveIt!.

💾 EOF