Unify Python demo scripts

This commit is contained in:
Robert Haschke 2024-07-12 05:42:07 +02:00
parent 93ef98ad67
commit 0fed09d431
16 changed files with 50 additions and 52 deletions

View File

@ -97,11 +97,6 @@ You can obtain a reference to the the PropertyMap of a stage like so
:start-after: [propertyTut10]
:end-before: [propertyTut10]
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut11]
:end-before: [propertyTut11]
As mentioned, each stage contains a PropertyMap.
Stages communicate to each other via their interfaces.

View File

@ -23,20 +23,20 @@ Panels:
Name: Motion Planning Tasks
Tasks View:
property_splitter:
- 600
- 643
- 0
solution_sorting:
column: 1
order: 0
solutions_splitter:
- 223
- 72
- 316
- 102
solutions_view_columns: ~
tasks_view_columns:
- 107
- 38
- 194
- 38
- 38
- 44
Preferences:
PromptSaveOnExit: true
Toolbars:
@ -153,6 +153,10 @@ Visualization Manager:
Markers:
All at once?: false
Value: true
approach: true
lift: true
place: true
retract: true
Name: Motion Planning Tasks
Robot:
Fixed Robot Color: 150; 50; 150
@ -238,7 +242,7 @@ Visualization Manager:
State Display Time: REALTIME
Task Solution Topic: /mtc_tutorial/solution
Tasks:
{}
pick + place: 24
Trail Step Size: 1
Value: true
Enabled: true
@ -289,9 +293,9 @@ Window Geometry:
collapsed: false
Motion Planning Tasks - Slider:
collapsed: false
QMainWindow State: 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
QMainWindow State: 000000ff00000000fd00000002000000000000015600000303fc020000000efc0000003d00000303000000e60100001cfa000000000100000002fb000000100044006900730070006c0061007900730100000000000001770000015600fffffffb0000000a005600690065007700730100000000ffffffff0000010000fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000013d000001a6000000000000000000000001000001a600000303fc0200000002fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b0073010000003d000002bc000000e700fffffffb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c006900640065007201000002ff000000410000004100ffffff000002a20000030300000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
Width: 1450
X: 1920
Y: 28
X: 2560
Y: 9

View File

@ -5,13 +5,14 @@ from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_alternatives")
roscpp_init("mtc_tutorial")
# Use the joint interpolation planner
jointPlanner = core.JointInterpolationPlanner()
# Create a task
task = core.Task()
task.name = "alternatives"
# Start from current robot state
currentState = stages.CurrentState("current state")

View File

@ -23,6 +23,7 @@ jointspace = core.JointInterpolationPlanner()
# [cartesianTut3]
task = core.Task()
task.name = "cartesian"
# start from current robot state
task.add(stages.CurrentState("current state"))

View File

@ -2,19 +2,20 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped, Pose, Vector3
from geometry_msgs.msg import PoseStamped, Pose, Point
from std_msgs.msg import Header
import time
from py_binding_tools import roscpp_init
roscpp_init("mtc_tutorial_compute_ik")
roscpp_init("mtc_tutorial")
# Specify the planning group
group = "panda_arm"
# Create a task
task = core.Task()
task.name = "compute IK"
# Add a stage to retrieve the current state
task.add(stages.CurrentState("current state"))
@ -32,7 +33,7 @@ generator = stages.GeneratePose("cartesian pose")
generator.setMonitoredStage(task["current state"])
# Configure target pose
# [propertyTut13]
pose = Pose(position=Vector3(z=0.2))
pose = Pose(position=Point(z=0.2))
generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose)
# [propertyTut13]

View File

@ -16,6 +16,8 @@ group = "panda_arm"
planner = core.PipelinePlanner()
task = core.Task()
task.name = "constrained"
task.add(stages.CurrentState("current state"))
co = CollisionObject()

View File

@ -5,10 +5,11 @@ from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_init("mtc_tutorial")
# Create a task
task = core.Task()
task.name = "current state"
# Get the current robot state
currentState = stages.CurrentState("current state")

View File

