Merge pull request #1 from sunava/popcorn-noetic

Popcorn noetic and 2handed manipulation

README.md

@@ -20,6 +20,7 @@ The core packages of CRAM are implemented in Common Lisp (with a little bit of C
 * `rosdep install --ignore-src --from-paths src/ -r`
 * `catkin_make`
 
+
 For ROS noetic and the current packages, one thing needs to be fixed. The package octomap contains a dependency to a ROS2 package, which can be ignored. Open the package.xml of octomap.
 
 * `roscd octomap`
@@ -59,6 +60,7 @@ If changes are made to the code and a **PullRequest** results from it, please ch
   * WIP     
 
 ----
+
 ### Directory
 * **cram_3d_world** Bullet physics engine-based and OpenGl offscreen rendering-based reasoning mechanisms.
 * **cram_3rdparty** 3rd party Lisp liabraries wrapped into ROS packages.

cram-20.04.rosinstall

@@ -34,5 +34,6 @@
 - git:
     local-name: kdl_ik_services
     uri: [email protected]:cram2/kdl_ik_service.git      
+
 
 

cram-install.sh

@@ -3,7 +3,7 @@
 install -d ~/roscram/cram_ws/src
 cd ~/roscram/cram_ws/src
 wstool init
-wstool merge https://raw.githubusercontent.com/cram2/cram/boxy-noetic/cram-20.04.rosinstall
+wstool merge https://raw.githubusercontent.com/cram2/cram/noetic/cram-20.04.rosinstall
 wstool update
 cd ..
 rosdep update
@@ -12,6 +12,7 @@ install -d ~/roscram/ros_emacs_utils_ws/src
 cd ~/roscram/ros_emacs_utils_ws/src
 git clone [email protected]:code-iai/ros_emacs_utils.git
 cd ..
+#install is necessary
 catkin_make install
 catkin_make
 source devel/setup.bash

cram_3d_world/cram_btr_spatial_relations_costmap/src/prolog.lisp

@@ -1,3 +1,4 @@
+
 ;;;
 ;;; Copyright (c) 2012, Gayane Kazhoyan <[email protected]>
 ;;;                     Amar Fayaz <[email protected]>
@@ -402,37 +403,36 @@
      ?costmap))
   ;;
   ;;;;;;;;;;;;;;; spatial relation ON for environment objects ;;;;;;;;;;;;;;;;;;;;;;
