Merge pull request #6 from cram2/boxy-melodic

Boxy melodic

cram_3d_world/cram_btr_visibility_costmap/src/location-designator-facts.lisp

@@ -94,21 +94,28 @@
               (location-visibility-costmap ?designator ?costmap))))
 
   (<- (desig-check-to-see ?desig ?robot-pose)
-    (desig:desig-prop ?desig (:object ?obj))
-    (desig:desig-location-prop ?desig ?obj-pose)
-    (btr:bullet-world ?w)
-    (cram-robot-interfaces:robot ?robot)
-    (assert (btr:object-pose ?w ?robot ?robot-pose))
-    (btr:object-not-in-collision ?w ?robot)
-    (cram-robot-interfaces:camera-frame ?robot ?cam-frame)
-    (btr:head-pointing-at ?w ?robot ?obj-pose)
-    (-> (btr:object ?w ?obj)
-        (btr:visible ?w ?robot ?obj)
+    ;; (desig:desig-prop ?desig (:object ?obj))
+    ;; (desig:desig-location-prop ?desig ?obj-pose)
+    (desig:desig-prop ?desig (:object ?some-object))
+    (desig:current-designator ?some-object ?object)
+    (lisp-fun man-int:get-object-pose-in-map ?object ?to-see-pose)
+    (-> (lisp-pred identity ?to-see-pose)
+        (and (btr:bullet-world ?w)
+             (cram-robot-interfaces:robot ?robot)
+             (assert (btr:object-pose ?w ?robot ?robot-pose))
+             (btr:object-not-in-collision ?w ?robot)
+             (cram-robot-interfaces:camera-frame ?robot ?cam-frame)
+             (btr:head-pointing-at ?w ?robot ?to-see-pose)
+             (desig:desig-prop ?object (:name ?object-name))
+             (-> (btr:object ?w ?object-name)
+                 (btr:visible ?w ?robot ?object-name)
+                 (true)))
         (true)))
 
   (<- (location-valid ?desig ?pose (desig-check-to-see ?desig ?pose))
     (cram-robot-interfaces:visibility-designator ?desig)
-    (desig:desig-prop ?desig (:object ?obj)))
+    (or (desig:desig-prop ?desig (:object ?obj))
+        (true)))
 
   (<- (btr-desig-solution-valid ?desig ?solution)
     (btr-desig-solution-valid ?desig ?solution ?_))

cram_3d_world/cram_bullet_reasoning/src/robot-model.lisp

@@ -555,10 +555,10 @@ current joint states"
           (setf new-value (cl-transforms:normalize-angle new-value)))
         (unless (and (<= new-value (cl-urdf:upper limits))
                      (>= new-value (cl-urdf:lower limits)))
-          ;; (setf new-value (min (max new-value (cl-urdf:lower limits))
-          ;;                      (cl-urdf:upper limits)))
-          (error "Trying to assert joint value for ~a to ~a but limits are (~a; ~a)"
-                 name new-value (cl-urdf:lower limits) (cl-urdf:upper limits))))
+          (setf new-value (min (max new-value (cl-urdf:lower limits))
+                               (cl-urdf:upper limits)))
+          (warn "Trying to assert joint value for ~a to ~a but limits are (~a; ~a)"
+                name new-value (cl-urdf:lower limits) (cl-urdf:upper limits))))
       (let ((joint-transform
               (cl-transforms:transform*
                (cl-transforms:reference-transform

cram_3d_world/cram_bullet_reasoning_belief_state/src/belief-state.lisp

@@ -110,3 +110,35 @@ is replaced with replacement.
     (if robot-object
         (btr:set-robot-state-from-tf cram-tf:*transformer* robot-object)
         (warn "ROBOT was not defined. Have you loaded a robot package?"))))
+
+
+
+(defun vary-kitchen-urdf (&optional (new-joint-states
+                                     ;; '(("sink_area_footprint_joint"
+                                     ;;    ((1.855d0 1.3d0 0.0d0) (0 0 1 0)))
+                                     ;;   ("oven_area_footprint_joint"
+                                     ;;    ((1.855d0 2.47d0 0.0d0) (0 0 1 0)))
+                                     ;;   ("kitchen_island_footprint_joint"
+                                     ;;    ((-1.365d0 0.59d0 0.0d0) (0 0 0 1)))
+                                     ;;   ("fridge_area_footprint_joint"
+                                     ;;    ((1.845d0 -0.73d0 0.0d0) (0 0 1 0)))
+                                     ;;   ("table_area_main_joint"
+                                     ;;    ((-2.4d0 -1.5d0 0.0d0) (0 0 1 0))))
+                                     '(("sink_area_footprint_joint"
+                                        ((1.155d0 0.9d0 0.0d0) (0 0 0 1)))
+                                       ("oven_area_footprint_joint"
+                                        ((-0.155d0 2.97d0 0.0d0) (0 0 -0.5 0.5)))
+                                       ("kitchen_island_footprint_joint"
+                                        ((-0.60d0 -0.2d0 0.0d0) (0 0 0.5 0.5)))
+                                       ("fridge_area_footprint_joint"
+                                        ((-2.30d0 0.5d0 0.0d0) (0 0 0.5 0.5)))
+                                       ("table_area_main_joint"
+                                        ((1.6d0 -1.0d0 0.0d0) (0 0 0.5 0.5))))))
+  (let ((kitchen-urdf-joints (cl-urdf:joints *kitchen-urdf*)))
+   (mapc (lambda (joint-name-poses-list-pair)
+           (destructuring-bind (joint-name poses-list)
+               joint-name-poses-list-pair
+             (let ((joint (gethash joint-name kitchen-urdf-joints)))
+               (setf (slot-value joint 'cl-urdf:origin)
+                     (cram-tf:list->transform poses-list)))))
+         new-joint-states)))

