mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
examples: add orientation path constraint
constrained.py: constrain orientation of attached object pickplace.py: keep object upright during transport
This commit is contained in:
parent
69b4606bca
commit
1acf72e0b4
@ -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
|
||||||
|
|||||||
@ -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")
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user