mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add demo pickplace example script
This commit is contained in:
parent
f7c2fadde6
commit
d6f1b69474
157
demo/scripts/pickplace.py
Executable file
157
demo/scripts/pickplace.py
Executable file
@ -0,0 +1,157 @@
|
||||
#! /usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
from moveit.task_constructor import core, stages
|
||||
from moveit_commander import PlanningSceneInterface
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from std_msgs.msg import Header
|
||||
import rospy
|
||||
import time
|
||||
|
||||
rospy.init_node("PickPlacePython")
|
||||
|
||||
# Specify robot parameters
|
||||
arm = "panda_arm"
|
||||
eef = "hand"
|
||||
|
||||
# Specify object parameters
|
||||
object_name = "grasp_object"
|
||||
object_radius = 0.02
|
||||
|
||||
# Start with a clear planning scene
|
||||
psi = PlanningSceneInterface()
|
||||
time.sleep(0.2)
|
||||
psi.remove_world_object()
|
||||
time.sleep(0.2)
|
||||
|
||||
# Grasp object properties
|
||||
objectRadius = 1.0
|
||||
objectPose = PoseStamped()
|
||||
objectPose.header.frame_id = "world"
|
||||
objectPose.pose.orientation.x = 1.0
|
||||
objectPose.pose.position.x = 0.30702
|
||||
objectPose.pose.position.y = 0.0
|
||||
objectPose.pose.position.z = 0.285
|
||||
|
||||
# Add the grasp object to the planning scene
|
||||
time.sleep(0.2)
|
||||
psi.add_sphere(object_name, objectPose, object_radius)
|
||||
time.sleep(0.2)
|
||||
|
||||
# Create a task
|
||||
task = core.Task("PandaPickPipelineExample")
|
||||
task.enableIntrospection()
|
||||
|
||||
# Start with the current state
|
||||
current = stages.CurrentState("current")
|
||||
task.add(current)
|
||||
|
||||
# Create a planner instance that is used to connect
|
||||
# the current state to the grasp approach pose
|
||||
pipelinePlanner = core.PipelinePlanner()
|
||||
pipelinePlanner.planner = "RRTConnectkConfigDefault"
|
||||
planners = [(arm, pipelinePlanner)]
|
||||
|
||||
# Connect the two stages
|
||||
connect = stages.Connect("connect1", planners)
|
||||
connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT)
|
||||
task.add(connect)
|
||||
|
||||
# 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.pregrasp = "open"
|
||||
grasp_generator.grasp = "close"
|
||||
grasp_generator.setMonitoredStage(task["current"])
|
||||
|
||||
# Make sure the object is not getting placed
|
||||
# in-collision with the base frame of the panda's gripper
|
||||
ikFrame = PoseStamped()
|
||||
ikFrame.header.frame_id = "panda_hand"
|
||||
ikFrame.pose.position.z = 0.09
|
||||
|
||||
# Generate a simple grasp for every grasp generator pose.
|
||||
# This stage wraps the inverse kinematic computation
|
||||
simpleGrasp = stages.SimpleGrasp(grasp_generator, "Simple Grasp")
|
||||
simpleGrasp.setIKFrame(ikFrame)
|
||||
|
||||
# Pick pipeline wrapper
|
||||
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)
|
||||
|
||||
# Add the pick stage to the task's stage hierarchy
|
||||
task.add(pick)
|
||||
|
||||
# Connect the pick stage with the now following
|
||||
# place stage
|
||||
connect = stages.Connect("connect2", planners)
|
||||
connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT)
|
||||
task.add(connect)
|
||||
|
||||
# Define the pose that the object should obtain
|
||||
# when it is placed
|
||||
placePose = PoseStamped()
|
||||
placePose.header.frame_id = "world"
|
||||
placePose.pose = objectPose.pose
|
||||
placePose.pose.position.y += 0.2 # modify place pose relative to pick pose
|
||||
|
||||
# The grasp generator samples possible grasp
|
||||
# frames around the destination pose.
|
||||
grasp_generator = stages.GeneratePlacePose("Generate Place Pose")
|
||||
grasp_generator.setMonitoredStage(task["Pick"])
|
||||
grasp_generator.object = object_name
|
||||
grasp_generator.pose = placePose
|
||||
grasp_generator.properties.configureInitFrom(
|
||||
core.Stage.PropertyInitializerSource.PARENT, ["ik_frame"]
|
||||
)
|
||||
|
||||
# Generate a grasp for every sampled pose by again wrapping
|
||||
# inverse kinematics computation
|
||||
simpleUnGrasp = stages.SimpleUnGrasp(grasp_generator, "Simple Grasp")
|
||||
simpleUnGrasp.setIKFrame(ikFrame)
|
||||
simpleUnGrasp.grasp = "close"
|
||||
simpleUnGrasp.pregrasp = "open"
|
||||
|
||||
# Place wrapper stage
|
||||
place = stages.Place(simpleUnGrasp, "Place")
|
||||
place.eef = eef
|
||||
place.object = object_name
|
||||
place.eef_frame = "panda_link8"
|
||||
|
||||
# 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)
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
time.sleep(1)
|
||||
|
||||
# Prevent the program from exiting,
|
||||
# giving you the opportunity to inspect the solutions
|
||||
# visually via the solution topic in rviz.
|
||||
rospy.spin()
|
||||
Loading…
Reference in New Issue
Block a user