Composing multi-step motion plans#459
Open
gonzalocasas wants to merge 10 commits into
Open
Conversation
Adds compas_fab.robots.MotionPlan and PlanStep — a backend-agnostic
container for assembling multi-stage motions (the canonical example is
pick-and-place: approach → contact → state change → retract → transfer
→ contact → state change → retract). The planner return types are
unchanged; MotionPlan is an additive composition layer the user (or a
GH canvas) builds on top.
JointTrajectory gains an optional start_state attribute that carries
the full RobotCellState the planner was given. Backends (MoveIt,
PyBullet, analytical Cartesian) populate it. start_configuration
becomes a property: explicit setter still works, otherwise it derives
from start_state.robot_configuration. This is a breaking ctor change
— start_configuration is no longer a constructor parameter (drop the
kwarg and assign via the property instead). New ctor signature:
(trajectory_points, joint_names, start_state, fraction, attributes).
MotionPlan key features:
* Linear chain of named PlanSteps; trajectory steps derive their
post-state automatically (last point applied), state-change steps
carry an explicit post-state.
* Fluent chaining: each append_* returns self.
* Name-based lookup (step_by_name); no index API by design, so a future
tree extension stays non-breaking.
* Optional robot_cell at construction stores a SHA-256 signature
(robot name + sorted tool/body ids); verify_cell() fails loudly on a
mismatched cell when loading.
* Serialisation strips trajectory.start_state inside the plan (the
chain owns it) and omits the derived post_state of trajectory steps
— only the plan's start_state and explicit state-change post-states
hit disk.
Bundled fixes that flow from the same work:
* MoveItPlanMotion / MoveItPlanCartesianMotion: unwrap RosValidation-
Error so typed planner errors (MPNoPlanFoundError, etc.) reach the
caller as documented, instead of being masked by the validation
wrapper. Also fixed a local-variable shadowing bug in the cartesian
branch (start_state was overwritten with a ROS RobotState message
for the request payload, then captured at the wrong type by the
response handler).
* convert_trajectory_points (MoveIt) and analytical Cartesian planner
now attach the parent trajectory's joint_names to each
JointTrajectoryPoint at construction — the ROS message spec only
carries names on the parent, so per-point name access was silently
empty.
* PyBullet plan_cartesian_motion drops the redundant
start_configuration kwarg now that start_state covers it.
Grasshopper components (CPython userobjects, COMPAS FAB / Planning):
* Cf_TrajectoryStep, Cf_StateChangeStep — wrap a JointTrajectory or a
RobotCellState into a named PlanStep next to the data it labels.
* Cf_MotionPlan — assembles a MotionPlan from an ordered list of
PlanSteps; emits the plan plus composite EE planes (DataTree, one
branch per trajectory step) and per-step polylines (list).
* Cf_DeconstructTrajectory now derives cell_states from
trajectory.start_state (the start_state input was dropped), and adds
velocities / accelerations / efforts DataTree outputs for plotting
the motion profile.
* Cf_PlanMotion / Cf_PlanCartesianMotion drop the redundant `error`
output (failures flag the component red via gh_error instead) and
emit `planes` + `polyline` outputs computed via the new local-FK
helper.
* trajectory_to_planes_and_polyline (compas_fab.ghpython) takes
robot_cell directly instead of digging through planner.client; uses
RobotCell.forward_kinematics_target_frame in-process so a 30-point
trajectory resolves in low-single-digit ms instead of a per-point
rosbridge round trip. Polyline is built in compas and converted via
polyline_to_rhino.
Docs:
* New "Assembling multi-stage motions" section in docs/concepts.md.
* New docs/developer/motion_plan.md — records what was deferred
(tree / branching), the three forward-compat hooks the current API
provides (iteration via methods, name-based lookup, alternatives
sibling key on serialization), and open questions for vNext.
* Runnable end-to-end examples for both PyBullet and ROS at
docs/backends/{pybullet,ros}/files/08_motion_plan_pick_and_place.py
with signposts from the respective backend pages.
Tests: 12 new motion-plan tests + 2 new trajectory tests
(test_start_configuration_derived_from_start_state,
test_start_state_roundtrips_through_serialization). 85/85 robots
tests pass, ruff clean, mkdocs --strict clean.
Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
Member
Author
|
Here's a preview of the docs for the concepts part: |
Member
Author
|
And this are the grasshopper components: |
… scrubbing
Adds MotionPlan.iter_cell_states(), a generator that yields one
RobotCellState per trajectory point + one per state-change step across
the entire realized path. Snapshots are independent copies (mutating
one does not affect the plan or other snapshots).
Cf_MotionPlan now exposes a `cell_states` output (flat list, slotted
between `polyline` and `duration`). The intended wiring is:
cell_states -> List Item <- index slider -> VisualizeRobotCell
so a single slider scrubs the whole pick-and-place end-to-end, not
just one trajectory. State-change steps contribute one frame each,
representing the instant of the change (gripper closed, tool
attached, etc.).
Tests: 2 new (covers every point + state-change is included, empty
plan yields nothing). 87/87 robots tests pass, ruff clean.
Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
Collapse the planned/unplanned PlanStep split into a single Action class wrapping start_state + optional trajectory + optional explicit post_state. A trajectory makes it planned (post-state derived); no trajectory makes it an unplanned/state-change action (post-state explicit). Single-class model means backtracking just clears the trajectory instead of swapping types. Action carries a free-form attributes bag with tags as the default, first-class key to drive differentiated downstream execution. MotionPlan -> ActionChain: same state-threading and serialization optimization (strip the duplicated start_state, re-thread on load), now mirroring Action.start_state onto the contained trajectory.start_state. Surface renamed: actions / iter_actions / action_by_name / append_action. MotionPlan/PlanStep were unreleased, so this is a clean rename with no back-compat aliases. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Cf_TrajectoryStep -> Cf_TrajectoryAction, Cf_StateChangeStep -> Cf_StateChangeAction (both now emit an Action and accept an optional comma-separated `tags` input that lands on the action's attributes), Cf_MotionPlan -> Cf_ActionChain (takes `actions`, emits `chain`). Component code.py/metadata.json updated to the Action/ActionChain API; icons carried over unchanged. Userobject regeneration is a separate step. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Rename developer/motion_plan.md -> developer/action.md and rewrite it;
update concepts.md, developer/index.md, backends/{ros,pybullet}.md, the
two 08_*pick_and_place example scripts, the icon-system catalog labels,
and mkdocs nav. Fold the rename + new tags feature into the existing
Unreleased CHANGELOG bullets.
Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
… into prep-release-motion-plan-wow
PR #459 targets the prep-release integration branch, not main, so the build workflow (and its build-cpython-components artifact job) was not running. Add prep-release to the push/pull_request branch filters so the Grasshopper userobjects are built and uploaded as an artifact for this PR. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Finish the Action rename in the icon catalog (glyph keys trajectoryStep -> trajectoryAction, stateChangeStep -> stateChangeAction, motionPlan -> actionChain). Redesign the "from library" glyphs around a stack-of-books motif (cell/tool/body), add a dedicated toolFromMesh glyph, and re-render the affected component icon.png files. Bump the catalog count to 37. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
Contributor
|
I guess we can considered it reviewed with you over zoom today. |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This PR implements a feature that has been long-discussed and -to a large extent- formalized in @yck011522 PhD thesis, namely, the data container for the sequence of trajectories and state changes that produces a full fabrication process. The ideal implementation of this will capture the concept of "unplanned step" which a backtracking planner can traverse and plan until it finds a "realized plan" containing the fully planned trajectories for each step.
As per Victor's sketch, initial state + a chain of transformations (trajectories / planner configs / etc) produce a new state:
This PR is modest in its ambition in one fundamental way: it does not YET implement the idea of "UnplannedStep" and backtracking. However, it definitely lays the foundation for this: the class
PlanStepcurrently only supports 'realized' steps (ie. planned, or fully concrete), but in a next iteration, a new sub-class can beUnrealizedPlanStepwhich is the trigger for the backtracking planner to plan and replace.There's another important contribution of this PR: de-bloating the serialization output. Since the
MotionPlancontains enough information, it can stripstart_statefrom trajectories that would be duplicated. I considered more sophisticated methods for data compaction, like delta encoders, but they would only marginally improve at the cost of high complexity.Lastly, this PR contains the new GH components that would allow for simple composition of these new
MotionPlans, and a couple of full end-to-end examples using Pybullet and ROS respectively.What type of change is this?
Checklist
Put an
xin the boxes that apply. You can also fill these out after creating the PR. If you're unsure about any of them, don't hesitate to ask. We're here to help! This is simply a reminder of what we are going to look for before merging your code.CHANGELOG.mdfile in theUnreleasedsection under the most fitting heading (e.g.Added,Changed,Removed).invoke test).invoke lint).compas_fab.robots.CollisionMesh.