Feat/13338 space satellite pnp cal#769
Draft
danwahl wants to merge 4 commits into
Draft
Conversation
…scene camera cal Rewire the scene-camera calibration leaf to GetImage + GetCameraInfo + CalibrateCameraPosePnp (single joint PnP over all tag corners), replacing the DetectAprilTags + per-tag GetDetectionPose + CalibrateCameraPose path. Replace the decorative calibration_tag_right/left/bottom links with TF-only calibration_tag_0/1/2 frames that reproduce the MuJoCo-rendered AprilTag poses exactly (via a calibration_tags_root matching MuJoCo's calibration_tool body), so the behavior's per-id TF lookups resolve to what the camera actually sees.
…or PnP Part B's tag frames added a spurious Rz(-90) parent rotation (calibration_tags_root) that displaced the tag positions from where they render in MuJoCo. CalibrateCameraPosePnp uses tag corners (orientation- and position-sensitive), so this made it drift to a different phantom camera pose at each waypoint, whereas the old center-only CalibrateCameraPose tolerated it. Parent calibration_tag_0/1/2 directly to calibration_tool_link at the proven tag-center offsets (same values the original centers-based calibration used); rpy="3.14159 0 0" keeps +Z out of the tag face for the corner PnP solve. Verified in sim: recovered optical pose is now consistent across all waypoints and the saved mount matches the URDF scene camera mount within noise.
…PnP calibration Add a "Calibrate Wrist Camera" objective that calibrates the wrist-mounted camera against three static satellite AprilTags, reusing the same CalibrateCameraPosePnp subtree as the scene-camera (eye-to-hand) calibration. Only the frames, topics, tag prefix, and waypoints differ. - Scene/URDF: three tag36h11 markers (ids 3/4/5, distinct from the tool plate 0/1/2) as angled plates on the drum top, plus a wrist_camera_mount_link frame on the flange to receive the calibrated extrinsics. - Objectives: thread tag_frame_prefix, mounting_pose_parent_frame, and waypoint_N_name through the shared subtrees (defaults preserve the scene-camera path unchanged); new thin calibrate_wrist_camera.xml wires the wrist ports. - Per-waypoint fix: express each waypoint's optical pose in the mount-parent frame before averaging. For a moving (eye-in-hand) camera the optical pose in base_link differs per waypoint, so averaging it directly biased the result; the mount-parent frame is rigid to the camera, so the cross-waypoint average is unbiased. This is a no-op for the eye-to-hand case (parent == base_link). Verified in sim: recovers the declared wrist mount [0, -0.056, -0.058] Ry180 to sub-millimeter, with per-waypoint PnP reprojection RMS ~0.38 px.
…ags + diverse waypoints Mount the three satellite AprilTags flat and coplanar on the servicer's front (+x) gold face, on the arm-base side of the docking mechanism, replacing the tilted drum-top placement (scene.xml geoms + matching sat_tag_* URDF frames; Ry90 so +Z exits the tag face). Regenerate the four wrist-camera waypoints for diverse, collision-free views that keep all three tags clear of the end effector. Sim-verified: every waypoint detects all 3 tags and the recovered wrist mount matches the URDF to ~0.6 mm / 0.02 deg.
|
Important Review skippedDraft detected. Please check the settings in the CodeRabbit UI or the ⚙️ Run configurationConfiguration used: Organization UI Review profile: CHILL Plan: Pro Plus Run ID: You can disable this status message by setting the Use the checkbox below for a quick retry:
Comment |
|
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.
https://github.com/PickNikRobotics/moveit_pro/pull/20390