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 c 8000 ode 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 |
|
Pushed code with following updates:
Since now there is a check_joint_limits function, we could deploy the code safely on the real robot. Missing things to do (with hardware):
Do you think these features could be enough to complete a first merge? Then, of course, over the future new features can be built on top of these (Weighted DLS, Optimization Solver, etc) |
I have tested a translation of the fixed jaw by the vector For the real robot, I have followed the assembly video to attach the servos to the links. But they might be using a different convention. I can run a sim to real script, enforcing this move successfully on the real robot. Lift10cmSim.mp4The start position is the "rotated" position. It uses the following angular positions for the joint space and end-effector positions for task space. The expected final position should be The MJCF config I'm relying on in the simulator used the opposite convention for I'm not sure how to adapt the DH table accordingly ... Can you help me with adding a unit test to hit those start & goal states for joint space & task space, when lifting the end effector upwards by 10 cm ? |
|
|
||
| # divide trajectory in steps | ||
| dist = RobotUtils.calc_distance(self.t_final, self.t_start) | ||
| self.n_steps = int(np.ceil(dist / delta)) |
There was a problem hiding this comment.
on a trivial example where dist = 0, this will be 0 and cause a division by 0.
Traceback (most recent call last):
File "/home/alexis/Documents/python/robotics/slobot/scripts/real.py", line 32, in <module>
qpos_goal = kin.inverse_kinematics(robot, qpos_current, ee_pos_goal)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/home/alexis/Documents/python/robotics/slobot/.venv/lib/python3.12/site-packages/lerobot/common/kinematics/kinematics.py", line 240, in inverse_kinematics
T_desired_interp = self._interp_execute(i)
^^^^^^^^^^^^^^^^^^^^^^^
File "/home/alexis/Documents/python/robotics/slobot/.venv/lib/python3.12/site-packages/lerobot/common/kinematics/kinematics.py", line 279, in _interp_execute
s = i / self.n_steps
~~^~~~~~~~~~~~~~
ZeroDivisionError: division by zero
Adding validation and returning early may work. It would also require a dedicated unit test.
There was a problem hiding this comment.
Thanks for the edge case, I just pushed a fix + added it to unit tests!
lerobot/common/kinematics/README.md
Outdated
| DH (Denavit–Hartenberg) frames provide a systematic and compact way to model the kinematics of robotic arms. Each joint and link transformation is represented by a standard set of parameters, allowing for consistent and scalable computation of **forward kinematics, inverse kinematics and Jacobians**. | ||
|
|
||
| <p align="center"> | ||
| <img src="./images/dh.PNG" alt="DH"/><br> |
There was a problem hiding this comment.
I would remove this diagram. It only shows a simple example.
You added the real one for the actual arm, which is a more relevant.
Thanks Alexis, the video and these inputs are very helpful: qpos_start = [-np.pi/2, -np.pi/2, np.pi/2, np.pi/2, -np.pi/2, np.pi/2] Could you please tell me the full Cartesian pose (wrt your base frame in the simulator) for ee_start and ee_goal? (i.e. I would need the rotation matrix for a full check). Also, could you please provide me these values (the same you have in the simulator)? // to be filled with correct numbers based on your motors assembly (here gripper is included) I inserted random numbers, but it's finally time to insert the real ones ;) I almost there, I'll finish my reasoning once I get these inputs and I'll update you! |
Sorry I missed the orientation. Does the quaternion works for you? Here's the full Cartesian pose: Here are the jacobian matrices, if it helps. Refer to the implementation
Here are the joint angular position ranges, as declared in Mujoco configuration |
for more information, see https://pre-commit.ci
|
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! |
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: