mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add further examples for basic stages
- create more granular mwe examples as reference material
in the documentation
This commit is contained in:
parent
923022c13b
commit
31577c10c0
43
demo/scripts/compute_ik.py
Executable file
43
demo/scripts/compute_ik.py
Executable 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
21
demo/scripts/current_state.py
Executable 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)
|
||||
27
demo/scripts/fix_collision_objects.py
Executable file
27
demo/scripts/fix_collision_objects.py
Executable 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
29
demo/scripts/fixed_state.py
Executable 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
49
demo/scripts/generate_pose.py
Executable 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)
|
||||
47
demo/scripts/modify_planning_scene.py
Executable file
47
demo/scripts/modify_planning_scene.py
Executable 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)
|
||||
Loading…
Reference in New Issue
Block a user