search

ycheng517 / ar4_ros_driver

ROS 2 driver for the AR4 robot arm
MIT License
45 stars 17 forks source link

AR4-MK3 calibration very far off #15

Closed navdotnetreqs closed 2 months ago

navdotnetreqs commented 2 months ago

Hi and thank you for the project!

I made a fresh Ubuntu installation just to test this out. After issues with locales (I disagree with the ros implementation of reading setups depending on user locale, but I digress..) I got things up and running! I even tested that I can move the arm from Rviz, nice.

The calibration is totally off, though. Axis 1-3 seem ok, but 4-6 are whacked. Here are initial calibration results using zero offsets with Chris Annin's "AR4-MK3 HMI interface 5.0" (left) and then using ar4_ros_driver (right):

ros2_ar4mk3-calib

Any way I could help narrow this down? Video of the calibration, logs? How is fine calibration done after one gets this part roughly right?

navdotnetreqs commented 2 months ago

https://github.com/user-attachments/assets/275da9ba-3458-444c-b4c3-2b1420d2b1fb

ycheng517 commented 2 months ago

Hi @navdotnetreqs thanks for reporting the issue. It looks like the angle limits in the firmware needs to be updated for MK3, as joints 4 and 6 have different angle limits than the MK2. I've pushed a new Arduino sketch for the MK3 (AR4_MK3.ino) here: https://github.com/ycheng517/ar4_ros_driver/tree/feature/mk3-calibration-fixes. Please try it out and let me know if it works better for you.

Apologies for the issue as I don't have MK3 so can't test with it.

navdotnetreqs commented 2 months ago

Thank you @ycheng517 ! I'm very glad to able to help out with testing on the MK3.

Good news, the calibration is much better with these changes! Quite close to the initial calibration with Annin's software - J3 and J5 are drooping a little bit.

image

I can probably get J5 to a better angle by moving the limit-switch a little. I will try tuning the calibration using the joint_offsets-file, as mentioned in #3

ycheng517 commented 2 months ago

Glad to hear! Yeah tuning the joint offsets in https://github.com/ycheng517/ar4_ros_driver/blob/main/ar_hardware_interface/config/joint_offsets/ar4_mk3.yaml is what I would do. Note that the joint offsets are at the driver level and not firmware level, so after calibration the robot will still appear unaligned, but then when you command the robot to move to origin (0 deg for every joint) in MoveIt, it should align much better. Let me know how that works out for you.

navdotnetreqs commented 2 months ago

Hi @ycheng517 ,

have not able to adjust the calibrations so far. To be clear, I should run MoveIt with ros2 launch ar_moveit_config ar_moveit.launch.py ar_model:=ar4_mk3 ?

Changes I make to ar4_mk3.yaml don't seem to make a difference. I run the robot to a random position, and then back to the predefined location "home" (joint values 0 deg).

Running MoveIt gives the following errors/warnings:

[move_group-1] [WARN] [1723487494.257508636] [move_group.move_group]: Falling back to using the the move_group node namespace (deprecated behavior).
[move_group-1] Error:   Semantic description is not specified for the same robot as the URDF
[move_group-1]          at line 664 in ./src/model.cpp

[rviz2-2] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-2]          at line 321 in /opt/ros/iron/include/class_loader/class_loader/class_loader_core.hpp
[move_group-1] [WARN] [1723487494.852260235] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-1] [ERROR] [1723487494.852274723] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
 [rviz2-2] [ERROR] [1723487497.772272871] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-2] [INFO] [1723487497.787573996] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-2] Error:   Semantic description is not specified for the same robot as the URDF
