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 [2019/02/21 09:59] – [Create a CRAM robot description package] 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 201: Line 202:
 The HSR does not have so many links and joins as the PR2 so our life is much easier on that point.  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 needed in the //cram_hsrb// package: +Which files are later needed in the //cram_hsrb// package: 
   * **cram_hsrb_description**   * **cram_hsrb_description**
     * arms.lisp     * arms.lisp
Line 219: Line 220:
  
 For reference and starting point take a look at those package for the pr2 or hsrb. For reference and starting point take a look at those package for the pr2 or hsrb.
-Open your ''roslisp_repl'' and load every packages you will need. +
  
  
Line 226: 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 260: 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 272: Line 272:
 {{: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 289: 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.
  
  
Line 304: Line 303:
 (btr:object btr:*current-bullet-world* 'my-robot) (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: 
Line 378: 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. 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 this +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> <code lisp>
Line 428: Line 427:
 ==== 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>