From 31577c10c084a1b6848bc1e2f0ab6988d0a5dbff Mon Sep 17 00:00:00 2001 From: cpetersmeier Date: Mon, 27 Dec 2021 16:24:16 +0100 Subject: [PATCH] add further examples for basic stages - create more granular mwe examples as reference material in the documentation --- demo/scripts/compute_ik.py | 43 +++++++++++++++++++++++ demo/scripts/current_state.py | 21 ++++++++++++ demo/scripts/fix_collision_objects.py | 27 +++++++++++++++ demo/scripts/fixed_state.py | 29 ++++++++++++++++ demo/scripts/generate_pose.py | 49 +++++++++++++++++++++++++++ demo/scripts/modify_planning_scene.py | 47 +++++++++++++++++++++++++ 6 files changed, 216 insertions(+) create mode 100755 demo/scripts/compute_ik.py create mode 100755 demo/scripts/current_state.py create mode 100755 demo/scripts/fix_collision_objects.py create mode 100755 demo/scripts/fixed_state.py create mode 100755 demo/scripts/generate_pose.py create mode 100755 demo/scripts/modify_planning_scene.py diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py new file mode 100755 index 00000000..5368773e --- /dev/null +++ b/demo/scripts/compute_ik.py @@ -0,0 +1,43 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +from geometry_msgs.msg import PoseStamped +import time + +roscpp_init("mtc_tutorial_compute_ik") + +# Specify the planning group +group = "panda_arm" + +# Create a frame at the origin of link8 +ikFrame = PoseStamped() +ikFrame.header.frame_id = "panda_link8" + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") + +# Calculate the inverse kinematics for the current robot state +computeIK = stages.ComputeIK("compute IK", currentState) + +# Specify the planning group that should be used for ik calculation +computeIK.group = group + +# ik_frame references possible tip frames of the kinematic tree of +# the chosen planning group +computeIK.ik_frame = ikFrame +computeIK.target_pose = ikFrame + +# Limit the number of inverse kinematics solutions to 4 +computeIK.max_ik_solutions = 4 + +# Add the stage to the task hierarchy +task.add(computeIK) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py new file mode 100755 index 00000000..35e19ec6 --- /dev/null +++ b/demo/scripts/current_state.py @@ -0,0 +1,21 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_current_state") + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") + +# Add the stage to the task hierarchy +task.add(currentState) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fix_collision_objects.py b/demo/scripts/fix_collision_objects.py new file mode 100755 index 00000000..0828f0e7 --- /dev/null +++ b/demo/scripts/fix_collision_objects.py @@ -0,0 +1,27 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_current_state") + +# Create a task +task = core.Task() + +# Add the current state to the task hierarchy +task.add(stages.CurrentState("current state")) + +# check for collisions and find corrections +fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects") + +# cut off length for collision fixing +fixCollisionObjects.max_penetration = 0.01 + +# Add the stage to the task hierarchy +task.add(fixCollisionObjects) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fixed_state.py b/demo/scripts/fixed_state.py new file mode 100755 index 00000000..70ccbf02 --- /dev/null +++ b/demo/scripts/fixed_state.py @@ -0,0 +1,29 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.core import planning_scene +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +from moveit_commander import PlanningScene +import time + +roscpp_init("mtc_tutorial_current_state") + + +# Create a task +task = core.Task() + +# Get the current robot state +fixedState = stages.FixedState("fixed state") + +# create an empty planning scene and assign it to the +# fixed state +planningScene = PlanningScene() +fixedState.setState(planningScene) + +# Add the stage to the task hierarchy +task.add(fixedState) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py new file mode 100755 index 00000000..6d727349 --- /dev/null +++ b/demo/scripts/generate_pose.py @@ -0,0 +1,49 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_compute_ik") + +# Specify the planning group +group = "panda_arm" + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") +task.add(currentState) + +# Create a planner instance that is used to connect +# the current state to the grasp approach pose +pipelinePlanner = core.PipelinePlanner() +pipelinePlanner.planner = "RRTConnectkConfigDefault" +planners = [(group, pipelinePlanner)] + +# Connect the two stages +connect = stages.Connect("connect1", planners) +connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT) +task.add(connect) + +# create an example pose wrt. the origin of the +# panda arm link8 +pose = PoseStamped() +pose.header.frame_id = "panda_link8" + +# Calculate the inverse kinematics for the current robot state +generatePose = stages.GeneratePose("generate pose") + +# spwan a pose whenever there is a solution of the monitored stage +generatePose.setMonitoredStage(task["current state"]) +generatePose.pose = pose + +# Add the stage to the task hierarchy +task.add(generatePose) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/modify_planning_scene.py b/demo/scripts/modify_planning_scene.py new file mode 100755 index 00000000..a4fbf251 --- /dev/null +++ b/demo/scripts/modify_planning_scene.py @@ -0,0 +1,47 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import PoseStamped +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_modify_planning_scene") + +# Create a task +task = core.Task() + +# Add the current state to the task hierarchy +task.add(stages.CurrentState("current state")) + +# Specify object parameters +object_name = "grasp_object" +object_radius = 0.02 + +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 + +object = CollisionObject() +object.header.frame_id = "world" +object.id = object_name +sphere = SolidPrimitive() +sphere.type = sphere.SPHERE +sphere.dimensions.insert(sphere.SPHERE_RADIUS, object_radius) + +object.primitives.append(sphere) +object.primitive_poses.append(objectPose.pose) +object.operation = object.ADD + +modifyPlanningScene = stages.ModifyPlanningScene("modify planning scene") +modifyPlanningScene.addObject(object) +task.add(modifyPlanningScene) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1)