;;;
;;; Copyright (c) 2010, Lorenz Moesenlechner <moesenle@in.tum.de>
;;; All rights reserved.
;;; 
;;; Redistribution and use in source and binary forms, with or without
;;; modification, are permitted provided that the following conditions are met:
;;; 
;;;     * Redistributions of source code must retain the above copyright
;;;       notice, this list of conditions and the following disclaimer.
;;;     * Redistributions in binary form must reproduce the above copyright
;;;       notice, this list of conditions and the following disclaimer in the
;;;       documentation and/or other materials provided with the distribution.
;;;     * Neither the name of the Intelligent Autonomous Systems Group/
;;;       Technische Universitaet Muenchen nor the names of its contributors 
;;;       may be used to endorse or promote products derived from this software 
;;;       without specific prior written permission.
;;; 
;;; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
;;; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
;;; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
;;; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
;;; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
;;; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
;;; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
;;; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
;;; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
;;; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
;;; POSSIBILITY OF SUCH DAMAGE.
;;;

(in-package :ct)

;;;
;;; This file contains a simple script for picking up an object
;;; standing on the table and places it on the other table, right next
;;; to another object. You find a couple of utility functions for
;;; selecting the correct grasp, for calculating pre-put-down and
;;; pre-grasp poses and for calculating the put-down and pick-up
;;; vectors.
;;;
;;; TODO: The plans in this file work for picking up the milk when it
;;; is oriented correctly and standing at the right location. Make the
;;; plans work for the cerial box as you find it in simulation.

(defun calculate-grasp (obj)
  "Returns a grasp to be used for picking up the object `obj'. The
return type must be a string.

`obj' is of type mapping_msgs-msg:<CollisionObject>"
  (declare (ignorable obj))
;;; TODO: Investigate the object dimensions and return a suitable
;;; grasp. You can access the object dimensions with
;;; 
;;; (obj-dimensions obj)
;;;
;;; which returns a cl-transforms:3d-vector. You can access x, y and z
;;; with the functions (cl-transforms:x vec) and friends. You can edit
;;; and add new grasps in the file
;;; cotesys_ros_grasping/config/pr2_grasps.yaml
;;; 
  "default")

(defun calculate-pre-grasp (obj)
;;; TODO: This function returns the pre-grasp point as a
;;; point-stamped. Currently it doesn't take into account the grasp
;;; and the dimensions of the object. Make it more general so that
;;; grasping the milk and grasping the cerial box works.
  (let ((obj-pose (tf:transform-pose
                   *tf* :pose (obj-pose obj)
                   :target-frame "/base_link")))
    (tf:make-point-stamped
     (tf:frame-id obj-pose)
     (tf:stamp obj-pose)
     (cl-transforms:v+
      (cl-transforms:origin obj-pose)
      (cl-transforms:make-3d-vector
       -0.18 0 0.3)))))

(defun calculate-grasp-vector (obj)
;;; TODO: This function returns the grasp vector that moves the
;;; gripper relative from the pre-grasp point to the point where to
;;; close the gripper. Currently, it doesn't take into account any
;;; object properties. Make it working for the milk and the cerial
;;; box.
  (declare (ignorable obj))
  (cl-transforms:make-3d-vector
   0.0 0 -0.2))

(defun calculate-pre-put-down-pose (obj goal-pose)
;;; TODO: This function is similar to CALCULATE-PRE-GRASP but for
;;; putting down the object. Make it work for the milk and the cerial
;;; box.
  (declare (ignorable obj))
  (tf:make-point-stamped
   (tf:frame-id goal-pose)
   (tf:stamp goal-pose)
   (cl-transforms:v+
    (cl-transforms:origin goal-pose)
    (cl-transforms:make-3d-vector 0 0 0.3))))

(defun calculate-put-down-vector (obj)
;;; TODO: This function is similar to CALCULATE-GRASP-VECTOR. Make it
;;; work for the milk and the cerial box.
  (declare (ignorable obj))
  (cl-transforms:make-3d-vector
   0.0 0 -0.2))

(let ((initialized nil))
  
  (defun init ()
    (unless initialized
      (startup-ros :name "cram_tutorial")
      (setf initialized t))))

(def-plan robot-at (loc)
;; This plan moves the robot to a specific location, represented by a
;; POSE-STAMPED.
  (ros-info (robot-at cram-tutorial) "Building collision map")
  (ros-info (robot-at cram-tutorial) "Parking arms")  
  (park-arm :left (calculate-grasp *left-gripper-obj*))
  (park-arm :right (calculate-grasp *right-gripper-obj*))
  (ros-info (robot-at cram-tutorial) "Navigating")
  (pursue
    (nav-to loc)
    ;; Hack to stop navigating if we got stuck
    ))

(def-plan object-grasped (obj-name side)
  ;; This plan grasps an object with the gripper indicated by
  ;; `side'. Side is either :right or :left.
  (let ((retry-count 5))
    (with-failure-handling
        ((no-ik-solution (e)
           (declare (ignore e))
           (when (>= retry-count 0)
             (decf retry-count)
             ;;; TODO: This failure handling is not very smart. It
             ;;; just randomly moves the base and retries. As a bonus,
             ;;; make this smarter and more robust, for instance by
             ;;; changing the grasp pose a little bit or moving the
             ;;; robot to a better location. If you have time, add
             ;;; failure handling for more errors. You can find the
             ;;; definitions of the motion planning errors that might
             ;;; occurr in src/grasping.lisp, right at the beginning
             ;;; of the file.
             (robot-at (tf:make-pose-stamped
                        "/base_link" 0.0
                        (cl-transforms:make-3d-vector (- (random 0.2) 0.1)
                                                      (- (random 0.3) 0.15)
                                                      0.0)
                        (cl-transforms:make-quaternion 0 0 0 1)))
             (retry))))
        (let* ((obj (find-object obj-name))
               (grasp (calculate-grasp obj))
               (pre-grasp-pos (calculate-pre-grasp obj))
               (grasp-vector (calculate-grasp-vector obj))
               (lift-vector (cl-transforms:make-3d-vector 0 0 0.1)))
          (ros-info (object-grasped cram-tutorial) "Opening gripper")
          (open-gripper side)
          (ros-info (object-grasped cram-tutorial) "Building collision map")
          (take-collision-map)
          (ros-info (object-grasped cram-tutorial) "Moving arm to pre-grasp")
          (with-failure-handling
              ((trajectory-controller-failed (e)
                  (declare (ignore e))
                  (retry)))
            (move-arm-to-point side pre-grasp-pos grasp))
          (ros-info (object-grasped cram-tutorial) "Grasping")
          (move-arm-relative side grasp-vector)
          (close-gripper side)
          (attach-bounding-box side obj)
          (attach-obj side obj)
          (ros-info (object-grasped cram-tutorial) "Lifting")
          (move-arm-relative side lift-vector)))))

