Repository navigation
Conversation
The arm is a base yaw, three parallel pitch joints and a roll through the tool tip, so kinematics.py solves any tip position and tool tilt in closed form from the URDF. The new Cartesian group (Y) moves the tip along straight lines in base_link with the tilt fixed; LB keeps only the strongest axis. At a joint speed limit or the workspace edge the whole step shrinks, so the tip stops on the line, and the roll turns against the yaw to keep the tool's heading. Each drive now gets the next setpoint one period ahead and the speed that reaches it in exactly one period, measured from its encoder, so all drives arrive together. acc is 255 because a gentle drive ramp adds the same rpm/s to every drive and the one with the largest change falls behind. In simulation a sideways cut at 30:1 gearing stays within 7-17 um of the line, compared with up to 186 um with the old 1.5x speed rule. At the 1:1 placeholder gearing it stays within about 0.13 mm, one encoder count at the tip.
D-pad left/right changes the tool pitch while the tip stays in place, so the shoulder, elbow and wrist move around a fixed point. The tilt uses the same closed-form step as straight moves, so a joint limit stops it with the tip still in place. It runs at up to 30 deg/s, scaled by the speed level.
- Start is refused while a tilt button or A is held, like the sticks. - Homing clears the Cartesian rates, so releasing A can't resume the last move. - Cartesian roll ramps with the acceleration limit and scales with the accepted step. - The drives' setpoint one period ahead stays within the URDF joint limits, which the drive node now reads from robot_description. - If one drive would pass max_rpm, every drive slows by the same factor, so they still arrive together. - The roll compensation is documented as stopping the tool spinning about its own axis; that keeps a blade's heading only when the tool points straight down. - The Cartesian launch test is registered where it won't conflict with #22.
The tests could press Start before the teleop node had loaded the robot description or received controller input, so the press was lost on a busy machine. They now send idle packets until teleop reports its group and fresh input. Same fix as on #28.
A second robot_description replaced an enabled teleop with a disabled one without sending a hold, so the drives kept their last target.
|
Important
This repository does not receive automatic reviews because it has fewer than 10 stars. ⚙️ Run configuration
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
Hamnah228
changed the title
yassinsolim
left a comment
yassinsolim
left a comment
There was a problem hiding this comment.
Thanks, Hamnah. drive_health.py and its boundary tests are solid, and all 785 tests pass. The problem is where it's wired: the monitor runs on the older waybionic_control host and its mock drives, which the arm doesn't use. The arm's drives run through sim_arm_drives_node in waybionic_teleop, both in simulation and on the real CAN bus.
- Evaluate each MKS drive in
sim_arm_drives_nodewithDriveHealthMonitor, from the commanded target, the encoder replies and the reply age. Move the monitor intowaybionic_teleopor import it. - Add the state, the target-versus-encoder error and the reply age to each
drive.<name>row, following the diagnostics contract: STALE when a drive isn't responding, ERROR for the other faults. - Keep your thresholds as parameters in
arm_drives.yaml, marked provisional: 5° following error, under 1° of movement in 2 s for a stall, and a 0.5 s reply timeout. - Add test-only faults to
SimulatedServo(stalled, not responding and disabled), and test each state without ROS, then with the drive node. - Update the README steps to use
ground_station.launch.py teleop:=true.
Keep the controller.link states in joy_udp_receiver; those are good. Once #27 is in main, rebase onto main and retarget this PR.
|
Hamnah, I merged main into this branch and retargeted the PR to main, so it now shows only your 11 files. Nothing was rewritten, so pull before you push again.
The full suite passes: 1055 tests. |
yassinsolim
left a comment
yassinsolim
left a comment
There was a problem hiding this comment.
Thanks, Hamnah. This is what the task asked for. The monitor runs in sim_arm_drives_node, and each drive.<name> row now shows the state, the error and the reply age. The thresholds are in arm_drives.yaml, and the simulated faults are tested both with and without ROS. CI passes. Three things before it merges:
- The error is measured at the motor, not the joint.
to_counts()multiplies by each drive'sgear_ratio, so(target - count) * 360 / COUNTS_PER_REVis the motor shaft's error. Every ratio is 1.0 today, so the numbers look right. With a 50:1 gearbox, though, a 0.1° lag at the joint is 5° at the motor, and every moving drive would report FOLLOWING_ERROR. Divide bydrive.gear_ratioboth for the positions you pass to the monitor and fortarget_encoder_error_deg. Add a test with a ratio other than 1. - DISABLED only works in simulation.
report()readsenabledfrom the simulated servo, which the host can't see on the real bus, so on the real arm it's always True. Use what the host knows instead: a drive is enabled once it has confirmed its ENABLE setup frame (mks_can.ENABLE not in self.unconfirmed[index]). A real drive that ignores the enable will then show as STALLED or fail its move, and the node already stops on a failed move. Keep thedisabledfault so the tests cover that path. - Merge main once #45 is in. It's merging today, and both PRs change
sim_arm_drives_node.py. Ask me if the merge conflicts.
|
Hamnah, #45 is merged now, so |
|
@Hamnah228 this PR now has merge conflicts with Merging git fetch origin
git checkout feature/drive-health-diagnostics-hamnah
git merge origin/main
files="BuildInstructions.md waybionic_bringup/CMakeLists.txt
waybionic_bringup/launch/ground_station.launch.py waybionic_bringup/scripts/camera_follower.py
waybionic_teleop/setup.py waybionic_teleop/test/test_gamepad.py waybionic_teleop/test/test_mks_can.py
waybionic_teleop/waybionic_teleop/gamepad.py waybionic_teleop/waybionic_teleop/mks_can.py
waybionic_teleop/waybionic_teleop/xinput_bridge.py"
git checkout --theirs -- $files
git add -- $filesThe other five need both sides kept:
Then run the full suite ( |
2 participants
What changed
DriveHealthMonitorwith the activeSimArmDrivespath inwaybionic_teleop, used by the arm in simulation and with the real MKS CAN drives.drive.<name>diagnostics row:OKFOLLOWING_ERRORSTALLEDNOT_RESPONDINGDISABLEDarm_drives.yaml:SimulatedServofor:controller.linkdiagnostics andINPUT_STALEstate injoy_udp_receiver.ground_station.launch.py teleop:=true.Why
The previous implementation evaluated health through the older
waybionic_controlhost/mock-drive path, which the arm no longer uses.This update moves the health monitoring onto the current arm-drive path so drive faults are detected from the same commanded targets, encoder feedback, and reply timing used by the simulated and real MKS drives.
This makes stalled, unresponsive, disabled, and off-target drives visible directly through the existing ROS diagnostics panel.
Diagnostics behavior
Each evaluated
drive.<name>row reports:stateDiagnostic levels follow the project diagnostics contract:
OK→OKNOT_RESPONDING→STALEFOLLOWING_ERROR→ERRORSTALLED→ERRORDISABLED→ERRORExisting setup/protocol fault precedence is preserved.
Validation
sim_arm_drives_nodetests: 52 passedOKduring normal simulated operation.OKafter clearing.git diff --checkpassed.controller.linkbehavior remains covered by its existing tests.Recorded sessions
New mock/simulated diagnostic sessions were recorded through the corrected
SimArmDrives/waybionic_teleoppath for:FOLLOWING_ERRORSTALLEDNOT_RESPONDINGDISABLEDRecording artifact:
task7-arm-drive-health-recordings-2026-10-17 (2).zip
Each recording was verified with
ros2 bag info.Limitations / notes
DISABLEDis validated usingSimulatedServo.arm_drives.yaml.controller.linkremains injoy_udp_receiverand was not changed as part of the drive-health rework.