From d6f1b69474187777216a4ca746f2cf917e61751e Mon Sep 17 00:00:00 2001 From: cpetersmeier Date: Wed, 22 Dec 2021 16:46:58 +0100 Subject: [PATCH] add demo pickplace example script --- demo/scripts/pickplace.py | 157 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 157 insertions(+) create mode 100755 demo/scripts/pickplace.py diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py new file mode 100755 index 00000000..68686202 --- /dev/null +++ b/demo/scripts/pickplace.py @@ -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()