### SimpleGrasp Stage Setup

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Sets up the SimpleGrasp stage, which encapsulates IK calculation and finger closing for picking. Requires a grasp generator and defines the IK frame.

```python
# 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
ik_frame = PoseStamped()
ik_frame.header.frame_id = "panda_hand"
ik_frame.pose.position.z = 0.1034  # tcp between fingers
ik_frame.pose.orientation.x = 1.0  # grasp from top
simpleGrasp.setIKFrame(ik_frame)
```

--------------------------------

### Iterate Through PropertyMap

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Provides an example of iterating through all properties in a PropertyMap and printing their keys and values.

```python
print("\n")
for i in pm:
    print(i, "\t\t", pm[i])
print("\n")
```

--------------------------------

### Configure MoveIt Motion Planning Tasks Properties Library

Source: https://github.com/moveit/moveit_task_constructor/blob/master/visualization/motion_planning_tasks/properties/CMakeLists.txt

Defines the library name, sources, and links against necessary packages like Qt and YAML. It also sets up include directories and installation rules.

```cmake
set(MOVEIT_LIB_NAME motion_planning_tasks_properties)

set(SOURCES
	property_factory.cpp
	property_from_yaml.cpp
)

find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML REQUIRED yaml-0.1)
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually
find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS})

add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES})

target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES} ${YAML_LIBRARIES})
target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>)
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS})

install(TARGETS ${MOVEIT_LIB_NAME}
	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

```

--------------------------------

### Initialize Task and Add Current State

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/cartesian.md

Create a new Task and add the current robot state as the starting point for the motion plan. This ensures the plan begins from the robot's current configuration.

```python
task = core.Task()
task.name = "cartesian"

# start from current robot state
task.add(stages.CurrentState("current state"))
```

--------------------------------

### Access Stage's PropertyMap

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Shows how to get a reference to the PropertyMap associated with a MoveIt Task Constructor stage.

```python
stage = stages.CurrentState("Current State")
props = stage.properties
```

--------------------------------

### SimpleUnGrasp Stage Setup

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Configures the SimpleUnGrasp stage for releasing an object at a specified Cartesian pose. It wraps pose generation and inverse kinematics computation for the place pipeline.

```python
# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
# [pickAndPlaceTut11]
simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
# [pickAndPlaceTut11]

# [pickAndPlaceTut12]
# [initAndConfigPlace]
```

--------------------------------

### core.Task - Top-level task container

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

The Task class is the root of a stage tree. It manages the robot model, drives the planning loop, and publishes solutions. This example demonstrates adding stages like CurrentState and Connect, then planning and publishing a solution.

```APIDOC
## core.Task - Top-level task container

`Task` is the root of a stage tree. It wraps a `SerialContainer` by default, owns the robot model, drives the planning loop, and publishes solutions for inspection in RViz.

```python
from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time

roscpp_init("my_task")

task = core.Task()
task.name = "my_task"

# Add stages (order matters for SerialContainer)
task.add(stages.CurrentState("current state"))

planner = core.PipelinePlanner()
planner.planner = "RRTConnectkConfigDefault"
task.add(stages.Connect("connect", [("panda_arm", planner)]))

move = stages.MoveTo("move to ready", core.JointInterpolationPlanner())
move.group = "panda_arm"
move.setGoal("ready")
task.add(move)

# Plan and publish best solution
if task.plan():
    task.publish(task.solutions[0])
else:
    task.printState()  # prints per-stage solution counts to stdout

del planner  # avoid ClassLoader warnings
time.sleep(1)
```
```

--------------------------------

### Launch Basic Environment

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/first-steps.md

Launch the basic MoveIt Task Constructor demo environment. This is a prerequisite for running individual demos.

```bash
roslaunch moveit_task_constructor_demo demo.launch
```

--------------------------------

### Create and Plan a Task with MoveIt Task Constructor

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Demonstrates creating a task, adding stages like CurrentState and Connect, setting a planner, and executing the plan. Ensure roscpp_init is called before creating the task.

```python
from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time

roscpp_init("my_task")

task = core.Task()
task.name = "my_task"

# Add stages (order matters for SerialContainer)
task.add(stages.CurrentState("current state"))

planner = core.PipelinePlanner()
planner.planner = "RRTConnectkConfigDefault"
task.add(stages.Connect("connect", [("panda_arm", planner)]))

move = stages.MoveTo("move to ready", core.JointInterpolationPlanner())
move.group = "panda_arm"
move.setGoal("ready")
task.add(move)

# Plan and publish best solution
if task.plan():
    task.publish(task.solutions[0])
else:
    task.printState()  # prints per-stage solution counts to stdout

del planner  # avoid ClassLoader warnings
time.sleep(1)
```

--------------------------------

### Run Individual Demos

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/first-steps.md

Execute specific MoveIt Task Constructor demos after launching the basic environment. Options include cartesian, modular, and pickplace.

