void KickEngineData::mirrorIfNecessary(JointRequest& joints)
{
if((positions[Phase::leftFootTra].z - positions[Phase::rightFootTra].z) > 5)
{
toLeftSupport = false;
}
else if((positions[Phase::leftFootTra].z - positions[Phase::rightFootTra].z) < -5)
{
toLeftSupport = true;
}
else
{
if(ref.y > 0)
{
toLeftSupport = true;
}
else
{
toLeftSupport = false;
}
}
if(currentKickRequest.mirror)
{
JointRequest old = joints;
for(int i = 0; i < JointData::numOfJoints; ++i)
{
if(i == JointData::HeadPitch)
continue;
joints.angles[i] = old.mirror(JointData::Joint(i));
}
}
}
void GetUpEngine::setNextJoints(GetUpEngineOutput& output)
{
//do stuff only if we are not at the end of the movement
if(lineCounter < maxCounter && motionID > -1)
{
const float& time = p.mofs[motionID].lines[lineCounter].duration;
ASSERT(time > 0);
ratio = (float)theFrameInfo.getTimeSince(lineStartTimeStamp) / time;
//check if we are done yet with the current line
if(ratio > 1.f)
{
lineStartTimeStamp = theFrameInfo.time;
startJoints = lastUnbalanced;
ratio = 0.f;
//update stiffness
if(++lineCounter < maxCounter)
for(unsigned i = 0; i < p.mofs[motionID].lines[lineCounter].singleMotorStiffnessChange.size(); ++i)
targetJoints.stiffnessData.stiffnesses[p.mofs[motionID].lines[lineCounter].singleMotorStiffnessChange[i].joint] = p.mofs[motionID].lines[lineCounter].singleMotorStiffnessChange[i].s;
}
//are we still not at the end?
if(lineCounter < maxCounter)
{
if(!balance && lineCounter >= p.mofs[motionID].balanceStartLine && p.mofs[motionID].balanceStartLine > -1)
{
theBalancer.init(mirror, p.balancingParams);
balance = true;
}
//set head joints
for(int i = 0; i < 2; ++i)
targetJoints.angles[i] = Angle::fromDegrees(p.mofs[motionID].lines[lineCounter].head[i]);
//set arm joints
for(int i = 0; i < 6; ++i)
{
targetJoints.angles[Joints::lShoulderPitch + i] = Angle::fromDegrees(p.mofs[motionID].lines[lineCounter].leftArm[i]);
targetJoints.angles[Joints::rShoulderPitch + i] = Angle::fromDegrees(p.mofs[motionID].lines[lineCounter].rightArm[i]);
targetJoints.angles[Joints::lHipYawPitch + i] = Angle::fromDegrees(p.mofs[motionID].lines[lineCounter].leftLeg[i]);
targetJoints.angles[Joints::rHipYawPitch + i] = Angle::fromDegrees(p.mofs[motionID].lines[lineCounter].rightLeg[i]);
}
//mirror joints if necessary
if(mirror)
{
JointRequest mirroredJoints;
mirroredJoints.mirror(targetJoints);
targetJoints = mirroredJoints;
}
if(p.mofs[motionID].lines[lineCounter].critical)
verifyUprightTorso(output);
}
}
interpolate(startJoints, targetJoints, ratio, output);
lastUnbalanced = output;
if(balance)
theBalancer.balanceJointRequest(output, p.balancingParams);
}
void KickEngineData::mirrorIfNecessary(JointRequest& joints)
{
if(currentKickRequest.mirror)
{
const JointRequest old = joints;
for(int i = 0; i < Joints::numOfJoints; ++i)
{
if(i == Joints::headPitch)
continue;
joints.angles[i] = old.mirror(static_cast<Joints::Joint>(i));
}
}
}
void SpecialActions::calculateJointRequest(JointRequest& jointRequest)
{
float ratio, f, t;
//joint angles
if(interpolationMode)
{
ratio = dataRepetitionCounter / static_cast<float>(dataRepetitionLength);
for(int i = 0; i < Joints::numOfJoints; ++i)
{
f = lastRequest.angles[i];
if(!mirror)
t = currentRequest.angles[i];
else
t = currentRequest.mirror(static_cast<Joints::Joint>(i));
// if fromAngle is off or ignore use JointAngles for further calculation
if(f == JointAngles::off || f == JointAngles::ignore)
f = theJointAngles.angles[i];
// if toAngle is off or ignore -> turn joint off/ignore
if(t == JointAngles::off || t == JointAngles::ignore)
jointRequest.angles[i] = t;
//interpolate
else
jointRequest.angles[i] = static_cast<float>(t + (f - t) * ratio);
}
}
else
{
if(!mirror)
jointRequest = currentRequest;
else
jointRequest.mirror(currentRequest);
}
//stiffness stuff
if(stiffnessInterpolationCounter <= 0)
{
if(!mirror)
jointRequest.stiffnessData = currentStiffnessRequest;
else
jointRequest.stiffnessData.mirror(currentStiffnessRequest);
}
else
{
ratio = static_cast<float>(stiffnessInterpolationCounter) / stiffnessInterpolationLength;
int f, t;
for(int i = 0; i < Joints::numOfJoints; i++)
{
f = lastStiffnessRequest.stiffnesses[i];
if(!mirror)
t = currentStiffnessRequest.stiffnesses[i];
else
t = currentStiffnessRequest.mirror(static_cast<Joints::Joint>(i));
if(t == f)
jointRequest.stiffnessData.stiffnesses[i] = t;
else
{
if(f == StiffnessData::useDefault)
f = theStiffnessSettings.stiffnesses[i];
if(t == StiffnessData::useDefault)
t = mirror ? theStiffnessSettings.mirror(static_cast<Joints::Joint>(i)) : theStiffnessSettings.stiffnesses[i];
jointRequest.stiffnessData.stiffnesses[i] = int(float(t) + float(f - t) * ratio);
}
}
}
}
void SpecialActions::calculateJointRequest(JointRequest& jointRequest)
{
float ratio, f, t;
//joint angles
if(interpolationMode)
{
ratio = dataRepetitionCounter / (float) dataRepetitionLength;
for(int i = 0; i < JointData::numOfJoints; ++i)
{
f = lastRequest.angles[i];
if(!mirror)
t = currentRequest.angles[i];
else
t = currentRequest.mirror((JointData::Joint)i);
// if fromAngle is off or ignore use JointData for further calculation
if(f == JointData::off || f == JointData::ignore)
f = theFilteredJointData.angles[i];
// if toAngle is off or ignore -> turn joint off/ignore
if(t == JointData::off || t == JointData::ignore)
jointRequest.angles[i] = t;
//interpolate
else
jointRequest.angles[i] = (float)(t + (f - t) * ratio);
}
}
else
{
if(!mirror)
jointRequest = currentRequest;
else
jointRequest.mirror(currentRequest);
}
//hardness stuff
if(hardnessInterpolationCounter <= 0)
{
if(!mirror)
jointRequest.jointHardness = currentHardnessRequest;
else
jointRequest.jointHardness.mirror(currentHardnessRequest);
}
else
{
ratio = ((float)hardnessInterpolationCounter) / hardnessInterpolationLength;
int f, t;
for(int i = 0; i < JointData::numOfJoints; i++)
{
f = lastHardnessRequest.hardness[i];
if(!mirror)
t = currentHardnessRequest.hardness[i];
else
t = currentHardnessRequest.mirror((JointData::Joint)i);
if(t == f)
jointRequest.jointHardness.hardness[i] = t;
else
{
if(f == HardnessData::useDefault)
f = theHardnessSettings.hardness[i];
if(t == HardnessData::useDefault)
t = mirror ? theHardnessSettings.mirror((JointData::Joint)i) : theHardnessSettings.hardness[i];
jointRequest.jointHardness.hardness[i] = int(float(t) + float(f - t) * ratio);
}
}
}
}
