Eurobin demo

cram-20.04-http.rosinstall

@@ -0,0 +1,31 @@
+# This rosinstall file is used to build docker images w/o an ssh-key setup
+- git:
+    local-name: cram
+    uri: http://github.com/cram2/cram.git
+    version: devel
+- git:
+    local-name: roslisp_common
+    uri: http://github.com/ros/roslisp_common.git
+    version: master  
+- git:
+    local-name: giskard_msgs
+    uri: http://github.com/SemRoCo/giskard_msgs.git
+    version: v0.5.0
+- git:
+    local-name: iai_common_msgs
+    uri: http://github.com/code-iai/iai_common_msgs.git
+    version: 46d6e9b9be386de9cf5e153f489c382e0c66f74c
+- git:
+    local-name: iai_maps
+    uri: http://github.com/code-iai/iai_maps.git
+    version: master
+- git:
+    local-name: iai_robots
+    uri: http://github.com/code-iai/iai_robots.git
+    version: master    
+- git:
+    local-name: kdl_ik_services
+    uri: http://github.com/cram2/kdl_ik_service.git      
+
+
+

cram_3d_world/cram_btr_spatial_relations_costmap/src/cost-functions.lisp

@@ -695,7 +695,7 @@ sampling is used the number of samples can be modified through the optional `sam
                              angle)))
            (if (eq tag :upside-down)
                (cl-transforms:q* orientation upside-down-orientation)
-               (print orientation))))
+               orientation)))
        (ecase tag
          (:random (loop for i from 1 to samples
                                           collect (random (* 2 pi))))

cram_3d_world/cram_bullet_reasoning/src/robot-model.lisp

@@ -304,6 +304,16 @@ Otherwise, the attachment is only used as information but does not affect the wo
           (second
            (find object-name (attached-objects robot-object) :key #'car))))
 
+(defun urdf-make-compound-shape (collision-elems collision-T-link color compound)
+  (let ((compound-shape (make-instance 'compound-shape)))
+    (dolist (collision collision-elems)
+      (add-child-shape compound-shape
+                       (cl-transforms:transform-pose
+                        collision-T-link (cl-urdf:origin collision))
+                       (urdf-make-collision-shape (cl-urdf:geometry collision) color compound)))
+    compound-shape))
+
+
 (defmethod initialize-instance :after ((robot-object robot-object)
                                        &key color name pose
                                          (collision-group :character-filter)
@@ -315,43 +325,56 @@ Otherwise, the attachment is only used as information but does not affect the wo
   Set `compound' T to spawn the robot as compound-shapes, instead of colored-boxes
   or convex-hull-shapes."
   (with-slots (rigid-bodies links urdf pose-reference-body) robot-object
-    (labels ((make-link-bodies (pose link)
+    (labels ((make-link-bodies (root-P-link link)
                "Returns the list of rigid bodies of `link' and all its sub-links"
-               (let* ((pose-transform (cl-transforms:reference-transform pose))
+               (let* ((root-T-link (cl-transforms:reference-transform root-P-link))
                       (collision-elem (cl-urdf:collision link))
+                      (collision-elems-list (cl-urdf:collisions link))
                       (bodies (mapcan (lambda (joint)
                                         (make-link-bodies (cl-transforms:transform-pose
-                                                           pose-transform (cl-urdf:origin joint))
+                                                           root-T-link (cl-urdf:origin joint))
                                                           (cl-urdf:child joint)))
-                                      (cl-urdf:to-joints link))))
+                                      (cl-urdf:to-joints link)))
+                      (color (apply-alpha-value
+                              (or color
+                                  (when (and (cl-urdf:visual link)
+                                             (cl-urdf:material
+                                              (cl-urdf:visual link)))
+                                    (cl-urdf:color
+                                     (cl-urdf:material
+                                      (cl-urdf:visual link))))
+                                  (let ((some-gray (/ (+ (random 5) 3) 10.0)))
+                                    (list some-gray some-gray some-gray
+                                          (or *robot-model-alpha* 1.0))))
+                              *robot-model-alpha*)))
                  (if collision-elem
-                     (cons (cons
-                            (cl-urdf:name link)
-                            (make-instance
-                                'rigid-body
-                              :name (make-rigid-body-name name (cl-urdf:name link))
-                              :mass 0
-                              :pose (cl-transforms:transform-pose
-                                     pose-transform (cl-urdf:origin collision-elem))
-                              :collision-shape (urdf-make-collision-shape
-                                                (cl-urdf:geometry collision-elem)
-                                                (apply-alpha-value
-                                                 (or color
-                                                     (when (and (cl-urdf:visual link)
-                                                                (cl-urdf:material
-                                                                 (cl-urdf:visual link)))
-                                                       (cl-urdf:color
-                                                        (cl-urdf:material
-                                                         (cl-urdf:visual link))))
-                                                     (let ((some-gray (/ (+ (random 5) 3) 10.0)))
-                                                       (list some-gray some-gray some-gray
-                                                             (or *robot-model-alpha* 1.0))))
-                                                 *robot-model-alpha*)
-                                                compound)
-                              :collision-flags :cf-default
-                              :group collision-group
-                              :mask collision-mask))
-                           bodies)
+                     (let ((link-P-collision (cl-urdf:origin collision-elem)))
+                       (cons (cons
+                              (cl-urdf:name link)
+                              (make-instance
+                                  'rigid-body
+                                :name (make-rigid-body-name name (cl-urdf:name link))
+                                :mass 0
+                                :pose (cl-transforms:transform-pose root-T-link link-P-collision)
+                                :collision-shape
+                                ;; If the urdf link has more than one collision element
+                                ;; make a compound shape and add all collisions to it.
+                                ;; For calculating the link pose, the rigid-body pose stays
+                                ;; the origin of the first collision root-P-collision.
+                                ;; The compound collision is offset by the inverse link-P-collision
+                                ;; to originate at root-T-link.
+                                (if (> (length collision-elems-list) 1)
+                                    (urdf-make-compound-shape collision-elems-list
+                                                              (cl-tf:transform-inv link-P-collision)
+                                                              color
+                                                              compound)
+                                    (urdf-make-collision-shape (cl-urdf:geometry collision-elem)
+                                                               color
+                                                               compound))
+                                :collision-flags :cf-default
+                                :group collision-group
+                                :mask collision-mask))
+                             bodies))
                      bodies))))
       (let ((bodies (make-link-bodies pose (cl-urdf:root-link urdf))))
         (initialize-rigid-bodies robot-object (mapcar #'cdr bodies))

cram_3d_world/cram_bullet_reasoning_belief_state/src/belief-state.lisp

@@ -38,8 +38,6 @@
    (cut:force-ll
     (prolog `(and
               (btr:bullet-world ?w)
-              ,@(when *spawn-debug-window*
-                  '((btr:debug-window ?w)))
               (btr:assert ?w (btr:object :static-plane
                                          :floor ((0 0 0) (0 0 0 1))
                                          :normal (0 0 1) :constant 0
@@ -74,7 +72,9 @@
                        (assert (btr:joint-state ?world ?robot
                                                 ((?torso-joint ?average-joint-value)))))
                   (warn "ROB-INT:ROBOT was not defined. ~
-                         Have you loaded a robot package?")))))))
+                         Have you loaded a robot package?"))
+              ,@(when *spawn-debug-window*
+                  '((btr:debug-window ?w))))))))
 
 (defun setup-world-database ()
   ;; make a clean world instance

cram_3d_world/cram_bullet_reasoning_belief_state/src/consistency-checking.lisp

@@ -42,11 +42,16 @@ Anything beyond this distance will cause a correction method to be triggered
 to try and correct the unstable object.
 If this threshold is NIL, no correction will happen.")
 
+(defparameter *perception-instability-correction-threshold* 0.03
+  "Once instability has been detected and the consistency checking kicks in,
+correct the pose in all axes where the offset is above this threshold.
+If the threshold is 0, correct along all axes.")
+
 (defparameter *perception-noise-correction-offset* 0.01
   "First distance in meters, with which the unstable perceived object will be
 counter-moved when it falls down upon simulation.")
 
-(defparameter *perception-noise-correction-iterations* 5
+(defparameter *perception-noise-correction-iterations* 30
   "Number of distances to try the correction approach with. In times.")
 
 (defparameter *perception-noise-correction-step* 0.01
@@ -80,7 +85,7 @@ Returns a list of ((:axis-1 sign-1) (:axis-2 sign-2)...)."
         with correction
         for distance in dist-list
         for axis in '(:x :y :z)
-        do (when (> (abs distance) *perception-instability-threshold*)
+        do (when (> (abs distance) *perception-instability-correction-threshold*)
              (push (/ distance (abs distance)) correction)
              (push axis correction))
         finally (return correction)))
@@ -112,7 +117,10 @@ Returns a list of ((:axis-1 sign-1) (:axis-2 sign-2)...)."
                 then (+ offset *perception-noise-correction-step*)
               for iteration from 1 to *perception-noise-correction-iterations*
               do (let* ((scaled-correction
-                          (mapcar (lambda (x) (if (numberp x) (* x offset) x))
+                          (mapcar (lambda (x)
+                                    (if (numberp x)
+                                        (* x offset)
+                                        x))
                                   object-pose-correction))
                         (object-corrected-pose
                           (apply #'cram-tf:translate-pose object-pose scaled-correction)))
@@ -132,4 +140,3 @@ Returns a list of ((:axis-1 sign-1) (:axis-2 sign-2)...)."
 
 
 
-

cram_3d_world/cram_bullet_reasoning_belief_state/src/event-handlers.lisp

@@ -85,6 +85,18 @@ and renames POSE into OLD-POSE."
         (register-object-designator-data
          object-data
          :type (desig:desig-prop-value (cpoe:event-object-designator event) :type)))
+    ;; visualize the pose as estimated by perception
+    (cram-tf:visualize-marker
+     object-pose
+     ;; :topic "cram_items"
+     :namespace (roslisp-utilities:rosify-underscores-lisp-name object-name)
+     :id 10
+     :marker-type :mesh_resource
+     :scale-list '(1 1 1)
+     :r-g-b-list '(1 0 0 0.2)
+     :mesh-path (second (assoc (desig:desig-prop-value
+                                (cpoe:event-object-designator event) :type)
+                               btr::*mesh-files*)))
     ;; after having spawned the object,
     ;; correct noise through world state consistency reasoning
     (stabilize-perceived-object-pose btr:*current-bullet-world* object-name object-pose)

cram_3d_world/cram_bullet_reasoning_belief_state/src/occasions.lisp

@@ -1,6 +1,7 @@
 ;;;
 ;;; Copyright (c) 2012, Lorenz Moesenlechner <moesenle@in.tum.de>
 ;;;               2020, Christopher Pollok <cpollok@uni-bremen.de>
+;;;               2022, Gayane Kazhoyan <kazhoyan@cs.uni-bremen.de>
 ;;; All rights reserved.
 ;;;
 ;;; Redistribution and use in source and binary forms, with or without
@@ -33,10 +34,12 @@
 (defparameter *torso-convergence-delta* 0.01 "In meters")
 (defparameter *gripper-joint-convergence-delta* 0.005 "In meters")
 (defparameter *arm-joints-convergence-delta* 0.0174 "In radians, about 1 deg.")
-(defparameter *ee-position-convergence-delta* 0.02 "In meters")
+(defparameter *ee-position-convergence-delta* 0.01 "In meters")
 (defparameter *ee-rotation-convergence-delta* 0.07 "In radians, about 4 deg.")
 (defparameter *looking-convergence-delta* 0.01 "In meters")
 (defparameter *looking-convergence-joints-delta* 0.07 "In radians, about 4 deg.")
+(defparameter *object-position-convergence-delta* 0.35 "In meters")
+(defparameter *object-rotation-convergence-delta* 0.70 "In radians, about 40 deg.")
 
 (def-fact-group occasions (cpoe:object-in-hand
                            cpoe:object-at-location
@@ -87,7 +90,9 @@
     (setof ?object (cpoe:object-in-hand ?object ?_) ?objects)
     (member ?object ?objects))
   ;;
-  (<- (cpoe:object-in-hand ?object :left-or-right)
+  (<- (cpoe:object-in-hand ?object ?arm)
+    (bound ?arm)
+    (equal ?arm :left-or-right)
     (or (cpoe:object-in-hand ?object :left)
         (cpoe:object-in-hand ?object :right)))
 
@@ -496,14 +501,38 @@
 
   ;; if ?location-designator is in the robot hand, check if object-in-hand
   (<- (%object-at-location ?world ?object ?location-designator)
-    (cpoe:object-in-hand ?object)
+    (cpoe:object-in-hand ?object ?_ ?_ ?object-holding-link)
     (desig:loc-desig? ?location-designator)
     (desig:current-designator ?location-designator ?current-location-designator)
     (or (desig:desig-prop ?current-location-designator (:on ?robot-designator))
         (desig:desig-prop ?current-location-designator (:in ?robot-designator)))
     (desig:current-designator ?robot-designator ?current-robot-designator)
     (spec:property ?current-robot-designator (:part-of ?robot-name))
-    (rob-int:robot ?robot-name))
+    (rob-int:robot ?robot-name)
+    (-> (spec:property ?current-robot-designator (:urdf-name ?robot-link))
+        (equal ?robot-link ?object-holding-link)
+        (true)))
+
+  ;; compare the exact poses
+  (<- (%object-at-location ?world ?object ?location-designator)
+    (not (cpoe:object-in-hand ?object))
+    (lisp-type ?location-designator desig:location-designator)
+    (btr:bullet-world ?world)
+    (lisp-fun desig:current-desig ?location-designator ?current-location)
+    (lisp-pred identity ?current-location)
+    (desig:designator-groundings ?current-location ?all-poses)
+    (once (member ?target-pose ?all-poses))
+    (object-designator-name ?object ?object-name)
+    (or (btr:object-pose ?world ?object-name ?object-pose)
+        (btr:object-bottom-pose ?world ?object-name ?object-pose))
+    (symbol-value cram-tf:*fixed-frame* ?fixed-frame)
+    (lisp-fun cl-transforms-stamped:pose->pose-stamped ?fixed-frame 0.0
+              ?object-pose ?object-pose-stamped)
+    (symbol-value *object-position-convergence-delta* ?delta-pos)
+    (symbol-value *object-rotation-convergence-delta* ?delta-rot)
+    (lisp-pred cram-tf:pose-stampeds-converged
+               ?object-pose-stamped ?target-pose
+               ?delta-pos ?delta-rot))
 
   (<- (%joint-at ?joint ?goal-state ?delta)
     (rob-int:robot ?robot)

cram_3d_world/cram_bullet_reasoning_utilities/src/package.lisp

@@ -36,6 +36,7 @@
   (:export
    ;; vis-tools.lisp
    #:visualize-designator-costmaps #:visualize-gripper
+   #:reset-debug-window
    ;; object-database.lisp
    #:scenario-objects-init-pose #:scenario-objects-default-color #:scenario-object-color
    #:scenario-object-shape #:scenario-object-extra-attributes

cram_3d_world/cram_bullet_reasoning_utilities/src/vis-tools.lisp

@@ -85,3 +85,16 @@
                        `(and (cram-robot-interfaces:robot ?robot)
                              (cram-robot-interfaces:standard<-particular-gripper-transform
                               ?robot ?transform)))))))))))))
+
+(defun reset-debug-window ()
+  "Terminates debug-window thread and launches a new one."
+  (let ((thread (find "Debug window"
+                      (sb-thread:list-all-threads)
+                      :key 'sb-thread:thread-name
+                      :test 'string=)))
+    (when thread
+      (sb-thread:terminate-thread thread)))
+  (when btr:*debug-window*
+    (cl-bullet-vis:close-window btr:*debug-window*))
+  (sleep 1)
+  (btr:add-debug-window btr:*current-bullet-world*))