cram_3d_world/cram_bullet_reasoning_belief_state/src/package.lisp

@@ -1,9 +1,10 @@
+;;;
 ;;; Copyright (c) 2012, Lorenz Moesenlechner <[email protected]>
 ;;; 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
@@ -13,7 +14,7 @@
 ;;;       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
@@ -45,5 +46,6 @@
    #:*robot-parameter* #:*kitchen-parameter*
    #:*spawn-debug-window*
    #:setup-world-database
+   #:vary-kitchen-urdf
    ;; process-modules
    #:world-state-detecting-pm))

cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-plans.lisp

@@ -198,7 +198,7 @@ retries with different search location or robot base location."
                                  "Search is about to give up. Retrying~%"))))
 
         ;; if the going action fails, pick another `?robot-location' sample and retry
-        (cpl:with-retry-counters ((robot-location-retries 2))
+        (cpl:with-retry-counters ((robot-location-retries 4))
           (cpl:with-failure-handling
               (((or common-fail:navigation-goal-in-collision
                     common-fail:looking-high-level-failure
@@ -277,7 +277,7 @@ and using the grasp and arm specified in `pick-up-action' (if not NIL)."
                    :description "Some designator could not be resolved.")))
 
     ;; take a new `?pick-up-robot-location' sample if a failure happens
-    (cpl:with-retry-counters ((relocation-for-ik-retries 10))
+    (cpl:with-retry-counters ((relocation-for-ik-retries 50))
       (cpl:with-failure-handling
           (((or common-fail:navigation-goal-in-collision
                 common-fail:looking-high-level-failure

cram_3d_world/cram_urdf_projection/src/low-level.lisp

@@ -39,6 +39,8 @@
 (defparameter *be-strict-with-collisions* nil
   "when grasping a spoon from table, fingers can collide with kitchen, so we might allow this")
 
+(defparameter *projection-convergence-delta-joint* 0.17 "in radiants, about 10 degrees")
+
 (defun robot-transform-in-map ()
   (let ((pose-in-map
           (cut:var-value
@@ -553,7 +555,27 @@ with the object, calculates similar angle around Y axis and applies the rotation
                  `(and
                    (btr:bullet-world ?world)
                    (rob-int:robot ?robot)
-                   (assert ?world (btr:joint-state ?robot ,joint-name-value-list)))))))))
+                   (assert ?world (btr:joint-state ?robot ,joint-name-value-list)))))
+               ;; check if joint state was indeed reached
+               (let* ((robot-object
+                        (btr:get-robot-object))
+                      (current-joint-state
+                        (mapcar (lambda (joint-name-and-value)
+                                  (btr:joint-state robot-object
+                                                   (car joint-name-and-value)))
+                                joint-name-value-list))
+                      (goal-joint-state
+                        (mapcar #'second joint-name-value-list)))
+                 (unless (cram-tf:values-converged current-joint-state goal-joint-state
+                                                   *projection-convergence-delta-joint*)
+                   (cpl:fail 'common-fail:manipulation-goal-not-reached
+                             :description (format nil "Projection did not converge to goal:~%~
+                                                   ~a (~a)~%should have been at~%~a~%~
+                                                   with delta-joint of ~a."
+                                                  arm
+                                                  current-joint-state
+                                                  goal-joint-state
+                                                  *projection-convergence-delta-joint*))))))))
     (set-configuration :left left-configuration)
     (set-configuration :right right-configuration)))
 

cram_common/cram_location_costmap/src/designator-integration.lisp

@@ -45,11 +45,6 @@
                 (unless (is-var ?cm)
                   ?cm))))))
 
