===== 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.

==== The URDF file for your robot ==== 
To load your robot in the PyCRAM simulation environment you need a URDF representation of your robot. 
Most manufacturers of robots provide an URDF file to the respective robot. If there is no URDF file for your robot you have to create one yourself, for this please look at the tutorials in the ROS wiki  [[http://wiki.ros.org/urdf/Tutorials|here]]. 


==== 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 this class are:
    * 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
    * 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:
<code> 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"]) </code>
This example is the robot description for the boxy robot of the Institute for Artificial Intelligence.

=== 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.
<code> 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})</code> 

=== 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, similar to the chain description, done by creating an object of the CameraDescription class. The camera description takes as arguments:
    * the link name of the camera in the URDF 
    * horizontal -and vertical angle 
    * minimal -and maximal height. 
The heights as well as the angles are optional arguments and don't have to be given. 
An example on how to do this can be seen below.
<code> camera = CameraDescription("head_mount_kinect2_rgb_optical_frame",
                                   minimal_height=0.0, maximal_height=2.5,
                                   horizontal_angle=0.99483, vertical_angle=0.75049)
 self.add_camera("camera", camera) </code>
                              
=== Additional Info === 
For more examples or a full implementation of a robot description please referee to the already implemented descriptions [[https://github.com/cram2/pycram/blob/master/pycram/src/pycram/robot_description.py#L510 | here]].
                              
==== Process Modules for your robot ==== 
To add a robot to PyCRAM the plans and designators can all stay the same the only thing that is robot-specific are the so-called process modules, these are the low-level implementation which interact 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 === 
To implement your own process modules you need to create a class which extends the base ProcessModule class of PyCRAM and override the _execute method. The method gets the designator as a parameter. An example for such an implementation can be seen below. 
<code> class BoxyNavigation(ProcessModule):
    """
    The process module to move the robot from one position to another.
    """

    def _execute(self, desig):
        solution = desig.reference()
        if solution['cmd'] == 'navigate':
            robot = BulletWorld.robot
            # Reset odom joints to zero
            for joint_name in robot_description.i.odom_joints:
                robot.set_joint_state(joint_name, 0.0)
            # Set actual goal pose
            robot.set_position_and_orientation(solution['target'], solution['orientation'])</code>
In the first line the process module resolves the designator, the resolved designator is then saved as a dictionary were the key is the parameter and the value is the value of this parameter. 
In the next line it is checked that this designator really references this process module. Then the robot object will be saved, for easier use later on.
Next on, the odom joints will be set to zero, which is specifically done for this robot to avoid weird positions and finally the position and orientation of the robot will be set to the values specified in the designator. 

This is just an example on how the code could look like for an robot in the simulation environment, but  there are no restrictions to the code. For example, for a real robot there could be a ROS publisher which publishes to a topic to move the robot. 

=== Registration === 
For PyCRAM to be able to choose the right process modules the process modules has to be registered. The registration basically tells PyCRAM which process modules are available and for which kind of designator they are intended. If a desiginator is performed PyCRAM then queries this registration to choose the right process module for this designator. 

To register your implemented process modules to PyCRAM you need to create a new class which extends the PyCRAM ProcessModules class, don't confuse this with the ProcessModule class which is for implementing the process modules. 

The constructor of this class takes the objects of the process modules as arguments, the rest of choosing the right process modules is done by PyCRAM in the background. The constructor takes the process modules in the following order:
    * navigation
    * pick_up 
    * place 
    * accessing 
    * park_arms 
    * move_head 
    * opening_gripper 
    * closing_gripper 
    * detecting 
    * move_tcp 
    * move_joints 
    * world_state_detecting 
At the moment the process modules have to be in this order, so PyCRAM can find the right process module for the respective designator. You don't have to provide all process modules and can always write None instead of the process module, but keep in mind that in this case the execution of the respective designator would fail. 
Here is how this looks like for the boxy robot. 
<code>class BoxyProcessModules(ProcessModules):

    def __init__(self):
        if not BoxyProcessModules.initialized:
            super().__init__(BoxyNavigation(), BoxyPickUp(), BoxyPlace(), BoxyAccessing(), BoxyParkArms(),
                             BoxyMoveHead(), BoxyMoveGripper(), BoxyMoveGripper(), BoxyDetecting(), BoxyMoveTCP(),
                             BoxyMoveJoints(), BoxyWorldStateDetecting())
</code>