```bash
rosrun moveit_task_constructor_demo cartesian
```

```bash
rosrun moveit_task_constructor_demo modular
```

```bash
roslaunch moveit_task_constructor_demo pickplace.launch
```

--------------------------------

### Implement Custom Generator Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Derive from the `Generator` stage to implement custom logic in the `compute` function. This example shows a basic custom generator that limits the number of calls.

```python
class PyGenerator(core.Generator):
    """Implements a custom 'Generator' stage."""

    max_calls = 3

    def __init__(self, name="Generator"):
        core.Generator.__init__(self, name)
        self.reset()

    def init(self, robot_model):
        self.ps = PlanningScene(robot_model)

    def reset(self):
        core.Generator.reset(self)
        self.num = self.max_calls

    def canCompute(self):
        return self.num > 0

    def compute(self):
        self.num = self.num - 1
        self.spawn(core.InterfaceState(self.ps), self.num)
```

--------------------------------

### Create and Describe a Property

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Demonstrates how to create a new property and assign it a descriptive string.

```python
p = core.Property()
p.setDescription("Foo Property")
```

--------------------------------

### Initialize PropertyMap from Dictionary

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Illustrates creating a PropertyMap and initializing it with properties from a Python dictionary. Properties can be of arbitrary types.

```python
pm = core.PropertyMap()
props = {"prop1": "test", "prop2": 21, "prop3": PoseStamped(), "prop4": 5.4}
pm.update(props)
```

--------------------------------

### Define gtest targets with .test files

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/test/CMakeLists.txt

Defines gtest targets that include a specific `.test` file, such as `move_to.test` and `move_relative.test`. This allows for more complex test setups managed by rostest.

```cmake
mtc_add_gtest(test_move_to.cpp move_to.test)
	mtc_add_gtest(test_move_relative.cpp move_relative.test)
```

--------------------------------

### Configure Property Initialization from Interface

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Illustrates how to configure a stage's properties to be initialized from an interface, specifying which properties to forward.

```python
props = computeIK.properties
props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
```

--------------------------------

### MoveIt Task Constructor: Parallel and Sequential Containers

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Demonstrates the usage of Alternatives, Fallbacks, and Merger containers for managing task execution. Alternatives evaluate all children simultaneously, Fallbacks process sequentially, and Merger combines trajectories.

```python
from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time

roscpp_init("containers_demo")

ompl  = core.PipelinePlanner("ompl");  ompl.planner  = "RRTConnectkConfigDefault"
pilz  = core.PipelinePlanner("pilz_industrial_motion_planner"); pilz.planner = "PTP"
joint = core.JointInterpolationPlanner()

task = core.Task()
task.name = "containers"
task.add(stages.CurrentState("start"))

# --- Alternatives: choose best of two goal configurations ---
alts = core.Alternatives("Alternatives")
for i, cfg in enumerate([
        {"panda_joint1":  1.0, "panda_joint2": -1.0, "panda_joint4": -2.5},
        {"panda_joint1": -3.0, "panda_joint2": -1.0, "panda_joint4": -2.0}]):
    m = stages.MoveTo(f"goal {i+1}", joint)
    m.group = "panda_arm"; m.setGoal(cfg)
    alts.insert(m)
task.add(alts)

# --- Fallbacks: prefer pilz, fall back to ompl ---
task2 = core.Task(); task2.name = "fallbacks"
task2.add(stages.CurrentState("start"))
fb = core.Fallbacks("Fallbacks")
for planner, name in [(pilz, "pilz"), (ompl, "ompl")]:
    m = stages.MoveTo(f"move ({name})", planner)
    m.group = "panda_arm"; m.setGoal("extended")
    fb.insert(m)
task2.add(fb)

# --- Merger: move arm and hand simultaneously ---
task3 = core.Task(); task3.name = "merger"
task3.add(stages.CurrentState("start"))
mg = core.Merger("Merger")
hand_move = stages.MoveTo("close hand", joint); hand_move.group = "hand"; hand_move.setGoal("close"); mg.insert(hand_move)
arm_move  = stages.MoveTo("extend arm",  joint); arm_move.group  = "panda_arm"; arm_move.setGoal("extended"); mg.insert(arm_move)
task3.add(mg)

for t in [task, task2, task3]:
    if t.plan():
        t.publish(t.solutions[0])
time.sleep(1)
```

--------------------------------

### stages.Connect

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Connector stage to bridge two states with motion planning. It plans a trajectory between the end state of a preceding stage and the start state of a following stage, potentially using multiple planners and merging trajectories.

```APIDOC
## stages.Connect

### Description
Connector stage to bridge two states with motion planning. It plans a trajectory between the end state of a preceding stage and the start state of a following stage, potentially using multiple planners and merging trajectories.

### Method
Not applicable (Python API)

### Endpoint
Not applicable (Python API)

### Parameters
- **planners** (list of tuples) - Required - A list of tuples, where each tuple contains a planning group name (string) and a planner configuration (e.g., `core.PipelinePlanner`).
- **path_constraints** (Constraints) - Optional - Constraints to be satisfied during the planned path (e.g., orientation constraints).

### Request Example
```python
from moveit.task_constructor import core, stages
from moveit_msgs.msg import Constraints, OrientationConstraint

