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);
}
}
}
}
void MotionCombinator::update(JointRequest& jointRequest)
{
specialActionOdometry += theSpecialActionsOutput.odometryOffset;
const JointRequest* jointRequests[MotionRequest::numOfMotions];
jointRequests[MotionRequest::walk] = &theWalkingEngineOutput;
jointRequests[MotionRequest::kick] = &theKickEngineOutput;
jointRequests[MotionRequest::specialAction] = &theSpecialActionsOutput;
jointRequests[MotionRequest::stand] = &theStandOutput;
jointRequests[MotionRequest::getUp] = &theGetUpEngineOutput;
jointRequests[MotionRequest::dmpKick] = &theDmpKickEngineOutput;
const JointRequest* armJointRequests[ArmMotionRequest::numOfArmMotions];
armJointRequests[ArmMotionRequest::none] = &theNonArmeMotionEngineOutput;
armJointRequests[ArmMotionRequest::keyFrame] = &theArmKeyFrameEngineOutput;
jointRequest.angles[Joints::headYaw] = theHeadJointRequest.pan;
jointRequest.angles[Joints::headPitch] = theHeadJointRequest.tilt;
copy(*jointRequests[theMotionSelection.targetMotion], jointRequest);
ASSERT(jointRequest.isValid());
// Find fully active motion and set MotionInfo
if(theMotionSelection.ratios[theMotionSelection.targetMotion] == 1.f)
{
Pose2f odometryOffset;
// default values
motionInfo.motion = theMotionSelection.targetMotion;
motionInfo.isMotionStable = true;
motionInfo.upcomingOdometryOffset = Pose2f();
lastJointAngles = theJointAngles;
switch(theMotionSelection.targetMotion)
{
case MotionRequest::walk:
odometryOffset = theWalkingEngineOutput.odometryOffset;
motionInfo.walkRequest = theWalkingEngineOutput.executedWalk;
motionInfo.upcomingOdometryOffset = theWalkingEngineOutput.upcomingOdometryOffset;
break;
case MotionRequest::kick:
odometryOffset = theKickEngineOutput.odometryOffset;
motionInfo.kickRequest = theKickEngineOutput.executedKickRequest;
motionInfo.isMotionStable = theKickEngineOutput.isStable;
break;
case MotionRequest::specialAction:
odometryOffset = specialActionOdometry;
specialActionOdometry = Pose2f();
motionInfo.specialActionRequest = theSpecialActionsOutput.executedSpecialAction;
motionInfo.isMotionStable = theSpecialActionsOutput.isMotionStable;
break;
case MotionRequest::getUp:
motionInfo.isMotionStable = false;
odometryOffset = theGetUpEngineOutput.odometryOffset;
break;
case MotionRequest::stand:
case MotionRequest::dmpKick:
default:
break;
}
if(theRobotInfo.hasFeature(RobotInfo::zGyro) && (theFallDownState.state == FallDownState::upright || theMotionSelection.targetMotion == MotionRequest::getUp))
{
Vector3f rotatedGyros = theInertialData.orientation * theInertialData.gyro.cast<float>();
odometryOffset.rotation = rotatedGyros.z() * theFrameInfo.cycleTime;
}
odometryData += odometryOffset;
ASSERT(jointRequest.isValid());
}
else // interpolate motions
{
const bool interpolateStiffness = !(theMotionSelection.targetMotion != MotionRequest::specialAction && theMotionSelection.specialActionRequest.specialAction == SpecialActionRequest::playDead &&
theMotionSelection.ratios[MotionRequest::specialAction] > 0.f); // do not interpolate from play_dead
for(int i = 0; i < MotionRequest::numOfMotions; ++i)
if(i != theMotionSelection.targetMotion && theMotionSelection.ratios[i] > 0.)
{
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::headYaw, Joints::headPitch);
if(theArmMotionSelection.armRatios[ArmMotionRequest::none] == 1)
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::lShoulderPitch, Joints::lHand);
if(theArmMotionSelection.armRatios[theArmMotionSelection.rightArmRatiosOffset + ArmMotionRequest::none] == 1)
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::rShoulderPitch, Joints::rHand);
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::lHipYawPitch, Joints::rAnkleRoll);
}
}
ASSERT(jointRequest.isValid());
auto combinateArmMotions = [&](Arms::Arm const arm)
{
const int ratioIndexOffset = arm * theArmMotionSelection.rightArmRatiosOffset;
const Joints::Joint startJoint = arm == Arms::left ? Joints::lShoulderPitch : Joints::rShoulderPitch;
const Joints::Joint endJoint = arm == Arms::left ? Joints::lHand : Joints::rHand;
if(theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] != 1.f)
{
if(theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] > 0 &&
ArmMotionRequest::none != theArmMotionSelection.targetArmMotion[arm])
copy(jointRequest, theNonArmeMotionEngineOutput, startJoint, endJoint);
if(ArmMotionRequest::none != theArmMotionSelection.targetArmMotion[arm])
copy(*armJointRequests[theArmMotionSelection.targetArmMotion[arm]], jointRequest, startJoint, endJoint);
}
if(theArmMotionSelection.armRatios[ratioIndexOffset + theArmMotionSelection.targetArmMotion[arm]] == 1.f)
{
armMotionInfo.armMotion[arm] = theArmMotionSelection.targetArmMotion[arm];
switch(theArmMotionSelection.targetArmMotion[arm])
{
case ArmMotionRequest::keyFrame:
armMotionInfo.armKeyFrameRequest = theArmMotionSelection.armKeyFrameRequest;
break;
case ArmMotionRequest::none:
default:
break;
}
}
else
{
const bool interpolateStiffness = !(theMotionSelection.targetMotion != MotionRequest::specialAction &&
theMotionSelection.specialActionRequest.specialAction == SpecialActionRequest::playDead &&
theMotionSelection.ratios[MotionRequest::specialAction] > 0.f &&
theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] > 0);
const JointRequest toJointRequest = theArmMotionSelection.targetArmMotion[arm] == ArmMotionRequest::none ?
*jointRequests[theMotionSelection.targetMotion] : *armJointRequests[theArmMotionSelection.targetArmMotion[arm]];
for(int i = 0; i < ArmMotionRequest::numOfArmMotions; ++i)
{
if(i != theArmMotionSelection.targetArmMotion[arm] && theArmMotionSelection.armRatios[ratioIndexOffset + i] > 0)
{
interpolate(*armJointRequests[i], toJointRequest, theArmMotionSelection.armRatios[ratioIndexOffset + i], jointRequest, interpolateStiffness, startJoint, endJoint);
}
}
}
ASSERT(jointRequest.isValid());
};
combinateArmMotions(Arms::left);
combinateArmMotions(Arms::right);
if(emergencyOffEnabled)
{
if(theFallDownState.state == FallDownState::falling && motionInfo.motion != MotionRequest::specialAction)
{
saveFall(jointRequest);
centerHead(jointRequest);
centerArms(jointRequest);
currentRecoveryTime = 0;
ASSERT(jointRequest.isValid());
}
else if((theFallDownState.state == FallDownState::staggering || theFallDownState.state == FallDownState::onGround) && (motionInfo.motion != MotionRequest::specialAction))
{
centerHead(jointRequest);
ASSERT(jointRequest.isValid());
}
else
{
if(theFallDownState.state == FallDownState::upright)
{
headJawInSavePosition = false;
headPitchInSavePosition = false;
isFallingStarted = false;
}
if(currentRecoveryTime < recoveryTime)
{
currentRecoveryTime += 1;
float ratio = (1.f / float(recoveryTime)) * currentRecoveryTime;
for(int i = 0; i < Joints::numOfJoints; i++)
{
jointRequest.stiffnessData.stiffnesses[i] = 30 + int(ratio * float(jointRequest.stiffnessData.stiffnesses[i] - 30));
}
}
}
}
float sum(0);
int count(0);
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore && lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore && lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:all", sum / count);
sum = 0;
count = 0;
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:differenceToJointData:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
lastJointRequest = jointRequest;
PLOT("module:MotionCombinator:differenceToJointData:all", sum / count);
#ifndef NDEBUG
if(!jointRequest.isValid())
{
{
std::string logDir = "";
#ifdef TARGET_ROBOT
logDir = "../logs/";
#endif
OutMapFile stream(logDir + "jointRequest.log");
stream << jointRequest;
OutMapFile stream2(logDir + "motionSelection.log");
stream2 << theMotionSelection;
}
ASSERT(false);
}
#endif
}
void MotionCombinator::update(JointRequest& jointRequest)
{
specialActionOdometry += theSpecialActionsOutput.odometryOffset;
copy(theLegJointRequest, jointRequest, theStiffnessSettings, Joints::headYaw, Joints::headPitch);
copy(theArmJointRequest, jointRequest, theStiffnessSettings, Joints::firstArmJoint, Joints::rHand);
copy(theLegJointRequest, jointRequest, theStiffnessSettings, Joints::firstLegJoint, Joints::rAnkleRoll);
ASSERT(jointRequest.isValid());
// Find fully active motion and set MotionInfo
if(theLegMotionSelection.ratios[theLegMotionSelection.targetMotion] == 1.f)
{
// default values
motionInfo.motion = theLegMotionSelection.targetMotion;
motionInfo.isMotionStable = true;
motionInfo.upcomingOdometryOffset = Pose2f();
Pose2f odometryOffset;
switch(theLegMotionSelection.targetMotion)
{
case MotionRequest::walk:
odometryOffset = theWalkingEngineOutput.odometryOffset;
motionInfo.walkRequest = theWalkingEngineOutput.executedWalk;
motionInfo.upcomingOdometryOffset = theWalkingEngineOutput.upcomingOdometryOffset;
break;
case MotionRequest::kick:
odometryOffset = theKickEngineOutput.odometryOffset;
motionInfo.kickRequest = theKickEngineOutput.executedKickRequest;
motionInfo.isMotionStable = theKickEngineOutput.isStable;
break;
case MotionRequest::specialAction:
odometryOffset = specialActionOdometry;
specialActionOdometry = Pose2f();
motionInfo.specialActionRequest = theSpecialActionsOutput.executedSpecialAction;
motionInfo.isMotionStable = theSpecialActionsOutput.isMotionStable;
break;
case MotionRequest::getUp:
motionInfo.isMotionStable = false;
odometryOffset = theGetUpEngineOutput.odometryOffset;
break;
case MotionRequest::stand:
default:
break;
}
if(theRobotInfo.hasFeature(RobotInfo::zGyro) && (theFallDownState.state == FallDownState::upright || theLegMotionSelection.targetMotion == MotionRequest::getUp))
{
Vector3f rotatedGyros = theInertialData.orientation * theInertialData.gyro.cast<float>();
odometryOffset.rotation = rotatedGyros.z() * theFrameInfo.cycleTime;
}
odometryData += odometryOffset;
}
if(emergencyOffEnabled && motionInfo.motion != MotionRequest::getUp)
{
if(theFallDownState.state == FallDownState::falling && motionInfo.motion != MotionRequest::specialAction)
{
safeFall(jointRequest);
centerHead(jointRequest);
currentRecoveryTime = 0;
ASSERT(jointRequest.isValid());
}
else if(theFallDownState.state == FallDownState::onGround && motionInfo.motion != MotionRequest::specialAction)
{
centerHead(jointRequest);
}
else
{
if(theFallDownState.state == FallDownState::upright)
{
headYawInSafePosition = false;
headPitchInSafePosition = false;
}
if(currentRecoveryTime < recoveryTime)
{
currentRecoveryTime += 1;
float ratio = (1.f / float(recoveryTime)) * currentRecoveryTime;
for(int i = 0; i < Joints::numOfJoints; i++)
{
jointRequest.stiffnessData.stiffnesses[i] = 30 + int(ratio * float(jointRequest.stiffnessData.stiffnesses[i] - 30));
}
}
}
}
if(debugArms)
debugReleaseArms(jointRequest);
eraseStiffness(jointRequest);
float sum = 0;
int count = 0;
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore &&
lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore &&
lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:all", sum / count);
sum = 0;
count = 0;
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:differenceToJointData:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += std::abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
lastJointRequest = jointRequest;
PLOT("module:MotionCombinator:differenceToJointData:all", sum / count);
#ifndef NDEBUG
if(!jointRequest.isValid())
{
{
std::string logDir = "";
#ifdef TARGET_ROBOT
logDir = "../logs/";
#endif
OutMapFile stream(logDir + "jointRequest.log");
stream << jointRequest;
OutMapFile stream2(logDir + "motionSelection.log");
stream2 << theLegMotionSelection;
}
ASSERT(false);
}
#endif
}
