Motion Execution in CoraPlex

Motion execution turns the actions of a plan into the motions a robot performs. The actions of a plan expand into giskard goals and tasks, all of them nodes of one statechart, and an executor runs that statechart, either in simulation or on a real robot. How the robot is actuated is up to the executor, so the same plan runs on every robot and with every executor.

Motions

A motion is a giskard goal or task. When an action expands, it creates the motions it needs from giskardpy.motion_statechart and puts them below itself in the statechart, for example a JointPositionList to move a joint, or a MoveGripper to open a gripper to a width.

Before an action creates a goal, it looks up the values that come from coraplex rather than from its own parameters: an arm’s tool frame comes from its EndEffector, and the link a Cartesian goal is expressed relative to comes from controlled_root. The mixins in coraplex.robot_plans.mixins do this for the goals several actions share.

The Plan Executors

A PlanExecutor runs a plan, the top-level nodes of a Statechart built in its context. It builds that context over its world out of the context extensions it is given, such as the RobotAccess of the robot performing the plan. compile() adds the collision avoidance goals when collision avoidance is enabled and an end motion once every plan node succeeded, and execute() runs the plan. An executor runs one statechart, so every plan gets an executor of its own.

Choosing Between Simulated and Real Execution

Which executor runs a plan decides how it is executed:

from coraplex.plans.context_extensions import RobotAccess
from cramph.context import StatechartContext
from coraplex.plans.executors import (
    RealRobotPlanExecutor,
    KinematicallySimulatedPlanExecutor,
)
from cramph.statechart import Statechart

executor = KinematicallySimulatedPlanExecutor(
    StatechartContext.with_extensions(world, [RobotAccess(robot)])
)
statechart = Statechart(context=executor.context)
statechart.add_node(plan)
executor.compile(statechart)
executor.execute()

A plan for the real robot is built and run the same way in a RealRobotPlanExecutor(StatechartContext.with_extensions(world, [RobotAccess(robot)]), ros_node=node). Code that is told how to execute at run time takes the executor type, as executor_type does. A plan does not ask what executes it: each executor adapts it with its own plan_adaptations, see below.

A plan can also be performed against a physically simulated world, which has to be started with start_stepped_simulation() already:

from coraplex.plans.executors import PhysicallySimulatedPlanExecutor

executor = PhysicallySimulatedPlanExecutor(
    StatechartContext.with_extensions(world, [RobotAccess(robot)]),
    simulation=simulation,
)

Kinematically, a joint arrives wherever it was commanded and a picked object is attached to the gripper. Physically, the commands are set points for the simulation’s servos, an object is held by friction alone, and a base cannot be put at a pose by writing it into the world.

Both executors build on cramph’s Executor. The simulated one is a StatechartExecutor that ticks the chart itself, and its MotionControl extension holds the controller configuration:

from giskardpy.motion_control import MotionControl
from giskardpy.qp.qp_controller_config import QPControllerConfig

executor = KinematicallySimulatedPlanExecutor(
    StatechartContext.with_extensions(world, [RobotAccess(robot)]),
    extensions=[MotionControl(QPControllerConfig(target_frequency=50))],
)

Collision avoidance is a setting of the executor:

executor = KinematicallySimulatedPlanExecutor(
    StatechartContext.with_extensions(world, [RobotAccess(robot)]),
    collision_avoidance=True,
)

What happens for each executor

• KinematicallySimulatedPlanExecutor: the chart is compiled and ticked against the world until every plan node succeeded. A plan that stops approaching its goal gives up and raises MotionMadeNoProgress, naming the tasks that stalled; one that keeps converging is never cut off for taking many ticks, only once it exceeds the simulated time limit.

• PhysicallySimulatedPlanExecutor: as the kinematically simulated one, but the physics advances one control period after every tick, and the robot is given time to come to a standstill once the plan ended.

• RealRobotPlanExecutor: the chart is sent to giskard via the GiskardClientExecutor, which grounds underspecified actions while giskard runs it.

• prepare() builds the chart without compiling or running it, for inspecting a plan as it would run.

Adapting a plan to its executor

Every executor offers each node of a plan to its robot_adaptations (see below), then to its plan_adaptations, and then to the transformations of the plan’s own PlanRewriting:

• KinematicallySimulatedPlanExecutor puts the base at its goal by writing the odometry (WriteTheOdometryInsteadOfCommandingAPose), unless the robot drives differentially, and keeps a Stretch in front of the container it closes. The trial of a candidate for an underspecified action writes the odometry for a differentially driven robot too (WriteTheOdometryInsteadOfDriving), see plan_adaptations.

• PhysicallySimulatedPlanExecutor closes a grasp to the object’s width (CloseFingersToObjectWidth) and lets contact carry a picked object (LetContactCarryTheBodies).

• RealRobotPlanExecutor closes a grasp to the object’s width and detects with RoboKudo (DetectWithRoboKudo).

An executor is given other adaptations as plan_adaptations=[...].

Robot-Specific Motions

Some robots need a different implementation of a motion. Such a robot gets a PlanTransformation that replaces the goal an action builds once the action is expanded, and that applies only to its robot type. Every executor applies these through its robot_adaptations, whichever executor it is:

• DriveWithTheDifferentialDrive drives the base of every robot on a differential drive with a DifferentialDriveBaseGoal, which arrives within the base_pose_threshold of the action.

• MoveStretchToolCenterPointInAStraightLine moves the tool center point of a Stretch to each of its targets along a straight line.

• KeepStretchFromTurningWhileMovingItsTool holds the heading of a Stretch’s base while its tool moves, so the base only drives along it. A target the arm does not reach from where the base faces fails rather than being reached by turning, so the plan has to stand the Stretch facing it. Opening and closing a container are left as they are.

• KeepTheTorsoUprightWhileUsingATool keeps Justin’s torso upright while it moves a tool.

A robot-specific transformation that only one way of executing needs belongs to that executor’s plan_adaptations instead, such as KeepStretchInFrontOfTheContainerItCloses for a kinematically simulated Stretch. An executor is given other robot adaptations as robot_adaptations=[...]; an empty list turns them off.

A transformation replaces a step of a control structure node with replace(); an action running several steps is the control structure node of those steps, so its steps are replaced, inserted before and after the same way. A ConfigurationTransformation instead changes an action’s settings right before it expands, so nothing it expanded into has to be replaced.

Key takeaways

• Actions build giskard goals directly; plans never execute them one at a time.

• A PlanExecutor compiles the statechart holding a plan and executes it.

• The executor’s type chooses kinematically simulated, physically simulated or real execution; collision avoidance is one of its settings.