pipeline = core.PipelinePlanner()
pipeline.planner = "RRTConnectkConfigDefault"
planners = [("panda_arm", pipeline)]

# Optional: path constraints
oc = OrientationConstraint()
oc.header.frame_id  = "world"
oc.link_name        = "object"
oc.orientation.w    = 1.0
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0

constraints = Constraints()
constraints.name = "upright"
constraints.orientation_constraints.append(oc)

connect = stages.Connect("transit", planners)
connect.path_constraints = constraints
```

### Response
Not applicable (Python API - returns success/failure of planning)
```

--------------------------------

### Initialize Planning Scene and Add Object

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Clears the planning scene and adds a box object for grasping. Ensure the object name is unique.

```python
# Specify object parameters
object_name = "object"
object_radius = 0.02

# Start with a clear planning scene
psi = PlanningSceneInterface(synchronous=True)
psi.remove_world_object()

# [initCollisionObject]
# Grasp object properties
objectPose = PoseStamped()
objectPose.header.frame_id = "world"
objectPose.pose.orientation.w = 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])
```

--------------------------------

### Configure and Forward Stage Properties

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Illustrates using `core.PropertyMap` for stage configuration and forwarding. Properties can be set directly, inherited from parents, or exposed to other maps. Ensure `roscpp_init` is called.

```python
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
from py_binding_tools import roscpp_init

roscpp_init("properties_demo")

# Create and populate a standalone PropertyMap
pm = core.PropertyMap()
pm.update({"speed": 0.5, "group": "panda_arm", "max_solutions": 4})
pm["timeout"] = 10.0
print("speed =", pm["speed"])
print("timeout =", pm["timeout"])

for name in pm:
    print(f"  {name}: {pm[name]}")

# Share properties between two maps
pm2 = core.PropertyMap()
pm.exposeTo(pm2, ["speed", "timeout"])

# Access and configure a stage's property map
stage = stages.CurrentState("cs")
props = stage.properties
props.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT, ["group"])

# Forward target_pose from child solution interface into ComputeIK
compute_ik = stages.ComputeIK("IK")
compute_ik.properties.configureInitFrom(
    core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
```

--------------------------------

### Configure Simple Grasp Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Sets up the `SimpleGrasp` stage, which combines inverse kinematics calculation with motion plan generation for grasping. The `ik_frame` specifies the pose for IK calculations relative to the end effector.

```python
# [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
ik_frame = PoseStamped()
ik_frame.header.frame_id = "panda_hand"
ik_frame.pose.position.z = 0.1034  # tcp between fingers
ik_frame.pose.orientation.x = 1.0  # grasp from top
simpleGrasp.setIKFrame(ik_frame)
# [initAndConfigSimpleGrasp]
```

--------------------------------

### Using Alternatives Stage for Multiple Execution Paths

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

The alternatives stage allows planning for multiple execution paths. Use this when you need to explore different ways to achieve a goal.

```python
# The alternatives stage supports multiple execution paths
alternatives = core.Alternatives("Alternatives")

# goal 1
goalConfig1 = {
    "panda_joint1": 1.0,
    "panda_joint2": -1.0,
    "panda_joint3": 0.0,
    "panda_joint4": -2.5,
    "panda_joint5": 1.0,
    "panda_joint6": 1.0,
    "panda_joint7": 1.0,
}

# goal 2
goalConfig2 = {
    "panda_joint1": -3.0,
    "panda_joint2": -1.0,
    "panda_joint3": 0.0,
    "panda_joint4": -2.0,
    "panda_joint5": 1.0,
    "panda_joint6": 2.0,
    "panda_joint7": 0.5,
}

# First motion plan to compare
moveTo1 = stages.MoveTo("Move To Goal Configuration 1", jointPlanner)
moveTo1.group = "panda_arm"
moveTo1.setGoal(goalConfig1)
alternatives.insert(moveTo1)

# Second motion plan to compare
moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner)
moveTo2.group = "panda_arm"
moveTo2.setGoal(goalConfig2)
alternatives.insert(moveTo2)

# Add the alternatives stage to the task hierarchy
task.add(alternatives)
```

--------------------------------

### High-level Pick and Place Container Stages

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Utilize `stages.Pick` and `stages.Place` for complete pick-and-place routines. These stages encapsulate sub-pipelines for approach, grasp, lift, lower, release, and retract motions.

