Created kinematics module exploiting DH frames: forward/inverse kinematics, Jacobians, pose interpolator #1001
Conversation
Added README file explaining kinematics module.
Added kinematics module, PDF exaplining DH frames and image
Edited README
Edited README
for more information, see https://pre-commit.ci
lerobot/common/kinematics/README.md
Outdated
| $$ | ||
|
|
||
| ### Note | ||
| The only remaining step is to **compute the corresponding DH table** (see image) given the real robots. Since I do not own SO100/koch/moss robots, I hope LeRobot Team can measure the corresponding parameters and provide the DH table. Happy to anwer any further question, I also attach the PDF "robot kinematics" for more details about DH tables. |
There was a problem hiding this comment.
For SO100, those measurements should be available in the CAD files in the STEP folder:
https://github.com/TheRobotStudio/SO-ARM100/tree/main/STEP
It will be more accurate than manually measuring the distances on the physical 3D-printed files.
Also would a simulator supporting URDF / MJCF format help to derive the DH table parameters ?
- mujoco
- maniskill
- genesis
- etc.
Here's the Mujoco config: https://github.com/google-deepmind/mujoco_menagerie/tree/main/trs_so_arm100/
Here's the URDF config: https://github.com/TheRobotStudio/SO-ARM100/blob/main/URDF/SO_5DOF_ARM100_8j_URDF.SLDASM/urdf/SO_5DOF_ARM100_8j_URDF.SLDASM.urdf
There was a problem hiding this comment.
Thanks for the resources, I'll have a look over the next days and I'll be back!
There was a problem hiding this comment.
Hey @alexis779 and @imstevenpmwork ! Here my updates:
-
I computed the SO100 Denavit-Hartenberg frames and table using precise measurements from the CAD model.
-
Updated the code with the corrected SO100 DH table.
-
Correctly tested the forward/inverse kinematics with the new DH convention.
-
Updated the README with images showing the DH frames and the DH table for SO100.
Note: the angles used in the code are DH angles, not direct motor angles — they depend on how the robot is assembled. As a final step, we could either update the assembly tutorial to match the DH conventions or explore methods to implement motors2DH and DH2motors conversions.
That said, the updated DH table can already be tested on the real robot! Please refer to the updated README.md for the correct angle conventions.
Let me know if you have any feedback — happy to chat about how to tackle that final step!
There was a problem hiding this comment.
I have a SO100 and I can test your script. I'll comment with the results of my QA.
There was a problem hiding this comment.
I'm interested in evaluating the torque that's just necessary to counterbalance the natural forces on the joints. Mainly the contributions from
- gravitational force applied by the forward links, for each joint
- Coulomb friction from the motor gears
I have read the gravitational torque can also be computed through the analytical jacobian, using the transpose of that matrix. I'll also need the Center Of Mass + mass of the higher links ...
Let me know if you have some thoughts on this new application of the jacobian, how to measure the parameters and derive the inputs
There was a problem hiding this comment.
Before testing on real robot, there's 1 feature missing, the ability to enforce qpos lower and upper limit.
Would your current approach relying on Damped Least Squared (DLS) be extensible to handle those additionnal constraints?
Compare DLS vs OMPL.
DLS
- DLS violates constraints, causing the simulator to enter impossible positions
- It also requires a well-calibrated damping factor, here
damping = 6e-4
MotionTrackingPseudoInverseSoARM100DLQ.mp4
OMPL
OMPL version will make sure to rotate the shoulder pan back to the other side when hitting the limit.
MotionTrackingPseudoInverseSoARM100OMPLLimit.mp4
Limits
lower = [-2.2000, -3.1416, 0.0000, -2.0000, -3.1416, -0.2000]
upper = [2.2000, 0.2000, 3.1416, 1.8000, 3.1416, 2.0000]
Constraint Programming
A possible solution is to formulate the problem for a Quadratic Solver, minimizing the squared error with linear constraints on the variables
Minimize ∥JΔq−Δx∥^2+λ∥Δq∥^2
Subject to
q_min ≤ q + Δq ≤ q_max
Script
To reproduce the simulation, apply this diff
diffik_controller.txt
to the Differential Inverse Kinematics example script.
There was a problem hiding this comment.
I'm interested in evaluating the torque that's just necessary to counterbalance the natural forces on the joints. Mainly the contributions from
- gravitational force applied by the forward links, for each joint
- Coulomb friction from the motor gears
I have read the gravitational torque can also be computed through the analytical jacobian, using the transpose of that matrix. I'll also need the Center Of Mass + mass of the higher links ...
Let me know if you have some thoughts on this new application of the jacobian, how to measure the parameters and derive the inputs
That's a really interesting problem! I've seen gravity compensation used in robots where you have access to low-level motor control and can send a feed-forward compensation term directly to the joint controller. Do we have that level of access on the SO100 motors? Or are you thinking of using this for other purposes?
For the compensation algorithm, you can definitely build on the DH frames. You’d need to express the center of mass and inertia tensor of each link with respect to its associated DH frame. Then, using the Newton-Euler recursive algorithm, you can propagate external forces and moments backward from the end-effector to compute joint torques due to gravity.
To estimate the mass and CoM of each link, you can use SolidWorks or any CAD tool. Just set the material (e.g. PLA for 3D printed parts). That should give reasonably accurate results for the structural parts. However, estimating contribution of the motors (inertia tensor) might be not precise to estimate from CAD.
As for Coulomb friction, the typical model is:
tau_friction = tau_coulumb + D*W
At equilibrium: tau_friction = Ki = tau_coulumb + DW
You can identify the parameters tau_coulumb and D via system identification: inject known currents, measure joint velocities W and fit the model.
There was a problem hiding this comment.
Before testing on real robot, there's 1 feature missing, the ability to enforce qpos lower and upper limit.
Would your current approach relying on Damped Least Squared (DLS) be extensible to handle those additionnal constraints?
Compare DLS vs OMPL.
DLS
- DLS violates constraints, causing the simulator to enter impossible positions
- It also requires a well-calibrated damping factor, here
damping = 6e-4MotionTrackingPseudoInverseSoARM100DLQ.mp4
OMPL
OMPL version will make sure to rotate the shoulder pan back to the other side when hitting the limit.
MotionTrackingPseudoInverseSoARM100OMPLLimit.mp4
Limits
lower = [-2.2000, -3.1416, 0.0000, -2.0000, -3.1416, -0.2000] upper = [2.2000, 0.2000, 3.1416, 1.8000, 3.1416, 2.0000]Constraint Programming
A possible solution is to formulate the problem for a Quadratic Solver, minimizing the squared error with linear constraints on the variables
Minimize ∥JΔq−Δx∥^2+λ∥Δq∥^2 Subject to q_min ≤ q + Δq ≤ q_maxScript
To reproduce the simulation, apply this diff diffik_controller.txt to the Differential Inverse Kinematics example script.
DLS does not take into account joint limits directly. However, as mentioned below, I just inserted a method check_joint_limits() to be called which raises an error in case of out of bound vales and thus provide safe deploy on real robot.
As future next steps, for sure we can explore more advanced techniques like weighted DLS (basically I use a weight matrix which penalizes usage of joints near limits) or optimization formulations.
Integrated SO100 DH table + updated README documentation uploading DH frames pictures
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
| print("Final joint angles = ", q_final) | ||
| print("Final pose direct kinematics = \n", T_final) | ||
|
|
||
| print("err_lin = ", RobotUtils.calc_lin_err(T_goal, T_final)) |
There was a problem hiding this comment.
can you also add a unit test that evaluates the delta between the original goal and the final state.
Assert that the error in euclidean norm is below 0.01 for example ?
There was a problem hiding this comment.
Sure, I updated the inverse_kinematics method by adding an assert statement in case the error norm is greater than 0.01.
Then, at raw 306 in the main I added a new non-reachable T_goal and if you run the code you see it gives you an AssertionError.
Just pushed the code ;)
There was a problem hiding this comment.
I ran the script and got an assertion error.
Traceback (most recent call last):
File "/home/alexis/Documents/python/robotics/slobot/scripts/kinematics.py", line 47, in <module>
q_final = kin.inverse_kinematics(robot, q_init, T_goal, use_orientation=False, k=0.8, n_iter=50)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/home/alexis/Documents/python/robotics/slobot/.venv/lib/python3.12/site-packages/lerobot/common/kinematics/kinematics.py", line 230, in inverse_kinematics
assert lin_error_norm < 1e-2, (
^^^^^^^^^^^^^^^^^^^^^
AssertionError: [ERROR] Large position error (0.0672). Check target reachability (position/orientation)
7% error sounds a bit high, any clue ?
There was a problem hiding this comment.
Good catch Alexis! However, here my goal was to trigger this type of error, so I set T_goal as a far position unreachable from the robot (even if full extended). So the high error is due to this reason.
I just pushed new code with unit testing in a seperate file (unit_tests.py). At row 30, you see I set T_goal even further (I add 1m along x-axis, so you notice even better it will not be reachable), thus triggering that error.
So I think everything behaves as expected about this issue ;)
for more information, see https://pre-commit.ci
There was a problem hiding this comment.
should this be included in this PR?
Pointing to a public download url should be sufficient.
To generate it, drag and drop your file into a Github comment. It will automatically upload it to the cloud.
Like this: robot kinematics.pdf
This online course is also a good resource: https://www.edx.org/learn/robotics/universita-degli-studi-di-napoli-federico-ii-robotics-foundations-i-robot-modeling
There was a problem hiding this comment.
Alright, I removed the file from the folder and inserted it as a comment in this PR thread!
| print("Final joint angles = ", q_final) | ||
| print("Final pose direct kinematics = \n", T_final) | ||
|
|
||
| print("err_lin = ", RobotUtils.calc_lin_err(T_goal, T_final)) |
There was a problem hiding this comment.
I ran the script and got an assertion error.
Traceback (most recent call last):
File "/home/alexis/Documents/python/robotics/slobot/scripts/kinematics.py", line 47, in <module>
q_final = kin.inverse_kinematics(robot, q_init, T_goal, use_orientation=False, k=0.8, n_iter=50)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/home/alexis/Documents/python/robotics/slobot/.venv/lib/python3.12/site-packages/lerobot/common/kinematics/kinematics.py", line 230, in inverse_kinematics
assert lin_error_norm < 1e-2, (
^^^^^^^^^^^^^^^^^^^^^
AssertionError: [ERROR] Large position error (0.0672). Check target reachability (position/orientation)
7% error sounds a bit high, any clue ?
| ] | ||
| } | ||
|
|
||
| FROM_DH_TO_MECH = {"so100": [90.0, 90.0, 90.0, 90.0, 90.0]} |
There was a problem hiding this comment.
SO-ARM-100 DOF is 6, so there should be 6 values in this array.
The 6th is for the mobile jaw. It is fine to ignore it for the purpose of forward kinematics.
Is the assumption that the end effector is the link connected to joint 5, the fixed jaw.
There was a problem hiding this comment.
Thanks, I just pushed a chage to handle the fact that q_init is a 6DOF vector including also the gripper!
So far, there is the assumption that the robot n-frame is exactly at the tip of the gripper, so forward/inverse kinematics behave accordingly (see SO100 DH frames image for references).
I have also introduced in the code nTtool, which someone can set and that could handle automatically the fact that maybe someone added a different gripper. However, as mentioned at row = 42 in kinematics.py, nTtool can be used only when use_orientation = True in the inverse_kinematics().
There was a problem hiding this comment.
rename the joints, Joint 1, Joint 2, ... to lerobot motor names
["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]
There was a problem hiding this comment.
Thanks for sure the current SO100 has its own frames, used also in simulation. However, I do not see a 1 to 1 correspondence with DH frames (someone has different orientation/origin).
So maybe we could just leave the kinematics.py module with DH frames as designed in dh1.png, and make sure people can use it just my completing the dh2mech() and mech2dh functions (which depends on SO100 assembly).
By doing so, maybe, at the end, also for the simulator can be done a mech2dh / dh2mech trick, by pretending that current simulator motor commands are mechanical quantities to be converted in DH quantities.
There was a problem hiding this comment.
This diagram is very helpful.
should we also add joint 6 for the mobile jaw in the left diagram ?
SoARM100LinkFrames.mp4
There was a problem hiding this comment.
See other comment on DH frames
|
Upload useful resource for DH convention: robot kinematics.pdf |
|
Thanks Alexis for the ee_start and ee_goal orientations! I was able to do more testing, and I just pushed:
Could you try this motion? Is the same you gave me, but it moves downwords so that the robot can keep the same orientation, while moving upwards the robot is not able to maintain it due to unreachable positions. If you run my kinematics.py, then you could compare it with the simulator. Last point: I am not sure about the conversion dh2mech and mech2dh for joint 1 and joint 5 (see lines 72 and 76). This is because I do not have enough data to compute the conversion function. So, could you please call the forward kinematics from the simulator on these configurations? If you could paste here the position + orientation for these 4 configs would be really helpful! Thank you, let me know! |
|
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
|
Thanks Alexis, with your last inputs I could carry out the last tests. I completed both the dh2mechanical and mechanical2dh methods to do these conversions, and I also added the related unit tests. Hopefully everything works now! Could you try to replicate in the simulator the motion I do in my main() in kinematics.py please? Something similar to: Basically, the Cartesian poses and the mechanical angles shall be aligned with the simulator now. Looking forward to your feedback, in case everything works, it's finally time to test on the real robot! |
Translation by dz=-0.1This move is impossible: The tip of the fixed jaw hits the ground before the arm can lower the fixed jaw by 0.1 m vertically. The z-location of the fixed jaw is where you see the horizontal red bar (not the green bar) coming out of the joint towards us. Translation by dz=+0.1The solution to the IK problem should be Do you think you can get a unit test to cover the lift by +0.1 m action ? |
|
Yes apologies, I did not realize the robot would have touched the ground. However, I did not use dz+=0.1 beacuse in the final pose the robot is not able anymore to keep the starting orientation (mechanically unfeasible). So with: My code reaches the goal pose. but you need to set use_orientation=False. Since it does not track the orientation, our final joint angles may be different, but the direct kinematic shall be the same (position part only). I would suggest using another test as well where the orientation is kept, so that we can compare also the orientation part. For example: In general, if you can, I would suggest you trying some poses before in simulation (to see they are feasible and their orientation is conserved), and then try to command those poses on my code and check the final direct kinematics. |
|
I was able to translate the fixed jaw by -0.05, keeping the same orientation. Compare the qpos vector from your version (L) and from Genesis (G) They match ! Here's the full script. Please make sure i used your IK library the way you intended. To make it pass, I had to update |
|
Happy to see the results match!! The logic behind your usage of my IK code is correct, but I noted you wrote on line 17: However, this is the intended usage for those 2 methods:
(See lines 320-361 in Let me know whether you think we can move to closing this PR ;) |
Ok that makes sense. I fixed my script accordingly.
Yes, this PR looks good to me! |
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
|
Awesome, thanks Alexis! I closed the PR then, please let me know if everything went well or if I need to do somethig more (manually close related issue #678, delete kinematics_module branch, .) |
|
This is cool, how come you closed it instead of merging it @leo-berte @alexis779 ? |
+1, why is this not merged? |
What this does
This module is designed to compute both forward and inverse kinematics accurately, and it is easily extensible to robots with more degrees of freedom.
@AdilZouitine: we spoke during Mistral AI Robotics Hackaton about a potential upgrade in the kinematics module.
@Cadene
Closes
Closes #678
Notes
Suggested labels: enhancement, feature request
How it was tested
In "main" inside kinematics.py I have performed the following testing example:
"so100"robot model.How to checkout & try?
Check the following files in the folder: