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