Franka Emika Panda in Gazebo with ROS and Docker

In my last post I’ve written about creating Gazebo packages from the existing panda models and mentioned that I’m also working on a docker image that includes ROS. Well here it is 🙂 You can check out the GitHub repository (be sure to leave a star if you like it), and – assuming you have an operating docker host – it should be very straight forward to set up.

Since I spent way too much time fixing dependencies and writing configuration files, I would like to share my experience (and of course the code :P) so that the solutions to the problems I encountered along the way are in one place.

High Level Overview

Before diving into the setup of the individual parts, I want to quickly talk about how the various components interact. This overview will make it easier to understand what each system is trying to accomplish, and what it expects.

A high level overview of how Panda is controlled in Gazebo. The simulator replaces the real robot at the level above the controllers (i.e. they use different controllers).

Contrary to my initially guess – that I could simply extend the Gazebo package I created earlier – I found that it is easier to start completely from scratch for this image. The logic here is that the robot will be spawned into Gazebo from ROS, and hence it makes sense to properly integrate things into the ROS architecture. This means using .urdf to describe the robot model, instead of .sdf files like I did in the previous post. It also means that I settled for one model – the robot with the gripper – although support for both versions can be added.

The main pipeline goes from MoveIt via ROS Control to Gazebo. MoveIt receives a goal pose and plans a trajectory to it. It then sends position commands to ROS control which sets targets for a series of PID controllers. These controllers are what steer the simulated joints in Gazebo. Gazebo then sends the joint encoder readings back to the ROS framework, where they are picked up by the PID controllers to stabilize the robot, MoveIt to decide the next step of the trajectory, and RViZ to visualize the robot. MoveIt and Gazebo both use the .urdf I generated in the last post, and I got the controller parameters from Erdal Pekel’s blog post.

ROS Control and Stabilizing the Robot

In retrospect getting the controllers to work is quite easy. While configuring it, it was quite annoying, because they didn’t seem to be loading properly. The issue was dependencies 🙂 Before I go into the packages needed to connect ROS control to gazebo, however, I want to show you the config file that I used to set the PID controls. I got this file from Erdal Pekel’s Blog who spend quite a bit of time tuning those numbers. Amazing job!

# panda: #useful if you use a namespace for the robot
# Publish joint states
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
panda_arm_controller:
type: effort_controllers/JointTrajectoryController
joints:
panda_joint1
panda_joint2
panda_joint3
panda_joint4
panda_joint5
panda_joint6
panda_joint7
gains:
panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
constraints:
goal_time: 2.0
state_publish_rate: 25
panda_hand_controller:
type: effort_controllers/JointTrajectoryController
joints:
panda_finger_joint1
panda_finger_joint2
gains:
panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }
state_publish_rate: 25
view raw panda_control.yaml hosted with ❤ by GitHub
PID parameters for the low-level joint controllers

To get these controllers to actually control the simulation, we can start them with a single line in the launch file, and point them to the `panda_controller.yaml`. To communicate with gazebo, however, they will need the `gazebo_ros_control` package, which doesn’t seem to ship with `gazebo_ros` nor with `ros_control`. Additionally the default ros container doesn’t ship with any controllers, so I had to install the effort controllers in the `effort_controllers` package, but also the `joint-trajectory-controller` which will install provide the controllers mentioned in above file. I didn’t test if the other effort controllers are necessary, so there might be a dependency on which this doesn’t actually depend.

If the low-level controllers are working properly, the arm in the simulation should freeze in a position that is different from the one it will settle in based on gravity. Usually, the simulation starts, and the arm falls. Then – a short while later – the controllers finish loading and stabilize the arm in some (slightly awkward) position.

Creating the MoveIt configuration

The MoveIt package in the repository (called `panda_moveit`) is the result of me using the moveit setup wizzard with the generated .urdf file. Initially, I tried using the official `panda_moveit_config` package, but failed to get things to work for two key reasons: (1) the official moveit config is meant for the physical robot, which uses controllers unsupported by Gazebo. This mismatch in controllers is detrimental. (2) I (unknowingly) chose a different naming convention in the .urdf compared to the officially used one. This can be fixed by me renaming things, but at this point I had already found out about reason (1), and thought I can keep it if I have to create my own config anyway.

To create this package, I followed the MoveIt Setup Assistant Tutorial, trying to stick as close to the existing `panda_moveit_config` as possible. For example, I included the ‘ready’ pose that panda uses to give the robot a default state 🙂

One file that I had to add manually to the generated configuration to get it to work was a config file for the MoveIt controllers. This tells MoveIT which controller groups it should use to actuate which motors, and what messages to send to the ROS control nodes. I then had to replace the `panda_moveit_controller_manager.launch.xml` with the newly created ones.

