diff --git a/cram-20.04-http.rosinstall b/cram-20.04-http.rosinstall new file mode 100644 index 0000000000..4b0c65aeb8 --- /dev/null +++ b/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 + + + diff --git a/cram_3d_world/cram_btr_spatial_relations_costmap/src/cost-functions.lisp b/cram_3d_world/cram_btr_spatial_relations_costmap/src/cost-functions.lisp index 905e21a6e2..d4a412a009 100644 --- a/cram_3d_world/cram_btr_spatial_relations_costmap/src/cost-functions.lisp +++ b/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)))) diff --git a/cram_3d_world/cram_bullet_reasoning/src/robot-model.lisp b/cram_3d_world/cram_bullet_reasoning/src/robot-model.lisp index 4009b4e608..326990413d 100644 --- a/cram_3d_world/cram_bullet_reasoning/src/robot-model.lisp +++ b/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)) diff --git a/cram_3d_world/cram_bullet_reasoning_belief_state/src/belief-state.lisp b/cram_3d_world/cram_bullet_reasoning_belief_state/src/belief-state.lisp index 9be00a8c57..ee8f9bdb1f 100644 --- a/cram_3d_world/cram_bullet_reasoning_belief_state/src/belief-state.lisp +++ b/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 diff --git a/cram_3d_world/cram_bullet_reasoning_belief_state/src/consistency-checking.lisp b/cram_3d_world/cram_bullet_reasoning_belief_state/src/consistency-checking.lisp index 5653cb760f..31918f7792 100644 --- a/cram_3d_world/cram_bullet_reasoning_belief_state/src/consistency-checking.lisp +++ b/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)...)." - diff --git a/cram_3d_world/cram_bullet_reasoning_belief_state/src/event-handlers.lisp b/cram_3d_world/cram_bullet_reasoning_belief_state/src/event-handlers.lisp index 3359d25516..3fb530c4f6 100644 --- a/cram_3d_world/cram_bullet_reasoning_belief_state/src/event-handlers.lisp +++ b/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) diff --git a/cram_3d_world/cram_bullet_reasoning_belief_state/src/occasions.lisp b/cram_3d_world/cram_bullet_reasoning_belief_state/src/occasions.lisp index 5bdbe72a14..8fb42078e0 100644 --- a/cram_3d_world/cram_bullet_reasoning_belief_state/src/occasions.lisp +++ b/cram_3d_world/cram_bullet_reasoning_belief_state/src/occasions.lisp @@ -1,6 +1,7 @@ ;;; ;;; Copyright (c) 2012, Lorenz Moesenlechner ;;; 2020, Christopher Pollok +;;; 2022, Gayane Kazhoyan ;;; 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) diff --git a/cram_3d_world/cram_bullet_reasoning_utilities/src/package.lisp b/cram_3d_world/cram_bullet_reasoning_utilities/src/package.lisp index 84d69ba984..d68f440e6c 100644 --- a/cram_3d_world/cram_bullet_reasoning_utilities/src/package.lisp +++ b/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 diff --git a/cram_3d_world/cram_bullet_reasoning_utilities/src/vis-tools.lisp b/cram_3d_world/cram_bullet_reasoning_utilities/src/vis-tools.lisp index b05ed84862..17f27db6e3 100644 --- a/cram_3d_world/cram_bullet_reasoning_utilities/src/vis-tools.lisp +++ b/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*)) diff --git a/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-designators.lisp b/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-designators.lisp index 805c4b17f6..34b87e7394 100644 --- a/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-designators.lisp +++ b/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-designators.lisp @@ -106,31 +106,57 @@ the `look-pose-stamped'." (and (spec:property ?location-designator (:on ?some-object-designator)) (equal ?object-accessible t)))) (desig:current-designator ?some-object-designator ?object-designator) + ;; location of the object that we are trying to access (once (or (spec:property ?object-designator (:location ?object-location-desig)) (equal ?object-location-desig nil))) - ;; arms - (-> (spec:property ?action-designator (:arms ?arms)) + ;; outer robot-location + (-> (desig:desig-prop ?action-designator (:outer-robot-location + ?some-outer-robot-location)) + (desig:current-designator ?some-outer-robot-location ?outer-robot-location) + (equal ?outer-robot-location NIL)) + ;; outer accessing / sealing arms + (-> (desig:desig-prop ?action-designator (:outer-arms ?outer-arms)) (true) - (setof ?arm (man-int:robot-free-hand ?robot ?arm) ?arms)) - ;; distance - (once (or (spec:property ?action-designator (:distance ?distance)) - (equal ?distance NIL))) + (equal ?outer-arms NIL)) + ;; outer accessing / sealing grasps + (-> (desig:desig-prop ?action-designator (:outer-grasps ?outer-grasps)) + (true) + (equal ?outer-grasps NIL)) + ;; robot-location (once (or (and (spec:property ?action-designator (:robot-location ?some-robot-location)) - (desig:current-designator ?robot-location ?robot-location)) + (desig:current-designator ?some-robot-location ?robot-location)) (desig:designator :location ((:reachable-for ?robot) ;; (:arm ?arm) (:object ?object-designator)) ?robot-location))) + ;; arms + (-> (spec:property ?action-designator (:arms ?arms)) + (true) + (setof ?arm (man-int:robot-free-hand ?robot ?arm) ?arms)) + ;; grasps + (-> (spec:property ?action-designator (:grasps ?grasps)) + (true) + (equal ?grasps NIL)) + ;; distance + (once (or (spec:property ?action-designator (:distance ?distance)) + (equal ?distance NIL))) + (desig:designator :action ((:type ?action-type) (:object ?object-designator) + (:object-accessible ?object-accessible) (:object-location ?object-location-desig) + (:outer-robot-location ?outer-robot-location) + (:outer-arms ?outer-arms) + (:outer-grasps ?outer-grasps) + + (:robot-location ?robot-location) (:arms ?arms) - (:distance ?distance) - (:robot-location ?robot-location)) + (:grasps ?grasps) + (:distance ?distance)) ?resolved-action-designator)) @@ -330,12 +356,112 @@ the `look-pose-stamped'." ?object-designator-with-location) (lisp-pred desig:equate ?object-designator ?object-designator-with-location))) + + ;; access deliver location robot base + (-> (desig:desig-prop ?action-designator + (:access-deliver-robot-location ?some-ad-robot-loc-desig)) + (desig:current-designator ?some-ad-robot-loc-desig + ?access-deliver-robot-location-designator) + (equal ?access-deliver-robot-location-designator NIL)) + ;; seal deliver location robot base + (-> (desig:desig-prop ?action-designator + (:seal-deliver-robot-location ?some-sd-robot-loc-desig)) + (desig:current-designator ?some-sd-robot-loc-desig + ?seal-deliver-robot-location-designator) + (equal ?seal-deliver-robot-location-designator NIL)) + ;; access / seal deliver arms + (-> (desig:desig-prop ?action-designator + (:access-seal-deliver-arms ?access-seal-deliver-arms)) + (true) + (equal ?access-seal-deliver-arms NIL)) + ;; access / seal deliver grasps + (-> (desig:desig-prop ?action-designator + (:access-seal-deliver-grasps ?access-seal-deliver-grasps)) + (true) + (equal ?access-seal-deliver-grasps NIL)) + + ;; access deliver outer location robot base + (-> (desig:desig-prop ?action-designator + (:access-deliver-outer-robot-location ?some-ado-robot-loc-desig)) + (desig:current-designator ?some-ado-robot-loc-desig + ?access-deliver-outer-robot-location-designator) + (equal ?access-deliver-outer-robot-location-designator NIL)) + ;; seal deliver outer location robot base + (-> (desig:desig-prop ?action-designator + (:seal-deliver-outer-robot-location ?some-sdo-robot-loc-desig)) + (desig:current-designator ?some-sdo-robot-loc-desig + ?seal-deliver-outer-robot-location-designator) + (equal ?seal-deliver-outer-robot-location-designator NIL)) + ;; access / seal deliver outer arms + (-> (desig:desig-prop ?action-designator + (:access-seal-deliver-outer-arms ?access-seal-deliver-outer-arms)) + (true) + (equal ?access-seal-deliver-outer-arms NIL)) + ;; access / seal deliver outer grasps + (-> (desig:desig-prop ?action-designator + (:access-seal-deliver-outer-grasps ?access-seal-deliver-outer-grasps)) + (true) + (equal ?access-seal-deliver-outer-grasps NIL)) + + ;; access search location robot base + (-> (desig:desig-prop ?action-designator + (:access-search-robot-location ?some-as-robot-loc-desig)) + (desig:current-designator ?some-as-robot-loc-desig + ?access-search-robot-location-designator) + (equal ?access-search-robot-location-designator NIL)) + ;; seal search location robot base + (-> (desig:desig-prop ?action-designator + (:seal-search-robot-location ?some-ss-robot-loc-desig)) + (desig:current-designator ?some-ss-robot-loc-desig + ?seal-search-robot-location-designator) + (equal ?seal-search-robot-location-designator NIL)) + ;; access / seal search arms + (-> (desig:desig-prop ?action-designator + (:access-seal-search-arms ?access-seal-search-arms)) + (true) + (equal ?access-seal-search-arms NIL)) + ;; access / seal search grasps + (-> (desig:desig-prop ?action-designator + (:access-seal-search-grasps ?access-seal-search-grasps)) + (true) + (equal ?access-seal-search-grasps NIL)) + + ;; access search outer location robot base + (-> (desig:desig-prop ?action-designator + (:access-search-outer-robot-location ?some-aso-robot-loc-desig)) + (desig:current-designator ?some-aso-robot-loc-desig + ?access-search-outer-robot-location-designator) + (equal ?access-search-outer-robot-location-designator NIL)) + ;; seal search outer location robot base + (-> (desig:desig-prop ?action-designator + (:seal-search-outer-robot-location ?some-sso-robot-loc-desig)) + (desig:current-designator ?some-sso-robot-loc-desig + ?seal-search-outer-robot-location-designator) + (equal ?seal-search-outer-robot-location-designator NIL)) + ;; access / seal search outer arms + (-> (desig:desig-prop ?action-designator + (:access-seal-search-outer-arms ?access-seal-search-outer-arms)) + (true) + (equal ?access-seal-search-outer-arms NIL)) + ;; access search outer grasps + (-> (desig:desig-prop ?action-designator + (:access-search-outer-grasps ?access-search-outer-grasps)) + (true) + (equal ?access-search-outer-grasps NIL)) + ;; seal search outer grasps + (-> (desig:desig-prop ?action-designator + (:seal-search-outer-grasps ?seal-search-outer-grasps)) + (true) + (equal ?seal-search-outer-grasps NIL)) + + ;; search location robot base (-> (desig:desig-prop ?action-designator (:search-robot-location ?some-s-robot-loc-desig)) (desig:current-designator ?some-s-robot-loc-desig ?search-robot-location-designator) (equal ?search-robot-location-designator NIL)) + ;; fetch location robot base (-> (desig:desig-prop ?action-designator (:fetch-robot-location ?some-f-robot-loc-desig)) @@ -350,6 +476,7 @@ the `look-pose-stamped'." (-> (spec:property ?action-designator (:grasps ?grasps)) (true) (equal ?grasps NIL)) + ;; deliver location (-> (and (spec:property ?action-designator (:target ?some-delivering-location-designator)) @@ -367,17 +494,40 @@ the `look-pose-stamped'." (desig:current-designator ?some-d-robot-loc-desig ?deliver-robot-location-designator) (equal ?deliver-robot-location-designator NIL)) + ;; resulting action desig (desig:designator :action ((:type :transporting) (:object ?object-designator-with-location) (:context ?context) + + (:access-search-robot-location ?access-search-robot-location-designator) + (:seal-search-robot-location ?seal-search-robot-location-designator) + (:access-seal-search-arms ?access-seal-search-arms) + (:access-seal-search-grasps ?access-seal-search-grasps) + (:access-search-outer-robot-location ?access-search-outer-robot-location-designator) + (:seal-search-outer-robot-location ?seal-search-outer-robot-location-designator) + (:access-seal-search-outer-arms ?access-seal-search-outer-arms) + (:access-search-outer-grasps ?access-search-outer-grasps) + (:seal-search-outer-grasps ?seal-search-outer-grasps) + + (:access-deliver-robot-location ?access-deliver-robot-location-designator) + (:seal-deliver-robot-location ?seal-deliver-robot-location-designator) + (:access-seal-deliver-arms ?access-seal-deliver-arms) + (:access-seal-deliver-grasps ?access-seal-deliver-grasps) + (:access-deliver-outer-robot-location ?access-deliver-outer-robot-location-designator) + (:seal-deliver-outer-robot-location ?seal-deliver-outer-robot-location-designator) + (:access-seal-deliver-outer-arms ?access-seal-deliver-outer-arms) + (:access-seal-deliver-outer-grasps ?access-seal-deliver-outer-grasps) + (:search-location ?search-location-designator) (:search-robot-location ?search-robot-location-designator) + (:fetch-robot-location ?fetch-robot-location-designator) (:arms ?arms) (:grasps ?grasps) + (:deliver-location ?delivering-location-designator) (:deliver-robot-location ?deliver-robot-location-designator)) ?resolved-action-designator))) diff --git a/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-plans.lisp b/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-plans.lisp index 9e65606a88..5402bf8618 100644 --- a/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-plans.lisp +++ b/cram_3d_world/cram_fetch_deliver_plans/src/fetch-and-deliver-plans.lisp @@ -108,21 +108,27 @@ turn the robot base such that it looks in the direction of target and look again (defun manipulate-environment (&key ((:type action-type)) ((:object ?object-to-manipulate)) + object-accessible ((:object-location ?object-location)) - ((:arms all-arms)) + ((:outer-robot-location ?outer-robot-location)) + ((:outer-arms ?outer-arms)) + ((:outer-grasps ?outer-grasps)) + + ((:robot-location ?robot-location)) + ((:arms ?all-arms)) + ((:grasps ?grasps)) ((:distance ?distance)) - ((:robot-location ?manipulate-robot-location)) &allow-other-keys) (declare (type keyword action-type) - (type list all-arms) + (type list ?all-arms ?grasps ?outer-arms ?outer-grasps) (type desig:object-designator ?object-to-manipulate) (type (or desig:location-designator null) ?object-location) (type (or number null) ?distance) - ;; here, ?manipulate-robot-location can only be null within the function + ;; here, ?robot-location can only be null within the function ;; but one should not pass a NULL location as argument, ;; otherwise it will just cpl:fail straight away. - (type (or null desig:location-designator) ?manipulate-robot-location)) + (type (or null desig:location-designator) ?robot-location ?outer-robot-location)) "Navigate to reachable location, check if opening/closing trajectory causes collisions, if yes, relocate and retry, if no collisions, open or close container." @@ -133,6 +139,12 @@ if yes, relocate and retry, if no collisions, open or close container." (exe:perform (desig:an action (type accessing) (location ?object-location) + (desig:when ?outer-arms + (arms ?outer-arms)) + (desig:when ?outer-grasps + (grasps ?outer-grasps)) + (desig:when ?outer-robot-location + (robot-location ?outer-robot-location)) (goal ?goal))))) (unless object-accessible @@ -142,11 +154,11 @@ if yes, relocate and retry, if no collisions, open or close container." (cpl:fail 'common-fail:environment-manipulation-impossible :description "Some designator could not be resolved."))) - (let* ((?arms all-arms) + (let* ((?arms ?all-arms) (?arm (cut:lazy-car ?arms)) ;; TODO: THIS LET IS A HACK because I'm lazy to make a subaction for accessing - (?manipulate-robot-location-with-arm - (desig:copy-designator ?manipulate-robot-location + (?robot-location-with-arm + (desig:copy-designator ?robot-location :new-description `((:arm ,?arm))))) ;; if opening- / closing-container fails, relocate @@ -154,20 +166,20 @@ if yes, relocate and retry, if no collisions, open or close container." (cpl:with-failure-handling (((or common-fail:environment-unreachable) (e) (common-fail:retry-with-loc-designator-solutions - ?manipulate-robot-location-with-arm + ?robot-location-with-arm relocation-retries (:error-object-or-string e - :warning-namespace (fd-plans environment) - :reset-designators (list ?manipulate-robot-location-with-arm) + :warning-namespace (fd-plans environment outer) + :reset-designators (list ?robot-location-with-arm) :rethrow-failure 'common-fail:environment-manipulation-impossible) ;; TODO: what if the another arm is holding an object! (exe:perform (desig:an action (type releasing))) (setf ?arms - all-arms) + ?all-arms) (setf ?arm (cut:lazy-car ?arms)) - (setf (cadr (find :arm (desig:description ?manipulate-robot-location-with-arm) + (setf (cadr (find :arm (desig:description ?robot-location-with-arm) :key #'car)) ?arm) (roslisp:ros-info (fd-plans environment) "Relocating...")))) @@ -192,15 +204,15 @@ if yes, relocate and retry, if no collisions, open or close container." (type releasing))) (setf ?arm (cut:lazy-car ?arms)) - (setf (cadr (find :arm (desig:description ?manipulate-robot-location-with-arm) + (setf (cadr (find :arm (desig:description ?robot-location-with-arm) :key #'car)) ?arm) - (desig:reset ?manipulate-robot-location-with-arm)))) + (desig:reset ?robot-location-with-arm)))) ;; navigate, open / close (exe:perform (desig:an action (type navigating) - (location ?manipulate-robot-location-with-arm))) + (location ?robot-location-with-arm))) (let ((manipulation-action (ecase action-type @@ -212,6 +224,8 @@ if yes, relocate and retry, if no collisions, open or close container." (desig:an action (type opening) (arm ?arm) + (desig:when ?grasps + (grasps ?grasps)) (object ?object-to-manipulate) (desig:when ?distance (distance ?distance)) @@ -224,6 +238,8 @@ if yes, relocate and retry, if no collisions, open or close container." (desig:an action (type closing) (arm ?arm) + (desig:when ?grasps + (grasps ?grasps)) (object ?object-to-manipulate) (desig:when ?distance (distance ?distance)) @@ -241,6 +257,12 @@ if yes, relocate and retry, if no collisions, open or close container." (exe:perform (desig:an action (type sealing) (location ?object-location) + (desig:when ?outer-arms + (arms ?outer-arms)) + (desig:when ?outer-grasps + (grasps ?outer-grasps)) + (desig:when ?outer-robot-location + (robot-location ?outer-robot-location)) (goal ?goal)))))) @@ -742,11 +764,33 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (defun transport (&key ((:object ?object-designator)) ((:context ?context)) + + ((:access-search-robot-location ?access-search-robot-location)) + ((:seal-search-robot-location ?seal-search-robot-location)) + ((:access-seal-search-arms ?access-seal-search-arms)) + ((:access-seal-search-grasps ?access-seal-search-grasps)) + ((:access-search-outer-robot-location ?access-search-outer-robot-location)) + ((:seal-search-outer-robot-location ?seal-search-outer-robot-location)) + ((:access-seal-search-outer-arms ?access-seal-search-outer-arms)) + ((:access-search-outer-grasps ?access-search-outer-grasps)) + ((:seal-search-outer-grasps ?seal-search-outer-grasps)) + + ((:access-deliver-robot-location ?access-deliver-robot-location)) + ((:seal-deliver-robot-location ?seal-deliver-robot-location)) + ((:access-seal-deliver-arms ?access-seal-deliver-arms)) + ((:access-seal-deliver-grasps ?access-seal-deliver-grasps)) + ((:access-deliver-outer-robot-location ?access-deliver-outer-robot-location)) + ((:seal-deliver-outer-robot-location ?seal-deliver-outer-robot-location)) + ((:access-seal-deliver-outer-arms ?access-seal-deliver-outer-arms)) + ((:access-seal-deliver-outer-grasps ?access-seal-deliver-outer-grasps)) + ((:search-location ?search-location)) - ((:search-robot-location ?search-base-location)) + ((:search-robot-location ?search-robot-location)) + ((:fetch-robot-location ?fetch-robot-location)) ((:arms ?arms)) ((:grasps ?grasps)) + ((:deliver-location ?delivering-location)) ((:deliver-robot-location ?deliver-robot-location)) &allow-other-keys) @@ -761,6 +805,18 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (exe:perform (desig:an action (type accessing) (location ?delivering-location) + (desig:when ?access-deliver-robot-location + (robot-location ?access-deliver-robot-location)) + (desig:when ?access-seal-deliver-arms + (arms ?access-seal-deliver-arms)) + (desig:when ?access-seal-deliver-grasps + (grasps ?access-seal-deliver-grasps)) + (desig:when ?access-deliver-outer-robot-location + (outer-robot-location ?access-deliver-outer-robot-location)) + (desig:when ?access-seal-deliver-outer-arms + (outer-arms ?access-seal-deliver-outer-arms)) + (desig:when ?access-seal-deliver-outer-grasps + (outer-grasps ?access-seal-deliver-outer-grasps)) (goal ?goal)))) ;; if we are not sure about the exact location of search-location, find it @@ -775,17 +831,30 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (exe:perform (desig:an action (type accessing) (location ?search-location) + (desig:when ?access-search-robot-location + (robot-location ?access-search-robot-location)) + (desig:when ?access-seal-search-arms + (arms ?access-seal-search-arms)) + (desig:when ?access-seal-search-grasps + (grasps ?access-seal-search-grasps)) + (desig:when ?access-search-outer-robot-location + (outer-robot-location ?access-search-outer-robot-location)) + (desig:when ?access-seal-search-outer-arms + (outer-arms ?access-seal-search-outer-arms)) + (desig:when ?access-search-outer-grasps + (outer-grasps ?access-search-outer-grasps)) (goal ?goal)))) ;; search for the object to find it's exact pose - (let ((?goal `(cpoe:object-in-hand ,?object-designator :left-or-right))) + (let ((?goal `(or (cpoe:object-in-hand ,?object-designator :left-or-right) + (cpoe:object-at-location ,?object-designator ,?delivering-location)))) (exe:perform (desig:an action (type searching) (object ?object-designator) (desig:when ?context (context ?context)) - (desig:when ?search-base-location - (robot-location ?search-base-location)) + (desig:when ?search-robot-location + (robot-location ?search-robot-location)) (goal ?goal)))) (setf ?object-designator (desig:current-desig ?object-designator)) (roslisp:ros-info (pp-plans transport) @@ -807,7 +876,8 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat #'proj-reasoning:pick-best-parameters-by-distance ;; fetch the object - (let ((?fetch-goal `(cpoe:object-in-hand ,?object-designator :left-or-right))) + (let ((?fetch-goal `(or (cpoe:object-in-hand ,?object-designator :left-or-right) + (cpoe:object-at-location ,?object-designator ,?delivering-location)))) (exe:perform (desig:an action (type fetching) (desig:when ?arms @@ -849,6 +919,18 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (exe:perform (desig:an action (type sealing) (location ?search-location) + (desig:when ?seal-search-robot-location + (robot-location ?seal-search-robot-location)) + (desig:when ?access-seal-search-arms + (arms ?access-seal-search-arms)) + (desig:when ?access-seal-search-grasps + (grasps ?access-seal-search-grasps)) + (desig:when ?seal-search-outer-robot-location + (outer-robot-location ?seal-search-outer-robot-location)) + (desig:when ?access-seal-search-outer-arms + (outer-arms ?access-seal-search-outer-arms)) + (desig:when ?seal-search-outer-grasps + (outer-grasps ?seal-search-outer-grasps)) (goal ?goal)))) ;; reset the target location @@ -856,6 +938,18 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (exe:perform (desig:an action (type sealing) (location ?delivering-location) + (desig:when ?seal-deliver-robot-location + (robot-location ?seal-deliver-robot-location)) + (desig:when ?access-seal-deliver-arms + (arms ?access-seal-deliver-arms)) + (desig:when ?access-seal-deliver-grasps + (grasps ?access-seal-deliver-grasps)) + (desig:when ?seal-deliver-outer-robot-location + (outer-robot-location ?seal-deliver-outer-robot-location)) + (desig:when ?access-seal-deliver-outer-arms + (outer-arms ?access-seal-deliver-outer-arms)) + (desig:when ?access-seal-deliver-outer-grasps + (outer-grasps ?access-seal-deliver-outer-grasps)) (goal ?goal)))) (desig:current-desig ?object-designator)) diff --git a/cram_3d_world/cram_urdf_environment_manipulation/src/action-designators.lisp b/cram_3d_world/cram_urdf_environment_manipulation/src/action-designators.lisp index 95f41a402a..e8b5e6cccf 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/action-designators.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/action-designators.lisp @@ -55,8 +55,10 @@ ;; find the grasp type (-> (spec:property ?action-designator (:grasp ?grasp)) (true) - (and (lisp-fun man-int:get-action-grasps :handle ?arm nil ?grasps) - (member ?grasp ?grasps))) + (-> (spec:property ?action-designator (:grasps ?grasps)) + (member ?grasp ?grasps) + (and (lisp-fun man-int:get-action-grasps :handle ?arm nil ?grasps) + (member ?grasp ?grasps)))) ;; find the joint (lisp-fun get-environment-link ?container-name ?btr-environment ?container-link) (lisp-fun get-connecting-joint ?container-link ?connecting-joint) @@ -96,6 +98,12 @@ (lisp-fun + ?current-state ?clipped-distance ?absolute-distance) (lisp-fun - ?current-state ?clipped-distance ?absolute-distance)) + ;; pose of child of joint + (lisp-fun cl-urdf:child ?connecting-joint ?door-link) + (lisp-fun cl-urdf:name ?door-link ?door-link-name) + (lisp-fun btr:link-pose ?environment-object ?door-link-name ?door-joint-pose) + + ;; infer missing information like ?gripper-opening, opening trajectory (lisp-fun man-int:get-action-gripper-opening ?container-type ?gripper-opening) @@ -173,6 +181,7 @@ (:left-retract-poses ?left-retract-poses) (:right-retract-poses ?right-retract-poses) (:joint-name ?joint-name) + (:door-joint-pose ?door-joint-pose) (:link-name ?handle-link) (:environment-name ?environment-name) (:environment-object ?environment-object) diff --git a/cram_3d_world/cram_urdf_environment_manipulation/src/environment.lisp b/cram_3d_world/cram_urdf_environment_manipulation/src/environment.lisp index f329006e82..ddd444e14b 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/environment.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/environment.lisp @@ -165,15 +165,17 @@ the `btr-environment' (of type cl-urdf:link)." (find-handle-under-link (get-environment-link container-name btr-environment))) -(defun find-handle-under-link (link) - "Return the link object of the handle under the given `link' object." +(defun find-handle-under-link (link &optional (link-name-part "handle")) + "Return the link object of the handle under the given `link' object. +If the handle link has children, check if there is a link for the center." (declare (type (or null cl-urdf:link) link)) - (the (or null cl-urdf:link) - (if (search "handle" (cl-urdf:name link)) - link - (reduce (lambda (&optional x y) (or x y)) - (mapcar 'find-handle-under-link - (mapcar 'cl-urdf:child (cl-urdf:to-joints link))))))) + (if (search link-name-part (cl-urdf:name link)) + (if (not (cl-urdf:to-joints link)) + link + (or (find-handle-under-link link "center") link)) + (reduce (lambda (&optional x y) (or x y)) + (mapcar 'find-handle-under-link + (mapcar 'cl-urdf:child (cl-urdf:to-joints link)))))) (defun get-joint-position (joint btr-environment) "Return the value of the `joint' in the `btr-environment'." diff --git a/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp index 50adfe1b51..b59b83e356 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp @@ -47,6 +47,7 @@ ((:right-retract-poses ?right-retract-poses)) joint-name ((:link-name ?link-name)) + ((:door-joint-pose ?door-joint-pose)) ((:environment-name ?environment-name)) ((:environment-object ?environment-object)) ((:container-object ?container-designator)) @@ -76,8 +77,9 @@ (type looking) (target (desig:a location (pose ?look-pose))))))) - (cpl:par - (roslisp:ros-info (env-manip plan) "Opening gripper and reaching") + (;cpl:par + cpl:seq + (roslisp:ros-info (env-manip plan) "Opening gripper and reaching") (let ((?goal `(cpoe:gripper-joint-at ,?arm ,?gripper-opening))) (exe:perform (desig:an action @@ -85,29 +87,32 @@ (gripper ?arm) (position ?gripper-opening) (goal ?goal)))) - (cpl:with-retry-counters ((reach-retries 2)) + (cpl:with-retry-counters ((reach-retries 0)) (cpl:with-failure-handling ((common-fail:manipulation-low-level-failure (e) (roslisp:ros-warn (env-plans manipulate) - "Manipulation messed up: ~a~%Failing." + "Manipulation messed up: ~a~%Retrying once then ignoring." e) (cpl:do-retry reach-retries - (cpl:retry)))) + (cpl:retry)) + (return))) (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) (exe:perform (desig:an action (type reaching) (left-poses ?left-reach-poses) (right-poses ?right-reach-poses) + (application-context ?type) (goal ?goal))))))) - (cpl:with-retry-counters ((grasp-retries 2)) + (cpl:with-retry-counters ((grasp-retries 1)) (cpl:with-failure-handling ((common-fail:manipulation-low-level-failure (e) (roslisp:ros-warn (env-plans manipulate) "Manipulation messed up: ~a~%Failing." e) (cpl:do-retry grasp-retries - (cpl:retry)))) + (cpl:retry)) + (return))) (let ((?goal `(cpoe:tool-frames-at ,?left-grasp-poses ,?right-grasp-poses))) (exe:perform (desig:an action @@ -122,12 +127,22 @@ ;;;;;;;;;;;;;;;;;;;; GRIPPING ;;;;;;;;;;;;;;;;;;;;;;;; (roslisp:ros-info (environment-manipulation manipulate-container) "Gripping") - ;; Gripping now both for closing and opening, as grasp pose can be funny. - ;; when (eq ?type :opening) - (exe:perform - (desig:an action - (type gripping) - (gripper ?arm))) + ;; Gripping both for closing and opening, as grasp pose can be funny. + ;; But for now, when gripping fails during closing, ignore the failure. + (if (eq ?type :opening) + (exe:perform + (desig:an action + (type gripping) + (gripper ?arm))) + (cpl:with-failure-handling + ((common-fail:gripper-low-level-failure (e) + (roslisp:ros-warn (env-plans manipulate) + "Gripping didn't work: ~a.~%Ignoring..." e) + (return))) + (exe:perform + (desig:an action + (type gripping) + (gripper ?arm))))) ;;;;;;;;;;;;;;;;;;;;;; MANIPULATING ;;;;;;;;;;;;;;;;;;;;;;; (roslisp:ros-info (environment-manipulation manipulate-container) @@ -136,10 +151,9 @@ (cpl:with-failure-handling ((common-fail:manipulation-low-level-failure (e) (roslisp:ros-warn (env-plans manipulate) - "Manipulation messed up: ~a~%Failing." + "Manipulation messed up: ~a~%Ignoring." e) - ;; (return) - )) + (return))) (let ((?push-or-pull (if (eq ?type :opening) :pulling @@ -152,6 +166,8 @@ (object (desig:an object (name ?environment-name))) (container-object ?container-designator) (link ?link-name) + (desig:when ?door-joint-pose + (door-joint-pose ?door-joint-pose)) (desig:when ?absolute-distance (distance ?absolute-distance)) (desig:when (eq ?arm :left) @@ -159,16 +175,20 @@ (desig:when (eq ?arm :right) (right-poses ?right-manipulate-poses)) (goal ?goal))))) - (cpl:seq - (exe:perform - (desig:an action - (type monitoring-joint-state) - (gripper ?arm))) - ;; sleep for half a second, - ;; maybe the action is nearly finished, so there is no need to fail - (cpl:sleep 1) - (cpl:fail 'common-fail:gripper-closed-completely - :description "Handle slipped"))) + ;; (cpl:seq + ;; (exe:perform + ;; (desig:an action + ;; (type monitoring-joint-state) + ;; (gripper ?arm))) + ;; (if (eq ?type :opening) + ;; ;; sleep for half a second, + ;; ;; maybe the action is nearly finished, so there is no need to fail + ;; (cpl:sleep 1) + ;; ;; if closing, then wait until the trajectory finishes, slipping is not a problem + ;; (cpl:sleep 1000)) + ;; (cpl:fail 'common-fail:gripper-closed-completely + ;; :description "Handle slipped")) + ) (when (and joint-name) (cram-occasions-events:on-event @@ -201,6 +221,7 @@ (type retracting) (left-poses ?left-retract-poses) (right-poses ?right-retract-poses) + (application-context ?type) (goal ?goal))))) (exe:perform (desig:an action diff --git a/cram_3d_world/cram_urdf_environment_manipulation/src/trajectories.lisp b/cram_3d_world/cram_urdf_environment_manipulation/src/trajectories.lisp index a7169fd243..f7fc8ce108 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/trajectories.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/trajectories.lisp @@ -30,7 +30,7 @@ (in-package :env-man) -(defparameter *handle-retract-offset* 0.05 "in meters") +(defparameter *handle-retract-offset* 0.15 "in meters") (defmethod man-int:get-action-trajectory :heuristics 20 ((action-type (eql :opening)) arm @@ -93,6 +93,9 @@ The parameters are analog to the ones of `get-action-trajectory'." (object-name-T-tool-pregrasp-stamped (get-container-to-gripper-transform manipulated-link-name arm grasp object-environment :pregrasp)) + (object-name-T-tool-2nd-pregrasp-stamped + (get-container-to-gripper-transform + manipulated-link-name arm grasp object-environment :2nd-pregrasp)) (joint-axis (get-container-axis object-name object-environment))) @@ -106,11 +109,13 @@ The parameters are analog to the ones of `get-action-trajectory'." (make-prismatic-trajectory base-T-object-name-stamped arm action-type object-name-T-tool-grasp-stamped object-name-T-tool-pregrasp-stamped + object-name-T-tool-2nd-pregrasp-stamped opening-distance joint-axis)) (:container-revolute (make-revolute-trajectory base-T-object-name-stamped arm action-type object-name-T-tool-grasp-stamped object-name-T-tool-pregrasp-stamped + object-name-T-tool-2nd-pregrasp-stamped opening-distance joint-axis)) (T (error "Unsupported container-type: ~a." object-type))))) @@ -118,6 +123,7 @@ The parameters are analog to the ones of `get-action-trajectory'." (defun make-prismatic-trajectory (base-T-object-name-stamped arm action-type object-name-T-tool-grasp-stamped object-name-T-tool-pregrasp-stamped + object-name-T-tool-2nd-pregrasp-stamped opening-distance axis) "Return a list of `man-int::traj-segment's representing a trajectory to open a container with prismatic joints. @@ -130,7 +136,7 @@ frame of the robot's end effector as the child (eg. `cram-tf:*robot-left-tool-fr `opening-distance' is the distance the object should be manipulated in m." (declare (type cl-transforms-stamped:transform-stamped base-T-object-name-stamped object-name-T-tool-grasp-stamped - object-name-T-tool-pregrasp-stamped) + object-name-T-tool-pregrasp-stamped object-name-T-tool-2nd-pregrasp-stamped) (type keyword arm action-type) (type number opening-distance)) @@ -150,7 +156,7 @@ frame of the robot's end effector as the child (eg. `cram-tf:*robot-left-tool-fr ,action-type :retracting) (list - (list object-name-T-tool-pregrasp-stamped) + (list object-name-T-tool-pregrasp-stamped object-name-T-tool-2nd-pregrasp-stamped) (list object-name-T-tool-grasp-stamped) traj-poses (list (cram-tf:apply-transform @@ -180,6 +186,7 @@ frame of the robot's end effector as the child (eg. `cram-tf:*robot-left-tool-fr (defun make-revolute-trajectory (base-T-object-name-stamped arm action-type object-name-T-tool-grasp-stamped object-name-T-tool-pregrasp-stamped + object-name-T-tool-2nd-pregrasp-stamped opening-angle axis) "Return a list of `man-int::traj-segment' representing a trajectory to open a @@ -193,7 +200,7 @@ frame of the robot's end effector as the child (eg. `cram-tf:*robot-left-tool-fr `opening-distance' is the distance the object should be manipulated in rad." (declare (type cl-transforms-stamped:transform-stamped base-T-object-name-stamped object-name-T-tool-grasp-stamped - object-name-T-tool-pregrasp-stamped) + object-name-T-tool-pregrasp-stamped object-name-T-tool-2nd-pregrasp-stamped) (type keyword arm action-type) (type number opening-angle)) (let* ((traj-poses (get-revolute-traj-poses object-name-T-tool-grasp-stamped @@ -211,7 +218,7 @@ frame of the robot's end effector as the child (eg. `cram-tf:*robot-left-tool-fr ,action-type :retracting) (list - (list object-name-T-tool-pregrasp-stamped) + (list object-name-T-tool-pregrasp-stamped object-name-T-tool-2nd-pregrasp-stamped) (list object-name-T-tool-grasp-stamped) traj-poses (list (cram-tf:apply-transform @@ -310,10 +317,14 @@ If :pregrasp, object-name-T-tool-pregrasp is returned, otherwise object-name-T-t :handle handle-name-keyword arm grasp-pose-name)) (handle-T-tool-stamped (if (eq pregrasp-or-grasp :pregrasp) - (car (man-int:get-object-type-to-gripper-pregrasp-transforms + (first (man-int:get-object-type-to-gripper-pregrasp-transforms :handle handle-name-keyword arm grasp-pose-name nil handle-T-tool-grasp-stamped)) - handle-T-tool-grasp-stamped)) + (if (eq pregrasp-or-grasp :2nd-pregrasp) + (second (man-int:get-object-type-to-gripper-pregrasp-transforms + :handle handle-name-keyword arm grasp-pose-name nil + handle-T-tool-grasp-stamped)) + handle-T-tool-grasp-stamped))) (object-name-T-tool-stamped (cram-tf:multiply-transform-stampeds object-name tool-frame diff --git a/cram_3d_world/cram_urdf_projection/src/low-level.lisp b/cram_3d_world/cram_urdf_projection/src/low-level.lisp index 30f0f628d2..23973798c4 100644 --- a/cram_3d_world/cram_urdf_projection/src/low-level.lisp +++ b/cram_3d_world/cram_urdf_projection/src/low-level.lisp @@ -52,6 +52,11 @@ (defparameter *base-resampling-y-limit* 0.2d0 "In meters.") +(defparameter *gripper-gripped-offset* 0.005d0 + "In meters, the minimal distance at which gripper grasps objects. Such that +the GRIPPING action closes the gripper not to complete minimal joint limit, +but to some small value.") + (defparameter *giskard-mode* nil "Use giskard or teleporting/IK solver for joint and cartesian motions.") @@ -567,7 +572,11 @@ with the object, calculates similar angle around Y axis and applies the rotation (btr:joint-state ?robot ((?joint ,(case action-type (:open '?max-limit) - ((:close :grip) '?min-limit) + (:close '?min-limit) + (:grip (+ *gripper-gripped-offset* + (cut:var-value + '?min-limit + solution-bindings))) (T (if (numberp action-type) (* action-type (cut:var-value diff --git a/cram_3d_world/cram_urdf_projection_reasoning/src/check-collisions.lisp b/cram_3d_world/cram_urdf_projection_reasoning/src/check-collisions.lisp index 681c14f985..b45ca3b8ef 100644 --- a/cram_3d_world/cram_urdf_projection_reasoning/src/check-collisions.lisp +++ b/cram_3d_world/cram_urdf_projection_reasoning/src/check-collisions.lisp @@ -454,9 +454,9 @@ Store found pose into designator or throw error if good pose not found." (right-poses-4 (desig:desig-prop-value action-referenced :right-retract-poses))) - (mapc (lambda (pose) - (btr:add-vis-axis-object pose :id (random 100) :length 0.1)) - (append left-poses-3 right-poses-3)) + ;; (mapc (lambda (pose) + ;; (btr:add-vis-axis-object pose :id (random 100) :length 0.1)) + ;; (append left-poses-3 right-poses-3)) (urdf-proj::gripper-action gripper-opening arm) @@ -483,6 +483,8 @@ Store found pose into designator or throw error if good pose not found." (cpl:fail 'common-fail:manipulation-goal-in-collision))))) (list left-poses-1 left-poses-2 left-poses-3 left-poses-4) (list right-poses-1 right-poses-2 right-poses-3 right-poses-4) + ;; have to avoid collisions when grasping, otherwise the grasp pose + ;; will be perpendicular to the handle instead of parallel to it (list :avoid-all :avoid-all :allow-all :allow-all)) ;; (when (eq (desig:desig-prop-value action-desig :type) :opening) ;; (when (btr:robot-colliding-objects-without-attached) diff --git a/cram_common/cram_common_designators/src/motions.lisp b/cram_common/cram_common_designators/src/motions.lisp index 3e3035ed60..e4beb3e38c 100644 --- a/cram_common/cram_common_designators/src/motions.lisp +++ b/cram_common/cram_common_designators/src/motions.lisp @@ -101,7 +101,9 @@ ?collision-object-a ?move-base ?prefer-base ?align-planes-left - ?align-planes-right)) + ?align-planes-right + ?straight-line + ?precise-tracking)) (property ?designator (:type :moving-tcp)) (once (or (property ?designator (:left-pose ?left-pose)) (equal ?left-pose nil))) @@ -120,10 +122,14 @@ (equal ?move-base nil))) (once (or (desig:desig-prop ?designator (:prefer-base ?prefer-base)) (equal ?prefer-base nil))) + (once (or (desig:desig-prop ?designator (:straight-line ?straight-line)) + (equal ?straight-line nil))) (once (or (desig:desig-prop ?designator (:align-planes-left ?align-planes-left)) (equal ?align-planes-left nil))) (once (or (desig:desig-prop ?designator (:align-planes-right ?align-planes-right)) - (equal ?align-planes-right nil)))) + (equal ?align-planes-right nil))) + (once (or (desig:desig-prop ?designator (:precise-tracking ?precise-tracking)) + (equal ?precise-tracking nil)))) (<- (motion-grounding ?designator (?push-or-pull ?arm ?poses ?joint-angle @@ -133,7 +139,8 @@ ?collision-object-a ?move-base ?prefer-base ?align-planes-left - ?align-planes-right)) + ?align-planes-right + ?joint-pose)) (or (and (property ?designator (:type :pushing)) (equal ?push-or-pull move-arm-push)) (and (property ?designator (:type :pulling)) @@ -155,6 +162,8 @@ (equal ?move-base nil))) (once (or (desig:desig-prop ?designator (:prefer-base ?prefer-base)) (equal ?prefer-base nil))) + (once (or (desig:desig-prop ?designator (:door-joint-pose ?joint-pose)) + (equal ?joint-pose nil))) (once (or (desig:desig-prop ?designator (:align-planes-left ?align-planes-left)) (equal ?align-planes-left nil))) (once (or (desig:desig-prop ?designator (:align-planes-right ?align-planes-right)) diff --git a/cram_common/cram_designator_specification/src/specs.lisp b/cram_common/cram_designator_specification/src/specs.lisp index 3b073a1594..73d28cc349 100644 --- a/cram_common/cram_designator_specification/src/specs.lisp +++ b/cram_common/cram_designator_specification/src/specs.lisp @@ -71,7 +71,7 @@ (<- (%property ?designator (?object-key ?object)) (lisp-pred typep ?designator desig:motion-designator) - (member ?object-key (:object :objects)) + (member ?object-key (:object :objects :source-object :target-object)) (property-member (?object-key ?object) ?designator) (assert-type ?object desig:object-designator "MOTION SPEC:PROPERTY")) @@ -136,7 +136,7 @@ :arm :direction :grasp :camera :type :context :link :configuration :left-configuration :right-configuration - :collision-mode)) + :collision-mode :side)) (property-member (?keyword-or-list-key ?value) ?designator) (assert-type ?value (or keyword list) "ACTION SPEC:PROPERTY")) @@ -180,7 +180,13 @@ (lisp-pred typep ?designator desig:action-designator) (member ?key (:function)) (property-member (?key ?value) ?designator) - (assert-type ?value (or function null) "ACTION SPEC:PROPERTY"))) + (assert-type ?value (or function null) "ACTION SPEC:PROPERTY")) + + (<- (%property ?designator (?key ?value)) + (lisp-pred typep ?designator desig:action-designator) + (member ?key (:park-arms)) + (property-member (?key ?value) ?designator) + (assert-type ?value boolean "ACTION SPEC:PROPERTY"))) (def-fact-group location-designator-specs (%property) diff --git a/cram_common/cram_location_costmap/src/cost-functions.lisp b/cram_common/cram_location_costmap/src/cost-functions.lisp index e62d4a39f2..a5671addf5 100644 --- a/cram_common/cram_location_costmap/src/cost-functions.lisp +++ b/cram_common/cram_location_costmap/src/cost-functions.lisp @@ -116,28 +116,28 @@ in a value of 1.0" start-y (resolution grid) (origin-y grid)) below (map-coordinate->array-index end-y (resolution grid) (origin-y grid)) - by (/ (resolution costmap-metadata) (resolution grid)) + by (/ (resolution costmap-metadata) (resolution grid)) for y-destination-index from (map-coordinate->array-index start-y (resolution costmap-metadata) (origin-y costmap-metadata)) below (map-coordinate->array-index end-y (resolution costmap-metadata) (origin-y costmap-metadata)) - by (/ (resolution costmap-metadata) - (resolution costmap-metadata)) + by (/ (resolution costmap-metadata) + (resolution costmap-metadata)) do (loop for x-source-index from (map-coordinate->array-index start-x (resolution grid) (origin-x grid)) below (map-coordinate->array-index end-x (resolution grid) (origin-x grid)) - by (/ (resolution costmap-metadata) (resolution grid)) + by (/ (resolution costmap-metadata) (resolution grid)) for x-destination-index from (map-coordinate->array-index start-x (resolution costmap-metadata) (origin-x costmap-metadata)) below (map-coordinate->array-index end-x (resolution costmap-metadata) (origin-x costmap-metadata)) - by (/ (resolution costmap-metadata) - (resolution costmap-metadata)) + by (/ (resolution costmap-metadata) + (resolution costmap-metadata)) do (setf (aref matrix (truncate y-destination-index) @@ -208,10 +208,9 @@ Optionally an angle `theta' can be given in radiant, if the input `matrix' needs ;; but simply setf ;; as we cannot setf values into the output matrix if it already has values, ;; we need an extra clean matrix to put the input in, and then add to the original - (let ((empty-output-matrix - (cma:make-double-matrix - (truncate (width costmap-metadata) (resolution costmap-metadata)) - (truncate (height costmap-metadata) (resolution costmap-metadata))))) + (let* ((array-width (truncate (width costmap-metadata) (resolution costmap-metadata))) + (array-height (truncate (height costmap-metadata) (resolution costmap-metadata))) + (empty-output-matrix (cma:make-double-matrix array-width array-height))) ;; If we look top down on the costmap visualization, ;; `start' is on the right bottom corner, `end' in on the top left. @@ -285,14 +284,14 @@ Optionally an angle `theta' can be given in radiant, if the input `matrix' needs y-coordinate-maybe-rotated-mid sin-theta cos-theta rotation-point-x rotation-point-y))) - (setf y-coordinate-maybe-rotated - (second new-x-and-y) - x-coordinate-maybe-rotated - (first new-x-and-y) - y-coordinate-maybe-rotated-mid - (second new-x-and-y-mid) - x-coordinate-maybe-rotated-mid - (first new-x-and-y-mid)) + (setf y-coordinate-maybe-rotated + (second new-x-and-y) + x-coordinate-maybe-rotated + (first new-x-and-y) + y-coordinate-maybe-rotated-mid + (second new-x-and-y-mid) + x-coordinate-maybe-rotated-mid + (first new-x-and-y-mid)) ;; check if the rotated points are out-of-bounds (when (or (< y-coordinate-maybe-rotated destination-origin-y) @@ -310,7 +309,7 @@ Optionally an angle `theta' can be given in radiant, if the input `matrix' needs destination-origin-x) (>= x-coordinate-maybe-rotated-mid destination-end-x)) - (continue)))) + (continue)))) ;; Get the indices from the input matrix to get the value ;; Get the corresponding indices from the maybe rotated point @@ -337,17 +336,29 @@ Optionally an angle `theta' can be given in radiant, if the input `matrix' needs (map-coordinate->array-index x-coordinate-maybe-rotated-mid (resolution costmap-metadata) destination-origin-x))) - - ;; Save the value from the rotated input-matrix in the - ;; corresponding field in the output-matrix. - (setf (aref empty-output-matrix - y-destination-index x-destination-index) - (aref matrix - y-input-index x-input-index) - (aref empty-output-matrix - y-destination-index-mid x-destination-index-mid) - (aref matrix - y-input-index x-input-index))))) + ;; check if the indices are out of bounds + (unless (or (< y-destination-index 0) + (>= y-destination-index array-height) + (< x-destination-index 0) + (>= x-destination-index array-width) + (< y-destination-index-mid 0) + (>= y-destination-index-mid array-height) + (< x-destination-index-mid 0) + (>= x-destination-index-mid array-width) + (< y-input-index 0) + (>= y-input-index array-height) + (< x-input-index 0) + (>= x-input-index array-width)) + ;; Save the value from the rotated input-matrix in the + ;; corresponding field in the output-matrix. + (setf (aref empty-output-matrix + y-destination-index x-destination-index) + (aref matrix + y-input-index x-input-index) + (aref empty-output-matrix + y-destination-index-mid x-destination-index-mid) + (aref matrix + y-input-index x-input-index)))))) finally (return (cma:m+ output-matrix empty-output-matrix))))))) (make-instance 'map-costmap-generator diff --git a/cram_common/cram_manipulation_interfaces/src/package.lisp b/cram_common/cram_manipulation_interfaces/src/package.lisp index f54d6b19c5..1852b5b73b 100644 --- a/cram_common/cram_manipulation_interfaces/src/package.lisp +++ b/cram_common/cram_manipulation_interfaces/src/package.lisp @@ -103,6 +103,8 @@ #:*x-across-z-grasp-rotation-2* #:*-x-across-z-grasp-rotation* #:*-x-across-z-grasp-rotation-2* + #:*-x-across-z-45-deg-grasp-rotation* + #:*-x-across-z+45-deg-grasp-rotation* #:*x-across-y-grasp-rotation* #:*x-across-y-30-deg-grasp-rotation* #:*x-across-y-24-deg-grasp-rotation* @@ -133,4 +135,5 @@ #:*rotation-around-y-90-list* #:*rotation-around-z+90-list* #:*rotation-around-z-90-list* - #:*rotation-around-z+180-list*)) + #:*rotation-around-z+180-list* + #:GET-OBJECT-TYPE-ROBOT-FRAME-TILT-APPROACH-TRANSFORM)) diff --git a/cram_common/cram_manipulation_interfaces/src/standard-grasps.lisp b/cram_common/cram_manipulation_interfaces/src/standard-grasps.lisp index 4a94456b71..8d9fb65913 100644 --- a/cram_common/cram_manipulation_interfaces/src/standard-grasps.lisp +++ b/cram_common/cram_manipulation_interfaces/src/standard-grasps.lisp @@ -53,6 +53,19 @@ '(( 0 0 1) (-1 0 0) ( 0 -1 0))) +(defparameter *-x-across-z+45-deg-grasp-rotation* + `((,(sin (/ pi 2)) 0 ,(sin (/ pi 2))) + (,(sin (/ pi 2)) 0 ,(sin (/ pi -2))) + (0 1 0))) +(defparameter *-x-across-z-45-deg-grasp-rotation* + `((,(sin (/ pi -2)) 0 ,(sin (/ pi 2))) + (,(sin (/ pi 2)) 0 ,(sin (/ pi 2))) + (0 1 0))) +;; (defparameter *-x-across-z-30-deg-grasp-rotation* +;; `((0 0 1) +;; (1 0 0) +;; (0 1 0))) + (defparameter *x-across-y-grasp-rotation* '(( 0 0 -1) ( 0 -1 0) diff --git a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp index e8e4fd245f..7ec14908a0 100644 --- a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp +++ b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp @@ -596,59 +596,234 @@ so we assume that all the source contents drops into the target right away." location objects-acted-on &key tilt-angle side) - (let* ((source-object - (first objects-acted-on)) - (source-object-name - (desig:desig-prop-value source-object :name)) - (source-object-type - (desig:desig-prop-value source-object :type)) + (print side) + (print arm) + (print "this is in pouring trajectories") + (let* (;; (source-object + ;; (first objects-acted-on)) + ;; (source-object-name + ;; (desig:desig-prop-value source-object :name)) + ;; (source-object-type + ;; (desig:desig-prop-value source-object :type)) (target-object - (second objects-acted-on)) + (first objects-acted-on)) (target-object-name (desig:desig-prop-value target-object :name)) (target-object-type (desig:desig-prop-value target-object :type)) (b-T-to (get-object-transform target-object)) - (to-T-so - (get-source-object-in-target-object-transform - source-object-type source-object-name - target-object-type target-object-name - side grasp)) - (so-T-stdg - (get-object-type-to-gripper-transform - source-object-type source-object-name arm grasp)) - (to-T-stdg - (reduce #'cram-tf:apply-transform - `(,to-T-so ,so-T-stdg) - :from-end T)) - (to-T-so-tilts - (case grasp - (:front (cram-tf:rotate-pose-in-own-frame - to-T-so :y tilt-angle)) - (:side (case arm - (:left (cram-tf:rotate-pose-in-own-frame - to-T-so :x tilt-angle)) - (:right (cram-tf:rotate-pose-in-own-frame - to-T-so :x (- tilt-angle))) - (t (error "arm can only be :left or :right")))) + + + (to-T-to-offset + (get-object-type-robot-frame-tilt-approach-transform + target-object-type arm side)) + ;; Since the grippers orientation should not depend on the + ;; orientation of the object it is omitted here. + (oTg-std + (cram-tf:copy-transform-stamped + (get-object-type-to-gripper-transform + target-object-type target-object-name arm side) + :rotation (cl-tf:make-identity-rotation))) + + (approach-pose + (cl-tf:copy-pose-stamped + (man-int:calculate-gripper-pose-in-base + (cram-tf:apply-transform + b-T-to + to-T-to-offset) + arm oTg-std) + :orientation + (cl-tf:rotation to-T-to-offset))) + (tilt-angle (cram-math:degrees->radians 40)) + (pre-tilting-poses + (case side + (:top-front (rotate-pose-in-own-frame-and-change-z + approach-pose :y (cram-math:degrees->radians 60) 0.05 0 0.031)) + (:top-left (rotate-pose-in-own-frame-and-change-z + approach-pose :x (cram-math:degrees->radians 60) 0.0 -0.05 0.031)) + (:top-right (cram-tf:apply-transform + (cram-tf:pose-stamped->transform-stamped + (rotate-pose-in-own-frame-and-change-z + approach-pose :x (- (cram-math:degrees->radians 80)) 0.0 0.0 0.06) + (if (eq arm :left) + cram-tf:*robot-left-tool-frame* + cram-tf:*robot-right-tool-frame*)) + (cl-transforms-stamped:make-transform-stamped + (if (eq arm :left) + cram-tf:*robot-left-tool-frame* + cram-tf:*robot-right-tool-frame*) + (if (eq arm :left) + cram-tf:*robot-left-tool-frame* + cram-tf:*robot-right-tool-frame*) + 0 + (cl-transforms:make-3d-vector 0 0.0 0) + (cl-transforms:make-identity-rotation)) + :result-as-pose-or-transform :pose)) + ;;0.031 z (t (error "can only pour from :side or :front")))) - (to-T-stdg-tilts - (reduce #'cram-tf:apply-transform - `(,to-T-so-tilts ,so-T-stdg) - :from-end T))) - (mapcar (lambda (label transforms) - (make-traj-segment + (tilting-poses + (case side + (:top-front (rotate-pose-in-own-frame-and-change-z + pre-tilting-poses :y tilt-angle -0.02 0 0.06)) + (:top-left (rotate-pose-in-own-frame-and-change-z + pre-tilting-poses :x tilt-angle 0 0 -0.05)) + (:top-right (cram-tf:rotate-pose-in-own-frame + pre-tilting-poses :x (- tilt-angle))) + (t (error "can only pour from :side or :front")))) + + (tilting-poses-second + (case side + (:top-front (cram-tf:rotate-pose-in-own-frame + tilting-poses :y tilt-angle)) + (:top-left (cram-tf:rotate-pose-in-own-frame + tilting-poses :x tilt-angle)) + (:top-right (cram-tf:rotate-pose-in-own-frame + tilting-poses :x (- tilt-angle))) + (t (error "can only pour from :side or :front"))) ) + + (tilting-poses-third + (case side + (:top-front (cram-tf:rotate-pose-in-own-frame + tilting-poses-second :y tilt-angle)) + (:top-left (cram-tf:rotate-pose-in-own-frame + tilting-poses-second :x tilt-angle)) + (:top-right (cram-tf:rotate-pose-in-own-frame + tilting-poses-second :x (- tilt-angle))) + (t (error "can only pour from :side or :front"))))) + + ;;(tilting-poses + ;; rotate-pose-in-own-frame + ;; (get-tilting-poses side (list approach-pose)))) + ;; (print "tilting-poses:") + ;; (print tilting-poses) + ;; (sleep 5) + + + ;; (print "poses") + ;; (print approach-pose) + ;; (print pre-tilting-poses) + ;; (sleep 10) + + (mapcar (lambda (label poses-in-base) + (man-int:make-traj-segment :label label - :poses (mapcar (alexandria:curry #'calculate-gripper-pose-in-map - b-T-to arm) - transforms))) + :poses (mapcar + (lambda (pose-in-base) + (let ((mTb (cram-tf:pose->transform-stamped + cram-tf:*fixed-frame* + cram-tf:*robot-base-frame* + 0.0 + (cram-tf:robot-current-pose))) + (bTg-std + (cram-tf:pose-stamped->transform-stamped + pose-in-base + (cl-tf:child-frame-id b-T-to)))) + (cl-tf:ensure-pose-stamped + (cram-tf:apply-transform mTb bTg-std)))) + poses-in-base))) + '(:reaching :tilting-down - :tilting-up - :retracting) - `(,to-T-stdg - ,to-T-stdg-tilts - ,to-T-stdg - ,to-T-stdg)))) + :tilting + :tilting-second + :tilting-third + ;; :retracting + ) + `((,approach-pose) + (,pre-tilting-poses) + (,tilting-poses) + (,tilting-poses-second) + (,tilting-poses-third) + ;; (,to-T-stdg + ))) + +) + +(defun get-tilting-poses (grasp approach-poses &optional (angle (cram-math:degrees->radians 100))) + (mapcar (lambda (?approach-pose) + ;;depending on the grasp the angle to tilt is different + (case grasp + (:top-front (rotate-once-pose ?approach-pose (+ angle) :y)) + (:top-left (rotate-once-pose ?approach-pose (+ angle) :x)) + (:top-right (rotate-once-pose ?approach-pose (- angle) :x)) + (:top (rotate-once-pose ?approach-pose (- angle) :y)) + (t (error "can only pour from :top-side, :top or :top-front")))) + approach-poses)) + +;;helper function for tilting +;;rotate the pose around the axis in an angle +(defun rotate-once-pose (pose angle axis) + (cl-transforms-stamped:copy-pose-stamped + pose + :orientation (let ((pose-orientation (cl-transforms:orientation pose))) + (cl-tf:normalize + (cl-transforms:q* + (cl-transforms:axis-angle->quaternion + (case axis + (:x (cl-transforms:make-3d-vector 1 0 0)) + (:y (cl-transforms:make-3d-vector 0 1 0)) + (:z (cl-transforms:make-3d-vector 0 0 1)) + (t (error "in ROTATE-ONCE-POSE forgot to specify axis properly: ~a" axis))) + angle) + pose-orientation))))) + + + + + +(defgeneric get-object-type-robot-frame-tilt-approach-transform (object-type arm grasp) + (:documentation "Returns a transform stamped") + (:method (object-type arm grasp) + (call-with-specific-type #'man-int:get-object-type-robot-frame-tilt-approach-transform + object-type arm grasp))) + +(defmethod get-object-type-robot-frame-tilt-approach-transform :around (object-type arm grasp) + (destructuring-bind + ((x y z) (ax ay az aw)) + (call-next-method) + (cl-tf:transform->transform-stamped + cram-tf:*robot-base-frame* + cram-tf:*robot-base-frame* + 0.0 + (cl-tf:pose->transform + (cl-transforms:make-pose + (cl-transforms:make-3d-vector x y z) + (cl-transforms:make-quaternion ax ay az aw)))))) + + + (defun rotate-pose-in-own-frame-and-change-z (pose axis angle x y z) + (let* ((pose-origin + (cl-transforms:origin pose)) + (new-origin + (cpl:par (when (not (eq z 0)) + (cl-transforms:copy-3d-vector pose-origin + :x + (+ (cl-transforms:x pose-origin) x) + :y + (+ (cl-transforms:y pose-origin) y) + :z + (- (cl-transforms:z pose-origin) z))) + (cl-transforms:copy-3d-vector pose-origin))) + + + (pose-orientation + (cl-transforms:orientation pose)) + (new-orientation + (cl-transforms:q* + pose-orientation + (cl-transforms:axis-angle->quaternion + (case axis + (:x (cl-transforms:make-3d-vector 1 0 0)) + (:y (cl-transforms:make-3d-vector 0 1 0)) + (:z (cl-transforms:make-3d-vector 0 0 1)) + (t (error "[CRAM-TF:ROTATE-POSE] axis ~a not specified properly" axis))) + angle)))) + + (etypecase pose + (cl-transforms-stamped:pose-stamped + (cl-transforms-stamped:copy-pose-stamped pose :origin new-origin :orientation new-orientation)) + (cl-transforms:pose + (cl-transforms:copy-pose pose :orientation new-orientation)) + ))) diff --git a/cram_common/cram_mobile_pick_place_plans/src/atomic-action-designators.lisp b/cram_common/cram_mobile_pick_place_plans/src/atomic-action-designators.lisp index dce1698988..a40f06a51c 100644 --- a/cram_common/cram_mobile_pick_place_plans/src/atomic-action-designators.lisp +++ b/cram_common/cram_mobile_pick_place_plans/src/atomic-action-designators.lisp @@ -110,16 +110,51 @@ (once (or (spec:property ?action-designator (:collision-mode ?collision)) (equal ?collision :avoid-all))) (infer-motion-flags ?action-designator - ?_ ?move-base ?align-planes-left ?align-planes-right) + ?_ ?move-base-inferred ?align-planes-left ?align-planes-right) + (once (or (desig:desig-prop ?action-designator (:move-base ?move-base)) + (equal ?move-base ?move-base-inferred))) + (-> (equal ?action-type :lifting) + (equal ?straight-line T) + (equal ?straight-line NIL)) (desig:designator :action ((:type ?action-type) (:left-poses ?left-poses) (:right-poses ?right-poses) (:collision-mode ?collision) (:move-base ?move-base) + (:straight-line ?straight-line) (:align-planes-left ?align-planes-left) (:align-planes-right ?align-planes-right)) ?resolved-action-designator)) + + (<- (desig:action-grounding ?action-designator (move-arms-in-sequence + ?resolved-action-designator)) + (or (spec:property ?action-designator (:type :tilting))) + (spec:property ?action-designator (:type ?action-type)) + (once (or (spec:property ?action-designator (:left-poses ?left-poses)) + (equal ?left-poses nil))) + (once (or (spec:property ?action-designator (:right-poses ?right-poses)) + (equal ?right-poses nil))) + (once (or (spec:property ?action-designator (:collision-mode ?collision)) + (equal ?collision :allow-all))) + (infer-motion-flags ?action-designator + ?_ ?move-base-inferred ?align-planes-left ?align-planes-right) + (once (or (desig:desig-prop ?action-designator (:move-base ?move-base)) + (equal ?move-base ?move-base-inferred))) + (-> (equal ?action-type :lifting) + (equal ?straight-line T) + (equal ?straight-line NIL)) + (desig:designator :action ((:type ?action-type) + (:left-poses ?left-poses) + (:right-poses ?right-poses) + (:collision-mode ?collision) + (:move-base ?move-base) + (:straight-line ?straight-line) + (:align-planes-left nil) + (:align-planes-right nil) + (:precise-tracking T)) + ?resolved-action-designator)) + (<- (desig:action-grounding ?action-designator (move-arms-in-sequence ?resolved-action-designator)) (or (spec:property ?action-designator (:type :retracting))) @@ -130,13 +165,20 @@ (equal ?right-poses nil))) (once (or (spec:property ?action-designator (:collision-mode ?collision)) (equal ?collision :allow-hand))) - (infer-motion-flags ?action-designator - ?_ ?move-base ?align-planes-left ?align-planes-right) + (-> (desig:desig-prop ?action-designator (:application-context :pouring)) + (and (equal ?move-base T) + (equal ?align-planes-left NIL) + (equal ?align-planes-right NIL) + (equal ?straight-line NIL)) + (and (infer-motion-flags ?action-designator + ?_ ?move-base ?align-planes-left ?align-planes-right) + (equal ?straight-line T))) (desig:designator :action ((:type ?action-type) (:left-poses ?left-poses) (:right-poses ?right-poses) (:collision-mode ?collision) (:move-base ?move-base) + (:straight-line ?straight-line) (:align-planes-left ?align-planes-left) (:align-planes-right ?align-planes-right)) ?resolved-action-designator)) @@ -165,11 +207,16 @@ (desig:designator :action ((:type ?action-type) (:left-poses ?left-poses) (:right-poses ?right-poses) + ;; TODO: check if :avoid-all is possible to achieve + ;; with environment manipulation as application-context (:collision-mode :allow-hand) (:collision-object-b ?collision-object-b) (:collision-object-b-link ?object-link) (:prefer-base ?prefer-base) - (:move-base ?move-base) + (:move-base NIL;; ?move-base + ) + (:straight-line T) + (:precise-tracking T) (:align-planes-left ?align-planes-left) (:align-planes-right ?align-planes-right)) ?resolved-action-designator)) @@ -197,7 +244,7 @@ (desig:designator :action ((:type :putting) (:left-poses ?left-poses) (:right-poses ?right-poses) - (:collision-mode :allow-all;; :allow-attached + (:collision-mode :allow-all ; :allow-attached ) (:collision-object-b ?other-object-name) (:collision-object-b-link ?object-link) @@ -223,6 +270,8 @@ (spec:property ?action-designator (:link ?handle-link)) (once (or (spec:property ?action-designator (:distance ?joint-angle)) (equal ?joint-angle NIL))) + (once (or (desig:desig-prop ?action-designator (:door-joint-pose ?door-joint-pose)) + (equal ?door-joint-pose NIL))) ;; infer the missing parameters (infer-motion-flags ?action-designator ?prefer-base ?move-base @@ -236,6 +285,7 @@ (:collision-object-b-link ?handle-link) (:prefer-base ?prefer-base) (:move-base ?move-base) + (:door-joint-pose ?door-joint-pose) (:align-planes-left ?align-planes-left) (:align-planes-right ?align-planes-right)) ?resolved-action-designator)) @@ -394,4 +444,27 @@ (<- (desig:action-grounding ?action-designator (wait ?action-designator)) (spec:property ?action-designator (:type :waiting)) - (spec:property ?action-designator (:duration ?_)))) + (spec:property ?action-designator (:duration ?_))) + + + + (<- (desig:action-grounding ?action-designator (move-arms-in-sequence + ?resolved-action-designator)) + (or (spec:property ?action-designator (:type :pancake))) + (spec:property ?action-designator (:type ?action-type)) + (once (or (spec:property ?action-designator (:left-poses ?left-poses)) + (equal ?left-poses nil))) + (once (or (spec:property ?action-designator (:right-poses ?right-poses)) + (equal ?right-poses nil))) + (once (or (spec:property ?action-designator (:collision-mode ?collision)) + (equal ?collision :avoid-all))) + (infer-motion-flags ?action-designator + ?_ ?move-base ?align-planes-left ?align-planes-right) + (desig:designator :action ((:type ?action-type) + (:left-poses ?left-poses) + (:right-poses ?right-poses) + (:collision-mode ?collision) + (:move-base t) + (:align-planes-left ?align-planes-left) + (:align-planes-right ?align-planes-right)) + ?resolved-action-designator))) diff --git a/cram_common/cram_mobile_pick_place_plans/src/atomic-action-plans.lisp b/cram_common/cram_mobile_pick_place_plans/src/atomic-action-plans.lisp index d228b4efd4..38830658a6 100644 --- a/cram_common/cram_mobile_pick_place_plans/src/atomic-action-plans.lisp +++ b/cram_common/cram_mobile_pick_place_plans/src/atomic-action-plans.lisp @@ -90,14 +90,17 @@ ((:collision-object-a ?collision-object-a)) ((:move-base ?move-base)) ((:prefer-base ?prefer-base)) + ((:straight-line ?straight-line)) ((:align-planes-left ?align-planes-left)) ((:align-planes-right ?align-planes-right)) + ((:precise-tracking ?precise-tracking)) &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 boolean ?move-base ?prefer-base ?align-planes-left ?align-planes-right)) + (type boolean ?move-base ?prefer-base ?straight-line + ?align-planes-left ?align-planes-right ?precise-tracking)) "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." @@ -118,32 +121,36 @@ while ignoring failures; and execute the last pose with propagating the failures (roslisp:ros-warn (pick-place move-arms-in-sequence) "~a~%Ignoring." e) (return))) - (exe:perform - (desig:a motion - (type moving-tcp) - (desig:when ?left-pose - (left-pose ?left-pose)) - (desig:when ?right-pose - (right-pose ?right-pose)) - (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)))) + (unwind-protect + (exe:perform + (desig:a motion + (type moving-tcp) + (desig:when ?left-pose + (left-pose ?left-pose)) + (desig:when ?right-pose + (right-pose ?right-pose)) + (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 ?straight-line + (straight-line ?straight-line)) + (desig:when ?precise-tracking + (precise-tracking ?precise-tracking)) + (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))))) left-poses right-poses)) @@ -156,32 +163,36 @@ while ignoring failures; and execute the last pose with propagating the failures ;; propagate failures up (roslisp:ros-error (pick-place move-arms-in-sequence) "~a~%Failing." e))) - (exe:perform - (desig:a motion - (type moving-tcp) - (desig:when ?left-pose - (left-pose ?left-pose)) - (desig:when ?right-pose - (right-pose ?right-pose)) - (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))))) + (unwind-protect + (exe:perform + (desig:a motion + (type moving-tcp) + (desig:when ?left-pose + (left-pose ?left-pose)) + (desig:when ?right-pose + (right-pose ?right-pose)) + (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 ?straight-line + (straight-line ?straight-line)) + (desig:when ?precise-tracking + (precise-tracking ?precise-tracking)) + (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)) @@ -196,6 +207,7 @@ while ignoring failures; and execute the last pose with propagating the failures ((:prefer-base ?prefer-base)) ((:align-planes-left ?align-planes-left)) ((:align-planes-right ?align-planes-right)) + ((:door-joint-pose ?door-joint-pose)) &allow-other-keys) (declare (type keyword ?type ?arm) (type list ?poses) @@ -233,7 +245,9 @@ With a continuous motion planner one could have fluent arch trajectories etc. (desig:when ?align-planes-left (align-planes-left ?align-planes-left)) (desig:when ?align-planes-right - (align-planes-right ?align-planes-right)))) + (align-planes-right ?align-planes-right)) + (desig:when ?door-joint-pose + (door-joint-pose ?door-joint-pose)))) (cram-occasions-events:on-event (make-instance 'cram-plan-occasions-events:robot-state-changed)))) @@ -348,8 +362,8 @@ In any case, issue ROBOT-STATE-CHANGED event." (gripper ?left-or-right) (desig:when ?effort (effort ?effort)))) - (roslisp:ros-info (pick-place grip) "Assert grasp into knowledge base") (when object-designator + (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 diff --git a/cram_common/cram_mobile_pick_place_plans/src/pick-place-designators.lisp b/cram_common/cram_mobile_pick_place_plans/src/pick-place-designators.lisp index e297a7e86b..244c1e2b6b 100644 --- a/cram_common/cram_mobile_pick_place_plans/src/pick-place-designators.lisp +++ b/cram_common/cram_mobile_pick_place_plans/src/pick-place-designators.lisp @@ -162,6 +162,9 @@ (equal ?robot-arm-is-also-a-neck T) (equal ?robot-arm-is-also-a-neck NIL)) + (once (or (spec:property ?action-designator (:park-arms ?park-arms)) + (equal ?park-arms T))) + ;; put together resulting action designator (desig:designator :action ((:type :picking-up) (:object ?current-object-desig) @@ -177,7 +180,8 @@ (:left-grasp-poses ?left-grasp-poses) (:right-grasp-poses ?right-grasp-poses) (:left-lift-poses ?left-lift-poses) - (:right-lift-poses ?right-lift-poses)) + (:right-lift-poses ?right-lift-poses) + (:park-arms ?park-arms)) ?resolved-action-designator)) diff --git a/cram_common/cram_mobile_pick_place_plans/src/pick-place-plans.lisp b/cram_common/cram_mobile_pick_place_plans/src/pick-place-plans.lisp index 8c4df98269..800c1552f1 100644 --- a/cram_common/cram_mobile_pick_place_plans/src/pick-place-plans.lisp +++ b/cram_common/cram_mobile_pick_place_plans/src/pick-place-plans.lisp @@ -53,6 +53,7 @@ ((:right-grasp-poses ?right-grasp-poses)) ((:left-lift-poses ?left-lift-poses)) ((:right-lift-poses ?right-lift-poses)) + park-arms &allow-other-keys) (declare (type desig:object-designator ?object-designator) (type keyword ?arm ?grasp) @@ -64,8 +65,8 @@ (ignore location-type)) "Open gripper, reach traj, grasp traj, close gripper, issue grasping event, lift." - (cram-tf:visualize-marker (man-int:get-object-pose ?object-designator) - :r-g-b-list '(1 1 0) :id 300) + ;; (cram-tf:visualize-marker (man-int:get-object-pose ?object-designator) + ;; :r-g-b-list '(1 1 0) :id 300) (unless robot-arm-is-also-a-neck (roslisp:ros-info (pick-place pick-up) "Looking") @@ -80,8 +81,9 @@ (type looking) (target (desig:a location (pose ?look-pose))))))) - (cpl:par - (roslisp:ros-info (pick-place pick-up) "Opening gripper and reaching") + (;cpl:par + cpl:seq + (roslisp:ros-info (pick-place pick-up) "Opening gripper and reaching") (let ((?goal `(cpoe:gripper-joint-at ,?arm ,?gripper-opening))) (exe:perform (desig:an action @@ -148,16 +150,21 @@ (desig:an action (type monitoring-joint-state) (gripper ?arm))) + (cram-occasions-events:on-event + (make-instance 'cpoe:object-detached-robot + :arm ?arm + :object-name (desig:desig-prop-value ?object-designator :name))) (cpl:fail 'common-fail:gripper-closed-completely :description "Object slipped"))) - (roslisp:ros-info (pick-place place) "Parking") - (exe:perform - (desig:an action - (type parking-arms) - ;; TODO: this will not work with dual-arm grasping - ;; but as our ?arm is declared as a keyword, - ;; for now this code is the right code - (arms (?arm))))) + (when park-arms + (roslisp:ros-info (pick-place place) "Parking") + (exe:perform + (desig:an action + (type parking-arms) + ;; TODO: this will not work with dual-arm grasping + ;; but as our ?arm is declared as a keyword, + ;; for now this code is the right code + (arms (?arm)))))) @@ -206,29 +213,30 @@ (target (desig:a location (pose ?look-pose))))))) (roslisp:ros-info (pick-place place) "Reaching") - (cpl:with-failure-handling - ((common-fail:manipulation-low-level-failure (e) - (roslisp:ros-warn (pp-plans pick-up) - "Manipulation messed up: ~a~%Ignoring." - e) - ;; (return) - )) - (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) - (exe:perform - (desig:an action - (type reaching) - (location ?target-location-designator) - (left-poses ?left-reach-poses) - (right-poses ?right-reach-poses) - (goal ?goal))))) + (cpl:with-retry-counters ((reach-retries 1)) + (cpl:with-failure-handling + ((common-fail:manipulation-low-level-failure (e) + (roslisp:ros-warn (pp-plans pick-up) + "Manipulation messed up: ~a~%Ignoring." + e) + (cpl:do-retry reach-retries + (cpl:retry)) + (return))) + (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) + (exe:perform + (desig:an action + (type reaching) + (location ?target-location-designator) + (left-poses ?left-reach-poses) + (right-poses ?right-reach-poses) + (goal ?goal)))))) (roslisp:ros-info (pick-place place) "Putting") (cpl:with-failure-handling ((common-fail:manipulation-low-level-failure (e) (roslisp:ros-warn (pp-plans pick-up) "Manipulation messed up: ~a~%Ignoring." e) - ;; (return) - )) + (return))) (let ((?goal `(cpoe:tool-frames-at ,?left-put-poses ,?right-put-poses))) (exe:perform (desig:an action @@ -240,7 +248,7 @@ (right-poses ?right-put-poses) (goal ?goal))))) (when ?placing-location-name - (roslisp:ros-info (boxy-plans connect) "Asserting assemblage connection in knowledge base") + (roslisp:ros-info (pp-plans put) "Asserting assemblage connection in knowledge base") (if other-object-is-a-robot (cram-occasions-events:on-event (make-instance 'cpoe:object-attached-robot @@ -248,7 +256,8 @@ (desig:desig-prop-value ?other-object-designator :urdf-name)) :not-loose t :object-name (desig:desig-prop-value ?object-designator :name) - :other-object-name (desig:desig-prop-value ?other-object-designator :name) + :other-object-name (or (desig:desig-prop-value ?other-object-designator :name) + (desig:desig-prop-value ?other-object-designator :part-of)) :grasp ?placing-location-name)) (cram-occasions-events:on-event (make-instance 'cpoe:object-attached-object diff --git a/cram_common/cram_mobile_pick_place_plans/src/pour-designators.lisp b/cram_common/cram_mobile_pick_place_plans/src/pour-designators.lisp index 5b822be250..8bc4a56eea 100644 --- a/cram_common/cram_mobile_pick_place_plans/src/pour-designators.lisp +++ b/cram_common/cram_mobile_pick_place_plans/src/pour-designators.lisp @@ -29,47 +29,57 @@ (in-package :pp-plans) -(def-fact-group pouring-designator (desig:action-grounding) +(def-fact-group pou-designator (desig:action-grounding) (<- (desig:action-grounding ?action-designator (pour ?resolved-designator)) (spec:property ?action-designator (:type :pouring)) - (spec:property ?action-designator (:target-object ?_)) - (once (or (spec:property ?action-designator (:arm ?_)) - (spec:property ?action-designator (:source-object ?_)))) - (once (or (spec:property ?action-designator (:wait-duration ?_)) - (true))) - (-> (spec:property ?action-designator (:sides ?sides)) - (eqaul ?resolved-designator ?action-designator) - (and (desig:desig-description ?action-designator ?description) - (append ?description ((:sides (:left-side :right-side :front :back))) - ?new-description) - (desig:designator :action ?new-description ?resolved-designator)))) + (spec:property ?action-designator (:on-object ?_)) + ;;pec:property ?action-designator (:object ?_))) + + (spec:property ?action-designator (:arm ?_)) + ;; (once (or (spec:property ?action-designator (:arm ?_)) + ;; (spec:property ?action-designator (:object ?_)))) + (once (or (spec:property ?action-designator (:wait-duration ?_)) + (true))) + (-> (spec:property ?action-designator (:sides ?sides)) + (eqaul ?resolved-designator ?action-designator) + (and (desig:desig-description ?action-designator ?description) + (append ?description ((:sides (:top-left :top-right :top-front))) + ?new-description) + (desig:designator :action ?new-description ?resolved-designator)))) + (<- (desig:action-grounding ?action-designator (pour-without-retries ?resolved-action-designator)) (spec:property ?action-designator (:type :pouring-without-retries)) - (spec:property ?action-designator (:side ?side)) - ;; source + + (-> (spec:property ?action-designator (:configuration ?side)) + (true) + (format "WARNINGL: please specify a side")) (-> (spec:property ?action-designator (:arm ?arm)) - (-> (spec:property ?action-designator (:source-object - ?source-designator)) - (once (or (cpoe:object-in-hand ?source-designator ?arm ?grasp) - (format "WARNING: Wanted to pour from object ~a ~ - with arm ~a, but it's not in the arm.~%" - ?source-designator ?arm))) - (cpoe:object-in-hand ?source-designator ?arm ?grasp)) - (-> (spec:property ?action-designator (:source-object - ?source-designator)) - (once (or (cpoe:object-in-hand ?source-designator ?arm ?grasp) - (format "WARNING: Wanted to pour from object ~a ~ - but it's not in any of the hands.~%" - ?source-designator))) - (cpoe:object-in-hand ?source-designator ?arm ?grasp))) - (desig:current-designator ?source-designator ?current-source-designator) - ;; destination / target - (spec:property ?action-designator (:target-object ?target-designator)) + (true) + (format "WARNINGL: please specify an arm")) + ;; ;; source + ;; (-> (spec:property ?action-designator (:arm ?arm)) + ;; (-> (spec:property ?action-designator (:object + ;; ?source-designator)) + ;; (once (or (cpoe:object-in-hand ?source-designator ?arm ?grasp) + ;; (format "WARNING: Wanted to pour from object ~a ~ + ;; with arm ~a, but it's not in the arm.~%" + ;; ?source-designator ?arm))) + ;; (cpoe:object-in-hand ?source-designator ?arm ?grasp)) + ;; (-> (spec:property ?action-designator (:object + ;; ?source-designator)) + ;; (once (or (cpoe:object-in-hand ?source-designator ?arm ?grasp) + ;; (format "WARNING: Wanted to pour from object ~a ~ + ;; but it's not in any of the hands.~%" + ;; ?source-designator))) + ;; (cpoe:object-in-hand ?source-designator ?arm ?grasp))) + ;; (desig:current-designator ?source-designator ?current-source-designator) + ;;destination / target + (spec:property ?action-designator (:on-object ?target-designator)) (desig:current-designator ?target-designator ?current-target-designator) ;; angle (-> (spec:property ?action-designator (:tilt-angle ?tilt-angle)) @@ -77,95 +87,126 @@ (lisp-fun man-int:get-tilt-angle-for-pouring ?source-type ?target-type ?tilt-angle)) ;; cartesian pouring trajectory - (equal ?objects-acted-on (?current-source-designator + (equal ?objects-acted-on (;; ?current-source-designator ?current-target-designator)) (-> (equal ?arm :left) (and (lisp-fun man-int:get-action-trajectory :pouring - ?arm ?grasp nil ?objects-acted-on + ?arm ?side nil ?objects-acted-on :tilt-angle ?tilt-angle :side ?side ?left-trajectory) (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :reaching ?left-reach-poses) (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :tilting-down ?left-tilt-down-poses) - (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :tilting-up + (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :tilting ?left-tilt-up-poses) - (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :retracting - ?left-retract-poses)) + (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :tilting-second + ?left-tilt-second-poses) + (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :tilting-third + ?left-tilt-third-poses) + ;; (lisp-fun man-int:get-traj-poses-by-label ?left-trajectory :retracting + ;; ?left-retract-poses) + ) + (and (equal ?left-reach-poses NIL) (equal ?left-tilt-down-poses NIL) (equal ?left-tilt-up-poses NIL) + (equal ?left-tilt-second-poses NIL) + (equal ?left-tilt-third-poses NIL) (equal ?left-retract-poses NIL))) + (-> (equal ?arm :right) (and (lisp-fun man-int:get-action-trajectory :pouring - ?arm ?grasp nil ?objects-acted-on + ?arm ?side nil ?objects-acted-on :tilt-angle ?tilt-angle :side ?side ?right-trajectory) (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :reaching ?right-reach-poses) (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :tilting-down ?right-tilt-down-poses) - (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :tilting-up + (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :tilting ?right-tilt-up-poses) - (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :retracting - ?right-retract-poses)) + (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :tilting-second + ?right-tilt-second-poses) + (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :tilting-third + ?right-tilt-third-poses) + ;; (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :retracting + ;; ?right-retract-poses) + ) + ;; (and (equal ?right-reach-poses t) + ;; (equal ?right-tilt-down-poses t) + ;; (equal ?right-tilt-up-poses t) + ;; (equal ?right-tilt-second-poses t) + ;; (equal ?right-tilt-third-poses t) + ;; (equal ?right-retract-poses t)) + (and (equal ?right-reach-poses NIL) (equal ?right-tilt-down-poses NIL) (equal ?right-tilt-up-poses NIL) + (equal ?right-tilt-second-poses NIL) + (equal ?right-tilt-third-poses NIL) (equal ?right-retract-poses NIL))) - (once (or (lisp-pred identity ?left-trajectory) - (lisp-pred identity ?right-trajectory))) + + ;; wait duration - (-> (spec:property ?action-designator (:wait-duration ?wait-duration)) - (true) - (lisp-fun man-int:get-wait-duration-for-pouring - ?source-type ?target-type ?tilt-angle - ?wait-duration)) + ;; (-> (spec:property ?action-designator (:wait-duration ?wait-duration)) + ;; (true) + ;; (lisp-fun man-int:get-wait-duration-for-pouring + ;; ?source-type ?taget-type ?tilt-angle + ;; ?wait-duration)) ;; look pose - (-> (lisp-pred identity ?left-grasp-poses) - (equal ?left-tilt-down-poses (?look-pose . ?_)) - (equal ?right-tilt-down-poses (?look-pose . ?_))) - ;; should look or not? - (rob-int:robot ?robot) - (-> (man-int:robot-arm-is-also-a-neck ?robot ?arm) - (equal ?robot-arm-is-also-a-neck T) - (equal ?robot-arm-is-also-a-neck NIL)) - ;; put together resulting designator + ;; (-> (lisp-pred identity ?left-trajectory) + ;; (equal ?left-tilt-down-poses (?look-pose . ?_)) + ;; (equal ?right-tilt-down-poses (?look-pose . ?_))) + ;; ;; ;; should look or not? + ;; (rob-int:robot ?robot) + ;; (-> (man-int:robot-arm-is-also-a-neck ?robot ?arm) + ;; (equal ?robot-arm-is-also-a-neck T) + ;; (equal ?robot-arm-is-also-a-neck NIL)) + + + ;;put together resulting designator (desig:designator :action ((:type :pouring) - (:source-object ?current-source-designator) + ;;(:object ?current-source-designator) (:arm ?arm) - (:side ?side) - (:grasp ?grasp) - (:target-object ?current-target-designator) - ;; (:other-object-is-a-robot ?other-object-is-a-robot) - (:look-pose ?look-pose) - (:robot-arm-is-also-a-neck ?robot-arm-is-also-a-neck) - (:wait-duration ?wait-duration) + (:configuration ?side) + ;;(:grasp ?grasp) + (:on-object ?current-target-designator) + ;; ;; (:other-object-is-a-robot ?other-object-is-a-robot) + ;;(:look-pose ?look-pose) + ;(:robot-arm-is-also-a-neck ?robot-arm-is-also-a-neck) + ;;(:wait-duration ?wait-duration) + (:left-reach-poses ?left-reach-poses) - (:right-reach-poses ?right-reach-poses) (:left-tilt-down-poses ?left-tilt-down-poses) - (:right-tilt-down-poses ?right-tilt-down-poses) (:left-tilt-up-poses ?left-tilt-up-poses) + (:left-tilt-second-poses ?left-tilt-second-poses) + (:left-tilt-third-poses ?left-tilt-third-poses) + + (:right-reach-poses ?right-reach-poses) + (:right-tilt-down-poses ?right-tilt-down-poses) (:right-tilt-up-poses ?right-tilt-up-poses) - (:left-retract-poses ?left-retract-poses) - (:right-retract-poses ?right-retract-poses)) - ?resolved-action-designator)) + (:right-tilt-second-poses ?right-tilt-second-poses) + (:right-tilt-third-poses ?right-tilt-third-poses) + ) + + ?resolved-action-designator))) - (<- (action-grounding ?action-designator (move-arms-in-sequence - ?resolved-action-designator)) - (spec:property ?action-designator (:type :tilting)) - (once (or (spec:property ?action-designator (:left-poses ?left-poses)) - (equal ?left-poses nil))) - (once (or (spec:property ?action-designator (:right-poses ?right-poses)) - (equal ?right-poses nil))) - (once (or (spec:property ?action-designator (:collision-mode ?collision)) - (equal ?collision :avoid-all))) - (desig:designator :action ((:type ?action-type) - (:left-poses ?left-poses) - (:right-poses ?right-poses) - (:collision-mode ?collision) - (:move-base nil) - (:align-planes-left nil) - (:align-planes-right nil)) - ?resolved-action-designator))) + ;; (<- (action-grounding ?action-designator (move-arms-in-sequence + ;; ?resolved-action-designator)) + ;; (spec:property ?action-designator (:type :tilting)) + ;; (once (or (spec:property ?action-designator (:left-poses ?left-poses)) + ;; (equal ?left-poses nil))) + ;; (once (or (spec:property ?action-designator (:right-poses ?right-poses)) + ;; (equal ?right-poses nil))) + ;; (once (or (spec:property ?action-designator (:collision-mode ?collision)) + ;; (equal ?collision :avoid-all))) + ;; (desig:designator :action ((:type ?action-type) + ;; (:left-poses ?left-poses) + ;; (:right-poses ?right-poses) + ;; (:collision-mode ?collision) + ;; (:move-base nil) + ;; (:align-planes-left nil) + ;; (:align-planes-right nil)) + ;; ?resolved-action-designator))) diff --git a/cram_common/cram_mobile_pick_place_plans/src/pour-plans.lisp b/cram_common/cram_mobile_pick_place_plans/src/pour-plans.lisp index 60a9f29f3f..54f21cbd00 100644 --- a/cram_common/cram_mobile_pick_place_plans/src/pour-plans.lisp +++ b/cram_common/cram_mobile_pick_place_plans/src/pour-plans.lisp @@ -41,148 +41,196 @@ (defun pour-without-retries (&key ((:arm ?arm)) - side - grasp + ((:side ?side)) + ;;grasp ((:left-reach-poses ?left-reach-poses)) - ((:right-reach-poses ?right-reach-poses)) ((:left-tilt-down-poses ?left-tilt-down-poses)) - ((:right-tilt-down-poses ?right-tilt-down-poses)) ((:left-tilt-up-poses ?left-tilt-up-poses)) + ((:left-tilt-second-poses ?left-tilt-second-poses)) + ((:left-tilt-third-poses ?left-tilt-third-poses)) + + ((:right-reach-poses ?right-reach-poses)) + ((:right-tilt-down-poses ?right-tilt-down-poses)) ((:right-tilt-up-poses ?right-tilt-up-poses)) - ((:left-retract-poses ?left-retract-poses)) - ((:right-retract-poses ?right-retract-poses)) - ((:target-object ?target-object)) - source-object + ((:right-tilt-second-poses ?right-tilt-second-poses)) + ((:right-tilt-third-poses ?right-tilt-third-poses)) + + ((:on-object ?on-object)) + ;;object ((:wait-duration ?wait-duration)) ((:look-location ?look-location)) robot-arm-is-also-a-neck &allow-other-keys) - (declare (type (or null list) - ?left-reach-poses ?right-reach-poses - ?left-tilt-down-poses ?right-tilt-down-poses - ?left-tilt-up-poses ?right-tilt-up-poses - ?left-retract-poses ?right-retract-poses) - (type desig:object-designator ?target-object source-object) - (type desig:location-designator ?look-location) - (type keyword ?arm side grasp) - (type number ?wait-duration) - (ignore side grasp source-object)) - (roslisp:ros-info (pick-place pour) "Reaching") - (cpl:with-failure-handling - ((common-fail:manipulation-low-level-failure (e) - (roslisp:ros-warn (pp-plans pour) - "Manipulation messed up: ~a~%Failing." - e))) - (cpl:par - (unless robot-arm-is-also-a-neck - (let ( ;; (?goal `(cpoe:looking-at ,?look-location)) - ) - (exe:perform (desig:an action - (type turning-towards) - (target ?look-location) - ;; (goal ?goal) - )))) + + ;; (declare (type (or null list) + ;; ?left-reach-poses ?right-reach-poses + ;; ?left-tilt-down-poses ?right-tilt-down-poses + ;; ?left-tilt-up-poses ?right-tilt-up-poses + ;; ?left-retract-poses ?right-retract-poses) + ;; (type desig:object-designator ?on-object object) + ;; (type desig:location-designator ?look-location) + ;; ;;(type keyword ?arm side grasp) + ;; (type number ?wait-duration) + ;; (ignore side grasp object)) + (let* ((sleepy nil) + (?movy nil) + (?align-planes-left nil) + (?align-planes-right nil) + (?move-base-when-reaching t)) + + (cpl:with-failure-handling + (((or common-fail:manipulation-low-level-failure + common-fail:manipulation-goal-not-reached) (e) + (roslisp:ros-warn (pp-plans pour-reach) + "Manipulation messed up: ~a~%Ignoring." + e) + (return))) (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) (exe:perform (desig:an action (type reaching) - (object ?target-object) + (object ?on-object) (left-poses ?left-reach-poses) (right-poses ?right-reach-poses) - (goal ?goal)))))) - (roslisp:ros-info (pick-place pour) "Tilting down") - (cpl:with-failure-handling - ((common-fail:manipulation-low-level-failure (e) - (roslisp:ros-warn (pp-plans pour) - "Manipulation messed up: ~a~%Failing." - e))) - (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-down-poses - ,?right-tilt-down-poses))) - (exe:perform - (desig:an action - (type tilting) - (left-poses ?left-tilt-down-poses) - (right-poses ?right-tilt-down-poses) - (goal ?goal))))) - (roslisp:ros-info (pick-place pour) "Waiting") - (exe:perform - (desig:an action - (type waiting) - (duration ?wait-duration))) - (roslisp:ros-info (pick-place pour) "Tilting up") - (cpl:with-failure-handling - ((common-fail:manipulation-low-level-failure (e) - (roslisp:ros-warn (pp-plans pour) - "Manipulation messed up: ~a~%Failing." - e))) - (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-up-poses - ,?right-tilt-up-poses))) - (exe:perform - (desig:an action - (type tilting) - (left-poses ?left-tilt-up-poses) - (right-poses ?right-tilt-up-poses) - (goal ?goal))))) - (roslisp:ros-info (pick-place pour) "Retracting") - (cpl:with-failure-handling - ((common-fail:manipulation-low-level-failure (e) - (roslisp:ros-warn (pp-plans pour) - "Manipulation messed up: ~a~%Ignoring." - e) - (return))) - (let ((?goal `(cpoe:tool-frames-at ,?left-retract-poses - ,?right-retract-poses))) - (exe:perform - (desig:an action - (type retracting) - (left-poses ?left-retract-poses) - (right-poses ?right-retract-poses) - (goal ?goal))))) - (roslisp:ros-info (pick-place pour) "Parking") - (exe:perform - (desig:an action - (type parking-arms) - (arms (?arm))))) + (move-base ?move-base-when-reaching) + (goal ?goal))))) + + (setf ?move-base-when-reaching nil) + + (cpl:with-retry-counters ((giskardside-retries 3)) + (cpl:with-failure-handling + (((or common-fail:manipulation-low-level-failure + common-fail:manipulation-goal-not-reached) (e) + (roslisp:ros-warn (pp-plans pour-reach) + "Manipulation messed up: ~a~%Failing." + e) + + (cpl:do-retry giskardside-retries + (break) + (cpl:retry)) + (return))) + (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) + (exe:perform + (desig:an action + (type reaching) + (object ?on-object) + (left-poses ?left-reach-poses) + (right-poses ?right-reach-poses) + (move-base ?move-base-when-reaching) + (goal ?goal)))))) + + + + + (when sleepy + (sleep 2)) + + (cpl:with-retry-counters ((giskardside-retries 3)) + (cpl:with-failure-handling + (((or common-fail:manipulation-low-level-failure + common-fail:manipulation-goal-not-reached) (e) + (roslisp:ros-warn (pp-plans pour-tilt-down-more) + "Manipulation messed up: ~a~%Failing." + e) + + (cpl:do-retry giskardside-retries + (cpl:retry)) + (return))) + + + (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-up-poses ,?right-tilt-up-poses))) + (exe:perform + (desig:an action + (type tilting) + (object ?on-object) + (left-poses ?left-tilt-up-poses) + (right-poses ?right-tilt-up-poses) + (align-planes-left ?align-planes-left) + (align-planes-right ?align-planes-right) + (move-base ?movy) + ;;(collision-mode :allow-attached) + (goal ?goal)))) + )) + + + (when sleepy + (sleep 5)) + + + + + + (cpl:with-retry-counters ((giskardside-retries 3)) + (cpl:with-failure-handling + (((or common-fail:manipulation-low-level-failure + common-fail:manipulation-goal-not-reached) (e) + (roslisp:ros-warn (pp-plans pour-retract) + "Manipulation messed up: ~a~%Failing." + e) + + (cpl:do-retry giskardside-retries + (cpl:retry)) + (return))) + + + (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) + (exe:perform + (desig:an action + (type retracting) + (object ?on-object) + (left-poses ?left-reach-poses) + (right-poses ?right-reach-poses) + (application-context pouring) + (goal ?goal)))) + )) + + + ) ) + (defun pour (&key ((:arm ?arm)) sides - ((:source-object ?source-object)) - ((:target-object ?target-object)) + ;; ((:object ?object)) + ((:on-object ?on-object)) ((:wait-duration ?wait-duration)) &allow-other-keys) - (declare (type desig:object-designator ?source-object ?target-object) + (declare (type desig:object-designator ;; ?object + ?on-object) (type keyword ?arm) - (type number ?wait-duration) + ;(type (or nil number) ?wait-duration) (type list sides)) - (let ((?side (cut:lazy-car sides))) - ;; if pouring fails, try to pour from another side - (cpl:with-retry-counters ((side-retries 3)) - (cpl:with-failure-handling - (((or common-fail:manipulation-low-level-failure - common-fail:object-unreachable - desig:designator-error) (e) - (common-fail:retry-with-list-solutions - sides - side-retries - (:error-object-or-string - (format NIL "Pouring failed: ~a.~%Next" e) - :warning-namespace (mpp-plans pour)) - (setf ?side (cut:lazy-car sides))))) + + ;; if pouring fails, try to pour from another side + ;; (cpl:with-retry-counters ((side-retries 3)) + ;; (cpl:with-failure-handling + ;; (((or common-fail:manipulation-low-level-failure + ;; common-fail:object-unreachable + ;; desig:designator-error) (e) + ;; (common-fail:retry-with-list-solutions + ;; sides + ;; side-retries + ;; (:error-object-or-string + ;; (format NIL "Pouring failed: ~a.~%Next" e) + ;; :warning-namespace (mpp-plans pour)) + ;; (setf ?side (cut:lazy-car sides))))) + (print ?side) (exe:perform (desig:an action (type pouring-without-retries) - (side ?side) - (target-object ?target-object) + (configuration ?side) + (on-object ?on-object) (desig:when ?arm (arm ?arm)) - (desig:when ?source-object - (source-object ?source-object)) + ;; (desig:when ?object + ;; (object ?object)) (desig:when ?wait-duration - (wait-duration ?wait-duration)))))))) + (wait-duration ?wait-duration)) + )) + )) @@ -204,3 +252,23 @@ (type pouring) (source-object ?source-object) (target-object ?target-object)))) + + +(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform + ((object-type (eql :bowl)) + arm + (grasp (eql :top-left))) + '((-0.01 0.245 0.020)(0 0 0 1))) + +(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform + ((object-type (eql :bowl)) + arm + (grasp (eql :top-right))) + '((-0.02 0.17 0.05)(0 0 0 1))) + +(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform + ((object-type (eql :bowl)) + arm + (grasp (eql :top-front))) + '((-0.1 0.0 0.019)(0 0 0 1))) + diff --git a/cram_common/cram_tf/src/package.lisp b/cram_common/cram_tf/src/package.lisp index b7e3747f87..b1c6fe76f7 100644 --- a/cram_common/cram_tf/src/package.lisp +++ b/cram_common/cram_tf/src/package.lisp @@ -73,6 +73,7 @@ #:*fixed-frame* #:*robot-base-frame* #:*odom-frame* #:*robot-torso-frame* #:*robot-torso-joint* #:*robot-left-tool-frame* #:*robot-right-tool-frame* + #:*robot-left-wrist-frame* #:*robot-right-wrist-frame* #:*broadcaster* *tf-broadcasting-enabled* *tf-broadcasting-topic* *tf-broadcasting-interval* ;; visualization diff --git a/cram_common/cram_tf/src/setup.lisp b/cram_common/cram_tf/src/setup.lisp index b02b93a543..6975e4e1cd 100644 --- a/cram_common/cram_tf/src/setup.lisp +++ b/cram_common/cram_tf/src/setup.lisp @@ -65,6 +65,11 @@ or in general at compile-time.") (defvar *robot-right-tool-frame* nil "Tool frame of the right arm. Initialized from CRAM robot desciption.") +(defvar *robot-left-wrist-frame* nil + "Wrist frame of the left arm. Initialized from CRAM robot description.") +(defvar *robot-right-wrist-frame* nil + "Wrist frame of the right arm. Initialized from CRAM robot desciption.") + (defun init-tf () (macrolet ((initialize-var (dynamic-var prolog-var) (let ((var-name (symbol-name dynamic-var))) @@ -110,8 +115,8 @@ or in general at compile-time.") (cl-urdf:name (cl-urdf:parse-urdf (cut:replace-all ; our pr2 urdf has some garbage inside :( - (roslisp:get-param rob-int:*robot-description-parameter*) - "\\" " "))) + (roslisp:get-param rob-int:*robot-description-parameter*) + "\\" " "))) :keyword))) (rob-int:set-robot-name robot-name) (roslisp:ros-info (cram-tf init-tf) "Robot name is ~a." robot-name)) @@ -141,7 +146,15 @@ or in general at compile-time.") (with-vars-bound (?right-tool-frame) (lazy-car (prolog `(and (robot ?robot) (robot-tool-frame ?robot :right ?right-tool-frame)))) - (initialize-var *robot-right-tool-frame* ?right-tool-frame)))) + (initialize-var *robot-right-tool-frame* ?right-tool-frame)) + (with-vars-bound (?left-wrist-frame) + (lazy-car (prolog `(and (robot ?robot) + (end-effector-link ?robot :left ?left-wrist-frame)))) + (initialize-var *robot-left-wrist-frame* ?left-wrist-frame)) + (with-vars-bound (?right-wrist-frame) + (lazy-car (prolog `(and (robot ?robot) + (end-effector-link ?robot :right ?right-wrist-frame)))) + (initialize-var *robot-right-wrist-frame* ?right-wrist-frame)))) (defun destroy-tf () (setf *transformer* nil) diff --git a/cram_common/cram_tf/src/utilities.lisp b/cram_common/cram_tf/src/utilities.lisp index cbfb0d8c58..a93a8fc4c9 100644 --- a/cram_common/cram_tf/src/utilities.lisp +++ b/cram_common/cram_tf/src/utilities.lisp @@ -143,11 +143,12 @@ (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) +(defun ensure-pose-in-frame (pose frame + &key use-current-ros-time use-zero-time transformer) (declare (type (or null cl-transforms:pose cl-transforms-stamped:pose-stamped))) (when pose (cl-transforms-stamped:transform-pose-stamped - *transformer* + (or transformer *transformer*) :pose (let ((pose-stamped (cl-transforms-stamped:ensure-pose-stamped pose frame 0.0))) (if use-zero-time @@ -157,10 +158,11 @@ :timeout *tf-default-timeout* :use-current-ros-time use-current-ros-time))) -(defun ensure-point-in-frame (point frame &key use-current-ros-time use-zero-time) +(defun ensure-point-in-frame (point frame + &key use-current-ros-time use-zero-time transformer) (declare (type (or cl-transforms:point cl-transforms-stamped:point-stamped))) (cl-transforms-stamped:transform-point-stamped - *transformer* + (or transformer *transformer*) :point (if (typep point 'cl-transforms-stamped:point-stamped) (if use-zero-time (with-slots (frame-id origin) point @@ -173,7 +175,7 @@ :use-current-ros-time use-current-ros-time)) (defun ensure-transform-in-frame (transform frame - &key use-current-ros-time use-zero-time) + &key use-current-ros-time use-zero-time transformer) (declare (type (or null cl-transforms-stamped:transform-stamped))) (when transform (let* ((child-frame @@ -182,7 +184,8 @@ (ensure-pose-in-frame (strip-transform-stamped transform) frame :use-current-ros-time use-current-ros-time - :use-zero-time use-zero-time))) + :use-zero-time use-zero-time + :transformer transformer))) (pose-stamped->transform-stamped new-pose-stamped child-frame)))) (defun translate-pose (pose &key (x 0.0) (y 0.0) (z 0.0)) @@ -222,6 +225,15 @@ (cl-transforms:pose (cl-transforms:copy-pose pose :orientation new-orientation))))) +(defun copy-transform-stamped (transform-stamped &key frame-id child-frame-id stamp + translation rotation) + (cl-transforms-stamped:make-transform-stamped + (or frame-id (cl-transforms-stamped:frame-id transform-stamped)) + (or child-frame-id (cl-transforms-stamped:child-frame-id transform-stamped)) + (or stamp (cl-transforms-stamped:stamp transform-stamped)) + (or translation (cl-transforms-stamped:translation transform-stamped)) + (or rotation (cl-transforms-stamped:rotation transform-stamped)))) + (defun rotate-pose-in-own-frame (pose axis angle) (let* ((pose-orientation (cl-transforms:orientation pose)) @@ -239,7 +251,8 @@ (cl-transforms-stamped:pose-stamped (cl-transforms-stamped:copy-pose-stamped pose :orientation new-orientation)) (cl-transforms:pose - (cl-transforms:copy-pose pose :orientation new-orientation))))) + (cl-transforms:copy-pose pose :orientation new-orientation)) + ))) (defun rotate-transform-in-own-frame (transform axis angle) (let* ((transform-rotation @@ -298,19 +311,19 @@ Multiply from the right with the yTz transform -- xTy * yTz == xTz." (when (string-not-equal (cl-transforms-stamped:frame-id x-y-transform) x-frame) - (warn "~%~%~%~%!!!!!~%~%~%In multiply-transform-stampeds X-Y-TRANSFORM~%~ + (warn "In multiply-transform-stampeds X-Y-TRANSFORM~%~ did not have correct parent frame: ~a and ~a" (cl-transforms-stamped:frame-id x-y-transform) x-frame)) (when (string-not-equal (cl-transforms-stamped:child-frame-id y-z-transform) z-frame) - (warn "~%~%~%~%!!!!!~%~%~%In multiply-transform-stampeds Y-Z-TRANSFORM~%~ + (warn "In multiply-transform-stampeds Y-Z-TRANSFORM~%~ did not have correct child frame: ~a and ~a" (cl-transforms-stamped:child-frame-id y-z-transform) z-frame)) (when (string-not-equal (cl-transforms-stamped:child-frame-id x-y-transform) (cl-transforms-stamped:frame-id y-z-transform)) - (warn "~%~%~%~%!!!!!~%~%~%In multiply-transform-stampeds X-Y-TRANSFORM and~%~ + (warn "In multiply-transform-stampeds X-Y-TRANSFORM and~%~ Y-Z-TRANSFORM did not have equal corresponding frames: ~a and ~a" (cl-transforms-stamped:child-frame-id x-y-transform) (cl-transforms-stamped:frame-id y-z-transform))) @@ -340,14 +353,6 @@ Multiply from the right with the yTz transform -- xTy * yTz == xTz." (cl-transforms-stamped:translation transform-stamped) (cl-transforms:rotation transform-stamped))) -(defun copy-transform-stamped (transform-stamped &key frame-id child-frame-id stamp - translation rotation) - (cl-transforms-stamped:make-transform-stamped - (or frame-id (cl-transforms-stamped:frame-id transform-stamped)) - (or child-frame-id (cl-transforms-stamped:child-frame-id transform-stamped)) - (or stamp (cl-transforms-stamped:stamp transform-stamped)) - (or translation (cl-transforms-stamped:translation transform-stamped)) - (or rotation (cl-transforms-stamped:rotation transform-stamped)))) (defun translate-transform-stamped (transform &key (x-offset 0.0) (y-offset 0.0) (z-offset 0.0)) @@ -391,6 +396,10 @@ Multiply from the right with the yTz transform -- xTy * yTz == xTz." (defun values-converged (values goal-values deltas) (flet ((value-converged (value goal-value delta) (<= (abs (- value goal-value)) delta))) + ;; if both values are NIL, assume they converged... + (when (and (null goal-values) + (null values)) + (return-from values-converged T)) ;; correct arguments (if (listp values) (if (or (atom goal-values) diff --git a/cram_common/cram_tf/src/visualization.lisp b/cram_common/cram_tf/src/visualization.lisp index e6932524eb..46ba0d534d 100644 --- a/cram_common/cram_tf/src/visualization.lisp +++ b/cram_common/cram_tf/src/visualization.lisp @@ -89,7 +89,7 @@ (r color) (first r-g-b-list) (g color) (second r-g-b-list) (b color) (third r-g-b-list) - (a color) 0.7 + (a color) (or (fourth r-g-b-list) 0.7) :frame_locked t :mesh_resource (or mesh-path "")))) ;; (roslisp:ros-warn (ll visualize-marker) "asked to visualize a null pose") diff --git a/cram_core/cram_designators/src/cram-designators/designator-protocol.lisp b/cram_core/cram_designators/src/cram-designators/designator-protocol.lisp index c19610e7d3..8d735c2009 100644 --- a/cram_core/cram_designators/src/cram-designators/designator-protocol.lisp +++ b/cram_core/cram_designators/src/cram-designators/designator-protocol.lisp @@ -140,29 +140,11 @@ (defvar *designator-pprint-description* t "If set to T, DESIGNATOR objects will be pretty printed with their description.") -;; (defmethod print-object ((object designator) stream) -;; (print-unreadable-object (object stream :type t :identity t) -;; (when *designator-pprint-description* -;; (write (description object) :stream stream)))) - (defmethod print-object ((object designator) stream) - (flet ((no-colon (keyword) - (if (symbolp keyword) - (intern (symbol-name keyword)) - keyword))) - (print-unreadable-object (object stream :type nil :identity nil) - (when *designator-pprint-description* - (write (no-colon (quantifier object)) :stream stream) - (write #\ :escape nil :stream stream) - (write (no-colon (get-desig-class object)) :stream stream) - (dolist (key-value (description object)) - (write #\linefeed :escape nil :stream stream) - (write " " :escape nil :stream stream) - (write #\( :escape nil :stream stream) - (write (no-colon (car key-value)) :stream stream) - (write #\ :escape nil :stream stream) - (write (no-colon (cadr key-value)) :stream stream) - (write #\) :escape nil :stream stream)))))) + (print-unreadable-object (object stream :type nil :identity nil) + (when *designator-pprint-description* + (format stream "(~A ~<~A~:@_~{~<(~A ~A)~:>~^~:@_~})~:>" + (quantifier object) (list (get-desig-class object) (description object)))))) (define-hook cram-utilities::on-equate-designators (successor parent)) diff --git a/cram_core/cram_task_tree_export/src/conversion.lisp b/cram_core/cram_task_tree_export/src/conversion.lisp index 804793851e..653c149f46 100644 --- a/cram_core/cram_task_tree_export/src/conversion.lisp +++ b/cram_core/cram_task_tree_export/src/conversion.lisp @@ -33,28 +33,28 @@ (defparameter *add-type* NIL "Adds the TYPE to each entry in the formatted tree.") -(defmethod format-node ((atom_ T)) +(defmethod format-desig ((atom_ T)) atom_) -(defmethod format-node ((symbol_ symbol)) +(defmethod format-desig ((symbol_ symbol)) symbol_) -(defmethod format-node ((string_ string)) +(defmethod format-desig ((string_ string)) string_) -(defmethod format-node ((number_ number)) +(defmethod format-desig ((number_ number)) number_) -(defmethod format-node ((list_ cons)) - (mapcar #'format-node list_)) +(defmethod format-desig ((list_ cons)) + (mapcar #'format-desig list_)) -(defmethod format-node ((desig cram-designators:designator)) +(defmethod format-desig ((desig cram-designators:designator)) (let ((desig-desc (cram-designators:description desig))) (mapcar (lambda (key-value) `(,@(when *add-type* (type-of (second key-value))) - (,(first key-value) - ,(format-node (second key-value))))) + ,(first key-value) + ,(format-desig (second key-value)))) desig-desc))) ;;;;;;;;;;;;;;;;;;;;;;;;; @@ -77,16 +77,16 @@ ("z" . ,z) ("w" . ,w))) -(defmethod format-node ((object cl-transforms:pose)) +(defmethod format-desig ((object cl-transforms:pose)) (with-slots ((origin cl-transforms:origin) (orientation cl-transforms:orientation)) object `(("origin" - . ,(format-node origin)) + . ,(format-desig origin)) ("orientation" - . ,(format-node orientation))))) + . ,(format-desig orientation))))) -(defmethod format-node ((object cl-transforms:3d-vector)) +(defmethod format-desig ((object cl-transforms:3d-vector)) (with-slots ((x cl-transforms:x) (y cl-transforms:y) (z cl-transforms:z)) @@ -95,7 +95,7 @@ ("y" . ,y) ("z" . ,z)))) -(defmethod format-node ((object cl-transforms:quaternion)) +(defmethod format-desig ((object cl-transforms:quaternion)) (with-slots ((x cl-transforms:x) (y cl-transforms:y) (z cl-transforms:z) @@ -106,25 +106,25 @@ ("z" . ,z) ("w" . ,w)))) -(defmethod format-node ((object cl-transforms:transform)) +(defmethod format-desig ((object cl-transforms:transform)) (with-slots ((origin cl-transforms:translation) (orientation cl-transforms:rotation)) object `(("translation" - . ,(format-node origin)) + . ,(format-desig origin)) ("rotation" - . ,(format-node orientation))))) + . ,(format-desig orientation))))) -(defmethod format-node ((object cl-transforms-stamped:stamped)) +(defmethod format-desig ((object cl-transforms-stamped:stamped)) (with-slots ((stamp cl-transforms-stamped:stamp) (frame-id cl-transforms-stamped:frame-id)) object `(("header" . ,(format-header stamp frame-id))))) -(defmethod format-node ((object cl-transforms-stamped:vector-stamped)) +(defmethod format-desig ((object cl-transforms-stamped:vector-stamped)) (with-slots ((x cl-transforms:x) (y cl-transforms:y) (z cl-transforms:z) (stamp cl-transforms-stamped:stamp) (frame-id cl-transforms-stamped:frame-id)) @@ -134,7 +134,7 @@ ("vector" . ,(format-point x y z))))) -(defmethod format-node ((object cl-transforms-stamped:point-stamped)) +(defmethod format-desig ((object cl-transforms-stamped:point-stamped)) (with-slots ((x cl-transforms:x) (y cl-transforms:y) (z cl-transforms:z) (stamp cl-transforms-stamped:stamp) (frame-id cl-transforms-stamped:frame-id)) @@ -144,7 +144,7 @@ ("point" . ,(format-point x y z))))) -(defmethod format-node ((object cl-transforms-stamped:pose-stamped)) +(defmethod format-desig ((object cl-transforms-stamped:pose-stamped)) (with-slots ((origin cl-transforms:origin) (orientation cl-transforms:orientation) (stamp cl-transforms-stamped:stamp) @@ -163,7 +163,7 @@ ("orientation" . ,(format-quaternion q1 q2 q3 w))))))))) -(defmethod format-node ((object cl-transforms-stamped:transform-stamped)) +(defmethod format-desig ((object cl-transforms-stamped:transform-stamped)) (with-slots ((origin cl-transforms:translation) (orientation cl-transforms:rotation) (stamp cl-transforms-stamped:stamp) diff --git a/cram_core/cram_task_tree_export/src/exporter.lisp b/cram_core/cram_task_tree_export/src/exporter.lisp index 6b28c89bef..6e3c60050f 100644 --- a/cram_core/cram_task_tree_export/src/exporter.lisp +++ b/cram_core/cram_task_tree_export/src/exporter.lisp @@ -33,9 +33,17 @@ (defparameter *export-file-path* "/tmp/task_tree_export.dot") (defmethod cl-dot:object-node ((node task-tree-node)) - (let ((node-label (format nil "~a" (format-node (node->designator node))))) + (let ((node-label (format-node node)) + (fill-color (case (node-status node) + (:SUCCEEDED "#ffffff") + (:FAILED "#ff9999") + (:EVAPORATED "#cccccc") + (otherwise "#e6e6e6")))) (make-instance 'cl-dot:node :attributes `(:label (:left ,node-label) ; text-align left + :style :filled + :color :black + :fillcolor ,fill-color :shape :box)))) (defmethod cl-dot:object-points-to ((node task-tree-node)) @@ -43,7 +51,7 @@ (defun export-task-tree (&key (data (get-task-tree)) - (path *export-file-path*) + (path (format nil "/tmp/task_tree_export-~d.dot" (sb-posix:time))) dry-run) "Exports the task-tree to a dot file. data - a task-tree-node @@ -59,7 +67,7 @@ dry-run - don't generate dot file, just the graph object " :if-does-not-exist :create))) (cl-dot:print-graph graph :stream stream) (close stream)) - (format T "File exported to ~a.~%~a~%~a~%~a" path + (format T "File exported to~%~a~%~a~%~a~%~a" path "Install graphviz to convert from .dot file:" "dot -Tpdf -o " "dot -Tsvg -o ")) diff --git a/cram_core/cram_task_tree_export/src/tree-utils.lisp b/cram_core/cram_task_tree_export/src/tree-utils.lisp index 7b79744be0..23b0c0eec3 100644 --- a/cram_core/cram_task_tree_export/src/tree-utils.lisp +++ b/cram_core/cram_task_tree_export/src/tree-utils.lisp @@ -40,13 +40,36 @@ (mapcar 'cdr (cpl-impl:task-tree-node-children node)))) (defun node-valid (node) - "Checks if given node execution :SUCCEEDED. -Moves step-by-step down through slots 'task', 'status' and 'value'" + "Checks if given node has any kind of sexp to be valid." + (slot-value (cpl-impl:task-tree-node-code node) 'cpl-impl::sexp)) + +(defun node-status (node) (let ((slinky (cpl-impl:task-tree-node-code node))) - (and (setf slinky (slot-value slinky 'task)) - (setf slinky (slot-value slinky 'status)) - (setf slinky (slot-value slinky 'value)) - (eq slinky :SUCCEEDED)))) + (when slinky + (setf slinky (slot-value slinky 'task)) + (when slinky + (setf slinky (slot-value slinky 'status)) + (when slinky + (slot-value slinky 'value)))))) + +(defun node-failure (node) + (when node + (let ((slinky (cpl-impl:task-tree-node-code node))) + (when slinky + (setf slinky (slot-value slinky 'task)) + (when slinky + (setf slinky (slot-value slinky 'result)) + (when slinky + (type-of slinky))))))) + +(defun format-node (node) + (let ((status (node-status node))) + (when node + (format nil "~{~a~^~%~}" + `(,status + ,@(unless (eq status :SUCCEEDED) + `(,(node-failure node))) + ,(format-desig (node->designator node))))))) (defun node->designator (node) "Provides a node's designator, if it has any." diff --git a/cram_demos/cram_object_knowledge/cram-object-knowledge.asd b/cram_demos/cram_object_knowledge/cram-object-knowledge.asd index 93b115a8bd..570e2dfd2a 100644 --- a/cram_demos/cram_object_knowledge/cram-object-knowledge.asd +++ b/cram_demos/cram_object_knowledge/cram-object-knowledge.asd @@ -41,7 +41,7 @@ :components ((:file "package") (:file "environment" :depends-on ("package")) - (:file "household" :depends-on ("package")) + (:file "multiple-trajectory-poses" :depends-on ("package")) + (:file "household" :depends-on ("package" "multiple-trajectory-poses")) (:file "assembly" :depends-on ("package")) - (:file "retail" :depends-on ("package")) - (:file "multiple-trajectory-poses" :depends-on ("package")))))) + (:file "retail" :depends-on ("package" "multiple-trajectory-poses")))))) diff --git a/cram_demos/cram_object_knowledge/src/environment.lisp b/cram_demos/cram_object_knowledge/src/environment.lisp index 4e12ec0ebb..4dd1034cb0 100644 --- a/cram_demos/cram_object_knowledge/src/environment.lisp +++ b/cram_demos/cram_object_knowledge/src/environment.lisp @@ -50,7 +50,8 @@ (<- (costmap:costmap-origin :apartment -5 -5)) (<- (costmap:costmap-resolution :apartment 0.04)) - (<- (man-int:object-tf-prefix :iai-kitchen "iai_kitchen/"))) + (<- (man-int:object-tf-prefix ?environment "iai_kitchen/") + (member ?environment (:iai-kitchen :apartment :dm-shelves :dm-room :store :storage)))) (def-fact-group environment-object-type-hierarchy (man-int:object-type-direct-subtype) @@ -74,6 +75,8 @@ (defmethod man-int:get-action-gripper-opening :heuristics 20 ((object-type (eql :container))) 0.06) +(defmethod man-int:get-action-gripper-opening :heuristics 20 ((object-type (eql :cupboard))) + 0.08) ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; @@ -91,12 +94,12 @@ (defmethod man-int:get-container-opening-distance :heuristics 20 ((container-name (eql :cabinet7-door-bottom-left))) - 0.95d0 ; 54 deg + 0.9d0 ; 54 deg ) (defmethod man-int:get-container-opening-distance :heuristics 20 ((container-name (eql :cabinet7))) - 0.95d0 ; 54 deg + 0.9d0 ; 54 deg ) ;; (defmethod man-int:get-container-opening-distance :heuristics 20 @@ -108,25 +111,42 @@ (defparameter *handle-grasp-x-offset* 0.0 "in meters") (defparameter *handle-grasp-y-offset* 0.0 "in meters") (defparameter *handle-grasp-z-offset* 0.0 "in meters") -(defparameter *handle-pregrasp-x-offset-open* 0.05 "in meters") -(defparameter *handle-pregrasp-y-offset-open* 0.05 "in meters") +(defparameter *handle-pregrasp-x-offset-open* 0.2 "in meters") +(defparameter *handle-2nd-pregrasp-x-offset-open* 0.15 "in meters") +(defparameter *handle-pregrasp-y-offset-open* 0.15 "in meters") +(defparameter *handle-2nd-pregrasp-y-offset-open* 0.15 "in meters") (defparameter *handle-pregrasp-x-offset-close* -0.0 "in meters") -(defparameter *handle-retract-offset* 0.05 "in meters") +;; (defparameter *handle-retract-offset* 0.15 "in meters") ; defined in env-manip +(defparameter *handle-side-grasp-x-offset* 0.0 "in meters") +(defparameter *handle-side-pregrasp-x-offset-open* 0.0 "in meters") + ;; SIDE grasp for handle along Z (man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :left-side - :grasp-translation `(0.0d0 ,(- *handle-grasp-y-offset*) ,*handle-grasp-z-offset*) + :grasp-translation `(,(- *handle-side-grasp-x-offset*) + ,(- *handle-grasp-y-offset*) + ,*handle-grasp-z-offset*) :grasp-rot-matrix man-int:*y-across-z-grasp-rotation* - :pregrasp-offsets `(0.0 ,*handle-pregrasp-y-offset-open* 0.0) - :2nd-pregrasp-offsets `(0.0 ,*handle-pregrasp-y-offset-open* 0.0) + :pregrasp-offsets `(,(- *handle-side-pregrasp-x-offset-open*) + ,*handle-pregrasp-y-offset-open* + 0.0) + :2nd-pregrasp-offsets `(0.0 + ,*handle-2nd-pregrasp-y-offset-open* + 0.0) :lift-translation '(0 0 0) :2nd-lift-translation '(0 0 0)) (man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :right-side - :grasp-translation `(0.0d0 ,*handle-grasp-y-offset* ,*handle-grasp-z-offset*) + :grasp-translation `(,*handle-side-grasp-x-offset* + ,*handle-grasp-y-offset* + ,*handle-grasp-z-offset*) :grasp-rot-matrix man-int:*-y-across-z-grasp-rotation* - :pregrasp-offsets `(0.0 ,(- *handle-pregrasp-y-offset-open*) 0.0) - :2nd-pregrasp-offsets `(0.0 ,(- *handle-pregrasp-y-offset-open*) 0.0) + :pregrasp-offsets `(,(- *handle-side-pregrasp-x-offset-open*) + ,(- *handle-pregrasp-y-offset-open*) + 0.0) + :2nd-pregrasp-offsets `(0.0 + ,(- *handle-2nd-pregrasp-y-offset-open*) + 0.0) :lift-translation '(0 0 0) :2nd-lift-translation '(0 0 0)) @@ -135,7 +155,16 @@ :grasp-translation `(,(- *handle-grasp-x-offset*) 0.0d0 ,*handle-grasp-z-offset*) :grasp-rot-matrix man-int:*-x-across-z-grasp-rotation* :pregrasp-offsets `(,(- *handle-pregrasp-x-offset-open*) 0.0 0.0) - :2nd-pregrasp-offsets `(,(- *handle-pregrasp-x-offset-open*) 0.0 0.0) + :2nd-pregrasp-offsets `(,(- *handle-2nd-pregrasp-x-offset-open*) 0.0 0.0) + :lift-translation '(0 0 0) + :2nd-lift-translation '(0 0 0)) + +;; BACK grasp for handle along Y +(man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :back-along-y + :grasp-translation `(,(- *handle-grasp-x-offset*) 0.0 ,*handle-grasp-z-offset*) + :grasp-rot-matrix man-int:*-x-across-y-grasp-rotation* + :pregrasp-offsets `(,(- *handle-pregrasp-x-offset-open*) 0.0 0.0) + :2nd-pregrasp-offsets `(,(- *handle-2nd-pregrasp-x-offset-open*) 0.0 0.0) :lift-translation '(0 0 0) :2nd-lift-translation '(0 0 0)) @@ -144,7 +173,7 @@ :grasp-translation `(,*handle-grasp-x-offset* 0.0d0 ,*handle-grasp-z-offset*) :grasp-rot-matrix man-int:*x-across-z-grasp-rotation* :pregrasp-offsets `(,*handle-pregrasp-x-offset-open* 0.0 0.0) - :2nd-pregrasp-offsets `(,*handle-pregrasp-x-offset-open* 0.0 0.0) + :2nd-pregrasp-offsets `(,*handle-2nd-pregrasp-x-offset-open* 0.0 0.0) :lift-translation '(0 0 0) :2nd-lift-translation '(0 0 0)) @@ -153,6 +182,24 @@ :grasp-translation `(,*handle-grasp-x-offset* ,*handle-grasp-y-offset* ,*handle-grasp-z-offset*) :grasp-rot-matrix man-int:*x-across-y-grasp-rotation* :pregrasp-offsets `(,*handle-pregrasp-x-offset-open* 0.0 0.0) - :2nd-pregrasp-offsets `(,*handle-pregrasp-x-offset-open* 0.0 0.0) + :2nd-pregrasp-offsets `(,*handle-2nd-pregrasp-x-offset-open* 0.0 0.0) + :lift-translation '(0 0 0) + :2nd-lift-translation '(0 0 0)) + +;; ANGLED 45 degree for door handles towards pos x & neg y +(man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :door-angled-2 + :grasp-translation `(,(- 0.025) 0.025 ,*handle-grasp-z-offset*) + :grasp-rot-matrix man-int:*-x-across-z+45-deg-grasp-rotation* + :pregrasp-offsets `(,(- *handle-pregrasp-x-offset-open*) 0.10 0.0) + :2nd-pregrasp-offsets `(,(- *handle-2nd-pregrasp-x-offset-open*) 0.10 0.0) + :lift-translation '(0 0 0) + :2nd-lift-translation '(0 0 0)) + +;; ANGLED 45 degree for door handles towards pos x & pos y +(man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :door-angled + :grasp-translation `(,(- 0.025) -0.015 ,*handle-grasp-z-offset*) + :grasp-rot-matrix man-int:*-x-across-z-45-deg-grasp-rotation* + :pregrasp-offsets `(,(- *handle-pregrasp-x-offset-open*) -0.10 0.0) + :2nd-pregrasp-offsets `(,(- *handle-2nd-pregrasp-x-offset-open*) -0.10 0.0) :lift-translation '(0 0 0) :2nd-lift-translation '(0 0 0)) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 1d8bbc12f1..03be791b5a 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -46,6 +46,7 @@ (<- (man-int:object-type-direct-subtype :kitchen-item :milk)) (<- (man-int:object-type-direct-subtype :kitchen-item :cereal)) (<- (man-int:object-type-direct-subtype :kitchen-item :bowl)) + (<- (man-int:object-type-direct-subtype :kitchen-item :open-box)) (<- (man-int:object-type-direct-subtype :kitchen-item :pot)) (<- (man-int:object-type-direct-subtype :kitchen-item :spatula)) @@ -133,7 +134,7 @@ ))) (<- (orientation-matters ?object-type) - (member ?object-type (:knife :fork :spoon :cutlery :spatula)))) + (member ?object-type (:knife :fork :spoon :cutlery :spatula :open-box)))) ;;;;;;;;;;;;;;;;;;;;;;;;;;;; CUTLERY ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; @@ -493,14 +494,25 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;; jeroen-cup ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(defparameter *jeroen-cup-grasp-xy-offset* 0.02 "in meters") -(defparameter *jeroen-cup-grasp-z-offset* 0.0 "in meters") -(defparameter *jeroen-cup-pregrasp-xy-offset* 0.15 "in meters") +(defparameter *jeroen-cup-grasp-xy-offset* 0.04 "in meters") +(defparameter *jeroen-cup-grasp-z-offset* 0.01 "in meters") +(defparameter *jeroen-cup-pregrasp-xy-offset* 0.20 "in meters") +(defparameter *jeroen-cup-2nd-pregrasp-xy-offset* 0.10 "in meters") +(defparameter *jeroen-cup-3rd-pregrasp-xy-offset* 0.05 "in meters") +(defparameter *jeroen-cup-4th-pregrasp-xy-offset* 0.04 "in meters") +(defparameter *jeroen-cup-5th-pregrasp-xy-offset* 0.03 "in meters") +(defparameter *jeroen-cup-6th-pregrasp-xy-offset* 0.02 "in meters") +(defparameter *jeroen-cup-7th-pregrasp-xy-offset* 0.01 "in meters") (defparameter *jeroen-cup-pregrasp-z-offset* 0.0 "in meters") (defparameter *jeroen-cup-top-grasp-x-offset* 0.0"in meters") (defparameter *jeroen-cup-top-grasp-z-offset* 0.03 "in meters") -(defparameter *jeroen-cup-bottom-pregrasp-z-offset* 0.01 "in meters") -(defparameter *jeroen-cup-bottom-lift-z-offset* 0.01 "in meters") +(defparameter *jeroen-cup-bottom-grasp-z-offset* 0.03 "in meters") +(defparameter *jeroen-cup-bottom-pregrasp-z-offset* 0.05 "in meters") +(defparameter *jeroen-cup-bottom-lift-z-offset* 0.05 "in meters") +(defparameter *jeroen-cup-postgrasp-x-offset* 0.2 "in meters") +(defparameter *jeroen-cup-postgrasp-y-offset* 0.0 "in meters") +(defparameter *jeroen-cup-surface-lift-offset* 0.3 "in meters") +(defparameter *jeroen-cup-lift-z-offset* 0.03 "in meters") ;; TOP grasp (man-int:def-object-type-to-gripper-transforms :jeroen-cup '(:left :right) :top @@ -521,7 +533,7 @@ ;; BOTTOM grasp (man-int:def-object-type-to-gripper-transforms :jeroen-cup '(:left :right) :bottom - :grasp-translation `(0.0d0 0.0d0 ,(- *jeroen-cup-grasp-z-offset*)) + :grasp-translation `(0.0d0 0.0d0 ,(- *jeroen-cup-bottom-grasp-z-offset*)) :grasp-rot-matrix man-int:*-z-across-x-grasp-rotation* :pregrasp-offsets `(0.0 0.0 ,(- *jeroen-cup-bottom-pregrasp-z-offset*)) :2nd-pregrasp-offsets `(0.0 0 ,(- *jeroen-cup-bottom-pregrasp-z-offset*)) @@ -534,16 +546,16 @@ :grasp-rot-matrix man-int:*y-across-z-flipped-grasp-rotation* :pregrasp-offsets `(0.0 ,*jeroen-cup-pregrasp-xy-offset* ,*jeroen-cup-pregrasp-z-offset*) :2nd-pregrasp-offsets `(0.0 ,*jeroen-cup-pregrasp-xy-offset* 0.0) - :lift-translation *lift-offset* - :2nd-lift-translation *lift-offset*) + :lift-translation `(0.0 0.0 ,*jeroen-cup-lift-z-offset*) + :2nd-lift-translation `(0.0 0.0 ,*jeroen-cup-lift-z-offset*)) (man-int:def-object-type-to-gripper-transforms :jeroen-cup '(:left :right) :right-side :grasp-translation `(0.0d0 ,*jeroen-cup-grasp-xy-offset* ,*jeroen-cup-grasp-z-offset*) :grasp-rot-matrix man-int:*-y-across-z-flipped-grasp-rotation* :pregrasp-offsets `(0.0 ,(- *jeroen-cup-pregrasp-xy-offset*) ,*jeroen-cup-pregrasp-z-offset*) :2nd-pregrasp-offsets `(0.0 ,(- *jeroen-cup-pregrasp-xy-offset*) 0.0) - :lift-translation *lift-offset* - :2nd-lift-translation *lift-offset*) + :lift-translation `(0.0 0.0 ,*jeroen-cup-lift-z-offset*) + :2nd-lift-translation `(0.0 0.0 ,*jeroen-cup-lift-z-offset*)) ;; BACK grasp (man-int:def-object-type-to-gripper-transforms :jeroen-cup '(:left :right) :back @@ -551,8 +563,8 @@ :grasp-rot-matrix man-int:*-x-across-z-grasp-rotation-2* :pregrasp-offsets `(,(- *jeroen-cup-pregrasp-xy-offset*) 0.0 ,*jeroen-cup-pregrasp-z-offset*) :2nd-pregrasp-offsets `(,(- *jeroen-cup-pregrasp-xy-offset*) 0.0 0.0) - :lift-translation *lift-offset* - :2nd-lift-translation *lift-offset*) + :lift-translation `(0.0 0.0 ,*jeroen-cup-lift-z-offset*) + :2nd-lift-translation `(0.0 0.0 ,*jeroen-cup-lift-z-offset*)) ;; FRONT grasp (man-int:def-object-type-to-gripper-transforms :jeroen-cup '(:left :right) :front @@ -560,10 +572,109 @@ :grasp-rot-matrix man-int:*x-across-z-grasp-rotation* :pregrasp-offsets `(,*jeroen-cup-pregrasp-xy-offset* 0.0 ,*jeroen-cup-pregrasp-z-offset*) :2nd-pregrasp-offsets `(,*jeroen-cup-pregrasp-xy-offset* 0.0 0.0) - :lift-translation *lift-offset* - :2nd-lift-translation *lift-offset*) - + :lift-translation `(0.0 0.0 ,*jeroen-cup-surface-lift-offset*) + :2nd-lift-translation `(0.0 0.0 ,*jeroen-cup-surface-lift-offset*)) +;; FRONT grasp from and onto shelf, only pregrasp and postgrasp are different from the above. +;; pregrasp left hand +(defmethod man-int:get-object-type-to-gripper-pregrasp-transforms ((type (eql :jeroen-cup)) + object-name + (arm (eql :left)) + (grasp (eql :front)) + (location (eql :shelf)) + grasp-transform) + (mapcar (lambda (x-offset) + (make-arm-transform + object-name arm + x-offset 0.0 *jeroen-cup-pregrasp-z-offset* + man-int:*x-across-z-grasp-rotation*)) + (list *jeroen-cup-pregrasp-xy-offset* + *jeroen-cup-2nd-pregrasp-xy-offset* + ;; *jeroen-cup-3rd-pregrasp-xy-offset* + ;; *jeroen-cup-4th-pregrasp-xy-offset* + ;; *jeroen-cup-5th-pregrasp-xy-offset* + ;; *jeroen-cup-6th-pregrasp-xy-offset* + ;; *jeroen-cup-7th-pregrasp-xy-offset* + ))) +;; pregrasp right hand +(defmethod man-int:get-object-type-to-gripper-pregrasp-transforms ((type (eql :jeroen-cup)) + object-name + (arm (eql :right)) + (grasp (eql :front)) + (location (eql :shelf)) + grasp-transform) + (man-int:get-object-type-to-gripper-pregrasp-transforms + type object-name :left grasp location grasp-transform)) +;; postgrasp left hand +(defmethod man-int:get-object-type-wrt-base-frame-lift-transforms ((type (eql :jeroen-cup)) + (arm (eql :left)) + (grasp (eql :front)) + (location (eql :shelf))) + (list (make-base-transform + 0.0 + 0.0 + *jeroen-cup-lift-z-offset*) + (make-base-transform + (- *jeroen-cup-postgrasp-x-offset*) + *jeroen-cup-postgrasp-y-offset* + *jeroen-cup-lift-z-offset*))) +;; postgrasp right hand +(defmethod man-int:get-object-type-wrt-base-frame-lift-transforms ((type (eql :jeroen-cup)) + (arm (eql :right)) + (grasp (eql :front)) + (location (eql :shelf))) + (man-int:get-object-type-wrt-base-frame-lift-transforms type :left grasp location)) +;; ;; FRONT grasp from and onto table, use a different pregrasp +;; (defmethod man-int:get-object-type-to-gripper-pregrasp-transforms ((type (eql :jeroen-cup)) +;; object-name +;; (arm (eql :left)) +;; (grasp (eql :front)) +;; (location (eql :surface)) +;; grasp-transform) +;; (mapcar (lambda (x-offset) +;; (make-arm-transform +;; object-name arm +;; x-offset 0.0 *jeroen-cup-pregrasp-z-offset* +;; man-int:*x-across-z-grasp-rotation*)) +;; (list *jeroen-cup-pregrasp-xy-offset* +;; ;; *jeroen-cup-2nd-pregrasp-xy-offset* +;; ;; *jeroen-cup-3rd-pregrasp-xy-offset* +;; ;; *jeroen-cup-4th-pregrasp-xy-offset* +;; ;; *jeroen-cup-5th-pregrasp-xy-offset* +;; ;; *jeroen-cup-6th-pregrasp-xy-offset* +;; ;; *jeroen-cup-7th-pregrasp-xy-offset* +;; ))) +;; ;; pregrasp right hand +;; (defmethod man-int:get-object-type-to-gripper-pregrasp-transforms ((type (eql :jeroen-cup)) +;; object-name +;; (arm (eql :right)) +;; (grasp (eql :front)) +;; (location (eql :surface)) +;; grasp-transform) +;; (man-int:get-object-type-to-gripper-pregrasp-transforms +;; type object-name :left grasp location grasp-transform)) + + + + + + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;; OPEN-BOX ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + +(defparameter *open-box-top-grasp-x-offset* 0.0 "in meters") +(defparameter *open-box-top-grasp-y-offset* 0.245 "in meters") +(defparameter *open-box-top-grasp-z-offset* 0.05 "in meters") +(defparameter *open-box-lift-z-offset* 0.1 "in meters") +;; TOP grasp +(man-int:def-object-type-to-gripper-transforms :open-box '(:left :right) :top + :grasp-translation `(,(- *open-box-top-grasp-x-offset*) + ,(- *open-box-top-grasp-y-offset*) + ,*open-box-top-grasp-z-offset*) + :grasp-rot-matrix man-int:*z-across-x-grasp-rotation* + :pregrasp-offsets `(0.0 0.0 ,*open-box-lift-z-offset*) + :2nd-pregrasp-offsets `(0.0 0.0 ,*open-box-lift-z-offset*) + :lift-translation `(0.0 0.0 ,*open-box-lift-z-offset*) + :2nd-lift-translation `(0.0 0.0 ,*open-box-lift-z-offset*)) ;;;;;;;;;;;;;;;;;;;;;;;;;;;;; mug ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; @@ -711,6 +822,7 @@ (defparameter *milk-grasp-xy-offset* 0.01 "in meters") (defparameter *milk-grasp-z-offset* 0.03 "in meters") (defparameter *milk-pregrasp-xy-offset* 0.15 "in meters") +(defparameter *milk-2nd-pregrasp-xy-offset* 0.05 "in meters") (defparameter *milk-lift-z-offset* 0.15 "in meters") ;; BACK grasp @@ -718,7 +830,7 @@ :grasp-translation `(,*milk-grasp-xy-offset* 0.0d0 ,*milk-grasp-z-offset*) :grasp-rot-matrix man-int:*-x-across-z-grasp-rotation* :pregrasp-offsets `(,(- *milk-pregrasp-xy-offset*) 0.0 ,*milk-lift-z-offset*) - :2nd-pregrasp-offsets `(,(- *milk-pregrasp-xy-offset*) 0.0 0.0) + :2nd-pregrasp-offsets `(,(- *milk-2nd-pregrasp-xy-offset*) 0.0 0.0) :lift-translation `(0.0 0.0 ,*milk-lift-z-offset*) :2nd-lift-translation `(0.0 0.0 ,*milk-lift-z-offset*)) @@ -727,7 +839,7 @@ :grasp-translation `(,(- *milk-grasp-xy-offset*) 0.0d0 ,*milk-grasp-z-offset*) :grasp-rot-matrix man-int:*x-across-z-grasp-rotation* :pregrasp-offsets `(,*milk-pregrasp-xy-offset* 0.0 ,*milk-lift-z-offset*) - :2nd-pregrasp-offsets `(,*milk-pregrasp-xy-offset* 0.0 0.0) + :2nd-pregrasp-offsets `(,*milk-2nd-pregrasp-xy-offset* 0.0 0.0) :lift-translation `(0.0 0.0 ,*milk-lift-z-offset*) :2nd-lift-translation `(0.0 0.0 ,*milk-lift-z-offset*)) @@ -736,7 +848,7 @@ :grasp-translation `(0.0d0 ,(- *milk-grasp-xy-offset*) ,*milk-grasp-z-offset*) :grasp-rot-matrix man-int:*y-across-z-grasp-rotation* :pregrasp-offsets `(0.0 ,*milk-pregrasp-xy-offset* ,*milk-lift-z-offset*) - :2nd-pregrasp-offsets `(0.0 ,*milk-pregrasp-xy-offset* 0.0) + :2nd-pregrasp-offsets `(0.0 ,*milk-2nd-pregrasp-xy-offset* 0.0) :lift-translation `(0.0 0.0 ,*milk-lift-z-offset*) :2nd-lift-translation `(0.0 0.0 ,*milk-lift-z-offset*)) @@ -744,7 +856,7 @@ :grasp-translation `(0.0d0 ,*milk-grasp-xy-offset* ,*milk-grasp-z-offset*) :grasp-rot-matrix man-int:*-y-across-z-grasp-rotation* :pregrasp-offsets `(0.0 ,(- *milk-pregrasp-xy-offset*) ,*milk-lift-z-offset*) - :2nd-pregrasp-offsets `(0.0 ,(- *milk-pregrasp-xy-offset*) 0.0) + :2nd-pregrasp-offsets `(0.0 ,(- *milk-2nd-pregrasp-xy-offset*) 0.0) :lift-translation `(0.0 0.0 ,*milk-lift-z-offset*) :2nd-lift-translation `(0.0 0.0 ,*milk-lift-z-offset*)) @@ -838,7 +950,7 @@ (defparameter *bowl-grasp-x-offset* 0.08 "in meters") (defparameter *bowl-tilted-pregrasp-x-offset* 0.08 "in meters") ;; (defparameter *edeka-red-bowl-grasp-z-offset* 0.0 "in meters") -(defparameter *bowl-grasp-z-offset* 0.02 "in meters") +(defparameter *bowl-grasp-z-offset* 0.15 "in meters") (defparameter *bowl-tilted-grasp-z-offset* 0.04 "in meters") (defparameter *bowl-pregrasp-z-offset* 0.20 "in meters") @@ -1519,11 +1631,15 @@ (man-int:def-object-type-in-other-object-transform :jeroen-cup :drawer :jeroen-cup-in-dishwasher-1 - :attachment-translation `(-0.2 0.15 0.13) + :attachment-translation `(-0.2 -0.15 0.13) :attachment-rot-matrix man-int:*rotation-around-x-180-matrix*) (man-int:def-object-type-in-other-object-transform :jeroen-cup :drawer :jeroen-cup-in-dishwasher-2 - :attachment-translation `(-0.2 0.15 0.13) + :attachment-translation `(-0.2 -0.15 0.13) :attachment-rot-matrix man-int:*rotation-around-y-180-matrix*) +(man-int:def-object-type-in-other-object-transform :jeroen-cup :shelf + :jeroen-cup-on-shelf + :attachment-translation `(0.20 0.08 0.10) + :attachment-rot-matrix man-int:*rotation-around-x-180-matrix*) diff --git a/cram_demos/cram_object_knowledge/src/multiple-trajectory-poses.lisp b/cram_demos/cram_object_knowledge/src/multiple-trajectory-poses.lisp index 025be0fcbe..e9cb563022 100644 --- a/cram_demos/cram_object_knowledge/src/multiple-trajectory-poses.lisp +++ b/cram_demos/cram_object_knowledge/src/multiple-trajectory-poses.lisp @@ -29,6 +29,27 @@ (in-package :objects) +(defun make-arm-transform (object-name arm x y z &optional rot-matrix) + (cl-transforms-stamped:make-transform-stamped + (roslisp-utilities:rosify-underscores-lisp-name object-name) + (ecase arm + (:left cram-tf:*robot-left-tool-frame*) + (:right cram-tf:*robot-right-tool-frame*)) + 0.0 + (cl-transforms:make-3d-vector x y z) + (if rot-matrix + (cl-transforms:matrix->quaternion + (make-array '(3 3) :initial-contents rot-matrix)) + (cl-transforms:make-identity-rotation)))) + +(defun make-base-transform (x y z) + (cl-transforms-stamped:make-transform-stamped + cram-tf:*robot-base-frame* + cram-tf:*robot-base-frame* + 0.0 + (cl-transforms:make-3d-vector x y z) + (cl-transforms:make-identity-rotation))) + (defmethod man-int:get-action-gripping-effort :heuristics 20 ((type (eql :shoe))) 30) diff --git a/cram_demos/cram_object_knowledge/src/retail.lisp b/cram_demos/cram_object_knowledge/src/retail.lisp index 351f7bbcf2..ce33509819 100644 --- a/cram_demos/cram_object_knowledge/src/retail.lisp +++ b/cram_demos/cram_object_knowledge/src/retail.lisp @@ -30,29 +30,6 @@ (in-package :objects) - -(defun make-arm-transform (object-name arm x y z &optional rot-matrix) - (cl-transforms-stamped:make-transform-stamped - (roslisp-utilities:rosify-underscores-lisp-name object-name) - (ecase arm - (:left cram-tf:*robot-left-tool-frame*) - (:right cram-tf:*robot-right-tool-frame*)) - 0.0 - (cl-transforms:make-3d-vector x y z) - (if rot-matrix - (cl-transforms:matrix->quaternion - (make-array '(3 3) :initial-contents rot-matrix)) - (cl-transforms:make-identity-rotation)))) - -(defun make-base-transform (x y z) - (cl-transforms-stamped:make-transform-stamped - cram-tf:*robot-base-frame* - cram-tf:*robot-base-frame* - 0.0 - (cl-transforms:make-3d-vector x y z) - (cl-transforms:make-identity-rotation))) - - (defparameter *default-retail-z-offset* 0.05 "in meters") (defparameter *default-retail-lift-offsets* `(0.0 0.0 ,*default-retail-z-offset*)) diff --git a/cram_demos/cram_projection_demos/cram-projection-demos.asd b/cram_demos/cram_projection_demos/cram-projection-demos.asd index f0da2c4346..618f0a1b79 100644 --- a/cram_demos/cram_projection_demos/cram-projection-demos.asd +++ b/cram_demos/cram_projection_demos/cram-projection-demos.asd @@ -63,6 +63,8 @@ cram-fetch-deliver-plans ; for action desig implementations cram-urdf-environment-manipulation ; for action desig implementations + ;; cram-giskard ;; this is only here because the projection demo uses giskard services + ;; costmaps cram-btr-visibility-costmap cram-btr-spatial-relations-costmap @@ -94,4 +96,5 @@ "household-demo")) (:file "apartment-demo" :depends-on ("package" "utils" ;; for initializing - "household-demo")))))) + "household-demo")) + (:file "eurobin-demo" :depends-on ("package" "household-demo")))))) diff --git a/cram_demos/cram_projection_demos/launch/apartment_pr2.launch b/cram_demos/cram_projection_demos/launch/apartment_pr2.launch new file mode 100644 index 0000000000..6a55bab060 --- /dev/null +++ b/cram_demos/cram_projection_demos/launch/apartment_pr2.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/cram_demos/cram_projection_demos/launch/apartment_tiago.launch b/cram_demos/cram_projection_demos/launch/apartment_tiago.launch index a3bc6588a1..acf3454192 100644 --- a/cram_demos/cram_projection_demos/launch/apartment_tiago.launch +++ b/cram_demos/cram_projection_demos/launch/apartment_tiago.launch @@ -5,6 +5,6 @@ - + diff --git a/cram_demos/cram_projection_demos/launch/everything.launch b/cram_demos/cram_projection_demos/launch/everything.launch index 5260e01d7b..a4392492e6 100644 --- a/cram_demos/cram_projection_demos/launch/everything.launch +++ b/cram_demos/cram_projection_demos/launch/everything.launch @@ -93,7 +93,7 @@ + '$(find iai_apartment)/urdf/apartment_unreal_with_window4.xacro'"/> diff --git a/cram_demos/cram_projection_demos/package.xml b/cram_demos/cram_projection_demos/package.xml index 20a7b67307..09ff3ac9ae 100644 --- a/cram_demos/cram_projection_demos/package.xml +++ b/cram_demos/cram_projection_demos/package.xml @@ -60,13 +60,13 @@ pr2_description pr2_arm_kinematics - iai_boxy_description - iai_donbot_description + + - ur_description + iai_kitchen - iai_refills_lab + iai_maps diff --git a/cram_demos/cram_projection_demos/resource/household/open_box.obj b/cram_demos/cram_projection_demos/resource/household/open_box.obj new file mode 100644 index 0000000000..965e510550 --- /dev/null +++ b/cram_demos/cram_projection_demos/resource/household/open_box.obj @@ -0,0 +1,262 @@ +# Blender 3.5.0 +# www.blender.org +mtllib package.mtl +o Cube.001 +v 0.250000 -0.250000 -0.100000 +v 0.240000 -0.240000 -0.090000 +v -0.250000 -0.250000 -0.100000 +v -0.240000 -0.240000 -0.090000 +v 0.250000 0.250000 -0.100000 +v 0.240000 0.240000 -0.090000 +v -0.250000 0.250000 -0.100000 +v -0.240000 0.240000 -0.090000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 -0.7071 0.7071 +vn 0.7071 -0.0000 0.7071 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 0.7071 0.7071 +vn -0.7071 -0.0000 0.7071 +vt 0.375000 0.500000 +vt 0.375000 0.500000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.125000 0.500000 +vt 0.125000 0.500000 +vt 0.125000 0.750000 +vt 0.125000 0.750000 +s 0 +usemtl Material.001 +f 5/5/1 1/1/1 3/3/1 7/7/1 +f 4/4/2 3/3/2 1/1/2 2/2/2 +f 6/6/3 2/2/3 1/1/3 5/5/3 +f 6/6/4 8/8/4 4/4/4 2/2/4 +f 6/6/5 5/5/5 7/7/5 8/8/5 +f 7/7/6 3/3/6 4/4/6 8/8/6 +o Cube.002 +v -0.250000 -0.240000 0.100000 +v -0.240000 -0.240000 0.100000 +v -0.240000 -0.240000 -0.100000 +v -0.250000 -0.240000 -0.100000 +v -0.240000 -0.230400 -0.090000 +v -0.250000 0.240000 0.100000 +v -0.240000 0.240000 0.100000 +v -0.250000 0.240000 -0.100000 +v -0.240000 0.240000 -0.100000 +v -0.240000 0.230400 -0.090000 +v -0.240000 0.230400 0.100000 +v -0.240000 -0.230400 0.100000 +vn -0.0000 -0.0000 1.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 1.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 -1.0000 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vt 0.625000 0.750000 +vt 0.625000 0.745000 +vt 0.625000 0.750000 +vt 0.375000 0.745000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.387500 0.755000 +vt 0.875000 0.750000 +vt 0.625000 1.000000 +vt 0.625000 0.000000 +vt 0.875000 0.745000 +vt 0.625000 0.005000 +vt 0.625000 1.000000 +vt 0.375000 1.000000 +vt 0.375000 0.000000 +vt 0.125000 0.750000 +vt 0.375000 0.005000 +vt 0.125000 0.745000 +vt 0.375000 1.000000 +vt 0.375000 1.000000 +vt 0.387500 0.995000 +vt 0.870000 0.745000 +vt 0.612500 1.000000 +vt 0.625000 0.995000 +vt 0.630000 0.745000 +vt 0.612500 0.750000 +vt 0.625000 0.755000 +s 0 +usemtl Material.002 +f 15/20/7 14/17/7 9/9/7 20/34/7 19/31/7 +f 9/9/7 10/10/7 20/34/7 +f 12/14/8 9/9/8 14/18/8 16/23/8 +f 17/26/9 16/24/9 14/19/9 15/21/9 +f 11/12/10 12/14/10 16/25/10 17/27/10 +f 10/10/11 9/9/11 12/14/11 11/12/11 +f 19/32/12 20/35/12 13/15/12 18/29/12 +f 18/30/12 13/16/12 20/36/12 10/11/12 11/13/12 17/28/12 15/22/12 19/33/12 +o Cube.004 +v 0.250000 -0.250000 0.100000 +v 0.250000 -0.240000 0.100000 +v 0.250000 -0.240000 -0.100000 +v 0.250000 -0.250000 -0.100000 +v 0.240000 -0.240000 -0.090000 +v -0.250000 -0.250000 0.100000 +v -0.250000 -0.240000 0.100000 +v -0.250000 -0.250000 -0.100000 +v -0.250000 -0.240000 -0.100000 +v -0.240000 -0.240000 -0.090000 +v -0.240000 -0.240000 0.100000 +v 0.240000 -0.240000 0.100000 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 -1.0000 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn 1.0000 -0.0000 -0.0000 +vn -0.0000 1.0000 -0.0000 +vt 0.625000 0.750000 +vt 0.625000 0.745000 +vt 0.625000 0.750000 +vt 0.375000 0.745000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.387500 0.755000 +vt 0.875000 0.750000 +vt 0.625000 1.000000 +vt 0.625000 0.000000 +vt 0.875000 0.745000 +vt 0.625000 0.005000 +vt 0.625000 1.000000 +vt 0.375000 1.000000 +vt 0.375000 0.000000 +vt 0.125000 0.750000 +vt 0.375000 0.005000 +vt 0.125000 0.745000 +vt 0.375000 1.000000 +vt 0.375000 1.000000 +vt 0.387500 0.995000 +vt 0.870000 0.745000 +vt 0.612500 1.000000 +vt 0.625000 0.995000 +vt 0.630000 0.745000 +vt 0.612500 0.750000 +vt 0.625000 0.755000 +s 0 +usemtl Material.008 +f 27/48/13 26/45/13 21/37/13 32/62/13 31/59/13 +f 21/37/13 22/38/13 32/62/13 +f 24/42/14 21/37/14 26/46/14 28/51/14 +f 29/54/15 28/52/15 26/47/15 27/49/15 +f 23/40/16 24/42/16 28/53/16 29/55/16 +f 22/38/17 21/37/17 24/42/17 23/40/17 +f 31/60/18 32/63/18 25/43/18 30/57/18 +f 30/58/18 25/44/18 32/64/18 22/39/18 23/41/18 29/56/18 27/50/18 31/61/18 +o Cube.005 +v -0.250000 0.250000 0.100000 +v -0.250000 0.240000 0.100000 +v -0.250000 0.240000 -0.100000 +v -0.250000 0.250000 -0.100000 +v -0.240000 0.240000 -0.090000 +v 0.250000 0.250000 0.100000 +v 0.250000 0.240000 0.100000 +v 0.250000 0.250000 -0.100000 +v 0.250000 0.240000 -0.100000 +v 0.240000 0.240000 -0.090000 +v 0.240000 0.240000 0.100000 +v -0.240000 0.240000 0.100000 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 1.0000 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -1.0000 -0.0000 +vt 0.625000 0.750000 +vt 0.625000 0.745000 +vt 0.625000 0.750000 +vt 0.375000 0.745000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.387500 0.755000 +vt 0.875000 0.750000 +vt 0.625000 1.000000 +vt 0.625000 0.000000 +vt 0.875000 0.745000 +vt 0.625000 0.005000 +vt 0.625000 1.000000 +vt 0.375000 1.000000 +vt 0.375000 0.000000 +vt 0.125000 0.750000 +vt 0.375000 0.005000 +vt 0.125000 0.745000 +vt 0.375000 1.000000 +vt 0.375000 1.000000 +vt 0.387500 0.995000 +vt 0.870000 0.745000 +vt 0.612500 1.000000 +vt 0.625000 0.995000 +vt 0.630000 0.745000 +vt 0.612500 0.750000 +vt 0.625000 0.755000 +s 0 +usemtl Material.009 +f 39/76/19 38/73/19 33/65/19 44/90/19 43/87/19 +f 33/65/19 34/66/19 44/90/19 +f 36/70/20 33/65/20 38/74/20 40/79/20 +f 41/82/21 40/80/21 38/75/21 39/77/21 +f 35/68/22 36/70/22 40/81/22 41/83/22 +f 34/66/23 33/65/23 36/70/23 35/68/23 +f 43/88/24 44/91/24 37/71/24 42/85/24 +f 42/86/24 37/72/24 44/92/24 34/67/24 35/69/24 41/84/24 39/78/24 43/89/24 +o Cube.007 +v 0.250000 0.240000 0.100000 +v 0.240000 0.240000 0.100000 +v 0.240000 0.240000 -0.100000 +v 0.250000 0.240000 -0.100000 +v 0.240000 0.230400 -0.090000 +v 0.250000 -0.240000 0.100000 +v 0.240000 -0.240000 0.100000 +v 0.250000 -0.240000 -0.100000 +v 0.240000 -0.240000 -0.100000 +v 0.240000 -0.230400 -0.090000 +v 0.240000 -0.230400 0.100000 +v 0.240000 0.230400 0.100000 +vn -0.0000 -0.0000 1.0000 +vn 1.0000 -0.0000 -0.0000 +vn -0.0000 -1.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 1.0000 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vt 0.625000 0.750000 +vt 0.625000 0.745000 +vt 0.625000 0.750000 +vt 0.375000 0.745000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.375000 0.750000 +vt 0.387500 0.755000 +vt 0.875000 0.750000 +vt 0.625000 1.000000 +vt 0.625000 0.000000 +vt 0.875000 0.745000 +vt 0.625000 0.005000 +vt 0.625000 1.000000 +vt 0.375000 1.000000 +vt 0.375000 0.000000 +vt 0.125000 0.750000 +vt 0.375000 0.005000 +vt 0.125000 0.745000 +vt 0.375000 1.000000 +vt 0.375000 1.000000 +vt 0.387500 0.995000 +vt 0.870000 0.745000 +vt 0.612500 1.000000 +vt 0.625000 0.995000 +vt 0.630000 0.745000 +vt 0.612500 0.750000 +vt 0.625000 0.755000 +s 0 +usemtl Material.010 +f 51/104/25 50/101/25 45/93/25 56/118/25 55/115/25 +f 45/93/25 46/94/25 56/118/25 +f 48/98/26 45/93/26 50/102/26 52/107/26 +f 53/110/27 52/108/27 50/103/27 51/105/27 +f 47/96/28 48/98/28 52/109/28 53/111/28 +f 46/94/29 45/93/29 48/98/29 47/96/29 +f 55/116/30 56/119/30 49/99/30 54/113/30 +f 54/114/30 49/100/30 56/120/30 46/95/30 47/97/30 53/112/30 51/106/30 55/117/30 diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index a4b8d9d62c..c59e0d4536 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -32,10 +32,13 @@ (defparameter *apartment-object-spawning-poses* '((:jeroen-cup "cabinet1_coloksu_level4" - ((0.20 0.15 0.08) (0 1 0 0))) + ((0.20 0.05 0.08) (1 0 0 0))) (:cup "cabinet1_coloksu_level4" - ((0.15 -0.1 0.08) (0 0 -1 0))))) + ((0.15 -0.1 0.08) (0 0 -1 0))) + (:bowl + "island_countertop" + ((0.34 0.5 0.1) (0 0 0 -1))))) (defun initialize-apartment () (sb-ext:gc :full t) @@ -48,12 +51,14 @@ ;; (setf proj-reasoning::*projection-checks-enabled* t) (kill-and-detach-all) + + ;; (giskard:reset-collision-scene) (setf (btr:joint-state (btr:get-environment-object) "cabinet1_door_top_left_joint") 0.0 (btr:joint-state (btr:get-environment-object) "cabinet7_door_bottom_left_joint") - 0.0 + 0.025 (btr:joint-state (btr:get-environment-object) "dishwasher_drawer_middle_joint") 0.0) @@ -62,7 +67,7 @@ (setf desig::*designators* (tg:make-weak-hash-table :weakness :key)) - ;; (coe:clear-belief) + (coe:clear-belief) (btr:clear-costmap-vis-object)) @@ -76,143 +81,765 @@ (defun apartment-demo (&key (step 0)) - (urdf-proj:with-simulated-robot + ;;urdf-proj:with-simulated-robot + (setf proj-reasoning::*projection-checks-enabled* nil) + (setf btr:*visibility-threshold* 0.7) + + (let* ((?object + (an object + (type jeroen-cup) + (name jeroen-cup-1))) + (?location-in-cupboard + (a location + (on (an object + (type shelf) + (urdf-name cabinet1-coloksu-level4) + (part-of apartment) + (location (a location + (in (an object + (type cupboard) + (urdf-name cabinet1-door-top-left) + (part-of apartment))))))) + (side (back left)) + (range-invert 0.2) + (range 0.25) + (orientation upside-down) + (for ?object))) + (?location-in-cupboard-with-attachment + (a location + (on (an object + (type shelf) + (urdf-name cabinet1-coloksu-level4) + (part-of apartment) + (location (a location + (in (an object + (type cupboard) + (urdf-name cabinet1-door-top-left) + (part-of apartment))))))) + (side (back left)) + (range-invert 0.2) + (range 0.25) + (orientation upside-down) + (for ?object) + (attachments (jeroen-cup-on-shelf)))) + (?location-on-island + (a location + (on (an object + (type surface) + (urdf-name island-countertop) + (part-of apartment))) + (side back) + (range 0.4) + (range-invert 0.3) + (for ?object))) + (?location-in-dishwasher + (a location + (above (an object + (type drawer) + (urdf-name dishwasher-drawer-middle) + (part-of apartment) + (location (a location + (in (an object + (type dishwasher) + (urdf-name cabinet7) + (part-of apartment))))))) + (for (desig:an object + (type jeroen-cup) + (name jeroen-cup-1))) + (attachments (jeroen-cup-in-dishwasher-1 jeroen-cup-in-dishwasher-2)))) + ;; (?location-in-hand + ;; (a location + ;; (in (an object + ;; (type robot))))) + ;; (?location-in-other-hand + ;; (a location + ;; (in (an object + ;; (type robot))))) + ;; (?location-on-island-upside-down + ;; (a location + ;; (on (an object + ;; (type surface) + ;; (urdf-name island-countertop) + ;; (part-of apartment))) + ;; (side (right back)) + ;; (range 0.5) + ;; (orientation upside-down) + ;; (for (an object + ;; (type jeroen-cup) + ;; (name jeroen-cup-1))))) + + + ;; hard-coded stuff for real-world demo + (?initial-parking-pose + (cl-transforms-stamped:make-pose-stamped + cram-tf:*fixed-frame* + 0.0 + (cl-transforms:make-3d-vector 1.5 1.5 0.0) + (cl-transforms:make-quaternion 0 0 0.5 0.5))) + + (?accessing-cupboard-door-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.3861795354091224 2.0873920232286345 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 1 0))) + (?accessing-cupboard-door-another-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.23 2.0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 1 0))) + (?accessing-cupboard-door-robot-poses + (list ?accessing-cupboard-door-robot-pose + ?accessing-cupboard-door-another-robot-pose)) + (?detecting-cupboard-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.1 2.002821242371188d0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -1 0))) + ;; (?fetching-cupboard-right-hand-robot-pose + ;; (cl-transforms-stamped:make-pose-stamped + ;; "map" + ;; 0.0 + ;; (cl-transforms:make-3d-vector 1.3668892503154677d0 1.6951934636420103d0 0.0d0) + ;; (cl-transforms:make-quaternion 0.0d0 0.0d0 0.9298226390305793d0 + ;; -0.36800561314986846d0))) + (?sealing-cupboard-door-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.6 1.0873920171726708 0.0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -1 0))) + (?sealing-cupboard-door-another-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.7 1.0873920171726708 0.0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -1 0))) + (?sealing-cupboard-door-robot-poses + (list ?sealing-cupboard-door-robot-pose + ?sealing-cupboard-door-another-robot-pose)) + + (?on-counter-top-cup-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.36 2.6 1.0126) + (cl-transforms:make-quaternion 0 0 1 0))) + (?delivering-counter-top-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.8 2.8 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.1 1))) + (?fetching-counter-top-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.7642620086669922d0 2.8088844299316404d0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.145986869931221d0 0.9892865419387817d0))) + (?on-counter-top-cup-upsidedown-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.37 2.6 1.0126) + (cl-transforms:make-quaternion 0 1 0 0))) + (?on-counter-top-cup-look-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.1 2.6 1.0126) + (cl-transforms:make-quaternion 0 1 0 0))) + + (?accessing-dishwasher-door-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.7 3.0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 0 1))) + (?accessing-dishwasher-drawer-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.48 3.71 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 0.071 0.9975))) + (?sealing-dishwasher-drawer-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.419999599456787d0 3.619999408721924d0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 0.09318785493943212d0 0.9956485442623755d0))) + (?deliver-dishwasher-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.6 3.4182098388671873d0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 0.555830717086792d0 0.8312955498695374d0)))) + + ;; park robot into the initial position (when (<= step 0) (initialize-apartment) - (setf btr:*visibility-threshold* 0.7) + ;; (btr-belief:vary-kitchen-urdf '(("handle_cab1_top_door_joint" + ;; ((-0.038d0 -0.5d0 -0.08d0) + ;; (0.706825181105366d0 0.0d0 + ;; 0.0d0 0.7073882691671998d0))))) + ;; (setf btr:*current-bullet-world* (make-instance 'btr:bt-reasoning-world)) + ;; (btr-belief:spawn-world) + (when cram-projection:*projection-environment* (spawn-objects-on-fixed-spots - :object-types '(:jeroen-cup :cup) + :object-types '(:jeroen-cup :cup :bowl) :spawning-poses-relative *apartment-object-spawning-poses*)) - (park-robot (cl-transforms-stamped:make-pose-stamped - cram-tf:*fixed-frame* - 0.0 - (cl-transforms:make-3d-vector 1.5 1.5 0.0) - (cl-transforms:make-quaternion 0 0 1 0)))) - - (let* ((?object - (an object - (type jeroen-cup) - (name jeroen-cup-1))) - (?location-in-cupboard - (a location - (on (an object - (type shelf) - (urdf-name cabinet1-coloksu-level4) - (part-of apartment) - (location (a location - (in (an object - (type cupboard) - (urdf-name cabinet1-door-top-left) - (part-of apartment))))))) - (side (back left)) - (range-invert 0.2) - (range 0.25) - (orientation upside-down) - (for ?object))) - (?location-on-island - (a location - (on (an object - (type surface) - (urdf-name island-countertop) - (part-of apartment))) - (side back) - (range 0.4) - (range-invert 0.3) - (for ?object))) - (?location-in-dishwasher - (a location - (above (an object - (type drawer) - (urdf-name dishwasher-drawer-middle) - (part-of apartment) - (location (a location - (in (an object - (type dishwasher) - (urdf-name cabinet7) - (part-of apartment))))))) - (for (desig:an object - (type jeroen-cup) - ;; need a name because of the attachment - (name some-name))) - (attachments (jeroen-cup-in-dishwasher-1 jeroen-cup-in-dishwasher-2)))) - ;; (?location-in-hand - ;; (a location - ;; (in (an object - ;; (type robot) - ;; (name tiago-dual) - ;; (part-of tiago-dual))))) - ;; (?location-in-other-hand - ;; (a location - ;; (in (an object - ;; (type robot) - ;; (name tiago-dual) - ;; (part-of tiago-dual))))) - (?location-on-island-upside-down - (a location - (on (an object - (type surface) - (urdf-name island-countertop) - (part-of apartment))) - (side (right back)) - (range-invert 1.5) - (orientation upside-down) - (for (an object - (type jeroen-cup)))))) - - ;; bring cup from cupboard to table - (when (<= step 1) + (park-robot ?initial-parking-pose)) + + ;; bring cup from cupboard to table + (when (<= step 1) + (let ((?goal `(and (cpoe:object-at-location ,(an object + (type jeroen-cup) + (name jeroen-cup-1)) + ,(a location + (pose ?on-counter-top-cup-pose))) + (cpoe:location-reset ,?location-in-cupboard)))) (exe:perform (an action (type transporting) (object (an object (type jeroen-cup) + (color blue) + (name jeroen-cup-1) (location ?location-in-cupboard))) - (target ?location-on-island)))) + (access-search-outer-robot-location (a location + (poses ?accessing-cupboard-door-robot-poses))) + (access-seal-search-outer-arms (left)) + (access-search-outer-grasps (back)) + (search-robot-location (a location + (pose ?detecting-cupboard-robot-pose + ;; ?fetching-cupboard-right-hand-robot-pose + ))) + (fetch-robot-location (a location + (pose ?detecting-cupboard-robot-pose + ;; ?fetching-cupboard-right-hand-robot-pose + ))) + (arms (left + ;; right + )) + (grasps (front)) + (target (a location + (pose ?on-counter-top-cup-pose)) + ;; ?location-on-island + ) + (deliver-robot-location (a location + (pose ?delivering-counter-top-robot-pose))) + (seal-search-outer-robot-location (a location + (poses ?sealing-cupboard-door-robot-poses))) + (seal-search-outer-grasps (back)) + (goal ?goal))))) - ;; put cup from island into dishwasher - (when (<= step 2) - (exe:perform - (an action - (type transporting) - (object (an object + ;; put cup from island into dishwasher + (when nil ; (<= step 2) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (color blue) + (name jeroen-cup-1) + (location ?location-on-island))) + (target ?location-in-dishwasher) + + (access-deliver-robot-location (a location + (pose ?accessing-dishwasher-drawer-robot-pose))) + (seal-deliver-robot-location (a location + (pose ?sealing-dishwasher-drawer-robot-pose))) + (access-seal-deliver-arms (left)) + (access-seal-deliver-grasps (back)) + + (access-deliver-outer-robot-location (a location + (pose ?accessing-dishwasher-door-robot-pose))) + (seal-deliver-outer-robot-location (a location + (pose ?accessing-dishwasher-door-robot-pose))) + (access-seal-deliver-outer-arms (left)) + (access-seal-deliver-outer-grasps (back)) + + (search-robot-location (a location + (pose ?fetching-counter-top-robot-pose))) + (fetch-robot-location (a location + (pose ?fetching-counter-top-robot-pose))) + (deliver-robot-location (a location + (pose ?deliver-dishwasher-robot-pose))) + (arms (right)) + (grasps (front))))) + + ;; put cup from dishwasher onto table upside-down + (when nil ; (<= step 3) + ;; let ((?goal `(cpoe:object-at-location ,?object ,?location-in-hand))) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (color blue) + (name jeroen-cup-1) + (location ?location-in-dishwasher))) + (grasps (bottom)) + (arms (right)) + + (deliver-robot-location (a location + (pose ?delivering-counter-top-robot-pose))) + ;; (target ?location-on-island-upside-down) + (target (a location + (pose ?on-counter-top-cup-upsidedown-pose))) + + (access-search-robot-location (a location + (pose ?accessing-dishwasher-drawer-robot-pose))) + (seal-search-robot-location (a location + (pose ?sealing-dishwasher-drawer-robot-pose))) + (access-seal-search-arms (left)) + (access-seal-search-grasps (back)) + + (access-search-outer-robot-location (a location + (pose ?accessing-dishwasher-door-robot-pose))) + (seal-search-outer-robot-location (a location + (pose ?accessing-dishwasher-door-robot-pose))) + (access-seal-search-outer-arms (left)) + (access-search-outer-grasps (back)) + (seal-search-outer-grasps (back))))) + + ;; bring cup to cupboard + (when nil ; (<= step 4) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (name jeroen-cup-1) + (color blue) + ;; (location ?location-on-island-upside-down) + (location (a location + (pose ;; ?on-counter-top-cup-upsidedown-pose + ?on-counter-top-cup-look-pose))))) + + (access-deliver-outer-robot-location (a location + (poses ?accessing-cupboard-door-robot-poses))) + (seal-deliver-outer-robot-location (a location + (poses ?sealing-cupboard-door-robot-poses))) + (access-seal-deliver-outer-arms (left)) + (access-seal-deliver-outer-grasps (back)) + + (search-robot-location (a location + (poses (?delivering-counter-top-robot-pose + ?fetching-counter-top-robot-pose)))) + (fetch-robot-location (a location + (poses (?delivering-counter-top-robot-pose + ?fetching-counter-top-robot-pose)))) + (arms (left)) + (grasps (front)) + + (target ?location-in-cupboard-with-attachment) + (deliver-robot-location (a location + (pose ?detecting-cupboard-robot-pose + ;; ?initial-parking-pose + ))))))) + + ;; (finalize) + ) + + + +(defun apartment-demo-pouring-only (&key (step 0)) + "copy and pasted stuff from apartment-demo above so i can later just add the pouring part in the real demo" + ;;(urdf-proj:with-simulated-robot + (setf proj-reasoning::*projection-checks-enabled* t) + (setf btr:*visibility-threshold* 0.7) + (let* ((?source-object + (an object + (type jeroen-cup) + (name jeroen-cup-1))) + + (?location-on-island + (a location + (on (an object + (type surface) + (urdf-name island-countertop) + (part-of apartment))) + (side back) + (range 0.4) + (range-invert 0.3) + (for ?source-object))) + + (?target-object + (an object + (type bowl) + (name bowl-1))) + + (?location-on-island-target + (a location + (on (an object + (type surface) + (urdf-name island-countertop) + (part-of apartment))) + (side back) + (range 0.4) + (range-invert 0.3) + (for ?target-object))) + + ;; hard-coded stuff for real-world demo + (?initial-parking-pose + (cl-transforms-stamped:make-pose-stamped + cram-tf:*fixed-frame* + 0.0 + (cl-transforms:make-3d-vector 1.6 3.3 0.0) + (cl-transforms:make-quaternion 0 0 0 1))) + + (?second-cup-park-pose + (cl-transforms-stamped:make-pose-stamped + cram-tf:*fixed-frame* + 0.0 + (cl-transforms:make-3d-vector 1.6 2.4 0.0) + (cl-transforms:make-quaternion 0 0 0 1))) + + (?third-cup-park-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.6867786693573 2.1811975717544556 0) + (cl-transforms:make-quaternion 0 0 0 1))) + + (?on-counter-top-source-cup-look-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2 3.0 1.0126) + (cl-transforms:make-quaternion 0 1 0 0))) + + (?on-counter-top-target-cup-look-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.49 2.2 1.0126) + (cl-transforms:make-quaternion 0 1 0 0)))) + + (when (<= step 0) + (initialize-apartment) + ;; (btr-belief:vary-kitchen-urdf '(("handle_cab1_top_door_joint" + ;; ((-0.038d0 -0.5d0 -0.08d0) + ;; (0.706825181105366d0 0.0d0 + ;; 0.0d0 0.7073882691671998d0))))) + ;; (setf btr:*current-bullet-world* (make-instance 'btr:bt-reasoning-world)) + ;; (btr-belief:spawn-world) + + (when cram-projection:*projection-environment* + (spawn-objects-on-fixed-spots + :object-types '(:jeroen-cup :bowl) + :spawning-poses-relative *apartment-object-spawning-poses*)) + (park-robot ?initial-parking-pose)) + + ;; get all the object designators + ;;we pour from red to blue so red=source blue=target + (when (<= step 1) + (park-robot ?initial-parking-pose) + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?initial-parking-pose))))) + + (exe:perform (desig:a motion + (type looking) + (pose ?on-counter-top-source-cup-look-pose))) + + (let* ((?source-object-desig + (desig:an object (type jeroen-cup) + (color blue) (name jeroen-cup-1) (location ?location-on-island))) - (target ?location-in-dishwasher)))) - ;; put cup from dishwasher onto table upside-down - (when (<= step 3) - ;; let ((?goal `(cpoe:object-at-location ,?object ,?location-in-hand))) + (?source-perceived-object-desig + (exe:perform (desig:an action + (type detecting) + (object ?source-object-desig))))) + + ;;TODO: Pick-up has parking in the end this need to be changed + ;;or with constraints + (cpl:with-retry-counters ((giskardside-retries 3)) + (cpl:with-failure-handling + (((or common-fail:manipulation-low-level-failure + common-fail:manipulation-goal-not-reached + common-fail:gripper-closed-completely) (e) + (roslisp:ros-warn (pp-plans pour-reach) + "Manipulation messed up: ~a~%Failing." + e) + + (cpl:do-retry giskardside-retries + (cpl:retry)) + (return))) + + (exe:perform (desig:an action + (type picking-up) + (arm :right) + (grasp :front) + (object ?source-perceived-object-desig) + (park-arms NIL))))))) + + ;; (exe:perform (desig:an action + ;; (type fetching) + ;; (arm :left) + ;; (grasp :front) + ;; (object ?source-perceived-object-desig) + ;; )) + ;;) + ;;) + (when (<= step 2) + (exe:perform + (desig:an action + (type positioning-arm) + (left-configuration park) + (right-configuration park))) + + ;; (exe:perform + ;; (desig:an action + ;; (type going) + ;; (target (desig:a location + ;; (pose ?second-cup-park-pose))))) + + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?third-cup-park-pose)))))) + + (when (<= step 3) + (exe:perform (desig:an action + (type moving-torso) + (joint-angle lower-limit))) + + (exe:perform (desig:a motion + (type looking) + (pose ?on-counter-top-target-cup-look-pose))) + + (let* ((?target-object-desig + (desig:an object + (type bowl) + (color red) + (name bowl-1) + (location ?location-on-island-target))) + + (?target-perceived-object-desig + (exe:perform (desig:an action + (type detecting) + (object ?target-object-desig))))) + (exe:perform (desig:an action + (type moving-torso) + (joint-angle upper-limit))) + (exe:perform - (an action - (type transporting) - (object (an object + (desig:an action + (type pouring) + ;; (object ?source-perceived-object-desig) + (on-object ?target-perceived-object-desig) + (arm :right) + (sides (:top-right)) + (wait-duration 5))))))) + +;; +;; (when (<= step 2) + +;; (?target-object-desig +;; (desig:an object +;; (type jeroen-cup) +;; (color blue) +;; (name jeroen-cup-1) +;; (location ?location-on-island))) +;; (?target-perceived-object-desig +;; (exe:perform (desig:an action +;; (type detecting) +;; (object ?target-object-desig))))) +;; (exe:perform (desig:a motion +;; (type looking) +;; (pose ?on-counter-top-bowl-look-pose))) + + +;; (let* ((?target-object-desig +;; (desig:an object +;; (type jeroen-cup) +;; (color blue) +;; (name jeroen-cup-1) +;; (location ?location-on-island))) +;; (?target-perceived-object-desig +;; (exe:perform (desig:an action +;; (type detecting) +;; (object ?target-object-desig))))) +;; ?perceived-object-desig + + + +(defun apartment-demo-merged (&key (step 0)) + ;;urdf-proj:with-simulated-robot + (setf proj-reasoning::*projection-checks-enabled* nil) + (setf btr:*visibility-threshold* 0.7) + + (let* ((?on-counter-top-cup-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.37 2.6 1.0126) + (cl-transforms:make-quaternion 0 0 1 0))) + + ;;hardcoded pouring stuff + + (?source-object + (an object + (type jeroen-cup) + (name jeroen-cup-1))) + + (?location-on-island + (a location + (on (an object + (type surface) + (urdf-name island-countertop) + (part-of apartment))) + (side back) + (range 0.4) + (range-invert 0.3) + (for ?source-object))) + + (?target-object + (an object + (type bowl) + (name bowl-1))) + + (?location-on-island-target + (a location + (on (an object + (type surface) + (urdf-name island-countertop) + (part-of apartment))) + (side back) + (range 0.4) + (range-invert 0.3) + (for ?target-object))) + + ;; hard-coded stuff for real-world demo + (?percieve-blue-cup-pose-pouring + (cl-transforms-stamped:make-pose-stamped + cram-tf:*fixed-frame* + 0.0 + (cl-transforms:make-3d-vector 1.6 2.5 0.0) + (cl-transforms:make-quaternion 0 0 0 1))) + + (?on-counter-top-source-cup-look-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.0 2.6 1.0126) + (cl-transforms:make-quaternion 0 1 0 0))) + + (?on-counter-top-target-cup-look-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.0 2.2 1.0126) + (cl-transforms:make-quaternion 0 1 0 0))) + (?third-cup-park-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.6867786693573 2.1811975717544556 0) + (cl-transforms:make-quaternion 0 0 0 1)))) + + (apartment-demo :step step) + + ;;pick-up-cup + (when (<= step 2) + + (exe:perform + (desig:an action + (type navigating) + (location (desig:a location + (pose ?percieve-blue-cup-pose-pouring))))) + + (let* ((?source-object-desig + (desig:an object (type jeroen-cup) + (color blue) (name jeroen-cup-1) - (location ?location-in-dishwasher))) - (target ?location-on-island-upside-down)))) - - ;; regrasp cup - ;; (when (<= step 4) - ;; (exe:perform - ;; (an action - ;; (type transporting) - ;; (object (an object - ;; (type jeroen-cup) - ;; (name jeroen-cup-1) - ;; (location ?location-in-hand))) - ;; (target ?location-in-other-hand)))) - - ;; bring cup to cupboard - (when (<= step 5) + (location ?location-on-island))) + + (?perceive-source-goal + `(cpoe:object-in-hand ,(an object + (type jeroen-cup) + (name jeroen-cup-1)))) + (?source-perceived-object-desig + (cpl:seq + (exe:perform + (desig:an action + (type looking) + (target (desig:a location + (pose ?on-counter-top-source-cup-look-pose))) + (goal ?perceive-source-goal))) + (exe:perform + (desig:an action + (type detecting) + (object ?source-object-desig) + (goal ?perceive-source-goal))))) + + (?source-grasped-object-desig + (let ((?goal `(cpoe:object-in-hand ,(an object + (type jeroen-cup) + (name jeroen-cup-1))))) + (exe:perform + (desig:an action + (type fetching) + (arms (right)) + (grasps (front)) + (object ?source-perceived-object-desig) + (robot-location (desig:a location + (pose ?percieve-blue-cup-pose-pouring))) + (look-location (desig:a location + (pose ?on-counter-top-source-cup-look-pose))) + (goal ?goal)))))))) + (when (<= step 3) (exe:perform - (an action - (type transporting) - (object (an object - (type jeroen-cup) - (name jeroen-cup-1) - (location ?location-on-island-upside-down))) - (target ?location-in-cupboard)))))) + (desig:an action + (type navigating) + (location (desig:a location + (pose ?third-cup-park-pose))))) + + (let* ((?target-object-desig + (desig:an object + (type bowl) + (color red) + (name bowl-1) + (location ?location-on-island-target))) + + (?target-perceived-object-desig + (cpl:seq + (exe:perform + (desig:an action + (type looking) + (target (desig:a location + (pose ?on-counter-top-target-cup-look-pose))))) + (exe:perform + (desig:an action + (type detecting) + (object ?target-object-desig)))))) + + (exe:perform + (desig:an action + (type pouring) + (on-object ?target-perceived-object-desig) + (arm :right) + (sides (:top-right)) + (wait-duration 5)))) + + (exe:perform + (desig:an action + (type placing) + (arm right) + (object ?source-object) + (target (a location + (pose ?on-counter-top-cup-pose))))) + ))) - (finalize)) diff --git a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp new file mode 100644 index 0000000000..7e9045112f --- /dev/null +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -0,0 +1,242 @@ +;;; +;;; Copyright (c) 2023, Arthur Niedzwiecki +;;; 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 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 +;;; 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. + +(in-package :demos) + +(defun initialize-eurobin () + (sb-ext:gc :full t) + ;; (setf proj-reasoning::*projection-checks-enabled* t) + (kill-and-detach-all) + (setf (btr:joint-state (btr:get-environment-object) + "window4_right_joint") + 0.0) + (btr-belief::publish-environment-joint-state + (btr:joint-states (btr:get-environment-object))) + (setf desig::*designators* (tg:make-weak-hash-table :weakness :key)) + (coe:clear-belief) + (btr:clear-costmap-vis-object)) + +(defun eurobin-demo (&key (step 0)) + ;;urdf-proj:with-simulated-robot + (setf proj-reasoning::*projection-checks-enabled* nil) + (setf btr:*visibility-threshold* 0.7) + + (let* (;; Initialize desigs for objects and locations + (?accessing-window-base-pose + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 9.7d0 4.3d0 0.0d0) + (cl-tf:euler->quaternion :az (* pi 0.75)))) + (?accessing-window-base-pose-2 + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 9.5d0 4.3d0 0.0d0) + (cl-tf:euler->quaternion :az (* pi 0.25)))) + (?accessing-window-base-front-pose + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 9.3d0 4.2d0 0.0d0) + (cl-tf:euler->quaternion :az (* pi 0.5)))) + (?picking-up-package-base-pose + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 9.5d0 4.6d0 0.0d0) + (cl-tf:euler->quaternion :az (* pi 0.5)))) + (?placing-package-base-pose + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 15.4d0 2.0d0 0.0d0) + (cl-tf:euler->quaternion :az (* pi 0.0)))) + (?box-delivery-pose + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 16.05d0 1.87d0 0.56d0) + (cl-tf:euler->quaternion :az (* pi 0.0)))) + (?item-delivery-base-pose + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 15.5d0 3.0d0 0.0d0) + (cl-tf:euler->quaternion :az (* pi 0.0)))) + (?item-delivery-pose + (cl-transforms-stamped:make-pose-stamped + "map" (roslisp:ros-time) + (cl-tf:make-3d-vector 16.2d0 3.0d0 0.48d0) + (cl-tf:euler->quaternion :az (* pi 0.0))))) + + ;;; --- + ;;; Step 0: Init apartment, reset joints, desigs and objects + (when (<= step 0) + (initialize-eurobin)) + + ;;; --- + ;;; Go and open the door + (when (<= step 1) + (park-robot ?accessing-window-base-front-pose) + ;; (exe:perform + ;; (desig:an action + ;; (type going) + ;; (target (desig:a location + ;; (pose ?accessing-window-base-pose-2))))) + ) + (when (<= step 2) + (exe:perform + (desig:an action + (type opening) + (arm right) + (distance 1.55) + (grasps (door-angled)) + ;; (link window4-left-handle) + (object (desig:an object + (type cupboard) + (urdf-name window4-right) + (part-of apartment)))))) + + + ;; (when (<= step 3) + ;; (exe:perform + ;; (desig:an action + ;; (type going) + ;; (target (desig:a location + ;; (pose ?accessing-window-base-pose-2)))))) + + ;; (when (<= step 4) + ;; (exe:perform + ;; (desig:an action + ;; (type opening) + ;; (arm right) + ;; (distance 1.2) + ;; (grasps (door-left)) + ;; (object (desig:an object + ;; (type cupboard) + ;; (urdf-name window4-right) + ;; (part-of apartment)))))) + + (unless (btr:object btr:*current-bullet-world* :package-stand) + (btr:add-object btr:*current-bullet-world* + :cylinder + :package-stand + '((9.95 5.40 0.35)(0 0 0 1)) + :size '(0.3 0.3 0.7) + :mass 1.0)) + (btr-utils:spawn-object :open-box :open-box :pose '((9.95 5.40 0.8) (0 0 0 1))) + + ;;; --- + ;;; Take the package and carry it to the table + (when (<= step 5) + ;; go to open door + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?picking-up-package-base-pose))))) + ;; look at package + (let ((?door-poi + (cl-tf:make-pose-stamped + "base_footprint" 0.0 + (cl-tf:make-3d-vector 1.0 -0.5 0.5) + (cl-tf:make-identity-rotation)))) + (exe:perform + (desig:an action (type looking) + (target (desig:a location + (pose ?door-poi)))))) + ;; perceive package + (let ((?package-desig (exe:perform + (desig:an action + (type detecting) + (object (desig:an object + (type open-box))))))) + ;; pick-up the package + (exe:perform + (desig:an action + (type picking-up) + (object ?package-desig) + (park-arms nil)))) + + ;; going to placing location + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?placing-package-base-pose))))) + + ;; place box + (exe:perform + (desig:an action + (type placing) + (target (desig:a location + (pose ?box-delivery-pose)))))) + + ;;; --- + ;;; Pick items out of the package and place + (when (<= step 3) + (btr-utils:spawn-object :jeroen-cup :jeroen-cup :pose '((16.05d0 1.87d0 0.56d0)(0 0 0 1))) + + (let ((?package-desig (exe:perform + (desig:an action + (type detecting) + (object (desig:an object + (type jeroen-cup))))))) + ;; pick-up the package + (exe:perform + (desig:an action + (type picking-up) + (object ?package-desig) + (grasp top)))) + + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?item-delivery-base-pose))))) + + (exe:perform + (desig:an action + (type placing) + (target (desig:a location + (pose ?item-delivery-pose))))))) + (finalize)) + + +;; visualize the gripper frames for opening a container +;; (let ((door-traj +;; (urdf-proj:with-simulated-robot +;; (man-int:get-action-trajectory :opening :left :door-left NIL +;; (list (desig:AN OBJECT +;; (TYPE CUPBOARD) +;; (URDF-NAME window4-right) +;; (PART-OF APARTMENT))) +;; :opening-distance 0.1)))) +;; (mapcan (lambda (pose) +;; (btr:add-vis-axis-object pose) +;; (sleep 0.5)) +;; (append ;; (man-int:get-traj-poses-by-label door-traj :reaching) +;; (man-int:get-traj-poses-by-label door-traj :grasping) +;; ;; (man-int:get-traj-poses-by-label door-traj :opening) +;; ;; (man-int:get-traj-poses-by-label door-traj :retracting) +;; ))) diff --git a/cram_demos/cram_projection_demos/src/package.lisp b/cram_demos/cram_projection_demos/src/package.lisp index 849ed8b14f..1aa7d45906 100644 --- a/cram_demos/cram_projection_demos/src/package.lisp +++ b/cram_demos/cram_projection_demos/src/package.lisp @@ -38,4 +38,6 @@ ;; assembly-demo #:assembly-demo ;; retail-demo - #:retail-demo)) + #:retail-demo + ;; eurobin-demo + #:eurobin-demo)) diff --git a/cram_demos/cram_projection_demos/src/retail-demo.lisp b/cram_demos/cram_projection_demos/src/retail-demo.lisp index d413258076..b13ec8e4f9 100644 --- a/cram_demos/cram_projection_demos/src/retail-demo.lisp +++ b/cram_demos/cram_projection_demos/src/retail-demo.lisp @@ -493,7 +493,8 @@ (cpl:with-failure-handling ((cpl:simple-plan-failure (e) - (declare (ignore e)) + (roslisp:ros-warn (demos retail) "Putting tabs onto Donbot failed: ~a~%~ + Ignoring." e) (return))) (exe:perform (desig:an action @@ -504,7 +505,8 @@ ))) (cpl:with-failure-handling ((cpl:simple-plan-failure (e) - (declare (ignore e)) + (roslisp:ros-warn (demos retail) "Transporting balea bottle failed: ~a~%~ + Ignoring." e) (return))) (exe:perform (desig:an action @@ -515,7 +517,8 @@ ))) (cpl:with-failure-handling ((cpl:simple-plan-failure (e) - (declare (ignore e)) + (roslisp:ros-warn (demos retail) "Putting tabs onto shelf failed: ~a~%~ + Ignoring." e) (return))) (exe:perform (desig:an action @@ -561,6 +564,33 @@ :r-g-b-list color :mesh-path mesh-path))))) +(defmethod cram-occasions-events:on-event + publish-object 3 ((event cram-plan-occasions-events:object-perceived-event)) + + (let ((items (remove-if-not (lambda (object) + (typep object 'btr:item)) + (btr:objects btr:*current-bullet-world*)))) + (dolist (item items) + (let* ((name + (btr:name item)) + (ros-name + (roslisp-utilities:rosify-underscores-lisp-name name)) + (pose + (btr:pose item)) + (mesh-path + (second (assoc (car (btr:item-types item)) btr::*mesh-files*))) + (color + (cl-bullet-vis:collision-shape-color + (cl-bullet:collision-shape + (btr:rigid-body item name))))) + (cram-tf:visualize-marker pose + ;; :topic "cram_items" + :namespace ros-name + :marker-type :mesh_resource + :scale-list '(1 1 1) + :r-g-b-list color + :mesh-path mesh-path))))) + diff --git a/cram_demos/cram_projection_demos/src/setup.lisp b/cram_demos/cram_projection_demos/src/setup.lisp index 917735f345..50138891ee 100644 --- a/cram_demos/cram_projection_demos/src/setup.lisp +++ b/cram_demos/cram_projection_demos/src/setup.lisp @@ -43,12 +43,12 @@ (btr:clear-costmap-vis-object) - (btr:add-objects-to-mesh-list "assembly_models" - :directory "fixtures" - :extension "stl") - (btr:add-objects-to-mesh-list "assembly_models" - :directory "battat/convention" - :extension "stl") + ;; (btr:add-objects-to-mesh-list "assembly_models" + ;; :directory "fixtures" + ;; :extension "stl") + ;; (btr:add-objects-to-mesh-list "assembly_models" + ;; :directory "battat/convention" + ;; :extension "stl") (btr:add-objects-to-mesh-list "cram_bullet_reasoning") (btr:add-objects-to-mesh-list "cram_projection_demos" :directory "resource/household") diff --git a/cram_pr2/cram_pr2_low_level/CATKIN_IGNORE b/cram_donbot/CATKIN_IGNORE similarity index 100% rename from cram_pr2/cram_pr2_low_level/CATKIN_IGNORE rename to cram_donbot/CATKIN_IGNORE diff --git a/cram_external_interfaces/cram_giskard/src/action-client.lisp b/cram_external_interfaces/cram_giskard/src/action-client.lisp index a4e974268d..5123e17682 100644 --- a/cram_external_interfaces/cram_giskard/src/action-client.lisp +++ b/cram_external_interfaces/cram_giskard/src/action-client.lisp @@ -65,8 +65,9 @@ (roslisp:ros-warn (giskard action-client) "Giskard action timed out.")) (:aborted - (roslisp:ros-warn (giskard cartesian) - "Giskard action aborted.~%Result: ~a" result))) + (roslisp:ros-warn (giskard action-client) + ;; "Giskard action aborted.~%Result: ~a" result + "Giskard action aborted."))) (when (and result (member (roslisp:symbol-code diff --git a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index cb3b111e42..64340ce9bd 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -46,89 +46,122 @@ collision-object-b collision-object-b-link collision-object-a - prefer-base allow-base + prefer-base allow-base straight-line align-planes-left align-planes-right - unmovable-joints) + unmovable-joints + precise-tracking) (declare (type (or null cl-transforms-stamped:pose-stamped) left-pose right-pose) (type (or null string) pose-base-frame) - (type boolean prefer-base align-planes-left align-planes-right) + (type boolean prefer-base straight-line + align-planes-left align-planes-right precise-tracking) (type (or null list) unmovable-joints)) (let ((arms (append (when left-pose '(:left)) (when right-pose '(:right))))) (make-giskard-goal :constraints (list - (make-avoid-joint-limits-constraint) + (make-avoid-joint-limits-constraint + :joint-list (append (when left-pose + (cut:var-value + '?joints + (car + (prolog:prolog + `(and (rob-int:robot ?robot-name) + (rob-int:arm-joints + ?robot-name :left ?joints)))))) + (when right-pose + (cut:var-value + '?joints + (car + (prolog:prolog + `(and (rob-int:robot ?robot-name) + (rob-int:arm-joints + ?robot-name :right ?joints)))))))) (when allow-base - (make-prefer-base-constraint - :base-weight (if prefer-base - *prefer-base-low-cost* - *allow-base-high-cost*))) - ;; ;; align planes is WIP in giskard - ;; (when align-planes-left - ;; (make-align-planes-constraint - ;; pose-base-frame - ;; "refills_finger" - ;; (cl-transforms-stamped:make-vector-stamped - ;; cram-tf:*robot-base-frame* 0.0 - ;; (cl-transforms:make-3d-vector 0 0 1)) - ;; (cl-transforms-stamped:make-vector-stamped - ;; cram-tf:*robot-base-frame* 0.0 - ;; (cl-transforms:make-3d-vector 0 0 1)))) - ;; (when align-planes-right - ;; (make-align-planes-constraint - ;; pose-base-frame - ;; "refills_finger" - ;; (cl-transforms-stamped:make-vector-stamped - ;; cram-tf:*robot-base-frame* 0.0 - ;; (cl-transforms:make-3d-vector 0 0 1)) - ;; (cl-transforms-stamped:make-vector-stamped - ;; cram-tf:*robot-base-frame* 0.0 - ;; (cl-transforms:make-3d-vector 0 0 1)))) + ;; PREFER BASE is implemented in a very hacky way. + ;; For now, disabling it, so the robot will always use the + ;; the arm and won't help the arm with the base much. + ;; (make-prefer-base-constraint + ;; :base-weight (if prefer-base + ;; *prefer-base-low-cost* + ;; *allow-base-high-cost*)) + ) + (when align-planes-left + (make-align-planes-constraint + pose-base-frame + cram-tf:*robot-left-tool-frame* + (cl-transforms-stamped:make-vector-stamped + cram-tf:*robot-base-frame* 0.0 + (cl-transforms:make-3d-vector 0 0 1)) + (cl-transforms-stamped:make-vector-stamped + cram-tf:*robot-base-frame* 0.0 + (cl-transforms:make-3d-vector 0 0 1)))) + (when align-planes-right + (make-align-planes-constraint + pose-base-frame + cram-tf:*robot-right-tool-frame* + (cl-transforms-stamped:make-vector-stamped + cram-tf:*robot-base-frame* 0.0 + (cl-transforms:make-3d-vector 0 0 1)) + (cl-transforms-stamped:make-vector-stamped + cram-tf:*robot-base-frame* 0.0 + (cl-transforms:make-3d-vector 0 0 1)))) (when unmovable-joints (make-unmovable-joints-constraint unmovable-joints)) - (make-base-velocity-constraint - *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) + ;; (make-base-velocity-constraint + ;; *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) (make-head-pointing-at-hand-constraint (if left-pose :left - :right))) - :cartesian-constraints (list (when left-pose - (make-cartesian-constraint - pose-base-frame - cram-tf:*robot-left-tool-frame* - left-pose)) - (when right-pose - (make-cartesian-constraint - pose-base-frame - cram-tf:*robot-right-tool-frame* - right-pose))) + :right)) + (when (eq (rob-int:get-robot-name) :tiago-dual) + (make-diffdrive-cartesian-goal-arm-constraint + (if left-pose + cram-tf:*robot-left-tool-frame* + cram-tf:*robot-right-tool-frame*))) + (when precise-tracking + (make-enable-velocity-trajectory-tracking-constraint)) + (when left-pose + (make-cartesian-constraint + pose-base-frame + ;; cram-tf:*robot-left-wrist-frame* + cram-tf:*robot-left-tool-frame* + left-pose + :straight-line straight-line + :avoid-collisions-much nil)) + (when right-pose + (make-cartesian-constraint + pose-base-frame + ;; cram-tf:*robot-right-wrist-frame* + cram-tf:*robot-right-tool-frame* + right-pose + :straight-line straight-line + :avoid-collisions-much nil))) :collisions (ecase collision-mode (:avoid-all (make-avoid-all-collision)) (:allow-all (make-allow-all-collision)) (:allow-hand (alexandria:flatten - (list ;; (make-avoid-all-collision) + (list (make-allow-hand-collision arms collision-object-b collision-object-b-link) (make-allow-hand-collision arms (rob-int:get-environment-name))))) (:allow-fingers (alexandria:flatten - (list ;; (make-avoid-all-collision) - (make-allow-fingers-collision - arms collision-object-b - collision-object-b-link) - (make-allow-fingers-collision - arms (rob-int:get-environment-name))))) + (list + (make-allow-fingers-collision + arms collision-object-b + collision-object-b-link) + (make-allow-fingers-collision + arms (rob-int:get-environment-name))))) (:allow-arm (alexandria:flatten - (list ;; (make-avoid-all-collision) + (list (make-allow-arm-collision arms collision-object-b collision-object-b-link) (make-allow-arm-collision arms (rob-int:get-environment-name))))) - (:allow-attached (make-avoid-all-collision) - ; attached objects are handled by giskard - ))))) + ;; TODO: this should allow collision between attached and environment + (:allow-attached (make-avoid-all-collision)))))) (defun make-arm-joint-action-goal (joint-state-left joint-state-right align-planes-left align-planes-right @@ -137,30 +170,30 @@ (type boolean align-planes-left align-planes-right)) (make-giskard-goal :constraints (list - (make-ee-velocity-constraint - :left - (if try-harder - (/ *arm-max-velocity-slow-xy* 3.0) - *arm-max-velocity-slow-xy*) - (if try-harder - (/ *arm-max-velocity-slow-theta* 3.0) - *arm-max-velocity-slow-theta*)) - (make-ee-velocity-constraint - :right - (if try-harder - (/ *arm-max-velocity-slow-xy* 3.0) - *arm-max-velocity-slow-xy*) - (if try-harder - (/ *arm-max-velocity-slow-theta* 3.0) - *arm-max-velocity-slow-theta*)) + ;; (make-ee-velocity-constraint + ;; :left + ;; (if try-harder + ;; (/ *arm-max-velocity-slow-xy* 3.0) + ;; *arm-max-velocity-slow-xy*) + ;; (if try-harder + ;; (/ *arm-max-velocity-slow-theta* 3.0) + ;; *arm-max-velocity-slow-theta*)) + ;; (make-ee-velocity-constraint + ;; :right + ;; (if try-harder + ;; (/ *arm-max-velocity-slow-xy* 3.0) + ;; *arm-max-velocity-slow-xy*) + ;; (if try-harder + ;; (/ *arm-max-velocity-slow-theta* 3.0) + ;; *arm-max-velocity-slow-theta*)) (make-cartesian-constraint cram-tf:*odom-frame* cram-tf:*robot-base-frame* (cl-transforms-stamped:pose->pose-stamped cram-tf:*robot-base-frame* 0.0 (cl-transforms:make-identity-pose)) - :max-velocity *base-max-velocity-slow-xy* - ;; :avoid-collisions-much t - ) + :max-linear-velocity *base-max-velocity-slow-xy* + :max-angular-velocity *base-max-velocity-slow-theta* + :avoid-collisions-much nil) (when align-planes-left (make-align-planes-tool-frame-constraint :left @@ -185,9 +218,44 @@ -(defun ensure-arm-cartesian-goal-input (frame goal-pose) +(defun ensure-arm-cartesian-goal-input (frame goal-pose arm) (when goal-pose - (cram-tf:ensure-pose-in-frame goal-pose frame))) + (let* ((tool-pose-in-correct-base-frame + (cram-tf:ensure-pose-in-frame goal-pose frame)) + (tool-frame + (if (eq arm :left) + cram-tf:*robot-left-tool-frame* + cram-tf:*robot-right-tool-frame*)) + (tool-transform-in-correct-base-frame + (cram-tf:pose-stamped->transform-stamped + tool-pose-in-correct-base-frame + tool-frame)) + (wrist-frame + (if (eq arm :left) + cram-tf:*robot-left-wrist-frame* + cram-tf:*robot-right-wrist-frame*)) + (ee-P-tcp + (cut:var-value + '?ee-P-tcp + (car + (prolog:prolog + `(and (rob-int:robot ?robot-name) + (rob-int:tcp-in-ee-pose ?robot-name ?ee-P-tcp)))))) + (tool-T-wrist + (cl-transforms-stamped:transform->transform-stamped + tool-frame + wrist-frame + 0.0 + (cl-transforms:transform-inv + (cl-transforms:pose->transform ee-P-tcp)))) + (wrist-pose-in-correct-base-frame + (cram-tf:multiply-transform-stampeds + frame wrist-frame + tool-transform-in-correct-base-frame + tool-T-wrist + :result-as-pose-or-transform :pose))) + ;; wrist-pose-in-correct-base-frame + tool-pose-in-correct-base-frame))) (defun ensure-arm-joint-goal-input (goal-configuration arm) (if (and (listp goal-configuration) @@ -236,14 +304,16 @@ collision-mode collision-object-b collision-object-b-link collision-object-a - move-base prefer-base + move-base prefer-base straight-line align-planes-left align-planes-right - unmovable-joints) + unmovable-joints + precise-tracking) (declare (type (or number null) action-timeout) (type (or cl-transforms-stamped:pose-stamped null) goal-pose-left goal-pose-right) (type (or string null) pose-base-frame) - (type boolean move-base prefer-base align-planes-left align-planes-right) + (type boolean move-base prefer-base straight-line precise-tracking + align-planes-left align-planes-right) (type (or list null) unmovable-joints)) (unless (or goal-pose-left goal-pose-right) @@ -257,34 +327,37 @@ (setf pose-base-frame cram-tf:*odom-frame*) (setf pose-base-frame cram-tf:*robot-base-frame*)) (setf goal-pose-left - (ensure-arm-cartesian-goal-input pose-base-frame goal-pose-left)) + (ensure-arm-cartesian-goal-input pose-base-frame goal-pose-left :left)) (setf goal-pose-right - (ensure-arm-cartesian-goal-input pose-base-frame goal-pose-right)) + (ensure-arm-cartesian-goal-input pose-base-frame goal-pose-right :right)) (cram-tf:visualize-marker (list goal-pose-left goal-pose-right) :r-g-b-list '(1 0 1)) - (call-action - :action-goal (make-arm-cartesian-action-goal - goal-pose-left goal-pose-right - pose-base-frame - collision-mode - :collision-object-b collision-object-b - :collision-object-b-link collision-object-b-link - :collision-object-a collision-object-a - :allow-base move-base - :prefer-base prefer-base - :align-planes-left align-planes-left - :align-planes-right align-planes-right - :unmovable-joints unmovable-joints) - :action-timeout action-timeout - :check-goal-function (lambda (result status) - (declare (ignore result status)) - (or (ensure-arm-cartesian-goal-reached - goal-pose-left cram-tf:*robot-left-tool-frame*) - (ensure-arm-cartesian-goal-reached - goal-pose-right cram-tf:*robot-right-tool-frame*))))) + (let ((goal (make-arm-cartesian-action-goal + goal-pose-left goal-pose-right + pose-base-frame + collision-mode + :collision-object-b collision-object-b + :collision-object-b-link collision-object-b-link + :collision-object-a collision-object-a + :allow-base move-base + :prefer-base prefer-base + :straight-line straight-line + :align-planes-left align-planes-left + :align-planes-right align-planes-right + :unmovable-joints unmovable-joints + :precise-tracking precise-tracking))) + (call-action + :action-goal goal + :action-timeout action-timeout + :check-goal-function (lambda (result status) + (declare (ignore result status)) + (or (ensure-arm-cartesian-goal-reached + goal-pose-left cram-tf:*robot-left-tool-frame*) + (ensure-arm-cartesian-goal-reached + goal-pose-right cram-tf:*robot-right-tool-frame*)))))) diff --git a/cram_external_interfaces/cram_giskard/src/base-goals.lisp b/cram_external_interfaces/cram_giskard/src/base-goals.lisp index 9d208c47d2..c447fcc168 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -43,31 +43,41 @@ ;; 0.5 0.25 "In meters/s") (defparameter *base-max-velocity-fast-theta* - 0.4 "In rad/s, about 23 deg/s.") + 1.0 "In rad/s, about 23 deg/s.") (defparameter *base-max-velocity-slow-xy* 0.04 "In meters/s") (defparameter *base-max-velocity-slow-theta* - 0.07 "In rad/s, about 11.5 deg.") + 0.1 "In rad/s, about 11.5 deg.") (defun make-giskard-base-action-goal (pose base-velocity) (declare (type cl-transforms-stamped:pose-stamped pose) (type (or keyword number null) base-velocity)) (make-giskard-goal :constraints (list - (make-cartesian-constraint - cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose - :avoid-collisions-much t - :max-velocity *base-max-velocity-fast-xy*) - (make-base-collision-avoidance-hint-constraint - *base-collision-avoidance-hint-link* - (cl-transforms-stamped:make-vector-stamped - cram-tf:*fixed-frame* 0.0 - *base-collision-avoidance-hint-vector*)) - (if (eq base-velocity :slow) - (make-base-velocity-constraint - *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) - (make-base-velocity-constraint - *base-max-velocity-fast-xy* *base-max-velocity-fast-theta*)) + (if (eq (rob-int:get-robot-name) :tiago-dual) + (make-diffdrive-base-goal + cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose + :avoid-collisions-much t + :max-linear-velocity *base-max-velocity-fast-xy* + :max-angular-velocity *base-max-velocity-fast-theta* + :always-forward nil) + (make-cartesian-constraint + cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose + :avoid-collisions-much t + :max-linear-velocity *base-max-velocity-fast-xy* + :max-angular-velocity *base-max-velocity-fast-theta*)) + (when (eq (rob-int:get-environment-name) :iai-kitchen) + (make-base-collision-avoidance-hint-constraint + *base-collision-avoidance-hint-link* + (cl-transforms-stamped:make-vector-stamped + cram-tf:*fixed-frame* 0.0 + *base-collision-avoidance-hint-vector*))) + (when (eq base-velocity :slow) + (make-base-velocity-constraint + *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) + ;; (make-base-velocity-constraint + ;; *base-max-velocity-fast-xy* *base-max-velocity-fast-theta*) + ) (make-head-pointing-constraint (cl-transforms-stamped:make-pose-stamped cram-tf:*robot-base-frame* 0.0 @@ -100,9 +110,11 @@ (cram-tf:visualize-marker goal-pose :r-g-b-list '(0 1 0)) - (call-action - :action-goal (make-giskard-base-action-goal goal-pose base-velocity) - :action-timeout action-timeout - :check-goal-function (lambda (result status) - (declare (ignore result status)) - (ensure-base-goal-reached goal-pose)))) + ;; Trying with slow velocity should happen on the motion level, not action level. + (let ((goal (make-giskard-base-action-goal goal-pose base-velocity))) + (call-action + :action-goal goal + :action-timeout action-timeout + :check-goal-function (lambda (result status) + (declare (ignore result status)) + (ensure-base-goal-reached goal-pose))))) diff --git a/cram_external_interfaces/cram_giskard/src/collision-scene.lisp b/cram_external_interfaces/cram_giskard/src/collision-scene.lisp index 2060593a8e..bb46696d68 100644 --- a/cram_external_interfaces/cram_giskard/src/collision-scene.lisp +++ b/cram_external_interfaces/cram_giskard/src/collision-scene.lisp @@ -155,6 +155,7 @@ pose mesh-path dimensions + scale joint-state-topic parent-link parent-link-group) @@ -169,13 +170,16 @@ (cl-transforms:make-identity-rotation)))) (unless dimensions (setf dimensions '(1.0 1.0 1.0))) + (unless scale + (setf scale (cl-transforms:make-3d-vector 1.0 1.0 1.0))) (let ((body (if mesh-path (roslisp:make-msg 'giskard_msgs-msg:worldbody :type (roslisp:symbol-code 'giskard_msgs-msg:worldbody :mesh_body) - :mesh mesh-path) + :mesh mesh-path + :scale (cl-transforms-stamped:to-msg scale)) (roslisp:make-msg 'giskard_msgs-msg:worldbody :type (roslisp:symbol-code @@ -523,10 +527,13 @@ The `parent-group-name' is usually the robot or environment." (defmethod coe:on-event giskard-attach-object ((event cpoe:object-attached-robot)) (unless cram-projection:*projection-environment* - (attach-object-to-arm-in-collision-scene - (cpoe:event-object-name event) - (cpoe:event-arm event) - (cpoe:event-link event)))) + (if (eq (cpoe:event-other-object-name event) (rob-int:get-environment-name)) + (roslisp:ros-warn (giskard coll-scene) + "Attaching objects to environment is not supported yet.") + (attach-object-to-arm-in-collision-scene + (cpoe:event-object-name event) + (cpoe:event-arm event) + (cpoe:event-link event))))) (defmethod coe:on-event giskard-detach-object 1 ((event cpoe:object-detached-robot)) (unless cram-projection:*projection-environment* diff --git a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp index 4cf2032d52..7a6a7d800f 100644 --- a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp @@ -31,61 +31,133 @@ (defun make-environment-manipulation-goal (open-or-close arm handle-link joint-state - prefer-base) + prefer-base + &optional hinge-point-stamped) (declare (type keyword open-or-close arm) (type symbol handle-link) (type (or number null) joint-state) - (type boolean prefer-base)) + (type boolean prefer-base) + (type (or null cl-transforms-stamped:point-stamped) hinge-point-stamped)) (make-giskard-goal :constraints (list (when prefer-base - (make-prefer-base-constraint :do-not-rotate T)) - (make-base-velocity-constraint - *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) + ;; PREFER BASE is implemented in a very hacky way. + ;; For now, disabling it, so the robot will always use the + ;; the arm and won't help the arm with the base much. + ;; (make-prefer-base-constraint :do-not-rotate T) + ) + ;; (make-base-velocity-constraint + ;; *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) (make-open-or-close-constraint open-or-close arm handle-link joint-state) - (make-avoid-joint-limits-constraint) + ;; (when (eq (rob-int:get-robot-name) :tiago-dual) + ;; (make-diffdrive-base-arch-constraint hinge-point-stamped)) + ;; (make-avoid-joint-limits-constraint + ;; :joint-list (cut:var-value + ;; '?joints + ;; (car + ;; (prolog:prolog + ;; `(and (rob-int:robot ?robot-name) + ;; (rob-int:arm-joints ?robot-name ,arm ?joints)))))) + ;; (make-head-pointing-at-hand-constraint arm) + ) + :collisions (make-constraints-vector + (make-allow-hand-collision + (list arm) (rob-int:get-environment-name) handle-link)))) + +(defun make-environment-manipulation-diffdrive-pregoal (arm handle-link + &optional hinge-point-stamped) + (declare (type keyword arm) + (type symbol handle-link) + (type (or null cl-transforms-stamped:point-stamped) hinge-point-stamped)) + (make-giskard-goal + :constraints (list + (let ((wrist-link + (if (eq arm :left) + cram-tf:*robot-left-wrist-frame* + cram-tf:*robot-right-wrist-frame*))) + (make-cartesian-constraint + cram-tf:*odom-frame* + wrist-link + (cl-transforms-stamped:pose->pose-stamped + wrist-link 0.0 + (cl-transforms:make-identity-pose)))) + (when (eq (rob-int:get-robot-name) :tiago-dual) + (make-diffdrive-base-arch-constraint hinge-point-stamped :small-weight t)) + (make-avoid-joint-limits-constraint + :joint-list (cut:var-value + '?joints + (car + (prolog:prolog + `(and (rob-int:robot ?robot-name) + (rob-int:arm-joints + ?robot-name ,arm ?joints)))))) (make-head-pointing-at-hand-constraint arm)) :collisions (make-constraints-vector - (ecase open-or-close - (:open (make-allow-hand-collision - (list arm) (rob-int:get-environment-name) handle-link)) - (:close (make-allow-arm-collision - (list arm) (rob-int:get-environment-name))))))) + (make-allow-hand-collision + (list arm) (rob-int:get-environment-name) handle-link)))) (defun call-environment-manipulation-action (&key action-timeout open-or-close arm handle-link joint-angle - prefer-base) + prefer-base + joint-pose) (declare (type keyword open-or-close arm) (type symbol handle-link) (type (or number null) joint-angle action-timeout) - (type boolean prefer-base)) + (type boolean prefer-base) + (type (or cl-transforms:pose null) joint-pose)) + + (let ((joint-point-stamped + (when joint-pose + (cl-transforms-stamped:make-point-stamped + cram-tf:*fixed-frame* + 0.0 + (cl-transforms:origin joint-pose))))) + + ;; (when (eq (rob-int:get-robot-name) :tiago-dual) + ;; (call-action + ;; :action-goal (make-environment-manipulation-diffdrive-pregoal + ;; arm handle-link joint-point-stamped) + ;; :action-timeout action-timeout)) - (if (eq open-or-close :open) - (dotimes (i 2) - (call-action - :action-goal (make-environment-manipulation-goal - open-or-close arm handle-link (/ joint-angle 2.0) prefer-base) - ;; This needs a fix: we open the joint 2x for the same joint state - ;; Instead do open half, update global env joint-state, open full. - :action-timeout action-timeout - :check-goal-function (lambda (result status) - (declare (ignore result)) - (when (or (not status) - (member status '(:preempted :aborted :timeout))) - (make-instance - 'common-fail:environment-manipulation-goal-not-reached - :description "Giskard action failed."))))) - (call-action - :action-goal (make-environment-manipulation-goal - open-or-close arm handle-link joint-angle prefer-base) - :action-timeout action-timeout - :check-goal-function (lambda (result status) - (declare (ignore result)) - (when (or (not status) - (member status '(:preempted :aborted :timeout))) - (make-instance - 'common-fail:environment-manipulation-goal-not-reached - :description "Giskard action failed.")))))) + (call-action + :action-goal (make-environment-manipulation-goal + open-or-close arm handle-link joint-angle prefer-base + joint-point-stamped) + :action-timeout action-timeout + :check-goal-function (lambda (result status) + (declare (ignore result)) + (when (or (not status) + (member status '(:preempted :aborted :timeout))) + (make-instance + 'common-fail:environment-manipulation-goal-not-reached + :description "Giskard action failed."))))) + ;; (if (eq open-or-close :open) + ;; (dotimes (i 2) + ;; (call-action + ;; :action-goal (make-environment-manipulation-goal + ;; open-or-close arm handle-link (/ joint-angle 2.0) prefer-base) + ;; ;; This needs a fix: we open the joint 2x for the same joint state + ;; ;; Instead do open half, update global env joint-state, open full. + ;; :action-timeout action-timeout + ;; :check-goal-function (lambda (result status) + ;; (declare (ignore result)) + ;; (when (or (not status) + ;; (member status '(:preempted :aborted :timeout))) + ;; (make-instance + ;; 'common-fail:environment-manipulation-goal-not-reached + ;; :description "Giskard action failed."))))) + ;; (call-action + ;; :action-goal (make-environment-manipulation-goal + ;; open-or-close arm handle-link joint-angle prefer-base) + ;; :action-timeout action-timeout + ;; :check-goal-function (lambda (result status) + ;; (declare (ignore result)) + ;; (when (or (not status) + ;; (member status '(:preempted :aborted :timeout))) + ;; (make-instance + ;; 'common-fail:environment-manipulation-goal-not-reached + ;; :description "Giskard action failed."))))) + ) diff --git a/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp b/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp index 61bc26cc05..bacc2194e5 100644 --- a/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp @@ -31,6 +31,7 @@ (in-package :giskard) (defparameter *gripper-convergence-delta-joint* 0.005 "In meters.") +(defparameter *gripper-gripped-min-position* 0.007 "In meters.") (defun make-gripper-action-goal (arm joint-angle effort) (declare (type keyword arm) @@ -42,12 +43,14 @@ (defun ensure-gripper-goal-input (action-type-or-position arm effort) (declare (type (or number keyword) action-type-or-position) - (type keyword arm) + (type (or keyword list) arm) (type (or number null) effort)) + (unless (listp arm) + (setf arm (list arm))) (let* ((bindings (car (prolog:prolog `(and (rob-int:robot ?robot) - (rob-int:gripper-joint ?robot ,arm ?gripper-joint) + (rob-int:gripper-joint ?robot ,(first arm) ?gripper-joint) (rob-int:joint-lower-limit ?robot ?gripper-joint ?lower-limit) (rob-int:joint-upper-limit ?robot ?gripper-joint ?upper-limit) (rob-int:gripper-meter-to-joint-multiplier ?robot ?mult))))) @@ -93,33 +96,45 @@ (:grip 15.0))))))) (list position effort)))) -(defun ensure-gripper-goal-reached (arm joint-angle) - (when (and joint-angle arm) - (let ((gripper-joint - (cut:var-value - '?gripper-joint - (car (prolog:prolog - `(and (rob-int:robot ?robot) - (rob-int:gripper-joint ?robot ,arm ?gripper-joint))))))) - (when (cut:is-var gripper-joint) - (error "[giskard] Robot gripper joint was not defined.")) - (let ((current-state (car (joints:joint-positions (list gripper-joint)))) - (goal-state joint-angle)) - (unless (cram-tf:values-converged current-state - goal-state - *gripper-convergence-delta-joint*) - (make-instance 'common-fail:manipulation-goal-not-reached - :description (format nil "Giskard did not converge to goal:~%~ +(defun ensure-gripper-goal-reached (arms action-type-or-position joint-angle) + (car + (mapcar (lambda (arm) + (when (and arm action-type-or-position joint-angle) + (let ((gripper-joint + (cut:var-value + '?gripper-joint + (car (prolog:prolog + `(and (rob-int:robot ?robot) + (rob-int:gripper-joint ?robot ,arm ?gripper-joint))))))) + (when (cut:is-var gripper-joint) + (error "[giskard] Robot gripper joint was not defined.")) + (let ((current-state (car (joints:joint-positions (list gripper-joint)))) + (goal-state joint-angle)) + (if (and (symbolp action-type-or-position) + (eq action-type-or-position :grip)) + (unless (> current-state *gripper-gripped-min-position*) + (make-instance 'common-fail:gripper-closed-completely + :description (format nil "Giskard commanded gripper ~a to grip,~%~ + but it closed completely to ~a,~%~ + which is below ~a." + arm current-state + *gripper-gripped-min-position*))) + (unless (cram-tf:values-converged current-state + goal-state + *gripper-convergence-delta-joint*) + (make-instance 'common-fail:manipulation-goal-not-reached + :description (format nil "Giskard did not converge to goal:~%~ arm: ~a, current state: ~a, goal state: ~a, ~ delta joint: ~a." - arm current-state goal-state - *gripper-convergence-delta-joint*))))))) + arm current-state goal-state + *gripper-convergence-delta-joint*)))))))) + arms))) (defun call-gripper-action (&key action-timeout action-type-or-position arm effort) (declare (type (or number null) action-timeout effort) - (type keyword arm) + (type (or keyword list) arm) (type (or number keyword) action-type-or-position)) (let* ((position-and-effort @@ -129,9 +144,21 @@ (effort (second position-and-effort))) - (call-action - :action-goal (make-gripper-action-goal arm goal-joint-angle effort) - :action-timeout action-timeout - ;; :check-goal-function (lambda () - ;; (ensure-gripper-goal-reached arm goal-joint-angle)) - ))) + (unless (listp arm) + (setf arm (list arm))) + + (mapcar (lambda (arm-element) + (call-action + :action-goal (make-gripper-action-goal arm-element goal-joint-angle effort) + :action-timeout action-timeout + :check-goal-function (lambda (result status) + ;; This check is only done after the action + ;; and never before, therefore check + ;; if result and status already exist. + (if (and result status) + (ensure-gripper-goal-reached + arm + action-type-or-position + goal-joint-angle) + :goal-not-achieved-yet)))) + arm))) diff --git a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp index dd1bafcfb9..3087f38eb2 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -51,7 +51,9 @@ (defun make-giskard-goal (&key constraints joint-constraints cartesian-constraints collisions - (goal-type :plan_and_execute_and_cut_off_shaking)) + (goal-type ;:plan_only + :plan_and_execute_and_cut_off_shaking + )) (roslisp:make-message 'giskard_msgs-msg:MoveGoal :type (roslisp:symbol-code 'giskard_msgs-msg:MoveGoal goal-type) @@ -119,16 +121,21 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; JSON CONSTRAINTS ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(defun make-avoid-joint-limits-constraint (&optional (percentage - *avoid-joint-limits-percentage*)) - (declare (type number percentage)) +(defun make-avoid-joint-limits-constraint (&key + (percentage + *avoid-joint-limits-percentage*) + joint-list) + (declare (type number percentage) + (type list joint-list)) (roslisp:make-message 'giskard_msgs-msg:constraint :type "AvoidJointLimits" :parameter_value_pair (alist->json-string - `(("percentage" . ,percentage))))) + `(("percentage" . ,percentage) + ,@(when joint-list + `(("joint_list" . ,(map 'vector #'identity joint-list)))))))) (defun make-prefer-base-constraint (&key @@ -173,7 +180,7 @@ (alist->json-string `(("root_link" . ,root-frame) ("tip_link" . ,tip-frame) - ("root_normal" + ("goal_normal" . (("message_type" . "geometry_msgs/Vector3Stamped") ("message" . ,(to-hash-table root-vector)))) ("tip_normal" @@ -221,8 +228,14 @@ ("message" . ,(to-hash-table (cram-tf:pose-stamped->point-stamped goal-pose))))) - ,@(when pointing-vector - `(("pointing_axis" . ,(to-hash-table pointing-vector)))))))) + ,@(if pointing-vector + `(("pointing_axis" . ,(to-hash-table pointing-vector))) + `(("pointing_axis" . (("message_type" . "geometry_msgs/Vector3Stamped") + ("message" . ,(to-hash-table + (cl-transforms-stamped:make-vector-stamped + tip-frame + 0.0 + (cl-transforms:make-3d-vector 0 0 1)))))))))))) (defun make-head-pointing-constraint (goal-pose) (declare (type cl-transforms-stamped:pose-stamped goal-pose)) @@ -279,6 +292,34 @@ ,@(when (and (eq open-or-close :open) goal-joint-state) `(("goal_joint_state" . ,goal-joint-state)))))))) +(defun make-diffdrive-base-arch-constraint (door-joint-point-stamped-in-map + &key small-weight) + (declare (type cl-transforms-stamped:point-stamped door-joint-point-stamped-in-map)) + (roslisp:make-message + 'giskard_msgs-msg:constraint + :type + "DiffDriveTangentialToPoint" + :parameter_value_pair + (alist->json-string + `(("goal_point" + . (("message_type" . "geometry_msgs/PointStamped") + ("message" . ,(to-hash-table + door-joint-point-stamped-in-map)))) + ,@(when small-weight + `(("weight" . ,(roslisp-msg-protocol:symbol-code + 'giskard_msgs-msg:constraint + :weight_below_ca)))))))) + +(defun make-diffdrive-cartesian-goal-arm-constraint (tip-link) + (declare (type string tip-link)) + (roslisp:make-message + 'giskard_msgs-msg:constraint + :type + "KeepHandInWorkspace" + :parameter_value_pair + (alist->json-string + `(("tip_link" . ,tip-link))))) + (defun make-grasp-bar-constraint (arm root-link tip-grasp-axis bar-axis bar-center bar-length) @@ -313,15 +354,18 @@ . ,root-link)))))) (defun make-cartesian-constraint (root-frame tip-frame goal-pose - &key max-velocity avoid-collisions-much) + &key max-linear-velocity max-angular-velocity + avoid-collisions-much straight-line) (declare (type string root-frame tip-frame) (type cl-transforms-stamped:pose-stamped goal-pose) - (type (or number null) max-velocity) - (type boolean avoid-collisions-much)) + (type (or number null) max-linear-velocity max-angular-velocity) + (type boolean avoid-collisions-much straight-line)) (roslisp:make-message 'giskard_msgs-msg:constraint :type - "CartesianPose" + (if straight-line + "CartesianPoseStraight" + "CartesianPose") :parameter_value_pair (alist->json-string `(("root_link" . ,root-frame) @@ -329,8 +373,44 @@ ("goal_pose" . (("message_type" . "geometry_msgs/PoseStamped") ("message" . ,(to-hash-table goal-pose)))) - ,@(when max-velocity - `(("max_linear_velocity" . ,max-velocity))) + ,@(when max-linear-velocity + `(("reference_linear_velocity" . ,max-linear-velocity))) + ,@(when max-angular-velocity + `(("reference_angular_velocity" . ,max-angular-velocity))) + ,@(if avoid-collisions-much + `(("weight" . ,(roslisp-msg-protocol:symbol-code + 'giskard_msgs-msg:constraint + :weight_below_ca + ))) + `(("weight" . ,(roslisp-msg-protocol:symbol-code + 'giskard_msgs-msg:constraint + :weight_above_ca ; that's the default weight anyway + )))))))) + +(defun make-diffdrive-base-goal (root-frame tip-frame goal-pose + &key max-linear-velocity max-angular-velocity + avoid-collisions-much always-forward) + (declare (type string root-frame tip-frame) + (type cl-transforms-stamped:pose-stamped goal-pose) + (type (or number null) max-linear-velocity max-angular-velocity) + (type boolean avoid-collisions-much always-forward)) + (roslisp:make-message + 'giskard_msgs-msg:constraint + :type + "DiffDriveBaseGoal" + :parameter_value_pair + (alist->json-string + `(("root_link" . ,root-frame) + ("tip_link" . ,tip-frame) + ("goal_pose" + . (("message_type" . "geometry_msgs/PoseStamped") + ("message" . ,(to-hash-table goal-pose)))) + ,@(when always-forward + `(("always_forward" . T))) + ,@(when max-linear-velocity + `(("max_linear_velocity" . ,max-linear-velocity))) + ,@(when max-angular-velocity + `(("max_angular_velocity" . ,max-angular-velocity))) ,@(if avoid-collisions-much `(("weight" . ,(roslisp-msg-protocol:symbol-code 'giskard_msgs-msg:constraint @@ -342,7 +422,7 @@ )))))))) (defun make-joint-constraint (joint-state &optional weights) -"`joint-state' is a list of two elements: (joint-names joint-positions). + "`joint-state' is a list of two elements: (joint-names joint-positions). `weights' is a list of the same length as `joint-names' and `joint-positions', a keyword, a number or NIL." (declare (type list joint-state) @@ -509,6 +589,15 @@ a keyword, a number or NIL." (list gripper-joints (make-list (length gripper-joints) :initial-element joint-angle))))) +(defun make-enable-velocity-trajectory-tracking-constraint () + (roslisp:make-message + 'giskard_msgs-msg:constraint + :type + "EnableVelocityTrajectoryTracking" + :parameter_value_pair + (alist->json-string + `(("enabled" . T))))) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; COLLISIONS ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (defun make-allow-all-collision () @@ -569,11 +658,12 @@ a keyword, a number or NIL." (let* ((robot-links-table (cl-urdf:links rob-int:*robot-urdf*)) (link-parent-alist (mapcar (lambda (link-name) - `(,link-name - . ,(cl-urdf:name - (cl-urdf:parent - (cl-urdf:from-joint - (gethash link-name robot-links-table)))))) + (let ((link-object + (gethash link-name robot-links-table))) + (when link-object + `(,link-name + . ,(cl-urdf:name + (cl-urdf:parent (cl-urdf:from-joint link-object))))))) link-names))) ;; remove all whose parent is in `link-names' (mapcar #'car @@ -595,7 +685,7 @@ a keyword, a number or NIL." (car (prolog:prolog `(and (rob-int:robot ?robot) - (rob-int:arm-links ?robot ,arm ?arm-links)))))) + (rob-int:arm-links ?robot ,arm . ?arm-links)))))) arms))) collision-group2) (when body-b diff --git a/cram_external_interfaces/cram_giskard/src/neck-goals.lisp b/cram_external_interfaces/cram_giskard/src/neck-goals.lisp index b3c732aab5..530e3d0a0c 100644 --- a/cram_external_interfaces/cram_giskard/src/neck-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/neck-goals.lisp @@ -44,13 +44,11 @@ (declare (type cl-transforms-stamped:pose-stamped goal-pose)) (let ((neck-joints (first (get-neck-joint-names-and-positions-list)))) (make-giskard-goal - :constraints `(,(make-pointing-constraint - root-link - camera-link - goal-pose) - ;; for PTU heads align planes doesn't make sense - ,@(when (> (length neck-joints) 2) - `((make-align-planes-constraint + :constraints (list (make-pointing-constraint + root-link camera-link goal-pose) + ;; for PTU heads align planes doesn't make sense + (when (> (length neck-joints) 2) + (make-align-planes-constraint root-link camera-link (cl-transforms-stamped:make-vector-stamped @@ -59,10 +57,6 @@ (cl-transforms-stamped:make-vector-stamped camera-link 0.0 (cl-transforms:make-3d-vector 0 -1 0))))) - ;; (cl-transforms-stamped:make-vector-stamped - ;; "rs_camera_depth_optical_frame" 0.0 - ;; (cl-transforms:make-3d-vector 0 0 1)) - ) :cartesian-constraints (when camera-offset (make-cartesian-constraint root-link camera-link diff --git a/cram_external_interfaces/cram_giskard/src/process-module.lisp b/cram_external_interfaces/cram_giskard/src/process-module.lisp index 7e5e98cb9e..16e22483e7 100644 --- a/cram_external_interfaces/cram_giskard/src/process-module.lisp +++ b/cram_external_interfaces/cram_giskard/src/process-module.lisp @@ -43,8 +43,10 @@ :collision-object-a (fifth rest-args) :move-base (sixth rest-args) :prefer-base (seventh rest-args) + :straight-line (tenth rest-args) :align-planes-left (eighth rest-args) - :align-planes-right (ninth rest-args))) + :align-planes-right (ninth rest-args) + :precise-tracking (nth 10 rest-args))) ; that's eleventh element (cram-common-designators:move-joints (giskard:call-arm-joint-action :goal-configuration-left argument-1 @@ -58,14 +60,16 @@ :arm argument-1 :handle-link (fifth rest-args) :joint-angle (second rest-args) - :prefer-base (eighth rest-args))) + :prefer-base (eighth rest-args) + :joint-pose (nth 10 rest-args))) (cram-common-designators:move-arm-push (giskard:call-environment-manipulation-action :open-or-close :close :arm argument-1 :handle-link (fifth rest-args) :joint-angle (second rest-args) - :prefer-base (eighth rest-args))) + :prefer-base (eighth rest-args) + :joint-pose (nth 10 rest-args))) (cram-common-designators:move-head (when argument-1 (giskard:call-neck-action diff --git a/cram_pr2/cram_pr2_process_modules/CATKIN_IGNORE b/cram_external_interfaces/cram_hpn/CATKIN_IGNORE similarity index 100% rename from cram_pr2/cram_pr2_process_modules/CATKIN_IGNORE rename to cram_external_interfaces/cram_hpn/CATKIN_IGNORE diff --git a/cram_external_interfaces/cram_robokudo/cram-robokudo.asd b/cram_external_interfaces/cram_robokudo/cram-robokudo.asd index 9b0e9be03e..3a4ac5a83f 100644 --- a/cram_external_interfaces/cram_robokudo/cram-robokudo.asd +++ b/cram_external_interfaces/cram_robokudo/cram-robokudo.asd @@ -37,6 +37,7 @@ roslisp-utilities cl-transforms cl-transforms-stamped + cl-tf2 cram-tf cram-common-failures cram-designators diff --git a/cram_external_interfaces/cram_robokudo/package.xml b/cram_external_interfaces/cram_robokudo/package.xml index f9b6839f4f..be91ad8249 100644 --- a/cram_external_interfaces/cram_robokudo/package.xml +++ b/cram_external_interfaces/cram_robokudo/package.xml @@ -16,6 +16,7 @@ roslisp_utilities cl_transforms cl_transforms_stamped + cl_tf2 cram_tf cram_common_failures cram_designators diff --git a/cram_external_interfaces/cram_robokudo/src/action-client.lisp b/cram_external_interfaces/cram_robokudo/src/action-client.lisp index 5ca54738e3..6b5238c7f4 100644 --- a/cram_external_interfaces/cram_robokudo/src/action-client.lisp +++ b/cram_external_interfaces/cram_robokudo/src/action-client.lisp @@ -29,6 +29,13 @@ (in-package :rk) +(defvar *robokudo-tf-buffer-client* nil) + +(defun init-tf-buffer-client () + (setf *robokudo-tf-buffer-client* (make-instance 'cl-tf2:buffer-client))) + +(roslisp-utilities:register-ros-init-function init-tf-buffer-client) + (defparameter *ros-action* "robokudo/query") (defun make-robokudo-action-client () @@ -39,6 +46,7 @@ (roslisp-utilities:register-ros-init-function make-robokudo-action-client) + ;;;;;;;;;;;;;;;;;;; INPUT ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (defun make-robokudo-query (key-value-pairs-list) @@ -132,13 +140,16 @@ (defun parse-result (message) (declare (type robokudo_msgs-msg:objectdesignator message)) "Returns a keyword key-value pairs list" - (flet ((to-keyword (string) + (flet ((to-keyword (string &key (underscores-or-camelcase :underscores)) (if (string-equal string "") nil - (roslisp-utilities:lispify-ros-underscore-name string :keyword)))) + (if (eq underscores-or-camelcase :underscores) + (roslisp-utilities:lispify-ros-underscore-name string :keyword) + (roslisp-utilities:lispify-ros-name string :keyword))))) `((:name ,(to-keyword ;; (roslisp:msg-slot-value message :uid) - (format nil "~a-1" (roslisp:msg-slot-value message :type)))) + (format nil "~a-1" (roslisp:msg-slot-value message :type)) + :underscores-or-camelcase :camelcase)) (:type ,(to-keyword (roslisp:msg-slot-value message :type))) (:shape ,(map 'list #'to-keyword (roslisp:msg-slot-value message :shape))) (:color ,(map 'list #'to-keyword (roslisp:msg-slot-value message :color))) @@ -196,6 +207,7 @@ (defun which-estimator-for-object (object-description) :ClusterPosePCAAnnotator + :ICPPOSEREFINEMENTANNOTATOR ;; (let ((type (second (find :type object-description :key #'car))) ;; (cad-model (find :cad-model object-description :key #'car)) ;; (obj-part (find :obj-part object-description :key #'car))) @@ -274,6 +286,8 @@ (:DMRoteBeteSaftBio :juice-box) (:JeroenCup + :jeroen-cup) + (:jeroen-cup :jeroen-cup)))) (setf rs-answer (subst-if `(:type ,cram-type) @@ -324,30 +338,45 @@ (let* ((pose-stamped-in-whatever (find-pose-in-object-designator combined-properties)) - (pose-stamped-in-base-frame - (if cram-tf:*robot-base-frame* - (cram-tf:ensure-pose-in-frame - pose-stamped-in-whatever - cram-tf:*robot-base-frame* - :use-zero-time t) - pose-stamped-in-whatever)) - (transform-stamped-in-base-frame - (cram-tf:pose-stamped->transform-stamped - pose-stamped-in-base-frame - (roslisp-utilities:rosify-underscores-lisp-name name))) - (pose-stamped-in-map-frame + (pose-stamped-in-map-frame-original-orientation (if cram-tf:*fixed-frame* (cram-tf:ensure-pose-in-frame pose-stamped-in-whatever cram-tf:*fixed-frame* - :use-zero-time t) + ;; :use-current-ros-time t + :transformer *robokudo-tf-buffer-client*) pose-stamped-in-whatever)) + (pose-stamped-in-map-frame + (cl-transforms-stamped:copy-pose-stamped + pose-stamped-in-map-frame-original-orientation + :orientation + ;; HACK + (if (< (cl-transforms:y + (cl-transforms:origin + pose-stamped-in-map-frame-original-orientation)) + 1.9) + ;; (cl-transforms:make-quaternion 1 0 0 0) + (cl-transforms:make-quaternion 0 0 0 1) + (cl-transforms:make-quaternion 0 0 1 0)))) (transform-stamped-in-map-frame (cram-tf:pose-stamped->transform-stamped pose-stamped-in-map-frame + (roslisp-utilities:rosify-underscores-lisp-name name))) + (pose-stamped-in-base-frame + (if cram-tf:*robot-base-frame* + (cram-tf:ensure-pose-in-frame + pose-stamped-in-map-frame + cram-tf:*robot-base-frame* + ;; :use-current-ros-time t + :transformer *robokudo-tf-buffer-client*) + pose-stamped-in-whatever)) + (transform-stamped-in-base-frame + (cram-tf:pose-stamped->transform-stamped + pose-stamped-in-base-frame (roslisp-utilities:rosify-underscores-lisp-name name)))) - (cram-tf:visualize-marker pose-stamped-in-whatever :r-g-b-list '(0 0 1) :id 1234) + ;; (cram-tf:visualize-marker pose-stamped-in-whatever :r-g-b-list '(0 0 1) :id 1234) + ;; (cram-tf:visualize-marker pose-stamped-in-map-frame :r-g-b-list '(1 0 0) :id 1235) (let* ((properties-without-pose (remove :pose combined-properties :key #'car)) @@ -372,6 +401,7 @@ ;;;;;;;;;;;;;;;;; ACTION ;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +(defvar *rs-result* nil) (defun call-robokudo-action (keyword-key-value-pairs-list &key (quantifier :all)) (declare (type (or keyword number) quantifier)) (multiple-value-bind (key-value-pairs-list quantifier) @@ -381,6 +411,7 @@ (actionlib-client:call-simple-action-client 'robokudo-action :action-goal (make-robokudo-query key-value-pairs-list)) + (setf *rs-result* result) (let* ((rs-parsed-result (ensure-robokudo-result result quantifier status)) (rs-result (ecase quantifier ((:a :an :the) (make-robokudo-designator diff --git a/cram_external_interfaces/cram_robosherlock/CATKIN_IGNORE b/cram_external_interfaces/cram_robosherlock/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/cram_external_interfaces/kdl_ik_service/package.xml b/cram_external_interfaces/kdl_ik_service/package.xml index 667896624c..7cbbae3f8d 100644 --- a/cram_external_interfaces/kdl_ik_service/package.xml +++ b/cram_external_interfaces/kdl_ik_service/package.xml @@ -1,4 +1,8 @@ - + + + kdl_ik_service 0.7.0 KDL Based IK solver @@ -11,7 +15,8 @@ catkin rospy - python_orocos_kdl + python_orocos_kdl + python3-pykdl urdfdom_py kdl_parser_py moveit_msgs diff --git a/cram_pr2/cram_pr2_process_modules/cram-pr2-process-modules.asd b/cram_pr2/cram_pr2_process_modules/cram-pr2-process-modules.asd index 4951cba13e..db6352a198 100644 --- a/cram_pr2/cram_pr2_process_modules/cram-pr2-process-modules.asd +++ b/cram_pr2/cram_pr2_process_modules/cram-pr2-process-modules.asd @@ -40,7 +40,7 @@ cram-common-designators cram-language ; for with-real-robot ;; cram-knowrob-world-state - cram-robosherlock + cram-robokudo cram-giskard cram-joint-states ; for joint state monitoring pm ;; cram-nav-pcontroller diff --git a/cram_pr2/cram_pr2_process_modules/package.xml b/cram_pr2/cram_pr2_process_modules/package.xml index f8dd74ae5f..e73ed91ee7 100644 --- a/cram_pr2/cram_pr2_process_modules/package.xml +++ b/cram_pr2/cram_pr2_process_modules/package.xml @@ -18,8 +18,8 @@ cram_common_failures cram_common_designators cram_language - cram_knowrob_world_state - cram_robosherlock + + cram_robokudo cram_giskard cram_joint_states diff --git a/cram_pr2/cram_pr2_process_modules/src/giskard.lisp b/cram_pr2/cram_pr2_process_modules/src/giskard.lisp index 51dba06942..63acd7a417 100644 --- a/cram_pr2/cram_pr2_process_modules/src/giskard.lisp +++ b/cram_pr2/cram_pr2_process_modules/src/giskard.lisp @@ -38,7 +38,8 @@ (desig:desig-prop ?motion-designator (:type :pulling)) (desig:desig-prop ?motion-designator (:type :pushing)) (desig:desig-prop ?motion-designator (:type :going)) - (desig:desig-prop ?motion-designator (:type :moving-torso)))) + (desig:desig-prop ?motion-designator (:type :moving-torso)) + (desig:desig-prop ?motion-designator (:type :looking)))) (prolog:<- (cpm:available-process-module giskard:giskard-pm) (prolog:not (cpm:projection-running ?_)))) diff --git a/cram_pr2/cram_pr2_process_modules/src/with-real-robot.lisp b/cram_pr2/cram_pr2_process_modules/src/with-real-robot.lisp index 10d03958a1..7bc037d247 100644 --- a/cram_pr2/cram_pr2_process_modules/src/with-real-robot.lisp +++ b/cram_pr2/cram_pr2_process_modules/src/with-real-robot.lisp @@ -31,8 +31,9 @@ (defmacro with-real-robot (&body body) `(cram-process-modules:with-process-modules-running - (rs:robosherlock-perception-pm ;; navp:navp-pm - pr2-grippers-pm pr2-ptu-pm giskard:giskard-pm joints:joint-state-pm + (rk:robokudo-perception-pm ;; navp:navp-pm + pr2-grippers-pm ;; pr2-ptu-pm + giskard:giskard-pm joints:joint-state-pm common-desig:wait-pm) (cpl-impl::named-top-level (:name :top-level) ,@body))) diff --git a/cram_robot_descriptions/cram_boxy_description/src/arms.lisp b/cram_robot_descriptions/cram_boxy_description/src/arms.lisp index 80f60339f7..982c3e5ac2 100644 --- a/cram_robot_descriptions/cram_boxy_description/src/arms.lisp +++ b/cram_robot_descriptions/cram_boxy_description/src/arms.lisp @@ -31,7 +31,7 @@ (defparameter *tcp-in-ee-pose* (cl-transforms:make-pose - (cl-transforms:make-3d-vector 0 0 0.3191d0) + (cl-transforms:make-3d-vector 0 0 0.3091d0) (cl-transforms:make-identity-rotation))) (defparameter *standard-to-boxy-gripper-transform* diff --git a/cram_robot_descriptions/cram_pr2_description/src/arm-kinematics.lisp b/cram_robot_descriptions/cram_pr2_description/src/arm-kinematics.lisp index cf33676341..9ba471c048 100644 --- a/cram_robot_descriptions/cram_pr2_description/src/arm-kinematics.lisp +++ b/cram_robot_descriptions/cram_pr2_description/src/arm-kinematics.lisp @@ -202,7 +202,7 @@ (<- (gripper-meter-to-joint-multiplier :pr2 5.0)) (<- (gripper-minimal-position :pr2 ?_ 0.013)) - (<- (gripper-convergence-delta :pr2 ?_ 0.005)) + (<- (gripper-convergence-delta :pr2 ?_ 0.02)) (<- (standard<-particular-gripper-transform :pr2 ?transform) (symbol-value *standard-to-pr2-gripper-transform* ?transform)) diff --git a/cram_robot_descriptions/cram_tiago_description/src/knowledge.lisp b/cram_robot_descriptions/cram_tiago_description/src/knowledge.lisp index 31c2cb0ca3..4c88de33d1 100644 --- a/cram_robot_descriptions/cram_tiago_description/src/knowledge.lisp +++ b/cram_robot_descriptions/cram_tiago_description/src/knowledge.lisp @@ -59,7 +59,13 @@ (cl-transforms:make-pose ;; The X should've been 0.150575d0, but the grasping frame is defined ;; on Tiago in such a shitty way that have to use a custom offset :/ - (cl-transforms:make-3d-vector 0.24d0 0 0) + ;; (cl-transforms:make-3d-vector 0.24d0 0 0) + ;; The iai_tiago URDF has FT sensors on the wrist, which the real robot does not. + ;; This offset is in tiago real robot URDF as published by the robot: + ;; (cl-transforms:make-3d-vector 0.22d0 0 0) + ;; But sticking to the URDF Tiago, as that's the one that will be shipped with CRAM. + ;; Giving the tool frame 1 cm more space inside to have more stable grasps: + (cl-transforms:make-3d-vector 0.23d0 0 0) (cl-transforms-stamped:make-quaternion 0.7071054825100438d0 -1.2986782349788673d-6 1.298673464711353d-6 @@ -291,20 +297,17 @@ ;; ("arm_left_7_joint" 0.0)) ) (robot-joint-states :tiago-dual :arm :left :carry-top ?joint-states)) - (<- (robot-joint-states :tiago-dual :arm :left :carry ?joint-states - ;; (("arm_left_1_joint" -1.0) - ;; ("arm_left_2_joint" 0.0) - ;; ("arm_left_3_joint" 1.5) - ;; ("arm_left_4_joint" 2.2) - ;; ("arm_left_5_joint" -1.5) - ;; ("arm_left_6_joint" 0.5) - ;; ("arm_left_7_joint" 0.0)) - ) - (robot-joint-states :tiago-dual :arm :left :carry-top ?joint-states)) + (<- (robot-joint-states :tiago-dual :arm :left :carry (("arm_left_1_joint" 0.3) + ("arm_left_2_joint" -0.87) + ("arm_left_3_joint" 1.68) + ("arm_left_4_joint" 1.7) + ("arm_left_5_joint" -0.6) + ("arm_left_6_joint" 1.12) + ("arm_left_7_joint" -0.09)))) (<- (robot-joint-states :tiago-dual :arm :left :carry-top (("arm_left_1_joint" 0.27) ("arm_left_2_joint" -1.07) ("arm_left_3_joint" 1.5) - ("arm_left_4_joint" 1.96) + ("arm_left_4_joint" 2.0) ("arm_left_5_joint" -2.0) ("arm_left_6_joint" 1.2) ("arm_left_7_joint" 0.5)))) diff --git a/cram_tiago/cram_tiago_process_modules/cram-tiago-process-modules.asd b/cram_tiago/cram_tiago_process_modules/cram-tiago-process-modules.asd index 8d0ed48a5c..1c74396816 100644 --- a/cram_tiago/cram_tiago_process_modules/cram-tiago-process-modules.asd +++ b/cram_tiago/cram_tiago_process_modules/cram-tiago-process-modules.asd @@ -35,15 +35,27 @@ :depends-on (cram-process-modules cram-designators cram-prolog + cram-language ; for grippers cram-tf cram-common-designators + cram-common-failures ; for grippers cram-robokudo ; for WITH-REAL-ROBOT cram-giskard - cram-joint-states) + cram-joint-states + + teleop_tools_msgs-msg ; for grippers action client + control_msgs-msg ; for grippers native action client + cram-simple-actionlib-client + roslisp + roslisp-utilities) :components ((:module "src" :components ((:file "package") - (:file "interface" :depends-on ("package")))))) + (:file "grippers" :depends-on ("package")) + (:file "grippers-via-fingers" :depends-on ("package")) + (:file "interface" :depends-on ("package" + "grippers" + "grippers-via-fingers")))))) diff --git a/cram_tiago/cram_tiago_process_modules/package.xml b/cram_tiago/cram_tiago_process_modules/package.xml index 07c39d5816..acf227825e 100644 --- a/cram_tiago/cram_tiago_process_modules/package.xml +++ b/cram_tiago/cram_tiago_process_modules/package.xml @@ -13,9 +13,16 @@ cram_process_modules cram_designators cram_prolog + cram_language cram_tf cram_common_designators + cram_common_failures cram_robokudo cram_giskard cram_joint_states + + teleop_tools_msgs + cram_simple_actionlib_client + roslisp + roslisp_utilities diff --git a/cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp b/cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp new file mode 100644 index 0000000000..2873259056 --- /dev/null +++ b/cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp @@ -0,0 +1,248 @@ +;;; +;;; Copyright (c) 2023, Gayane Kazhoyan +;;; Arthur Niedzwiecki +;;; 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 Institute for Artificial Intelligence/ +;;; Universitaet Bremen 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 +;;; 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. + +(in-package :tiago-pm) + +(defparameter *gripper-action-timeout* 20.0 "in seconds") + +(defparameter *gripper-convergence-delta* 0.005 "in meters") +(defparameter *gripper-gripped-min-position* 0.007 "In meters.") +(defparameter *gripper-joint-limit-tiny-offset* 0.00 + "In meters, not to go too close to joint limits.") + +(cpm:def-process-module grippers-via-fingers-pm (motion-designator) + (destructuring-bind (command action-type-or-position which-gripper &optional effort) + (desig:reference motion-designator) + (ecase command + (cram-common-designators:move-gripper-joint + (call-gripper-action + :action-type-or-position action-type-or-position + :arm which-gripper + :effort effort + :check-goal t + :action-timeout *gripper-action-timeout*))))) + + +(defun make-gripper-action-clients () + (actionlib-client:make-simple-action-client + 'gripper-right-left-finger + "gripper_right_left_finger/gripper_cmd" + "control_msgs/GripperCommandAction" + *gripper-action-timeout*) + + (actionlib-client:make-simple-action-client + 'gripper-right-right-finger + "gripper_right_right_finger/gripper_cmd" + "control_msgs/GripperCommandAction" + *gripper-action-timeout*) + + (actionlib-client:make-simple-action-client + 'gripper-left-left-finger + "gripper_left_left_finger/gripper_cmd" + "control_msgs/GripperCommandAction" + *gripper-action-timeout*) + + (actionlib-client:make-simple-action-client + 'gripper-left-right-finger + "gripper_left_right_finger/gripper_cmd" + "control_msgs/GripperCommandAction" + *gripper-action-timeout*)) + +(roslisp-utilities:register-ros-init-function make-gripper-action-clients) + +(defun make-gripper-action-goal (position max-effort) + (roslisp:make-message + 'control_msgs-msg:grippercommandgoal + (:position :command) position + (:max_effort :command) max-effort)) + +(defun call-one-gripper-action (arm + position + max-effort + &optional action-timeout) + (if (eq arm :left) + (cpl:par + (actionlib-client:call-simple-action-client + 'gripper-left-left-finger + :action-goal (make-gripper-action-goal position max-effort) + :action-timeout action-timeout) + (actionlib-client:call-simple-action-client + 'gripper-left-right-finger + :action-goal (make-gripper-action-goal position max-effort) + :action-timeout action-timeout)) + (cpl:par + (actionlib-client:call-simple-action-client + 'gripper-right-left-finger + :action-goal (make-gripper-action-goal position max-effort) + :action-timeout action-timeout) + (actionlib-client:call-simple-action-client + 'gripper-right-right-finger + :action-goal (make-gripper-action-goal position max-effort) + :action-timeout action-timeout)))) + +(defun ensure-gripper-goal-input (action-type-or-position arm max-effort) + (declare (type (or number keyword) action-type-or-position) + (type (or keyword list) arm)) + (let* ((bindings + (car (prolog:prolog + `(and (rob-int:robot ?robot) + (rob-int:gripper-joint ?robot ,arm ?gripper-joint) + (rob-int:joint-lower-limit ?robot ?gripper-joint ?lower-limit) + (rob-int:joint-upper-limit ?robot ?gripper-joint ?upper-limit) + (rob-int:gripper-meter-to-joint-multiplier ?robot ?mult))))) + (gripper-joint + (cut:var-value '?gripper-joint bindings)) + (gripper-lower-limit + (cut:var-value '?lower-limit bindings)) + (gripper-upper-limit + (cut:var-value '?upper-limit bindings)) + (gripper-multiplier + (cut:var-value '?mult bindings))) + (when (cut:is-var gripper-joint) + (error "[tiago gripper] Robot gripper joint was not defined.")) + (let* ((position + (etypecase action-type-or-position + (number + (cond + ((< (* gripper-multiplier action-type-or-position) + gripper-lower-limit) + (roslisp:ros-warn (giskard gripper) + "POSITION (~a) cannot be < ~a. Clipping." + (* gripper-multiplier action-type-or-position) + gripper-lower-limit) + (+ gripper-lower-limit *gripper-joint-limit-tiny-offset*)) + ((> (* gripper-multiplier action-type-or-position) + gripper-upper-limit) + (roslisp:ros-warn (giskard gripper) + "POSITION (~a) shouldn't be > ~a. Clipping." + (* gripper-multiplier action-type-or-position) + gripper-upper-limit) + (- gripper-upper-limit *gripper-joint-limit-tiny-offset*)) + (t + ;; in case the gripper is commanded in radian or so + (* gripper-multiplier action-type-or-position)))) + (keyword + (ecase action-type-or-position + (:open (- gripper-upper-limit *gripper-joint-limit-tiny-offset*)) + (:close (+ gripper-lower-limit *gripper-joint-limit-tiny-offset*)) + ;; (:grip (+ gripper-lower-limit *gripper-joint-limit-tiny-offset*)) + (:grip -1.0))))) + (effort (or max-effort + (if (eq action-type-or-position :grip) + 100 + 10)))) + (list position effort)))) + +(defun ensure-gripper-goal-reached (arm action-type-or-position joint-angle) + (when (and arm action-type-or-position joint-angle) + (let ((gripper-joints + (mapcar (lambda (bindings) + (cut:var-value '?gripper-joint bindings)) + (cut:force-ll + (prolog:prolog + `(and (rob-int:robot ?robot) + (rob-int:gripper-joint ?robot ,arm ?gripper-joint))))))) + (when (cut:is-var gripper-joints) + (error "[tiago-pm] Robot gripper joint was not defined.")) + (let ((current-states (joints:joint-positions gripper-joints)) + (goal-state joint-angle)) + ;; (roslisp:ros-info (tiago-pm grippers) "Gripper current state: ~a, ~ + ;; gripper goal: ~a." + ;; current-states goal-state) + (if (and (symbolp action-type-or-position) + (eq action-type-or-position :grip)) + (when (< (/ (+ (first current-states) (second current-states)) 2.0) + *gripper-gripped-min-position*) + (roslisp:ros-warn (tiago-pm gripper) + "Gripper closed completely, nothing grasped.") + ;; (cpl:fail + ;; (make-instance 'common-fail:gripper-closed-completely + ;; :description (format nil "Gripper ~a should have gripped,~%~ + ;; but it closed completely to ~a,~%~ + ;; which is below ~a." + ;; arm current-states + ;; *gripper-gripped-min-position*))) + ) + (unless (cram-tf:values-converged current-states + (list goal-state goal-state) + *gripper-convergence-delta*) + (cpl:fail + (make-instance 'common-fail:gripper-goal-not-reached + :description (format nil "Gripper did not converge to goal:~%~ + arm: ~a, current state: ~a, goal state: ~a, ~ + delta joint: ~a." + arm current-states goal-state + *gripper-convergence-delta*))))) + (roslisp:ros-info (tiago-pm grippers) "Gripper goal reached."))))) + + +(defun call-gripper-action (&key + action-timeout + action-type-or-position arm effort + check-goal) + (declare (type (or number null) action-timeout) + (type (or keyword list) arm) + (type (or number keyword) action-type-or-position) + (type (or number null) effort)) + + (unless (listp arm) + (setf arm (list arm))) + + (cpl:with-retry-counters ((gripping-retries 1)) + (cpl:with-failure-handling + + ((common-fail:gripper-low-level-failure (e) + (roslisp:ros-warn (tiago-pm grippers) "~a~%" e) + (cpl:do-retry gripping-retries + (roslisp:ros-warn (tiago-pm grippers) "Retrying.") + (cpl:sleep 1.0) + (cpl:retry)) + (roslisp:ros-warn (tiago-pm grippers) "Failing to action level."))) + + (mapcar (lambda (arm-element) + (let ((position-and-effort + (ensure-gripper-goal-input action-type-or-position arm-element effort))) + (multiple-value-bind (result status) + + (call-one-gripper-action + arm-element + (first position-and-effort) + (second position-and-effort) + action-timeout) + + (when (and result status check-goal) + (cpl:sleep 1.5) + + (ensure-gripper-goal-reached + arm-element + action-type-or-position + (first position-and-effort)) + :goal-not-achieved-yet)))) + arm)))) diff --git a/cram_tiago/cram_tiago_process_modules/src/grippers.lisp b/cram_tiago/cram_tiago_process_modules/src/grippers.lisp new file mode 100644 index 0000000000..4a286c7656 --- /dev/null +++ b/cram_tiago/cram_tiago_process_modules/src/grippers.lisp @@ -0,0 +1,223 @@ +;;; +;;; Copyright (c) 2022, Gayane Kazhoyan +;;; 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 Institute for Artificial Intelligence/ +;;; Universitaet Bremen 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 +;;; 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. + +(in-package :tiago-pm) + +(defparameter *gripper-action-timeout* 20.0 "in seconds") + +(defparameter *gripper-convergence-delta* 0.005 "in meters") +(defparameter *gripper-gripped-min-position* 0.007 "In meters.") +(defparameter *gripper-joint-limit-tiny-offset* 0.00 + "In meters, not to go too close to joint limits.") + +(cpm:def-process-module grippers-pm (motion-designator) + (destructuring-bind (command action-type-or-position which-gripper &optional effort) + (desig:reference motion-designator) + (ecase command + (cram-common-designators:move-gripper-joint + (call-gripper-action + :action-type-or-position action-type-or-position + :arm which-gripper + :effort effort + :check-goal t + :action-timeout *gripper-action-timeout*))))) + + +(defun make-gripper-action-clients () + (actionlib-client:make-simple-action-client + 'right-gripper + "gripper_right_controller/increment" + "teleop_tools_msgs/IncrementAction" + *gripper-action-timeout*) + + (actionlib-client:make-simple-action-client + 'left-gripper + "gripper_left_controller/increment" + "teleop_tools_msgs/IncrementAction" + *gripper-action-timeout*)) + +(roslisp-utilities:register-ros-init-function make-gripper-action-clients) + +(defun make-gripper-action-goal (deltas) + (roslisp:make-message + 'teleop_tools_msgs-msg:incrementgoal + :increment_by (map 'vector #'identity deltas))) + +(defun call-one-gripper-action (arm deltas action-timeout) + (actionlib-client:call-simple-action-client + (if (eq arm :left) + 'left-gripper + 'right-gripper) + :action-goal (make-gripper-action-goal deltas) + :action-timeout action-timeout)) + +(defun ensure-gripper-goal-input (action-type-or-position arm) + (declare (type (or number keyword) action-type-or-position) + (type (or keyword list) arm)) + (let* ((bindings + (car (prolog:prolog + `(and (rob-int:robot ?robot) + (rob-int:gripper-joint ?robot ,arm ?gripper-joint) + (rob-int:joint-lower-limit ?robot ?gripper-joint ?lower-limit) + (rob-int:joint-upper-limit ?robot ?gripper-joint ?upper-limit) + (rob-int:gripper-meter-to-joint-multiplier ?robot ?mult))))) + (gripper-joint + (cut:var-value '?gripper-joint bindings)) + (gripper-lower-limit + (cut:var-value '?lower-limit bindings)) + (gripper-upper-limit + (cut:var-value '?upper-limit bindings)) + (gripper-multiplier + (cut:var-value '?mult bindings))) + (when (cut:is-var gripper-joint) + (error "[tiago gripper] Robot gripper joint was not defined.")) + (let* ((position + (etypecase action-type-or-position + (number + (cond + ((< action-type-or-position gripper-lower-limit) + (roslisp:ros-warn (giskard gripper) + "POSITION (~a) cannot be < ~a. Clipping." + action-type-or-position gripper-lower-limit) + (+ gripper-lower-limit *gripper-joint-limit-tiny-offset*)) + ((> action-type-or-position gripper-upper-limit) + (roslisp:ros-warn (giskard gripper) + "POSITION (~a) shouldn't be > ~a. Clipping." + action-type-or-position gripper-upper-limit) + (- gripper-upper-limit *gripper-joint-limit-tiny-offset*)) + (t + ;; in case the gripper is commanded in radian or so + (* gripper-multiplier action-type-or-position)))) + (keyword + (ecase action-type-or-position + (:open (- gripper-upper-limit *gripper-joint-limit-tiny-offset*)) + (:close (+ gripper-lower-limit *gripper-joint-limit-tiny-offset*)) + ;; (:grip (+ gripper-lower-limit *gripper-joint-limit-tiny-offset*)) + (:grip -1.0))))) + (gripper-joints + (mapcar (lambda (bindings) + (cut:var-value '?gripper-joint bindings)) + (cut:force-ll + (prolog:prolog + `(and (rob-int:robot ?robot) + (rob-int:gripper-joint ?robot ,arm ?gripper-joint)))))) + (current-states + (joints:joint-positions gripper-joints)) + (current-state-left + (first current-states)) + (current-state-right + (second current-states)) + (delta-left + (- position current-state-left)) + (delta-right + (- position current-state-right))) + (list delta-left delta-right position)))) + +(defun ensure-gripper-goal-reached (arm action-type-or-position joint-angle) + (when (and arm action-type-or-position joint-angle) + (let ((gripper-joints + (mapcar (lambda (bindings) + (cut:var-value '?gripper-joint bindings)) + (cut:force-ll + (prolog:prolog + `(and (rob-int:robot ?robot) + (rob-int:gripper-joint ?robot ,arm ?gripper-joint))))))) + (when (cut:is-var gripper-joints) + (error "[tiago-pm] Robot gripper joint was not defined.")) + (let ((current-states (joints:joint-positions gripper-joints)) + (goal-state joint-angle)) + ;; (roslisp:ros-info (tiago-pm grippers) "Gripper current state: ~a, ~ + ;; gripper goal: ~a." + ;; current-states goal-state) + (if (and (symbolp action-type-or-position) + (eq action-type-or-position :grip)) + (when (< (/ (+ (first current-states) (second current-states)) 2.0) + *gripper-gripped-min-position*) + (cpl:fail + (make-instance 'common-fail:gripper-closed-completely + :description (format nil "Gripper ~a should have gripped,~%~ + but it closed completely to ~a,~%~ + which is below ~a." + arm current-states + *gripper-gripped-min-position*)))) + (unless (cram-tf:values-converged current-states + (list goal-state goal-state) + *gripper-convergence-delta*) + (cpl:fail + (make-instance 'common-fail:gripper-goal-not-reached + :description (format nil "Gripper did not converge to goal:~%~ + arm: ~a, current state: ~a, goal state: ~a, ~ + delta joint: ~a." + arm current-states goal-state + *gripper-convergence-delta*))))) + (roslisp:ros-info (tiago-pm grippers) "Gripper goal reached."))))) + + +(defun call-gripper-action (&key + action-timeout + action-type-or-position arm effort + check-goal) + (declare (type (or number null) action-timeout) + (type (or keyword list) arm) + (type (or number keyword) action-type-or-position) + (ignore effort)) + + (unless (listp arm) + (setf arm (list arm))) + + (cpl:with-retry-counters ((gripping-retries 1)) + (cpl:with-failure-handling + + ((common-fail:gripper-low-level-failure (e) + (roslisp:ros-warn (tiago-pm grippers) "~a~%" e) + (cpl:do-retry gripping-retries + (roslisp:ros-warn (tiago-pm grippers) "Retrying.") + (cpl:sleep 1.0) + (cpl:retry)) + (roslisp:ros-warn (tiago-pm grippers) "Failing to action level."))) + + (mapcar (lambda (arm-element) + (let ((offset-left-and-right-and-position + (ensure-gripper-goal-input action-type-or-position arm-element))) + (multiple-value-bind (result status) + + (call-one-gripper-action + arm-element + (subseq offset-left-and-right-and-position 0 2) + action-timeout) + + (when (and result status check-goal) + (cpl:sleep 1.5) + + (ensure-gripper-goal-reached + arm-element + action-type-or-position + (third offset-left-and-right-and-position)) + :goal-not-achieved-yet)))) + arm)))) diff --git a/cram_tiago/cram_tiago_process_modules/src/interface.lisp b/cram_tiago/cram_tiago_process_modules/src/interface.lisp index 57faacbb96..92a08eab9a 100644 --- a/cram_tiago/cram_tiago_process_modules/src/interface.lisp +++ b/cram_tiago/cram_tiago_process_modules/src/interface.lisp @@ -41,17 +41,38 @@ (desig:desig-prop ?motion-designator (:type :going)) (desig:desig-prop ?motion-designator (:type :moving-torso)) (desig:desig-prop ?motion-designator (:type :looking)) - (desig:desig-prop ?motion-designator (:type :gripping)) + ;; (desig:desig-prop ?motion-designator (:type :gripping)) + ;; (desig:desig-prop ?motion-designator (:type :opening-gripper)) + ;; (desig:desig-prop ?motion-designator (:type :closing-gripper)) + ;; (desig:desig-prop ?motion-designator (:type :moving-gripper-joint)) + )) + + (<- (cpm:matching-process-module ?motion-designator grippers-pm) + (or (desig:desig-prop ?motion-designator (:type :gripping)) + (desig:desig-prop ?motion-designator (:type :opening-gripper)) + (desig:desig-prop ?motion-designator (:type :closing-gripper)) + (desig:desig-prop ?motion-designator (:type :moving-gripper-joint)))) + + (<- (cpm:matching-process-module ?motion-designator grippers-via-fingers-pm) + (or (desig:desig-prop ?motion-designator (:type :gripping)) (desig:desig-prop ?motion-designator (:type :opening-gripper)) (desig:desig-prop ?motion-designator (:type :closing-gripper)) (desig:desig-prop ?motion-designator (:type :moving-gripper-joint)))) - (<- (cpm:available-process-module giskard:giskard-pm) + (<- (cpm:available-process-module ?pm) + (member ?pm (grippers-pm grippers-via-fingers-pm giskard:giskard-pm)) (not (cpm:projection-running ?_)))) (defmacro with-real-robot (&body body) `(cram-process-modules:with-process-modules-running - (rk:robokudo-perception-pm giskard:giskard-pm joints:joint-state-pm + (rk:robokudo-perception-pm giskard:giskard-pm grippers-pm joints:joint-state-pm btr-belief:world-state-detecting-pm common-desig:wait-pm) (cpl-impl::named-top-level (:name :top-level) ,@body))) + +(defmacro with-mujoco-robot (&body body) + `(cram-process-modules:with-process-modules-running + (giskard:giskard-pm grippers-via-fingers-pm joints:joint-state-pm + btr-belief:world-state-detecting-pm common-desig:wait-pm) + (cpl-impl::named-top-level (:name :top-level) + ,@body))) diff --git a/cram_tiago/cram_tiago_process_modules/src/package.lisp b/cram_tiago/cram_tiago_process_modules/src/package.lisp index 9ce524b3db..51f21c4fc4 100644 --- a/cram_tiago/cram_tiago_process_modules/src/package.lisp +++ b/cram_tiago/cram_tiago_process_modules/src/package.lisp @@ -34,4 +34,5 @@ (:use #:common-lisp #:cram-prolog) (:export ;; interface - #:with-real-robot)) + #:with-real-robot + #:with-mujoco-robot)) diff --git a/eurobin.rosinstall b/eurobin.rosinstall new file mode 100644 index 0000000000..89f2d31be9 --- /dev/null +++ b/eurobin.rosinstall @@ -0,0 +1,28 @@ +- git: + local-name: cram + uri: http://github.com/artnie/cram.git + version: eurobin-demo +- 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.1 +- 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: AddWindows +- git: + local-name: robokudo_msgs + uri: http://gitlab.informatik.uni-bremen.de/robokudo/robokudo_msgs.git + version: main +- git: + local-name: iai_tiago + uri: http://github.com/code-iai/iai_tiago.git + version: main \ No newline at end of file diff --git a/jupyter/README.md b/jupyter/README.md new file mode 100644 index 0000000000..ee1ab81186 --- /dev/null +++ b/jupyter/README.md @@ -0,0 +1,14 @@ +# common-lisp-jupyter + +* Install cram to you workspace by following [this guide](https://cram-system.org/installation) +* Verify your installation by running `roslisp_repl` and loading a tutorial, as mentioned in CRAM's general README +* Follow [this guide](https://cram-system.org/tutorials/advanced/jupyter) to install the Python dependencies for the common-lisp-jupyter kernel + * it will use the rosinstall file in this directory to load the necessary lisp projects into your workspace +* When your workspace is set up with the jupyter deps, install the kernel via sbcl by loading the installer: + +```bash +/usr/bin/sbcl --dynamic-space-size 4096 --load sbcl-jupyter-kernel-installer.lisp +``` + +* Check if the kernel is placed at the right spot. Go to `~/.local/share/jupyter/` and look for `common-lisp`, there should be a `kernel.json`. +* Verify that common-lisp-jupyter works by running `jupyter-lab`. It should load the common-lisp kernel. \ No newline at end of file diff --git a/jupyter/sbcl-jupyter-kernel-installer.lisp b/jupyter/sbcl-jupyter-kernel-installer.lisp new file mode 100644 index 0000000000..15c2e1f499 --- /dev/null +++ b/jupyter/sbcl-jupyter-kernel-installer.lisp @@ -0,0 +1,18 @@ +;; When the dependencies for common-lisp-jupyter are prepared, +;; load this file to install the jupyter kernel with +;; /usr/bin/sbcl --dynamic-space-size 4096 --load sbcl-kernel-installer.lisp + +(load (parse-namestring (concatenate 'string (sb-ext:posix-getenv "ROS_ROOT") "lisp/scripts/roslisp-sbcl-init"))) +(asdf:load-system :common-lisp-jupyter) +(defmethod jupyter:command-line :around ((instance jupyter/common-lisp::user-installer)) + (let* ((original-command-line (call-next-method)) + (additional-entries `("--dynamic-space-size" + "4096" + ,jupyter/common-lisp::+eval-flag+ + "(load (parse-namestring (concatenate 'string (sb-ext:posix-getenv \"ROS_ROOT\") \"lisp/scripts/roslisp-sbcl-init\")))"))) + (concatenate 'list + (subseq original-command-line 0 1) + additional-entries + (subseq original-command-line 1)))) +(cl-jupyter:install) +(quit)