Differential Inverse Kinematics Control for Mobile Manipulation

This repository contains ROS packages for control and other utilities for the UTIAS mobile manipulator (the "Thing"). In particular, it contains the code used for the experiments in this paper (the PDF can also be found here). You can also find a video here.

This repository has been superseded by mobile_manipulation_central and is no longer maintained.

Contents

1. Robot Information
2. Software Installation
3. Packages
4. Simulation
5. Experiment
6. Development

Robot Information

Hardware

The robot consists of a UR10 manipulator mounted on a Ridgeback omnidirectional mobile base. The base has a Hokuyo UST-10LX laser range finder mounted at the front that provides a two-dimensional scan in a 270 degree arc in front of the robot. The end effector has a Robotiq FT 300 force torque sensor mounted at the wrist to measure the applied wrench, as well as Robotiq 3 finger gripper for manipulation. See the datasheets for more details on each of the components.

Software

The robot is currently running Ubuntu 14.04 on its onboard computer, so the code of this repository is designed to be built and run on that Ubuntu version. If you have a later version of Ubuntu, you can emulate a 14.04 environment using docker.

The UR10 is running firmware version 3.9 and communicating using ur_modern_driver, which is deprecated. A complete upgrade of the system and dependencies to a more recent version of Ubuntu, ROS, driver, and firmware is planned for sometime in the future.

Software Installation

First, install required dependencies we can get from apt (in the following, <distro> can be one of indigo, kinetic, or melodic). We always need Eigen:

sudo apt install libeigen3-dev

The robotiq packages for the gripper (more on this below) require:

sudo apt install ros-<distro>-soem ros-<distro>-socketcan-interface

Running the UR10 driver onboard the laptop requires:

sudo apt install ros-<distro>-ur-modern-driver

DEPRECATED (only do this if things don't work): Symlink to Eigen:

sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen

Next, clone vicon_bridge and robotiq into the src directory of your catkin workspace.

Finally, you'll also need to install qpOASES, which is the current QP solver of choice for this project. Clone the repository somewhere convenient (not your catkin workspace; it's not a ROS package). Then, to compile, simply enter the cloned repo and type make. To get ROS to find the qpOASES headers, you'll need to make some symlinks:

sudo ln -s <path/to/qpOASES>/include/qpOASES.hpp /usr/local/include/qpOASES.hpp
sudo ln -s <path/to/qpOASES>/include/qpOASES     /usr/local/include/qpOASES
sudo ln -s <path/to/qpOASES>/bin/libqpOASES.so   /usr/local/lib/libqpOASES.so

Now, clone this repository into the src directory of your catkin workspace.

Generate kinematics code

Some of the kinematics code is automatically generated (from symbolic math in Python). To generate it, you'll need some Python packages:

python -m pip install --user numpy sympy dill

where python is the Python executable used by your ROS installation (newer ROS versions use Python 3).

We need to install the Python files only first, without compiling the C++ code (since the autogenerated code is not generated yet). To do so, run

catkin build mm_kinematics -DSKIP_CPP_BUILD=ON

Then, to generate the functions:

cd mm_kinematics/scripts
./generate_all.sh [python_executable]

where you can pass the desired Python executable to use (full path or just the command name, such as python2 or python3). If not given, defaults to python.

Build

You should now be able to build with catkin (if you do not have catkin_tools installed, see here):

# normal build
catkin build

# with compiler optimizations (this makes it run much faster)
catkin build -DCMAKE_BUILD_TYPE=Release

Packages

Within this metapackage, we have:

mm_control

Controllers for both joint space and Cartesian (task) space. Written in C++.

mm_force_torque

Tools for working with the Robotiq force-torque sensor.

To run the driver for real sensor, use

roslaunch mm_force_torque bringup.launch

but note that this must be run onboard the robot. A very basic simulation of the FT sensor is also available in scripts/simulate_ft_sensor.py.

The main utility is the script scripts/wrench.py, which processes wrenches from the sensor by filtering and rotating into the world frame. This information is then published under the topic /mm_wrench/info.

mm_gripper

Very basic tools for working with the gripper.

Bring up the gripper using

roslaunch mm_gripper bringup.launch

Then command the gripper to open or close using

rosrun mm_gripper gripper.py <cmd> [delay]

where <cmd> is either o for open or c for close, and [delay] is a an optional number of seconds to wait before executing the desired command. For example,

rosrun mm_gripper gripper.py c 5

will close the gripper in 5 seconds, which allows me to position something to be grabbed.

mm_kinematics

