add further examples for basic stages

- create more granular mwe examples as reference material
      in the documentation
This commit is contained in:
cpetersmeier 2021-12-27 16:24:16 +01:00 committed by Robert Haschke
parent 923022c13b
commit 31577c10c0
6 changed files with 216 additions and 0 deletions

43
demo/scripts/compute_ik.py Executable file
View File

@ -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)

21
demo/scripts/current_state.py Executable file
View File

@ -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)

View File

@ -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)

29
demo/scripts/fixed_state.py Executable file
View File

@ -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)

49
demo/scripts/generate_pose.py Executable file
View File

@ -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)

View File

@ -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)