### Bug when caculating cyclic Joint's Velocity

Posted:

**10 Oct 2017, 03:12**Hi：

We are using simGetObjectFloatParameter() with parameter sim_jointfloatparam_velocity (2012) to get a joint's Velocity. it works fine in most cases.

Here we found if a cyclic Joint's speed exceed some threshold, cyclic Joint's Velocity will be abnormal, e.g. from positive(correct) to negative (incorrect)

I dig out some implementation of simGetObjectFloatParameter(), like following code only use _jointPosition and _previousJointPosition, without considering how many circles executed and what's the direction of execution.

void CJoint::measureJointVelocity(float dt)

{

if (_jointType==sim_joint_spherical_subtype)

return;

if (_previousJointPositionIsValid)

{

if (_positionIsCyclic)

else

_measuredJointVelocity_velocityMeasurement=(_jointPosition-_previousJointPosition_velocityMeasurement)/dt;

}

_previousJointPosition_velocityMeasurement=_jointPosition;

_previousJointPositionIsValid=true;

}

float tt::getAngleMinusAlpha(float angle,float alpha)

{ // Returns angle-alpha. Angle and alpha are cyclic angles!!

}

Is it possible next release we can have a more sophisticated way to get cyclic Joint's Velocity :)

Samuel

We are using simGetObjectFloatParameter() with parameter sim_jointfloatparam_velocity (2012) to get a joint's Velocity. it works fine in most cases.

Here we found if a cyclic Joint's speed exceed some threshold, cyclic Joint's Velocity will be abnormal, e.g. from positive(correct) to negative (incorrect)

I dig out some implementation of simGetObjectFloatParameter(), like following code only use _jointPosition and _previousJointPosition, without considering how many circles executed and what's the direction of execution.

void CJoint::measureJointVelocity(float dt)

{

if (_jointType==sim_joint_spherical_subtype)

return;

if (_previousJointPositionIsValid)

{

if (_positionIsCyclic)

**_measuredJointVelocity_velocityMeasurement=tt::getAngleMinusAlpha(_jointPosition,_previousJointPosition_velocityMeasurement)/dt;**else

_measuredJointVelocity_velocityMeasurement=(_jointPosition-_previousJointPosition_velocityMeasurement)/dt;

}

_previousJointPosition_velocityMeasurement=_jointPosition;

_previousJointPositionIsValid=true;

}

float tt::getAngleMinusAlpha(float angle,float alpha)

{ // Returns angle-alpha. Angle and alpha are cyclic angles!!

**double sinAngle0=sin(double(angle));**

double sinAngle1=sin(double(alpha));

double cosAngle0=cos(double(angle));

double cosAngle1=cos(double(alpha));

double sin_da=sinAngle0*cosAngle1-cosAngle0*sinAngle1;

double cos_da=cosAngle0*cosAngle1+sinAngle0*sinAngle1;

double angle_da=atan2(sin_da,cos_da);

return(float(angle_da));double sinAngle1=sin(double(alpha));

double cosAngle0=cos(double(angle));

double cosAngle1=cos(double(alpha));

double sin_da=sinAngle0*cosAngle1-cosAngle0*sinAngle1;

double cos_da=cosAngle0*cosAngle1+sinAngle0*sinAngle1;

double angle_da=atan2(sin_da,cos_da);

return(float(angle_da));

}

Is it possible next release we can have a more sophisticated way to get cyclic Joint's Velocity :)

Samuel