Giskard interface in PyCRAM

This notebook should provide you with an example on how to use the Giskard interface. This includes how to use the interface, how it actually works and how to extend it.

We start by installing and launching Giskard. For the installation please follow the instructions here. After you finish the installation you should be able to launch Giskard by calling:

roslaunch giskardpy giskardpy_pr2_standalone.launch

This way you can launch Giskard for any robot that is supported:

roslaunch giskardpy giskardpy_hsr_standalone.launch

“Standalone” means that Giskard only uses a simulated robot and does not look for a real robot. If you want to use Giskard with a real robot you have to switch out “standalone” with “iai”, e.g:

roslaunch giskardpy giskardpy_hsr_iai.launch

To see what Giskard is doing you can start RViz, there should already be a MarkerArray when starting otherwise you have to add this manually.

How to use the Giskard interface

Everything related to the Giskard interface is located in pycram.external_interfaces.giskard. The content of the file can be roughly divided into three parts: 1. Methods to manage the belief states between PyCRAM and Giskard 2. Motion goals that should be sent to Giskard for execution 3. Helper methods to construct ROS messages

The most useful methods are the ones for sending and executing Motion goals. These are the ones we will mostly look at.

We will now start by setting up PyCRAM and then try to send some simple motion goals.

from pycram.bullet_world import BulletWorld, Object
from pycram.enums import ObjectType

world = BulletWorld()
pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf")

When you are working on the real robot you also need to initialize the RobotStateUpdater, this module updates the robot in the BulletWorld with the pose and joint state of the real robot.

You might need to change to topic names to fit the topic names as published by your robot.

from pycram.ros.robot_state_updater import RobotStateUpdater

r = RobotStateUpdater("/tf", "/joint_states")

Now we have a PyCRAM belief state set up, belief state in this case just refers to the BulletWorld since the BulletWorld represents what we believe the world to look like.

The next step will be to send a simple motion goal. The motion goal we will be sending is moving the torso up. For this we just need to move one joint, so we can use the achive_joint_goal. This method takes a dictionary with the joints that should be moved and the target value for the joints.

Look at RViz to see the robot move, since we call Giskard for movement the robot in the BulletWorld will not move.

from pycram.external_interfaces import giskard

giskard.achieve_joint_goal({"torso_lift_joint": 0.28})

For Giskard everything is connected by joints (this is called a World Tree by Giskard) therefore we can move the robot’s base by using motion goals between the map origin and the robot base. (e.g. by sending a “base_link” goal in the “map” frame).

In the example below we use a cartesian goal, meaning we give Giskard a goal pose, a root link and a tip link and Giskard tries to move all joints between root link and tip link such that the tip link is at the goal pose.

This sort of movement is fine for short distances, but keep in mind that Giskard has no collision avoidance for longer journeys. So using MoveBase for longer distances is a better idea.

from pycram.external_interfaces import giskard
from pycram.pose import Pose

giskard.achieve_cartesian_goal(Pose([1, 0, 0]), "base_link", "map")

Now for the last example: we will move the gripper using full body motion control.

We will again use the cartesian goal, but now between “map” and “r_gripper_tool_frame” frames. This will not only move the robot (because the kinematic chain between “map” and “base_link” as used in the previous example is also part of this chain) but also move the arm of the robot such that it reaches the goal pose.

from pycram.external_interfaces import giskard
from pycram.pose import Pose

giskard.achieve_cartesian_goal(Pose([1, 0.5, 0.7]), "r_gripper_tool_frame", "map")

That concludes this example you can now close the BulletWorld by using the “exit” method.

world.exit()

How the Giskard interface works

The PyCRAM interface to Giskard mostly relies on the Python interface that Giskard already provides (tutorial and the source code). This interface provides methods to achieve motion goals and load things into the Giskard believe state.

What PyCRAM does with this, is: Synchronize the belief state of Giskard with the one of PyCRAM by loading the environment URDF in Giskard, this is done before any motion goal is sent. Furthermore, the motion goals are wrapped in methods that use PyCRAM data types.

You can also set collisions between different groups of links. By default, Giskard avoids all collisions but for things like grasping an object you want to allow collisions of the gripper. The interface also supports the following collision modes: * avoid_all_collisions * allow_self_collision * allow_gripper_collision The collision mode can be set by calling the respective method, after calling the method the collision mode is valid for the next motion goal. Afterwards, it defaults back to avoid_all_collisions.

There is a init_giskard_interface method which can be used as a decorator. This decorator should be used on all methods that access the giskard_wrapper, since it assures that the interface is working and checks if Giskard died or the imports for the giskard_msgs failed.

Extend the Giskard interface

At the moment the PyCRAM Giskard interface is mostly a wrapper around the Python interface provided by Giskard. If you want to extend the interface there are two ways: * Wrap more motion goals which are provided by the Python interface * Design new Higher-Level motion goals by combining the motion goals already provided