Implement joint features in TPE

[diegoferigo]

Desired behavior

Being able to change joint positions in TPE. This would also mean that TPE would allow models to move (by resetting their joint position since it's just kinematic), enabling the simulation of moving robots that can collide with the other models.

Alternatives considered

Using other physics engines is a possibility, but depending on the use case, a kinematic physics engine like TPE could be useful. Speed here is one of the main reasons.

Edit: another alternative would be operating directly on the links, since Link::UpdatePose is implemented, but only operating on the joint velocities is currently supported. This means that the process of moving a model involves the computation of the right velocity to bring q_t=k --> q_t=k+1 (given the time step), update the linear velocities of all links, and then step the simulator.

Implementation suggestion

It seems to me the TPE does not currently implements the {Get,Set}BasicJointState interfaces. This means that the Physics system of ign-gazebo (here and here) does not process any of the ResetJoint* components that would kinematically move the joint.

Additional context

I'm not sure what's the roadmap for TPE, but it seems that the lack of the joint features was never ticketed.