mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
196 lines
5.5 KiB
Python
Executable File
196 lines
5.5 KiB
Python
Executable File
#! /usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
import rclcpp
|
|
from moveit.task_constructor import core, stages
|
|
from moveit_commander import PlanningSceneInterface
|
|
from geometry_msgs.msg import PoseStamped, TwistStamped
|
|
from moveit_msgs.msg import Constraints, OrientationConstraint
|
|
import math, time
|
|
|
|
rclcpp.init()
|
|
node = rclcpp.Node("mtc_tutorial")
|
|
|
|
# [pickAndPlaceTut1]
|
|
# Specify robot parameters
|
|
arm = "panda_arm"
|
|
eef = "hand"
|
|
# [pickAndPlaceTut1]
|
|
|
|
# [pickAndPlaceTut2]
|
|
# Specify object parameters
|
|
object_name = "object"
|
|
object_radius = 0.02
|
|
|
|
# Start with a clear planning scene
|
|
psi = PlanningSceneInterface(synchronous=True)
|
|
psi.remove_world_object()
|
|
|
|
# [initCollisionObject]
|
|
# Grasp object properties
|
|
objectPose = PoseStamped()
|
|
objectPose.header.frame_id = "world"
|
|
objectPose.pose.orientation.w = 1.0
|
|
objectPose.pose.position.x = 0.30702
|
|
objectPose.pose.position.y = 0.0
|
|
objectPose.pose.position.z = 0.285
|
|
# [initCollisionObject]
|
|
|
|
# Add the grasp object to the planning scene
|
|
psi.add_box(object_name, objectPose, size=[0.1, 0.05, 0.03])
|
|
# [pickAndPlaceTut2]
|
|
|
|
# [pickAndPlaceTut3]
|
|
# Create a task
|
|
task = core.Task()
|
|
task.name = "pick + place"
|
|
# [pickAndPlaceTut3]
|
|
|
|
# [pickAndPlaceTut4]
|
|
# Start with the current state
|
|
task.add(stages.CurrentState("current"))
|
|
|
|
# [initAndConfigConnect]
|
|
# Create a planner instance that is used to connect
|
|
# the current state to the grasp approach pose
|
|
pipeline = core.PipelinePlanner(node)
|
|
pipeline.planner = "RRTConnectkConfigDefault"
|
|
planners = [(arm, pipeline)]
|
|
|
|
# Connect the two stages
|
|
task.add(stages.Connect("connect", planners))
|
|
# [initAndConfigConnect]
|
|
# [pickAndPlaceTut4]
|
|
|
|
# [pickAndPlaceTut5]
|
|
# [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 = math.pi / 2
|
|
grasp_generator.pregrasp = "open"
|
|
grasp_generator.grasp = "close"
|
|
grasp_generator.setMonitoredStage(task["current"]) # Generate solutions for all initial states
|
|
# [initAndConfigGenerateGraspPose]
|
|
# [pickAndPlaceTut5]
|
|
|
|
# [pickAndPlaceTut6]
|
|
# [initAndConfigSimpleGrasp]
|
|
# SimpleGrasp container encapsulates IK calculation of arm pose as well as finger closing
|
|
simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp")
|
|
# 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 # tcp between fingers
|
|
ik_frame.pose.orientation.x = 1.0 # grasp from top
|
|
simpleGrasp.setIKFrame(ik_frame)
|
|
# [initAndConfigSimpleGrasp]
|
|
# [pickAndPlaceTut6]
|
|
|
|
# [pickAndPlaceTut7]
|
|
# [initAndConfigPick]
|
|
# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object
|
|
pick = stages.Pick(simpleGrasp, "Pick")
|
|
pick.eef = eef
|
|
pick.object = object_name
|
|
|
|
# Twist to approach the object
|
|
approach = TwistStamped()
|
|
approach.header.frame_id = "world"
|
|
approach.twist.linear.z = -1.0
|
|
pick.setApproachMotion(approach, 0.03, 0.1)
|
|
|
|
# Twist to lift the object
|
|
lift = TwistStamped()
|
|
lift.header.frame_id = "panda_hand"
|
|
lift.twist.linear.z = -1.0
|
|
pick.setLiftMotion(lift, 0.03, 0.1)
|
|
# [pickAndPlaceTut7]
|
|
|
|
# [pickAndPlaceTut8]
|
|
# Add the pick stage to the task's stage hierarchy
|
|
task.add(pick)
|
|
# [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
|
|
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.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")
|
|
place_generator.setMonitoredStage(task["Pick"])
|
|
place_generator.object = object_name
|
|
place_generator.pose = placePose
|
|
# [initAndConfigGeneratePlacePose]
|
|
# [pickAndPlaceTut10]
|
|
|
|
# [initAndConfigSimpleUnGrasp]
|
|
# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
|
|
# [pickAndPlaceTut11]
|
|
simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
|
|
# [pickAndPlaceTut11]
|
|
|
|
# [pickAndPlaceTut12]
|
|
# [initAndConfigPlace]
|
|
# Place container comprises placing, ungrasping, and retracting
|
|
place = stages.Place(simpleUnGrasp, "Place")
|
|
place.eef = eef
|
|
place.object = object_name
|
|
place.eef_frame = "panda_link8"
|
|
# [initAndConfigSimpleUnGrasp]
|
|
|
|
# Twist to retract from the object
|
|
retract = TwistStamped()
|
|
retract.header.frame_id = "world"
|
|
retract.twist.linear.z = 1.0
|
|
place.setRetractMotion(retract, 0.03, 0.1)
|
|
|
|
# Twist to place the object
|
|
placeMotion = TwistStamped()
|
|
placeMotion.header.frame_id = "panda_hand"
|
|
placeMotion.twist.linear.z = 1.0
|
|
place.setPlaceMotion(placeMotion, 0.03, 0.1)
|
|
|
|
# Add the place pipeline to the task's hierarchy
|
|
task.add(place)
|
|
# [initAndConfigPlace]
|
|
# [pickAndPlaceTut12]
|
|
|
|
# [pickAndPlaceTut13]
|
|
if task.plan():
|
|
task.publish(task.solutions[0])
|
|
|
|
# avoid ClassLoader warning
|
|
del pipeline
|
|
del planners
|
|
# [pickAndPlaceTut13]
|
|
|
|
# Prevent the program from exiting, giving you the opportunity to inspect solutions in rviz
|
|
time.sleep(3600)
|