void URMove::computeVelocities(){
if( targetJointPose.size() >0) {
if( currentJointVelocity.size() != currentPose.size() ) {
currentJointVelocity.assign( currentPose.size(), 0.0f );
}
avgAccel = 0;
previousJointVelocity = currentJointVelocity;
minSpeed = FLT_MAX;
maxSpeed = FLT_MIN;
for( int i = 0; i < currentPose.size(); i++ ) {
currentJointVelocity[i] = ofAngleDifferenceRadians( currentPose[i], targetPose[i] )/deltaTime/speedDivider;
currentJointVelocity[i] = ofClamp(currentJointVelocity[i], -PI/2, PI/2);
minSpeed = MIN(minSpeed.get(), currentJointVelocity[i]);
maxSpeed = MAX(maxSpeed.get(), currentJointVelocity[i]);
jointAccelerations[i] = (currentJointVelocity[i] - previousJointVelocity[i])/deltaTime/speedDivider;
if(fabs(jointAccelerations[i]) > fabs(avgAccel)){
avgAccel =jointAccelerations[i];
}
}
// cout << "avgAccel : " << avgAccel << endl;
} else {
// for(int i = 0; i < currentJointVelocity.size(); i++){
// currentJointVelocity[i] = 0;
// }
}
}
//--------------------------------------------------
float ofLerpRadians(float currentAngle, float targetAngle, float pct) {
return currentAngle + ofAngleDifferenceRadians(currentAngle,targetAngle) * pct;
}