Merge pull request cram2#9 from cram2/boxy-melodic

Boxy melodic

Author: artnie

.gitignore

@@ -2,4 +2,5 @@
 build
 *#
 latest_asdf_check
-.#*
+.#*
+*~

LICENSE

@@ -0,0 +1,30 @@
+
+Copyright (c) 2009 - 2021, CRAM team
+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 name of the
+      Institute for Artificial Intelligence/Universitaet Bremen,
+      nor the names of their 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.

cram-18.04.rosinstall

@@ -0,0 +1,21 @@
+# THIS IS AN AUTOGENERATED FILE, LAST GENERATED USING wstool ON 2018-10-16
+- git:
+    local-name: cram
+    uri: https://github.com/cram2/cram.git
+    version: master-melodic
+- git:
+    local-name: giskard_msgs
+    uri: https://github.com/SemRoCo/giskard_msgs.git
+    version: 5bfb2a2dc0d58635fab4eceaf93a43c2729a4cdf
+- git:
+    local-name: iai_common_msgs
+    uri: https://github.com/code-iai/iai_common_msgs.git
+    version: 46d6e9b9be386de9cf5e153f489c382e0c66f74c
+- git:
+    local-name: iai_maps
+    uri: https://github.com/code-iai/iai_maps.git
+    version: 497632f5bbf8f028b5cf25470efeefbaaf9ce388
+- git:
+    local-name: genlisp
+    uri: https://github.com/ros/genlisp.git
+    version: 3ac633abacdf5ab321d23ed013c7d5b7da97736d

cram_actions/cram-fetch-deliver-plans.asd

@@ -44,7 +44,9 @@
 
                cram-common-failures
                cram-mobile-pick-place-plans
+               cram-robot-interfaces ; at least for (robot ?r)
                cram-manipulation-interfaces
+               cram-location-costmap ; for resetting the costmap cache
 
                cram-urdf-projection-reasoning ; for projection-based reasoning
                cram-urdf-environment-manipulation)

cram_actions/cram_mobile_pick_place_plans/cram-mobile-pick-place-plans.asd

@@ -43,7 +43,7 @@
                cram-designators
                cram-occasions-events
                cram-executive
-               cram-utilities ; for cut:var-value of prolog stuff
+               cram-utilities ; for cut:var-value of prolog stuff and equalize-lists
 
                cram-tf
                cram-plan-occasions-events

cram_actions/cram_mobile_pick_place_plans/src/atomic-action-designators.lisp

@@ -52,8 +52,8 @@
 
 
 (defun go-with-torso (&key
-                     ((:joint-angle ?joint-angle))
-                     &allow-other-keys)
+                        ((:joint-angle ?joint-angle))
+                      &allow-other-keys)
   (declare (type (or number keyword) ?joint-angle))
   "Go to `?joint-angle' with torso, if a failure happens propagate it up, robot-state-changed event."
   (unwind-protect
@@ -79,12 +79,16 @@
                                 ((:collision-object-b ?collision-object-b))
                                 ((:collision-object-b-link ?collision-object-b-link))
                                 ((:collision-object-a ?collision-object-a))
-                                ((:move-the-ass ?move-the-ass))
+                                ((:move-base ?move-base))
+                                ((:prefer-base ?prefer-base))
+                                ((:align-planes-left ?align-planes-left))
+                                ((:align-planes-right ?align-planes-right))
                               &allow-other-keys)
   (declare (type (or list cl-transforms-stamped:pose-stamped) left-poses right-poses)
            (type (or null keyword) ?collision-mode)
            (type (or null symbol) ?collision-object-b ?collision-object-a)
-           (type (or null string symbol) ?collision-object-b-link))
+           (type (or null string symbol) ?collision-object-b-link)
+           (type boolean ?move-base ?prefer-base ?align-planes-left ?align-planes-right))
   "Move arms through all but last poses of `left-poses' and `right-poses',
 while ignoring failures; and execute the last pose with propagating the failures."
 
@@ -120,8 +124,14 @@ while ignoring failures; and execute the last pose with propagating the failures
                           (collision-object-b-link ?collision-object-b-link))
                         (desig:when ?collision-object-a
                           (collision-object-a ?collision-object-a))
