====== Introducing a new robot to CRAM ======

To understand how to introduce a new robot to CRAM, let us first look at how PR2 is integrated into the system.

===== Motion designators on PR2 =====

==== What are they? ====

First of all, we need to define the atomic actions that the robot is supposed to execute.

For mobile manipulation robots that would include //driving//, //moving-arm//, //opening-gripper//, //moving-neck//, etc.

For a flying robot with a distance sensor that would include //flying//, //distance-sensing//, etc.

For a human-robot interaction robot that would include //saying//, //listening//.

For mobile manipulation robots of CRAM the defined motion designators can be found in "cram_common_designators" package in [[https://github.com/cram2/cram/blob/master/cram_common/cram_common_designators/src/motions.lisp|motions.lisp]] file:

This is how we define motion designator grounding aka referencing for //looking// motions:
<code lisp>
(def-fact-group ptu-motions (motion-grounding)

  (<- (desig:motion-grounding ?designator (move-head :pose ?pose))
    (desig:desig-prop ?designator (:type :looking))
    (desig:desig-prop ?designator (:target ?location-designator))
    (desig:designator-groundings ?location-designator ?poses)
    (member ?pose ?poses))

  (<- (desig:motion-grounding ?designator (move-head :frame ?frame))
    (desig:desig-prop ?designator (:type :looking))
    (desig:desig-prop ?designator (:frame ?frame)))

  (<- (desig:motion-grounding ?designator (move-head :direction ?direction))
    (desig:desig-prop ?designator (:type :looking))
    (desig:desig-prop ?designator (:direction ?direction))))
</code>

Motions are executed with PERFORM calls:

<code lisp>
(on-real-pr2
  (perform
    (a motion
       (type going)
       (target (a location (pose xyz))))))
       
(on-simulated-pr2
  (perform
    (a motion
       (type going)
       (target (a location (pose xyz))))))
       
(on-real-boxy
  (perform
    (a motion
       (type going)
       (target (a location (pose xyz))))))     
</code>

As you can see, the PERFORM call stays //exactly// the same, only the environment setup commands (ON-REAL-BOXY etc) change, which completely change command execution.

**Motion designators are the abstraction layer for plans from robot hardware platform**

==== Executing motion designators on real PR2 ====

=== Low-level ROS interfaces ===

We start with low-level ROS clients for our PR2: it's the [[https://github.com/cram2/cram/tree/master/cram_pr2/cram_pr2_low_level|cram_pr2_low_level]] package.

This package has clients for different ROS actions / services / topics that the real PR2 robot provides.

E.g., let's look at the looking action:

https://github.com/cram2/cram/blob/master/cram_pr2/cram_pr2_low_level/src/ptu.lisp

=== Connecting low-level functions to motion designators -- process modules ===

A process module is a software component that is responsible for one resource of the robot, e.g. it's base, it's camera, it's neck, it's arms or it's left arm etc.

When you call

<code lisp>
(perform (a motion
            (type looking)
            (direction forward)))
</code>

The PTU (pan-tilt-unit) process module automatically finds the low-level Lisp function that should be executed for motions of type looking.

It also does primitive resource management of the robot's neck resource: it enqueues all PERFORM calls that go to the neck such that they don't conflict with each other when you have a concurrent program.

== Defining process modules ==

**Question**: how does the PM (process module) know which low-level function to call?

For the PR2 we can see the code in the [[https://github.com/cram2/cram/tree/master/cram_pr2/cram_pr2_process_modules|cram_pr2_process_modules]] package, e.g. in [[https://github.com/cram2/cram/blob/master/cram_pr2/cram_pr2_process_modules/src/ptu.lisp|ptu.lisp]] file.

First, the PM references the motion designator that it gets (that's implemented in a file we looked at before -- [[https://github.com/cram2/cram/blob/master/cram_common/cram_common_designators/src/motions.lisp|motions.lisp]]).

Then it says, if the command from the referenced motion designator is //cram-common-designators:move-head// then I'll call the low-level function //pr2-ll:call-ptu-action//.

== Matching process modules to designators ==

**Question**: how does the PTU PM get only motion designators of type //looking// and not of type //moving-arm//?

That is done by assigning different types of designators to different types of PMs -- [[https://github.com/cram2/cram/blob/master/cram_pr2/cram_pr2_process_modules/src/designators.lisp|let's look in the designators.lisp file]].

=== Performing the motions ===

<code lisp>
(cram-process-modules:with-process-modules-running
    (pr2-pms:pr2-grippers-pm pr2-pms:pr2-ptu-pm)
  (cpl:top-level
    (exe:perform
     (desig:a motion
              (type looking)
              (frame "r_gripper_tool_frame")))))
              
(cram-process-modules:with-process-modules-running
    (pr2-pms:pr2-grippers-pm pr2-pms:pr2-ptu-pm)
  (cpl:top-level
    (let ((?pose-stamped
            (cl-transforms-stamped:make-pose-stamped
             "map"
             0.0
             (cl-transforms:make-3d-vector 0 0 0)
             (cl-transforms:make-quaternion 0 0 0 1))))
      (exe:perform
       (desig:a motion
                (type looking)
                (target (desig:a location
                                 (pose ?pose-stamped))))))))
</code>

==== Executing motion designators on projected PR2 ====

Now let's take a different robot -- the projected PR2 -- and do the exercise one more time.

=== Low-level ===

Let's define the low-level interfaces for projected PR2 -- [[https://github.com/cram2/cram/blob/master/cram_pr2/cram_pr2_projection/src/low-level.lisp#L143|the low-level.lisp file]].

=== Process modules ===

== Defining process modules ==

Now let's define a PTU process module -- [[https://github.com/cram2/cram/blob/master/cram_pr2/cram_pr2_projection/src/process-modules.lisp#L52|process-modules.lisp file]].

== Matching process modules to designators ==

Now let's say which PM corresponds to which motion designator -- [[https://github.com/cram2/cram/blob/master/cram_pr2/cram_pr2_projection/src/process-modules.lisp#L95|same file bottom]].

== Performing motions ==

The first line is something very specific for projection, so ignore it, if you're not writing projection code you will not need this.

<code lisp>
(let ((cram-projection:*projection-environment* 'pr2-proj:pr2-bullet-projection-environment))
  (cram-process-modules:with-process-modules-running
      (pr2-proj::pr2-proj-ptu)
    (cpl:top-level
      (let ((?pose-stamped
              (cl-transforms-stamped:make-pose-stamped
               "map"
               0.0
               (cl-transforms:make-3d-vector 1.0 0 0.5)
               (cl-transforms:make-quaternion 0 0 0 1))))
        (exe:perform
         (desig:a motion
                  (type looking)
                  (target (desig:a location
                                   (pose ?pose-stamped)))))))))
</code>

==== Summary: your ToDo checklist to implement performing motion designators on your robot ====

  * Create a new CRAM package with dependency on //cram-prolog// and //cram-designators//. In your namespace //use// //cram-prolog// for convenience. Look at [[tutorials/beginner/package_for_turtlesim|creating a CRAM package]] tutorial for reference.

  * Implement your motion designator grounding predicates. See [[tutorials/advanced/new-robot#what_are_they|example above]]. To learn more about grounding motion designators look at the [[tutorials/beginner/motion_designators|motion designators tutorial]].

  * Add a new file (or create a new package) for your low-level functionality. It should add dependencies on all ROS message packages that you use for communication, on the ROS Lisp API //roslisp//, on //actionlib// if you use ActionLib, //roslisp-utilities// if you use the ROS startup functionality etc. Message packages in ASDF are defined by adding //-msg// suffix to the ROS package name, e.g. //std_msgs-msg//. See [[http://wiki.ros.org/roslisp/Tutorials/OverviewVersion|roslisp tutorials]] for more on this.

  * Define process modules for your low-level functions. This will add dependency on //cram-process-modules//. See [[tutorials/beginner/process_modules_2|PM tutorial]]. Also define Prolog predicates //matching-process-module// and //available-process-module// for your PMs (see [[tutorials/beginner/assigning_actions_2|corresponding tutorial]].

  * Finally, to test, //perform// the motions through //with-process-modules-running// macro. This will add a dependency on //cram-executive//, which defines the //perform// command.


===== Action designators and plans =====

Everything above motion designators should be independent of the robot platform.
For example, try performing a picking up action with your robot.
See [[http://cram-system.org/tutorials/intermediate/simple_mobile_manipulation_plan|this tutorial]] for an example.
===== Projection for your new robot =====

==== URDF ====

The first thing you need is a URDF of your robot. Find the package that upload your robot's URDF to the ROS parameter server.

For example, for HSR you can find the urdf here: [[https://github.com/ToyotaResearchInstitute/hsr_description]].

Then simply execute in your terminal: <code> roslaunch hsr_description upload.launch </code>
The HSR does not have so many links and joins as the PR2 so our life is much easier on that point. 

Which files are later needed in the //cram_hsrb// package: 
  * **cram_hsrb_description**
    * arms.lisp
    * general-knowledge.lisp
    * package.lisp
  * **cram_hsrb_projection**
    * action-designators.lisp
    * ik.lisp
    * low-level.lisp
    * package.lisp
    * process-modules.lisp
    * projection-environment.lisp
    * resources.lisp ?TODO: do we really need this?
    * setup-urdf.lisp ;;at least for the hsrb we come to that point later in the tutorial
    * tf.lisp
  * **hsrb_arm_kinematics** ;;python code TODO: should we explain?

For reference and starting point take a look at those package for the pr2 or hsrb.





==== Spawn the robot in Bullet ====

Use the Prolog assertion predicates. See example of how PR2 is spawned from a URDF in the bullet tutorial: http://cram-system.org/tutorials/intermediate/bullet_world#bullet_world_initialization. Open your ''roslisp_repl'' and load the cram-bullet-world-tutorial, for now this containts everything we need for the first step. 


You will find the code from the bullet world tutorial in short form here. For explanation what the single commands are doing please visit the tutorial above mentioned.

**Example for HSR:**

<code lisp>
CL-USER> (ros-load:load-system "cram_bullet_world_tutorial" :cram-bullet-world-tutorial)
CL-USER> (in-package :cram-bullet-world-tutorial)
BTW-TUT> (roslisp:start-ros-node "bullet_world")

BTW-TUT> (cram-occupancy-grid-costmap::init-occupancy-grid-costmap) ; Inits the occupancy map for navigation.
BTW-TUT> (cram-bullet-reasoning-belief-state::ros-time-init) ; Resets the simulated time.
BTW-TUT> (cram-location-costmap::location-costmap-vis-init) ; Inits publishing of markers in RViz.
BTW-TUT> (cram-tf::init-tf) ; Inits the TF listener object.


BTW-TUT> (prolog:prolog '(btr:bullet-world ?world))

BTW-TUT> (prolog:prolog '(and (btr:bullet-world ?world)
                              (btr:debug-window ?world)))
                              
BTW-TUT> (prolog:prolog '(and (btr:bullet-world ?world)
                              (assert (btr:object ?world :static-plane :floor ((0 0 0) (0 0 0 1))
                                                  :normal (0 0 1) :constant 0))))
                                                  
BTW-TUT> (let ((robot-urdf
                   (cl-urdf:parse-urdf
                    (roslisp:get-param "robot_description"))))
             (prolog:prolog
              `(and (btr:bullet-world ?world)
                    (cram-robot-interfaces:robot ?robot)
                    (assert (btr:object ?world :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf ,robot-urdf))
                    (assert (btr:joint-state ?world ?robot ?joint-states))
                    (assert (btr:joint-state ?world ?robot (("torso_lift_joint" 0.15d0)))))))
</code>


If you follow all the instruction corretly the HSR will be spawned. And should currently looking like this or in total grey (depend on which URDF is used).



{{:tutorials:advanced:hsr_in_bullet_world.png?700|}}

The ''(cram-tf::init-tf)''command initalized some variables:
<code lisp>
(defvar *robot-base-frame* nil
  "Robot's TF tree root frame. Be careful, this parameter and the ones below
are only initialized when a ROS node is started. DO NOT USE THESE IN OTHER PARAMETERS,
or in general at compile-time.")

(defvar *robot-torso-frame* nil)

(defvar *robot-torso-joint* nil)

(defvar *robot-left-tool-frame* nil
  "Tool frame of the left arm. Initialized from CRAM robot description.")
(defvar *robot-right-tool-frame* nil
  "Tool frame of the right arm. Initialized from CRAM robot desciption.")
</code>
Usually u will define those parameters in the ROBOT_description package. For now to simply test the beginning its not necessry that this parameters got filled from the PR2_description package. But be aware that some joint and links does not have the same name. For example the HSR does not contain any joint or links with the name gripper like the PR2 does.


Try to move the robot joints using Bullet Prolog predicates or Lisp functions. E.g.

<code lisp>
(btr:set-robot-state-from-joints
 '(("torso_lift_joint"  0.1))
 (btr:object btr:*current-bullet-world* 'my-robot))
</code>

If you are unsure what joints are possible to move use:
<code lisp>
(btr:object btr:*current-bullet-world* 'my-robot)
</code>


After lifting the torso the HSR should looks like this: 

{{:tutorials:advanced:hsrb_torso_move.png?700|}}


As you can see for the HSR lifting the 'torso_lift_joint' is not enough. The 'torso_lift_joint' and the 'arm_lift_joint' are mimic joints. The URDF-Tag mimic is not supported from the Bullet World yet.
How we define mimic joints will be shown later in the tutorial.


The next step is to define the robot_description and robot_projection packages, so that we can use the robot fully in the bullet world.



==== Create a CRAM robot description package ====

Similar to ''cram_pr2_description'' or ''cram_boxy_description'', it should contain Prolog queries describing the robot semantically. You need to decide how you define each part of the robot for example Boxy does have a own neck.lisp file in the package. The HSR is similar to the PR2, hence the description package contains arm.lisp and knowledge.lisp.

The queries are implementations of the interfaces defined in ''cram_robot_interfaces''. It is important that the joints and links are correctly sorted.
Define the most important links and joints there e.g.

<code lisp>
;;arm.lisp file
(in-package :cram-hsrb-description)

(defparameter *standard-to-hsrb-gripper-transform*
  (cl-transforms-stamped:make-identity-transform))

(def-fact-group hsrb-arm-facts (end-effector-link
                                robot-tool-frame
                                arm-joints arm-links
                                gripper-joint
                                gripper-link
                                standard-to-particular-gripper-transform)

  (<- (end-effector-link hsrb :left "wrist_roll_link"))


  (<- (robot-tool-frame hsrb :left "gripper_tool_joint"))
  (<- (robot-tool-frame hsrb :right "ATTENTION please don't use this frame"))

  (<- (arm-joints hsrb :left ("arm_flex_joint"
                              "arm_roll_joint"
                              "wrist_flex_joint"
                              "wrist_roll_joint")))

  (<- (arm-links hsrb :left ("arm_flex_link"
                             "arm_roll_link"
                             "wrist_flex_link"
                             "wrist_roll_link")))


  (<- (gripper-joint hsrb :left "hand_motor_joint"))

  (<- (gripper-link hsrb :left ?link)
    (bound ?link)
    (lisp-fun search "hand_palm_link" ?link ?pos)
    (lisp-pred identity ?pos))


  (<- (standard-to-particular-gripper-transform hsrb ?transform)
    (symbol-value *standard-to-hsrb-gripper-transform* ?transform)))

</code>
First we define a fact-group that containts end-effector-link, robot-tool-frame, arm-joints arm-links, gripper-joint, gripper-link and standard-to-particular-gripper-transform. The end-effector-link of a robot is the tool that replace the hand from a human and it is connected to the arm. With the end-effector-link the robot is interacting with his environment. We also use this for calculating distances from object to gripper. Since you can replace the grippers mostly on robots you need to add a specific offset for calculating distances (gripper length).


As you can see for the HSR (the full name is HSRB, that's why we called him like that in the lisp files) the arm is defined as :LEFT arm. This makes everything more easier, since all of the other lisp functions are working with the key words :LEFT or :RIGHT. So we highly recommence to take either :LEFT or :RIGHT for one handed or both for two handed. 
For the cram_tf packages it's necessary to describe left and right robot-tool-frame, even tho the HSR only has one arm. This will be fixed soon but for now we use this hack:
<code lisp>
 (<- (robot-tool-frame hsrb :right "ATTENTION please don't use this frame")). 
</code>

If no **gripper_tool_joint** is defined for your robot you need to add one in the URDF it self. It is important that the parent will be the wrist_palm_link and the joint is fixed. The origin you will need to change yourself it should be directly in the middle of the gripper. However since we dont want to change the original URDF we decided that there will be a setup-urdf.lisp file for the HSR that takes care of the necessary URDF changes. 

<code lisp>
;;the tool-frame for the hsrb, since the urdf does not provide one
(defparameter *tool-frame*
      "<joint name=\"gripper_tool_joint\" type=\"fixed\">
         <origin rpy=\"0.0 0.0 -80.14159265359\" xyz=\"0.012 0.0 0.075\"/>
         <parent link=\"hand_palm_link\" />
          <child link=\"gripper_tool_frame\"/>
        </joint>
        <link name=\"gripper_tool_frame\"/>")
</code>
{{:tutorials:advanced:gripper_tool_frame_hsrb.png?700|}}


Then the moveable arm-joints/links are defined and the gripper-joints/links.



Next step is to define the general-knowledge.lisp:

<code lisp>
(in-package :hsrb-descr)

(def-fact-group hsrb-metadata (robot
                               robot-odom-frame
                               robot-base-frame robot-torso-link-joint
                               arm
                               camera-frame
                               robot-pan-tilt-links robot-pan-tilt-joints)
  (<- (robot hsrb))

  (<- (robot-odom-frame hsrb "odom"))

  (<- (robot-base-frame hsrb "base_footprint"))
  (<- (robot-torso-link-joint hsrb "arm_lift_link" "arm_lift_joint"))

  ;mimic joint for arm_lift_link implemented in lower-level.lisp (cram-hsrb-projection) 
  ;(<- (robot-torso-link-joint hsrb "torso_lift_link" "torso_lift_joint"))

  (<- (arm hsrb :left))
  (<- (camera-frame hsrb "head_rgbd_sensor_link"))
  (<- (robot-pan-tilt-links hsrb "head_pan_link" "head_tilt_link"))
  (<- (robot-pan-tilt-joints hsrb "head_pan_joint" "head_tilt_joint")))
</code>

It is important to describe the robot-base-frame here and the torso-link/joint. What arms does the robot have and the camera he is using. The HSR does have two cameras (left-eye right-eye) but there is one topic that contains the information of both.
In the description package you can also see that we declared the 'arm_lift_joint' as robot-torso-link-joint the mimic joint is implemented in the cram-hsrb-projection package at least for now.
==== Create a CRAM YOUR_ROBOT_NAME projection package ====

Similar to ''cram_pr2_projection'' and ''cram_boxy_projection'', it should first of all have a ''low-level.lisp'' file, which defines interactions with the Bullet world. Every function needs to be working here. However the only function that is differently from the PR2 is 'move-torso'. Here we defined that when the robot-torso-link moves that the 'torso-lift-link' should move as well. ATTENTION this is not a good way to deal with the Problem. We are known of the issue and will find another way to fix mimic joints. 


<code lisp>
;;in low-level.lisp
;;;;;;;;;;;;;;;;; TORSO ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun move-torso (joint-angle)
  (declare (type number joint-angle))
  (let* ((bindings
           (car
            (prolog:prolog
             `(and (cram-robot-interfaces:robot ?robot)
                   (btr:bullet-world ?w)
                   (cram-robot-interfaces:robot-torso-link-joint ?robot ?_ ?joint)
                   (cram-robot-interfaces:joint-lower-limit ?robot ?joint ?lower)
                   (cram-robot-interfaces:joint-upper-limit ?robot ?joint ?upper)))))
         (lower-limit
           (cut:var-value '?lower bindings))
         (upper-limit
           (cut:var-value '?upper bindings))
         (cropped-joint-angle
           (if (< joint-angle lower-limit)
               lower-limit
               (if (> joint-angle upper-limit)
                   upper-limit
                   joint-angle))))
    (prolog:prolog
     `(btr:assert (btr:joint-state ?w ?robot ((?joint ,cropped-joint-angle))))
     bindings)
    (prolog:prolog
     `(btr:assert (btr:joint-state ?w ?robot (("torso_lift_joint" ,cropped-joint-angle))))
     bindings)
    (unless (< (abs (- joint-angle cropped-joint-angle)) 0.0001)
      (cpl:fail 'common-fail:torso-goal-not-reached
                :description (format nil "Torso goal ~a was out of joint limits" joint-angle)
                :torso joint-angle)))
  ;; (cram-occasions-events:on-event
  ;;  (make-instance 'cram-plan-occasions-events:robot-state-changed))
  )
</code>

**setup-urdf.lisp**

In the setup-urdf.lisp we make sure that the original urdf from the hsrb is working with our bullet world. This function needs to be called in the setup.lisp before spawning the robot.

<code lisp>
(in-package :hsrb-proj)

(defparameter *robot-parameter* "robot_description")

;;the collision-box for base_footprint, since the urdf does not provide it
(defparameter *collision-box*
  ">
   <collision>
      <origin rpy=\"0 0 0\" xyz=\"0 0 0.071\"/>
      <geometry>
        <box size=\"0.001 0.001 0.001\"/>
      </geometry>
   </collision>
  </link>")

;;the tool-frame for the hsrb, since the urdf does not provide one
(defparameter *tool-frame*
      "<joint name=\"gripper_tool_joint\" type=\"fixed\">
         <origin rpy=\"0.0 0.0 -80.14159265359\" xyz=\"0.012 0.0 0.075\"/>
         <parent link=\"hand_palm_link\" />
          <child link=\"gripper_tool_frame\"/>
        </joint>
        <link name=\"gripper_tool_frame\"/>")


;;call this function in your demo seutp.lisp it also takes care of the parameter in rob-int e.g
;; (defun setup-bullet-world ()
;;   (setf btr:*current-bullet-world* (make-instance 'btr:bt-reasoning-world))
;;   (let* ((robot (hsrb-proj::get-urdf))
;;          (kitchen (or *kitchen-urdf* .....

(defun get-urdf ()
  (let* ((robi (substitute #\SPACE #\` 
                           (roslisp:get-param *robot-parameter*)))
         (robot (setf rob-int:*robot-urdf*
                      (cl-urdf:parse-urdf
                       (concatenate 'string
                                    ;;plus 15 because of base_footprint"
                                    (subseq robi 0 (+ 15 (search "base_footprint" robi)))
                                    *collision-box*
                                    ;;plus 17 because of base_footprint"/>
                                    (subseq robi (+ 17 (search "base_footprint" robi))
                                            (search "<link name=\"hand_motor_dummy_link\"" robi))
                                    *tool-frame*
                                    (subseq robi (search "<link name=\"hand_motor_dummy_link\"" robi)))))))
    robot))
</code>