Tool mounting - #472
Open
gonzalocasas wants to merge 7 commits into
Open
Conversation
The tools of the ToolLibrary were modelled reaching along their own +X, following REP 199, while every planning group of the RobotCellLibrary ends at a link whose +Z points away from the arm — `tool0` on the industrial robots, `panda_hand_tcp` on the Panda. The two conventions disagreed, so each cell carried the same rotation in its attachment frame to bridge them, and a tool authored against one robot did not necessarily fit another. Adopt the z+ convention for our tools, so a tool mounts on any robot of the library with an identity attachment frame. The tools are still modelled along +X internally and re-framed on the way out through `ToolModel.reframe_base`, which keeps the change to one line per tool and leaves the geometry untouched. The TCF follows the same convention: its Z axis states the direction the tool works in, so a `TargetMode.TOOL` target aligns the tool along the target's Z. Re-framing alone would have left the TCF's X axis running along the tool, which mounts the tool correctly but sends every tool-mode target off at 90 degrees. The printing tool is the exception, its nozzle points across the mounting axis, so its TCF is carried along with the geometry to keep pointing out of the nozzle. The beams the gripper cells hold are authored in TCF coordinates, so they were re-authored to match the rotated TCF and stay where they were. Nothing moves: the attached tools, their TCP positions and the grasped beams were compared against main and land in exactly the same place, down to the vertex. What changes is the orientation of the tool's base frame and of its TCF, which is the point. Note the `flange` link that some robot models ship is the opposite convention (REP 199 puts x+ away from the last link). Since only some models have one, picking it as the end effector link would leave the library with two mounting conventions depending on the robot, which is exactly what this commit removes; `get_end_effector_link_name` documents that. Requires the `reframe_base` support of the next compas_robots release. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
`MoveItInverseKinematics` sent the target pose without an `ik_link_name`, letting MoveIt interpret it at the default tip link of the planning group. That tip is the end effector link compas_fab computes the Planner Coordinate Frame at for every robot in the library, but nothing guarantees it for a group configured with a different tip, and the failure is silent: the IK result comes back rotated. Add the field to `PositionIKRequest` and always state the link, defaulting to the group's end effector link and overridable with the new `"link"` option, mirroring `plan_cartesian_motion`. Verified against a live ROS 2 MoveIt stack: IK round-trips through model FK at the requested link. Also use `get_end_effector_link_name` instead of `get_link_names(group)[-1]` in the PyBullet cell state feature, same result, explicit intent. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
A tool built from a Rhino mesh had to be modelled in the tool's own base frame, because that frame was hardcoded to the origin: the mesh coordinates *were* the base frame. Draw a gripper reaching along X and it mounts sideways, with nothing to say so. Add a `base_plane` input to `Tool From Mesh`: the plane, in the coordinates you modelled in, at which the robot's flange takes hold of the geometry. Its Z axis points away from the robot, matching the tools of the ToolLibrary, so a tool drawn reaching along world Z needs no plane at all and one drawn along another axis is mounted by wiring a plane rather than redrawing. Backed by `base_frame` of compas_robots' ToolModel, which folds it into the link origins instead of baking it into the mesh, so it can be re-wired at any time. It is only passed when wired, so an older compas_robots keeps working for definitions that don't use it. The component also remarks when the TCP does not sit roughly on the tool's +Z, since that means the tool will point sideways once attached. The direction from the mount to the TCP is only a hint — it says nothing about the roll of the tool, and a gripper with a lateral TCP offset would come out subtly tilted rather than obviously wrong — so it is reported rather than applied. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
The UR solvers' DH chain ends at the robot's flange, whose x+ points away from the last link, while the planning group ends at `tool0`, whose z+ does. The two are a fixed rotation apart, so the analytical planner and the URDF model disagreed about the orientation of the end effector: a target handed to the planner was interpreted rotated, and the robot it drew never reached the frame it had been asked for. Tool and workpiece modes compounded it, since the tool offset was then applied along the wrong axis. `AnalyticalKinematics` already documented `flange_frame` as the way to state that offset — "should match the end effector of the robot in the URDF model" — but nothing ever applied it, and no solver set one. Apply it in the analytical forward and inverse kinematics, and declare it once on `OffsetWristKinematics`, which covers the whole UR family. The spherical wrist solvers need no correction, their orientation already matches. Analytical FK now agrees with the robot model to 1e-10, and matches the zero-configuration frame the PyBullet tests have always expected, which it never did before. A TargetMode.TOOL round trip reaches its target exactly. The three tests that broke had the uncorrected frame baked into them. The forward kinematics expectation is updated and cross-checked against the robot model; the two inverse kinematics tests now assert that each solution actually reaches the target, read back through the model, rather than pinning joint values that encode a convention. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
…s, solvers and tool definitions to test tool mounting
Member
Author
|
You can review all of the generate cell snapshots (in GLB/GLTF format) if you click on the |
Member
Author
|
/cc @jf--- |
Member
Author
|
Obivously, the dependency to |
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
1 participant
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.
Following up on compas-dev/compas_robots#33, this PR uses the newly added
base_framefor tool models, to correctly mount tools onto robot models. I've also introduced one extra param to the analytical solvers, to allow them to match urdf definition of mount points.Since the test matrix is large (multiple backends, multiple robot cells, at least two conventions for defining tools either along X+ or along Z+), I added a script and corresponding github action, that will ran them all, and generate GLTF files for the resulting cells with a tool mounted on them. This basically verifies that the change is consistent across: pybullet/analytical solver/ros - tool x+/tool z+ - URs/ABBs robots
Here's a screenshot of the results of this script for the UR5+ROS, with two tool orientations (X+ and Z+), both of them are identically mounted and that's a good thing!
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.