From 1acf72e0b4347e0d4fe89a379de73f6d03a09633 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 15 Oct 2024 15:30:00 +0200
Subject: [PATCH] examples: add orientation path constraint

constrained.py: constrain orientation of attached object
pickplace.py: keep object upright during transport
---
 demo/scripts/constrained.py | 24 +++++++++++++++++++-----
 demo/scripts/pickplace.py   | 37 +++++++++++++++++++++++++++++--------
 2 files changed, 48 insertions(+), 13 deletions(-)

diff --git a/demo/scripts/constrained.py b/demo/scripts/constrained.py
index 4efc53c2a..1849f0fd2 100755
--- a/demo/scripts/constrained.py
+++ b/demo/scripts/constrained.py
@@ -38,21 +38,35 @@
 
 mps = stages.ModifyPlanningScene("modify planning scene")
 mps.addObject(co)
+
+co.id = "object"
+co.primitives[0].type = SolidPrimitive.BOX
+co.primitives[0].dimensions = [0.1, 0.05, 0.03]
+pose = co.primitive_poses[0]
+pose.position.x = 0.30702
+pose.position.y = 0.0
+pose.position.z = 0.485
+pose.orientation.x = pose.orientation.w = 0.70711  # 90° about x
+mps.addObject(co)
+mps.attachObjects(["object"], "panda_hand")
+
 task.add(mps)
 
 move = stages.MoveRelative("y +0.4", planner)
+move.timeout = 5
 move.group = group
 header = Header(frame_id="world")
 move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0)))
 
 constraints = Constraints()
 oc = OrientationConstraint()
+oc.parameterization = oc.ROTATION_VECTOR
 oc.header.frame_id = "world"
-oc.link_name = "panda_hand"
-oc.orientation.x = 1.0
-oc.absolute_x_axis_tolerance = 0.01
-oc.absolute_y_axis_tolerance = 0.01
-oc.absolute_z_axis_tolerance = 0.01
+oc.link_name = "object"
+oc.orientation.x = oc.orientation.w = 0.70711  # 90° about x
+oc.absolute_x_axis_tolerance = 0.1
+oc.absolute_y_axis_tolerance = 0.1
+oc.absolute_z_axis_tolerance = 3.14
 oc.weight = 1.0
 constraints.orientation_constraints.append(oc)
 move.path_constraints = constraints
diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py
index 5105c6d12..709b786c9 100755
--- a/demo/scripts/pickplace.py
+++ b/demo/scripts/pickplace.py
@@ -5,7 +5,8 @@
 from moveit.task_constructor import core, stages
 from moveit_commander import PlanningSceneInterface
 from geometry_msgs.msg import PoseStamped, TwistStamped
-import time
+from moveit_msgs.msg import Constraints, OrientationConstraint
+import math, time
 
 roscpp_init("mtc_tutorial")
 
@@ -17,7 +18,7 @@
 
 # [pickAndPlaceTut2]
 # Specify object parameters
-object_name = "grasp_object"
+object_name = "object"
 object_radius = 0.02
 
 # Start with a clear planning scene
@@ -28,7 +29,7 @@
 # Grasp object properties
 objectPose = PoseStamped()
 objectPose.header.frame_id = "world"
-objectPose.pose.orientation.x = 1.0
+objectPose.pose.orientation.w = 1.0
 objectPose.pose.position.x = 0.30702
 objectPose.pose.position.y = 0.0
 objectPose.pose.position.z = 0.285
@@ -56,7 +57,7 @@
 planners = [(arm, pipeline)]
 
 # Connect the two stages
-task.add(stages.Connect("connect1", planners))
+task.add(stages.Connect("connect", planners))
 # [initAndConfigConnect]
 # [pickAndPlaceTut4]
 
@@ -64,7 +65,7 @@
 # [initAndConfigGenerateGraspPose]
 # The grasp generator spawns a set of possible grasp poses around the object
 grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
-grasp_generator.angle_delta = 0.2
+grasp_generator.angle_delta = math.pi / 2
 grasp_generator.pregrasp = "open"
 grasp_generator.grasp = "close"
 grasp_generator.setMonitoredStage(task["current"])  # Generate solutions for all initial states
@@ -78,7 +79,8 @@
 # Set frame for IK calculation in the center between the fingers
 ik_frame = PoseStamped()
 ik_frame.header.frame_id = "panda_hand"
-ik_frame.pose.position.z = 0.1034
+ik_frame.pose.position.z = 0.1034  # tcp between fingers
+ik_frame.pose.orientation.x = 1.0  # grasp from top
 simpleGrasp.setIKFrame(ik_frame)
 # [initAndConfigSimpleGrasp]
 # [pickAndPlaceTut6]
@@ -109,16 +111,35 @@
 # [initAndConfigPick]
 # [pickAndPlaceTut8]
 
+# Define orientation constraint to keep the object upright
+oc = OrientationConstraint()
+oc.parameterization = oc.ROTATION_VECTOR
+oc.header.frame_id = "world"
+oc.link_name = "object"
+oc.orientation.w = 1
+oc.absolute_x_axis_tolerance = 0.1
+oc.absolute_y_axis_tolerance = 0.1
+oc.absolute_z_axis_tolerance = math.pi
+oc.weight = 1.0
+
+constraints = Constraints()
+constraints.name = "object:upright"
+constraints.orientation_constraints.append(oc)
+
 # [pickAndPlaceTut9]
 # Connect the Pick stage with the following Place stage
-task.add(stages.Connect("connect2", planners))
+con = stages.Connect("connect", planners)
+con.path_constraints = constraints
+task.add(con)
 # [pickAndPlaceTut9]
 
 # [pickAndPlaceTut10]
 # [initAndConfigGeneratePlacePose]
 # Define the pose that the object should have after placing
 placePose = objectPose
-placePose.pose.position.y += 0.2  # shift object by 20cm along y axis
+placePose.pose.position.x = -0.2
+placePose.pose.position.y = -0.6
+placePose.pose.position.z = 0.0
 
 # Generate Cartesian place poses for the object
 place_generator = stages.GeneratePlacePose("Generate Place Pose")