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.

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 motions.lisp file:

This is how we define motion designator grounding aka referencing for looking motions:

(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))))

Motions are executed with PERFORM calls:

(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))))))     

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

We start with low-level ROS clients for our PR2: it's the 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

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

(perform (a motion
            (type looking)
            (direction forward)))

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.

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

For the PR2 we can see the code in the cram_pr2_process_modules package, e.g. in ptu.lisp file.

First, the PM references the motion designator that it gets (that's implemented in a file we looked at before – 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.

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 – let's look in the designators.lisp file.

(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))))))))

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

Let's define the low-level interfaces for projected PR2 – the low-level.lisp file.

Now let's define a PTU process module – process-modules.lisp file.

Now let's say which PM corresponds to which motion designator – same file bottom.

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

(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)))))))))

• Create a new CRAM package with dependency on cram-prolog and cram-designators. In your namespace use cram-prolog for convenience. Look at creating a CRAM package tutorial for reference.

• Implement your motion designator grounding predicates. See example above. To learn more about grounding motion designators look at the 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 roslisp tutorials for more on this.

• Define process modules for your low-level functions. This will add dependency on cram-process-modules. See PM tutorial. Also define Prolog predicates matching-process-module and available-process-module for your PMs (see 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.

Everything above motion designators should be independent of the robot platform. For example, try performing a picking up action with your robot. See this tutorial for an example.

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:

 roslaunch hsr_description upload.launch 

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.

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:

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)))))))

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

The (cram-tf::init-tf)command initalized some variables:

(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.")

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.

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

If you are unsure what joints are possible to move use:

(btr:object btr:*current-bullet-world* 'my-robot)

After lifting the torso the HSR should looks like this:

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.

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.

;;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)))

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:

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

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.

;;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\"/>")

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

Next step is to define the general-knowledge.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")))

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.

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.

;;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))
  )

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.

(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))