```python
from moveit.task_constructor import core, stages
from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, TwistStamped
import math, time
from py_binding_tools import roscpp_init

roscpp_init("pick_place_demo")

arm, eef, object_name = "panda_arm", "hand", "box"
psi = PlanningSceneInterface(synchronous=True)
psi.remove_world_object()

obj_pose = PoseStamped()
obj_pose.header.frame_id = "world"
obj_pose.pose.position.x, obj_pose.pose.position.z = 0.307, 0.285
obj_pose.pose.orientation.w = 1.0
psi.add_box(object_name, obj_pose, size=[0.1, 0.05, 0.03])

pipeline = core.PipelinePlanner()
pipeline.planner = "RRTConnectkConfigDefault"
planners = [(arm, pipeline)]

task = core.Task()
task.name = "pick + place"
task.add(stages.CurrentState("current"))
task.add(stages.Connect("connect", planners))

# -- Pick --
grasp_gen = stages.GenerateGraspPose("grasp pose")
grasp_gen.angle_delta = math.pi / 2
grasp_gen.pregrasp, grasp_gen.grasp = "open", "close"
grasp_gen.setMonitoredStage(task["current"])

ik_frame = PoseStamped()
ik_frame.header.frame_id = "panda_hand"
ik_frame.pose.position.z, ik_frame.pose.orientation.x = 0.1034, 1.0
simple_grasp = stages.SimpleGrasp(grasp_gen, "Grasp")
simple_grasp.setIKFrame(ik_frame)

pick = stages.Pick(simple_grasp, "Pick")
pick.eef, pick.object = eef, object_name
approach = TwistStamped(); approach.header.frame_id = "world"
approach.twist.linear.z = -1.0
pick.setApproachMotion(approach, 0.03, 0.1)
lift = TwistStamped(); lift.header.frame_id = "panda_hand"
lift.twist.linear.z = -1.0
pick.setLiftMotion(lift, 0.03, 0.1)
task.add(pick)

task.add(stages.Connect("connect2", planners))

# -- Place --
place_pose = PoseStamped()
place_pose.header.frame_id = "world"
place_pose.pose.position.x, place_pose.pose.position.y = -0.2, -0.6
place_pose.pose.orientation.w = 1.0

place_gen = stages.GeneratePlacePose("place pose")
place_gen.setMonitoredStage(task["Pick"])
place_gen.object, place_gen.pose = object_name, place_pose

simple_ungrasp = stages.SimpleUnGrasp(place_gen, "UnGrasp")
place = stages.Place(simple_ungrasp, "Place")
place.eef, place.object, place.eef_frame = eef, object_name, "panda_link8"
retract = TwistStamped(); retract.header.frame_id = "world"
retract.twist.linear.z = 1.0
place.setRetractMotion(retract, 0.03, 0.1)
place_motion = TwistStamped(); place_motion.header.frame_id = "panda_hand"
place_motion.twist.linear.z = 1.0
place.setPlaceMotion(place_motion, 0.03, 0.1)
task.add(place)

if task.plan():
    task.publish(task.solutions[0])
del pipeline, planners
time.sleep(3600)
```

--------------------------------

### Generate Place Poses

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Initiates the configuration for the place task by sampling possible place poses.

```python
# [initAndConfigGeneratePlacePose]
```

--------------------------------

### Spawn Pre-defined State with FixedState Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Initializes and sets a PlanningScene for a FixedState stage. The robot model must be loaded before creating the PlanningScene.

```python
# Initialize a PlanningScene for use in a FixedState stage
task.loadRobotModel()  # load the robot model (usually done in init())
planningScene = PlanningScene(task.getRobotModel())

# Create a FixedState stage and pass the created PlanningScene as its state
fixedState = stages.FixedState("fixed state")
fixedState.setState(planningScene)

# Add the stage to the task hierarchy
task.add(fixedState)
```

--------------------------------

### MoveIt Task Constructor: Planner Interfaces

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Illustrates the use of various planner interfaces including PipelinePlanner, CartesianPath, JointInterpolationPlanner, and MultiPlanner. These are used to define motion planning strategies within MoveIt tasks.

```python
from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import time

roscpp_init("planners_demo")

# OMPL pipeline planner
ompl = core.PipelinePlanner("ompl")
ompl.planner = "RRTConnectkConfigDefault"

# PILZ industrial motion planner (point-to-point)
pilz = core.PipelinePlanner("pilz_industrial_motion_planner")
pilz.planner = "PTP"

# Cartesian path planner
cart = core.CartesianPath()
cart.max_cartesian_speed = 0.2           # m/s
cart.cartesian_speed_limited_link = "panda_hand"
cart.setStepSize(0.01)                   # interpolation resolution (m)
cart.setMinFraction(0.9)                 # require ≥90% path completion

# Joint interpolation (no collision avoidance)
joint = core.JointInterpolationPlanner()

# MultiPlanner: try pilz first, fall back to ompl
multi = core.MultiPlanner()
multi.add(pilz, ompl)

task = core.Task()
task.name = "planners"
task.add(stages.CurrentState("start"))

m1 = stages.MoveTo("cart move", cart);  m1.group = "panda_arm"; m1.setGoal("ready");    task.add(m1)
m2 = stages.MoveTo("multi move", multi); m2.group = "panda_arm"; m2.setGoal("extended"); task.add(m2)

if task.plan():
    task.publish(task.solutions[0])
del ompl, pilz, cart, joint, multi
time.sleep(1)
```

