Repository navigation
Merge OpenArm VR teleop and supermarket sim into dev - #2
Merged
Merged
Conversation
Teleoperation stuck mid-motion at intervals and re-gripping did not recover it. The cause was not the solver. Every delta was gated on the headset's clock-corrected stamp by three checks that returned without a word, and the headset's offset stepped backwards every 30 s by construction: ClockSync threw its window away on a timer and re-estimated from the next single sample, which can only be larger, so stamps jumped earlier by that sample's jitter. Past the 0.25 s timeout every delta was dropped until the next window, the watchdog held the arm, and the age gate survived a re-grip while the alignment check refused to re-engage against a pose the same jump was rejecting. Liveness is now arrival time on this machine's steady clock, and stamps are kept only for recording. On the headset the estimate is a true sliding-window minimum slewed at 5 ms/s, so it no longer steps; corrections over a second still snap, because a suspend can leave it 158 s out and slewing that away would take hours. The per-tick snapshot IK is replaced by a resolved-rate servo. It owns the commanded configuration, seeded from the measured joints at grip press, and integrates it down the pose error: the Jacobian from KDL's ChainJntToJacSolver, the joint velocity from a damped least-squares inverse damped only near a singularity (Nakamura and Hanafusa 1986; Chiaverini, Oriolo and Walker 1994), and the spare seventh joint spent pulling the arm to mid-range through the null space (Liegeois 1977). One IK branch per press, velocity and joint bounds by construction, and the 0.10 rad clamp and the two-seed two-solver fallback are gone. Worst-case cost per tick falls from 3.3 ms to 0.009 ms and stops depending on where the target is. Beyond its reach the arm tracks the closest pose it can hold instead of freezing, and reports unreachable only once the gap has stopped closing for half a second. Joint speed goes from 0.35 to 1.5 rad/s: 20 deg/s was slower than any hand. Engaging now asks the operator to bring the controller to the gripper in position as well as orientation. The controller's position was being discarded; it is captured and checked against openarm.alignment_tolerance_m. The status topic carries vive_vr_ros2/TeleopStatus instead of a string: the state word, a ready flag, both errors and the tolerances they are judged against. Sending the tolerances keeps the headset from holding a second copy that could disagree with the gate. VrTeleopIndicator tints the gripper red through amber to green, green only from the PC's ready flag. The state align_orientation becomes align_pose. Control tuning lives in config/vive_vr.yaml rather than only in C++ defaults. prepare stops deleting every URDF visual and setting discardvisual, which had left the arm drawn as bare collision hulls with nothing to texture. Measured rather than assumed: the importer puts visuals in group 1 and collisions in group 0, so the export takes group 1 alone or the hulls would cover the meshes. fix_mesh_references now covers visuals too, since a bimanual description mirrors one arm from the other's meshes. Verified by colcon test: 5 tests, 0 failures. Both C++ checks run against a new seven-joint fixture carrying the OpenArm names, so they need no OpenArm description; OPENARM_TEST_MODEL still adds a run against the real robot. The bridge check covers the freeze directly, asserting that stamps seconds old and stamps stepping backwards are still followed, and that a controller half a metre from the gripper does not engage. Not verified: the real OpenArm model and anything in a headset, neither being available here.
- Separate real and simulated launch paths while sharing the existing IK - Bridge VR targets to ROS controllers using live motor feedback - Add CAN pacing, feedback validation, fault handling, and diagnostics - Tune physical teleoperation for one-to-one hand translation - Add guarded return-to-zero, cancellation, and position-status commands - Handle small elbow and gripper zero offsets without recalibration - Add regression tests and hardware setup documentation
- Move real-arm engagement from side grip to rear trigger - Reset progress tracking when the end-effector target moves - Hide intrusive recovery panels for advisory reach warnings - Update headset instructions and regression tests
The store is generated from the Dual-Arm Supermarket shelving and scanned products (pinned upstream commit), fully stocked, and enclosed with walls, ceiling panels, chilled cabinets, produce and checkouts. TIAGo comes from the Menagerie with its arm tucked through joint refs. scene_export now writes each mesh asset once per material and places every use as a child instance node; the store's glb drops from ~2 GB to 119 MB, and scenes without repeats export as before. SceneNode takes actuator commands on ~/ctrl for the actuators named in ctrl_actuators, zeroed after ctrl_timeout_s of silence; sim.launch.py passes ctrl_actuators through. tiago_patrol.py follows the world's patrol.yaml with pure pursuit on the wheels.
0.4.0 is a breaking release: robots and loops go through an Env, and ensure_plugins_loaded() and a Robot's index maps are internal. SceneNode loads its world through Env::adopt inside init_env, which loads the plugins first, and frees the adopted pair itself. OpenArmIk builds its chain in a private Env running on a copy of the model and reads the qpos, dof and actuator indices from the model directly, with the same actuator lookup 0.3 used. find_package asks for 0.4, as the package now matches within a minor version only; dependencies.repos pins v0.4.0, and the OpenArm launcher and docs name the MuJoCo 3.14.0 library.
A gripper closed on an object stops moving while the bridge's command kept closing, passing the 8 mm following check within ~0.5 s and disarming both arms. The command now leads the measured finger position by at most 4 mm, so a stalled grip holds a bounded squeeze; joint-limit checks are unchanged. on_activate re-arms the first-read fresh-reply wait and the first-write dt allowance, which previously applied to the first activation only. With motion bounds off, a held first delta reported translation_limit or rotation_limit; the reason now comes from the check that fired. PollWorldLock set the label colour before its null check; UpdateLockLabel already sets it, guarded. The hardware doc gave an 8 ms CAN batch limit; the code uses 30 ms.
Add dual-arm teleoperation and VR hand guidance
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
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.
OpenArm VR teleoperation (simulation and guarded hardware) by @lightonray, plus the supermarket scene, exporter instancing and mj_kdl_wrapper 0.4.0 migration. Reviewed in #1.