(def-plan object-placed (goal-pose side)
  ;;; This plan puts down an object at the goal pose
  (let ((pre-put-down-pose (calculate-pre-put-down-pose (get-obj side) goal-pose))
        (put-down-vector (calculate-put-down-vector (get-obj side))))
    ;;; TODO: Add failure handling to this plan. Currently it doesn't
    ;;; handle any errors, but they might occurr during execution.
    (ros-info (object-grasped cram-tutorial) "Building collision map")
    (take-collision-map)
    (ros-info (object-placed cram-tutorial) "Moving arm to pre-put-down pose")
    (move-arm-to-point side pre-put-down-pose (calculate-grasp (get-obj side)))
    (ros-info (object-placed cram-tutorial) "Putting down")    
    (move-arm-relative side put-down-vector)
    (open-gripper side)
    (detach-bounding-box side (get-obj side))
    (remove-obj side)))

(def-top-level-plan pick-and-place (obj side)
  ;;; This plan picks up an object named `obj' with the gripper
  ;;; indicated by `side', drives right and puts it right next to the
  ;;; first object it sees there.
  (take-collision-map)
  (park-arm :left)
  (park-arm :right)  
  (robot-at (get-approach-pose (find-checkerboard)))
  (move-spine 0.3)
  (robot-at (tf:make-pose-stamped
             "/base_link" 0.0
             (cl-transforms:make-3d-vector 0.5 0.0 0.0)
             (cl-transforms:make-quaternion 0 0 0 1)))
  (object-grasped obj side)
  (robot-at (tf:make-pose-stamped
             "/base_link" 0.0
             (cl-transforms:make-3d-vector 0.0 -1.2 0.0)
             (cl-transforms:make-quaternion 0 0 0 1)))
  (let* ((obj (find-object "cluster_0"))
         (obj-pose (obj-pose obj)))
    (object-placed (cl-tf:make-pose-stamped
                    (tf:frame-id obj-pose)
                    (tf:stamp obj-pose)
                    (cl-transforms:v+
                     (cl-transforms:origin obj-pose)
                     (cl-transforms:make-3d-vector -0.1 -0.3 0.0))
                    (cl-transforms:make-quaternion 0 0 0 1))
                   side)))