--------------------------------

### Initialize Cartesian and Joint Interpolation Planners

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/cartesian.md

Instantiate the CartesianPath and JointInterpolationPlanner. The CartesianPath planner requires setting maximum speed and the speed-limited link.

```python
# Cartesian and joint-space interpolation planners
cartesian = core.CartesianPath()
cartesian.max_cartesian_speed = 0.1  # m/s
cartesian.cartesian_speed_limited_link = "panda_hand"
jointspace = core.JointInterpolationPlanner()
```

--------------------------------

### Pick Stage Configuration

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Sets up the Pick stage for executing a pick operation, including approach and lift motions. Requires defining the end-effector, object name, and motion twists.

```python
# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object
pick = stages.Pick(simpleGrasp, "Pick")
pick.eef = eef
pick.object = object_name

# Twist to approach the object
approach = TwistStamped()
approach.header.frame_id = "world"
approach.twist.linear.z = -1.0
pick.setApproachMotion(approach, 0.03, 0.1)

pick.cartesian_solver.max_velocity_scaling_factor = 0.1

# Twist to lift the object
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)
```

--------------------------------

### Connect Pick to Place Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Connects the completed pick stage to the subsequent place stage. Path constraints can be applied here.

```python
# Connect the Pick stage with the following Place stage
con = stages.Connect("connect", planners)
con.path_constraints = constraints
task.add(con)
```

--------------------------------

### Move to Named Posture with MoveTo Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Uses planners to compute a motion plan to a named posture (e.g., 'ready') using joint-space interpolation.

```python
# moveTo named posture, using joint-space interplation
move = stages.MoveTo("moveTo ready", jointspace)
move.group = group
move.setGoal("ready")
task.add(move)
```

--------------------------------

### Create and Initialize Task

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Initializes a new MoveIt Task Constructor task and sets its name.

```python
# Create a task
task = core.Task()
task.name = "pick + place"
```

--------------------------------

### Task Planning and Cleanup

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Computes the solution for the defined task hierarchy and publishes the first solution. It's important to delete planner instances afterward to avoid potential warnings.

```python
if task.plan():
    task.publish(task.solutions[0])

# avoid ClassLoader warning
del pipeline
del planners
```

--------------------------------

### Generate Grasp Pose Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Sets up the GenerateGraspPose stage to sample grasp poses around an object. Use this for inverse kinematics calculations. Requires setting angle delta, pregrasp, and grasp actions.

```python
# The grasp generator spawns a set of possible grasp poses around the object
grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
grasp_generator.angle_delta = math.pi / 2
grasp_generator.pregrasp = "open"
grasp_generator.grasp = "close"
grasp_generator.setMonitoredStage(task["current"])
  # Generate solutions for all initial states
```

--------------------------------

### Copy Properties Between PropertyMaps

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Demonstrates how to copy properties from one PropertyMap to another, optionally selecting a subset of properties to expose.

```python
pm2 = core.PropertyMap()
pm.exposeTo(pm2, ["prop2", "prop4"])
```

--------------------------------

### Check and Retrieve Property Values

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Shows how to check if a property is defined and retrieve its current, default, and description values.

```python
assert p.defined()
print(p.value())
print(p.defaultValue())
print(p.description())
```

--------------------------------

### GenerateGraspPose: Sample Grasp Poses Around an Object

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

GenerateGraspPose samples end-effector poses around an object for grasping. It requires a monitored stage to inherit scene state and is typically followed by an IK solver. Configure angle delta, pregrasp, and grasp poses as needed.

```python
from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init
import math

roscpp_init("grasp_pose_demo")

task = core.Task()
task.name = "generate grasp"
task.add(stages.CurrentState("current"))

grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
grasp_generator.angle_delta = math.pi / 4   # 8 candidate orientations
grasp_generator.pregrasp    = "open"        # named hand pose before grasp
grasp_generator.grasp       = "close"       # named hand pose after grasp
grasp_generator.setMonitoredStage(task["current"])  # inherit scene state

# Wrap with IK solver to yield joint configurations
computeIK = stages.ComputeIK("grasp IK", grasp_generator)
computeIK.group = "panda_arm"
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header
computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_hand"))
computeIK.max_ik_solutions = 4
computeIK.properties.configureInitFrom(
    core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
task.add(computeIK)

if task.plan():
    task.publish(task.solutions[0])
```

--------------------------------

### MoveTo Stage with Different Goal Types in MoveIt Task Constructor

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Illustrates using the MoveTo stage with various goal types: a named SRDF pose, explicit joint values, and a Cartesian pose. Requires roscpp_init.

