I am working on this in ROS2 as we speak. I have created a new node and imported the dependencies orocos_kdl and kdl_parser. These are both available in ROS or ROS2 as they are used by TF. I then followed the example in this test unit of the kdl_parser:
https://github.com/ros2/kdl_parser/bl...
However, the test is actually a False Positive and fails to test anything but passes! Simple bug they forgot to size the torques_1/2 arrays using chain.getNrOfJoints(). Still, this test is a good code example of computing the Inverse Dynamics and with the simple code fix of torques_1.resize( chain.getNrOfJoints() )added it works. I have a github issue posted here:
https://github.com/ros/kdl_parser/iss...
So in this code you pass in q, qdot and qdotdot and wrenches (f_ext) vectors:
Parameters:
- q The current joint positions
- q_dot The current joint velocities
- q_dotdot The current joint accelerations
- f_ext The external forces (no gravity) on the segments. Gravity vector is provided in the constructor.
Output parameters:
- torques the resulting torques for the joints
There are other Solvers in the orocos_kdl and kdl_parser libraries. If solving this kind if ID problem is not what your looking for take a look around the github code, unit tests, and docs and I bet there is the one you are looking for. If you are using ROS2 I can share my code...I just made it yesterday so dont expect much lol but we could possibly work on it together if we are solving the same problem. At the moment I am adding gravity compensation for a humanoid, but will be moving on to contact detection and other issues.
