Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision
Previous revision
tutorials:advanced:new-robot [2018/12/09 19:40] – [Spawn the robot in Bullet] vanessatutorials:advanced:new-robot [2019/03/05 12:08] (current) – [Action designators and plans] gkazhoya
Line 188: Line 188:
 ===== Action designators and plans ===== ===== Action designators and plans =====
  
-This is TODO... +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 ===== ===== Projection for your new robot =====
  
Line 199: Line 200:
  
 Then simply execute in your terminal: <code> roslaunch hsr_description upload.launch </code> 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. Open your ''roslisp_repl'' and load every packages you will need.+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. 
  
  
Line 206: Line 227:
 ==== Spawn the robot in Bullet ==== ==== 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.+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
  
  
Line 240: Line 261:
                     (cram-robot-interfaces:robot ?robot)                     (cram-robot-interfaces:robot ?robot)
                     (assert (btr:object ?world :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf ,robot-urdf))                     (assert (btr:object ?world :urdf ?robot ((0 0 0) (0 0 0 1)) :urdf ,robot-urdf))
-                    (cram-robot-interfaces:robot-arms-parking-joint-states ?robot ?joint-states) 
                     (assert (btr:joint-state ?world ?robot ?joint-states))                     (assert (btr:joint-state ?world ?robot ?joint-states))
                     (assert (btr:joint-state ?world ?robot (("torso_lift_joint" 0.15d0)))))))                     (assert (btr:joint-state ?world ?robot (("torso_lift_joint" 0.15d0)))))))
Line 250: Line 270:
  
  
-{{ :tutorials:advanced:hsr_in_bullet_world.png?700|}}+{{:tutorials:advanced:hsr_in_bullet_world.png?700|}}
  
- +The ''(cram-tf::init-tf)''command initalized some variables:
-At this moment the command: ''(cram-tf::init-tf)'' loads some variables:+
 <code lisp> <code lisp>
 (defvar *robot-base-frame* nil (defvar *robot-base-frame* nil
Line 269: Line 288:
   "Tool frame of the right arm. Initialized from CRAM robot desciption.")   "Tool frame of the right arm. Initialized from CRAM robot desciption.")
 </code> </code>
-Usually u will define those parameters in the ROBOT_description package. For now to simply test the beginning it is okay 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.+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. Try to move the robot joints using Bullet Prolog predicates or Lisp functions. E.g.
  
-<code>+<code lisp>
 (btr:set-robot-state-from-joints (btr:set-robot-state-from-joints
- '(("torso_joint 0.1))+ '(("torso_lift_joint 0.1))
  (btr:object btr:*current-bullet-world* 'my-robot))  (btr:object btr:*current-bullet-world* 'my-robot))
 </code> </code>
Line 282: Line 301:
 If you are unsure what joints are possible to move use: If you are unsure what joints are possible to move use:
 <code lisp> <code lisp>
-(btr:object btr:*current-bullet-world* 'cram-pr2-description:pr2)+(btr:object btr:*current-bullet-world* 'my-robot)
 </code> </code>
-to get the instance of the spawned Bullet object there is a function.+
  
 After lifting the torso the HSR should looks like this:  After lifting the torso the HSR should looks like this: 
  
 {{:tutorials:advanced:hsrb_torso_move.png?700|}} {{: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.
  
  
Line 299: Line 322:
 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. 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''.+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. Define the most important links and joints there e.g.
  
Line 319: Line 342:
  
  
-  (<- (robot-tool-frame hsrb :left "gripper_tool_frame"))+  (<- (robot-tool-frame hsrb :left "gripper_tool_joint"))
   (<- (robot-tool-frame hsrb :right "ATTENTION please don't use this frame"))   (<- (robot-tool-frame hsrb :right "ATTENTION please don't use this frame"))
  
Line 333: Line 356:
  
  
-  (<- (gripper-joint hsrb :left "hand_motor_frame_joint"))+  (<- (gripper-joint hsrb :left "hand_motor_joint"))
  
   (<- (gripper-link hsrb :left ?link)   (<- (gripper-link hsrb :left ?link)
Line 354: Line 377:
 </code> </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:+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> +<code lisp
-<joint name="gripper_tool_joint" type="fixed"> +;;the tool-frame for the hsrb, since the urdf does not provide one 
-        <origin xyz="0 0 0.07rpy="0 0 1.6" /> +(defparameter *tool-frame* 
-         <parent link="${prefix}_palm_link" /> +      "<joint name=\"gripper_tool_joint\" type=\"fixed\"> 
-          <child link="gripper_tool_frame"/>+         <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>         </joint>
-        <link name="gripper_tool_frame"/>+        <link name=\"gripper_tool_frame\"/>")
 </code> </code>
-{{ :tutorials:advanced:gripper_tool_frame_hsrb.png?700 |}}+{{:tutorials:advanced:gripper_tool_frame_hsrb.png?700|}}
  
  
 Then the moveable arm-joints/links are defined and the gripper-joints/links. Then the moveable arm-joints/links are defined and the gripper-joints/links.
 +
 +
  
 Next step is to define the general-knowledge.lisp: Next step is to define the general-knowledge.lisp:
Line 378: Line 405:
                                robot-base-frame robot-torso-link-joint                                robot-base-frame robot-torso-link-joint
                                arm                                arm
-                               camera-frame)+                               camera-frame 
 +                               robot-pan-tilt-links robot-pan-tilt-joints)
   (<- (robot hsrb))   (<- (robot hsrb))
  
Line 384: Line 412:
  
   (<- (robot-base-frame hsrb "base_footprint"))   (<- (robot-base-frame hsrb "base_footprint"))
-  (<- (robot-torso-link-joint hsrb "torso_lift_joint" "torso_lift_link"))+  (<- (robot-torso-link-joint hsrb "arm_lift_link" "arm_lift_joint"))
  
-  (<- (arm hsrb :left))+  ;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"))
  
-  (<- (camera-frame hsrb "/hsrb/head_rgbd_sensor")))+  (<- (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> </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. 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 ==== ==== 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.+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>