```python
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header
from py_binding_tools import roscpp_init

roscpp_init("move_to_demo")

jointspace = core.JointInterpolationPlanner()
cartesian  = core.CartesianPath()

task = core.Task()
task.name = "move_to"
task.add(stages.CurrentState("start"))

# Goal: named pose from SRDF
move1 = stages.MoveTo("to ready", jointspace)
move1.group = "panda_arm"
move1.setGoal("ready")
task.add(move1)

# Goal: explicit joint values
move2 = stages.MoveTo("to config", jointspace)
move2.group = "panda_arm"
move2.setGoal({"panda_joint1": 1.0, "panda_joint2": -1.0,
               "panda_joint4": -2.5, "panda_joint6":  1.0})
task.add(move2)

# Goal: Cartesian pose
target = PoseStamped(header=Header(frame_id="world"))
target.pose.position.x = 0.4
target.pose.position.z = 0.5
target.pose.orientation.w = 1.0
move3 = stages.MoveTo("to pose", cartesian)
move3.group = "panda_arm"
move3.setGoal(target)
task.add(move3)

if task.plan():
    task.publish(task.solutions[0])
```

--------------------------------

### Use CurrentState Stage in MoveIt Task Constructor

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Shows how to use the CurrentState stage to initialize a MoveIt Task Constructor pipeline with the robot's current joint configuration. Requires roscpp_init.

```python
from moveit.task_constructor import core, stages
from py_binding_tools import roscpp_init

roscpp_init("current_state_demo")

task = core.Task()
task.name = "current state demo"

# CurrentState seeds the pipeline with the actual robot joint configuration
task.add(stages.CurrentState("current state"))

if task.plan():
    task.publish(task.solutions[0])
```

--------------------------------

### Configure Pick Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Defines the `Pick` stage, which orchestrates approaching, grasping, and lifting the object. Approach and lift motions are specified using `TwistStamped`.

```python
# [initAndConfigPick]
# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object
pick = stages.Pick(simpleGrasp, "Pick")
pick.eef = eef
pick.object = object_name

# Twist to approach the object
approach = TwistStamped()
approach.header.frame_id = "world"
approach.twist.linear.z = -1.0
pick.setApproachMotion(approach, 0.03, 0.1)

pick.cartesian_solver.max_velocity_scaling_factor = 0.1

# Twist to lift the object
lift = TwistStamped()
lift.header.frame_id = "panda_hand"
lift.twist.linear.z = -1.0
pick.setLiftMotion(lift, 0.03, 0.1)

# Add the pick stage to the task's stage hierarchy
task.add(pick)
# [initAndConfigPick]
```

--------------------------------

### Place Stage Configuration

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Configures the Place stage, which includes ungrasping and retracting motions. Specify the end-effector, object name, and frame. Ensure the retract and place motions are defined using TwistStamped.

```python
# Place container comprises placing, ungrasping, and retracting
place = stages.Place(simpleUnGrasp, "Place")
place.eef = eef
place.object = object_name
place.eef_frame = "panda_link8"

# Twist to retract from the object
retract = TwistStamped()
retract.header.frame_id = "world"
retract.twist.linear.z = 1.0
place.setRetractMotion(retract, 0.03, 0.1)

# Twist to place the object
placeMotion = TwistStamped()
placeMotion.header.frame_id = "panda_hand"
placeMotion.twist.linear.z = 1.0
place.setPlaceMotion(placeMotion, 0.03, 0.1)

# Add the place pipeline to the task's hierarchy
task.add(place)
```

--------------------------------

### Define Place Pose and Generate Place Poses

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Sets the target pose for placing an object and configures a generator for Cartesian place poses. Ensure the object name and desired pose are correctly specified.

```python
placePose = objectPose
placePose.pose.position.x = -0.2
placePose.pose.position.y = -0.6
placePose.pose.position.z = 0.0

place_generator = stages.GeneratePlacePose("Generate Place Pose")
place_generator.setMonitoredStage(task["Pick"])
place_generator.object = object_name
place_generator.pose = placePose
```

--------------------------------

### Generate Place Pose Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Configures the GeneratePlacePose stage to determine possible placement poses for an object. It requires the object name and a target place pose.

```python
place_generator = stages.GeneratePlacePose("Generate Place Pose")
place_generator.setMonitoredStage(task["Pick"])
place_generator.object = object_name
place_generator.pose = placePose
```

--------------------------------

### Connect: Bridge Two States with Motion Planning

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Use Connect to plan a trajectory between two states using a specified planner. Path constraints can be optionally applied to maintain specific conditions during the motion. Ensure the planner is correctly configured.