-                        (desig:when ?move-the-ass
-                          (move-the-ass ?move-the-ass))))
+                        (desig:when ?move-base
+                          (move-base ?move-base))
+                        (desig:when ?prefer-base
+                          (prefer-base ?prefer-base))
+                        (desig:when ?align-planes-left
+                          (align-planes-left ?align-planes-left))
+                        (desig:when ?align-planes-right
+                          (align-planes-right ?align-planes-right))))
 
               (cram-occasions-events:on-event
                (make-instance 'cram-plan-occasions-events:robot-state-changed))))
@@ -152,16 +162,78 @@ while ignoring failures; and execute the last pose with propagating the failures
                   (collision-object-b-link ?collision-object-b-link))
                 (desig:when ?collision-object-a
                   (collision-object-a ?collision-object-a))
-                (desig:when ?move-the-ass
-                  (move-the-ass ?move-the-ass))))
+                (desig:when ?move-base
+                  (move-base ?move-base))
+                (desig:when ?prefer-base
+                  (prefer-base ?prefer-base))
+                (desig:when ?align-planes-left
+                  (align-planes-left ?align-planes-left))
+                (desig:when ?align-planes-right
+                  (align-planes-right ?align-planes-right))))
 
       (cram-occasions-events:on-event
        (make-instance 'cram-plan-occasions-events:robot-state-changed)))))
 
+(defun manipulate-environment (&key
+                                 ((:type ?type))
+                                 ((:arm ?arm))
+                                 ((:poses ?poses))
+                                 ((:distance ?distance))
+                                 ((:collision-mode ?collision-mode))
+                                 ((:collision-object-b ?collision-object-b))
+                                 ((:collision-object-b-link ?collision-object-b-link))
+                                 ((:collision-object-a ?collision-object-a))
+                                 ((:move-base ?move-base))
+                                 ((:prefer-base ?prefer-base))
+                                 ((:align-planes-left ?align-planes-left))
+                                 ((:align-planes-right ?align-planes-right))
+                               &allow-other-keys)
+  (declare (type keyword ?type ?arm)
+           (type list ?poses)
+           (type (or number null) ?distance)
+           (type (or keyword null) ?collision-mode)
+           (type (or symbol null) ?collision-object-b ?collision-object-a)
+           (type (or string symbol null) ?collision-object-b-link)
+           (type boolean ?move-base ?prefer-base
+                 ?align-planes-left ?align-planes-right))
+  "Execute an environment manipulation trajectory.
+In projection it would be executed by following the list of poses in cartesian space.
+With a continuous motion planner one could have fluent arch trajectories etc.
+`?type' is either :PUSHING or :PULLING."
+
+  (unwind-protect
+       (exe:perform
+        (desig:a motion
+                 (type ?type)
+                 (arm ?arm)
+                 (poses ?poses)
+                 (desig:when ?distance
+                   (joint-angle ?distance))
+                 (desig:when ?collision-mode
+                   (collision-mode ?collision-mode))
+                 (desig:when ?collision-object-b
+                   (collision-object-b ?collision-object-b))
+                 (desig:when ?collision-object-b-link
+                   (collision-object-b-link ?collision-object-b-link))
+                 (desig:when ?collision-object-a
+                   (collision-object-a ?collision-object-a))
+                 (desig:when ?move-base
+                   (move-base ?move-base))
+                 (desig:when ?prefer-base
+                   (prefer-base ?prefer-base))
+                 (desig:when ?align-planes-left
+                   (align-planes-left ?align-planes-left))
+                 (desig:when ?align-planes-right
+                   (align-planes-right ?align-planes-right))))
+    (cram-occasions-events:on-event
+     (make-instance 'cram-plan-occasions-events:robot-state-changed))))
+
 
 (defun move-arms-into-configuration (&key
                                        ((:left-joint-states ?left-joint-states))
                                        ((:right-joint-states ?right-joint-states))
+                                       ((:align-planes-left ?align-planes-left))
+                                       ((:align-planes-right ?align-planes-right))
                                      &allow-other-keys)
   (declare (type list ?left-joint-states ?right-joint-states))
   "Calls moving-arm-joints motion, while ignoring failures, and robot-state-changed event."
@@ -179,7 +251,11 @@ while ignoring failures; and execute the last pose with propagating the failures
                    (desig:when ?left-joint-states
                      (left-joint-states ?left-joint-states))
                    (desig:when ?right-joint-states
-                     (right-joint-states ?right-joint-states))))
+                     (right-joint-states ?right-joint-states))
+                   (desig:when ?align-planes-left
+                     (align-planes-left ?align-planes-left))
+                   (desig:when ?align-planes-right
+                     (align-planes-right ?align-planes-right))))
          ;; (cpl:seq
          ;;   (exe:perform
          ;;    (desig:a motion
@@ -224,7 +300,6 @@ while ignoring failures; and execute the last pose with propagating the failures
                ((:gripper ?left-or-right))
                ((:effort ?effort))
                ((:object object-designator))
-               ((:grasped-object new-object-designator))
                ((:grasp ?grasp))
              &allow-other-keys)
   (declare (type (or keyword list) ?left-or-right)
@@ -247,13 +322,14 @@ In any case, issue ROBOT-STATE-CHANGED event."
                      (desig:when ?effort
                        (effort ?effort))))
            (roslisp:ros-info (pick-place grip) "Assert grasp into knowledge base")
-           (cram-occasions-events:on-event
-            (make-instance 'cpoe:object-attached-robot
-              :arm ?left-or-right
-              :object-name (desig:desig-prop-value object-designator :name)
-              :grasp ?grasp))
-           (desig:equate object-designator new-object-designator)
-           new-object-designator))
+           (when object-designator
+             (cram-occasions-events:on-event
+              (make-instance 'cpoe:object-attached-robot
+                :arm ?left-or-right
+                :object-name (desig:desig-prop-value object-designator :name)
+                :object-designator object-designator
+                :grasp ?grasp))
+             (desig:current-desig object-designator))))
     (cram-occasions-events:on-event
      (make-instance 'cram-plan-occasions-events:robot-state-changed))))
 
@@ -337,9 +413,11 @@ In any case, issue ROBOT-STATE-CHANGED event."
   (declare (type desig:object-designator ?object-designator))
   "Call detecting motion on `?object-designator', retry on failure, issue perceived event,
 equate resulting designator to the original one."
-  (let ((retries (if (find :cad-model (desig:properties ?object-designator) :key #'car)
-                     1
-                     4)))
+  (let ((retries 1
+          ;; (if (find :cad-model (desig:properties ?object-designator) :key #'car)
+          ;;     1
+          ;;     4)
+          ))
     (cpl:with-retry-counters ((perceive-retries retries))
       (cpl:with-failure-handling
           ((common-fail:perception-low-level-failure (e)
@@ -355,19 +433,38 @@ equate resulting designator to the original one."
                (resulting-designator
                  (funcall object-chosing-function resulting-designators)))
           (if (listp resulting-designators)
-              (mapcar (lambda (desig)
-                        (cram-occasions-events:on-event
-                         (make-instance 'cram-plan-occasions-events:object-perceived-event
-                           :object-designator desig
-                           :perception-source :whatever))
-                        ;; doesn't make sense to equate all these desigs together
-                        ;; (desig:equate ?object-designator desig)
-                        )
-                      resulting-designators)
+              (mapc (lambda (desig)
+                      (cram-occasions-events:on-event
+                       (make-instance 'cram-plan-occasions-events:object-perceived-event
+                         :object-designator desig
+                         :perception-source :whatever))
+                      ;; doesn't make sense to equate all these desigs together
+                      ;; (desig:equate ?object-designator desig)
+                      )
+                    resulting-designators)
               (progn
                 (cram-occasions-events:on-event
                  (make-instance 'cram-plan-occasions-events:object-perceived-event
                    :object-designator resulting-designators
                    :perception-source :whatever))
                 (desig:equate ?object-designator resulting-designator)))
-          resulting-designator)))))
+          (desig:current-desig resulting-designator))))))
+
+
+(defun park-arms (&key
+                    ((:left-arm ?left-arm-p))
+                    ((:right-arm ?right-arm-p))
+                  &allow-other-keys)
+  (declare (type boolean ?left-arm-p ?right-arm-p))
+  "Puts the arms into a parking configuration"
+  (let* ((left-config (when ?left-arm-p :park))
+         (right-config (when ?right-arm-p :park))
+         (?goal `(cpoe:arms-positioned-at ,left-config ,right-config)))
+    (exe:perform
+     (desig:an action
+               (type positioning-arm)
+               (desig:when ?left-arm-p
+                 (left-configuration park))
+               (desig:when ?right-arm-p
+                 (right-configuration park))
+               (goal ?goal)))))

cram_actions/cram_mobile_pick_place_plans/src/atomic-action-plans.lisp

@@ -84,9 +84,7 @@
 (defun perceive-and-drive-and-pick-up-plan (?type &key (?arm '(:left :right))
                                                     ?color ?cad-model)
   (exe:perform (desig:an action
-                         (type positioning-arm)
-                         (left-configuration park)
-                         (right-configuration park)))
+                         (type parking-arms)))
   (let ((object-description `((:type ,?type))))
     (when ?color
       (push `(:color ,?color) object-description))
@@ -133,7 +131,7 @@
 ;;                               cram-tf:*fixed-frame*
 ;;                               :use-zero-time t)))
 ;;
-;;           (let ((?pose-for-base (cl-tf:pose->pose-stamped
+;;           (let ((?pose-for-base (cl-transforms:pose->pose-stamped
 ;;                                  "map"
 ;;                                  0.0
 ;;                                  (cl-transforms-stamped:make-identity-pose))))