mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Use verbose delimiters for source code instead of line numbers
This commit is contained in:
parent
813d9a7de7
commit
e57d198dd7
@ -19,7 +19,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/alt
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/alternatives.py
|
||||
:language: python
|
||||
:lines: 22-60
|
||||
:start-after: [initAndConfigAlternatives]
|
||||
:end-before: [initAndConfigAlternatives]
|
||||
|
||||
.. _subsubsec-howto-fallbacks:
|
||||
|
||||
@ -32,7 +33,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/fal
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/fallbacks.py
|
||||
:language: python
|
||||
:lines: 21-38
|
||||
:start-after: [initAndConfigFallbacks]
|
||||
:end-before: [initAndConfigFallbacks]
|
||||
|
||||
.. _subsubsec-howto-merger:
|
||||
|
||||
@ -44,7 +46,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/mer
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/merger.py
|
||||
:language: python
|
||||
:lines: 20-36
|
||||
:start-after: [initAndConfigMerger]
|
||||
:end-before: [initAndConfigMerger]
|
||||
|
||||
.. _subsubsec-howto-connect:
|
||||
|
||||
@ -57,7 +60,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/pic
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 42-49
|
||||
:start-after: [initAndConfigConnect]
|
||||
:end-before: [initAndConfigConnect]
|
||||
|
||||
.. _subsubsec-howto-fix-collision-objects:
|
||||
|
||||
@ -69,7 +73,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/fix
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/fix_collision_objects.py
|
||||
:language: python
|
||||
:lines: 16-23
|
||||
:start-after: [initAndConfig]
|
||||
:end-before: [initAndConfig]
|
||||
|
||||
.. _subsubsec-howto-generate-place-pose:
|
||||
|
||||
@ -85,7 +90,13 @@ Download the full example code here: :download:`Source <./../../demo/scripts/pic
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 24-31, 89-97
|
||||
:start-after: [initCollisionObject]
|
||||
:end-before: [initCollisionObject]
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:start-after: [initAndConfigGeneratePlacePose]
|
||||
:end-before: [initAndConfigGeneratePlacePose]
|
||||
|
||||
.. _subsubsec-howto-generate-grasp-pose:
|
||||
|
||||
@ -101,7 +112,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/pic
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 51-56
|
||||
:start-after: [initAndConfigGenerateGraspPose]
|
||||
:end-before: [initAndConfigGenerateGraspPose]
|
||||
|
||||
.. _subsubsec-howto-generate-pose:
|
||||
|
||||
@ -113,7 +125,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/gen
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/generate_pose.py
|
||||
:language: python
|
||||
:lines: 32-45
|
||||
:start-after: [initAndConfigGeneratePose]
|
||||
:end-before: [initAndConfigGeneratePose]
|
||||
|
||||
.. _subsubsec-howto-pick:
|
||||
|
||||
@ -126,7 +139,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/pic
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 66-84
|
||||
:start-after: [initAndConfigPick]
|
||||
:end-before: [initAndConfigPick]
|
||||
|
||||
.. _subsubsec-howto-place:
|
||||
|
||||
@ -139,7 +153,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/pic
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 102-121
|
||||
:start-after: [initAndConfigPlace]
|
||||
:end-before: [initAndConfigPlace]
|
||||
|
||||
.. _subsubsec-howto-simplegrasp:
|
||||
|
||||
@ -153,7 +168,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/pic
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 58-64
|
||||
:start-after: [initAndConfigSimpleGrasp]
|
||||
:end-before: [initAndConfigSimpleGrasp]
|
||||
|
||||
.. _subsubsec-howto-simpleungrasp:
|
||||
|
||||
@ -167,7 +183,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/pic
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 99-106
|
||||
:start-after: [initAndConfigSimpleUnGrasp]
|
||||
:end-before: [initAndConfigSimpleUnGrasp]
|
||||
|
||||
.. _subsubsec-howto-modify-planning-scene:
|
||||
|
||||
@ -179,7 +196,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/mod
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/modify_planning_scene.py
|
||||
:language: python
|
||||
:lines: 19-43
|
||||
:start-after: [initAndConfigModifyPlanningScene]
|
||||
:end-before: [initAndConfigModifyPlanningScene]
|
||||
|
||||
.. _subsubsec-howto-fixed-state:
|
||||
|
||||
@ -191,7 +209,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/fix
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/fixed_state.py
|
||||
:language: python
|
||||
:lines: 16-25
|
||||
:start-after: [initAndConfigFixedState]
|
||||
:end-before: [initAndConfigFixedState]
|
||||
|
||||
.. _subsubsec-howto-compute-ik:
|
||||
|
||||
@ -206,8 +225,8 @@ Download the full example code here: :download:`Source <./../../demo/scripts/com
|
||||
|
||||
.. literalinclude:: ./../../demo/scripts/compute_ik.py
|
||||
:language: python
|
||||
:lines: 35-45
|
||||
|
||||
:start-after: [initAndConfigComputeIk]
|
||||
:end-before: [initAndConfigComputeIk]
|
||||
|
||||
.. _subsubsec-howto-move-to:
|
||||
|
||||
@ -219,8 +238,8 @@ Download the full example code here: :download:`Source <../../demo/scripts/carte
|
||||
|
||||
.. literalinclude:: ../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 51-55
|
||||
|
||||
:start-after: [initAndConfigMoveTo]
|
||||
:end-before: [initAndConfigMoveTo]
|
||||
|
||||
.. _subsubsec-howto-move-relative:
|
||||
|
||||
@ -232,8 +251,8 @@ Download the full example code here: :download:`Source <../../demo/scripts/carte
|
||||
|
||||
.. literalinclude:: ../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 26-31
|
||||
|
||||
:start-after: [initAndConfigMoveRelative]
|
||||
:end-before: [initAndConfigMoveRelative]
|
||||
|
||||
.. _subsec-howto-stage-extension:
|
||||
|
||||
@ -252,7 +271,7 @@ passed through the stage.
|
||||
|
||||
.. literalinclude:: ./../../core/python/test/rostest_trampoline.py
|
||||
:language: python
|
||||
:lines: 72-87
|
||||
:pyobject: PyMoveRelX
|
||||
|
||||
.. _subsubsec-howto-generator-extension:
|
||||
|
||||
@ -264,7 +283,7 @@ logic in the compute function.
|
||||
|
||||
.. literalinclude:: ./../../core/python/test/rostest_trampoline.py
|
||||
:language: python
|
||||
:lines: 23-44
|
||||
:pyobject: PyGenerator
|
||||
|
||||
.. _subsubsec-howto-monitoring-generator-extension:
|
||||
|
||||
@ -279,4 +298,4 @@ monitored stage (e.g. computation of inverse kinematics).
|
||||
|
||||
.. literalinclude:: ./../../core/python/test/rostest_trampoline.py
|
||||
:language: python
|
||||
:lines: 47-69
|
||||
:pyobject: PyMonitoringGenerator
|
||||
|
@ -13,7 +13,8 @@ end effector that we want to use.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 14-15
|
||||
:start-after: [cartesianTut1]
|
||||
:end-before: [cartesianTut1]
|
||||
|
||||
The moveit task constructor provides different planners.
|
||||
We will use the ``CartesianPath`` and ``JointInterpolation``
|
||||
@ -21,7 +22,8 @@ planners for this example.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 17-19
|
||||
:start-after: [cartesianTut2]
|
||||
:end-before: [cartesianTut2]
|
||||
|
||||
Lets start by initializing a task and adding the current
|
||||
planning scene state and robot state to it.
|
||||
@ -29,7 +31,8 @@ This will be the starting state for our motion plan.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 21-24
|
||||
:start-after: [cartesianTut3]
|
||||
:end-before: [cartesianTut3]
|
||||
|
||||
To compute a relative motion in cartesian space, we can use
|
||||
the ``MoveRelative`` stage. Specify the planning group and
|
||||
@ -39,27 +42,31 @@ geometry message.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 26-31
|
||||
:start-after: [initAndConfigMoveRelative]
|
||||
:end-before: [initAndConfigMoveRelative]
|
||||
|
||||
Similarly we can move along a different axis.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 33-37
|
||||
:start-after: [cartesianTut4]
|
||||
:end-before: [cartesianTut4]
|
||||
|
||||
The ``MoveRelative`` stage also offers an interface to
|
||||
``Twist`` messages, allowing to specify rotations.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 39-43
|
||||
:start-after: [cartesianTut5]
|
||||
:end-before: [cartesianTut5]
|
||||
|
||||
Lastly, we can compute linear movements in cartiesian space
|
||||
Lastly, we can compute linear movements in cartesian space
|
||||
by providing offsets in joint space.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 45-49
|
||||
:start-after: [cartesianTut6]
|
||||
:end-before: [cartesianTut6]
|
||||
|
||||
If we want to specify goals instead of directions,
|
||||
we can use the ``MoveTo`` stage. In the following example
|
||||
@ -69,7 +76,8 @@ the robot configuration.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/cartesian.py
|
||||
:language: python
|
||||
:lines: 51-55
|
||||
:start-after: [initAndConfigMoveTo]
|
||||
:end-before: [initAndConfigMoveTo]
|
||||
|
||||
Lastly, we invoke the planning mechanism that traverses
|
||||
the task hierarchy for us and compute a valid motion plan.
|
||||
|
@ -11,7 +11,8 @@ end effector that you want to use.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 12-14
|
||||
:start-after: [pickAndPlaceTut1]
|
||||
:end-before: [pickAndPlaceTut1]
|
||||
|
||||
Next, we add the object that we want to displace to the
|
||||
planning scene. To this end, make sure that the planning scene
|
||||
@ -19,13 +20,15 @@ not already contains such object.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 16-33
|
||||
:start-after: [pickAndPlaceTut2]
|
||||
:end-before: [pickAndPlaceTut2]
|
||||
|
||||
At this point, we are ready to create the task hierarchy.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 39-40
|
||||
:start-after: [pickAndPlaceTut3]
|
||||
:end-before: [pickAndPlaceTut3]
|
||||
|
||||
The pipeline planner encapsulates the moveit interface
|
||||
to sampling-based geometric motion planners.
|
||||
@ -42,7 +45,8 @@ the following stages.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 42-49
|
||||
:start-after: [pickAndPlaceTut4]
|
||||
:end-before: [pickAndPlaceTut4]
|
||||
|
||||
To pick the object, we first need to know possible end effector
|
||||
poses with which we can perform a successful grasp.
|
||||
@ -53,7 +57,8 @@ point.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 51-56
|
||||
:start-after: [pickAndPlaceTut5]
|
||||
:end-before: [pickAndPlaceTut5]
|
||||
|
||||
Next, we need to compute the inverse kinematics of the robot arm
|
||||
for all previously sampled poses. This way we can
|
||||
@ -64,7 +69,8 @@ the object to the robot an disabling collision.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 58-64
|
||||
:start-after: [pickAndPlaceTut6]
|
||||
:end-before: [pickAndPlaceTut6]
|
||||
|
||||
Lastly, we can insert all the previous steps into the ``Pick``
|
||||
container stage. At this point we might also specify approach and
|
||||
@ -72,7 +78,8 @@ lift twists for the robot relative to the object we want to grasp.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 66-81
|
||||
:start-after: [pickAndPlaceTut7]
|
||||
:end-before: [pickAndPlaceTut7]
|
||||
|
||||
Since all the previous stages were chained together via their
|
||||
constructor arguments, we only need to add the top level ``Pick``
|
||||
@ -80,42 +87,48 @@ stage to the task hierarchy.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 83-84
|
||||
:start-after: [pickAndPlaceTut8]
|
||||
:end-before: [pickAndPlaceTut8]
|
||||
|
||||
Thats everything we need for picking an object!
|
||||
Lets find a motion plan to place the object
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 86-87
|
||||
:start-after: [pickAndPlaceTut9]
|
||||
:end-before: [pickAndPlaceTut9]
|
||||
|
||||
Similar to the picking procedure, we define the place task.
|
||||
First, start with sampling place poses.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 89-97
|
||||
:start-after: [pickAndPlaceTut10]
|
||||
:end-before: [pickAndPlaceTut10]
|
||||
|
||||
Next, wrap the inverse kinematics computation and gripper
|
||||
movements.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 99-100
|
||||
:start-after: [pickAndPlaceTut11]
|
||||
:end-before: [pickAndPlaceTut11]
|
||||
|
||||
Lastly, add place and retract motions and add the ``Place``
|
||||
stage to the task hierarchy.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 102-121
|
||||
:start-after: [pickAndPlaceTut12]
|
||||
:end-before: [pickAndPlaceTut12]
|
||||
|
||||
Finally, compute solutions for the task hierarchy and delete
|
||||
the planner instances.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/pickplace.py
|
||||
:language: python
|
||||
:lines: 120-128
|
||||
:start-after: [pickAndPlaceTut13]
|
||||
:end-before: [pickAndPlaceTut13]
|
||||
|
||||
At this point, you might inspect the task hierarchy in the mtc rviz
|
||||
plugin.
|
||||
|
@ -15,7 +15,8 @@ a value to it.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 18-25
|
||||
:start-after: [propertyTut1]
|
||||
:end-before: [propertyTut1]
|
||||
|
||||
Notice that a property always has two values: the current value
|
||||
and the default value. Before we use the property, we might want to
|
||||
@ -23,13 +24,15 @@ check if the current value defined.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 27-28
|
||||
:start-after: [propertyTut2]
|
||||
:end-before: [propertyTut2]
|
||||
|
||||
Now we are ready to safely retrieve the values of the proprty!
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 30-37
|
||||
:start-after: [propertyTut3]
|
||||
:end-before: [propertyTut3]
|
||||
|
||||
The Property Map
|
||||
^^^^^^^^^^^^^^^^
|
||||
@ -44,39 +47,45 @@ of arbitrary type.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 39-47
|
||||
:start-after: [propertyTut4]
|
||||
:end-before: [propertyTut4]
|
||||
|
||||
Properties can also be initialized using a more pythonic way.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 49-50
|
||||
:start-after: [propertyTut5]
|
||||
:end-before: [propertyTut5]
|
||||
|
||||
There are two ways to retrieve properties back from the property map.
|
||||
We might only be interested in in the value of the property:
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 52-53
|
||||
:start-after: [propertyTut6]
|
||||
:end-before: [propertyTut6]
|
||||
|
||||
Or we can obtain a refernece to the whole property object.
|
||||
Or we can obtain a reference to the whole property object.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 55-56
|
||||
:start-after: [propertyTut7]
|
||||
:end-before: [propertyTut7]
|
||||
|
||||
The PropertyMap class additionally provides an iterator that can be used in loops.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 58-62
|
||||
:start-after: [propertyTut8]
|
||||
:end-before: [propertyTut8]
|
||||
|
||||
Remember that wer initialized our PropertyMap by using a dict. In fact, you
|
||||
can also use an existing PropertMap to copy over some properties.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 64-67
|
||||
:start-after: [propertyTut9]
|
||||
:end-before: [propertyTut9]
|
||||
|
||||
Accessing Properties of a Stage
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
@ -85,7 +94,14 @@ You can obtain a reference to the the PropertyMap of a stage like so
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/properties.py
|
||||
:language: python
|
||||
:lines: 15-16, 74-75
|
||||
: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.
|
||||
@ -94,7 +110,18 @@ you can use the reference of a stages' property object.
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/compute_ik.py
|
||||
:language: python
|
||||
:lines: 27-28, 35-36, 40-42
|
||||
:start-after: [propertyTut12]
|
||||
:end-before: [propertyTut12]
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/compute_ik.py
|
||||
:language: python
|
||||
:start-after: [propertyTut13]
|
||||
:end-before: [propertyTut13]
|
||||
|
||||
.. literalinclude:: ../../../demo/scripts/compute_ik.py
|
||||
:language: python
|
||||
:start-after: [propertyTut14]
|
||||
:end-before: [propertyTut14]
|
||||
|
||||
Take a look at the :ref:`How-To-Guides <subsubsec-howto-compute-ik>`
|
||||
for a full example of this.
|
||||
|
@ -19,6 +19,7 @@ currentState = stages.CurrentState("current state")
|
||||
# Add the current state to the task hierarchy
|
||||
task.add(currentState)
|
||||
|
||||
# [initAndConfigAlternatives]
|
||||
# The alternatives stage supports multiple execution paths
|
||||
alternatives = core.Alternatives("Alternatives")
|
||||
|
||||
@ -58,6 +59,7 @@ alternatives.insert(moveTo2)
|
||||
|
||||
# Add the alternatives stage to the task hierarchy
|
||||
task.add(alternatives)
|
||||
# [initAndConfigAlternatives]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -11,49 +11,64 @@ from moveit.python_tools import roscpp_init
|
||||
|
||||
roscpp_init("mtc_tutorial")
|
||||
|
||||
# [cartesianTut1]
|
||||
group = "panda_arm"
|
||||
eef_frame = "panda_link8"
|
||||
# [cartesianTut1]
|
||||
|
||||
# [cartesianTut2]
|
||||
# Cartesian and joint-space interpolation planners
|
||||
cartesian = core.CartesianPath()
|
||||
jointspace = core.JointInterpolationPlanner()
|
||||
# [cartesianTut2]
|
||||
|
||||
# [cartesianTut3]
|
||||
task = core.Task()
|
||||
|
||||
# start from current robot state
|
||||
task.add(stages.CurrentState("current state"))
|
||||
# [cartesianTut3]
|
||||
|
||||
# [initAndConfigMoveRelative]
|
||||
# move along x
|
||||
move = stages.MoveRelative("x +0.2", cartesian)
|
||||
move.group = group
|
||||
header = Header(frame_id="world")
|
||||
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
|
||||
task.add(move)
|
||||
# [initAndConfigMoveRelative]
|
||||
|
||||
# [cartesianTut4]
|
||||
# move along y
|
||||
move = stages.MoveRelative("y -0.3", cartesian)
|
||||
move.group = group
|
||||
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0)))
|
||||
task.add(move)
|
||||
# [cartesianTut4]
|
||||
|
||||
# [cartesianTut5]
|
||||
# rotate about z
|
||||
move = stages.MoveRelative("rz +45°", cartesian)
|
||||
move.group = group
|
||||
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0))))
|
||||
task.add(move)
|
||||
# [cartesianTut5]
|
||||
|
||||
# [cartesianTut6]
|
||||
# Cartesian motion, defined as joint-space offset
|
||||
move = stages.MoveRelative("joint offset", cartesian)
|
||||
move.group = group
|
||||
move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6))
|
||||
task.add(move)
|
||||
# [cartesianTut6]
|
||||
|
||||
# [initAndConfigMoveTo]
|
||||
# moveTo named posture, using joint-space interplation
|
||||
move = stages.MoveTo("moveTo ready", jointspace)
|
||||
move.group = group
|
||||
move.setGoal("ready")
|
||||
task.add(move)
|
||||
|
||||
# [initAndConfigMoveTo]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -24,25 +24,33 @@ planner = core.PipelinePlanner() # create default planning pipeline
|
||||
task.add(stages.Connect("connect", [(group, planner)])) # operate on group
|
||||
del planner # Delete PipelinePlanner when not explicitly needed anymore
|
||||
|
||||
# [propertyTut12]
|
||||
# Add a Cartesian pose generator
|
||||
generator = stages.GeneratePose("cartesian pose")
|
||||
# [propertyTut12]
|
||||
# Inherit PlanningScene state from "current state" stage
|
||||
generator.setMonitoredStage(task["current state"])
|
||||
# Configure target pose
|
||||
# [propertyTut13]
|
||||
pose = Pose(position=Vector3(z=0.2))
|
||||
generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose)
|
||||
# [propertyTut13]
|
||||
|
||||
# [initAndConfigComputeIk]
|
||||
# Wrap Cartesian generator into a ComputeIK stage to yield a joint pose
|
||||
computeIK = stages.ComputeIK("compute IK", generator)
|
||||
computeIK.group = group # Use the group-specific IK solver
|
||||
computeIK.ik_frame = "panda_link8" # Which end-effector frame should reach the target?
|
||||
computeIK.max_ik_solutions = 4 # Limit the number of IK solutions
|
||||
# [propertyTut14]
|
||||
props = computeIK.properties
|
||||
# derive target_pose from child's solution
|
||||
props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
|
||||
# [propertyTut14]
|
||||
|
||||
# Add the stage to the task hierarchy
|
||||
task.add(computeIK)
|
||||
# [initAndConfigComputeIk]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -18,6 +18,7 @@ task = core.Task()
|
||||
currentState = stages.CurrentState("Current State")
|
||||
task.add(currentState)
|
||||
|
||||
# [initAndConfigFallbacks]
|
||||
# create a fallback container to fall back to a different planner
|
||||
# if motion generation fails with the primary one
|
||||
fallbacks = core.Fallbacks("Fallbacks")
|
||||
@ -36,6 +37,7 @@ fallbacks.insert(moveTo2)
|
||||
|
||||
# add the fallback container to the task hierarchy
|
||||
task.add(fallbacks)
|
||||
# [initAndConfigFallbacks]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -13,6 +13,7 @@ task = core.Task()
|
||||
# Add the current state to the task hierarchy
|
||||
task.add(stages.CurrentState("current state"))
|
||||
|
||||
# [initAndConfig]
|
||||
# check for collisions and find corrections
|
||||
fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects")
|
||||
|
||||
@ -21,6 +22,7 @@ fixCollisionObjects.max_penetration = 0.01
|
||||
|
||||
# Add the stage to the task hierarchy
|
||||
task.add(fixCollisionObjects)
|
||||
# [initAndConfig]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -13,6 +13,7 @@ roscpp_init("mtc_tutorial_current_state")
|
||||
# Create a task
|
||||
task = core.Task()
|
||||
|
||||
# [initAndConfigFixedState]
|
||||
# Initialize a PlanningScene for use in a FixedState stage
|
||||
task.loadRobotModel() # load the robot model (usually done in init())
|
||||
planningScene = PlanningScene(task.getRobotModel())
|
||||
@ -23,6 +24,7 @@ fixedState.setState(planningScene)
|
||||
|
||||
# Add the stage to the task hierarchy
|
||||
task.add(fixedState)
|
||||
# [initAndConfigFixedState]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -29,6 +29,7 @@ connect = stages.Connect("connect1", planners)
|
||||
connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT)
|
||||
task.add(connect)
|
||||
|
||||
# [initAndConfigGeneratePose]
|
||||
# create an example pose wrt. the origin of the
|
||||
# panda arm link8
|
||||
pose = PoseStamped()
|
||||
@ -43,6 +44,7 @@ generatePose.pose = pose
|
||||
|
||||
# Add the stage to the task hierarchy
|
||||
task.add(generatePose)
|
||||
# [initAndConfigGeneratePose]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -17,6 +17,7 @@ task = core.Task()
|
||||
currentState = stages.CurrentState("current state")
|
||||
task.add(currentState)
|
||||
|
||||
# [initAndConfigMerger]
|
||||
# the merger plans for two parallel execution paths
|
||||
merger = core.Merger("Merger")
|
||||
|
||||
@ -34,6 +35,7 @@ merger.insert(moveTo2)
|
||||
|
||||
# add the merger stage to the task hierarchy
|
||||
task.add(merger)
|
||||
# [initAndConfigMerger]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -16,6 +16,7 @@ task = core.Task()
|
||||
# Add the current state to the task hierarchy
|
||||
task.add(stages.CurrentState("current state"))
|
||||
|
||||
# [initAndConfigModifyPlanningScene]
|
||||
# Specify object parameters
|
||||
object_name = "grasp_object"
|
||||
object_radius = 0.02
|
||||
@ -41,6 +42,7 @@ object.operation = object.ADD
|
||||
modifyPlanningScene = stages.ModifyPlanningScene("modify planning scene")
|
||||
modifyPlanningScene.addObject(object)
|
||||
task.add(modifyPlanningScene)
|
||||
# [initAndConfigModifyPlanningScene]
|
||||
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
@ -9,10 +9,13 @@ import time
|
||||
|
||||
roscpp_init("pickplace")
|
||||
|
||||
# [pickAndPlaceTut1]
|
||||
# Specify robot parameters
|
||||
arm = "panda_arm"
|
||||
eef = "hand"
|
||||
# [pickAndPlaceTut1]
|
||||
|
||||
# [pickAndPlaceTut2]
|
||||
# Specify object parameters
|
||||
object_name = "grasp_object"
|
||||
object_radius = 0.02
|
||||
@ -21,6 +24,7 @@ object_radius = 0.02
|
||||
psi = PlanningSceneInterface(synchronous=True)
|
||||
psi.remove_world_object()
|
||||
|
||||
# [initCollisionObject]
|
||||
# Grasp object properties
|
||||
objectPose = PoseStamped()
|
||||
objectPose.header.frame_id = "world"
|
||||
@ -28,17 +32,23 @@ objectPose.pose.orientation.x = 1.0
|
||||
objectPose.pose.position.x = 0.30702
|
||||
objectPose.pose.position.y = 0.0
|
||||
objectPose.pose.position.z = 0.285
|
||||
# [initCollisionObject]
|
||||
|
||||
# Add the grasp object to the planning scene
|
||||
psi.add_box(object_name, objectPose, size=[0.1, 0.05, 0.03])
|
||||
# [pickAndPlaceTut2]
|
||||
|
||||
# [pickAndPlaceTut3]
|
||||
# Create a task
|
||||
task = core.Task("PandaPickPipelineExample")
|
||||
task.enableIntrospection()
|
||||
# [pickAndPlaceTut3]
|
||||
|
||||
# [pickAndPlaceTut4]
|
||||
# Start with the current state
|
||||
task.add(stages.CurrentState("current"))
|
||||
|
||||
# [initAndConfigConnect]
|
||||
# Create a planner instance that is used to connect
|
||||
# the current state to the grasp approach pose
|
||||
pipeline = core.PipelinePlanner()
|
||||
@ -47,14 +57,22 @@ planners = [(arm, pipeline)]
|
||||
|
||||
# Connect the two stages
|
||||
task.add(stages.Connect("connect1", planners))
|
||||
# [initAndConfigConnect]
|
||||
# [pickAndPlaceTut4]
|
||||
|
||||
# [pickAndPlaceTut5]
|
||||
# [initAndConfigGenerateGraspPose]
|
||||
# The grasp generator spawns a set of possible grasp poses around the object
|
||||
grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
|
||||
grasp_generator.angle_delta = 0.2
|
||||
grasp_generator.pregrasp = "open"
|
||||
grasp_generator.grasp = "close"
|
||||
grasp_generator.setMonitoredStage(task["current"]) # Generate solutions for all initial states
|
||||
# [initAndConfigGenerateGraspPose]
|
||||
# [pickAndPlaceTut5]
|
||||
|
||||
# [pickAndPlaceTut6]
|
||||
# [initAndConfigSimpleGrasp]
|
||||
# SimpleGrasp container encapsulates IK calculation of arm pose as well as finger closing
|
||||
simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp")
|
||||
# Set frame for IK calculation in the center between the fingers
|
||||
@ -62,7 +80,11 @@ ik_frame = PoseStamped()
|
||||
ik_frame.header.frame_id = "panda_hand"
|
||||
ik_frame.pose.position.z = 0.1034
|
||||
simpleGrasp.setIKFrame(ik_frame)
|
||||
# [initAndConfigSimpleGrasp]
|
||||
# [pickAndPlaceTut6]
|
||||
|
||||
# [pickAndPlaceTut7]
|
||||
# [initAndConfigPick]
|
||||
# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object
|
||||
pick = stages.Pick(simpleGrasp, "Pick")
|
||||
pick.eef = eef
|
||||
@ -79,13 +101,21 @@ lift = TwistStamped()
|
||||
lift.header.frame_id = "panda_hand"
|
||||
lift.twist.linear.z = -1.0
|
||||
pick.setLiftMotion(lift, 0.03, 0.1)
|
||||
# [pickAndPlaceTut7]
|
||||
|
||||
# [pickAndPlaceTut8]
|
||||
# Add the pick stage to the task's stage hierarchy
|
||||
task.add(pick)
|
||||
# [initAndConfigPick]
|
||||
# [pickAndPlaceTut8]
|
||||
|
||||
# [pickAndPlaceTut9]
|
||||
# Connect the Pick stage with the following Place stage
|
||||
task.add(stages.Connect("connect2", planners))
|
||||
# [pickAndPlaceTut9]
|
||||
|
||||
# [pickAndPlaceTut10]
|
||||
# [initAndConfigGeneratePlacePose]
|
||||
# Define the pose that the object should have after placing
|
||||
placePose = objectPose
|
||||
placePose.pose.position.y += 0.2 # shift object by 20cm along y axis
|
||||
@ -95,15 +125,23 @@ place_generator = stages.GeneratePlacePose("Generate Place Pose")
|
||||
place_generator.setMonitoredStage(task["Pick"])
|
||||
place_generator.object = object_name
|
||||
place_generator.pose = placePose
|
||||
# [initAndConfigGeneratePlacePose]
|
||||
# [pickAndPlaceTut10]
|
||||
|
||||
# [initAndConfigSimpleUnGrasp]
|
||||
# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
|
||||
# [pickAndPlaceTut11]
|
||||
simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
|
||||
# [pickAndPlaceTut11]
|
||||
|
||||
# [pickAndPlaceTut12]
|
||||
# [initAndConfigPlace]
|
||||
# Place container comprises placing, ungrasping, and retracting
|
||||
place = stages.Place(simpleUnGrasp, "Place")
|
||||
place.eef = eef
|
||||
place.object = object_name
|
||||
place.eef_frame = "panda_link8"
|
||||
# [initAndConfigSimpleUnGrasp]
|
||||
|
||||
# Twist to retract from the object
|
||||
retract = TwistStamped()
|
||||
@ -119,13 +157,17 @@ place.setPlaceMotion(placeMotion, 0.03, 0.1)
|
||||
|
||||
# Add the place pipeline to the task's hierarchy
|
||||
task.add(place)
|
||||
# [initAndConfigPlace]
|
||||
# [pickAndPlaceTut12]
|
||||
|
||||
# [pickAndPlaceTut13]
|
||||
if task.plan():
|
||||
task.publish(task.solutions[0])
|
||||
|
||||
# avoid ClassLoader warning
|
||||
del pipeline
|
||||
del planners
|
||||
# [pickAndPlaceTut13]
|
||||
|
||||
# Prevent the program from exiting, giving you the opportunity to inspect solutions in rviz
|
||||
time.sleep(3600)
|
||||
|
@ -12,21 +12,28 @@ 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()
|
||||
|
||||
# Set a descriptive string to describe the properties function
|
||||
p.setDescription("Foo Property")
|
||||
# [propertyTut1]
|
||||
|
||||
# Set the current and the default value
|
||||
p.setValue("Bar")
|
||||
|
||||
# [propertyTut2]
|
||||
# Check if the property is defined
|
||||
assert p.defined()
|
||||
# [propertyTut2]
|
||||
|
||||
# [propertyTut3]
|
||||
# Retrieve the stored value
|
||||
print(p.value())
|
||||
|
||||
@ -35,39 +42,54 @@ print(p.defaultValue())
|
||||
|
||||
# Retrieve the description
|
||||
print(p.description())
|
||||
# [propertyTut3]
|
||||
|
||||
# [propertyTut4]
|
||||
# Create a property map
|
||||
pm = core.PropertyMap()
|
||||
props = {"prop1": "test", "prop2": 21, "prop3": PoseStamped(), "prop4": 5.4}
|
||||
pm.update(props)
|
||||
# [propertyTut4]
|
||||
|
||||
# [propertyTut5]
|
||||
# Add a property to the property map using the pythonic way
|
||||
pm["prop5"] = 2
|
||||
# [propertyTut5]
|
||||
|
||||
# [propertyTut6]
|
||||
# Return the value of a property
|
||||
print(pm["prop5"])
|
||||
# [propertyTut6]
|
||||
|
||||
# [propertyTut7]
|
||||
# Return the underlying property object
|
||||
p2 = pm.property("prop5")
|
||||
# [propertyTut7]
|
||||
|
||||
# [propertyTut8]
|
||||
# Iterate through all the values in the property map
|
||||
print("\n")
|
||||
for i in pm:
|
||||
print(i, "\t\t", pm[i])
|
||||
print("\n")
|
||||
# [propertyTut8]
|
||||
|
||||
# [propertyTut9]
|
||||
# A new property map can also be configured using an existing one
|
||||
# You can also only use a subset of the properties that should be configured.
|
||||
pm2 = core.PropertyMap()
|
||||
pm.exposeTo(pm2, ["prop2", "prop4"])
|
||||
# [propertyTut9]
|
||||
|
||||
# Lets test that by printing out our properties
|
||||
for i in pm2:
|
||||
print(i, "\t\t", pm2[i])
|
||||
print("\n")
|
||||
|
||||
# [propertyTut11]
|
||||
# Access the property map of the stage
|
||||
props = currentState.properties
|
||||
# [propertyTut11]
|
||||
|
||||
# Add the stage to the task hierarchy
|
||||
task.add(currentState)
|
||||
|
Loading…
Reference in New Issue
Block a user