-(defmethod desig:reset ((desig location-designator))
-  "Deleted cached costmap associated with this designator"
-  (let ((first-designator (desig:first-desig desig)))
-    (remhash first-designator *costmap-cache*)))
-
 (defun get-cached-costmap-maxvalue (costmap)
   (or (gethash costmap *costmap-max-values)
       (setf (gethash costmap *costmap-max-values)

cram_common/cram_tf/src/package.lisp

@@ -46,7 +46,7 @@
    #:pose->flat-list #:pose->flat-list-w-first
    #:pose->list
    #:flat-list->pose #:flat-list->transform #:flat-list-w-first->pose
-   #:list->pose
+   #:list->pose #:list->transform
    #:ensure-pose-in-frame #:ensure-point-in-frame
    #:translate-pose #:rotate-pose
    #:rotate-pose-in-own-frame #:rotate-transform-in-own-frame

cram_common/cram_tf/src/utilities.lisp

@@ -112,6 +112,13 @@
      (cl-transforms:make-3d-vector x y z)
      (cl-transforms:make-quaternion q1 q2 q3 w))))
 
+(defun list->transform (pose-list)
+  (destructuring-bind ((x y z) (q1 q2 q3 w))
+      pose-list
+    (cl-transforms:make-transform
+     (cl-transforms:make-3d-vector x y z)
+     (cl-transforms:make-quaternion q1 q2 q3 w))))
+
 (defun ensure-pose-in-frame (pose frame &key use-current-ros-time use-zero-time)
   (declare (type (or null cl-transforms:pose cl-transforms-stamped:pose-stamped)))
   (when pose

cram_core/cram_designators/src/cram-designators/designator-protocol.lisp

@@ -126,9 +126,15 @@
   (:method ((solution-1 array) (solution-2 array))
     (equalp solution-1 solution-2)))
 
-(defgeneric reset (desig)
-  (:documentation "Resets the designator by deleting all associated solutions.
-Used for recalculating a designator when its dependent designator got updated."))
+(defun reset (desig)
+  "Resets the designator by deleting all associated solutions.
+Used for recalculating a designator when its dependent designator got updated.
+The actual implementation creates a copy of the designator, such that only
+the description is preserved. The returned designator is equated to the input one,
+in case one would want to get back to the old one through the equated designator chain."
+  (let ((desig-copy (copy-designator desig)))
+    (equate desig desig-copy)
+    (setf desig desig-copy)))
 
 (defvar *designator-pprint-description* t
   "If set to T, DESIGNATOR objects will be pretty printed with their description.")
@@ -232,10 +238,6 @@ class (derived from class DESIGNATOR), e.g. OBJECT-DESIGNATOR."
   "Allow asking current-desig also on NULL objects."
   NIL)
 
-(defmethod reset ((desig designator))
-  "Empties the data slot"
-  (setf (slot-value desig 'data) nil))
-
 (defun newest-effective-designator (desig)
   (labels ((find-effective-desig (desig)
              (cond ((not desig) nil)

cram_core/cram_designators/src/cram-designators/location-designator.lisp

@@ -209,10 +209,6 @@ either :ACCEPT, :REJECT, :MAYBE-REJECT or :UNKNOWN."
 (defmethod resolve-designator ((desig location-designator) (role t))
   (resolve-location-designator-through-generators-and-validators desig))
 
-(defmethod reset :after ((desig location-designator))
-  "Empties the current-solution slot"
-  (setf (slot-value desig 'current-solution) nil))
-
 (defun validate-location-designator-solution (designator solution)
   (declare (type location-designator designator))
   (labels ((validate (validation-functions designator solution

cram_external_interfaces/cram_robosherlock/src/robosherlock-ros.lisp

@@ -75,6 +75,7 @@
                          key-value-pair
                        (let ((value (car values)))
                          ;; below are the only keys supported by RS at the moment
+                         ;; TODO: make sure that an already perceived object doesn't get reperceived with its original SHAPE and SIZE...
                          (if (or (eql key :type)
                                  (eql key :shape)
                                  (eql key :color)
@@ -217,7 +218,9 @@
               (:EdekaRedBowl
                :bowl)
               (:WeideMilchSmall
-               :milk))))
+               :milk)
+              (:BLUEPLASTICSPOON
+               :spoon))))
       (setf rs-answer
             (subst-if `(:type ,cram-type)
                       (lambda (x)

cram_knowrob/cram_knowrob_vr/cram-knowrob-vr.asd

@@ -48,15 +48,16 @@
                cram-prolog
                cram-common-failures
                cram-urdf-projection
-               cram-pr2-description
+               ;; cram-pr2-description
                cram-robot-interfaces
                cram-fetch-deliver-plans
                ;; costmaps are loaded for comparison with heuristics experiments
                cram-location-costmap
                cram-btr-visibility-costmap
                cram-btr-spatial-relations-costmap
                cram-robot-pose-gaussian-costmap
-               cram-occupancy-grid-costmap)
+               ;; cram-occupancy-grid-costmap
+               )
 	:components
 
 	((:module "src"