```python
from moveit.task_constructor import core, stages
from moveit_msgs.msg import Constraints, OrientationConstraint
from py_binding_tools import roscpp_init

roscpp_init("connect_demo")

pipeline = core.PipelinePlanner()
pipeline.planner = "RRTConnectkConfigDefault"
planners = [("panda_arm", pipeline)]

task = core.Task()
task.name = "connect"
task.add(stages.CurrentState("start"))

# Optional: path constraints (keep object upright during transit)
oc = OrientationConstraint()
oc.header.frame_id  = "world"
oc.link_name        = "object"
oc.orientation.w    = 1.0
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0

constraints = Constraints()
constraints.name = "upright"
constraints.orientation_constraints.append(oc)

connect = stages.Connect("transit", planners)
connect.path_constraints = constraints
task.add(connect)

if task.plan():
    task.publish(task.solutions[0])
del pipeline
```

--------------------------------

### Add Current State and Connect Stages

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/pick-and-place.md

Adds the current robot state to the task and configures a pipeline planner to connect stages. Ensure the planner name is valid.

```python
# 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()
pipeline.planner = "RRTConnectkConfigDefault"
planners = [(arm, pipeline)]

# Connect the two stages
task.add(stages.Connect("connect", planners))
# [initAndConfigConnect]
```

--------------------------------

### Catch and Print InitStageException for Detailed Info

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/troubleshooting.md

Use this snippet to catch `InitStageException` during task planning and print its detailed error message to `std::cerr`. This is useful for diagnosing interface mismatches between planning pipeline stages.

```c++
try {
        task.plan();
} catch (const InitStageException& e) {
        std::cerr << e << std::endl;
        throw;
}
```

--------------------------------

### Place Stage Configuration

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Configures the Place stage for executing a place operation, including retract and place motions. Requires defining the end-effector, object name, and motion twists.

```python
# 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()
retract.header.frame_id = "world"
retract.twist.linear.z = 1.0
place.setRetractMotion(retract, 0.03, 0.1)

# Twist to place the object
placeMotion = TwistStamped()
placeMotion.header.frame_id = "panda_hand"
placeMotion.twist.linear.z = 1.0
place.setPlaceMotion(placeMotion, 0.03, 0.1)

# Add the place pipeline to the task's hierarchy
task.add(place)
```

--------------------------------

### Move To Named Pose with Joint Interpolation

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/cartesian.md

Use the MoveTo stage with the JointInterpolationPlanner to move the robot to a predefined named pose, such as 'ready'. The target pose must be defined in the robot's URDF configuration.

```python
# moveTo named posture, using joint-space interplation
move = stages.MoveTo("moveTo ready", jointspace)
move.group = group
move.setGoal("ready")
```

--------------------------------

### Generate Pose Stage

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Configures the GeneratePose stage to spawn a pose based on solutions from a monitored stage. Useful for creating poses relative to robot links.

```python
# 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)
```

--------------------------------

### Custom Generator and Propagator Stages

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Shows how to create custom MTC stages by subclassing `core.Generator` and `stages.MoveRelative`. The `CountdownGenerator` emits planning scenes, while `MoveXForward` translates the end-effector. Ensure `roscpp_init` is called.

```python
from moveit.task_constructor import core, stages
from moveit.planning import PlanningScene
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header
from py_binding_tools import roscpp_init
import time

roscpp_init("custom_stage_demo")

# --- Custom Generator: emits N planning scenes ---
class CountdownGenerator(core.Generator):
    max_calls = 3
    def __init__(self, name="Countdown"):
        core.Generator.__init__(self, name)
        self.reset()
    def init(self, robot_model):
        self.ps = PlanningScene(robot_model)
    def reset(self):
        core.Generator.reset(self)
        self.remaining = self.max_calls
    def canCompute(self):
        return self.remaining > 0
    def compute(self):
        self.remaining -= 1
        self.spawn(core.InterfaceState(self.ps), cost=float(self.remaining))

# --- Custom Propagator: translate end-effector +x ---
class MoveXForward(stages.MoveRelative):
    def __init__(self, distance, planner, name="Move +X"):
        stages.MoveRelative.__init__(self, name, planner)
        from geometry_msgs.msg import Vector3Stamped, Vector3
        self.group = "panda_arm"
        self.setDirection(
            Vector3Stamped(header=Header(frame_id="world"),
                           vector=Vector3(distance, 0, 0))
        )
    def computeForward(self, from_state):
        return stages.MoveRelative.computeForward(self, from_state)
    def computeBackward(self, to_state):
        return stages.MoveRelative.computeBackward(self, to_state)

cart = core.CartesianPath()
task = core.Task()
task.name = "custom stages"
task.add(CountdownGenerator("gen"))
task.add(MoveXForward(0.1, cart))

if task.plan():
    task.publish(task.solutions[0])
time.sleep(1)
```

--------------------------------

### Place Object with MoveIt Task Constructor

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

Defines a 'Place' stage for MoveIt Task Constructor, specifying the end-effector, object, and its frame for placement operations.

```python
place = stages.Place(simpleUnGrasp, "Place")
place.eef = eef
place.object = object_name
place.eef_frame = "panda_link8"
```

--------------------------------

