We are implementing differentiable models of robot manipulators, which allows us to learn typically assumed to be known models of robots for control and motion planning.
The function find_joint_of_body returns the joint index ordered by which <joint> fields are defined within the URDF file.
The joint indices contained in controlled_joints (which is also the indexing used throughout other parts of the robot model) is ordered by which <link> fields are defined within the URDF file.
compute_endeffector_jacobian erroneously uses the first indexing scheme to locate joints, which will cause errors when the links are defined in a different order than the joints within the URDF.
Fixes issue #37
The function
find_joint_of_bodyreturns the joint index ordered by which<joint>fields are defined within the URDF file. The joint indices contained incontrolled_joints(which is also the indexing used throughout other parts of the robot model) is ordered by which<link>fields are defined within the URDF file.compute_endeffector_jacobianerroneously uses the first indexing scheme to locate joints, which will cause errors when the links are defined in a different order than the joints within the URDF.