-  ;; (<- (costmap:desig-costmap ?designator ?costmap)
-  ;;   (desig:desig-prop ?designator (:on ?object))
-  ;;   (not (desig:desig-prop ?designator (:attachment ?_)))
-  ;;   (not (desig:desig-prop ?designator (:attachments ?_)))
-  ;;   (spec:property ?object (:urdf-name ?urdf-name))
-  ;;   (spec:property ?object (:part-of ?environment-name))
-  ;;   (btr:bullet-world ?world)
-  ;;   (btr:%object ?world ?environment-name ?environment-object)
-  ;;   (lisp-fun get-link-rigid-body ?environment-object ?urdf-name ?environment-link)
-  ;;   (lisp-pred identity ?environment-link)
-  ;;   (costmap:costmap ?costmap)
-  ;;   ;; costmap
-  ;;   (costmap:costmap-add-function
-  ;;    on-bounding-box
-  ;;    (make-object-bounding-box-costmap-generator ?environment-link)
-  ;;    ?costmap)
-  ;;   ;; height generator
-  ;;   (once (or (and (desig:desig-prop ?designator (:for ?for-object))
-  ;;                  (object-designator-from-name-or-type ?for-object ?for-object-name)
-  ;;                  (btr:%object ?world ?for-object-name ?for-object-instance)
-  ;;                  (costmap:costmap-add-height-generator
-  ;;                   (make-object-on/in-object-bb-height-generator
-  ;;                    ?environment-link ?for-object-instance :on)
-  ;;                   ?costmap))
-  ;;             (costmap:costmap-add-cached-height-generator
-  ;;              (make-object-bounding-box-height-generator ?environment-link :on)
-  ;;              ?costmap)))
-  ;;   ;; orientation generator
-  ;;   (once (or (desig:desig-prop ?designator (:orientation ?orientation-type))
-  ;;             (equal ?orientation-type :random)))
-  ;;   (generate-orientations ?orientation-type ?environment-link nil ?costmap))
+  (<- (costmap:desig-costmap ?designator ?costmap)
+    (desig:desig-prop ?designator (:on ?object))
+    (btr-belief:object-designator-name ?object ?object-instance-name)
+    (spec:property ?object (:urdf-name ?urdf-name))
+    (spec:property ?object (:part-of ?environment-name))
+    (btr:bullet-world ?world)
+    (btr:%object ?world ?environment-name ?environment-object)
+    (lisp-fun get-link-rigid-body ?environment-object ?urdf-name ?environment-link)
+    (lisp-pred identity ?environment-link)
+    (costmap:costmap ?costmap)
+    ;; costmap
+    (costmap:costmap-add-function
+     on-bounding-box
+     (make-object-bounding-box-costmap-generator ?environment-link)
+     ?costmap)
+    ;; height generator
+    (once (or (and (desig:desig-prop ?designator (:for ?for-object))
+                   (object-designator-from-name-or-type ?for-object ?for-object-name)
+                   (btr:%object ?world ?for-object-name ?for-object-instance)
+                   (costmap:costmap-add-height-generator
+                    (make-object-on/in-object-bb-height-generator
+                     ?environment-link ?for-object-instance :on)
+                    ?costmap))
+              (costmap:costmap-add-cached-height-generator
+               (make-object-bounding-box-height-generator ?environment-link :on)
+               ?costmap)))
+    ;; orientation generator
+    (once (or (desig:desig-prop ?designator (:orientation ?orientation-type))
+              (equal ?orientation-type :random)))
+    (generate-orientations ?orientation-type ?environment-link nil ?costmap))
 
 
   ;;;;;;;;;;;;;;; spatial relation ABOVE for environment objects ;;;;;;;;;;;;
@@ -668,3 +668,4 @@
               ?costmap))
         (true))))
 
+

cram_3d_world/cram_bullet_reasoning/src/items.lisp

@@ -51,6 +51,11 @@
     (:bread "package://cram_bullet_reasoning/resource/bread.stl" nil)
     (:bowl-compound "package://cram_bullet_reasoning/resource/bowl_compound.dae" nil)
     (:bowl-non-compound "package://cram_bullet_reasoning/resource/bowl_non_compound.stl" nil)
+    (:ikea-bowl "package://cram_bullet_reasoning/resource/ikea_bowl.stl" nil)
+    (:ikea-bowl-ww "package://cram_bullet_reasoning/resource/ikea_bowl_ww.stl" nil)
+    (:popcorn-pot "package://cram_bullet_reasoning/resource/popcorn-pot.stl" nil)
+    (:popcorn-pot-lid "package://cram_bullet_reasoning/resource/popcorn-pot-lid.stl" nil)
+    (:salt "package://cram_bullet_reasoning/resource/salt.stl" nil)
     (:fork "package://cram_bullet_reasoning/resource/fork.stl" nil)
     (:knife "package://cram_bullet_reasoning/resource/knife.stl" nil)
     (:big-knife "package://cram_bullet_reasoning/resource/big-knife.stl" nil)
@@ -129,7 +134,8 @@ The name in the list is a keyword that is created by lispifying the filename."
              (physics-utils:calculate-aabb
               (physics-utils:3d-model-vertices model)))))))))
 
