From 0c4044b3a6dc45e6bd051257c1388c1ee9449c49 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Wed, 10 Aug 2022 17:20:15 +0200 Subject: [PATCH 01/97] [desig] prettier pretty-printer for designators --- .../cram-designators/designator-protocol.lisp | 26 +++---------------- 1 file changed, 4 insertions(+), 22 deletions(-) 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)) From 0b1f52e019d9d576e196335fcb4705ef0c78cc50 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Wed, 17 Aug 2022 17:33:32 +0200 Subject: [PATCH 02/97] [btr-belief] OBJECT-IN-HAND should not return :left-or-right as hand if hand is unbound. --- .../cram_bullet_reasoning_belief_state/src/occasions.lisp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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..7f162ef945 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 @@ -87,7 +87,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))) From 7b8398cc1cac66a3f3061513226890e5549c2fd4 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 22 Aug 2022 22:45:59 +0200 Subject: [PATCH 03/97] [kdl_service] in 20.04 python_orocos_kdl became a Debian non-ROS package dependency --- cram_external_interfaces/kdl_ik_service/package.xml | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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 From 84d89e5c30b226ba1dcda032a72e536e9e6b039c Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 23 Aug 2022 00:35:09 +0200 Subject: [PATCH 04/97] GRASPS and DOOR-JOINT-POSE properties for environment manipulation --- .../src/fetch-and-deliver-designators.lisp | 5 +++++ .../src/fetch-and-deliver-plans.lisp | 3 +++ .../src/action-designators.lisp | 13 +++++++++++-- .../src/plans.lisp | 3 +++ .../cram_common_designators/src/motions.lisp | 5 ++++- .../src/atomic-action-designators.lisp | 3 +++ 6 files changed, 29 insertions(+), 3 deletions(-) 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..12661e0a76 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 @@ -113,6 +113,10 @@ the `look-pose-stamped'." (-> (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))) @@ -129,6 +133,7 @@ the `look-pose-stamped'." (:object-accessible ?object-accessible) (:object-location ?object-location-desig) (:arms ?arms) + (:grasps ?grasps) (:distance ?distance) (:robot-location ?robot-location)) ?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..7ae911dbed 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 @@ -111,6 +111,7 @@ turn the robot base such that it looks in the direction of target and look again object-accessible ((:object-location ?object-location)) ((:arms all-arms)) + ((:grasps ?grasps)) ((:distance ?distance)) ((:robot-location ?manipulate-robot-location)) &allow-other-keys) @@ -212,6 +213,7 @@ if yes, relocate and retry, if no collisions, open or close container." (desig:an action (type opening) (arm ?arm) + (grasps ?grasps) (object ?object-to-manipulate) (desig:when ?distance (distance ?distance)) @@ -224,6 +226,7 @@ if yes, relocate and retry, if no collisions, open or close container." (desig:an action (type closing) (arm ?arm) + (grasps ?grasps) (object ?object-to-manipulate) (desig:when ?distance (distance ?distance)) 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/plans.lisp b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp index 50adfe1b51..6f6cebc4c0 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)) @@ -152,6 +153,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) diff --git a/cram_common/cram_common_designators/src/motions.lisp b/cram_common/cram_common_designators/src/motions.lisp index 3e3035ed60..9884d2dd60 100644 --- a/cram_common/cram_common_designators/src/motions.lisp +++ b/cram_common/cram_common_designators/src/motions.lisp @@ -133,7 +133,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 +156,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_mobile_pick_place_plans/src/atomic-action-designators.lisp b/cram_common/cram_mobile_pick_place_plans/src/atomic-action-designators.lisp index dce1698988..891e3b3aff 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 @@ -223,6 +223,8 @@ (spec:property ?action-designator (:link ?handle-link)) (once (or (spec:property ?action-designator (:distance ?joint-angle)) (equal ?joint-angle NIL))) + (once (or (spec:property ?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 +238,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)) From 98c04c25661c28facbd80a86daa5420ce913fac2 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 23 Aug 2022 00:35:38 +0200 Subject: [PATCH 05/97] [cram_tf] *robot-arm-wrist-frame* global var --- cram_common/cram_tf/src/package.lisp | 1 + cram_common/cram_tf/src/setup.lisp | 19 ++++++++++++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) 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) From 0c6971449add2e97f9548fd6b57e83be67d613b4 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 23 Aug 2022 00:41:03 +0200 Subject: [PATCH 06/97] [giskard] tiago stuff and commented out align planes --- .../cram_giskard/src/arm-goals.lisp | 113 +++++++++++++----- .../cram_giskard/src/base-goals.lisp | 24 ++-- .../src/environment-manipulation-goals.lisp | 84 ++++++++----- .../cram_giskard/src/gripper-goals.lisp | 25 ++-- .../src/making-goal-messages.lisp | 54 ++++++++- .../cram_giskard/src/process-module.lisp | 6 +- 6 files changed, 222 insertions(+), 84 deletions(-) diff --git a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index cb3b111e42..fc38128792 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -85,22 +85,36 @@ ;; 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-unmovable-joints-constraint + (append + unmovable-joints + (when (eq (rob-int:get-robot-name) :tiago-dual) + (mapcar (lambda (binds) + (cut:var-value '?joint binds)) + (cut:force-ll + (prolog:prolog + `(and (rob-int:robot ?robot-name) + (rob-int:gripper-joint ?robot-name ?_ ?joint))))))))) (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))) + :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*)))) :cartesian-constraints (list (when left-pose (make-cartesian-constraint pose-base-frame - cram-tf:*robot-left-tool-frame* + cram-tf:*robot-left-wrist-frame* left-pose)) (when right-pose (make-cartesian-constraint pose-base-frame - cram-tf:*robot-right-tool-frame* + cram-tf:*robot-right-wrist-frame* right-pose))) :collisions (ecase collision-mode (:avoid-all (make-avoid-all-collision)) @@ -114,11 +128,11 @@ 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))))) + (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) (make-allow-arm-collision @@ -128,7 +142,7 @@ arms (rob-int:get-environment-name))))) (:allow-attached (make-avoid-all-collision) ; attached objects are handled by giskard - ))))) + ))))) (defun make-arm-joint-action-goal (joint-state-left joint-state-right align-planes-left align-planes-right @@ -161,33 +175,68 @@ :max-velocity *base-max-velocity-slow-xy* ;; :avoid-collisions-much t ) - (when align-planes-left - (make-align-planes-tool-frame-constraint - :left - (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-tool-frame-constraint - :right - (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-left + ;; (make-align-planes-tool-frame-constraint + ;; :left + ;; (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-tool-frame-constraint + ;; :right + ;; (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)))) + ) :joint-constraints (list (make-joint-constraint joint-state-left) (make-joint-constraint joint-state-right)) :collisions (list (make-avoid-all-collision)))) -(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))) (defun ensure-arm-joint-goal-input (goal-configuration arm) (if (and (listp goal-configuration) @@ -257,9 +306,9 @@ (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) diff --git a/cram_external_interfaces/cram_giskard/src/base-goals.lisp b/cram_external_interfaces/cram_giskard/src/base-goals.lisp index 9d208c47d2..cdbaec96ab 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -54,15 +54,21 @@ (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 (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-velocity *base-max-velocity-fast-xy*) + (make-cartesian-constraint + cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose + :avoid-collisions-much t + :max-velocity *base-max-velocity-fast-xy*)) + (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*))) (if (eq base-velocity :slow) (make-base-velocity-constraint *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) 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..5ce95cef21 100644 --- a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp @@ -31,11 +31,13 @@ (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 @@ -44,6 +46,8 @@ *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) (make-open-or-close-constraint open-or-close arm handle-link joint-state) + (when (eq (rob-int:get-robot-name) :tiago-dual) + (make-diffdrive-base-arch-constraint hinge-point-stamped)) (make-avoid-joint-limits-constraint) (make-head-pointing-at-hand-constraint arm)) :collisions (make-constraints-vector @@ -57,35 +61,55 @@ 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)) - (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.")))))) + (let ((joint-point-stamped (cl-transforms-stamped:make-point-stamped + cram-tf:*fixed-frame* + 0.0 + (cl-transforms:origin joint-pose)))) + + (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..796ff71984 100644 --- a/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp @@ -42,12 +42,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))))) @@ -119,7 +121,7 @@ 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 +131,14 @@ (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 () + ;; (ensure-gripper-goal-reached arm goal-joint-angle)) + )) + 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..f35fbba96d 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) @@ -279,6 +281,25 @@ ,@(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) + (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" . ,door-joint-point-stamped-in-map))))) + +(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 +)) + (defun make-grasp-bar-constraint (arm root-link tip-grasp-axis bar-axis bar-center bar-length) @@ -341,8 +362,37 @@ :weight_above_ca ; that's the default weight anyway )))))))) +(defun make-diffdrive-base-goal (root-frame tip-frame goal-pose + &key max-velocity avoid-collisions-much) + (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)) + (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 max-velocity + `(("max_linear_velocity" . ,max-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-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) diff --git a/cram_external_interfaces/cram_giskard/src/process-module.lisp b/cram_external_interfaces/cram_giskard/src/process-module.lisp index 7e5e98cb9e..6e5aa3eede 100644 --- a/cram_external_interfaces/cram_giskard/src/process-module.lisp +++ b/cram_external_interfaces/cram_giskard/src/process-module.lisp @@ -58,14 +58,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 11 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 11 rest-args))) (cram-common-designators:move-head (when argument-1 (giskard:call-neck-action From ef4fed97e99bc970301f1624beb7db2aaadb0a30 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 23 Aug 2022 22:35:44 +0200 Subject: [PATCH 07/97] [giskard] making goals properly --- .../cram_giskard/src/making-goal-messages.lisp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) 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 f35fbba96d..aec293d095 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -289,7 +289,10 @@ "DiffDriveTangentialToPoint" :parameter_value_pair (alist->json-string - `(("goal_point" . ,door-joint-point-stamped-in-map))))) + `(("goal_point" + . (("message_type" . "geometry_msgs/PointStamped") + ("message" . ,(to-hash-table + door-joint-point-stamped-in-map)))))))) (defun make-diffdrive-cartesian-goal-arm-constraint (tip-link) (declare (type string tip-link)) @@ -298,7 +301,8 @@ :type "KeepHandInWorkspace" :parameter_value_pair -)) + (alist->json-string + `(("tip_link" . ,tip-link))))) (defun make-grasp-bar-constraint (arm root-link tip-grasp-axis From 691dcb50ee48ddf5b0e50806579ac2b644113774 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 23 Aug 2022 22:36:24 +0200 Subject: [PATCH 08/97] [giskard] (fifth list) == (nth 4 list) --- cram_external_interfaces/cram_giskard/src/process-module.lisp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cram_external_interfaces/cram_giskard/src/process-module.lisp b/cram_external_interfaces/cram_giskard/src/process-module.lisp index 6e5aa3eede..9c4fdcab37 100644 --- a/cram_external_interfaces/cram_giskard/src/process-module.lisp +++ b/cram_external_interfaces/cram_giskard/src/process-module.lisp @@ -59,7 +59,7 @@ :handle-link (fifth rest-args) :joint-angle (second rest-args) :prefer-base (eighth rest-args) - :joint-pose (nth 11 rest-args))) + :joint-pose (nth 10 rest-args))) (cram-common-designators:move-arm-push (giskard:call-environment-manipulation-action :open-or-close :close @@ -67,7 +67,7 @@ :handle-link (fifth rest-args) :joint-angle (second rest-args) :prefer-base (eighth rest-args) - :joint-pose (nth 11 rest-args))) + :joint-pose (nth 10 rest-args))) (cram-common-designators:move-head (when argument-1 (giskard:call-neck-action From dfec34f931db623a8a45656fe8b83b2bfd33cdab Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 23 Aug 2022 22:39:36 +0200 Subject: [PATCH 09/97] [env-manip] added 2nd pregrasp pose for handle --- .../src/trajectories.lisp | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) 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..57d22f643f 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/trajectories.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/trajectories.lisp @@ -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 From 93eb92ba15762f422a07b3204285888d1e56277e Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 23 Aug 2022 22:50:21 +0200 Subject: [PATCH 10/97] [obj-know] defined 2nd pregrasps for objects of type handle --- .../cram_object_knowledge/src/environment.lisp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/cram_demos/cram_object_knowledge/src/environment.lisp b/cram_demos/cram_object_knowledge/src/environment.lisp index 4e12ec0ebb..b85e5e6a52 100644 --- a/cram_demos/cram_object_knowledge/src/environment.lisp +++ b/cram_demos/cram_object_knowledge/src/environment.lisp @@ -109,7 +109,9 @@ (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-2nd-pregrasp-x-offset-open* 0.01 "in meters") (defparameter *handle-pregrasp-y-offset-open* 0.05 "in meters") +(defparameter *handle-2nd-pregrasp-y-offset-open* 0.01 "in meters") (defparameter *handle-pregrasp-x-offset-close* -0.0 "in meters") (defparameter *handle-retract-offset* 0.05 "in meters") @@ -118,7 +120,7 @@ :grasp-translation `(0.0d0 ,(- *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) + :2nd-pregrasp-offsets `(0.0 ,*handle-2nd-pregrasp-y-offset-open* 0.0) :lift-translation '(0 0 0) :2nd-lift-translation '(0 0 0)) @@ -126,7 +128,7 @@ :grasp-translation `(0.0d0 ,*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) + :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 +137,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)) @@ -144,7 +146,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 +155,6 @@ :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)) From 5241a7607be257939a73d007d9de1bbddfce4f8b Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 23 Aug 2022 23:08:21 +0200 Subject: [PATCH 11/97] [boxy-desc] apparently boxy's finger length changed --- cram_robot_descriptions/cram_boxy_description/src/arms.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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* From d3f8b3bc07ac917c60069d59575bba5efd64006f Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 23 Aug 2022 23:09:07 +0200 Subject: [PATCH 12/97] [obj-know] arm goal achieved delta is 1 cm now, so tweaked handle pregrasp poses --- cram_demos/cram_object_knowledge/src/environment.lisp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cram_demos/cram_object_knowledge/src/environment.lisp b/cram_demos/cram_object_knowledge/src/environment.lisp index b85e5e6a52..a08b8b84b8 100644 --- a/cram_demos/cram_object_knowledge/src/environment.lisp +++ b/cram_demos/cram_object_knowledge/src/environment.lisp @@ -108,10 +108,10 @@ (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-2nd-pregrasp-x-offset-open* 0.01 "in meters") -(defparameter *handle-pregrasp-y-offset-open* 0.05 "in meters") -(defparameter *handle-2nd-pregrasp-y-offset-open* 0.01 "in meters") +(defparameter *handle-pregrasp-x-offset-open* 0.06 "in meters") +(defparameter *handle-2nd-pregrasp-x-offset-open* 0.02 "in meters") +(defparameter *handle-pregrasp-y-offset-open* 0.06 "in meters") +(defparameter *handle-2nd-pregrasp-y-offset-open* 0.02 "in meters") (defparameter *handle-pregrasp-x-offset-close* -0.0 "in meters") (defparameter *handle-retract-offset* 0.05 "in meters") From 920cf7681dc02360996faf63138ddf7f8b64637a Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 23 Aug 2022 23:09:40 +0200 Subject: [PATCH 13/97] [btr-belief] arm goal achieved delta is now 1 cm instead of previously 2 cm --- .../cram_bullet_reasoning_belief_state/src/occasions.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7f162ef945..cf3fa52e14 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 @@ -33,7 +33,7 @@ (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.") From e3336d3a117ae9d608cb44a011c4fbda6fa5f723 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 23 Aug 2022 23:11:59 +0200 Subject: [PATCH 14/97] [fd-plans] don't add GRASPS property if the value is NIL. --- .../src/fetch-and-deliver-plans.lisp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 7ae911dbed..e40f6d907b 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 @@ -213,7 +213,8 @@ if yes, relocate and retry, if no collisions, open or close container." (desig:an action (type opening) (arm ?arm) - (grasps ?grasps) + (desig:when ?grasps + (grasps ?grasps)) (object ?object-to-manipulate) (desig:when ?distance (distance ?distance)) @@ -226,7 +227,8 @@ if yes, relocate and retry, if no collisions, open or close container." (desig:an action (type closing) (arm ?arm) - (grasps ?grasps) + (desig:when ?grasps + (grasps ?grasps)) (object ?object-to-manipulate) (desig:when ?distance (distance ?distance)) From ac3fe5d13d176d4430453f6871d0e41743741f8f Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Wed, 24 Aug 2022 22:16:01 +0200 Subject: [PATCH 15/97] [giskard goal-msgs] fixed Arthur'sbug and added optional params to 2 goals --- .../src/making-goal-messages.lisp | 35 ++++++++++++------- 1 file changed, 23 insertions(+), 12 deletions(-) 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 aec293d095..133b8b751d 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -121,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 @@ -281,7 +286,8 @@ ,@(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) +(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 @@ -292,7 +298,11 @@ `(("goal_point" . (("message_type" . "geometry_msgs/PointStamped") ("message" . ,(to-hash-table - door-joint-point-stamped-in-map)))))))) + 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)) @@ -623,11 +633,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 @@ -649,7 +660,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 From 96c22bd79af275238760aa0f0fdef61d0892a671 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Thu, 25 Aug 2022 23:39:41 +0200 Subject: [PATCH 16/97] [fd-plans] propagated parameters of TRANSPORTING's children up to top level --- .../src/fetch-and-deliver-designators.lisp | 154 ++++++++++++++++-- .../src/fetch-and-deliver-plans.lisp | 122 ++++++++++++-- 2 files changed, 248 insertions(+), 28 deletions(-) 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 12661e0a76..31f5d3c6e8 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,9 +106,32 @@ 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))) + ;; 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) + (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 ?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) @@ -120,22 +143,20 @@ the `look-pose-stamped'." ;; distance (once (or (spec:property ?action-designator (:distance ?distance)) (equal ?distance NIL))) - ;; robot-location - (once (or (and (spec:property ?action-designator (:robot-location - ?some-robot-location)) - (desig:current-designator ?robot-location ?robot-location)) - (desig:designator :location ((:reachable-for ?robot) - ;; (:arm ?arm) - (:object ?object-designator)) - ?robot-location))) + (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) (:grasps ?grasps) - (:distance ?distance) - (:robot-location ?robot-location)) + (:distance ?distance)) ?resolved-action-designator)) @@ -335,12 +356,104 @@ 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 / seal search outer grasps + (-> (desig:desig-prop ?action-designator + (:access-seal-search-outer-grasps ?access-seal-search-outer-grasps)) + (true) + (equal ?access-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)) @@ -355,6 +468,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)) @@ -372,17 +486,37 @@ 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-seal-search-outer-grasps ?access-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-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 e40f6d907b..6427330cee 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,22 +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." @@ -134,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 @@ -143,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 @@ -155,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) + :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...")))) @@ -193,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 @@ -246,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)))))) @@ -747,11 +764,32 @@ 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-seal-search-outer-grasps ?access-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) @@ -766,6 +804,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 @@ -780,6 +830,18 @@ 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-seal-search-outer-grasps + (outer-grasps ?access-seal-search-outer-grasps)) (goal ?goal)))) ;; search for the object to find it's exact pose @@ -789,8 +851,8 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (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) @@ -854,6 +916,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 ?access-seal-search-outer-grasps + (outer-grasps ?access-seal-search-outer-grasps)) (goal ?goal)))) ;; reset the target location @@ -861,6 +935,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)) From aa902921e1289be6bfc0f340cb645272c2ff04ee Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Thu, 25 Aug 2022 23:40:38 +0200 Subject: [PATCH 17/97] [urdf-proj] when GRIPPING in projection, close gripper to 0.5 cm not 0 --- cram_3d_world/cram_urdf_projection/src/low-level.lisp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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 From 966aa6e1ac0b5b23dddf315e40e1d38c0529fc2c Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Thu, 25 Aug 2022 23:44:11 +0200 Subject: [PATCH 18/97] [proj-demos] apartment demo --- .../src/apartment-demo.lisp | 322 ++++++++++-------- 1 file changed, 182 insertions(+), 140 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index a4b8d9d62c..bccac7a97f 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -32,7 +32,7 @@ (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) (0 1 0 0))) (:cup "cabinet1_coloksu_level4" ((0.15 -0.1 0.08) (0 0 -1 0))))) @@ -53,7 +53,7 @@ 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) @@ -76,143 +76,185 @@ (defun apartment-demo (&key (step 0)) - (urdf-proj:with-simulated-robot - (when (<= step 0) - (initialize-apartment) - (setf btr:*visibility-threshold* 0.7) - (when cram-projection:*projection-environment* - (spawn-objects-on-fixed-spots - :object-types '(:jeroen-cup :cup) - :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) - (exe:perform - (an action - (type transporting) - (object (an object - (type jeroen-cup) - (location ?location-in-cupboard))) - (target ?location-on-island)))) - - ;; put cup from island into dishwasher - (when (<= step 2) - (exe:perform - (an action - (type transporting) - (object (an object - (type jeroen-cup) - (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))) - (exe:perform - (an action - (type transporting) - (object (an object - (type jeroen-cup) - (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) - (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)))))) + ;;urdf-proj:with-simulated-robot + (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) + + (setf proj-reasoning::*projection-checks-enabled* nil) + + (setf btr:*visibility-threshold* 0.7) + (when cram-projection:*projection-environment* + (spawn-objects-on-fixed-spots + :object-types '(:jeroen-cup :cup) + :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))))) + + + ;; hard-coded stuff for real-world demo + (?accessing-cupboard-door-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-robot-location + (a location + (pose ?accessing-cupboard-door-pose))) + (?accessing-dishwasher-door-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.6 3.5 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 0 1))) + (?detecting-cupboard-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.2624905511520543d0 2.002821242371188d0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.980183726892722d0 0.19809053873088875d0)))) + + ;; bring cup from cupboard to table + (when (<= step 1) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (location ?location-in-cupboard))) + (access-seal-search-outer-arms (left)) + (access-seal-search-outer-grasps (right-side)) + (access-search-outer-robot-location ?accessing-cupboard-door-robot-location) + (search-robot-location (a location + (pose ?detecting-cupboard-pose))) + (fetch-robot-location (a location + (pose ?detecting-cupboard-pose))) + (arms (left)) + (grasps (front)) + (target ?location-on-island)))) + + ;; put cup from island into dishwasher + (when (<= step 2) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (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))) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (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) + (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))))) (finalize)) From 20d1b4f8e973cfa499f9f5ed8423dd5adaeec71f Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 14:55:57 +0200 Subject: [PATCH 19/97] [bs] perception pose correction trigger and axis correction threshold are separate numbers now. --- .../src/consistency-checking.lisp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) 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..436b855e9e 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* 10 "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)...)." - From 713ebd68491fb99dfb91486f6d142cf29028ddd7 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 14:58:14 +0200 Subject: [PATCH 20/97] [bs] OBJECT-AT-LOCATION works with location desigs by comparing one grounding to obj pose. --- .../src/occasions.lisp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) 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 cf3fa52e14..04d893153d 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 @@ -37,6 +38,8 @@ (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 @@ -507,6 +510,26 @@ (spec:property ?current-robot-designator (:part-of ?robot-name)) (rob-int:robot ?robot-name)) + ;; compare the exact poses + (<- (%object-at-location ?world ?object ?location-designator) + (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) (btr:bullet-world ?world) From 13e6974504d911362da96aeb80b8640ddaefa285 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 16:09:22 +0200 Subject: [PATCH 21/97] [fd-plans] split access-seal-search-outer-grasps + better goals for SEARCHING & FETCHING. --- .../src/fetch-and-deliver-designators.lisp | 19 +++++++++++++++---- .../src/fetch-and-deliver-plans.lisp | 19 +++++++++++-------- 2 files changed, 26 insertions(+), 12 deletions(-) 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 31f5d3c6e8..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 @@ -379,6 +379,7 @@ the `look-pose-stamped'." (: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)) @@ -424,6 +425,7 @@ the `look-pose-stamped'." (: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)) @@ -441,11 +443,17 @@ the `look-pose-stamped'." (:access-seal-search-outer-arms ?access-seal-search-outer-arms)) (true) (equal ?access-seal-search-outer-arms NIL)) - ;; access / seal search outer grasps + ;; access search outer grasps (-> (desig:desig-prop ?action-designator - (:access-seal-search-outer-grasps ?access-seal-search-outer-grasps)) + (:access-search-outer-grasps ?access-search-outer-grasps)) (true) - (equal ?access-seal-search-outer-grasps NIL)) + (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 @@ -501,12 +509,15 @@ the `look-pose-stamped'." (: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-seal-search-outer-grasps ?access-seal-search-outer-grasps) + (: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) 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 6427330cee..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 @@ -169,7 +169,7 @@ if yes, relocate and retry, if no collisions, open or close container." ?robot-location-with-arm relocation-retries (:error-object-or-string e - :warning-namespace (fd-plans environment) + :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! @@ -772,7 +772,8 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat ((: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-seal-search-outer-grasps ?access-seal-search-outer-grasps)) + ((: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)) @@ -840,12 +841,13 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (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-seal-search-outer-grasps - (outer-grasps ?access-seal-search-outer-grasps)) + (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) @@ -874,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 @@ -926,8 +929,8 @@ If a failure happens, try a different `?target-location' or `?target-robot-locat (outer-robot-location ?seal-search-outer-robot-location)) (desig:when ?access-seal-search-outer-arms (outer-arms ?access-seal-search-outer-arms)) - (desig:when ?access-seal-search-outer-grasps - (outer-grasps ?access-seal-search-outer-grasps)) + (desig:when ?seal-search-outer-grasps + (outer-grasps ?seal-search-outer-grasps)) (goal ?goal)))) ;; reset the target location From fed350a29fd3e81b74c698162f7032214e7cca34 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 16:14:29 +0200 Subject: [PATCH 22/97] [env-manip] APPLICATION-CONTEXT parameter for REACHING & RETRACTING --- cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp | 2 ++ 1 file changed, 2 insertions(+) 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 6f6cebc4c0..a95ef60e2b 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp @@ -100,6 +100,7 @@ (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-failure-handling @@ -204,6 +205,7 @@ (type retracting) (left-poses ?left-retract-poses) (right-poses ?right-retract-poses) + (application-context ?type) (goal ?goal))))) (exe:perform (desig:an action From 1fe3872e75aa7753c3022c6117c887befafaad9c Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 16:23:16 +0200 Subject: [PATCH 23/97] [env-manip] ignore gripping failure when closing doors / drawers. --- .../src/plans.lisp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) 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 a95ef60e2b..9619dba53d 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp @@ -124,12 +124,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) From d183dfeef871dc6abe835a121f965b0e4eb3efd9 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 16:29:47 +0200 Subject: [PATCH 24/97] [proj-reason] allow collisions between the fingers and the handle when projecting env manip --- .../cram_urdf_projection_reasoning/src/check-collisions.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..73c75a8417 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 @@ -483,7 +483,7 @@ 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) - (list :avoid-all :avoid-all :allow-all :allow-all)) + (list :avoid-all :allow-fingers :allow-all :allow-all)) ;; (when (eq (desig:desig-prop-value action-desig :type) :opening) ;; (when (btr:robot-colliding-objects-without-attached) ;; (roslisp:ros-warn (coll-check environment) From ca27600ba49b2fab784dcac6daa1dcfe017e6bf2 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 16:41:59 +0200 Subject: [PATCH 25/97] [comm-desig + pp-plans] STRAIGHT-LINE symbolic constraint + fixed for DOOR-JOINT-POSE parameter. --- .../cram_common_designators/src/motions.lisp | 5 ++++- .../src/atomic-action-designators.lisp | 14 ++++++++++++-- .../src/atomic-action-plans.lisp | 15 ++++++++++++--- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/cram_common/cram_common_designators/src/motions.lisp b/cram_common/cram_common_designators/src/motions.lisp index 9884d2dd60..f29119a14e 100644 --- a/cram_common/cram_common_designators/src/motions.lisp +++ b/cram_common/cram_common_designators/src/motions.lisp @@ -101,7 +101,8 @@ ?collision-object-a ?move-base ?prefer-base ?align-planes-left - ?align-planes-right)) + ?align-planes-right + ?straight-line)) (property ?designator (:type :moving-tcp)) (once (or (property ?designator (:left-pose ?left-pose)) (equal ?left-pose nil))) @@ -120,6 +121,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 (: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)) 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 891e3b3aff..05722714d0 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 @@ -111,11 +111,15 @@ (equal ?collision :avoid-all))) (infer-motion-flags ?action-designator ?_ ?move-base ?align-planes-left ?align-planes-right) + (-> (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)) @@ -132,11 +136,16 @@ (equal ?collision :allow-hand))) (infer-motion-flags ?action-designator ?_ ?move-base ?align-planes-left ?align-planes-right) + ;; (-> (or (desig:desig-prop ?action-designator (:application-context :opening)) + ;; (desig:desig-prop ?action-designator (:application-context :closing))) + ;; (equal ?straight-line T) + ;; (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 T) (:align-planes-left ?align-planes-left) (:align-planes-right ?align-planes-right)) ?resolved-action-designator)) @@ -170,6 +179,7 @@ (:collision-object-b-link ?object-link) (:prefer-base ?prefer-base) (:move-base ?move-base) + (:straight-line T) (:align-planes-left ?align-planes-left) (:align-planes-right ?align-planes-right)) ?resolved-action-designator)) @@ -197,7 +207,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,7 +233,7 @@ (spec:property ?action-designator (:link ?handle-link)) (once (or (spec:property ?action-designator (:distance ?joint-angle)) (equal ?joint-angle NIL))) - (once (or (spec:property ?action-designator (:door-joint-pose ?door-joint-pose)) + (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 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..a6d629210e 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,6 +90,7 @@ ((: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)) &allow-other-keys) @@ -97,7 +98,8 @@ (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)) "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." @@ -137,6 +139,8 @@ while ignoring failures; and execute the last pose with propagating the failures (move-base ?move-base)) (desig:when ?prefer-base (prefer-base ?prefer-base)) + (desig:when ?straight-line + (straight-line ?straight-line)) (desig:when ?align-planes-left (align-planes-left ?align-planes-left)) (desig:when ?align-planes-right @@ -175,6 +179,8 @@ while ignoring failures; and execute the last pose with propagating the failures (move-base ?move-base)) (desig:when ?prefer-base (prefer-base ?prefer-base)) + (desig:when ?straight-line + (straight-line ?straight-line)) (desig:when ?align-planes-left (align-planes-left ?align-planes-left)) (desig:when ?align-planes-right @@ -196,6 +202,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 +240,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 +357,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 From 3a99c1396c0550b156e2baf65eba1504c19603ca Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 16:43:07 +0200 Subject: [PATCH 26/97] [mpp-plans] commented out a call to VISUALIZE-MARKER: it's good for occasional debugging but annoying in demos. --- .../cram_mobile_pick_place_plans/src/pick-place-plans.lisp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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..5185f65dba 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 @@ -64,8 +64,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") From 28d0536bf74605f75e1e149221b00b9dd3a18cc0 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 16:45:00 +0200 Subject: [PATCH 27/97] [tf] in VALUES-CONVERGED, if both values are NIL, assume they converged. --- cram_common/cram_tf/src/utilities.lisp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/cram_common/cram_tf/src/utilities.lisp b/cram_common/cram_tf/src/utilities.lisp index cbfb0d8c58..3f582d26c1 100644 --- a/cram_common/cram_tf/src/utilities.lisp +++ b/cram_common/cram_tf/src/utilities.lisp @@ -391,6 +391,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) From aab0a3164e60a4774b806b4df9ffffcca173b65c Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:22:22 +0200 Subject: [PATCH 28/97] [obj-know] handle and jeroen cup grasp tweaks + refactored multiple pregrasp/postgrasp pose code --- .../cram-object-knowledge.asd | 6 +- .../src/environment.lisp | 40 ++++++++++-- .../cram_object_knowledge/src/household.lisp | 64 +++++++++++++++++-- .../src/multiple-trajectory-poses.lisp | 21 ++++++ .../cram_object_knowledge/src/retail.lisp | 23 ------- 5 files changed, 115 insertions(+), 39 deletions(-) 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 a08b8b84b8..6dc0fbb0d9 100644 --- a/cram_demos/cram_object_knowledge/src/environment.lisp +++ b/cram_demos/cram_object_knowledge/src/environment.lisp @@ -74,6 +74,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) ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; @@ -113,22 +115,37 @@ (defparameter *handle-pregrasp-y-offset-open* 0.06 "in meters") (defparameter *handle-2nd-pregrasp-y-offset-open* 0.02 "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-2nd-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-2nd-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)) @@ -141,6 +158,15 @@ :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)) + ;; FRONT grasp for handle along Z (man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :front :grasp-translation `(,*handle-grasp-x-offset* 0.0d0 ,*handle-grasp-z-offset*) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 1d8bbc12f1..f95910a4cf 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -493,14 +493,23 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;; jeroen-cup ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(defparameter *jeroen-cup-grasp-xy-offset* 0.02 "in meters") +(defparameter *jeroen-cup-grasp-xy-offset* 0.04 "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-pregrasp-xy-offset* 0.30 "in meters") +(defparameter *jeroen-cup-2nd-pregrasp-xy-offset* 0.10 "in meters") +(defparameter *jeroen-cup-3rd-pregrasp-xy-offset* 0.07 "in meters") +(defparameter *jeroen-cup-4th-pregrasp-xy-offset* 0.04 "in meters") +(defparameter *jeroen-cup-5th-pregrasp-xy-offset* 0.01 "in meters") +(defparameter *jeroen-cup-6th-pregrasp-xy-offset* 0.00 "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-postgrasp-x-offset* 0.4 "in meters") +(defparameter *jeroen-cup-postgrasp-y-offset* 0.2 "in meters") +(defparameter *jeroen-cup-surface-lift-offset* 0.3 "in meters") ;; TOP grasp (man-int:def-object-type-to-gripper-transforms :jeroen-cup '(:left :right) :top @@ -559,9 +568,53 @@ :grasp-translation `(,(- *jeroen-cup-grasp-xy-offset*) 0.0d0 ,*jeroen-cup-grasp-z-offset*) :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*) + :2nd-pregrasp-offsets `(,*jeroen-cup-2nd-pregrasp-xy-offset* 0.0 0.0) + :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 *lift-z-offset*) + (make-base-transform + (- *jeroen-cup-postgrasp-x-offset*) *jeroen-cup-postgrasp-y-offset* *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)) @@ -1526,4 +1579,3 @@ :jeroen-cup-in-dishwasher-2 :attachment-translation `(-0.2 0.15 0.13) :attachment-rot-matrix man-int:*rotation-around-y-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*)) From f21251ab8bba117bd61180c691a51a556a6f433b Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:22:58 +0200 Subject: [PATCH 29/97] [proj-reason + mpp-plans] when grasping a handle, avoid all collisions, otherwise wrong grasp will be chosen --- .../cram_urdf_projection_reasoning/src/check-collisions.lisp | 4 +++- .../src/atomic-action-designators.lisp | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) 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 73c75a8417..ded3eb8703 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 @@ -483,7 +483,9 @@ 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) - (list :avoid-all :allow-fingers :allow-all :allow-all)) + ;; 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) ;; (roslisp:ros-warn (coll-check environment) 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 05722714d0..b74c02b1db 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 @@ -174,6 +174,8 @@ (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) From 08716471d81e8bfae4e2e5918eeebbc3b38d3bc5 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:27:53 +0200 Subject: [PATCH 30/97] [proj-demos] apartment demo --- .../src/apartment-demo.lisp | 178 ++++++++++++------ 1 file changed, 118 insertions(+), 60 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index bccac7a97f..72745a9aba 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -32,7 +32,7 @@ (defparameter *apartment-object-spawning-poses* '((:jeroen-cup "cabinet1_coloksu_level4" - ((0.20 0.05 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))))) @@ -77,6 +77,8 @@ (defun apartment-demo (&key (step 0)) ;;urdf-proj:with-simulated-robot + ;; (setf proj-reasoning::*projection-checks-enabled* nil) + (setf btr:*visibility-threshold* 0.7) (when (<= step 0) (initialize-apartment) ;; (btr-belief:vary-kitchen-urdf '(("handle_cab1_top_door_joint" @@ -86,9 +88,6 @@ ;; (setf btr:*current-bullet-world* (make-instance 'btr:bt-reasoning-world)) ;; (btr-belief:spawn-world) - (setf proj-reasoning::*projection-checks-enabled* nil) - - (setf btr:*visibility-threshold* 0.7) (when cram-projection:*projection-environment* (spawn-objects-on-fixed-spots :object-types '(:jeroen-cup :cup) @@ -142,8 +141,7 @@ (part-of apartment))))))) (for (desig:an object (type jeroen-cup) - ;; need a name because of the attachment - (name some-name))) + (name jeroen-cup-1))) (attachments (jeroen-cup-in-dishwasher-1 jeroen-cup-in-dishwasher-2)))) ;; (?location-in-hand ;; (a location @@ -167,73 +165,115 @@ (range-invert 1.5) (orientation upside-down) (for (an object - (type jeroen-cup))))) + (type jeroen-cup) + (name jeroen-cup-1))))) ;; hard-coded stuff for real-world demo - (?accessing-cupboard-door-pose + (?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-robot-location - (a location - (pose ?accessing-cupboard-door-pose))) - (?accessing-dishwasher-door-pose + (?accessing-cupboard-door-another-robot-pose (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 1.6 3.5 0.0d0) - (cl-transforms:make-quaternion 0.0d0 0.0d0 0 1))) - (?detecting-cupboard-pose + (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.2624905511520543d0 2.002821242371188d0 0.0d0) - (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.980183726892722d0 0.19809053873088875d0)))) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.980183726892722d0 0.19809053873088875d0))) + (?on-counter-top-cup-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 2.38 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.6 2.2 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.1 1))) + (?sealing-cupboard-door-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.6861833377510809 1.0873920171726708 0.0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 0.9238795325112867 0.3826834323650899))) + (?accessing-dishwasher-door-robot-pose + (cl-transforms-stamped:make-pose-stamped + "map" + 0.0 + (cl-transforms:make-3d-vector 1.6 3.5 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 0 1)))) ;; bring cup from cupboard to table (when (<= step 1) - (exe:perform - (an action - (type transporting) - (object (an object - (type jeroen-cup) - (location ?location-in-cupboard))) - (access-seal-search-outer-arms (left)) - (access-seal-search-outer-grasps (right-side)) - (access-search-outer-robot-location ?accessing-cupboard-door-robot-location) - (search-robot-location (a location - (pose ?detecting-cupboard-pose))) - (fetch-robot-location (a location - (pose ?detecting-cupboard-pose))) - (arms (left)) - (grasps (front)) - (target ?location-on-island)))) + (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) + (name jeroen-cup-1) + (location ?location-in-cupboard))) + (access-search-outer-robot-location (a location + (poses ?accessing-cupboard-door-robot-poses))) + (access-seal-search-outer-arms (left)) + (access-search-outer-grasps (right-side)) + (search-robot-location (a location + (pose ?detecting-cupboard-robot-pose))) + (fetch-robot-location (a location + (pose ?detecting-cupboard-robot-pose))) + (arms (left)) + (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 + (pose ?sealing-cupboard-door-robot-pose))) + (seal-search-outer-grasps (right-side)) + (goal ?goal))))) ;; put cup from island into dishwasher - (when (<= step 2) - (exe:perform - (an action - (type transporting) - (object (an object - (type jeroen-cup) - (name jeroen-cup-1) - (location ?location-on-island))) - (target ?location-in-dishwasher)))) + ;; (when (<= step 2) + ;; (exe:perform + ;; (an action + ;; (type transporting) + ;; (object (an object + ;; (type jeroen-cup) + ;; (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))) - (exe:perform - (an action - (type transporting) - (object (an object - (type jeroen-cup) - (name jeroen-cup-1) - (location ?location-in-dishwasher))) - (target ?location-on-island-upside-down)))) + ;; (when (<= step 3) + ;; ;; let ((?goal `(cpoe:object-at-location ,?object ,?location-in-hand))) + ;; (exe:perform + ;; (an action + ;; (type transporting) + ;; (object (an object + ;; (type jeroen-cup) + ;; (name jeroen-cup-1) + ;; (location ?location-in-dishwasher))) + ;; (target ?location-on-island-upside-down)))) ;; regrasp cup ;; (when (<= step 4) @@ -247,14 +287,32 @@ ;; (target ?location-in-other-hand)))) ;; bring cup to cupboard - (when (<= step 5) - (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))))) + ;; (when (<= step 5) + ;; (exe:perform + ;; (an action + ;; (type transporting) + ;; (object (an object + ;; (type jeroen-cup) + ;; (name jeroen-cup-1) + ;; ;; (location ?location-on-island-upside-down) + ;; (location ?location-on-island))) + ;; (search-robot-location (a location + ;; (pose ?delivering-counter-top-robot-pose))) + ;; (fetch-robot-location (a location + ;; (pose ?delivering-counter-top-robot-pose))) + ;; (arms (right)) + ;; (grasps (front)) + ;; (access-deliver-outer-robot-location (a location + ;; (poses ?accessing-cupboard-door-robot-poses))) + ;; (access-seal-search-outer-arms (left)) + ;; (access-search-outer-grasps (right-side)) + + ;; (target ?location-in-cupboard) + ;; (deliver-robot-location (a location + ;; (pose ?delivering-counter-top-robot-pose))) + ;; (seal-search-outer-robot-location (a location + ;; (pose ?sealing-cupboard-door-robot-pose))) + ;; (seal-search-outer-grasps (right-side))))) + ) (finalize)) From b19792de956cea02842b7a0fe4cc70b57fa840d3 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:30:07 +0200 Subject: [PATCH 31/97] [donbot] catkin ignore disappeared somehow from real donbot interface packages, brought it back. --- cram_donbot/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 cram_donbot/CATKIN_IGNORE diff --git a/cram_donbot/CATKIN_IGNORE b/cram_donbot/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 From 4715218bf1235649ea548cd6c56d45942b47fec8 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:31:32 +0200 Subject: [PATCH 32/97] [hpn] catkin_ignoring... --- cram_external_interfaces/cram_hpn/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 cram_external_interfaces/cram_hpn/CATKIN_IGNORE diff --git a/cram_external_interfaces/cram_hpn/CATKIN_IGNORE b/cram_external_interfaces/cram_hpn/CATKIN_IGNORE new file mode 100644 index 0000000000..e69de29bb2 From adfcfd533764b5a6417107b3597e86cb732078ea Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:32:51 +0200 Subject: [PATCH 33/97] Bye bye RoboSherlock :'( --- cram_external_interfaces/cram_robosherlock/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 cram_external_interfaces/cram_robosherlock/CATKIN_IGNORE 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 From 8b05d6408ac15b6ef81d9026982b9e65dc7f3042 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:36:19 +0200 Subject: [PATCH 34/97] [rk] fixed type conversion to keyword, comment out visualization markers, hack for orientation: Currently, RK is unable to detect if an object is upright or upside-down, so hacking the upside-down temporarily on the CRAM side. --- .../cram_robokudo/src/action-client.lisp | 46 +++++++++++++------ 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/cram_external_interfaces/cram_robokudo/src/action-client.lisp b/cram_external_interfaces/cram_robokudo/src/action-client.lisp index 5ca54738e3..8669356ce6 100644 --- a/cram_external_interfaces/cram_robokudo/src/action-client.lisp +++ b/cram_external_interfaces/cram_robokudo/src/action-client.lisp @@ -132,13 +132,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))) @@ -274,6 +277,8 @@ (:DMRoteBeteSaftBio :juice-box) (:JeroenCup + :jeroen-cup) + (:jeroen-cup :jeroen-cup)))) (setf rs-answer (subst-if `(:type ,cram-type) @@ -324,30 +329,41 @@ (let* ((pose-stamped-in-whatever (find-pose-in-object-designator combined-properties)) - (pose-stamped-in-base-frame - (if cram-tf:*robot-base-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:*robot-base-frame* + cram-tf:*fixed-frame* :use-zero-time t) pose-stamped-in-whatever)) - (transform-stamped-in-base-frame + (pose-stamped-in-map-frame + (cl-transforms-stamped:copy-pose-stamped + pose-stamped-in-map-frame-original-orientation + :orientation + (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)))) + (transform-stamped-in-map-frame (cram-tf:pose-stamped->transform-stamped - pose-stamped-in-base-frame + pose-stamped-in-map-frame (roslisp-utilities:rosify-underscores-lisp-name name))) - (pose-stamped-in-map-frame - (if cram-tf:*fixed-frame* + (pose-stamped-in-base-frame + (if cram-tf:*robot-base-frame* (cram-tf:ensure-pose-in-frame - pose-stamped-in-whatever - cram-tf:*fixed-frame* + pose-stamped-in-map-frame + cram-tf:*robot-base-frame* :use-zero-time t) pose-stamped-in-whatever)) - (transform-stamped-in-map-frame + (transform-stamped-in-base-frame (cram-tf:pose-stamped->transform-stamped - pose-stamped-in-map-frame + 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)) From 39ed97124c0f4e9d076fb87b0e37955d73e51425 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:47:04 +0200 Subject: [PATCH 35/97] [tiago-know] sideways carry joint config + tweaked tool frame 1 cm. --- .../cram_tiago_description/src/knowledge.lisp | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) 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)))) From fc0f062db2628b9c60a19cde44de3d879dd5fffa Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 17:51:01 +0200 Subject: [PATCH 36/97] [tiago-pms] controlling grippers through an action and not Giskard, works better. --- .../cram-tiago-process-modules.asd | 13 +- .../cram_tiago_process_modules/package.xml | 7 + .../src/grippers.lisp | 223 ++++++++++++++++++ .../src/interface.lisp | 15 +- 4 files changed, 253 insertions(+), 5 deletions(-) create mode 100644 cram_tiago/cram_tiago_process_modules/src/grippers.lisp 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..07d7e84ceb 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,24 @@ :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 + cram-simple-actionlib-client + roslisp + roslisp-utilities) :components ((:module "src" :components ((:file "package") - (:file "interface" :depends-on ("package")))))) + (:file "grippers" :depends-on ("package")) + (:file "interface" :depends-on ("package" + "grippers")))))) 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.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..7844098bca 100644 --- a/cram_tiago/cram_tiago_process_modules/src/interface.lisp +++ b/cram_tiago/cram_tiago_process_modules/src/interface.lisp @@ -41,17 +41,26 @@ (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:available-process-module giskard:giskard-pm) + + (<- (cpm:available-process-module ?pm) + (member ?pm (grippers-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))) From 5073f272c0778ba7051c381eb8f8323ad581243b Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Mon, 5 Sep 2022 23:35:56 +0200 Subject: [PATCH 37/97] [giskard] tiago updates cleaned up. --- .../cram_giskard/src/action-client.lisp | 5 +- .../cram_giskard/src/arm-goals.lisp | 75 ++++++++++--------- .../cram_giskard/src/base-goals.lisp | 4 +- .../cram_giskard/src/collision-scene.lisp | 6 +- .../src/environment-manipulation-goals.lisp | 53 +++++++++++-- .../cram_giskard/src/gripper-goals.lisp | 64 ++++++++++------ .../src/making-goal-messages.lisp | 14 ++-- .../cram_giskard/src/neck-goals.lisp | 16 ++-- .../cram_giskard/src/process-module.lisp | 1 + 9 files changed, 156 insertions(+), 82 deletions(-) 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 fc38128792..0c739dc26d 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -46,18 +46,34 @@ 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) (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) (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 @@ -85,16 +101,7 @@ ;; cram-tf:*robot-base-frame* 0.0 ;; (cl-transforms:make-3d-vector 0 0 1)))) (when unmovable-joints - (make-unmovable-joints-constraint - (append - unmovable-joints - (when (eq (rob-int:get-robot-name) :tiago-dual) - (mapcar (lambda (binds) - (cut:var-value '?joint binds)) - (cut:force-ll - (prolog:prolog - `(and (rob-int:robot ?robot-name) - (rob-int:gripper-joint ?robot-name ?_ ?joint))))))))) + (make-unmovable-joints-constraint unmovable-joints)) (make-base-velocity-constraint *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) (make-head-pointing-at-hand-constraint @@ -105,44 +112,43 @@ (make-diffdrive-cartesian-goal-arm-constraint (if left-pose cram-tf:*robot-left-tool-frame* - cram-tf:*robot-right-tool-frame*)))) - :cartesian-constraints (list (when left-pose - (make-cartesian-constraint - pose-base-frame - cram-tf:*robot-left-wrist-frame* - left-pose)) - (when right-pose - (make-cartesian-constraint - pose-base-frame - cram-tf:*robot-right-wrist-frame* - right-pose))) + cram-tf:*robot-right-tool-frame*))) + (when left-pose + (make-cartesian-constraint + pose-base-frame cram-tf:*robot-left-wrist-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* 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) + (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 @@ -173,8 +179,7 @@ cram-tf:*robot-base-frame* 0.0 (cl-transforms:make-identity-pose)) :max-velocity *base-max-velocity-slow-xy* - ;; :avoid-collisions-much t - ) + :avoid-collisions-much nil) ;; (when align-planes-left ;; (make-align-planes-tool-frame-constraint ;; :left @@ -285,14 +290,15 @@ 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) (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 + align-planes-left align-planes-right) (type (or list null) unmovable-joints)) (unless (or goal-pose-left goal-pose-right) @@ -324,6 +330,7 @@ :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) diff --git a/cram_external_interfaces/cram_giskard/src/base-goals.lisp b/cram_external_interfaces/cram_giskard/src/base-goals.lisp index cdbaec96ab..f42ed8d84f 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -58,7 +58,8 @@ (make-diffdrive-base-goal cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose :avoid-collisions-much t - :max-velocity *base-max-velocity-fast-xy*) + :max-velocity *base-max-velocity-fast-xy* + :always-forward nil) (make-cartesian-constraint cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose :avoid-collisions-much t @@ -106,6 +107,7 @@ (cram-tf:visualize-marker goal-pose :r-g-b-list '(0 1 0)) + ;; Trying with slow velocity should happen on the motion level, not action level. (call-action :action-goal (make-giskard-base-action-goal goal-pose base-velocity) :action-timeout action-timeout diff --git a/cram_external_interfaces/cram_giskard/src/collision-scene.lisp b/cram_external_interfaces/cram_giskard/src/collision-scene.lisp index 2060593a8e..c99461f116 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-identity-vector))) (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 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 5ce95cef21..9e09d98603 100644 --- a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp @@ -48,14 +48,49 @@ open-or-close arm handle-link joint-state) (when (eq (rob-int:get-robot-name) :tiago-dual) (make-diffdrive-base-arch-constraint hinge-point-stamped)) - (make-avoid-joint-limits-constraint) + (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 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 + (make-allow-hand-collision + (list arm) (rob-int:get-environment-name) handle-link)))) (defun call-environment-manipulation-action (&key action-timeout @@ -74,6 +109,12 @@ 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)) + (call-action :action-goal (make-environment-manipulation-goal open-or-close arm handle-link joint-angle prefer-base diff --git a/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp b/cram_external_interfaces/cram_giskard/src/gripper-goals.lisp index 796ff71984..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) @@ -95,27 +96,39 @@ (: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 @@ -138,7 +151,14 @@ (call-action :action-goal (make-gripper-action-goal arm-element goal-joint-angle effort) :action-timeout action-timeout - ;; :check-goal-function (lambda () - ;; (ensure-gripper-goal-reached arm goal-joint-angle)) - )) + :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 133b8b751d..69512373c7 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -348,15 +348,17 @@ . ,root-link)))))) (defun make-cartesian-constraint (root-frame tip-frame goal-pose - &key max-velocity avoid-collisions-much) + &key max-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 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) @@ -377,11 +379,11 @@ )))))))) (defun make-diffdrive-base-goal (root-frame tip-frame goal-pose - &key max-velocity avoid-collisions-much) + &key max-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-velocity) - (type boolean avoid-collisions-much)) + (type boolean avoid-collisions-much always-forward)) (roslisp:make-message 'giskard_msgs-msg:constraint :type @@ -393,6 +395,8 @@ ("goal_pose" . (("message_type" . "geometry_msgs/PoseStamped") ("message" . ,(to-hash-table goal-pose)))) + ,@(when always-forward + `(("always_forward" . T))) ,@(when max-velocity `(("max_linear_velocity" . ,max-velocity))) ,@(if avoid-collisions-much 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 9c4fdcab37..4933d9670c 100644 --- a/cram_external_interfaces/cram_giskard/src/process-module.lisp +++ b/cram_external_interfaces/cram_giskard/src/process-module.lisp @@ -43,6 +43,7 @@ :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))) (cram-common-designators:move-joints From dbb4238816952c04546a79c331bc034581a4c032 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 6 Sep 2022 00:49:20 +0200 Subject: [PATCH 38/97] [spatial-cm] removed unnecessary print. --- .../cram_btr_spatial_relations_costmap/src/cost-functions.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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)))) From 38c6da6eeb886cc4d1896da310d024be8e3a7321 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 6 Sep 2022 00:49:52 +0200 Subject: [PATCH 39/97] [bs] OBJECT-AT-LOCATION considers the link if the location is on the robot --- .../cram_bullet_reasoning_belief_state/src/occasions.lisp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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 04d893153d..b52e29d8f2 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 @@ -501,14 +501,17 @@ ;; 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) From 9427613b665fcbf531dcd968b07bee4ae069380a Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 6 Sep 2022 00:50:24 +0200 Subject: [PATCH 40/97] [proj-reason] removed environment manipulation trajectory axes visualization. --- .../src/check-collisions.lisp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 ded3eb8703..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) From 359e8337a333b060e27502ae787b8585c52b44ad Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 6 Sep 2022 00:51:00 +0200 Subject: [PATCH 41/97] [proj-demos] debug prints for retail demo. --- cram_demos/cram_projection_demos/src/retail-demo.lisp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/retail-demo.lisp b/cram_demos/cram_projection_demos/src/retail-demo.lisp index d413258076..85f7850e22 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 From d9298792e987f5a977da30e4fd85ee9aab9fdf05 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 6 Sep 2022 00:51:18 +0200 Subject: [PATCH 42/97] [obj-know + proj-demos] apartment demo tweaks for PR2. --- .../cram_object_knowledge/src/household.lisp | 32 ++--- .../src/apartment-demo.lisp | 115 +++++++++--------- 2 files changed, 73 insertions(+), 74 deletions(-) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index f95910a4cf..b3168c0d47 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -507,9 +507,10 @@ (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-postgrasp-x-offset* 0.4 "in meters") -(defparameter *jeroen-cup-postgrasp-y-offset* 0.2 "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.05 "in meters") ;; TOP grasp (man-int:def-object-type-to-gripper-transforms :jeroen-cup '(:left :right) :top @@ -543,16 +544,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 @@ -560,8 +561,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 @@ -606,9 +607,11 @@ (grasp (eql :front)) (location (eql :shelf))) (list (make-base-transform - 0.0 0.0 *lift-z-offset*) + 0.0 0.0 *jeroen-cup-lift-z-offset*) (make-base-transform - (- *jeroen-cup-postgrasp-x-offset*) *jeroen-cup-postgrasp-y-offset* *lift-z-offset*))) + (- *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)) @@ -764,6 +767,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 @@ -771,7 +775,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*)) @@ -780,7 +784,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*)) @@ -789,7 +793,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*)) @@ -797,7 +801,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*)) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 72745a9aba..a63eaf0b30 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -96,7 +96,7 @@ 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)))) + (cl-transforms:make-quaternion 0 0 0.5 0.5)))) (let* ((?object (an object @@ -143,18 +143,14 @@ (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) - ;; (name tiago-dual) - ;; (part-of tiago-dual))))) + (?location-in-hand + (a location + (in (an object + (type robot))))) ;; (?location-in-other-hand ;; (a location ;; (in (an object - ;; (type robot) - ;; (name tiago-dual) - ;; (part-of tiago-dual))))) + ;; (type robot))))) (?location-on-island-upside-down (a location (on (an object @@ -189,13 +185,13 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 1.2624905511520543d0 2.002821242371188d0 0.0d0) - (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.980183726892722d0 0.19809053873088875d0))) + (cl-transforms:make-3d-vector 1.1 2.002821242371188d0 0.0d0) + (cl-transforms:make-quaternion 0.0d0 0.0d0 -1 0))) (?on-counter-top-cup-pose (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.38 2.6 1.0126) + (cl-transforms:make-3d-vector 2.43 2.6 1.0126) (cl-transforms:make-quaternion 0 0 1 0))) (?delivering-counter-top-robot-pose (cl-transforms-stamped:make-pose-stamped @@ -234,7 +230,7 @@ (access-search-outer-robot-location (a location (poses ?accessing-cupboard-door-robot-poses))) (access-seal-search-outer-arms (left)) - (access-search-outer-grasps (right-side)) + (access-search-outer-grasps (back)) (search-robot-location (a location (pose ?detecting-cupboard-robot-pose))) (fetch-robot-location (a location @@ -249,31 +245,31 @@ (pose ?delivering-counter-top-robot-pose))) (seal-search-outer-robot-location (a location (pose ?sealing-cupboard-door-robot-pose))) - (seal-search-outer-grasps (right-side)) + (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 - ;; (type jeroen-cup) - ;; (name jeroen-cup-1) - ;; (location ?location-on-island))) - ;; (target ?location-in-dishwasher)))) + (when (<= step 2) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (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))) - ;; (exe:perform - ;; (an action - ;; (type transporting) - ;; (object (an object - ;; (type jeroen-cup) - ;; (name jeroen-cup-1) - ;; (location ?location-in-dishwasher))) - ;; (target ?location-on-island-upside-down)))) + (when (<= step 3) + ;; let ((?goal `(cpoe:object-at-location ,?object ,?location-in-hand))) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (name jeroen-cup-1) + (location ?location-in-dishwasher))) + (target ?location-on-island-upside-down)))) ;; regrasp cup ;; (when (<= step 4) @@ -287,32 +283,31 @@ ;; (target ?location-in-other-hand)))) ;; bring cup to cupboard - ;; (when (<= step 5) - ;; (exe:perform - ;; (an action - ;; (type transporting) - ;; (object (an object - ;; (type jeroen-cup) - ;; (name jeroen-cup-1) - ;; ;; (location ?location-on-island-upside-down) - ;; (location ?location-on-island))) - ;; (search-robot-location (a location - ;; (pose ?delivering-counter-top-robot-pose))) - ;; (fetch-robot-location (a location - ;; (pose ?delivering-counter-top-robot-pose))) - ;; (arms (right)) - ;; (grasps (front)) - ;; (access-deliver-outer-robot-location (a location - ;; (poses ?accessing-cupboard-door-robot-poses))) - ;; (access-seal-search-outer-arms (left)) - ;; (access-search-outer-grasps (right-side)) + (when (<= step 5) + (exe:perform + (an action + (type transporting) + (object (an object + (type jeroen-cup) + (name jeroen-cup-1) + ;; (location ?location-on-island-upside-down) + (location ?location-on-island))) + (search-robot-location (a location + (pose ?delivering-counter-top-robot-pose))) + (fetch-robot-location (a location + (pose ?delivering-counter-top-robot-pose))) + (arms (right)) + (grasps (front)) + (access-deliver-outer-robot-location (a location + (poses ?accessing-cupboard-door-robot-poses))) + (access-seal-search-outer-arms (left)) + (access-search-outer-grasps (right-side)) - ;; (target ?location-in-cupboard) - ;; (deliver-robot-location (a location - ;; (pose ?delivering-counter-top-robot-pose))) - ;; (seal-search-outer-robot-location (a location - ;; (pose ?sealing-cupboard-door-robot-pose))) - ;; (seal-search-outer-grasps (right-side))))) - ) + (target ?location-in-cupboard) + (deliver-robot-location (a location + (pose ?delivering-counter-top-robot-pose))) + (seal-search-outer-robot-location (a location + (pose ?sealing-cupboard-door-robot-pose))) + (seal-search-outer-grasps (right-side)))))) (finalize)) From 2bdb1dce3eec17c8f118d6a810c617691216f751 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 6 Sep 2022 20:44:17 +0200 Subject: [PATCH 43/97] [mpp-plans] bugfix in object to environment attachment: name of environment is often stored in PART-OF parameter not NAME. --- .../cram_mobile_pick_place_plans/src/pick-place-plans.lisp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 5185f65dba..c27fe76296 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 @@ -248,7 +248,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 From 064eea1c45a722f2b45bedc136023974243867a3 Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Tue, 6 Sep 2022 20:44:46 +0200 Subject: [PATCH 44/97] [obj-know + proj-demos] apartment demo for pr2 --- .../cram_object_knowledge/src/household.lisp | 16 +- .../src/apartment-demo.lisp | 143 +++++++++++++----- 2 files changed, 117 insertions(+), 42 deletions(-) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index b3168c0d47..19e0da0df5 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -505,8 +505,9 @@ (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") @@ -531,7 +532,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*)) @@ -1576,10 +1577,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.05 0.08) + :attachment-rot-matrix man-int:*rotation-around-x-180-matrix*) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index a63eaf0b30..a0a9b0c2de 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -77,7 +77,7 @@ (defun apartment-demo (&key (step 0)) ;;urdf-proj:with-simulated-robot - ;; (setf proj-reasoning::*projection-checks-enabled* nil) + (setf proj-reasoning::*projection-checks-enabled* nil) (setf btr:*visibility-threshold* 0.7) (when (<= step 0) (initialize-apartment) @@ -117,7 +117,8 @@ (range-invert 0.2) (range 0.25) (orientation upside-down) - (for ?object))) + (for ?object) + (attachments (jeroen-cup-on-shelf)))) (?location-on-island (a location (on (an object @@ -143,10 +144,10 @@ (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-hand + ;; (a location + ;; (in (an object + ;; (type robot))))) ;; (?location-in-other-hand ;; (a location ;; (in (an object @@ -158,7 +159,7 @@ (urdf-name island-countertop) (part-of apartment))) (side (right back)) - (range-invert 1.5) + (range 0.5) (orientation upside-down) (for (an object (type jeroen-cup) @@ -187,6 +188,13 @@ 0.0 (cl-transforms:make-3d-vector 1.1 2.002821242371188d0 0.0d0) (cl-transforms:make-quaternion 0.0d0 0.0d0 -1 0))) + (?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))) + (?on-counter-top-cup-pose (cl-transforms-stamped:make-pose-stamped "map" @@ -197,20 +205,45 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 1.6 2.2 0.0d0) + (cl-transforms:make-3d-vector 1.8 2.8 0.0d0) (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.1 1))) - (?sealing-cupboard-door-robot-pose + (?fetching-counter-top-robot-pose (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 1.6861833377510809 1.0873920171726708 0.0) - (cl-transforms:make-quaternion 0.0d0 0.0d0 0.9238795325112867 0.3826834323650899))) + (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.43 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.6 3.5 0.0d0) - (cl-transforms:make-quaternion 0.0d0 0.0d0 0 1)))) + (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)))) ;; bring cup from cupboard to table (when (<= step 1) @@ -257,7 +290,30 @@ (type jeroen-cup) (name jeroen-cup-1) (location ?location-on-island))) - (target ?location-in-dishwasher)))) + (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 (<= step 3) @@ -269,45 +325,58 @@ (type jeroen-cup) (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)))) + (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 (<= step 5) + (when (<= step 4) (exe:perform (an action (type transporting) (object (an object (type jeroen-cup) (name jeroen-cup-1) - ;; (location ?location-on-island-upside-down) - (location ?location-on-island))) + (location ?location-on-island-upside-down) + ;; (location ?location-on-island) + )) + + (access-deliver-outer-robot-location (a location + (poses ?accessing-cupboard-door-robot-poses))) + (seal-deliver-outer-robot-location (a location + (pose ?sealing-cupboard-door-robot-pose))) + (access-seal-deliver-outer-arms (left)) + (access-seal-deliver-outer-grasps (back)) + (search-robot-location (a location (pose ?delivering-counter-top-robot-pose))) (fetch-robot-location (a location (pose ?delivering-counter-top-robot-pose))) - (arms (right)) + (arms (left)) (grasps (front)) - (access-deliver-outer-robot-location (a location - (poses ?accessing-cupboard-door-robot-poses))) - (access-seal-search-outer-arms (left)) - (access-search-outer-grasps (right-side)) (target ?location-in-cupboard) (deliver-robot-location (a location - (pose ?delivering-counter-top-robot-pose))) - (seal-search-outer-robot-location (a location - (pose ?sealing-cupboard-door-robot-pose))) - (seal-search-outer-grasps (right-side)))))) + (pose ?detecting-cupboard-robot-pose))))))) (finalize)) From 369c255947561e7e2bc00f872150404135ae5014 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Wed, 7 Sep 2022 21:14:10 +0200 Subject: [PATCH 45/97] [cm] bugfix in costmap rotation: there was an array out of bounds error that happened very rarely. --- .../src/cost-functions.lisp | 71 +++++++++++-------- 1 file changed, 41 insertions(+), 30 deletions(-) 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 From 1ca4f6b1dec2e89967951ff0861340f6c641ad54 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Wed, 7 Sep 2022 21:15:32 +0200 Subject: [PATCH 46/97] [pr2-pms] switched from RS to RK --- .../cram_pr2_process_modules/cram-pr2-process-modules.asd | 2 +- cram_pr2/cram_pr2_process_modules/package.xml | 4 ++-- cram_pr2/cram_pr2_process_modules/src/with-real-robot.lisp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) 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/with-real-robot.lisp b/cram_pr2/cram_pr2_process_modules/src/with-real-robot.lisp index 10d03958a1..7e68100ec0 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,7 +31,7 @@ (defmacro with-real-robot (&body body) `(cram-process-modules:with-process-modules-running - (rs:robosherlock-perception-pm ;; navp:navp-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) From a9543c3886fe8804d3835673646054208db87284 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 12 Sep 2022 09:44:32 +0200 Subject: [PATCH 47/97] pr2 demo save --- .../src/occasions.lisp | 1 + .../src/plans.lisp | 31 +++-- .../src/trajectories.lisp | 2 +- .../src/atomic-action-plans.lisp | 112 +++++++++--------- .../src/pick-place-plans.lisp | 46 +++---- cram_common/cram_tf/src/utilities.lisp | 6 +- .../src/environment.lisp | 15 +-- .../cram_object_knowledge/src/household.lisp | 65 +++++++--- cram_demos/cram_projection_demos/package.xml | 8 +- .../src/apartment-demo.lisp | 100 +++++++++++----- .../cram_projection_demos/src/setup.lisp | 12 +- .../cram_giskard/src/arm-goals.lisp | 49 ++++---- .../cram_giskard/src/base-goals.lisp | 11 +- .../cram_giskard/src/collision-scene.lisp | 13 +- .../src/environment-manipulation-goals.lisp | 4 +- .../src/making-goal-messages.lisp | 20 ++-- .../cram_robokudo/src/action-client.lisp | 6 +- cram_pr2/cram_pr2_low_level/CATKIN_IGNORE | 0 .../cram_pr2_process_modules/CATKIN_IGNORE | 0 .../cram_pr2_process_modules/src/giskard.lisp | 3 +- .../src/with-real-robot.lisp | 3 +- 21 files changed, 307 insertions(+), 200 deletions(-) delete mode 100644 cram_pr2/cram_pr2_low_level/CATKIN_IGNORE delete mode 100644 cram_pr2/cram_pr2_process_modules/CATKIN_IGNORE 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 b52e29d8f2..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 @@ -515,6 +515,7 @@ ;; 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) 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 9619dba53d..e5031df9e7 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp @@ -77,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 @@ -86,14 +87,15 @@ (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 @@ -102,14 +104,15 @@ (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 @@ -148,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 @@ -178,9 +180,12 @@ (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) + (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"))) 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 57d22f643f..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 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 a6d629210e..276932a646 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 @@ -120,34 +120,34 @@ 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 ?straight-line - (straight-line ?straight-line)) - (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 ?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)) @@ -160,34 +160,34 @@ 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 ?straight-line - (straight-line ?straight-line)) - (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 ?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)) 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 c27fe76296..68bd33981c 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 @@ -80,8 +80,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,6 +149,10 @@ (desig:an action (type monitoring-joint-state) (gripper ?arm))) + (cram-occasions-events:on-event + (make-instance 'cpoe:object-detached-robot + :arm ?arm + :object-designator ?object-designator)) (cpl:fail 'common-fail:gripper-closed-completely :description "Object slipped"))) (roslisp:ros-info (pick-place place) "Parking") @@ -206,29 +211,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 +246,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 diff --git a/cram_common/cram_tf/src/utilities.lisp b/cram_common/cram_tf/src/utilities.lisp index 3f582d26c1..8fd111eac2 100644 --- a/cram_common/cram_tf/src/utilities.lisp +++ b/cram_common/cram_tf/src/utilities.lisp @@ -298,19 +298,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))) diff --git a/cram_demos/cram_object_knowledge/src/environment.lisp b/cram_demos/cram_object_knowledge/src/environment.lisp index 6dc0fbb0d9..3b32a56bf6 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) @@ -93,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 @@ -110,10 +111,10 @@ (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.06 "in meters") -(defparameter *handle-2nd-pregrasp-x-offset-open* 0.02 "in meters") -(defparameter *handle-pregrasp-y-offset-open* 0.06 "in meters") -(defparameter *handle-2nd-pregrasp-y-offset-open* 0.02 "in meters") +(defparameter *handle-pregrasp-x-offset-open* 0.15 "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.15 "in meters") ; defined in env-manip (defparameter *handle-side-grasp-x-offset* 0.0 "in meters") diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 19e0da0df5..5b6bc61b2d 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -493,15 +493,15 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;; jeroen-cup ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(defparameter *jeroen-cup-grasp-xy-offset* 0.04 "in meters") +(defparameter *jeroen-cup-grasp-xy-offset* 0.0 "in meters") (defparameter *jeroen-cup-grasp-z-offset* 0.0 "in meters") -(defparameter *jeroen-cup-pregrasp-xy-offset* 0.30 "in meters") -(defparameter *jeroen-cup-2nd-pregrasp-xy-offset* 0.10 "in meters") -(defparameter *jeroen-cup-3rd-pregrasp-xy-offset* 0.07 "in meters") +(defparameter *jeroen-cup-pregrasp-xy-offset* 0.20 "in meters") +(defparameter *jeroen-cup-2nd-pregrasp-xy-offset* 0.07 "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.01 "in meters") -(defparameter *jeroen-cup-6th-pregrasp-xy-offset* 0.00 "in meters") -(defparameter *jeroen-cup-7th-pregrasp-xy-offset* -0.01 "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") @@ -570,7 +570,7 @@ :grasp-translation `(,(- *jeroen-cup-grasp-xy-offset*) 0.0d0 ,*jeroen-cup-grasp-z-offset*) :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-2nd-pregrasp-xy-offset* 0.0 0.0) + :2nd-pregrasp-offsets `(,*jeroen-cup-pregrasp-xy-offset* 0.0 0.0) :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. @@ -588,11 +588,12 @@ 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*))) + ;; *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 @@ -609,16 +610,46 @@ (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*))) + ;; (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)) 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/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index a0a9b0c2de..0ea2756e2b 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -79,30 +79,28 @@ ;;urdf-proj:with-simulated-robot (setf proj-reasoning::*projection-checks-enabled* nil) (setf btr:*visibility-threshold* 0.7) - (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 :cup) - :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 0.5 0.5)))) (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) @@ -167,6 +165,13 @@ ;; 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" @@ -188,6 +193,13 @@ 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" @@ -219,12 +231,18 @@ 0.0 (cl-transforms:make-3d-vector 2.43 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.6 3.5 0.0d0) + (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 @@ -245,6 +263,22 @@ (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) + ;; (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) + :spawning-poses-relative *apartment-object-spawning-poses*)) + (park-robot ?initial-parking-pose)) + ;; bring cup from cupboard to table (when (<= step 1) (let ((?goal `(and (cpoe:object-at-location ,(an object @@ -258,6 +292,7 @@ (type transporting) (object (an object (type jeroen-cup) + (color blue) (name jeroen-cup-1) (location ?location-in-cupboard))) (access-search-outer-robot-location (a location @@ -265,10 +300,16 @@ (access-seal-search-outer-arms (left)) (access-search-outer-grasps (back)) (search-robot-location (a location - (pose ?detecting-cupboard-robot-pose))) + (pose ?detecting-cupboard-robot-pose + ;; ?fetching-cupboard-right-hand-robot-pose + ))) (fetch-robot-location (a location - (pose ?detecting-cupboard-robot-pose))) - (arms (left)) + (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)) @@ -288,6 +329,7 @@ (type transporting) (object (an object (type jeroen-cup) + (color blue) (name jeroen-cup-1) (location ?location-on-island))) (target ?location-in-dishwasher) @@ -323,6 +365,7 @@ (type transporting) (object (an object (type jeroen-cup) + (color blue) (name jeroen-cup-1) (location ?location-in-dishwasher))) (grasps (bottom)) @@ -357,9 +400,11 @@ (object (an object (type jeroen-cup) (name jeroen-cup-1) - (location ?location-on-island-upside-down) - ;; (location ?location-on-island) - )) + (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))) @@ -375,8 +420,9 @@ (arms (left)) (grasps (front)) - (target ?location-in-cupboard) + (target ?location-in-cupboard-with-attachment) (deliver-robot-location (a location - (pose ?detecting-cupboard-robot-pose))))))) + (pose ;; ?detecting-cupboard-robot-pose + ?initial-parking-pose))))))) (finalize)) 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_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index 0c739dc26d..609dd455cc 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -102,8 +102,8 @@ ;; (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 @@ -115,12 +115,18 @@ cram-tf:*robot-right-tool-frame*))) (when left-pose (make-cartesian-constraint - pose-base-frame cram-tf:*robot-left-wrist-frame* left-pose + 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* right-pose + 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 @@ -157,22 +163,22 @@ (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 @@ -241,7 +247,8 @@ tool-transform-in-correct-base-frame tool-T-wrist :result-as-pose-or-transform :pose))) - wrist-pose-in-correct-base-frame))) + ;; 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) diff --git a/cram_external_interfaces/cram_giskard/src/base-goals.lisp b/cram_external_interfaces/cram_giskard/src/base-goals.lisp index f42ed8d84f..1e30399f50 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -70,11 +70,12 @@ (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*)) + (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 diff --git a/cram_external_interfaces/cram_giskard/src/collision-scene.lisp b/cram_external_interfaces/cram_giskard/src/collision-scene.lisp index c99461f116..bb46696d68 100644 --- a/cram_external_interfaces/cram_giskard/src/collision-scene.lisp +++ b/cram_external_interfaces/cram_giskard/src/collision-scene.lisp @@ -171,7 +171,7 @@ (unless dimensions (setf dimensions '(1.0 1.0 1.0))) (unless scale - (setf scale (cl-transforms:make-identity-vector))) + (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 @@ -527,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 9e09d98603..17b68a3350 100644 --- a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp @@ -42,8 +42,8 @@ :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*) + ;; (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) (when (eq (rob-int:get-robot-name) :tiago-dual) 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 69512373c7..182913d0eb 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -367,16 +367,16 @@ . (("message_type" . "geometry_msgs/PoseStamped") ("message" . ,(to-hash-table goal-pose)))) ,@(when max-velocity - `(("max_linear_velocity" . ,max-velocity))) + `(("reference_velocity" . ,max-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 - )))))))) + `(("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-velocity avoid-collisions-much always-forward) @@ -398,7 +398,7 @@ ,@(when always-forward `(("always_forward" . T))) ,@(when max-velocity - `(("max_linear_velocity" . ,max-velocity))) + `(("reference_velocity" . ,max-velocity))) ,@(if avoid-collisions-much `(("weight" . ,(roslisp-msg-protocol:symbol-code 'giskard_msgs-msg:constraint diff --git a/cram_external_interfaces/cram_robokudo/src/action-client.lisp b/cram_external_interfaces/cram_robokudo/src/action-client.lisp index 8669356ce6..a034b673ac 100644 --- a/cram_external_interfaces/cram_robokudo/src/action-client.lisp +++ b/cram_external_interfaces/cram_robokudo/src/action-client.lisp @@ -199,6 +199,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))) @@ -340,12 +341,13 @@ (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 @@ -388,6 +390,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) @@ -397,6 +400,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_pr2/cram_pr2_low_level/CATKIN_IGNORE b/cram_pr2/cram_pr2_low_level/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/cram_pr2/cram_pr2_process_modules/CATKIN_IGNORE b/cram_pr2/cram_pr2_process_modules/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 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 7e68100ec0..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 @@ -32,7 +32,8 @@ (defmacro with-real-robot (&body body) `(cram-process-modules:with-process-modules-running (rk:robokudo-perception-pm ;; navp:navp-pm - pr2-grippers-pm pr2-ptu-pm giskard:giskard-pm joints:joint-state-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))) From a2f5c207fbaab196ee92eb2d6c3abffd75be01d3 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 13 Sep 2022 18:27:12 +0200 Subject: [PATCH 48/97] [apart-demo] pouring start vanessa --- .../src/apartment-demo.lisp | 118 ++++++++++++++++++ 1 file changed, 118 insertions(+) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 0ea2756e2b..b7910d24f3 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -37,6 +37,14 @@ "cabinet1_coloksu_level4" ((0.15 -0.1 0.08) (0 0 -1 0))))) +(defparameter *apartment-object-spawning-poses-pouring* + '((:jeroen-cup + "island_countertop" + ((0.30 0.05 0.1) (0 0 0 1))) + (:cup + "cabinet1_coloksu_level4" + ((0.15 -0.1 0.08) (0 0 -1 0))))) + (defun initialize-apartment () (sb-ext:gc :full t) @@ -426,3 +434,113 @@ ?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* nil) + (setf btr:*visibility-threshold* 0.7) + (let* ((?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 ?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 2.8 0.0) + (cl-transforms:make-quaternion 0 0 0 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.43 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.39 2.76 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 :cup) + :spawning-poses-relative *apartment-object-spawning-poses-pouring*)) + (park-robot ?initial-parking-pose) + + ;; pick-up cup to pour + (when (<= step 1) + + ;; (exe:perform + ;; (an action + ;; (type transporting) + ;; (object (an object + ;; (type jeroen-cup) + ;; (color blue) + ;; (name jeroen-cup-1) + ;; (location ?location-in-cupboard))) + ;; (cpl:par + ;; (exe:perform (desig:an action + ;; (type parking-arms))) + + (exe:perform (desig:a motion + (type looking) + (pose ?on-counter-top-cup-look-pose)))) + + + (let* ((?object-desig + (desig:an object + (type jeroen-cup) + (color blue) + (name jeroen-cup-1) + (location ?location-on-island))) + (?perceived-object-desig + (exe:perform (desig:an action + (type detecting) + (object ?object-desig))))) + +;; ?perceived-object-desig + ;; (dotimes (i 3) + ;; (exe:perform (desig:an action + ;; (type looking) + ;; (object ?perceived-object-desig)))) + (exe:perform (desig:an action + (type picking-up) + (arm :right) + (grasp :left-side) + (object ?perceived-object-desig)))) + + + + + )))) From 10920acf27104bdfe1b2271346df3e7bce52e8cd Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 13 Sep 2022 19:04:22 +0200 Subject: [PATCH 49/97] [proj-demos] apt demo: removed superfluous paren --- cram_demos/cram_projection_demos/src/apartment-demo.lisp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index b7910d24f3..09fffeb3d1 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -542,5 +542,4 @@ - - )))) + ))) From 66046efe6a6495b52c1c6b5dca7e3d54dd8b31c9 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 13 Sep 2022 23:03:23 +0200 Subject: [PATCH 50/97] pr2 demo save: * using tf buffer for transforms of perception results, it's slower but more accurate * visualizing marker of perceived object before bullet stabilization and after * tweaking poses in demo and grasping poses of jeroen-cup * disallowing base movement in the last phase of the grasping trajectory. --- .../src/event-handlers.lisp | 12 ++ .../src/plans.lisp | 4 +- .../src/atomic-action-designators.lisp | 3 +- cram_common/cram_tf/src/utilities.lisp | 15 +- cram_common/cram_tf/src/visualization.lisp | 2 +- .../cram_object_knowledge/src/household.lisp | 19 ++- .../src/apartment-demo.lisp | 154 ++++++++++-------- .../src/retail-demo.lisp | 27 +++ .../cram_giskard/src/arm-goals.lisp | 8 +- .../src/environment-manipulation-goals.lisp | 10 +- .../cram_robokudo/cram-robokudo.asd | 1 + .../cram_robokudo/package.xml | 1 + .../cram_robokudo/src/action-client.lisp | 14 +- 13 files changed, 170 insertions(+), 100 deletions(-) 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_urdf_environment_manipulation/src/plans.lisp b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp index e5031df9e7..b295742702 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp @@ -166,8 +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 ?door-joint-pose + (door-joint-pose ?door-joint-pose)) (desig:when ?absolute-distance (distance ?absolute-distance)) (desig:when (eq ?arm :left) 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 b74c02b1db..148f88e698 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 @@ -180,7 +180,8 @@ (: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) (:align-planes-left ?align-planes-left) (:align-planes-right ?align-planes-right)) diff --git a/cram_common/cram_tf/src/utilities.lisp b/cram_common/cram_tf/src/utilities.lisp index 8fd111eac2..6897b46bba 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)) 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_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 5b6bc61b2d..e70150e1ef 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -493,7 +493,7 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;; jeroen-cup ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; -(defparameter *jeroen-cup-grasp-xy-offset* 0.0 "in meters") +(defparameter *jeroen-cup-grasp-xy-offset* 0.04 "in meters") (defparameter *jeroen-cup-grasp-z-offset* 0.0 "in meters") (defparameter *jeroen-cup-pregrasp-xy-offset* 0.20 "in meters") (defparameter *jeroen-cup-2nd-pregrasp-xy-offset* 0.07 "in meters") @@ -511,7 +511,7 @@ (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.05 "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 @@ -609,12 +609,13 @@ (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*) - )) + 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)) @@ -1618,5 +1619,5 @@ (man-int:def-object-type-in-other-object-transform :jeroen-cup :shelf :jeroen-cup-on-shelf - :attachment-translation `(0.20 0.05 0.08) + :attachment-translation `(0.20 0.08 0.10) :attachment-rot-matrix man-int:*rotation-around-x-180-matrix*) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 09fffeb3d1..8198766bd4 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -201,25 +201,34 @@ 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 + ;; (?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.3668892503154677d0 1.6951934636420103d0 0.0d0) - (cl-transforms:make-quaternion 0.0d0 0.0d0 0.9298226390305793d0 - -0.36800561314986846d0))) - (?sealing-cupboard-door-robot-pose + (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.6 1.0873920171726708 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.43 2.6 1.0126) + (cl-transforms:make-3d-vector 2.4 2.6 1.0126) (cl-transforms:make-quaternion 0 0 1 0))) (?delivering-counter-top-robot-pose (cl-transforms-stamped:make-pose-stamped @@ -237,7 +246,7 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.43 2.6 1.0126) + (cl-transforms:make-3d-vector 2.4 2.6 1.0126) (cl-transforms:make-quaternion 0 1 0 0))) (?on-counter-top-cup-look-pose (cl-transforms-stamped:make-pose-stamped @@ -326,7 +335,7 @@ (deliver-robot-location (a location (pose ?delivering-counter-top-robot-pose))) (seal-search-outer-robot-location (a location - (pose ?sealing-cupboard-door-robot-pose))) + (poses ?sealing-cupboard-door-robot-poses))) (seal-search-outer-grasps (back)) (goal ?goal))))) @@ -417,28 +426,31 @@ (access-deliver-outer-robot-location (a location (poses ?accessing-cupboard-door-robot-poses))) (seal-deliver-outer-robot-location (a location - (pose ?sealing-cupboard-door-robot-pose))) + (poses ?sealing-cupboard-door-robot-poses))) (access-seal-deliver-outer-arms (left)) (access-seal-deliver-outer-grasps (back)) (search-robot-location (a location - (pose ?delivering-counter-top-robot-pose))) + (poses (?delivering-counter-top-robot-pose + ?fetching-counter-top-robot-pose)))) (fetch-robot-location (a location - (pose ?delivering-counter-top-robot-pose))) + (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))))))) + (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" + "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* nil) (setf btr:*visibility-threshold* 0.7) @@ -483,63 +495,63 @@ (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 :cup) - :spawning-poses-relative *apartment-object-spawning-poses-pouring*)) - (park-robot ?initial-parking-pose) - - ;; pick-up cup to pour - (when (<= step 1) - - ;; (exe:perform - ;; (an action - ;; (type transporting) - ;; (object (an object - ;; (type jeroen-cup) - ;; (color blue) - ;; (name jeroen-cup-1) - ;; (location ?location-in-cupboard))) - ;; (cpl:par - ;; (exe:perform (desig:an action - ;; (type parking-arms))) - - (exe:perform (desig:a motion - (type looking) - (pose ?on-counter-top-cup-look-pose)))) - - - (let* ((?object-desig - (desig:an object - (type jeroen-cup) - (color blue) - (name jeroen-cup-1) - (location ?location-on-island))) - (?perceived-object-desig - (exe:perform (desig:an action - (type detecting) - (object ?object-desig))))) - -;; ?perceived-object-desig - ;; (dotimes (i 3) - ;; (exe:perform (desig:an action - ;; (type looking) - ;; (object ?perceived-object-desig)))) + (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 :cup) + :spawning-poses-relative *apartment-object-spawning-poses-pouring*)) + (park-robot ?initial-parking-pose) + + ;; pick-up cup to pour + (when (<= step 1) + + ;; (exe:perform + ;; (an action + ;; (type transporting) + ;; (object (an object + ;; (type jeroen-cup) + ;; (color blue) + ;; (name jeroen-cup-1) + ;; (location ?location-in-cupboard))) + ;; (cpl:par + ;; (exe:perform (desig:an action + ;; (type parking-arms))) + + (exe:perform (desig:a motion + (type looking) + (pose ?on-counter-top-cup-look-pose)))) + + + (let* ((?object-desig + (desig:an object + (type jeroen-cup) + (color blue) + (name jeroen-cup-1) + (location ?location-on-island))) + (?perceived-object-desig (exe:perform (desig:an action - (type picking-up) - (arm :right) - (grasp :left-side) - (object ?perceived-object-desig)))) + (type detecting) + (object ?object-desig))))) + + ;; ?perceived-object-desig + ;; (dotimes (i 3) + ;; (exe:perform (desig:an action + ;; (type looking) + ;; (object ?perceived-object-desig)))) + (exe:perform (desig:an action + (type picking-up) + (arm :right) + (grasp :left-side) + (object ?perceived-object-desig)))) - ))) + ))) diff --git a/cram_demos/cram_projection_demos/src/retail-demo.lisp b/cram_demos/cram_projection_demos/src/retail-demo.lisp index 85f7850e22..b13ec8e4f9 100644 --- a/cram_demos/cram_projection_demos/src/retail-demo.lisp +++ b/cram_demos/cram_projection_demos/src/retail-demo.lisp @@ -564,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_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index 609dd455cc..92ddbb0637 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -373,7 +373,7 @@ :action-timeout action-timeout :check-goal-function (lambda (result status) (declare (ignore result status)) - (or (ensure-arm-joint-goal-reached - goal-configuration-left :left) - (ensure-arm-joint-goal-reached - goal-configuration-right :right)))))) + (and (ensure-arm-joint-goal-reached + goal-configuration-left :left) + (ensure-arm-joint-goal-reached + goal-configuration-right :right)))))) 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 17b68a3350..17af26c05b 100644 --- a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp @@ -104,10 +104,12 @@ (type boolean prefer-base) (type (or cl-transforms:pose null) joint-pose)) - (let ((joint-point-stamped (cl-transforms-stamped:make-point-stamped - cram-tf:*fixed-frame* - 0.0 - (cl-transforms:origin 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 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 a034b673ac..d3b5bcda1c 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) @@ -335,7 +343,8 @@ (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 @@ -357,7 +366,8 @@ (cram-tf:ensure-pose-in-frame pose-stamped-in-map-frame cram-tf:*robot-base-frame* - :use-zero-time t) + ;; :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 From abc62e3a50a686a66e9ffda17cc6ae76dca6de26 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Wed, 14 Sep 2022 17:04:48 +0200 Subject: [PATCH 51/97] [apart-demo] pouring continued --- .../src/apartment-demo.lisp | 148 +++++++++++++----- 1 file changed, 106 insertions(+), 42 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 8198766bd4..0a2ce9b347 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -458,6 +458,7 @@ (an object (type jeroen-cup) (name jeroen-cup-1))) + (?location-on-island (a location (on (an object @@ -468,30 +469,34 @@ (range 0.4) (range-invert 0.3) (for ?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 2.8 0.0) + (cl-transforms:make-3d-vector 1.6 3.0 0.0) (cl-transforms:make-quaternion 0 0 0 1))) - (?fetching-counter-top-robot-pose + + (?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))) + + (?on-counter-top-source-cup-look-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.43 2.6 1.0126) - ;; (cl-transforms:make-quaternion 0 1 0 0))) - (?on-counter-top-cup-look-pose + (cl-transforms:make-3d-vector 2.49 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.39 2.76 1.0126) + (cl-transforms:make-3d-vector 2.49 2.35 1.0126) (cl-transforms:make-quaternion 0 1 0 0))) ) @@ -508,50 +513,109 @@ (spawn-objects-on-fixed-spots :object-types '(:jeroen-cup :cup) :spawning-poses-relative *apartment-object-spawning-poses-pouring*)) - (park-robot ?initial-parking-pose) + (park-robot ?initial-parking-pose)) - ;; pick-up cup to pour - (when (<= step 1) - - ;; (exe:perform - ;; (an action - ;; (type transporting) - ;; (object (an object - ;; (type jeroen-cup) - ;; (color blue) - ;; (name jeroen-cup-1) - ;; (location ?location-in-cupboard))) - ;; (cpl:par - ;; (exe:perform (desig:an action - ;; (type parking-arms))) + ;; 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-cup-look-pose)))) + (exe:perform (desig:a motion + (type looking) + (pose ?on-counter-top-source-cup-look-pose))) - (let* ((?object-desig + (let* ((?source-object-desig (desig:an object (type jeroen-cup) (color blue) (name jeroen-cup-1) (location ?location-on-island))) - (?perceived-object-desig + + (?source-perceived-object-desig (exe:perform (desig:an action (type detecting) - (object ?object-desig))))) + (object ?source-object-desig))))) - ;; ?perceived-object-desig - ;; (dotimes (i 3) - ;; (exe:perform (desig:an action - ;; (type looking) - ;; (object ?perceived-object-desig)))) + ;;TODO: Pick-up has parking in the end this need to be changed + ;;or with constraints (exe:perform (desig:an action (type picking-up) - (arm :right) - (grasp :left-side) - (object ?perceived-object-desig)))) + (arm :left) + (grasp :front) + (object ?source-perceived-object-desig))) + + ;;) + ;;) + ;;(when (<= step 2) + + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?second-cup-park-pose))))) + + (exe:perform (desig:a motion + (type looking) + (pose ?on-counter-top-target-cup-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))))) + ;;) + ;;) + + ;;(when (<= step 3) + + + (exe:perform + (desig:an action + (type pouring) + (source-object ?source-perceived-object-desig) + (target-object ?target-perceived-object-desig)))))))) + + ;; + ;; (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 + - ))) From 91afc047c07e8edca79c53ecb129327362717789 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Fri, 16 Sep 2022 15:20:53 +0200 Subject: [PATCH 52/97] PRECISE-TRACKING for Cart. motions and PARK-ARMS for PICKING-UP --- .../cram_common_designators/src/motions.lisp | 7 ++++-- .../src/specs.lisp | 8 ++++++- .../src/atomic-action-designators.lisp | 1 + .../src/atomic-action-plans.lisp | 7 +++++- .../src/pick-place-designators.lisp | 6 ++++- .../src/pick-place-plans.lisp | 18 +++++++------- .../cram_giskard/src/arm-goals.lisp | 24 ++++++++++++------- .../src/making-goal-messages.lisp | 9 +++++++ .../cram_giskard/src/process-module.lisp | 3 ++- 9 files changed, 60 insertions(+), 23 deletions(-) diff --git a/cram_common/cram_common_designators/src/motions.lisp b/cram_common/cram_common_designators/src/motions.lisp index f29119a14e..e4beb3e38c 100644 --- a/cram_common/cram_common_designators/src/motions.lisp +++ b/cram_common/cram_common_designators/src/motions.lisp @@ -102,7 +102,8 @@ ?move-base ?prefer-base ?align-planes-left ?align-planes-right - ?straight-line)) + ?straight-line + ?precise-tracking)) (property ?designator (:type :moving-tcp)) (once (or (property ?designator (:left-pose ?left-pose)) (equal ?left-pose nil))) @@ -126,7 +127,9 @@ (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 diff --git a/cram_common/cram_designator_specification/src/specs.lisp b/cram_common/cram_designator_specification/src/specs.lisp index 3b073a1594..174df3f33a 100644 --- a/cram_common/cram_designator_specification/src/specs.lisp +++ b/cram_common/cram_designator_specification/src/specs.lisp @@ -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_mobile_pick_place_plans/src/atomic-action-designators.lisp b/cram_common/cram_mobile_pick_place_plans/src/atomic-action-designators.lisp index 148f88e698..4d18e081ad 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 @@ -183,6 +183,7 @@ (: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)) 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 276932a646..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 @@ -93,13 +93,14 @@ ((: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 ?straight-line - ?align-planes-left ?align-planes-right)) + ?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." @@ -142,6 +143,8 @@ while ignoring failures; and execute the last pose with propagating the failures (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 @@ -182,6 +185,8 @@ while ignoring failures; and execute the last pose with propagating the failures (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 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 68bd33981c..cbf9a0b167 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) @@ -155,14 +156,15 @@ :object-designator ?object-designator)) (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)))))) diff --git a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index 92ddbb0637..c884587ce6 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -48,10 +48,12 @@ collision-object-a 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 straight-line 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))))) @@ -113,6 +115,8 @@ (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 @@ -299,12 +303,13 @@ collision-object-a 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 straight-line + (type boolean move-base prefer-base straight-line precise-tracking align-planes-left align-planes-right) (type (or list null) unmovable-joints)) @@ -340,7 +345,8 @@ :straight-line straight-line :align-planes-left align-planes-left :align-planes-right align-planes-right - :unmovable-joints unmovable-joints) + :unmovable-joints unmovable-joints + :precise-tracking precise-tracking) :action-timeout action-timeout :check-goal-function (lambda (result status) (declare (ignore result status)) @@ -373,7 +379,7 @@ :action-timeout action-timeout :check-goal-function (lambda (result status) (declare (ignore result status)) - (and (ensure-arm-joint-goal-reached - goal-configuration-left :left) - (ensure-arm-joint-goal-reached - goal-configuration-right :right)))))) + (or (ensure-arm-joint-goal-reached + goal-configuration-left :left) + (ensure-arm-joint-goal-reached + goal-configuration-right :right)))))) 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 182913d0eb..c63a200001 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -577,6 +577,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 () diff --git a/cram_external_interfaces/cram_giskard/src/process-module.lisp b/cram_external_interfaces/cram_giskard/src/process-module.lisp index 4933d9670c..16e22483e7 100644 --- a/cram_external_interfaces/cram_giskard/src/process-module.lisp +++ b/cram_external_interfaces/cram_giskard/src/process-module.lisp @@ -45,7 +45,8 @@ :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 From a2fc99b8ddd2c2334a4841f84ead6967a6b4dc14 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Fri, 16 Sep 2022 15:21:19 +0200 Subject: [PATCH 53/97] [obj-know + proj-demos] minor pose tweaking --- cram_demos/cram_object_knowledge/src/household.lisp | 2 +- cram_demos/cram_projection_demos/src/apartment-demo.lisp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index e70150e1ef..9f49959166 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -496,7 +496,7 @@ (defparameter *jeroen-cup-grasp-xy-offset* 0.04 "in meters") (defparameter *jeroen-cup-grasp-z-offset* 0.0 "in meters") (defparameter *jeroen-cup-pregrasp-xy-offset* 0.20 "in meters") -(defparameter *jeroen-cup-2nd-pregrasp-xy-offset* 0.07 "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") diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 0a2ce9b347..6d51680ff3 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -228,7 +228,7 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.4 2.6 1.0126) + (cl-transforms:make-3d-vector 2.37 2.6 1.0126) (cl-transforms:make-quaternion 0 0 1 0))) (?delivering-counter-top-robot-pose (cl-transforms-stamped:make-pose-stamped @@ -246,7 +246,7 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.4 2.6 1.0126) + (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 From 94d3402efa8cc401f760ad01432a88acf8e7b34c Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Thu, 22 Sep 2022 14:37:27 +0200 Subject: [PATCH 54/97] working on pouring --- .../src/specs.lisp | 4 +- .../src/package.lisp | 3 +- .../src/trajectories.lisp | 156 ++++++++++- .../src/atomic-action-designators.lisp | 25 +- .../src/pour-designators.lisp | 139 ++++----- .../src/pour-plans.lisp | 264 ++++++++++-------- cram_common/cram_tf/src/utilities.lisp | 25 +- .../cram_object_knowledge/src/household.lisp | 2 +- .../src/apartment-demo.lisp | 47 +++- 9 files changed, 450 insertions(+), 215 deletions(-) diff --git a/cram_common/cram_designator_specification/src/specs.lisp b/cram_common/cram_designator_specification/src/specs.lisp index 174df3f33a..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")) diff --git a/cram_common/cram_manipulation_interfaces/src/package.lisp b/cram_common/cram_manipulation_interfaces/src/package.lisp index f54d6b19c5..22848a98d5 100644 --- a/cram_common/cram_manipulation_interfaces/src/package.lisp +++ b/cram_common/cram_manipulation_interfaces/src/package.lisp @@ -133,4 +133,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/trajectories.lisp b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp index e8e4fd245f..ebec80c5c3 100644 --- a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp +++ b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp @@ -596,6 +596,9 @@ so we assume that all the source contents drops into the target right away." location objects-acted-on &key tilt-angle side) + (print side) + (print grasp) + (print "this is in pouring trajectories") (let* ((source-object (first objects-acted-on)) (source-object-name @@ -617,21 +620,19 @@ so we assume that all the source contents drops into the target right away." side grasp)) (so-T-stdg (get-object-type-to-gripper-transform - source-object-type source-object-name arm grasp)) + target-object-type target-object-name arm side)) (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")))) + (case side + (:top-front (cram-tf:rotate-pose-in-own-frame + to-T-so :z tilt-angle)) + (:top-left (cram-tf:rotate-pose-in-own-frame + to-T-so :x tilt-angle)) + (:top-right (cram-tf:rotate-pose-in-own-frame + to-T-so :y (- tilt-angle))) (t (error "can only pour from :side or :front")))) (to-T-stdg-tilts (reduce #'cram-tf:apply-transform @@ -648,7 +649,134 @@ so we assume that all the source contents drops into the target right away." :tilting-down :tilting-up :retracting) - `(,to-T-stdg - ,to-T-stdg-tilts - ,to-T-stdg - ,to-T-stdg)))) + `((,to-T-stdg) + (,to-T-stdg-tilts) + (,to-T-stdg) + (,to-T-stdg))))) + + +;; (defmethod man-int:get-action-trajectory :heuristics 20 ((action-type (eql :pouring)) +;; arm +;; grasp +;; location +;; objects-acted-on +;; &key ) +;; (let* ((object +;; (car objects-acted-on)) +;; (object-name +;; (desig:desig-prop-value object :name)) +;; (object-type +;; (desig:desig-prop-value object :type)) +;; (bTo +;; (man-int:get-object-transform object)) +;; ;; The first part of the btb-offset transform encodes the +;; ;; translation difference between the gripper and the +;; ;; object. The static defined orientation of bTb-offset +;; ;; describes how the gripper should be orientated to approach +;; ;; the object in which something should be poured into. This +;; ;; depends mostly on the defined coordinate frame of the +;; ;; object and how objects should be rotated to pour something +;; ;; out of them. +;; (bTb-offset +;; (man-int::get-object-type-robot-frame-tilt-approach-transform +;; object-type arm grasp)) +;; ;; Since the grippers orientation should not depend on the +;; ;; orientation of the object it is omitted here. +;; (oTg-std +;; (cram-tf:copy-transform-stamped +;; (man-int:get-object-type-to-gripper-transform +;; object-type object-name arm grasp) +;; :rotation (cl-tf:make-identity-rotation))) +;; (approach-pose +;; (cl-tf:copy-pose-stamped +;; (man-int:calculate-gripper-pose-in-base +;; (cram-tf:apply-transform +;; (cram-tf:copy-transform-stamped +;; bTb-offset +;; :rotation (cl-tf:make-identity-rotation)) +;; bTo) +;; arm oTg-std) +;; :orientation +;; (cl-tf:rotation bTb-offset))) + +;; (tilting-poses +;; (get-tilting-poses grasp (list approach-pose)))) +;; (mapcar (lambda (label poses-in-base) +;; (man-int:make-traj-segment +;; :label label +;; :poses (mapcar +;; (lambda (pose-in-base) +;; (let ((mTb (cram-tf:pose->transform-stamped +;; cram-tf:*fixed-frame* +;; cram-tf:*robot-base-frame* +;; 0.0 +;; (btr:pose (btr:get-robot-object)))) +;; (bTg-std +;; (cram-tf:pose-stamped->transform-stamped +;; pose-in-base +;; (cl-tf:child-frame-id bTo)))) +;; (cl-tf:ensure-pose-stamped +;; (cram-tf:apply-transform mTb bTg-std)))) +;; poses-in-base))) + +;; ;; (mapcar (lambda (label transforms) +;; ;; (make-traj-segment +;; ;; :label label +;; ;; :poses (mapcar (alexandria:curry #'calculate-gripper-pose-in-map +;; ;; b-T-to arm) +;; ;; transforms))) +;; '(:approach +;; :tilting) +;; `((,approach-pose) +;; ,tilting-poses)))) + +(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)))))) 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 4d18e081ad..e7edf99b42 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 @@ -411,4 +411,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/pour-designators.lisp b/cram_common/cram_mobile_pick_place_plans/src/pour-designators.lisp index 5b822be250..121c2648f4 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,38 +29,42 @@ (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 ?_)) + (spec:property ?action-designator (:on-object ?_)) (once (or (spec:property ?action-designator (:arm ?_)) - (spec:property ?action-designator (:source-object ?_)))) + (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 (:left-side :right-side :front :back))) + (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")) + ;; ;; source (-> (spec:property ?action-designator (:arm ?arm)) - (-> (spec:property ?action-designator (:source-object + (-> (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 (:source-object + (-> (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 ~ @@ -68,8 +72,8 @@ ?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)) + ;;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)) @@ -80,7 +84,7 @@ (equal ?objects-acted-on (?current-source-designator ?current-target-designator)) (-> (equal ?arm :left) - (and (lisp-fun man-int:get-action-trajectory :pouring + (and (lisp-fun man-int:get-action-trajectory :pouring ?arm ?grasp nil ?objects-acted-on :tilt-angle ?tilt-angle :side ?side ?left-trajectory) @@ -92,54 +96,61 @@ ?left-tilt-up-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-down-poses NIL (equal ?left-tilt-up-poses NIL) - (equal ?left-retract-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 - :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 - ?right-tilt-up-poses) - (lisp-fun man-int:get-traj-poses-by-label ?right-trajectory :retracting - ?right-retract-poses)) + ;; ;; (and (lisp-fun man-int:get-action-trajectory :pouring + ;; ;; ?arm ?grasp 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 + ;; ;; ?right-tilt-up-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-retract-poses t)) (and (equal ?right-reach-poses NIL) (equal ?right-tilt-down-poses NIL) (equal ?right-tilt-up-poses NIL) (equal ?right-retract-poses NIL))) - (once (or (lisp-pred identity ?left-trajectory) - (lisp-pred identity ?right-trajectory))) + ;; (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 + ?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 + ;; ;; 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 (desig:designator :action ((:type :pouring) - (:source-object ?current-source-designator) + (:object ?current-source-designator) (:arm ?arm) - (:side ?side) + (:configuration ?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) + (: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) @@ -148,24 +159,26 @@ (:left-tilt-up-poses ?left-tilt-up-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-retract-poses ?right-retract-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..6fde92e2bf 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,7 +41,7 @@ (defun pour-without-retries (&key ((:arm ?arm)) - side + ((:side ?side)) grasp ((:left-reach-poses ?left-reach-poses)) ((:right-reach-poses ?right-reach-poses)) @@ -51,138 +51,159 @@ ((: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 + ((: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) - )))) - (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) + + (print "pour-without-retries executable") + (print ?side) + (print ?arm) + (print object) + (print ?on-object) + (print ?left-tilt-down-poses) + (print ?wait-duration) + (print grasp) + (sleep 5) + + + ;; (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)) + ;; (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) + ;; )))) + ;; (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) (exe:perform (desig:an action - (type reaching) - (object ?target-object) + (type pancake) + (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))))) + ;;(collision-mode :allow-all) + ))) + ;;(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))))) (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)) - + (print 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))))) - + ;; (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 ?wait-duration - (wait-duration ?wait-duration)))))))) + (desig:when ?object + (object ?object)) + ;; (desig:when ?wait-duration + ;; (wait-duration ?wait-duration)) + )) + (sleep 5) + )) @@ -204,3 +225,28 @@ (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.0 0.2 0.24)(0 0 -0.707 0.707))) + +(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform + ((object-type (eql :bowl)) + arm + (grasp (eql :top-right))) + '((0.0 -0.2 0.26)(0 0 0.707 0.707))) + +(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform + ((object-type (eql :bowl)) + arm + (grasp (eql :top-front))) + '((-0.02 0.0 0.15)(0 0 0 1))) + +(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform + ((object-type (eql :bowl)) + arm + (grasp (eql :top))) + '((0.02 0.0 0.26)(0 0 1 0))) diff --git a/cram_common/cram_tf/src/utilities.lisp b/cram_common/cram_tf/src/utilities.lisp index 6897b46bba..566b8cf328 100644 --- a/cram_common/cram_tf/src/utilities.lisp +++ b/cram_common/cram_tf/src/utilities.lisp @@ -225,9 +225,19 @@ (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) + (print axis) (let* ((pose-orientation - (cl-transforms:orientation pose)) + (cl-transforms:rotation pose)) (new-orientation (cl-transforms:q* pose-orientation @@ -242,7 +252,10 @@ (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)) + (cl-transforms-stamped:transform-stamped + (copy-transform-stamped pose :rotation new-orientation) + )))) (defun rotate-transform-in-own-frame (transform axis angle) (let* ((transform-rotation @@ -343,14 +356,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)) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 9f49959166..88e4a8b49b 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -928,7 +928,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") diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 6d51680ff3..371d2db28d 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -40,10 +40,10 @@ (defparameter *apartment-object-spawning-poses-pouring* '((:jeroen-cup "island_countertop" - ((0.30 0.05 0.1) (0 0 0 1))) - (:cup - "cabinet1_coloksu_level4" - ((0.15 -0.1 0.08) (0 0 -1 0))))) + ((0.34 -0.9 0.1) (0 0 0 -1))) + (:bowl + "island_countertop" + ((0.34 0.5 0.1) (0 0 0 -1))))) (defun initialize-apartment () (sb-ext:gc :full t) @@ -452,9 +452,9 @@ (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* nil) + (setf proj-reasoning::*projection-checks-enabled* t) (setf btr:*visibility-threshold* 0.7) - (let* ((?object + (let* ((?source-object (an object (type jeroen-cup) (name jeroen-cup-1))) @@ -468,7 +468,23 @@ (side back) (range 0.4) (range-invert 0.3) - (for ?object))) + (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 @@ -499,7 +515,6 @@ (cl-transforms:make-3d-vector 2.49 2.35 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" @@ -511,7 +526,7 @@ (when cram-projection:*projection-environment* (spawn-objects-on-fixed-spots - :object-types '(:jeroen-cup :cup) + :object-types '(:jeroen-cup :bowl) :spawning-poses-relative *apartment-object-spawning-poses-pouring*)) (park-robot ?initial-parking-pose)) @@ -566,9 +581,9 @@ (pose ?on-counter-top-target-cup-look-pose))) (let* ((?target-object-desig (desig:an object - (type jeroen-cup) - (color blue) - (name jeroen-cup-1) + (type bowl) + (color red) + (name bowl-1) (location ?location-on-island))) (?target-perceived-object-desig @@ -584,8 +599,12 @@ (exe:perform (desig:an action (type pouring) - (source-object ?source-perceived-object-desig) - (target-object ?target-perceived-object-desig)))))))) + (object ?source-perceived-object-desig) + (on-object ?target-perceived-object-desig) + (arm :left) + (sides (:top-front)) + ;; (wait-duration 5) + ))))))) ;; ;; (when (<= step 2) From baaeae39c1410a647c11c23141c389934322eee0 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Fri, 23 Sep 2022 13:34:24 +0200 Subject: [PATCH 55/97] pouring is working --- .../cram_manipulation_interfaces/src/trajectories.lisp | 6 +++--- .../cram_mobile_pick_place_plans/src/pour-plans.lisp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp index ebec80c5c3..a7d48b5014 100644 --- a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp +++ b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp @@ -619,8 +619,8 @@ so we assume that all the source contents drops into the target right away." target-object-type target-object-name side grasp)) (so-T-stdg - (get-object-type-to-gripper-transform - target-object-type target-object-name arm side)) + (get-object-type-robot-frame-tilt-approach-transform + target-object-type arm side)) (to-T-stdg (reduce #'cram-tf:apply-transform `(,to-T-so ,so-T-stdg) @@ -649,7 +649,7 @@ so we assume that all the source contents drops into the target right away." :tilting-down :tilting-up :retracting) - `((,to-T-stdg) + `((,so-T-stdg) (,to-T-stdg-tilts) (,to-T-stdg) (,to-T-stdg))))) 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 6fde92e2bf..b380fbf6a6 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 @@ -243,10 +243,10 @@ ((object-type (eql :bowl)) arm (grasp (eql :top-front))) - '((-0.02 0.0 0.15)(0 0 0 1))) + '((-0.09 0.0 0.40)(0 0 1 1))) (defmethod man-int:get-object-type-robot-frame-tilt-approach-transform ((object-type (eql :bowl)) arm (grasp (eql :top))) - '((0.02 0.0 0.26)(0 0 1 0))) + '((0.0 0.0 0.26)(0 0 1 0))) From 1b323679aeba91fbc115fd0c2c114be8b70039c0 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Thu, 29 Sep 2022 14:51:44 +0200 Subject: [PATCH 56/97] [apartment-demo] demo-merged now --- .../src/trajectories.lisp | 261 +++++----- .../src/atomic-action-designators.lisp | 26 + .../src/pour-designators.lisp | 168 +++--- .../src/pour-plans.lisp | 317 +++++++----- cram_common/cram_tf/src/utilities.lisp | 7 +- .../src/apartment-demo.lisp | 482 +++++++++++++++++- 6 files changed, 932 insertions(+), 329 deletions(-) diff --git a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp index a7d48b5014..cc0adb5f4e 100644 --- a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp +++ b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp @@ -597,138 +597,135 @@ so we assume that all the source contents drops into the target right away." objects-acted-on &key tilt-angle side) (print side) - (print grasp) + (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)) + (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 + + + (bTb-offset (get-object-type-robot-frame-tilt-approach-transform target-object-type arm side)) - (to-T-stdg - (reduce #'cram-tf:apply-transform - `(,to-T-so ,so-T-stdg) - :from-end T)) - (to-T-so-tilts + ;; 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 + (cram-tf:copy-transform-stamped + bTb-offset + :rotation (cl-tf:make-identity-rotation)) + b-T-to) + arm oTg-std) + :orientation + (cl-tf:rotation bTb-offset))) + (tilt-angle (cram-math:degrees->radians 70)) + (pre-tilting-poses + (case side + (:top-front (rotate-pose-in-own-frame-and-change-z + approach-pose :y (cram-math:degrees->radians 40) 0.6 0 0.01)) + ;;1.5 was nice in projection + (:top-left (rotate-pose-in-own-frame-and-change-z + approach-pose :x (cram-math:degrees->radians 40) 0.0 -0.05 0.031)) + (:top-right (rotate-pose-in-own-frame-and-change-z + approach-pose :x (- (cram-math:degrees->radians 40)) 0.0 0.05 0.031 )) + (t (error "can only pour from :side or :front")))) + + (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 - to-T-so :z tilt-angle)) + tilting-poses :y tilt-angle)) (:top-left (cram-tf:rotate-pose-in-own-frame - to-T-so :x tilt-angle)) + tilting-poses :x tilt-angle)) (:top-right (cram-tf:rotate-pose-in-own-frame - to-T-so :y (- tilt-angle))) - (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))) + tilting-poses :x (- tilt-angle))) + (t (error "can only pour from :side or :front"))) ) - (mapcar (lambda (label transforms) - (make-traj-segment + (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) - `((,so-T-stdg) - (,to-T-stdg-tilts) - (,to-T-stdg) - (,to-T-stdg))))) - - -;; (defmethod man-int:get-action-trajectory :heuristics 20 ((action-type (eql :pouring)) -;; arm -;; grasp -;; location -;; objects-acted-on -;; &key ) -;; (let* ((object -;; (car objects-acted-on)) -;; (object-name -;; (desig:desig-prop-value object :name)) -;; (object-type -;; (desig:desig-prop-value object :type)) -;; (bTo -;; (man-int:get-object-transform object)) -;; ;; The first part of the btb-offset transform encodes the -;; ;; translation difference between the gripper and the -;; ;; object. The static defined orientation of bTb-offset -;; ;; describes how the gripper should be orientated to approach -;; ;; the object in which something should be poured into. This -;; ;; depends mostly on the defined coordinate frame of the -;; ;; object and how objects should be rotated to pour something -;; ;; out of them. -;; (bTb-offset -;; (man-int::get-object-type-robot-frame-tilt-approach-transform -;; object-type arm grasp)) -;; ;; Since the grippers orientation should not depend on the -;; ;; orientation of the object it is omitted here. -;; (oTg-std -;; (cram-tf:copy-transform-stamped -;; (man-int:get-object-type-to-gripper-transform -;; object-type object-name arm grasp) -;; :rotation (cl-tf:make-identity-rotation))) -;; (approach-pose -;; (cl-tf:copy-pose-stamped -;; (man-int:calculate-gripper-pose-in-base -;; (cram-tf:apply-transform -;; (cram-tf:copy-transform-stamped -;; bTb-offset -;; :rotation (cl-tf:make-identity-rotation)) -;; bTo) -;; arm oTg-std) -;; :orientation -;; (cl-tf:rotation bTb-offset))) - -;; (tilting-poses -;; (get-tilting-poses grasp (list approach-pose)))) -;; (mapcar (lambda (label poses-in-base) -;; (man-int:make-traj-segment -;; :label label -;; :poses (mapcar -;; (lambda (pose-in-base) -;; (let ((mTb (cram-tf:pose->transform-stamped -;; cram-tf:*fixed-frame* -;; cram-tf:*robot-base-frame* -;; 0.0 -;; (btr:pose (btr:get-robot-object)))) -;; (bTg-std -;; (cram-tf:pose-stamped->transform-stamped -;; pose-in-base -;; (cl-tf:child-frame-id bTo)))) -;; (cl-tf:ensure-pose-stamped -;; (cram-tf:apply-transform mTb bTg-std)))) -;; poses-in-base))) - -;; ;; (mapcar (lambda (label transforms) -;; ;; (make-traj-segment -;; ;; :label label -;; ;; :poses (mapcar (alexandria:curry #'calculate-gripper-pose-in-map -;; ;; b-T-to arm) -;; ;; transforms))) -;; '(:approach -;; :tilting) -;; `((,approach-pose) -;; ,tilting-poses)))) + :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) @@ -780,3 +777,39 @@ so we assume that all the source contents drops into the target right away." (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 e7edf99b42..582ed7ade6 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 @@ -124,6 +124,32 @@ (: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 :avoid-all))) + (infer-motion-flags ?action-designator + ?_ ?move-base ?align-planes-left ?align-planes-right) + (-> (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 nil) + (:straight-line ?straight-line) + (:align-planes-left nil) + (:align-planes-right nil)) + ?resolved-action-designator)) + (<- (desig:action-grounding ?action-designator (move-arms-in-sequence ?resolved-action-designator)) (or (spec:property ?action-designator (:type :retracting))) 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 121c2648f4..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 @@ -34,17 +34,20 @@ (<- (desig:action-grounding ?action-designator (pour ?resolved-designator)) (spec:property ?action-designator (:type :pouring)) (spec:property ?action-designator (:on-object ?_)) - (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)))) - + ;;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)))) + @@ -55,23 +58,26 @@ (-> (spec:property ?action-designator (:configuration ?side)) (true) (format "WARNINGL: please specify a side")) - ;; ;; 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) + (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) @@ -81,60 +87,78 @@ (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 + (and (lisp-fun man-int:get-action-trajectory :pouring + ?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-down-poses NIL) (equal ?left-tilt-up-poses NIL) - (equal ?left-retract-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 - ;; ;; :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 - ;; ;; ?right-tilt-up-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-retract-poses t)) + (and (lisp-fun man-int:get-action-trajectory :pouring + ?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 + ?right-tilt-up-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 ?taget-type ?tilt-angle - ?wait-duration)) - ;; ;; look pose - ;; (-> (lisp-pred identity ?left-grasp-poses) + ;; (-> (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-trajectory) ;; (equal ?left-tilt-down-poses (?look-pose . ?_)) ;; (equal ?right-tilt-down-poses (?look-pose . ?_))) - ;; ;; should look or not? + ;; ;; ;; 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) @@ -143,25 +167,29 @@ ;;put together resulting designator (desig:designator :action ((:type :pouring) - (:object ?current-source-designator) + ;;(:object ?current-source-designator) (:arm ?arm) (:configuration ?side) - (:grasp ?grasp) + ;;(: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) + ;(: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) + (:right-tilt-second-poses ?right-tilt-second-poses) + (:right-tilt-third-poses ?right-tilt-third-poses) ) - + ?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 b380fbf6a6..8db2e2f881 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 @@ -42,32 +42,25 @@ (defun pour-without-retries (&key ((:arm ?arm)) ((:side ?side)) - grasp + ;;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)) + ((:right-tilt-second-poses ?right-tilt-second-poses)) + ((:right-tilt-third-poses ?right-tilt-third-poses)) + ((:on-object ?on-object)) - object + ;;object ((:wait-duration ?wait-duration)) ((:look-location ?look-location)) robot-arm-is-also-a-neck &allow-other-keys) - - (print "pour-without-retries executable") - (print ?side) - (print ?arm) - (print object) - (print ?on-object) - (print ?left-tilt-down-poses) - (print ?wait-duration) - (print grasp) - (sleep 5) - ;; (declare (type (or null list) ;; ?left-reach-poses ?right-reach-poses @@ -79,103 +72,203 @@ ;; ;;(type keyword ?arm side grasp) ;; (type number ?wait-duration) ;; (ignore side grasp 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) - ;; )))) - ;; (let ((?goal `(cpoe:tool-frames-at ,?left-reach-poses ,?right-reach-poses))) - (exe:perform - (desig:an action - (type pancake) - (object ?on-object) - (left-poses ?left-reach-poses) - (right-poses ?right-reach-poses) - ;;(collision-mode :allow-all) - ))) - ;;(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))))) + (let* ((sleepy t) + (movy t) + (?align-planes-left nil) + (?align-planes-right 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 + (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) + (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-tilt-down) + ;; "Manipulation messed up: ~a~%Failing." + ;; e) + + ;; (cpl:do-retry giskardside-retries + ;; (cpl:retry)) + ;; (return))) + + + ;; (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-down-poses ,?right-tilt-down-poses))) + ;; (exe:perform + ;; (desig:an action + ;; (type tilting) + ;; (object ?on-object) + ;; (left-poses ?left-tilt-down-poses) + ;; (right-poses ?right-tilt-down-poses) + ;; (align-planes-left ?align-planes-left) + ;; (align-planes-right ?align-planes-right) + ;; (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 reaching) + (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 10)) + + ;; (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-second) + ;; "Manipulation messed up: ~a~%Failing." + ;; e) + + ;; (cpl:do-retry giskardside-retries + ;; (cpl:retry)) + ;; (return))) + + + ;; (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-second-poses ,?right-tilt-up-poses))) + ;; (exe:perform + ;; (desig:an action + ;; (type tilting) + ;; (object ?on-object) + ;; (left-poses ?left-tilt-second-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 10)) + + + ;; (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-third) + ;; "Manipulation messed up: ~a~%Failing." + ;; e) + + ;; (cpl:do-retry giskardside-retries + ;; (cpl:retry)) + ;; (return))) + + + ;; (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-third-poses ,?right-tilt-up-poses))) + ;; (exe:perform + ;; (desig:an action + ;; (type tilting) + ;; (object ?on-object) + ;; (left-poses ?left-tilt-third-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) + (goal ?goal)))) + )) + + + ) ) + (defun pour (&key ((:arm ?arm)) sides - ((:object ?object)) + ;; ((:object ?object)) ((:on-object ?on-object)) ((:wait-duration ?wait-duration)) &allow-other-keys) - (declare (type desig:object-designator ?object ?on-object) + (declare (type desig:object-designator ;; ?object + ?on-object) (type keyword ?arm) ;(type (or nil number) ?wait-duration) (type list sides)) - (print 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 @@ -197,12 +290,11 @@ (on-object ?on-object) (desig:when ?arm (arm ?arm)) - (desig:when ?object - (object ?object)) - ;; (desig:when ?wait-duration - ;; (wait-duration ?wait-duration)) + ;; (desig:when ?object + ;; (object ?object)) + (desig:when ?wait-duration + (wait-duration ?wait-duration)) )) - (sleep 5) )) @@ -231,22 +323,17 @@ ((object-type (eql :bowl)) arm (grasp (eql :top-left))) - '((0.0 0.2 0.24)(0 0 -0.707 0.707))) + '((-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.0 -0.2 0.26)(0 0 0.707 0.707))) + '((-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-front))) - '((-0.09 0.0 0.40)(0 0 1 1))) + '((-0.1 0.0 0.019)(0 0 0 1))) -(defmethod man-int:get-object-type-robot-frame-tilt-approach-transform - ((object-type (eql :bowl)) - arm - (grasp (eql :top))) - '((0.0 0.0 0.26)(0 0 1 0))) diff --git a/cram_common/cram_tf/src/utilities.lisp b/cram_common/cram_tf/src/utilities.lisp index 566b8cf328..a93a8fc4c9 100644 --- a/cram_common/cram_tf/src/utilities.lisp +++ b/cram_common/cram_tf/src/utilities.lisp @@ -235,9 +235,8 @@ (or rotation (cl-transforms-stamped:rotation transform-stamped)))) (defun rotate-pose-in-own-frame (pose axis angle) - (print axis) (let* ((pose-orientation - (cl-transforms:rotation pose)) + (cl-transforms:orientation pose)) (new-orientation (cl-transforms:q* pose-orientation @@ -253,9 +252,7 @@ (cl-transforms-stamped:copy-pose-stamped pose :orientation new-orientation)) (cl-transforms:pose (cl-transforms:copy-pose pose :orientation new-orientation)) - (cl-transforms-stamped:transform-stamped - (copy-transform-stamped pose :rotation new-orientation) - )))) + ))) (defun rotate-transform-in-own-frame (transform axis angle) (let* ((transform-rotation diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 371d2db28d..61de5878a0 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -35,12 +35,15 @@ ((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))))) (defparameter *apartment-object-spawning-poses-pouring* '((:jeroen-cup "island_countertop" - ((0.34 -0.9 0.1) (0 0 0 -1))) + ((0.34 -0.5 0.1) (0 0 0 -1))) (:bowl "island_countertop" ((0.34 0.5 0.1) (0 0 0 -1))))) @@ -491,7 +494,7 @@ (cl-transforms-stamped:make-pose-stamped cram-tf:*fixed-frame* 0.0 - (cl-transforms:make-3d-vector 1.6 3.0 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 @@ -500,6 +503,13 @@ 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 @@ -512,8 +522,10 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.49 2.35 1.0126) + (cl-transforms:make-3d-vector 2.49 2.2 1.0126) (cl-transforms:make-quaternion 0 1 0 0))) + + ) (when (<= step 0) (initialize-apartment) @@ -560,51 +572,97 @@ ;;TODO: Pick-up has parking in the end this need to be changed ;;or with constraints - (exe:perform (desig:an action + (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 :left) + (arm :right) (grasp :front) - (object ?source-perceived-object-desig))) + (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) - + (when (<= step 2) (exe:perform - (desig:an action - (type going) - (target (desig:a location - (pose ?second-cup-park-pose))))) + (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))) + (location ?location-on-island-target))) (?target-perceived-object-desig (exe:perform (desig:an action (type detecting) (object ?target-object-desig))))) - ;;) - ;;) - - ;;(when (<= step 3) - - + (exe:perform (desig:an action + (type moving-torso) + (joint-angle upper-limit))) + (exe:perform (desig:an action (type pouring) - (object ?source-perceived-object-desig) + ;(object ?source-perceived-object-desig) (on-object ?target-perceived-object-desig) - (arm :left) - (sides (:top-front)) - ;; (wait-duration 5) - ))))))) + (arm :right) + (sides (:top-right)) + (wait-duration 5) + ))) +))) ;; ;; (when (<= step 2) @@ -638,3 +696,377 @@ +(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* ((?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.37 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))) + + (?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))) + + ;;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.49 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.49 2.2 1.0126) + (cl-transforms:make-quaternion 0 1 0 0)))) + + + ;; park robot into the initial position + (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 :cup :bowl) + :spawning-poses-relative *apartment-object-spawning-poses*)) + (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))) + (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))))) + + ;;pick-up-cup + (when (<= step 2) + + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?percieve-blue-cup-pose-pouring))))) + + (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))) + + (?source-perceived-object-desig + (exe:perform (desig:an action + (type detecting) + (object ?source-object-desig)))) + + + (?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:a motion + (type looking) + (pose ?on-counter-top-target-cup-look-pose))) + + (exe:perform (desig:an action + (type detecting) + (object ?target-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 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))))) + + )) + + + + From 2c318c4ac9d6c633bd80b896aa03333f213fc979 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 19:34:58 +0200 Subject: [PATCH 57/97] [mpp-plans] fixed detachment event again --- .../cram_mobile_pick_place_plans/src/pick-place-plans.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 cbf9a0b167..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 @@ -153,7 +153,7 @@ (cram-occasions-events:on-event (make-instance 'cpoe:object-detached-robot :arm ?arm - :object-designator ?object-designator)) + :object-name (desig:desig-prop-value ?object-designator :name))) (cpl:fail 'common-fail:gripper-closed-completely :description "Object slipped"))) (when park-arms From c58c989cee2bd28193f9744dc656db2f9f658050 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 20:04:49 +0200 Subject: [PATCH 58/97] [btr-belief] in consistency checking increased number of search iterations --- .../src/consistency-checking.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 436b855e9e..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 @@ -51,7 +51,7 @@ If the threshold is 0, correct along all axes.") "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* 10 +(defparameter *perception-noise-correction-iterations* 30 "Number of distances to try the correction approach with. In times.") (defparameter *perception-noise-correction-step* 0.01 From cd55d346d8aa712d06df2e9a1b5e3c05633f9124 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 20:05:03 +0200 Subject: [PATCH 59/97] [proj-demos] apartment demo indentation --- .../src/apartment-demo.lisp | 293 ++++++++---------- 1 file changed, 135 insertions(+), 158 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 61de5878a0..08b0c24c94 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -445,8 +445,8 @@ (target ?location-in-cupboard-with-attachment) (deliver-robot-location (a location (pose ?detecting-cupboard-robot-pose - ;; ?initial-parking-pose - ))))))) + ;; ?initial-parking-pose + ))))))) (finalize)) @@ -457,11 +457,11 @@ ;;(urdf-proj:with-simulated-robot (setf proj-reasoning::*projection-checks-enabled* t) (setf btr:*visibility-threshold* 0.7) - (let* ((?source-object + (let* ((?source-object (an object (type jeroen-cup) (name jeroen-cup-1))) - + (?location-on-island (a location (on (an object @@ -477,7 +477,7 @@ (an object (type bowl) (name bowl-1))) - + (?location-on-island-target (a location (on (an object @@ -488,7 +488,7 @@ (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 @@ -497,36 +497,34 @@ (cl-transforms:make-3d-vector 1.6 3.3 0.0) (cl-transforms:make-quaternion 0 0 0 1))) - (?second-cup-park-pose + (?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 + (?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.49 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))) - - - ) + (cl-transforms:make-quaternion 0 1 0 0)))) + (when (<= step 0) (initialize-apartment) ;; (btr-belief:vary-kitchen-urdf '(("handle_cab1_top_door_joint" @@ -544,7 +542,6 @@ ;; 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 @@ -557,14 +554,13 @@ (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))) - + (?source-perceived-object-desig (exe:perform (desig:an action (type detecting) @@ -573,127 +569,115 @@ ;;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 + (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 - (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 - + + ;; (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 + (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)) @@ -849,7 +833,7 @@ 0.0 (cl-transforms:make-3d-vector 1.8 2.8 0.0d0) (cl-transforms:make-quaternion 0.0d0 0.0d0 -0.1 1))) - + (?on-counter-top-cup-upsidedown-pose (cl-transforms-stamped:make-pose-stamped "map" @@ -864,12 +848,12 @@ (cl-transforms:make-quaternion 0 1 0 0))) ;;hardcoded pouring stuff - + (?source-object (an object (type jeroen-cup) (name jeroen-cup-1))) - + (?location-on-island (a location (on (an object @@ -885,7 +869,7 @@ (an object (type bowl) (name bowl-1))) - + (?location-on-island-target (a location (on (an object @@ -896,7 +880,7 @@ (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 @@ -911,15 +895,14 @@ 0.0 (cl-transforms:make-3d-vector 2.49 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.49 2.2 1.0126) (cl-transforms:make-quaternion 0 1 0 0)))) - + ;; park robot into the initial position (when (<= step 0) @@ -993,37 +976,37 @@ (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))) - + (?source-perceived-object-desig (exe:perform (desig:an action (type detecting) (object ?source-object-desig)))) - + (?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:a motion (type looking) (pose ?on-counter-top-target-cup-look-pose))) - + (exe:perform (desig:an action (type detecting) (object ?target-object-desig)))))) - + ;;TODO: Pick-up has parking in the end this need to be changed ;;or with constraints @@ -1035,12 +1018,12 @@ (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) @@ -1054,8 +1037,7 @@ (on-object ?target-perceived-object-desig) (arm :right) (sides (:top-right)) - (wait-duration 5) - )))) + (wait-duration 5))))) (exe:perform (desig:an action @@ -1063,10 +1045,5 @@ (arm right) (object ?source-object) (target (a location - (pose ?on-counter-top-cup-pose))))) - - )) - - - + (pose ?on-counter-top-cup-pose))))))) From 0db9e5b1aa0dd2411ed8bf66b754972be73c83dc Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 21:12:54 +0200 Subject: [PATCH 60/97] [proj-demos] apartment demo removed copy paste of pick-place part --- .../src/apartment-demo.lisp | 227 +----------------- 1 file changed, 8 insertions(+), 219 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 08b0c24c94..4395e5bc9a 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -59,6 +59,7 @@ ;; (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 @@ -343,7 +344,7 @@ (goal ?goal))))) ;; put cup from island into dishwasher - (when (<= step 2) + (when nil ; (<= step 2) (exe:perform (an action (type transporting) @@ -378,7 +379,7 @@ (grasps (front))))) ;; put cup from dishwasher onto table upside-down - (when (<= step 3) + (when nil ; (<= step 3) ;; let ((?goal `(cpoe:object-at-location ,?object ,?location-in-hand))) (exe:perform (an action @@ -413,7 +414,7 @@ (seal-search-outer-grasps (back))))) ;; bring cup to cupboard - (when (<= step 4) + (when nil ; (<= step 4) (exe:perform (an action (type transporting) @@ -448,7 +449,8 @@ ;; ?initial-parking-pose ))))))) - (finalize)) + ;; (finalize) + ) @@ -685,167 +687,12 @@ (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 + (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))) - (?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))) - - (?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))) ;;hardcoded pouring stuff @@ -903,65 +750,7 @@ (cl-transforms:make-3d-vector 2.49 2.2 1.0126) (cl-transforms:make-quaternion 0 1 0 0)))) - - ;; park robot into the initial position - (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 :cup :bowl) - :spawning-poses-relative *apartment-object-spawning-poses*)) - (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))) - (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))))) + (apartment-demo :step step) ;;pick-up-cup (when (<= step 2) From ce310382ae8fa89e631dcf3b7cf767ff9fb3ef12 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 22:07:18 +0200 Subject: [PATCH 61/97] [rk] cup is right side up in cupboard: such that we can pour from it into the bowl. --- cram_external_interfaces/cram_robokudo/src/action-client.lisp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cram_external_interfaces/cram_robokudo/src/action-client.lisp b/cram_external_interfaces/cram_robokudo/src/action-client.lisp index d3b5bcda1c..6b5238c7f4 100644 --- a/cram_external_interfaces/cram_robokudo/src/action-client.lisp +++ b/cram_external_interfaces/cram_robokudo/src/action-client.lisp @@ -355,7 +355,8 @@ (cl-transforms:origin pose-stamped-in-map-frame-original-orientation)) 1.9) - (cl-transforms:make-quaternion 1 0 0 0) + ;; (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 From ef631dfbae441a6a4ce0b967a6bc9baa86624e35 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 22:07:47 +0200 Subject: [PATCH 62/97] [giskard] align planes are back --- .../cram_giskard/src/arm-goals.lisp | 78 +++++++++---------- 1 file changed, 38 insertions(+), 40 deletions(-) diff --git a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index c884587ce6..c54124fd1c 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -81,27 +81,26 @@ :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)))) + (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 @@ -190,25 +189,24 @@ (cl-transforms:make-identity-pose)) :max-velocity *base-max-velocity-slow-xy* :avoid-collisions-much nil) - ;; (when align-planes-left - ;; (make-align-planes-tool-frame-constraint - ;; :left - ;; (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-tool-frame-constraint - ;; :right - ;; (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-left + (make-align-planes-tool-frame-constraint + :left + (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-tool-frame-constraint + :right + (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))))) :joint-constraints (list (make-joint-constraint joint-state-left) (make-joint-constraint joint-state-right)) :collisions (list (make-avoid-all-collision)))) From 40a7f91364baa6df39552226f24906ae2b4504fa Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 22:08:16 +0200 Subject: [PATCH 63/97] [proj-demos] in apartment demo changed going to navigating for better failure handling --- .../src/apartment-demo.lisp | 40 +++++++++++-------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 4395e5bc9a..4743a575cc 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -757,13 +757,15 @@ (exe:perform (desig:an action - (type going) - (target (desig:a location - (pose ?percieve-blue-cup-pose-pouring))))) + (type navigating) + (location (desig:a location + (pose ?percieve-blue-cup-pose-pouring))))) - (exe:perform (desig:a motion - (type looking) - (pose ?on-counter-top-source-cup-look-pose))) + (exe:perform + (desig:an action + (type looking) + (target (desig:a location + (pose ?on-counter-top-source-cup-look-pose))))) (let* ((?source-object-desig @@ -774,9 +776,10 @@ (location ?location-on-island))) (?source-perceived-object-desig - (exe:perform (desig:an action - (type detecting) - (object ?source-object-desig)))) + (exe:perform + (desig:an action + (type detecting) + (object ?source-object-desig)))) (?target-object-desig @@ -787,14 +790,17 @@ (location ?location-on-island-target))) (?target-perceived-object-desig - (cpl::seq - (exe:perform (desig:a motion - (type looking) - (pose ?on-counter-top-target-cup-look-pose))) - - (exe:perform (desig:an action - (type detecting) - (object ?target-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)))))) ;;TODO: Pick-up has parking in the end this need to be changed From a55ed3657dcb17fa1e2355ba09fccb0bbd2ef85f Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 22:41:41 +0200 Subject: [PATCH 64/97] [obj-know] grasping jeroen-cup 1 cm higher: because there is no upside-down grasps anymore. --- cram_demos/cram_object_knowledge/src/household.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 88e4a8b49b..2ebc4ceb50 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -494,7 +494,7 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;; jeroen-cup ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (defparameter *jeroen-cup-grasp-xy-offset* 0.04 "in meters") -(defparameter *jeroen-cup-grasp-z-offset* 0.0 "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") From 3b320937a43da3c42c33d2099b489970219f9833 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Mon, 3 Oct 2022 22:42:54 +0200 Subject: [PATCH 65/97] [proj-demos] in apartment demo changed PICKING-UP into FETCHING for better failure handling. --- .../src/apartment-demo.lisp | 100 ++++++++++++------ 1 file changed, 69 insertions(+), 31 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 4743a575cc..5ec293eba4 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -761,13 +761,6 @@ (location (desig:a location (pose ?percieve-blue-cup-pose-pouring))))) - (exe:perform - (desig:an action - (type looking) - (target (desig:a location - (pose ?on-counter-top-source-cup-look-pose))))) - - (let* ((?source-object-desig (desig:an object (type jeroen-cup) @@ -775,11 +768,37 @@ (name jeroen-cup-1) (location ?location-on-island))) + (?perceive-source-goal + `(cpoe:object-in-hand ,(an object + (type jeroen-cup) + (name jeroen-cup-1)))) (?source-perceived-object-desig - (exe:perform - (desig:an action - (type detecting) - (object ?source-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))) + (goal ?goal))))) (?target-object-desig @@ -796,7 +815,6 @@ (type looking) (target (desig:a location (pose ?on-counter-top-target-cup-look-pose))))) - (exe:perform (desig:an action (type detecting) @@ -805,27 +823,47 @@ ;;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))) + ;; (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 + ;; (let ((?source-perceived-object-desig + ;; (cpl:seq + ;; (exe:perform + ;; (desig:an action + ;; (type parking-arms))) + ;; (exe:perform + ;; (desig:an action + ;; (type looking) + ;; (target (desig:a location + ;; (pose ?on-counter-top-source-cup-look-pose))))) + ;; (exe:perform + ;; (desig:an action + ;; (type detecting) + ;; (object ?source-object-desig)))))) + ;; (cpl:retry))) + ;; (return))) + + + ;; (let ((?goal `(cpoe:object-in-hand ,(an object + ;; (type jeroen-cup) + ;; (name jeroen-cup-1))))) + ;; (exe:perform + ;; (desig:an action + ;; (type picking-up) + ;; (arm :right) + ;; (grasp :front) + ;; (object ?source-perceived-object-desig) + ;; (park-arms NIL) + ;; (goal ?goal)))))) - (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 pouring) From a0b19cc51c85e90c946d0b0a72524a3cb842459d Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 4 Oct 2022 12:37:49 +0200 Subject: [PATCH 66/97] [c_projection_demos] apartement demo adjustments for pouring --- .../src/trajectories.lisp | 10 +-- .../src/atomic-action-designators.lisp | 5 +- .../src/pour-plans.lisp | 90 +------------------ 3 files changed, 11 insertions(+), 94 deletions(-) diff --git a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp index cc0adb5f4e..3a071334bb 100644 --- a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp +++ b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp @@ -637,16 +637,16 @@ so we assume that all the source contents drops into the target right away." arm oTg-std) :orientation (cl-tf:rotation bTb-offset))) - (tilt-angle (cram-math:degrees->radians 70)) + (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 40) 0.6 0 0.01)) - ;;1.5 was nice in projection + approach-pose :y (cram-math:degrees->radians 70) 0.6 0 0.01)) (:top-left (rotate-pose-in-own-frame-and-change-z - approach-pose :x (cram-math:degrees->radians 40) 0.0 -0.05 0.031)) + approach-pose :x (cram-math:degrees->radians 70) 0.0 -0.05 0.031)) (:top-right (rotate-pose-in-own-frame-and-change-z - approach-pose :x (- (cram-math:degrees->radians 40)) 0.0 0.05 0.031 )) + approach-pose :x (- (cram-math:degrees->radians 70)) 0.0 0.05 -0.05 )) + ;;0.031 z (t (error "can only pour from :side or :front")))) (tilting-poses 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 582ed7ade6..f69e15e8da 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 @@ -144,10 +144,11 @@ (:left-poses ?left-poses) (:right-poses ?right-poses) (:collision-mode ?collision) - (:move-base nil) + (:move-base t) (:straight-line ?straight-line) (:align-planes-left nil) - (:align-planes-right nil)) + (:align-planes-right nil) + (:precise-tracking T)) ?resolved-action-designator)) (<- (desig:action-grounding ?action-designator (move-arms-in-sequence 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 8db2e2f881..d593715376 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 @@ -102,32 +102,7 @@ )) - ;; (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-tilt-down) - ;; "Manipulation messed up: ~a~%Failing." - ;; e) - - ;; (cpl:do-retry giskardside-retries - ;; (cpl:retry)) - ;; (return))) - - - ;; (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-down-poses ,?right-tilt-down-poses))) - ;; (exe:perform - ;; (desig:an action - ;; (type tilting) - ;; (object ?on-object) - ;; (left-poses ?left-tilt-down-poses) - ;; (right-poses ?right-tilt-down-poses) - ;; (align-planes-left ?align-planes-left) - ;; (align-planes-right ?align-planes-right) - ;; (goal ?goal)))) - ;; )) + (when sleepy (sleep 2)) @@ -148,7 +123,7 @@ (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-up-poses ,?right-tilt-up-poses))) (exe:perform (desig:an action - (type reaching) + (type tilting) (object ?on-object) (left-poses ?left-tilt-up-poses) (right-poses ?right-tilt-up-poses) @@ -158,66 +133,7 @@ ;;(collision-mode :allow-attached) (goal ?goal)))) )) - ;; (when sleepy - ;; (sleep 10)) - - ;; (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-second) - ;; "Manipulation messed up: ~a~%Failing." - ;; e) - - ;; (cpl:do-retry giskardside-retries - ;; (cpl:retry)) - ;; (return))) - - ;; (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-second-poses ,?right-tilt-up-poses))) - ;; (exe:perform - ;; (desig:an action - ;; (type tilting) - ;; (object ?on-object) - ;; (left-poses ?left-tilt-second-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 10)) - - - ;; (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-third) - ;; "Manipulation messed up: ~a~%Failing." - ;; e) - - ;; (cpl:do-retry giskardside-retries - ;; (cpl:retry)) - ;; (return))) - - - ;; (let ((?goal `(cpoe:tool-frames-at ,?left-tilt-third-poses ,?right-tilt-up-poses))) - ;; (exe:perform - ;; (desig:an action - ;; (type tilting) - ;; (object ?on-object) - ;; (left-poses ?left-tilt-third-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)) @@ -329,7 +245,7 @@ ((object-type (eql :bowl)) arm (grasp (eql :top-right))) - '((-0.01 -0.245 0.020)(0 0 0 1))) + '((-0.01 -0.245 0.025)(0 0 0 1))) (defmethod man-int:get-object-type-robot-frame-tilt-approach-transform ((object-type (eql :bowl)) From afb233d998b225a69cb1930485766a42b77d0d81 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 4 Oct 2022 16:48:19 +0200 Subject: [PATCH 67/97] demo tweaking pouring works! --- .../src/trajectories.lisp | 34 ++-- .../src/atomic-action-designators.lisp | 28 +-- .../src/pour-plans.lisp | 39 +++-- .../src/apartment-demo.lisp | 162 +++++++----------- .../src/arm-kinematics.lisp | 2 +- 5 files changed, 133 insertions(+), 132 deletions(-) diff --git a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp index 3a071334bb..7ec14908a0 100644 --- a/cram_common/cram_manipulation_interfaces/src/trajectories.lisp +++ b/cram_common/cram_manipulation_interfaces/src/trajectories.lisp @@ -615,7 +615,7 @@ so we assume that all the source contents drops into the target right away." (get-object-transform target-object)) - (bTb-offset + (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 @@ -630,22 +630,36 @@ so we assume that all the source contents drops into the target right away." (cl-tf:copy-pose-stamped (man-int:calculate-gripper-pose-in-base (cram-tf:apply-transform - (cram-tf:copy-transform-stamped - bTb-offset - :rotation (cl-tf:make-identity-rotation)) - b-T-to) + b-T-to + to-T-to-offset) arm oTg-std) :orientation - (cl-tf:rotation bTb-offset))) + (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 70) 0.6 0 0.01)) + 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 70) 0.0 -0.05 0.031)) - (:top-right (rotate-pose-in-own-frame-and-change-z - approach-pose :x (- (cram-math:degrees->radians 70)) 0.0 0.05 -0.05 )) + 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")))) 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 f69e15e8da..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,7 +110,9 @@ (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)) @@ -134,9 +136,11 @@ (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))) + (equal ?collision :allow-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)) @@ -144,7 +148,7 @@ (:left-poses ?left-poses) (:right-poses ?right-poses) (:collision-mode ?collision) - (:move-base t) + (:move-base ?move-base) (:straight-line ?straight-line) (:align-planes-left nil) (:align-planes-right nil) @@ -161,18 +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) - ;; (-> (or (desig:desig-prop ?action-designator (:application-context :opening)) - ;; (desig:desig-prop ?action-designator (:application-context :closing))) - ;; (equal ?straight-line T) - ;; (equal ?straight-line T)) + (-> (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 T) + (:straight-line ?straight-line) (: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/pour-plans.lisp b/cram_common/cram_mobile_pick_place_plans/src/pour-plans.lisp index d593715376..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 @@ -72,12 +72,31 @@ ;; ;;(type keyword ?arm side grasp) ;; (type number ?wait-duration) ;; (ignore side grasp object)) - (let* ((sleepy t) - (movy t) + (let* ((sleepy nil) + (?movy nil) (?align-planes-left nil) - (?align-planes-right 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 ?on-object) + (left-poses ?left-reach-poses) + (right-poses ?right-reach-poses) + (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 @@ -87,10 +106,9 @@ 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 @@ -98,8 +116,8 @@ (object ?on-object) (left-poses ?left-reach-poses) (right-poses ?right-reach-poses) - (goal ?goal)))) - )) + (move-base ?move-base-when-reaching) + (goal ?goal)))))) @@ -129,7 +147,7 @@ (right-poses ?right-tilt-up-poses) (align-planes-left ?align-planes-left) (align-planes-right ?align-planes-right) - (move-base movy) + (move-base ?movy) ;;(collision-mode :allow-attached) (goal ?goal)))) )) @@ -162,6 +180,7 @@ (object ?on-object) (left-poses ?left-reach-poses) (right-poses ?right-reach-poses) + (application-context pouring) (goal ?goal)))) )) @@ -245,7 +264,7 @@ ((object-type (eql :bowl)) arm (grasp (eql :top-right))) - '((-0.01 -0.245 0.025)(0 0 0 1))) + '((-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)) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 5ec293eba4..56369e6ba0 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -40,14 +40,6 @@ "island_countertop" ((0.34 0.5 0.1) (0 0 0 -1))))) -(defparameter *apartment-object-spawning-poses-pouring* - '((:jeroen-cup - "island_countertop" - ((0.34 -0.5 0.1) (0 0 0 -1))) - (:bowl - "island_countertop" - ((0.34 0.5 0.1) (0 0 0 -1))))) - (defun initialize-apartment () (sb-ext:gc :full t) @@ -162,18 +154,18 @@ ;; (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))))) + ;; (?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 @@ -232,7 +224,7 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.37 2.6 1.0126) + (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 @@ -296,7 +288,7 @@ (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 ?initial-parking-pose)) @@ -517,7 +509,7 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.49 3.0 1.0126) + (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 @@ -539,7 +531,7 @@ (when cram-projection:*projection-environment* (spawn-objects-on-fixed-spots :object-types '(:jeroen-cup :bowl) - :spawning-poses-relative *apartment-object-spawning-poses-pouring*)) + :spawning-poses-relative *apartment-object-spawning-poses*)) (park-robot ?initial-parking-pose)) ;; get all the object designators @@ -740,15 +732,21 @@ (cl-transforms-stamped:make-pose-stamped "map" 0.0 - (cl-transforms:make-3d-vector 2.49 2.6 1.0126) + (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.49 2.2 1.0126) - (cl-transforms:make-quaternion 0 1 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) @@ -798,85 +796,49 @@ (object ?source-perceived-object-desig) (robot-location (desig:a location (pose ?percieve-blue-cup-pose-pouring))) - (goal ?goal))))) - - - (?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)))))) - - - ;;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 - ;; (let ((?source-perceived-object-desig - ;; (cpl:seq - ;; (exe:perform - ;; (desig:an action - ;; (type parking-arms))) - ;; (exe:perform - ;; (desig:an action - ;; (type looking) - ;; (target (desig:a location - ;; (pose ?on-counter-top-source-cup-look-pose))))) - ;; (exe:perform - ;; (desig:an action - ;; (type detecting) - ;; (object ?source-object-desig)))))) - ;; (cpl:retry))) - ;; (return))) - - - ;; (let ((?goal `(cpoe:object-in-hand ,(an object - ;; (type jeroen-cup) - ;; (name jeroen-cup-1))))) - ;; (exe:perform - ;; (desig:an action - ;; (type picking-up) - ;; (arm :right) - ;; (grasp :front) - ;; (object ?source-perceived-object-desig) - ;; (park-arms NIL) - ;; (goal ?goal)))))) - - + (look-location (desig:a location + (pose ?on-counter-top-source-cup-look-pose))) + (goal ?goal)))))))) + (when (<= step 3) (exe:perform (desig:an action - (type pouring) - (on-object ?target-perceived-object-desig) - (arm :right) - (sides (:top-right)) - (wait-duration 5))))) - + (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))))))) + (pose ?on-counter-top-cup-pose))))) + ))) 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)) From c81bc6ecdc0d77a28773cb93b97af07d970eb3a0 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Wed, 22 Mar 2023 22:39:36 +0100 Subject: [PATCH 68/97] [btr] Add robot support for multiple collisions --- .../src/robot-model.lisp | 85 ++++++++++++------- 1 file changed, 54 insertions(+), 31 deletions(-) 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)) From 4634c2c0e0a7858a6f654d2139bd27d72fa16cec Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 22 Oct 2021 12:15:54 +0200 Subject: [PATCH 69/97] Add function to reset bullet window --- .../src/package.lisp | 1 + .../src/vis-tools.lisp | 13 +++++++++++++ 2 files changed, 14 insertions(+) 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*)) From eb0f4e7e739cbf302e70cdde1a794c3355c65a46 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Tue, 30 Aug 2022 17:11:41 +0200 Subject: [PATCH 70/97] [tt-export] Keep failed and highlight --- .../cram_task_tree_export/src/conversion.lisp | 44 +++++++++---------- .../cram_task_tree_export/src/exporter.lisp | 14 ++++-- .../cram_task_tree_export/src/tree-utils.lisp | 35 ++++++++++++--- 3 files changed, 62 insertions(+), 31 deletions(-) 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." From 86fab8a5ca6e710c6ec2812c8d1f9c0dbefd39a3 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 31 Mar 2023 14:12:36 +0200 Subject: [PATCH 71/97] Add eurobin demo first draft --- .../src/eurobin-demo.lisp | 198 ++++++++++++++++++ 1 file changed, 198 insertions(+) create mode 100644 cram_demos/cram_projection_demos/src/eurobin-demo.lisp 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..307c42d12d --- /dev/null +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -0,0 +1,198 @@ +;;; +;;; 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) + +(defparameter *eurobin-object-spawning-poses* + '((:eurobin-package + "some_origin_frame" + ((0.20 0.05 0.08) (1 0 0 0))))) + +(defparameter *robot-base-pose-door* + (cl-tf:make-pose-stamped "map" 0.0 + (cl-tf:make-3d-vector 10.0d0 4.0d0 0.0d0) + (cl-tf:euler->quaternion :az (/ pi 2)))) + +(defun initialize-eurobin () + (sb-ext:gc :full t) + + ;;(when ccl::*is-logging-enabled* + ;; (setf ccl::*is-client-connected* nil) + ;; (ccl::connect-to-cloud-logger) + ;; (ccl::reset-logged-owl)) + + ;; (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:joint-state (btr:get-environment-object) + ;; "cabinet7_door_bottom_left_joint") + ;; 0.025 + ;; (btr:joint-state (btr:get-environment-object) + ;; "dishwasher_drawer_middle_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 finalize () + ;; (setf proj-reasoning::*projection-reasoning-enabled* nil) + + ;;(when ccl::*is-logging-enabled* + ;; (ccl::export-log-to-owl "ease_milestone_2018.owl") + ;; (ccl::export-belief-state-to-owl "ease_milestone_2018_belief.owl")) + (sb-ext:gc :full t)) + + +(defun eurobin-demo (&key (step 0)) + ;;urdf-proj:with-simulated-robot + (setf proj-reasoning::*projection-checks-enabled* nil) + (setf btr:*visibility-threshold* 0.7) + + ;;; --- + ;;; Step 0: Init apartment, reset joints, desigs and objects + (when (<= step 0) + (initialize-eurobin) + ;; (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) + ;; :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))) + ) + + ;; Door trajectory + ;; (let ((door-traj + ;; (urdf-proj:with-simulated-robot + ;; (man-int:get-action-trajectory :opening :right :right-side NIL + ;; (list (desig:AN OBJECT + ;; (TYPE CUPBOARD) + ;; (URDF-NAME window4-right) + ;; (PART-OF APARTMENT))) + ;; :opening-distance 0.0)))) + ;; (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) + ;; ))) + + + (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)))) + ) + + ;;; --- + ;;; Go and open the door + (when (<= step 1) + (exe:perform + (desig:AN ACTION + (TYPE going) + (target (desig:a location + (pose ?accessing-window-base-pose))))) + + (exe:perform + (desig:AN ACTION + (TYPE opening) + (ARM right) + (distance 0.0) + (GRASPS (left-side)) + (OBJECT (desig:AN OBJECT + (TYPE cupboard) + (URDF-NAME window4-right) + (PART-OF APARTMENT))))) + ;; (exe:perform + ;; (desig:AN ACTION + ;; (TYPE going) + ;; (target (desig:a location + ;; (pose ?accessing-window-base-pose-2))))) + ;; (exe:perform + ;; (desig:AN ACTION + ;; (TYPE opening) + ;; (ARM left) + ;; (distance 1.5) + ;; (GRASPS (front)) + ;; (OBJECT (desig:AN OBJECT + ;; (TYPE cupboard) + ;; (URDF-NAME window4-right) + ;; (PART-OF APARTMENT))))) + ) + + ;; (btr-utils:spawn-object :package :package :pose '((9.95 5.30 1.2)(0 0 0 1))) + + ;;; --- + ;;; Take the package with both hands and carry it to the table + (when (<= step 2)) + + ;;; --- + ;;; Cut the package open along its tape + (when (<= step 3)) + + ;;; --- + ;;; Open the package lid + (when (<= step 4)) + + ;;; --- + ;;; Pick items out of the package and place + (when (<= step 5)) + + ) + + (finalize)) From da96ef5a6b7485efad6df4ebf9b760e046e59d8f Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Wed, 5 Apr 2023 18:15:44 +0200 Subject: [PATCH 72/97] Finish basic eurobin scenario, add box mesh --- .../cram_object_knowledge/src/household.lisp | 24 +- .../cram-projection-demos.asd | 3 +- .../resource/household/open_box.obj | 262 ++++++++++++++++++ .../src/eurobin-demo.lisp | 253 +++++++++-------- 4 files changed, 423 insertions(+), 119 deletions(-) create mode 100644 cram_demos/cram_projection_demos/resource/household/open_box.obj diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 2ebc4ceb50..5819c2d73d 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 :package)))) ;;;;;;;;;;;;;;;;;;;;;;;;;;;; CUTLERY ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; @@ -655,6 +656,27 @@ + + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 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 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (defparameter *mug-back-grasp-x-offset* 0.04 "in meters") diff --git a/cram_demos/cram_projection_demos/cram-projection-demos.asd b/cram_demos/cram_projection_demos/cram-projection-demos.asd index f0da2c4346..7e3544dca2 100644 --- a/cram_demos/cram_projection_demos/cram-projection-demos.asd +++ b/cram_demos/cram_projection_demos/cram-projection-demos.asd @@ -94,4 +94,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/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/eurobin-demo.lisp b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp index 307c42d12d..31faa09fec 100644 --- a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -29,54 +29,19 @@ (in-package :demos) -(defparameter *eurobin-object-spawning-poses* - '((:eurobin-package - "some_origin_frame" - ((0.20 0.05 0.08) (1 0 0 0))))) - -(defparameter *robot-base-pose-door* - (cl-tf:make-pose-stamped "map" 0.0 - (cl-tf:make-3d-vector 10.0d0 4.0d0 0.0d0) - (cl-tf:euler->quaternion :az (/ pi 2)))) - (defun initialize-eurobin () (sb-ext:gc :full t) - - ;;(when ccl::*is-logging-enabled* - ;; (setf ccl::*is-client-connected* nil) - ;; (ccl::connect-to-cloud-logger) - ;; (ccl::reset-logged-owl)) - ;; (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:joint-state (btr:get-environment-object) - ;; "cabinet7_door_bottom_left_joint") - ;; 0.025 - ;; (btr:joint-state (btr:get-environment-object) - ;; "dishwasher_drawer_middle_joint") - ;; 0.0 - ) + 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 finalize () - ;; (setf proj-reasoning::*projection-reasoning-enabled* nil) - - ;;(when ccl::*is-logging-enabled* - ;; (ccl::export-log-to-owl "ease_milestone_2018.owl") - ;; (ccl::export-belief-state-to-owl "ease_milestone_2018_belief.owl")) - (sb-ext:gc :full t)) - (defun eurobin-demo (&key (step 0)) ;;urdf-proj:with-simulated-robot @@ -87,46 +52,14 @@ ;;; Step 0: Init apartment, reset joints, desigs and objects (when (<= step 0) (initialize-eurobin) - ;; (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) - ;; :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))) - ) - - ;; Door trajectory - ;; (let ((door-traj - ;; (urdf-proj:with-simulated-robot - ;; (man-int:get-action-trajectory :opening :right :right-side NIL - ;; (list (desig:AN OBJECT - ;; (TYPE CUPBOARD) - ;; (URDF-NAME window4-right) - ;; (PART-OF APARTMENT))) - ;; :opening-distance 0.0)))) - ;; (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) - ;; ))) - - - (let* ( - ;; Initialize desigs for objects and locations + (park-robot (cl-transforms-stamped:make-pose-stamped + cram-tf:*fixed-frame* + 0.0 + (cl-transforms:make-3d-vector 9.7d0 3.0d0 0.0d0) + (cl-transforms:make-quaternion 0 0 1 0)))) + + + (let* (;; Initialize desigs for objects and locations (?accessing-window-base-pose (cl-transforms-stamped:make-pose-stamped "map" (roslisp:ros-time) @@ -137,62 +70,148 @@ "map" (roslisp:ros-time) (cl-tf:make-3d-vector 9.5d0 4.3d0 0.0d0) (cl-tf:euler->quaternion :az (* pi 0.25)))) - ) + (?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.53d0) + (cl-tf:euler->quaternion :az (* pi 0.0))))) ;;; --- ;;; Go and open the door (when (<= step 1) (exe:perform - (desig:AN ACTION - (TYPE going) + (desig:an action + (type going) (target (desig:a location (pose ?accessing-window-base-pose))))) (exe:perform - (desig:AN ACTION - (TYPE opening) - (ARM right) - (distance 0.0) - (GRASPS (left-side)) - (OBJECT (desig:AN OBJECT - (TYPE cupboard) - (URDF-NAME window4-right) - (PART-OF APARTMENT))))) - ;; (exe:perform - ;; (desig:AN ACTION - ;; (TYPE going) - ;; (target (desig:a location - ;; (pose ?accessing-window-base-pose-2))))) - ;; (exe:perform - ;; (desig:AN ACTION - ;; (TYPE opening) - ;; (ARM left) - ;; (distance 1.5) - ;; (GRASPS (front)) - ;; (OBJECT (desig:AN OBJECT - ;; (TYPE cupboard) - ;; (URDF-NAME window4-right) - ;; (PART-OF APARTMENT))))) - ) - - ;; (btr-utils:spawn-object :package :package :pose '((9.95 5.30 1.2)(0 0 0 1))) - - ;;; --- - ;;; Take the package with both hands and carry it to the table - (when (<= step 2)) + (desig:an action + (type opening) + (arm right) + (distance 0.5) + (grasps (left-side)) + (object (desig:an object + (type cupboard) + (urdf-name window4-right) + (part-of apartment))))) + (exe:perform + (desig:an action + (type going) + (target (desig:a location + (pose ?accessing-window-base-pose-2))))) + (exe:perform + (desig:an action + (type opening) + (arm left) + (distance 1.5) + (grasps (front)) + (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))) ;;; --- - ;;; Cut the package open along its tape - (when (<= step 3)) + ;;; Take the package and carry it to the table + (when (<= step 2) + ;; 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))))) - ;;; --- - ;;; Open the package lid - (when (<= step 4)) + ;; 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 5)) + (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)) From 870041318fda7c399f0b4d5176b55ae43923b84c Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Thu, 6 Apr 2023 17:09:32 +0200 Subject: [PATCH 73/97] Make eurobin box orientation matter --- .../cram_object_knowledge/src/household.lisp | 2 +- .../src/eurobin-demo.lisp | 35 ++++++++++--------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/cram_demos/cram_object_knowledge/src/household.lisp b/cram_demos/cram_object_knowledge/src/household.lisp index 5819c2d73d..03be791b5a 100644 --- a/cram_demos/cram_object_knowledge/src/household.lisp +++ b/cram_demos/cram_object_knowledge/src/household.lisp @@ -134,7 +134,7 @@ ))) (<- (orientation-matters ?object-type) - (member ?object-type (:knife :fork :spoon :cutlery :spatula :package)))) + (member ?object-type (:knife :fork :spoon :cutlery :spatula :open-box)))) ;;;;;;;;;;;;;;;;;;;;;;;;;;;; CUTLERY ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; diff --git a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp index 31faa09fec..54320b3c4e 100644 --- a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -109,27 +109,28 @@ (desig:an action (type opening) (arm right) - (distance 0.5) + (distance 1.5) (grasps (left-side)) (object (desig:an object (type cupboard) (urdf-name window4-right) (part-of apartment))))) - (exe:perform - (desig:an action - (type going) - (target (desig:a location - (pose ?accessing-window-base-pose-2))))) - (exe:perform - (desig:an action - (type opening) - (arm left) - (distance 1.5) - (grasps (front)) - (object (desig:an object - (type cupboard) - (urdf-name window4-right) - (part-of apartment)))))) + ;; (exe:perform + ;; (desig:an action + ;; (type going) + ;; (target (desig:a location + ;; (pose ?accessing-window-base-pose-2))))) + ;; (exe:perform + ;; (desig:an action + ;; (type opening) + ;; (arm left) + ;; (distance 1.5) + ;; (grasps (front)) + ;; (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* @@ -138,7 +139,7 @@ '((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))) + (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 From cd3dbc25d5c0e23141f5e502bbf4835f409d22db Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Thu, 6 Apr 2023 17:11:42 +0200 Subject: [PATCH 74/97] Temp change of iai_apartment urdf in AddWindows branch --- .../cram_projection_demos/launch/apartment_pr2.launch | 10 ++++++++++ .../cram_projection_demos/launch/everything.launch | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) create mode 100644 cram_demos/cram_projection_demos/launch/apartment_pr2.launch 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..4c5f5be540 --- /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/everything.launch b/cram_demos/cram_projection_demos/launch/everything.launch index 5260e01d7b..fe413eeb3d 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_with_window4.xacro'"/> From b8fe594d13419fe0cdf381fbb8db3d4608f495e2 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Thu, 6 Apr 2023 17:24:12 +0200 Subject: [PATCH 75/97] Deliver eurobin item lower --- cram_demos/cram_projection_demos/src/eurobin-demo.lisp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp index 54320b3c4e..62dc4fe738 100644 --- a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -93,7 +93,7 @@ (?item-delivery-pose (cl-transforms-stamped:make-pose-stamped "map" (roslisp:ros-time) - (cl-tf:make-3d-vector 16.2d0 3.0d0 0.53d0) + (cl-tf:make-3d-vector 16.2d0 3.0d0 0.48d0) (cl-tf:euler->quaternion :az (* pi 0.0))))) ;;; --- From 0b05a916cf36f503beb80f23f1a48a7ef37357a8 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Thu, 13 Apr 2023 16:20:59 +0200 Subject: [PATCH 76/97] [giskard] Update CartesianPose velocity fields --- .../cram_giskard/src/arm-goals.lisp | 3 ++- .../cram_giskard/src/base-goals.lisp | 6 +++-- .../src/making-goal-messages.lisp | 22 ++++++++++++------- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index c54124fd1c..108951aa16 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -187,7 +187,8 @@ (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* + :max-lin-velocity *base-max-velocity-slow-xy* + :max-ang-velocity *base-max-velocity-slow-theta* :avoid-collisions-much nil) (when align-planes-left (make-align-planes-tool-frame-constraint diff --git a/cram_external_interfaces/cram_giskard/src/base-goals.lisp b/cram_external_interfaces/cram_giskard/src/base-goals.lisp index 1e30399f50..1a5a886ddb 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -58,12 +58,14 @@ (make-diffdrive-base-goal cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose :avoid-collisions-much t - :max-velocity *base-max-velocity-fast-xy* + :max-lin-velocity *base-max-velocity-fast-xy* + :max-ang-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-velocity *base-max-velocity-fast-xy*)) + :max-lin-velocity *base-max-velocity-fast-xy* + :max-ang-velocity *base-max-velocity-slow-theta*)) (when (eq (rob-int:get-environment-name) :iai-kitchen) (make-base-collision-avoidance-hint-constraint *base-collision-avoidance-hint-link* 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 c63a200001..28decb859d 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -348,10 +348,11 @@ . ,root-link)))))) (defun make-cartesian-constraint (root-frame tip-frame goal-pose - &key max-velocity avoid-collisions-much straight-line) + &key max-lin-velocity max-ang-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 (or number null) max-lin-velocity max-ang-velocity) (type boolean avoid-collisions-much straight-line)) (roslisp:make-message 'giskard_msgs-msg:constraint @@ -366,8 +367,10 @@ ("goal_pose" . (("message_type" . "geometry_msgs/PoseStamped") ("message" . ,(to-hash-table goal-pose)))) - ,@(when max-velocity - `(("reference_velocity" . ,max-velocity))) + ,@(when max-lin-velocity + `(("reference_linear_velocity" . ,max-lin-velocity))) + ,@(when max-ang-velocity + `(("reference_angular_velocity" . ,max-ang-velocity))) ,@(if avoid-collisions-much `(("weight" . ,(roslisp-msg-protocol:symbol-code 'giskard_msgs-msg:constraint @@ -379,10 +382,11 @@ )))))))) (defun make-diffdrive-base-goal (root-frame tip-frame goal-pose - &key max-velocity avoid-collisions-much always-forward) + &key max-lin-velocity max-ang-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-velocity) + (type (or number null) max-lin-velocity max-ang-velocity) (type boolean avoid-collisions-much always-forward)) (roslisp:make-message 'giskard_msgs-msg:constraint @@ -397,8 +401,10 @@ ("message" . ,(to-hash-table goal-pose)))) ,@(when always-forward `(("always_forward" . T))) - ,@(when max-velocity - `(("reference_velocity" . ,max-velocity))) + ,@(when max-lin-velocity + `(("reference_linear_velocity" . ,max-lin-velocity))) + ,@(when max-ang-velocity + `(("reference_angular_velocity" . ,max-ang-velocity))) ,@(if avoid-collisions-much `(("weight" . ,(roslisp-msg-protocol:symbol-code 'giskard_msgs-msg:constraint From 2d906a5a6599d852f0a011342adaf2e834870d1a Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 14 Apr 2023 15:47:23 +0200 Subject: [PATCH 77/97] Revert "[giskard] Update CartesianPose velocity fields" This reverts commit 0b05a916cf36f503beb80f23f1a48a7ef37357a8. --- .../cram_giskard/src/arm-goals.lisp | 3 +-- .../cram_giskard/src/base-goals.lisp | 6 ++--- .../src/making-goal-messages.lisp | 22 +++++++------------ 3 files changed, 11 insertions(+), 20 deletions(-) diff --git a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index 108951aa16..c54124fd1c 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -187,8 +187,7 @@ (cl-transforms-stamped:pose->pose-stamped cram-tf:*robot-base-frame* 0.0 (cl-transforms:make-identity-pose)) - :max-lin-velocity *base-max-velocity-slow-xy* - :max-ang-velocity *base-max-velocity-slow-theta* + :max-velocity *base-max-velocity-slow-xy* :avoid-collisions-much nil) (when align-planes-left (make-align-planes-tool-frame-constraint diff --git a/cram_external_interfaces/cram_giskard/src/base-goals.lisp b/cram_external_interfaces/cram_giskard/src/base-goals.lisp index 1a5a886ddb..1e30399f50 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -58,14 +58,12 @@ (make-diffdrive-base-goal cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose :avoid-collisions-much t - :max-lin-velocity *base-max-velocity-fast-xy* - :max-ang-velocity *base-max-velocity-fast-theta* + :max-velocity *base-max-velocity-fast-xy* :always-forward nil) (make-cartesian-constraint cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose :avoid-collisions-much t - :max-lin-velocity *base-max-velocity-fast-xy* - :max-ang-velocity *base-max-velocity-slow-theta*)) + :max-velocity *base-max-velocity-fast-xy*)) (when (eq (rob-int:get-environment-name) :iai-kitchen) (make-base-collision-avoidance-hint-constraint *base-collision-avoidance-hint-link* 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 28decb859d..c63a200001 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -348,11 +348,10 @@ . ,root-link)))))) (defun make-cartesian-constraint (root-frame tip-frame goal-pose - &key max-lin-velocity max-ang-velocity - avoid-collisions-much straight-line) + &key max-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-lin-velocity max-ang-velocity) + (type (or number null) max-velocity) (type boolean avoid-collisions-much straight-line)) (roslisp:make-message 'giskard_msgs-msg:constraint @@ -367,10 +366,8 @@ ("goal_pose" . (("message_type" . "geometry_msgs/PoseStamped") ("message" . ,(to-hash-table goal-pose)))) - ,@(when max-lin-velocity - `(("reference_linear_velocity" . ,max-lin-velocity))) - ,@(when max-ang-velocity - `(("reference_angular_velocity" . ,max-ang-velocity))) + ,@(when max-velocity + `(("reference_velocity" . ,max-velocity))) ,@(if avoid-collisions-much `(("weight" . ,(roslisp-msg-protocol:symbol-code 'giskard_msgs-msg:constraint @@ -382,11 +379,10 @@ )))))))) (defun make-diffdrive-base-goal (root-frame tip-frame goal-pose - &key max-lin-velocity max-ang-velocity - avoid-collisions-much always-forward) + &key max-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-lin-velocity max-ang-velocity) + (type (or number null) max-velocity) (type boolean avoid-collisions-much always-forward)) (roslisp:make-message 'giskard_msgs-msg:constraint @@ -401,10 +397,8 @@ ("message" . ,(to-hash-table goal-pose)))) ,@(when always-forward `(("always_forward" . T))) - ,@(when max-lin-velocity - `(("reference_linear_velocity" . ,max-lin-velocity))) - ,@(when max-ang-velocity - `(("reference_angular_velocity" . ,max-ang-velocity))) + ,@(when max-velocity + `(("reference_velocity" . ,max-velocity))) ,@(if avoid-collisions-much `(("weight" . ,(roslisp-msg-protocol:symbol-code 'giskard_msgs-msg:constraint From ae618d459f72ce1d5930e575f093cf00e5b7d3f9 Mon Sep 17 00:00:00 2001 From: TIAGo Ext User Date: Tue, 14 Mar 2023 14:20:08 +0100 Subject: [PATCH 78/97] [giskard] updates for the newest giskard version --- .../cram_giskard/src/arm-goals.lisp | 60 ++++++++++--------- .../cram_giskard/src/base-goals.lisp | 20 ++++--- .../src/environment-manipulation-goals.lisp | 6 +- .../src/making-goal-messages.lisp | 23 ++++--- 4 files changed, 65 insertions(+), 44 deletions(-) diff --git a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp index c54124fd1c..64340ce9bd 100644 --- a/cram_external_interfaces/cram_giskard/src/arm-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/arm-goals.lisp @@ -77,10 +77,14 @@ (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*))) + ;; 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 @@ -187,7 +191,8 @@ (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* + :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 @@ -330,28 +335,29 @@ (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 - :straight-line straight-line - :align-planes-left align-planes-left - :align-planes-right align-planes-right - :unmovable-joints unmovable-joints - :precise-tracking precise-tracking) - :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 1e30399f50..cc3dc79cdc 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -43,11 +43,11 @@ ;; 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) @@ -63,7 +63,8 @@ (make-cartesian-constraint cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose :avoid-collisions-much t - :max-velocity *base-max-velocity-fast-xy*)) + :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* @@ -109,9 +110,10 @@ (cram-tf:visualize-marker goal-pose :r-g-b-list '(0 1 0)) ;; Trying with slow velocity should happen on the motion level, not action level. - (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)))) + (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/environment-manipulation-goals.lisp b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp index 17af26c05b..acaeb8d19c 100644 --- a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp @@ -41,7 +41,11 @@ (make-giskard-goal :constraints (list (when prefer-base - (make-prefer-base-constraint :do-not-rotate T)) + ;; 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 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 c63a200001..881a06e37c 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -180,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" @@ -228,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)) @@ -348,10 +354,11 @@ . ,root-link)))))) (defun make-cartesian-constraint (root-frame tip-frame goal-pose - &key max-velocity avoid-collisions-much straight-line) + &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 (or number null) max-linear-velocity max-angular-velocity) (type boolean avoid-collisions-much straight-line)) (roslisp:make-message 'giskard_msgs-msg:constraint @@ -366,8 +373,10 @@ ("goal_pose" . (("message_type" . "geometry_msgs/PoseStamped") ("message" . ,(to-hash-table goal-pose)))) - ,@(when max-velocity - `(("reference_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 From eeed5f1b0fd1130520bac331cf4d19682ddb9d7a Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 21 Apr 2023 15:22:13 +0200 Subject: [PATCH 79/97] [btr-belief] Load urdf before debug window --- .../src/belief-state.lisp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 From f53a1f8e5b3d22606deac63fa6b8140f7b02815f Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 21 Apr 2023 16:43:02 +0200 Subject: [PATCH 80/97] [giskard] Update velocity contraint for diff-drive --- .../cram_giskard/src/base-goals.lisp | 3 ++- .../cram_giskard/src/making-goal-messages.lisp | 11 +++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/cram_external_interfaces/cram_giskard/src/base-goals.lisp b/cram_external_interfaces/cram_giskard/src/base-goals.lisp index cc3dc79cdc..c447fcc168 100644 --- a/cram_external_interfaces/cram_giskard/src/base-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/base-goals.lisp @@ -58,7 +58,8 @@ (make-diffdrive-base-goal cram-tf:*odom-frame* cram-tf:*robot-base-frame* pose :avoid-collisions-much t - :max-velocity *base-max-velocity-fast-xy* + :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 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 881a06e37c..3087f38eb2 100644 --- a/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp +++ b/cram_external_interfaces/cram_giskard/src/making-goal-messages.lisp @@ -388,10 +388,11 @@ )))))))) (defun make-diffdrive-base-goal (root-frame tip-frame goal-pose - &key max-velocity avoid-collisions-much always-forward) + &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-velocity) + (type (or number null) max-linear-velocity max-angular-velocity) (type boolean avoid-collisions-much always-forward)) (roslisp:make-message 'giskard_msgs-msg:constraint @@ -406,8 +407,10 @@ ("message" . ,(to-hash-table goal-pose)))) ,@(when always-forward `(("always_forward" . T))) - ,@(when max-velocity - `(("reference_velocity" . ,max-velocity))) + ,@(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 From 9b26560e53035f8495224fa46d9d88ff59fb398f Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 21 Apr 2023 16:58:16 +0200 Subject: [PATCH 81/97] [demo] Add 45 degree grasp angle for doors --- .../src/package.lisp | 2 ++ .../src/standard-grasps.lisp | 13 +++++++++++++ .../cram_object_knowledge/src/environment.lisp | 18 ++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/cram_common/cram_manipulation_interfaces/src/package.lisp b/cram_common/cram_manipulation_interfaces/src/package.lisp index 22848a98d5..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* 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_demos/cram_object_knowledge/src/environment.lisp b/cram_demos/cram_object_knowledge/src/environment.lisp index 3b32a56bf6..d17a398660 100644 --- a/cram_demos/cram_object_knowledge/src/environment.lisp +++ b/cram_demos/cram_object_knowledge/src/environment.lisp @@ -185,3 +185,21 @@ :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 from the right for doors +(man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :door-right + :grasp-translation `(,(- 0.05) 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 from the left for doors +(man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :door-left + :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)) From d5f3f95a2cc13eaef7b327c39c81ee3df705c473 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 21 Apr 2023 17:32:26 +0200 Subject: [PATCH 82/97] [demo] update eurobin demo --- .../cram-projection-demos.asd | 2 + .../src/eurobin-demo.lisp | 68 ++++++++++--------- 2 files changed, 38 insertions(+), 32 deletions(-) diff --git a/cram_demos/cram_projection_demos/cram-projection-demos.asd b/cram_demos/cram_projection_demos/cram-projection-demos.asd index 7e3544dca2..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 diff --git a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp index 62dc4fe738..59be2207eb 100644 --- a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -39,6 +39,7 @@ (btr-belief::publish-environment-joint-state (btr:joint-states (btr:get-environment-object))) (setf desig::*designators* (tg:make-weak-hash-table :weakness :key)) + (giskard:reset-collision-scene) ;; (coe:clear-belief) (btr:clear-costmap-vis-object)) @@ -48,17 +49,6 @@ (setf proj-reasoning::*projection-checks-enabled* nil) (setf btr:*visibility-threshold* 0.7) - ;;; --- - ;;; Step 0: Init apartment, reset joints, desigs and objects - (when (<= step 0) - (initialize-eurobin) - (park-robot (cl-transforms-stamped:make-pose-stamped - cram-tf:*fixed-frame* - 0.0 - (cl-transforms:make-3d-vector 9.7d0 3.0d0 0.0d0) - (cl-transforms:make-quaternion 0 0 1 0)))) - - (let* (;; Initialize desigs for objects and locations (?accessing-window-base-pose (cl-transforms-stamped:make-pose-stamped @@ -70,6 +60,11 @@ "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.7d0 4.3d0 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) @@ -96,41 +91,50 @@ (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-front-pose))))) + ) + (when (<= step 2) + (exe:perform + (desig:an action + (type opening) + (arm left) + (distance 0.3) + (grasps (door-left)) + (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))))) + (pose ?accessing-window-base-pose-2)))))) + (when (<= step 4) (exe:perform (desig:an action (type opening) (arm right) - (distance 1.5) - (grasps (left-side)) + (distance 1.2) + (grasps (door-left)) (object (desig:an object (type cupboard) (urdf-name window4-right) - (part-of apartment))))) - ;; (exe:perform - ;; (desig:an action - ;; (type going) - ;; (target (desig:a location - ;; (pose ?accessing-window-base-pose-2))))) - ;; (exe:perform - ;; (desig:an action - ;; (type opening) - ;; (arm left) - ;; (distance 1.5) - ;; (grasps (front)) - ;; (object (desig:an object - ;; (type cupboard) - ;; (urdf-name window4-right) - ;; (part-of apartment))))) - ) + (part-of apartment)))))) (unless (btr:object btr:*current-bullet-world* :package-stand) (btr:add-object btr:*current-bullet-world* @@ -143,7 +147,7 @@ ;;; --- ;;; Take the package and carry it to the table - (when (<= step 2) + (when (<= step 5) ;; go to open door (exe:perform (desig:an action From e1ec3ef4912d8f116b2961a5a24c4ca0f7bf18e2 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 21 Apr 2023 17:34:54 +0200 Subject: [PATCH 83/97] [tiago] Add gripper client for native actions --- .../src/gripper-wrapper.lisp | 245 ++++++++++++++++++ 1 file changed, 245 insertions(+) create mode 100644 cram_tiago/cram_tiago_process_modules/src/gripper-wrapper.lisp diff --git a/cram_tiago/cram_tiago_process_modules/src/gripper-wrapper.lisp b/cram_tiago/cram_tiago_process_modules/src/gripper-wrapper.lisp new file mode 100644 index 0000000000..856103a2aa --- /dev/null +++ b/cram_tiago/cram_tiago_process_modules/src/gripper-wrapper.lisp @@ -0,0 +1,245 @@ +;;; +;;; 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-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*) + (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)))) From 05f04bbda132d04e560fbec83f177e616b0af79b Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Wed, 26 Apr 2023 18:29:20 +0200 Subject: [PATCH 84/97] [demo] Replace giskard call with coe:clear-belief --- cram_demos/cram_projection_demos/src/apartment-demo.lisp | 5 +++-- cram_demos/cram_projection_demos/src/eurobin-demo.lisp | 4 +--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/cram_demos/cram_projection_demos/src/apartment-demo.lisp b/cram_demos/cram_projection_demos/src/apartment-demo.lisp index 56369e6ba0..c59e0d4536 100644 --- a/cram_demos/cram_projection_demos/src/apartment-demo.lisp +++ b/cram_demos/cram_projection_demos/src/apartment-demo.lisp @@ -51,7 +51,8 @@ ;; (setf proj-reasoning::*projection-checks-enabled* t) (kill-and-detach-all) - (giskard:reset-collision-scene) + + ;; (giskard:reset-collision-scene) (setf (btr:joint-state (btr:get-environment-object) "cabinet1_door_top_left_joint") 0.0 @@ -66,7 +67,7 @@ (setf desig::*designators* (tg:make-weak-hash-table :weakness :key)) - ;; (coe:clear-belief) + (coe:clear-belief) (btr:clear-costmap-vis-object)) diff --git a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp index 59be2207eb..95f05c40a2 100644 --- a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -39,11 +39,9 @@ (btr-belief::publish-environment-joint-state (btr:joint-states (btr:get-environment-object))) (setf desig::*designators* (tg:make-weak-hash-table :weakness :key)) - (giskard:reset-collision-scene) - ;; (coe:clear-belief) + (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) From 1d9071f32ec016f4d14d837b91f4dfcdb9f9529a Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Wed, 26 Apr 2023 20:09:35 +0200 Subject: [PATCH 85/97] [tiago-pm] Add mujoco tiago makro and grippers --- .../cram-tiago-process-modules.asd | 5 ++++- ...pper-wrapper.lisp => grippers-via-fingers.lisp} | 2 +- .../cram_tiago_process_modules/src/interface.lisp | 14 +++++++++++++- .../cram_tiago_process_modules/src/package.lisp | 3 ++- 4 files changed, 20 insertions(+), 4 deletions(-) rename cram_tiago/cram_tiago_process_modules/src/{gripper-wrapper.lisp => grippers-via-fingers.lisp} (99%) 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 07d7e84ceb..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 @@ -46,6 +46,7 @@ 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) @@ -54,5 +55,7 @@ :components ((:file "package") (:file "grippers" :depends-on ("package")) + (:file "grippers-via-fingers" :depends-on ("package")) (:file "interface" :depends-on ("package" - "grippers")))))) + "grippers" + "grippers-via-fingers")))))) diff --git a/cram_tiago/cram_tiago_process_modules/src/gripper-wrapper.lisp b/cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp similarity index 99% rename from cram_tiago/cram_tiago_process_modules/src/gripper-wrapper.lisp rename to cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp index 856103a2aa..d14ee3ecb9 100644 --- a/cram_tiago/cram_tiago_process_modules/src/gripper-wrapper.lisp +++ b/cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp @@ -37,7 +37,7 @@ (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) +(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 diff --git a/cram_tiago/cram_tiago_process_modules/src/interface.lisp b/cram_tiago/cram_tiago_process_modules/src/interface.lisp index 7844098bca..92a08eab9a 100644 --- a/cram_tiago/cram_tiago_process_modules/src/interface.lisp +++ b/cram_tiago/cram_tiago_process_modules/src/interface.lisp @@ -53,9 +53,14 @@ (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 ?pm) - (member ?pm (grippers-pm giskard:giskard-pm)) + (member ?pm (grippers-pm grippers-via-fingers-pm giskard:giskard-pm)) (not (cpm:projection-running ?_)))) (defmacro with-real-robot (&body body) @@ -64,3 +69,10 @@ 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)) From e95e7740f19d35e7efccc1496aa548321c5b4c37 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 5 May 2023 15:12:28 +0200 Subject: [PATCH 86/97] [env-man] Search handle link center frame --- .../src/environment.lisp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) 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'." From e6ebee4018206a263974c0feedfaca59b2d4c103 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 5 May 2023 15:14:45 +0200 Subject: [PATCH 87/97] HACK ignore gripper fingers joint state check until the controller is fixed and those fingers dont go bonkers all the time --- .../src/plans.lisp | 27 ++++++++++--------- .../src/grippers-via-fingers.lisp | 17 +++++++----- 2 files changed, 24 insertions(+), 20 deletions(-) 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 b295742702..b59b83e356 100644 --- a/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp +++ b/cram_3d_world/cram_urdf_environment_manipulation/src/plans.lisp @@ -175,19 +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))) - (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"))) + ;; (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 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 index d14ee3ecb9..2873259056 100644 --- a/cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp +++ b/cram_tiago/cram_tiago_process_modules/src/grippers-via-fingers.lisp @@ -180,13 +180,16 @@ (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*)))) + (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*) From 123b8c63f17ac28fcdc2b46a97e59a36cf8df823 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 5 May 2023 15:19:54 +0200 Subject: [PATCH 88/97] HACK ignore extra opening contraints in giskard --- .../src/environment-manipulation-goals.lisp | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) 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 acaeb8d19c..7a6a7d800f 100644 --- a/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp +++ b/cram_external_interfaces/cram_giskard/src/environment-manipulation-goals.lisp @@ -50,16 +50,17 @@ ;; *base-max-velocity-slow-xy* *base-max-velocity-slow-theta*) (make-open-or-close-constraint open-or-close arm handle-link joint-state) - (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)) + ;; (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)))) @@ -115,11 +116,11 @@ 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)) + ;; (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)) (call-action :action-goal (make-environment-manipulation-goal From 4c3926d523cfafc5ed526363af4ae35e3328fb87 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 5 May 2023 15:29:12 +0200 Subject: [PATCH 89/97] [demo] Tune pregrasp for door handles and rename --- .../src/environment.lisp | 12 ++-- .../src/eurobin-demo.lisp | 66 ++++++++++++------- 2 files changed, 50 insertions(+), 28 deletions(-) diff --git a/cram_demos/cram_object_knowledge/src/environment.lisp b/cram_demos/cram_object_knowledge/src/environment.lisp index d17a398660..4dd1034cb0 100644 --- a/cram_demos/cram_object_knowledge/src/environment.lisp +++ b/cram_demos/cram_object_knowledge/src/environment.lisp @@ -111,7 +111,7 @@ (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.15 "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") @@ -186,17 +186,17 @@ :lift-translation '(0 0 0) :2nd-lift-translation '(0 0 0)) -;; ANGLED from the right for doors -(man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :door-right - :grasp-translation `(,(- 0.05) 0.025 ,*handle-grasp-z-offset*) +;; 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 from the left for doors -(man-int:def-object-type-to-gripper-transforms :handle '(:left :right) :door-left +;; 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) diff --git a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp index 95f05c40a2..7e9045112f 100644 --- a/cram_demos/cram_projection_demos/src/eurobin-demo.lisp +++ b/cram_demos/cram_projection_demos/src/eurobin-demo.lisp @@ -61,7 +61,7 @@ (?accessing-window-base-front-pose (cl-transforms-stamped:make-pose-stamped "map" (roslisp:ros-time) - (cl-tf:make-3d-vector 9.7d0 4.3d0 0.0d0) + (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 @@ -102,38 +102,41 @@ ;; (desig:an action ;; (type going) ;; (target (desig:a location - ;; (pose ?accessing-window-base-front-pose))))) + ;; (pose ?accessing-window-base-pose-2))))) ) (when (<= step 2) - (exe:perform - (desig:an action - (type opening) - (arm left) - (distance 0.3) - (grasps (door-left)) - (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)) + (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 @@ -218,3 +221,22 @@ (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) +;; ))) From cdc3ebe03509965d1562ae4dd1b18e3b35434009 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 5 May 2023 15:29:58 +0200 Subject: [PATCH 90/97] [demo] Export eurobin-demo --- cram_demos/cram_projection_demos/src/package.lisp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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)) From d45bc9d654d0bba046405a5649dff4890ebdf5c4 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Fri, 5 May 2023 15:33:02 +0200 Subject: [PATCH 91/97] [demo] Launch unreal apartment version & tf_buffer --- cram_demos/cram_projection_demos/launch/apartment_pr2.launch | 2 +- cram_demos/cram_projection_demos/launch/apartment_tiago.launch | 2 +- cram_demos/cram_projection_demos/launch/everything.launch | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cram_demos/cram_projection_demos/launch/apartment_pr2.launch b/cram_demos/cram_projection_demos/launch/apartment_pr2.launch index 4c5f5be540..6a55bab060 100644 --- a/cram_demos/cram_projection_demos/launch/apartment_pr2.launch +++ b/cram_demos/cram_projection_demos/launch/apartment_pr2.launch @@ -5,6 +5,6 @@ - + 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 fe413eeb3d..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'"/> From 5e5f6fdd3d78dbb22a932a30e0f8971e1485d6d5 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Tue, 28 Jun 2022 15:27:20 +0200 Subject: [PATCH 92/97] Add http rosinstall for docker script --- cram-20.04-http.rosinstall | 39 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 cram-20.04-http.rosinstall diff --git a/cram-20.04-http.rosinstall b/cram-20.04-http.rosinstall new file mode 100644 index 0000000000..5710001731 --- /dev/null +++ b/cram-20.04-http.rosinstall @@ -0,0 +1,39 @@ +# 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: noetic +- 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: hsrb_meshes + uri: http://github.com/ToyotaResearchInstitute/hsr_meshes.git + version: master +- git: + local-name: hsrb_description + uri: http://github.com/code-iai/hsr_description.git + version: gripper_tool_frame_noetic +- git: + local-name: kdl_ik_services + uri: http://github.com/cram2/kdl_ik_service.git + + + From 6dc4129cc7520ea5a0864d940d03e662a8d6ed0d Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna <33067562+sunava@users.noreply.github.com> Date: Tue, 26 Jul 2022 12:38:18 +0200 Subject: [PATCH 93/97] Update cram-20.04-http.rosinstall --- cram-20.04-http.rosinstall | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/cram-20.04-http.rosinstall b/cram-20.04-http.rosinstall index 5710001731..4b0c65aeb8 100644 --- a/cram-20.04-http.rosinstall +++ b/cram-20.04-http.rosinstall @@ -2,7 +2,7 @@ - git: local-name: cram uri: http://github.com/cram2/cram.git - version: noetic + version: devel - git: local-name: roslisp_common uri: http://github.com/ros/roslisp_common.git @@ -23,14 +23,6 @@ local-name: iai_robots uri: http://github.com/code-iai/iai_robots.git version: master -- git: - local-name: hsrb_meshes - uri: http://github.com/ToyotaResearchInstitute/hsr_meshes.git - version: master -- git: - local-name: hsrb_description - uri: http://github.com/code-iai/hsr_description.git - version: gripper_tool_frame_noetic - git: local-name: kdl_ik_services uri: http://github.com/cram2/kdl_ik_service.git From 69ee83cb8c2f9ca3d807e1f728fcee2c31ad192f Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Fri, 25 Mar 2022 16:36:25 +0100 Subject: [PATCH 94/97] [jupyter] added a rosinstall for common-lisp-jupyter and its deps --- common-lisp-jupyter.rosinstall | 105 +++++++++++++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 common-lisp-jupyter.rosinstall diff --git a/common-lisp-jupyter.rosinstall b/common-lisp-jupyter.rosinstall new file mode 100644 index 0000000000..378f9346e5 --- /dev/null +++ b/common-lisp-jupyter.rosinstall @@ -0,0 +1,105 @@ +## Jupyter Common Lisp kernel package -- common-lisp-jupyter -- and its dependencies +#################################################################################### + +## TODO: assign specific commit numbers to ensure upstream updates don't break stuff + +## The repo addresses are taken from +## https://github.com/quicklisp/quicklisp-projects/blob/master/projects//source.txt + +- git: + local-name: common-lisp-jupyter + uri: https://github.com/yitzchak/common-lisp-jupyter.git +# version: master + +- git: + local-name: bordeaux-threads + uri: https://github.com/sionescu/bordeaux-threads.git + +# bordeaux-threads dep +- git: + local-name: global-vars + uri: https://github.com/lmj/global-vars.git + +- git: + local-name: cl-base64 + uri: https://gitlab.common-lisp.net/nyxt/cl-base64.git + +- git: + local-name: cl-indentify + uri: https://github.com/yitzchak/cl-indentify.git + +- git: + local-name: closer-mop + uri: https://github.com/pcostanza/closer-mop.git + +- git: + local-name: dissect + uri: https://github.com/Shinmera/dissect.git + +- git: + local-name: eclector + uri: https://github.com/robert-strandh/eclector.git + +# eclector dep +- git: + local-name: acclimation + uri: https://github.com/robert-strandh/Acclimation.git + +- git: + local-name: ironclad + uri: https://github.com/sharplispers/ironclad.git + +- git: + local-name: multilang-documentation + uri: https://github.com/Shinmera/multilang-documentation.git + +# multilang-documentation dep +- git: + local-name: documentation-utils + uri: https://github.com/Shinmera/documentation-utils.git + +# documentation-utils dep +- git: + local-name: trivial-indent + uri: https://github.com/Shinmera/trivial-indent.git + +# multilang-documentation dep +- git: + local-name: language-codes + uri: https://github.com/Shinmera/language-codes.git + +# multilang-documentation dep +- git: + local-name: system-locale + uri: https://github.com/Shinmera/system-locale.git + +# sudo apt install libzmq3-dev +- git: + local-name: pzmq + uri: https://github.com/orivej/pzmq.git + +# pzmq dep, newest version, our cram_3rdparty version is outdated +#- git: +# local-name: cffi +# uri: https://gitlab.common-lisp.net/cffi/cffi.git + +- git: + local-name: puri + uri: https://gitlab.common-lisp.net/clpm/puri.git + +- git: + local-name: static-vectors + uri: https://github.com/sionescu/static-vectors.git + +- git: + local-name: trivial-do + uri: https://github.com/yitzchak/trivial-do.git + +- git: + local-name: trivial-mimes + uri: https://github.com/Shinmera/trivial-mimes.git + +- git: + local-name: shasht + uri: https://github.com/yitzchak/shasht.git + From 5e3adcd5d7fb17df2ba403cd06947ebfac25194a Mon Sep 17 00:00:00 2001 From: Gayane Kazhoyan Date: Wed, 6 Jul 2022 13:37:37 +0200 Subject: [PATCH 95/97] [jupyter] moved rosinstall to the jupyter directory --- common-lisp-jupyter.rosinstall | 105 --------------------------------- 1 file changed, 105 deletions(-) delete mode 100644 common-lisp-jupyter.rosinstall diff --git a/common-lisp-jupyter.rosinstall b/common-lisp-jupyter.rosinstall deleted file mode 100644 index 378f9346e5..0000000000 --- a/common-lisp-jupyter.rosinstall +++ /dev/null @@ -1,105 +0,0 @@ -## Jupyter Common Lisp kernel package -- common-lisp-jupyter -- and its dependencies -#################################################################################### - -## TODO: assign specific commit numbers to ensure upstream updates don't break stuff - -## The repo addresses are taken from -## https://github.com/quicklisp/quicklisp-projects/blob/master/projects//source.txt - -- git: - local-name: common-lisp-jupyter - uri: https://github.com/yitzchak/common-lisp-jupyter.git -# version: master - -- git: - local-name: bordeaux-threads - uri: https://github.com/sionescu/bordeaux-threads.git - -# bordeaux-threads dep -- git: - local-name: global-vars - uri: https://github.com/lmj/global-vars.git - -- git: - local-name: cl-base64 - uri: https://gitlab.common-lisp.net/nyxt/cl-base64.git - -- git: - local-name: cl-indentify - uri: https://github.com/yitzchak/cl-indentify.git - -- git: - local-name: closer-mop - uri: https://github.com/pcostanza/closer-mop.git - -- git: - local-name: dissect - uri: https://github.com/Shinmera/dissect.git - -- git: - local-name: eclector - uri: https://github.com/robert-strandh/eclector.git - -# eclector dep -- git: - local-name: acclimation - uri: https://github.com/robert-strandh/Acclimation.git - -- git: - local-name: ironclad - uri: https://github.com/sharplispers/ironclad.git - -- git: - local-name: multilang-documentation - uri: https://github.com/Shinmera/multilang-documentation.git - -# multilang-documentation dep -- git: - local-name: documentation-utils - uri: https://github.com/Shinmera/documentation-utils.git - -# documentation-utils dep -- git: - local-name: trivial-indent - uri: https://github.com/Shinmera/trivial-indent.git - -# multilang-documentation dep -- git: - local-name: language-codes - uri: https://github.com/Shinmera/language-codes.git - -# multilang-documentation dep -- git: - local-name: system-locale - uri: https://github.com/Shinmera/system-locale.git - -# sudo apt install libzmq3-dev -- git: - local-name: pzmq - uri: https://github.com/orivej/pzmq.git - -# pzmq dep, newest version, our cram_3rdparty version is outdated -#- git: -# local-name: cffi -# uri: https://gitlab.common-lisp.net/cffi/cffi.git - -- git: - local-name: puri - uri: https://gitlab.common-lisp.net/clpm/puri.git - -- git: - local-name: static-vectors - uri: https://github.com/sionescu/static-vectors.git - -- git: - local-name: trivial-do - uri: https://github.com/yitzchak/trivial-do.git - -- git: - local-name: trivial-mimes - uri: https://github.com/Shinmera/trivial-mimes.git - -- git: - local-name: shasht - uri: https://github.com/yitzchak/shasht.git - From c493cf7083290b41bfcefdc51a18125c0168a0fb Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Thu, 30 Jun 2022 13:09:24 +0200 Subject: [PATCH 96/97] Add jupyter kernel installer and readme. --- jupyter/README.md | 14 ++++++++++++++ jupyter/sbcl-jupyter-kernel-installer.lisp | 18 ++++++++++++++++++ 2 files changed, 32 insertions(+) create mode 100644 jupyter/README.md create mode 100644 jupyter/sbcl-jupyter-kernel-installer.lisp 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) From 9746ccc9526675e1944a75a52d6c4ea0adec98c5 Mon Sep 17 00:00:00 2001 From: Arthur Niedzwiecki Date: Tue, 9 May 2023 16:54:36 +0200 Subject: [PATCH 97/97] Add deps for eurobin docker image --- eurobin.rosinstall | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 eurobin.rosinstall 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