examples: add orientation path constraint

constrained.py: constrain orientation of attached object
pickplace.py: keep object upright during transport
This commit is contained in:
Robert Haschke 2024-10-15 15:30:00 +02:00
parent 69b4606bca
commit 1acf72e0b4
2 changed files with 48 additions and 13 deletions

View File

@ -38,21 +38,35 @@ co.operation = co.ADD
mps = stages.ModifyPlanningScene("modify planning scene") mps = stages.ModifyPlanningScene("modify planning scene")
mps.addObject(co) 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) task.add(mps)
move = stages.MoveRelative("y +0.4", planner) move = stages.MoveRelative("y +0.4", planner)
move.timeout = 5
move.group = group move.group = group
header = Header(frame_id="world") header = Header(frame_id="world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0))) move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, 0.4, 0)))
constraints = Constraints() constraints = Constraints()
oc = OrientationConstraint() oc = OrientationConstraint()
oc.parameterization = oc.ROTATION_VECTOR
oc.header.frame_id = "world" oc.header.frame_id = "world"
oc.link_name = "panda_hand" oc.link_name = "object"
oc.orientation.x = 1.0 oc.orientation.x = oc.orientation.w = 0.70711 # 90° about x
oc.absolute_x_axis_tolerance = 0.01 oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.01 oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 0.01 oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0 oc.weight = 1.0
constraints.orientation_constraints.append(oc) constraints.orientation_constraints.append(oc)
move.path_constraints = constraints move.path_constraints = constraints

View File

@ -5,7 +5,8 @@ from py_binding_tools import roscpp_init
from moveit.task_constructor import core, stages from moveit.task_constructor import core, stages
from moveit_commander import PlanningSceneInterface from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, TwistStamped from geometry_msgs.msg import PoseStamped, TwistStamped
import time from moveit_msgs.msg import Constraints, OrientationConstraint
import math, time
roscpp_init("mtc_tutorial") roscpp_init("mtc_tutorial")
@ -17,7 +18,7 @@ eef = "hand"
# [pickAndPlaceTut2] # [pickAndPlaceTut2]
# Specify object parameters # Specify object parameters
object_name = "grasp_object" object_name = "object"
object_radius = 0.02 object_radius = 0.02
# Start with a clear planning scene # Start with a clear planning scene
@ -28,7 +29,7 @@ psi.remove_world_object()
# Grasp object properties # Grasp object properties
objectPose = PoseStamped() objectPose = PoseStamped()
objectPose.header.frame_id = "world" 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.x = 0.30702
objectPose.pose.position.y = 0.0 objectPose.pose.position.y = 0.0
objectPose.pose.position.z = 0.285 objectPose.pose.position.z = 0.285
@ -56,7 +57,7 @@ pipeline.planner = "RRTConnectkConfigDefault"
planners = [(arm, pipeline)] planners = [(arm, pipeline)]
# Connect the two stages # Connect the two stages
task.add(stages.Connect("connect1", planners)) task.add(stages.Connect("connect", planners))
# [initAndConfigConnect] # [initAndConfigConnect]
# [pickAndPlaceTut4] # [pickAndPlaceTut4]
@ -64,7 +65,7 @@ task.add(stages.Connect("connect1", planners))
# [initAndConfigGenerateGraspPose] # [initAndConfigGenerateGraspPose]
# The grasp generator spawns a set of possible grasp poses around the object # The grasp generator spawns a set of possible grasp poses around the object
grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose") 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.pregrasp = "open"
grasp_generator.grasp = "close" grasp_generator.grasp = "close"
grasp_generator.setMonitoredStage(task["current"]) # Generate solutions for all initial states grasp_generator.setMonitoredStage(task["current"]) # Generate solutions for all initial states
@ -78,7 +79,8 @@ simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp")
# Set frame for IK calculation in the center between the fingers # Set frame for IK calculation in the center between the fingers
ik_frame = PoseStamped() ik_frame = PoseStamped()
ik_frame.header.frame_id = "panda_hand" 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) simpleGrasp.setIKFrame(ik_frame)
# [initAndConfigSimpleGrasp] # [initAndConfigSimpleGrasp]
# [pickAndPlaceTut6] # [pickAndPlaceTut6]
@ -109,16 +111,35 @@ task.add(pick)
# [initAndConfigPick] # [initAndConfigPick]
# [pickAndPlaceTut8] # [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] # [pickAndPlaceTut9]
# Connect the Pick stage with the following Place stage # 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] # [pickAndPlaceTut9]
# [pickAndPlaceTut10] # [pickAndPlaceTut10]
# [initAndConfigGeneratePlacePose] # [initAndConfigGeneratePlacePose]
# Define the pose that the object should have after placing # Define the pose that the object should have after placing
placePose = objectPose 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 # Generate Cartesian place poses for the object
place_generator = stages.GeneratePlacePose("Generate Place Pose") place_generator = stages.GeneratePlacePose("Generate Place Pose")