[rviz2-2]          at line 664 in ./src/model.cpp
[move_group-1] [WARN] [1723487511.004732992] [moveit.ros_planning.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
ycheng517 commented 2 months ago

You need to specify ar_model:=ar4_mk3 in both the ar_moveit.launch.py command and the ar_hardware.launch.py commands. In fact, it's the ar_hardware_interface which is launched by ar_hardware.launch.py that reads the joint offsets. Hope that's the missing piece.

Regarding the warning logs:

navdotnetreqs commented 2 months ago

Hi again!

Unfortunately adjusting the ar4_mk3.yaml (or even the ar4.yaml) files had no effect. I tried making large changes, but they were not reflected in calibration, moveit starting pose or robot behaviour (as in telling it to go to zero-values for joints).

I modified the const int REST_ENC_POSITIONS[] in AR4_MK3.ino, and with this I can get the joints aligned nicely after calibration. However, this makes the pose in MoveIt/RViz wrong. Eg. I lift J5 up by changing the rest position to get it to horizontal -> in RViz it's pointing slightly up.

I have no previous experience with RViz/MoveIt/ROS2 so I haven't yet figured out exactly how the pose is in alignment with the values in the Teensy code.

For instance, I don't fully understand what function "JC" (joint calibration) in the Teensy code (AR4_MK3) outputs. Eg. JC A 76119 B -23230 C 28757 D -89498 E -11242 F 39897

Is it the encoder positions at the limits switches? Since the encoder positions are reset to constant values

encPos[i].write(ENC_RANGE_STEPS[i] * ENC_MAX_AT_ANGLE_MIN[i]);

after collecting those values, doesn't it mean whatever relevance the encoder position values at the limit switches had, is now lost?

navdotnetreqs commented 2 months ago

Hi @ycheng517 , I noticed that the joint limits defined in ar.ros2_control.xacro don't match the ones in AR4_MK3.ino - they seem to be the values for the MK2. Does this make a difference? Could that stop the offsets configs from working properly?

Eg. 

<joint name="joint_6">
        <command_interface name="position">
          <param name="min">{**-155***pi/180}</param>
          <param name="max">{**155***pi/180}</param>
        </command_interface>
        <state_interface name="position">
          <param name="initial_value">0.0</param>
        </state_interface>
        <param name="position_offset">${joint_offset_parameters['joint_6']}</param>
      </joint>
ycheng517 commented 2 months ago

I noticed that the joint limits defined in ar.ros2_control.xacro don't match the ones in AR4_MK3.ino - they seem to be the values for the MK2. Does this make a difference? Could that stop the offsets configs from working properly?

Nice find, I will fix that. That should prevent you from moving beyond that limit for a given joint, it shouldn't matter for joint offsets though which is the problem you're having.

I don't fully understand what function "JC" (joint calibration) in the Teensy code (AR4_MK3) outputs.

That output is currently unused so don't worry about it, it could be used in the future for saving calibrations. How calibration works is: after each joint reaches the limit switch, the encoder position is reset to ENC_RANGE_STEPS[i] * ENC_MAX_AT_ANGLE_MIN[i] which should either be the max angle limit or min angle limit in the number of encoder steps. For example, the limit switch position for J6 is recognized as +180 degrees.

REST_ENC_POSITIONS shouldn't affect angle offsets between firmware and the driver. It'll only change the starting angle position in RViz. Keep in mind that firmware outputs angles and not encoder count to the driver when the driver polls for joint position (with the command JP\n)

Adjusting ar4_mk3.yaml having no effect is definitely the issue, I won't be able to check it on my AR4 MK1 until the weekend though. If you have time to debug, you can add some print statements around this line to see what joint offset gets actually used: https://github.com/ycheng517/ar4_ros_driver/blob/main/ar_hardware_interface/src/ar_hardware_interface.cpp#L99

Oh one final important note: after changing the yaml file (or making any change) you need to re-build the source code (by doing colcon build)

ycheng517 commented 2 months ago

Semantic description is not specified for the same robot as the URDF is fixed in the latest main branch by daf9a969a9e100e70d6c07b977e1349d4140c2d8 . Thanks for reporting the issue!

navdotnetreqs commented 2 months ago

Hi @ycheng517 Thank you for your posts and fixes! I'm glad I could help by finding a few minor things.

I was unaware that I had to do a rebuild after changing the joints_offsets! I can confirm that after a rebuild the offset calibration is working! I will modify the rest position in the teensy code, so the initial (post-calibration) position aligns with the one in RViz/Moveit.

Every now and then the calibration somehow fails, one or more joints don't reach the goal and the teensy gets stuck in a loop. I'll open a new issue for this.