Add a way to get the speed of a motor and/or joint

[ShuffleWire]

Is your feature request related to a problem? Please describe.
I'm working with rotating part, and I wish I could get the current rotational speed of a motor. This extend to linear motor, and actually to any joint.

Describe the solution you'd like
Add a "speed_feeback" in Motor llike the "force_feedback"
OR
Add a SpeedSensor like PositionSensor
OR
Extend PositionSensor to return speed

Describe alternatives you've considered
I'm using a GPS on interesting point of my part, and manually deduce the speed of the part using gps_get_speed(). But that force me to create a lot of these, and that not very efficient

[omichel]

Since speed sensors in the real world for motors or joint are implemented as position sensors for which the speed is derived, I would propose to extend the PositionSensor to return also the speed. It is a matter of adding an API function like wb_position_sensor_get_speed(). I would be happy to review your PR about it.

[ShuffleWire]

Afaik, WbPositionSensor derivate from WbJointDevice from WbJoint
The position is extracted from it, but I don't how to get the speed... Could be ask ODE for that ? or should I compute it manually, remembering the last value etc...

[omichel]

Both options are possible, but I would rather ask ODE for that since deriving the velocity from the position can be done on the controller side (no need for a new API function). Note: I believe a better name would be wb_position_sensor_get_velocity() which is more consistent with the motor API. To get the velocity from ODE, you can use these functions:

const dReal * dBodyGetLinearVel (dBodyID);
const dReal * dBodyGetAngularVel (dBodyID);

depending on the type of joint (linear or rotational).
See http://ode.org/wiki/index.php?title=Manual

[omichel]

Note: because the velocities retrieved by ODE are absolute, they should be subtracted from the velocity of the parent body to get a relative velocity which is what we want in this case.

[ShuffleWire]

Well, that seems like a bit of work, but nice way to dive into ODE ! I will give it a try at home :)
I agree that using ODE data is way better.
What if the parent Solid is inside [nested] Transform-derivated node ? Should I subtract all the velocity to get a relative velocity ? I guess I might be able to use the same code than for position, if ODE process them the same way...

[omichel]

You should simply find the parent ODE body of the ODE body you want to measure the velocity. Only a single subtraction of the velocities of two bodies should be sufficient.

[ShuffleWire]

Thank for the feeback :) Will do my best !

[llessieux]

If you want I can try to have a go at it next week ;)

[ShuffleWire]

If you want I can try to have a go at it next week ;)

Yes ! Go ahead ! I'll be in holiday this week, but I might find some time to follow.

[ShuffleWire]

I have tried to work again on that, and to understand how others sensors are developed.

found out that in void WbHingeJoint::postPhysicsStep() in WbHingeJoint.cpp
there is usage of the function double angleRate = dJointGetHingeAngleRate(mJoint); at line 383

could it be a more direct way to get the rotational speed of the hinge ? That would avoid me to work on a new ode function like dBodyGetAngularVel

what do you think ? @omichel

[omichel]

Yes, that seems to be a good idea.

[ShuffleWire]

MR in #5377