How to handle fixed joints

[diegoferigo]

If few cases, urdf and sdf models can have fixed joints and gym-ignition already supports them:

https://github.com/robotology/gym-ignition/blob/fb40fd156a2f6c0c1c5315555014650527426709/gym_ignition/base/robot/robot_joints.py#L11-L15

However, in the current status, methods like robot_joints.joint_names() will return the list of all joints, including the fixed ones. In general, we should discuss how to handle fixed joints for all the vectorized methods (#126).

I propose the following:

1. Add a include_fixed_joints: bool = False argument to robot_joints.joint_names(). In this way, downstream code can get optionally get all the joint names including the fixed ones. Then, using robot_joints.joint_type(joint_name) it can filter the joints by type.
2. All the other vectorized methods operate only on the non-fixed joints. This means that methods like robot_joints.joint_positions() will return only the controllable joints.
3. Since also fixed joints can have a position different than zero that could be required by downstream code, non-vectorized methods like robot_joints.joint_position(fixed_joint_name) should work also on fixed joints.
4. The function robot_joints.dofs() should return only the number of controllable joints.

@traversaro any comment?

[raffaello-camoriano]

Regarding points 1 and 2, which advantages are implied in restricting robot_joints.joint_names() and vectorized methods to non-fixed joints as a default?

As a counterexample, I can think of vectorized methods returning all joint info being useful for tasks involving forward kinematics (can you think of any others?). But if you think that the majority of use cases only require info from the non-fixed joints then your proposal seems fine. It really depends on what's the expected set of relevant use cases.

[diegoferigo]

Regarding points 1 and 2, which advantages are implied in restricting robot_joints.joint_names() and vectorized methods to non-fixed joints as a default?

By construction the position of a fixed joint does not change. You don't need to query it every time. Users could do it in the very beginning and store the values in a buffer.

Forward kinematics is another example that needs to know only the initial position of fixed joints.

[raffaello-camoriano]

By construction the position of a fixed joint does not change. You don't need to query it every time. Users could do it in the very beginning and store the values in a buffer.

Forward kinematics is another example that needs to know only the initial position of fixed joints.

Then I agree: better favoring performance by default for vectorized methods since they're meant to be called frequently.

[traversaro]

@traversaro any comment?

I think it is to determine the "best" API is important to understand which joint types of SDF (see http://sdformat.org/spec?ver=1.6&elem=joint#joint_type) we want to support. It could make sense to support just fixed, revolute and prismatic, while if we want to eventually support ball the API probably we cannot have a lot of simplifying assumptions.

Having said that, in my experience it is better to clearly separate the concept of degree of freedom from the concept of joint (as we did for example in iDynTree, see https://robotology.github.io/idyntree/devel/classiDynTree_1_1Model.html#details and https://robotology.github.io/idyntree/master/classiDynTree_1_1IJoint.html#details ). So, for example I think it is more clear to have robot.joint_names() and robot.dof_names() rather then having a parameter that changes the semantics of the method. Furthermore, I would try to avoid to confuse "degree of freedom" with "controllable joints", as it is quite common to have models with non-fixed joints that are passing joints that can't be controlled (for example when the robot manipulates an articulated object, ask @CarlottaSartore for examples of this : ) ).

[traversaro]

Since also fixed joints can have a position different than zero that could be required by downstream code, non-vectorized methods like robot_joints.joint_position(fixed_joint_name) should work also on fixed joints.

I am not sure what you mean. As you can read in https://link.springer.com/article/10.1007/s11071-010-9717-3 (by far the best pubblication regarding modelling joints) the typically used mathematical model of the joints is just a function that maps an element of a configuration space $C$ to the rigid transform ${}^B H_{D} \in SE(3)$ that describes the transform between the link frame $P$ and the link frame $C$:

$$ {}^B H_{D}(.) : C \mapsto SE(3) $$

The configuration space $C$ depends on the type of joint (it is a closed subset of $\mathbb{R}^1$ for limited revolute and prismatic joints, $SO(3)$ for spherical joints, etc etc). For a given joint, the methods such as "joint_position" return an element of $C$, for example a single scalar double for revolute joints. The fixed joint is a type of joint for which ${}^B H_D$ is a constant, so the configuration space $C$ does not exists/it the empty set. For this reason, which kind of value should joint_position(.) return for a fixed joint?

[diegoferigo]

I am not sure what you mean. [...] The fixed joint is a type of joint for which ${}^B H_D$ is a constant, so the configuration space $C$ does not exists/it the empty set.

Thanks for the reference @traversaro. I think here there's some confusion due to a particular case I had in mind. Strictly speaking about fixed joints, all what you said is correct. In fact, fixed joints are defined with their origin element (example). However, if sdformat is told not to lump fixed joints, the output is a fake revolute joint with a default position:

https://github.com/robotology/gym-ignition/blob/12a4e66c5cf3ce4a1577e306c3b254f682ef0a0b/gym_ignition_data/iCubGazeboV2_5/iCubGazeboV2_5.sdf#L777-L802

When I opened this issue I had in mind this use case, since I had to manually remove all the *_ft_* joints. Though, this is very specific to iCub's model and how we model FT sensors. Strictly speaking, these joints are not fixed in the resulting SDF and it makes sense to have them in the RobotJoints.joint_names list. The only drawback is that the implementation of vectorized methods discussed in #126 will also include data from these fake revolute joints.

[diegoferigo]

I think it is to determine the "best" API is important to understand which joint types of SDF (see http://sdformat.org/spec?ver=1.6&elem=joint#joint_type) we want to support. It could make sense to support just fixed, revolute and prismatic, while if we want to eventually support ball the API probably we cannot have a lot of simplifying assumptions.

Supporting joints with more that 1 DoF si definitely more challenging and it would require a careful rethinking of the interfaces, as you pointed out. I am seeing more and more frameworks that, stretching urdf, allow parsing a new spherical joint type. Honestly, I do not want to go in that direction, since SDF supports natively ball joints and models with sperical joints could be designed directly in SDF. Then, I have no experience about how to control such joints (maybe a rotational PID could be an option?).

I propose to have a f2f meeting and evaluate:

1) If we want to support multi-DoF joints 2) If we want to have the interfaces ready for future support already in v1.0 3) If not, think about how the interfaces should be changed in the future to avoid breaking changes in downstream code

[traversaro]

I propose to have a f2f meeting and evaluate:

I agree that the discussion on support for joints such as ball or revolute2 is not directly related to this issue.

[traversaro]

Strictly speaking about fixed joints, all what you said is correct. In fact, fixed joints are defined with their origin element (example). However, if sdformat is told not to lump fixed joints, the output is a fake revolute joint with a default position:

Ah, I got your point now. I think the issue there is in the model, and on missing docs on the Gazebo side (remarkably, both are actually my fault :D: ). The disableFixedJointLumping option was added in sdformat URDF parser back when SDF did not supported fixed joints. After SDF gained support for fixed joints, a new option preserveFixedJoint was added ( https://bitbucket.org/osrf/sdformat/pull-requests/352/add-preservefixedjoint-option-to-the-urdf/diff ), that actually transforms URDF fixed joints to SDF fixed joints. I am not sure about all the different possible combinations of SDF and Gazebo versions (see http://gazebosim.org/tutorials?tut=install_dependencies_from_source) but I guess the option for sure it is fully supported since at least Gazebo 8. Unfortunately, the iCub models were apparently never updated to use preserveFixedJoint (see the comment in https://github.com/robotology/icub-model-generator/pull/53#issuecomment-315361491), but that should be easy. Once we do that, we do not need to manually add strange logic to handle the "revolute but actually fixed" joints.

[diegoferigo]

Thanks for the explanation @traversaro, now it's clear. The models have been updated accordingly, only the autogeneration inside icub-models is still missing.

At this point, we propagated the changes where needed, there's no more need to understand how to treat fixed joints, since they are not associated with any joint position. In our APIs they will not be listed in RobotJoints.joint_names() nor in RobotJoints.joint_positions().

Maybe we can move the discussion about supported joints, vectorization, and separation between dofs and joints to #126.