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)
_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));
}
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));
}
Is it possible next release we can have a more sophisticated way to get cyclic Joint's Velocity :)
Samuel