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

Boxy melodic

Author: artnie

cram_actions/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_bullet_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_bullet_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_bullet_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_bullet_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_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_integration/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"