@ -5,7 +5,7 @@ from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_fallbacks")
roscpp_init("mtc_tutorial")
# use cartesian and joint interpolation planners
cartesianPlanner = core.CartesianPath()
@ -13,6 +13,7 @@ jointPlanner = core.JointInterpolationPlanner()
# initialize the mtc task
task = core.Task()
task.name = "fallbacks"
# add the current planning scene state to the task hierarchy
currentState = stages.CurrentState("Current State")

View File

@ -5,10 +5,11 @@ from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_init("mtc_tutorial")
# Create a task
task = core.Task()
task.name = "fix collision objects"
# Add the current state to the task hierarchy
task.add(stages.CurrentState("current state"))

View File

@ -7,11 +7,12 @@ from py_binding_tools import roscpp_init
from moveit.core.planning_scene import PlanningScene
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_init("mtc_tutorial")
# Create a task
task = core.Task()
task.name = "fixed state"
# [initAndConfigFixedState]
# Initialize a PlanningScene for use in a FixedState stage

View File

@ -6,13 +6,14 @@ from geometry_msgs.msg import PoseStamped
from py_binding_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_compute_ik")
roscpp_init("mtc_tutorial")
# Specify the planning group
group = "panda_arm"
# Create a task
task = core.Task()
task.name = "generate pose"
# Get the current robot state
currentState = stages.CurrentState("current state")

View File

@ -5,13 +5,14 @@ from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_merger")
roscpp_init("mtc_tutorial")
# use the joint interpolation planner
planner = core.JointInterpolationPlanner()
# the task will contain our stages
task = core.Task()
task.name = "merger"
# start from current robot state
currentState = stages.CurrentState("current state")

View File

@ -8,10 +8,11 @@ from geometry_msgs.msg import PoseStamped
from py_binding_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_modify_planning_scene")
roscpp_init("mtc_tutorial")
# Create a task
task = core.Task()
task.name = "modify planning scene"
# Add the current state to the task hierarchy
task.add(stages.CurrentState("current state"))

View File

@ -7,15 +7,16 @@ import time
roscpp_init("mtc_tutorial")
ompl_pipelinePlanner = core.PipelinePlanner("ompl")
ompl_pipelinePlanner.planner = "RRTConnectkConfigDefault"
pilz_pipelinePlanner = core.PipelinePlanner("pilz_industrial_motion_planner")
pilz_pipelinePlanner.planner = "PTP"
ompl_planner = core.PipelinePlanner("ompl")
ompl_planner.planner = "RRTConnectkConfigDefault"
pilz_planner = core.PipelinePlanner("pilz_industrial_motion_planner")
pilz_planner.planner = "PTP"
multiPlanner = core.MultiPlanner()
multiPlanner.add(pilz_pipelinePlanner, ompl_pipelinePlanner)
multiPlanner.add(pilz_planner, ompl_planner)
# Create a task
task = core.Task()
task.name = "multi planner"
# Start from current robot state
currentState = stages.CurrentState("current state")

View File

@ -7,7 +7,7 @@ from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, TwistStamped
import time
roscpp_init("pickplace")
roscpp_init("mtc_tutorial")
# [pickAndPlaceTut1]
# Specify robot parameters
@ -40,8 +40,8 @@ psi.add_box(object_name, objectPose, size=[0.1, 0.05, 0.03])
# [pickAndPlaceTut3]
# Create a task
task = core.Task("PandaPickPipelineExample")
task.enableIntrospection()
task = core.Task()
task.name = "pick + place"
# [pickAndPlaceTut3]
# [pickAndPlaceTut4]

View File

@ -9,14 +9,6 @@ from py_binding_tools import roscpp_init
roscpp_init("mtc_tutorial")
# Create a task container
task = core.Task()
# [propertyTut10]
# Create a current state to capture the current planning scene state
currentState = stages.CurrentState("Current State")
# [propertyTut10]
# [propertyTut1]
# Create a property
p = core.Property()
@ -86,15 +78,9 @@ for i in pm2:
print(i, "\t\t", pm2[i])
print("\n")
# [propertyTut11]
# [propertyTut10]
# Create a stage
stage = stages.CurrentState("Current State")
# Access the property map of the stage
props = currentState.properties
# [propertyTut11]
# Add the stage to the task hierarchy
task.add(currentState)
if task.plan():
task.publish(task.solutions[0])
time.sleep(100)
props = stage.properties
# [propertyTut10]