Bug when caculating cyclic Joint's Velocity

Report crashes, strange behaviour, or apparent bugs
Post Reply
SamuelXu
Posts: 8
Joined: 30 Oct 2016, 11:12

Bug when caculating cyclic Joint's Velocity

Post by SamuelXu »

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

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: Bug when caculating cyclic Joint's Velocity

Post by coppelia »

Hello Samuel,

unfortunately V-REP is not able to know what you did: e.g. imagine you your cyclic joint is at position 0, and you programmatically set its next position at 350 degrees. What should be the correct answer? Did you intend to make a -10 degrees move, a 350 degrees move, a 710 degrees move? Only you will know that and should take that into account. V-REP cannot guess what has happened, even with a revolute joint handled by the physics engine, this is not always trivial.
So in the end, V-REP will return the shortest cyclic distance and consider that the direction of movement.

Cheers

Post Reply