This is an old revision of the document!


Adding your own robot to PyCRAM

PyCRAM can work with any robot, as long as a few requirements are fulfilled. These are:

  • An URDF file of your robot.
  • A description for the robot.
  • Process modules for this robot.

A robot description for your robot

The robot description contains all knowledge about your robot which has to be explicitly given. This includes, but not limited to:

  • The name of the base link.
  • The name of the torso link, if there is one.
  • Joint chains, for example the joint chain of each arm.
  • Joint configurations, for example the parking position of the arms.

Initialization

In PyCRAM the robot description is a class which extends the RobotDescription class. The attributes to initialize are moistly self explanatory. Name is the general name of the robot, base_frame is the origin of the URDF while base_link is the base link in the URDf, torso_link and torso_joint are the names of the torso joint and link, if there are any and lastly ik_joints means all non-fixed joints this is used for ik resolution and has to be given. Optionally an odom frame and the corresponding joints can also be specified. This then looks like this:

 class BoxyDescription(RobotDescription):

    def __init__(self):
        ik_joints = [some joints]  
        super().__init__("boxy", "base_footprint", "base_link", "triangle_base_link",
                         "triangle_base_joint", ik_joints, odom_frame="odom",
                         odom_joints=["odom_x_joint", "odom_y_joint", "odom_z_joint"]) 

Joint Chains

Now the joint chains can be added, to add a joint chain you first need to create an object of the ChainDescription class, this needs a name for this chain as well as all joints and all links in this chain. Next you can add static joint chains, meaning a special configuration for this joint chain. This is is done by calling the method add_static_joint_chain, this method takes a dictionary as argument with the name of the chain as key and a list with the joint positions as value. An example of this can be seen below.

 neck_links = ["neck_shoulder_link", "neck_upper_arm_link", "neck_forearm_link",
                      "neck_wrist_1_link", "neck_wrist_2_link", "neck_wrist_3_link"]
 neck_joints = ["neck_shoulder_pan_joint", "neck_shoulder_lift_joint", "neck_elbow_joint",
                       "neck_wrist_1_joint", "neck_wrist_2_joint", "neck_wrist_3_joint"] 
 neck_away = [-1.3155, -1.181355, -1.9562, 0.142417, 1.13492, 0.143467]
 neck_down = [-1.176, -3.1252, -0.8397, 0.83967, 1.1347, -0.0266]
 neck_down_left = [-0.776, -3.1252, -0.8397, 0.83967, 1.1347, -0.0266]
 neck = ChainDescription("neck", neck_joints, neck_links) 
 neck.add_static_joint_chains({"away": neck_away, "down": neck_down, "down_left": neck_down_left})

Cameras

In addition to the joint chains the camera or cameras if there are more than one can also be specified in the robot description. This is, like the chain description, done by creating an object of the CameraDescription class. The camera description takes as argument the link name of the camera in the URDF and additional arguments which specify the parameter of the camera sensor. This additional parameter are horizontal -and vertical angle as well as minimal -and maximal height. An example on how to do this can be seen below.

 minimal_height = 1.27
 maximal_height = 1.60
 horizontal_angle = 0.99483
 vertical_angle = 0.75049
 kinect = CameraDescription("head_mount_kinect_rgb_optical_frame",
                              horizontal_angle=horizontal_angle, vertical_angle=vertical_angle,
                              minimal_height=minimal_height, maximal_height=maximal_height) 

Additional Info

For more examples or a full implementation of a robot description please referee to the already implemented descriptions here.

Process Modules

To add a robot to PyCRAM the plans and designators can all stay the same the only thing specific are the so-called process modules, these are the low-level implementation which interacts directly with the robot. Normally one process module is responsible for one physical resource, for example one process module for the gripper and another to navigate the robot in the room. Besides this differentiation there is another one, that is if the process module is for the real robot or for the simulation. That is because in the simulation a lot of steps a real robot would perform are left out, for example if moving the robot it is teleported to the target location instead driving to it. This is done to save time and speed up the simulation, so the robot can simulate different scenarios before executing one.

Implementation