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 }