How-To Guides

Stage Usage

Alternatives

Using the alternatives stage, you can plan for multiple execution paths. Download the full example code here: Source

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

Fallbacks

The fallbacks stage provides alternative motion planners if planning fails with the primary one. Download the full example code here: Source

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

Merger

Plan and execute sequences in parallel using the merger stage. Download the full example code here: Source

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

Connect

Connect two stages by finding a motion plan between them. The code snippet is part of the Pick and Place guide. Download the full example code here: Source

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

FixCollisionObjects

Check for collisions and resolve them if applicable. Download the full example code here: Source

# check for collisions and find corrections
fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects")

# cut off length for collision fixing
fixCollisionObjects.max_penetration = 0.01

# Add the stage to the task hierarchy
task.add(fixCollisionObjects)

GeneratePlacePose

Sample feasible poses around an object pose. Considers geometry of primitive object type. Solutions can be used for inverse kinematics calculations. The code snippet is part of the Pick and Place guide. Download the full example code here: Source

# 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

# Define the pose that the object should have after placing
placePose = objectPose
placePose.pose.position.x = -0.2
placePose.pose.position.y = -0.6
placePose.pose.position.z = 0.0

# Generate Cartesian place poses for the object
place_generator = stages.GeneratePlacePose("Generate Place Pose")
place_generator.setMonitoredStage(task["Pick"])
place_generator.object = object_name
place_generator.pose = placePose

GenerateGraspPose

Sample poses around an object pose by providing sample density angle_delta. Solutions can be used for inverse kinematics calculations. The code snippet is part of the Pick and Place guide. Download the full example code here: Source

# 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

GeneratePose

Spawn a pose on new solutions of the monitored stage. Download the full example code here: Source

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

Pick

Wraps the task pipeline to execute a pick. The code snippet is part of the Pick and Place guide. Download the full example code here: Source

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

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

Place

Wraps the task pipeline to execute a pick. The code snippet is part of the Pick and Place guide. Download the full example code here: Source

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

SimpleGrasp

Wraps the pose generation and inverse kinematics computation for the pick pipeline. The code snippet is part of the Pick and Place guide. Download the full example code here: Source

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

SimpleUnGrasp

Wraps the pose generation and inverse kinematics computation for the place pipeline. The code snippet is part of the Pick and Place guide. Download the full example code here: Source

# 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"

ModifyPlanningScene

Modify the planning scene. Download the full example code here: Source

# Specify object parameters
object_name = "grasp_object"
object_radius = 0.02

objectPose = PoseStamped()
objectPose.header.frame_id = "world"
objectPose.pose.orientation.x = 1.0
objectPose.pose.position.x = 0.30702
objectPose.pose.position.y = 0.0
objectPose.pose.position.z = 0.285

object = CollisionObject()
object.header.frame_id = "world"
object.id = object_name
sphere = SolidPrimitive()
sphere.type = sphere.SPHERE
sphere.dimensions.insert(sphere.SPHERE_RADIUS, object_radius)

object.primitives.append(sphere)
object.primitive_poses.append(objectPose.pose)
object.operation = object.ADD

modifyPlanningScene = stages.ModifyPlanningScene("modify planning scene")
modifyPlanningScene.addObject(object)
task.add(modifyPlanningScene)

FixedState

Spawn a pre-defined state. Download the full example code here: Source

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

ComputeIK

Compute the inverse kinematics of the monitored stages’ solution. Be sure to correctly configure the target_pose property to be derived from the monitored stage as shown in the example. Download the full example code here: Source

# 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
# Which end-effector frame should reach the target?
computeIK.ik_frame = PoseStamped(header=Header(frame_id="panda_link8"))
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)

MoveTo

Use planners to compute a motion plan. Download the full example code here: Source

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

MoveRelative

Move along a relative offset. Download the full example code here: Source

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

Stage Extension

MoveRelative

You may derive from this stage to extend its functionality. MoveRelative itself derives from the propagator stage that alters solutions (i.e. computes a motion plan) when they are passed through the stage.

class PyMoveRelX(stages.MoveRelative):
    """Implements a custom propagator stage."""

    def __init__(self, x, planner, name="Move ±x"):
        stages.MoveRelative.__init__(self, name, planner)
        self.group = PLANNING_GROUP
        self.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
        self.setDirection(
            Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 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)

Generator

Derive from the Generator stage to implement your own logic in the compute function.

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)

MonitoringGenerator

Derive from the MonitoringGenerator stage to implement your own logic in the compute function. Use the monitoring generator instead of the normal generator if you need to access solutions of the monitored stage (e.g. computation of inverse kinematics).

class PyMonitoringGenerator(core.MonitoringGenerator):
    """Implements a custom 'MonitoringGenerator' stage."""

    solution_multiplier = 2

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

    def reset(self):
        core.MonitoringGenerator.reset(self)
        self.upstream_solutions = list()

    def onNewSolution(self, sol):
        self.upstream_solutions.append(sol)

    def canCompute(self):
        return bool(self.upstream_solutions)

    def compute(self):
        scene = self.upstream_solutions.pop(0).end.scene
        for i in range(self.solution_multiplier):
            self.spawn(core.InterfaceState(scene), i)