### Add Sphere Collision Object and Attach

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

Demonstrates adding a sphere as a collision object and then attaching it to a robot link. Ensure the `CollisionObject` and `stages.ModifyPlanningScene` are correctly defined.

```python
obj = CollisionObject()
obj.header.frame_id = "world"
obj.id = "sphere"
sphere = SolidPrimitive()
sphere.type  = SolidPrimitive.SPHERE
sphere.dimensions = [0.02]   # radius
pose = PoseStamped()
pose.header.frame_id = "world"
pose.pose.position.x, pose.pose.position.z = 0.307, 0.285
pose.pose.orientation.w = 1.0
obj.primitives.append(sphere)
obj.primitive_poses.append(pose.pose)
obj.operation = CollisionObject.ADD

modify = stages.ModifyPlanningScene("add sphere")
modify.addObject(obj)
task.add(modify)

attach = stages.ModifyPlanningScene("attach sphere")
attach.attachObject("sphere", "panda_hand")
task.add(attach)

if task.plan():
    task.publish(task.solutions[0])
```

--------------------------------

### Retrieve Property Value and Object

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/tutorials/properties.md

Shows two ways to retrieve properties from a PropertyMap: by value using dictionary access, or by obtaining the full property object.

```python
print(pm["prop5"])
p2 = pm.property("prop5")
```

--------------------------------

### Using Fallbacks Stage for Alternative Planners

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

The fallbacks stage provides alternative motion planners if the primary one fails. This is useful for ensuring task completion by having backup planning strategies.

```python
# create a fallback container to fall back to a different planner
# if motion generation fails with the primary one
fallbacks = core.Fallbacks("Fallbacks")

# primary motion plan
moveTo1 = stages.MoveTo("Move To Goal Configuration 1", cartesianPlanner)
moveTo1.group = "panda_arm"
moveTo1.setGoal("extended")
fallbacks.insert(moveTo1)

# fallback motion plan
moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner)
moveTo2.group = "panda_arm"
moveTo2.setGoal("extended")
fallbacks.insert(moveTo2)

# add the fallback container to the task hierarchy
task.add(fallbacks)
```

--------------------------------

### stages.GenerateGraspPose

Source: https://context7.com/moveit/moveit_task_constructor/llms.txt

MonitoringGenerator stage to sample grasp poses around an object. It samples end-effector poses by rotating an approach direction and monitors another stage to inherit scene state, feeding solutions to subsequent stages like ComputeIK.

```APIDOC
## stages.GenerateGraspPose

### Description
MonitoringGenerator stage to sample grasp poses around an object. It samples end-effector poses by rotating an approach direction and monitors another stage to inherit scene state, feeding solutions to subsequent stages like ComputeIK.

### Method
Not applicable (Python API)

### Endpoint
Not applicable (Python API)

### Parameters
- **angle_delta** (float) - Optional - The angular increment for sampling grasp orientations.
- **pregrasp** (string) - Optional - The named end-effector pose before grasping.
- **grasp** (string) - Optional - The named end-effector pose after grasping.
- **monitored_stage** (core.Stage) - Required - The stage to monitor for scene state inheritance.
- **ik_frame** (PoseStamped) - Optional - The pose of the end-effector relative to the robot's base frame.
- **max_ik_solutions** (int) - Optional - The maximum number of Inverse Kinematics solutions to find.
- **target_pose** (PoseStamped) - Required (via configureInitFrom) - The target pose of the object to grasp.

### Request Example
```python
from moveit.task_constructor import core, stages
import math

# Assuming 'task' is a core.Task object and 'current' is a stage added to it
grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
grasp_generator.angle_delta = math.pi / 4
grasp_generator.pregrasp    = "open"
grasp_generator.grasp       = "close"
grasp_generator.setMonitoredStage(task["current"])

computeIK = stages.ComputeIK("grasp IK", grasp_generator)
computeIK.group = "panda_arm"
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Header
computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_hand"))
computeIK.max_ik_solutions = 4
computeIK.properties.configureInitFrom(
    core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
```

### Response
Not applicable (Python API - returns success/failure of planning and generated poses/joint configurations)
```

--------------------------------

### Using Merger Stage for Parallel Execution

Source: https://github.com/moveit/moveit_task_constructor/blob/master/core/doc/howto.md

The merger stage allows planning and executing sequences in parallel. Use this when multiple actions need to occur simultaneously.

```python
# the merger plans for two parallel execution paths
merger = core.Merger("Merger")

# first simultaneous execution
moveTo1 = stages.MoveTo("Move To Home", planner)
moveTo1.group = "hand"
moveTo1.setGoal("close")
merger.insert(moveTo1)

# second simultaneous execution
moveTo2 = stages.MoveTo("Move To Ready", planner)
moveTo2.group = "panda_arm"
moveTo2.setGoal("extended")
merger.insert(moveTo2)

# add the merger stage to the task hierarchy
task.add(merger)
```