Contains forward kinematics code in both C++ and Python for use by other packages.

mm_lidar

Tools for using the Hokuyo laser scanner mounted at the front of the base. The laser scanner is automatically started by the software onboard the robot and publishes its raw data to /front/scan. This package contains scripts for processing the detections into actual positions of objects in the world frame, primarily for obstacle avoidance.

mm_math_util

Miscellaneous math utilities shared by other packages.

mm_msgs

Defines custom ROS messages used by other packages in the project.

mm_simulation

Basic simulation of the Thing kinematics in Python with a simple matplotlib visualization. There is also a Gazebo simulation available here.

mm_trajectories

Trajectory generation. Generates trajectories to be tracked by the controllers.

mm_vicon

Contains Vicon estimation code and launch files for the Vicon system. To publish the mobile base pose from Vicon, connect to the Vicon wifi DSL_DroneNet_5G (see the wiki for the password) and run roslaunch mm_vicon vicon.launch.

Calibration

Scripts for calibration can be found in mm_control/scripts. The calibration routine optimizes over two SE(3) transforms, one between the base and arm, and one at the end effector, in order to minimize the error between the forward kinematic model and the measured EE pose.

The collect_calibration_data.py script moves the robot to a sequence of configurations and records the joint angles and EE pose as measured by Vicon (as a position and quaternion). Vicon and the joint control node must be already running.

Configurations to which the robot will move are specified in mm_control/config/calibration_configs.yaml. When changing this file, be careful to test in simulation to ensure there are no self-collisions! The results of the data collection procedure are written to mm_control/data/calibration_data_<date>_<time>.json.

To perform the calibration procedure, go to mm_control/scripts and run

calibrate.py ../data/<calibration_data_file>

(or substitute the path to whichever calibration data file you like). The optimized transforms are written to mm_control/data/calibration_params_<date>_<time>.json, where <date> and <time> match those of the calibration data file used.

Simulation

This repository contains a very basic simulation of the robot in mm_simulation, which is just a wireframe in a 3D matplotlib plot. To use this, run:

> roscore
> roslaunch mm_simulation simulation.launch

Alternatively, there is a separate respository containing packages for a Gazebo simulation, which should be entirely compatible with the controllers in this package. Assuming that repository has been installed in the workspace, built, and sourced, then instead of

> roslaunch mm_simulation sim.launch

one can use

> roslaunch mm_gazebo simulation.launch

Everything else should remain the same.

Experiment

Steps for experiments on the real robot:

1. Connect to DSL_DroneNet_5G network.
2. Connect to the Thing via Ethernet.
3. Ensure ROS is configured to use the Thing as master:

   laptop > export ROS_IP=192.168.131.100
   laptop > export ROS_MASTER_URI=http://cpr-tor11-01:11311

   If you run rostopic list, you should see various topics related to the Thing listed.
4. Run:

   # on laptop
   laptop > roslaunch mm_vicon vicon.launch
   
   # alternatively, if Vicon is not available
   laptop > roslaunch mm_vicon spoof.launch
   
   # start the UR10 driver (the Ridgeback starts automatically onboard the
   # robot) and the mux node that publishes the combined /mm_joint_states
   # topic.
   # NOTE: this does usually run fine on the laptop, but can occasionally
   # generate garbage values for joint position or velocity. On the other hand,
   # running on the Thing may have time sync issues between the two computers.
   laptop > roslaunch mm_control bringup.launch
   
   # start the controller, e.g. differential inverse kinematics
   laptop > rosrun mm_control diff_ik
   
   # if using force control
   robot > roslaunch mm_force_control force.launch
   laptop > rosrun mm_force_control force.py
   
   # example of running a trajectory
   # inspect the file first to ensure the trajectory does what you expect
   laptop > rosrun mm_trajectories line.py

Go Home

Requires bringup.launch to already be launched and running. Run:

laptop > roslaunch mm_control home.launch config:=<config>

where <config> the name of one of the home configurations defined in mm_control/config/home.yaml. If the config argument is omitted, it defaults to the standard home position.

Gripper

Open or close the gripper (experiment only). Run on the laptop:

laptop > roslaunch mm_gripper bringup.launch

# <cmd> is either o (open) or c (close)
laptop > rosrun mm_gripper gripper.py <cmd>

Set desired force

To set the desired force at runtime, use:

laptop > rostopic pub /force/desired std_msgs/Float64 <value>

Development

C++ code should be formatted using clang-format and Python code should be formatted using Black.

C++ tests use gtest and Python tests use pytest.