-;;;;;;;;;;;;;;;;;;;;;;;;;; ATTACHMENTS ;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ ;;;;;;;;;;;;;;;;;;;;;;;;;; ATTACHMENTS ;;;;;;;;;;;;;;;;;;;;;;;;;;
 
 (defmethod attach-object ((other-object item) (object item)
                           &key attachment-type loose
@@ -267,7 +273,8 @@ it is possible to change the pose of its attachments when its pose changes."
             ;; If all attachments from root head passed, remove all.
             (if (equal (name object) (car (last already-moved)))
                 (setf already-moved '()))))
-        (call-next-method))))
+        (call-next-method))))       
+
 
 ;;;;;;;;;;;;;;;;;;;;; SPAWNING MESH AND PRIMITIVE-SHAPED ITEMS ;;;;;;;;;;;;
 
@@ -545,3 +552,121 @@ The length, width and height have to be given for the function to work."
                   :collision-shape (make-instance 'bt-vis:colored-box-shape
                                      :half-extents (ensure-vector size)
                                      :color color))))))
+
+;;;;;;;;;;;;;;;;;;;;;;;;;; ATTACHMENTS ;;;;;;;;;;;;;;;;;;;;;;;;;;
+(defun object-attached-to-robot-p (object)
+  (when object
+    (assoc (name object) (btr:attached-objects (get-robot-object)))))
+
+(defun get-collision-information-from-robot (object)
+  (declare (type item object))
+  (list (caddr (assoc (name object) (attached-objects (get-robot-object))))))
+
+(defmethod attach-object ((other-object item) (object item)
+                          &key attachment-type loose
+                          skip-removing-loose link grasp)
+  "Attaches `object' to `other-object': adds an attachment to the
+attached-objects lists of each other. `attachment-type' is a keyword
+that specifies the type of attachment. `loose' specifies if the attachment
+is bidirectional (nil) or unidirectional (t). `skip-removing-loose' is for
+attaching more objects unidirectional and should be for this T. See
+`attach-object' above."
+  (declare (ignore link grasp)) ;; used in robot-model.lisp
+  (when (equal (name object) (name other-object))
+    (warn "Cannot attach an object to itself: ~a" (name object))
+    (return-from attach-object))
+  (when (member (name object) (attached-objects other-object))
+    (warn "Item ~a already attached to ~a. Ignoring new attachment."
+          (name object) (name other-object))
+    (return-from attach-object))
+  (unless skip-removing-loose
+    (remove-loose-attachment-for object))
+  (push (cons (name object)
+              (cons
+               (list (make-attachment :object (name object)
+                                      :attachment attachment-type))
+               ;; Since robot objects are not in the attached-objects
+               ;; list of items, this has to be copied manuelly:
+               (if (object-attached-to-robot-p object)
+                   (get-collision-information-from-robot object)
+                   (create-static-collision-information object))))
+        (slot-value other-object 'attached-objects))
+  (push (cons (name other-object)
+              (cons
+               (list (make-attachment :object (name other-object)
+                                      :loose loose :attachment attachment-type))
+               (create-static-collision-information other-object)))
+        (slot-value object 'attached-objects)))
+
+(defmethod attach-object ((other-objects list) (object item)
+                          &key attachment-type loose)
+  "Will be used if an attachment should be made from one item to more
+than one item. If `loose' T the other attachments have to be made with
+`skip-removing-loose' as T to prevent removing loose attachments between
+the element before in `other-objects' and `object'."
+  (attach-object (first other-objects) object :attachment-type attachment-type :loose loose)
+  (mapcar (lambda (obj)
+            (attach-object obj object
+                           :attachment-type attachment-type :loose loose
+                           :skip-removing-loose T))
+          (cdr other-objects)))
+
+(defmethod detach-object ((other-object item) (object item) &key)
+  "Removes item names from the given arguments in the corresponding `attached-objects' lists
+   of the given items."
+  (when (equal (name object) (name other-object))
+    (warn "Cannot attach an object to itself: ~a" (name object))
+    (return-from detach-object))
+  (flet ((get-attachment-object (elem)
+           (attachment-object (car (second elem))))
+         (get-collision-info (attached obj)
+           (cdr (cdr (assoc (name attached) (attached-objects obj))))))
+    (let ((object-collision-info (get-collision-info object other-object))
+          (other-object-collision-info (get-collision-info other-object object)))
+      (setf (slot-value other-object 'attached-objects)
+            (remove (name object) (attached-objects other-object)
+                    :key #'get-attachment-object :test #'equal))
+      (setf (slot-value object 'attached-objects)
+            (remove (name other-object) (attached-objects object)
+                    :key #'get-attachment-object :test #'equal))
+      (reset-collision-information object object-collision-info)
+      (reset-collision-information other-object other-object-collision-info))))
+
+(defmethod detach-all-objects ((object item))
+  (with-slots (attached-objects) object
+    (dolist (attached-object attached-objects)
+      (let ((object-name (car attached-object)))
+        (if (object *current-bullet-world* object-name)
+            (detach-object (name object) object-name))))))
+
+(let ((already-moved '()))
+  (defmethod (setf pose) :around (new-value (object item))
+    "Since we save the original pose of the object at the time of attaching,
+it is possible to change the pose of its attachments when its pose changes."
+    (if (and (slot-boundp object 'attached-objects)
+             (> (length (attached-objects object)) 0))
+        (let ((carrier-transform
+                (cl-transforms:transform-diff
+                 (cl-transforms:pose->transform new-value)
+                 (cl-transforms:pose->transform (pose object)))))
+          ;; If no attached item already moved or wasn't already moved
+          (unless (and already-moved
+                       (member (name object) already-moved :test #'equal))
+            (push (name object) already-moved)
+            (call-next-method)
+            (dolist (attachment (remove-if #'attachment-loose
+                                           (mapcar #'car
+                                                   (mapcar #'second
+                                                           (attached-objects object)))))
+              (let ((current-attachment-pose
+                      (pose (object *current-bullet-world* (attachment-object attachment)))))
+                (when (and carrier-transform current-attachment-pose)
+                  (setf (pose (btr:object btr:*current-bullet-world*
+                                          (attachment-object attachment)))
+                        (cl-transforms:transform-pose
+                         carrier-transform
+                         current-attachment-pose)))))
+            ;; If all attachments from root head passed, remove all.
+            (if (equal (name object) (car (last already-moved)))
+                (setf already-moved '()))))
+        (call-next-method))))

cram_3d_world/cram_bullet_reasoning/src/objects.lisp

@@ -549,3 +549,11 @@ recursive function."
                  (remove-loose-attachment-for attached-object)))
       (if (equal (car (last already-visited)) (name object))
           (setf already-visited '())))))
+
+
+(defun get-objects-for-type (object-type)
+  (remove-if-not #'identity
+                 (mapcar (lambda (obj) (when (typep obj 'btr:item)
+                                         (when (eql (first (btr:item-types obj)) object-type)
+                                           obj)))
+                         (btr:objects btr:*current-bullet-world*))))

cram_3d_world/cram_bullet_reasoning/src/package.lisp

@@ -115,4 +115,7 @@
            timeline-current-world-state timeline-lookup
            holds-in-world with-timeline
            ;; simple-sem-map
-           #:*mesh-path-whitelist*))
+           #:*mesh-path-whitelist*
+           ;; Query the bullet world
+           get-objects-for-type
+           ))

cram_3d_world/cram_bullet_reasoning/src/robot-model-utils.lisp

@@ -226,7 +226,6 @@ Should it be taken out and made PR2-specific?"
      delta)))
 
 
-
 (defun get-robot-object ()
   (object *current-bullet-world* (rob-int:get-robot-name)))
 

cram_3d_world/cram_bullet_reasoning_belief_state/src/belief-state.lisp

@@ -36,6 +36,7 @@
   ;; make a clean world instance
   (setf btr:*current-bullet-world* (make-instance 'btr:bt-reasoning-world))
 
+
   ;; get the environment URDF from the ROS parameter server
   (let ((kitchen-urdf-string
           (roslisp:get-param rob-int:*environment-description-parameter* nil)))