controller_list:
name: panda_arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
panda_joint1
panda_joint2
panda_joint3
panda_joint4
panda_joint5
panda_joint6
panda_joint7
name: panda_hand_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
parallel: true
joints:
panda_finger_joint1
panda_finger_joint2
The controller configuration for MoveIt

To start MoveIt and the planning context together with Gazebo, I added the `move_group.launch` file to the launch file in the `panda_gazebo` package. I didn’t touch the other launch files that the moveit setup assistant generated; hence, they are likely in a broken state.

Controlling the Simulator from RViZ

Finally, and to add some icing on the cake, controlling the Gazebo simulation through RViZ provides a very good test if everything is working as it should. I added the RViZ node to the launch file in the `panda_gazebo` package. After launching the file and waiting for everything to load, I created a layout in RViZ, configured it to my liking, and stored the layout as an .rviz config file. I then added this file as an argument to the launch options for the RViZ node.

A strange situation that I encountered here was that the static frame RViZ tries to use as reference in the global settings was set to `map` instead of `world`. I’m not 100% sure why the default is map, but if it is set to this, the interactive markers won’t show for panda’s endeffector.

With all this in place, I can now use the interact handles to drag the robot into a desired goal position and click ‘plan and execute’.

Putting It All Together in Docker

After all this work of assembling the pieces into a working pipeline, I thought I can take some additional time to alleviate some of the pain others may have when setting this up; especially the pain that comes from missing dependencies. As a result, I decided to create a docker image that is portable and will set all this up for you.

The Dockerfile itself is very short:

FROM osrf/ros:kinetic-desktop-full-xenial
RUN apt-get update \
&& apt-get install -y \
ros-kinetic-gazebo-ros \
ros-kinetic-gazebo-ros-pkgs \
ros-kinetic-gazebo-ros-control \
ros-kinetic-joint-state-controller \
ros-kinetic-effort-controllers \
ros-kinetic-position-controllers \
ros-kinetic-joint-trajectory-controller \
ros-kinetic-ros-control \
ros-kinetic-moveit\
&& rm -rf /var/lib/apt/lists/*
COPY assets/catkin_ws/src/panda_gazebo/models /root/.gazebo/models
COPY assets/catkin_ws/ /catkin_ws
COPY assets/ros_entrypoint.sh /ros_entrypoint.sh
CMD roslaunch panda_gazebo panda.launch
view raw Dockerfile hosted with ❤ by GitHub
Dockerfile used to generate the image

It just installs the necessary ROS packages (it is totally my own stupidity, but I can’t stress enough how much time I wasted figuring out which packages I need), adds the workspace created above, and then modifies the image’s entrypoint to lay the catkin workspace over the default ROS workspace.

One interesting thing happens in line 16, where I fix a bug that is very unique to gazebo in docker. As a container starts ‘fresh’ gazebo doesn’t have any models downloaded, which means it will download the two models it needs to create `empty.world`. This costs time and, unfortunately, interacts with ROS control in such a way that the controllers crash. To correct this, I pre-populate gazebo’s model cache with the models it needs.

The other file I use for building the image is the build script:

#!/bin/bash
# generate the .urdf
docker run –rm -v ${PWD}/assets/panda_xacro:/xacro osrf/ros:kinetic-desktop-full rosrun xacro xacro –inorder /xacro/panda_arm_hand.urdf.xacro > ${PWD}/assets/catkin_ws/src/panda_description/urdf/panda.urdf
# build the ROS packages
docker run –rm -it -v ${PWD}/assets/catkin_ws:/catkin_ws osrf/ros:kinetic-desktop-full-xenial bash -c "apt update && apt install -y python-catkin-tools && cd /catkin_ws && catkin build"
# Build the image
docker build -t panda_gazebo_sim:latest ${PWD}
view raw build.sh hosted with ❤ by GitHub
Build script for the docker image

The script is again very short. Line 4 generates the .urdf file from the .xacro file (a process I used in my post on how to get panda into Gazebo). Line 7 is interesting, because we spin up a clean ROS image to build the catkin workspace that we created, and then use those generated files to add them to the final image. Arguably this could be solved more elegantly via multi-stage builds; however, I learned about this feature after implementing things this way. Thus I will leave it as is until a future refactor, and will use multi-stage builds in future images.

That’s it! These posts usually take my entire weekend to write. If you think they are good, be sure to to like the post, and leave a star on my GitHub repository. This way I know that writing these is a worthy use of my time.

Happy coding!

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s