1 ;;;
   2 ;;; Copyright (c) 2010, Lorenz Moesenlechner <moesenle@in.tum.de>
   3 ;;; All rights reserved.
   4 ;;; 
   5 ;;; Redistribution and use in source and binary forms, with or without
   6 ;;; modification, are permitted provided that the following conditions are met:
   7 ;;; 
   8 ;;;     * Redistributions of source code must retain the above copyright
   9 ;;;       notice, this list of conditions and the following disclaimer.
  10 ;;;     * Redistributions in binary form must reproduce the above copyright
  11 ;;;       notice, this list of conditions and the following disclaimer in the
  12 ;;;       documentation and/or other materials provided with the distribution.
  13 ;;;     * Neither the name of the Intelligent Autonomous Systems Group/
  14 ;;;       Technische Universitaet Muenchen nor the names of its contributors 
  15 ;;;       may be used to endorse or promote products derived from this software 
  16 ;;;       without specific prior written permission.
  17 ;;; 
  18 ;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  19 ;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  20 ;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21 ;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  22 ;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  23 ;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  24 ;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  25 ;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  26 ;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  27 ;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  28 ;;; POSSIBILITY OF SUCH DAMAGE.
  29 ;;;
  30 
  31 (in-package :ct)
  32 
  33 ;;;
  34 ;;; This file contains a simple script for picking up an object
  35 ;;; standing on the table and places it on the other table, right next
  36 ;;; to another object. You find a couple of utility functions for
  37 ;;; selecting the correct grasp, for calculating pre-put-down and
  38 ;;; pre-grasp poses and for calculating the put-down and pick-up
  39 ;;; vectors.
  40 ;;;
  41 ;;; TODO: The plans in this file work for picking up the milk when it
  42 ;;; is oriented correctly and standing at the right location. Make the
  43 ;;; plans work for the cerial box as you find it in simulation.
  44 
  45 (defun calculate-grasp (obj)
  46   "Returns a grasp to be used for picking up the object `obj'. The
  47 return type must be a string.
  48 
  49 `obj' is of type mapping_msgs-msg:<CollisionObject>"
  50   (declare (ignorable obj))
  51 ;;; TODO: Investigate the object dimensions and return a suitable
  52 ;;; grasp. You can access the object dimensions with
  53 ;;; 
  54 ;;; (obj-dimensions obj)
  55 ;;;
  56 ;;; which returns a cl-transforms:3d-vector. You can access x, y and z
  57 ;;; with the functions (cl-transforms:x vec) and friends. You can edit
  58 ;;; and add new grasps in the file
  59 ;;; cotesys_ros_grasping/config/pr2_grasps.yaml
  60 ;;; 
  61   "default")
  62 
  63 (defun calculate-pre-grasp (obj)
  64 ;;; TODO: This function returns the pre-grasp point as a
  65 ;;; point-stamped. Currently it doesn't take into account the grasp
  66 ;;; and the dimensions of the object. Make it more general so that
  67 ;;; grasping the milk and grasping the cerial box works.
  68   (let ((obj-pose (tf:transform-pose
  69                    *tf* :pose (obj-pose obj)
  70                    :target-frame "/base_link")))
  71     (tf:make-point-stamped
  72      (tf:frame-id obj-pose)
  73      (tf:stamp obj-pose)
  74      (cl-transforms:v+
  75       (cl-transforms:origin obj-pose)
  76       (cl-transforms:make-3d-vector
  77        -0.18 0 0.3)))))
  78 
  79 (defun calculate-grasp-vector (obj)
  80 ;;; TODO: This function returns the grasp vector that moves the
  81 ;;; gripper relative from the pre-grasp point to the point where to
  82 ;;; close the gripper. Currently, it doesn't take into account any
  83 ;;; object properties. Make it working for the milk and the cerial
  84 ;;; box.
  85   (declare (ignorable obj))
  86   (cl-transforms:make-3d-vector
  87    0.0 0 -0.2))
  88 
  89 (defun calculate-pre-put-down-pose (obj goal-pose)
  90 ;;; TODO: This function is similar to CALCULATE-PRE-GRASP but for
  91 ;;; putting down the object. Make it work for the milk and the cerial
  92 ;;; box.
  93   (declare (ignorable obj))
  94   (tf:make-point-stamped
  95    (tf:frame-id goal-pose)
  96    (tf:stamp goal-pose)
  97    (cl-transforms:v+
  98     (cl-transforms:origin goal-pose)
  99     (cl-transforms:make-3d-vector 0 0 0.3))))
 100 
 101 (defun calculate-put-down-vector (obj)
 102 ;;; TODO: This function is similar to CALCULATE-GRASP-VECTOR. Make it
 103 ;;; work for the milk and the cerial box.
 104   (declare (ignorable obj))
 105   (cl-transforms:make-3d-vector
 106    0.0 0 -0.2))
 107 
 108 (let ((initialized nil))
 109   
 110   (defun init ()
 111     (unless initialized
 112       (startup-ros :name "cram_tutorial")
 113       (setf initialized t))))
 114 
 115 (def-plan robot-at (loc)
 116 ;; This plan moves the robot to a specific location, represented by a
 117 ;; POSE-STAMPED.
 118   (ros-info (robot-at cram-tutorial) "Building collision map")
 119   (ros-info (robot-at cram-tutorial) "Parking arms")  
 120   (park-arm :left (calculate-grasp *left-gripper-obj*))
 121   (park-arm :right (calculate-grasp *right-gripper-obj*))
 122   (ros-info (robot-at cram-tutorial) "Navigating")
 123   (pursue
 124     (nav-to loc)
 125     ;; Hack to stop navigating if we got stuck
 126     ))
 127 
 128 (def-plan object-grasped (obj-name side)
 129   ;; This plan grasps an object with the gripper indicated by
 130   ;; `side'. Side is either :right or :left.
 131   (let ((retry-count 5))
 132     (with-failure-handling
 133         ((no-ik-solution (e)
 134            (declare (ignore e))
 135            (when (>= retry-count 0)
 136              (decf retry-count)
 137              ;;; TODO: This failure handling is not very smart. It
 138              ;;; just randomly moves the base and retries. As a bonus,
 139              ;;; make this smarter and more robust, for instance by
 140              ;;; changing the grasp pose a little bit or moving the
 141              ;;; robot to a better location. If you have time, add
 142              ;;; failure handling for more errors. You can find the
 143              ;;; definitions of the motion planning errors that might
 144              ;;; occurr in src/grasping.lisp, right at the beginning
 145              ;;; of the file.
 146              (robot-at (tf:make-pose-stamped
 147                         "/base_link" 0.0
 148                         (cl-transforms:make-3d-vector (- (random 0.2) 0.1)
 149                                                       (- (random 0.3) 0.15)
 150                                                       0.0)
 151                         (cl-transforms:make-quaternion 0 0 0 1)))
 152              (retry))))
 153         (let* ((obj (find-object obj-name))
 154                (grasp (calculate-grasp obj))
 155                (pre-grasp-pos (calculate-pre-grasp obj))
 156                (grasp-vector (calculate-grasp-vector obj))
 157                (lift-vector (cl-transforms:make-3d-vector 0 0 0.1)))
 158           (ros-info (object-grasped cram-tutorial) "Opening gripper")
 159           (open-gripper side)
 160           (ros-info (object-grasped cram-tutorial) "Building collision map")
 161           (take-collision-map)
 162           (ros-info (object-grasped cram-tutorial) "Moving arm to pre-grasp")
 163           (with-failure-handling
 164               ((trajectory-controller-failed (e)
 165                   (declare (ignore e))
 166                   (retry)))
 167             (move-arm-to-point side pre-grasp-pos grasp))
 168           (ros-info (object-grasped cram-tutorial) "Grasping")
 169           (move-arm-relative side grasp-vector)
 170           (close-gripper side)
 171           (attach-bounding-box side obj)
 172           (attach-obj side obj)
 173           (ros-info (object-grasped cram-tutorial) "Lifting")
 174           (move-arm-relative side lift-vector)))))
 175 
 176 (def-plan object-placed (goal-pose side)
 177   ;;; This plan puts down an object at the goal pose
 178   (let ((pre-put-down-pose (calculate-pre-put-down-pose (get-obj side) goal-pose))
 179         (put-down-vector (calculate-put-down-vector (get-obj side))))
 180     ;;; TODO: Add failure handling to this plan. Currently it doesn't
 181     ;;; handle any errors, but they might occurr during execution.
 182     (ros-info (object-grasped cram-tutorial) "Building collision map")
 183     (take-collision-map)
 184     (ros-info (object-placed cram-tutorial) "Moving arm to pre-put-down pose")
 185     (move-arm-to-point side pre-put-down-pose (calculate-grasp (get-obj side)))
 186     (ros-info (object-placed cram-tutorial) "Putting down")    
 187     (move-arm-relative side put-down-vector)
 188     (open-gripper side)
 189     (detach-bounding-box side (get-obj side))
 190     (remove-obj side)))
 191 
 192 (def-top-level-plan pick-and-place (obj side)
 193   ;;; This plan picks up an object named `obj' with the gripper
 194   ;;; indicated by `side', drives right and puts it right next to the
 195   ;;; first object it sees there.
 196   (take-collision-map)
 197   (park-arm :left)
 198   (park-arm :right)  
 199   (robot-at (get-approach-pose (find-checkerboard)))
 200   (move-spine 0.3)
 201   (robot-at (tf:make-pose-stamped
 202              "/base_link" 0.0
 203              (cl-transforms:make-3d-vector 0.5 0.0 0.0)
 204              (cl-transforms:make-quaternion 0 0 0 1)))
 205   (object-grasped obj side)
 206   (robot-at (tf:make-pose-stamped
 207              "/base_link" 0.0
 208              (cl-transforms:make-3d-vector 0.0 -1.2 0.0)
 209              (cl-transforms:make-quaternion 0 0 0 1)))
 210   (let* ((obj (find-object "cluster_0"))
 211          (obj-pose (obj-pose obj)))
 212     (object-placed (cl-tf:make-pose-stamped
 213                     (tf:frame-id obj-pose)
 214                     (tf:stamp obj-pose)
 215                     (cl-transforms:v+
 216                      (cl-transforms:origin obj-pose)
 217                      (cl-transforms:make-3d-vector -0.1 -0.3 0.0))
 218                     (cl-transforms:make-quaternion 0 0 0 1))
 219                    side)))