void KickEngineData::simpleCalcArmJoints(const Joints::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions, const Vector3f& armPos, const Vector3f& handRotAng) { float sign = joint == Joints::lShoulderPitch ? 1.f : -1.f; const Vector3f target = armPos - Vector3f(theRobotDimensions.armOffset.x(), theRobotDimensions.armOffset.y() * sign, theRobotDimensions.armOffset.z()); jointRequest.angles[joint + 0] = std::atan2(target.z(), target.x()); jointRequest.angles[joint + 1] = std::atan2(target.y() * sign, std::sqrt(sqr(target.x()) + sqr(target.z()))); float length2ElbowJoint = Vector3f(theRobotDimensions.upperArmLength, theRobotDimensions.yOffsetElbowToShoulder, 0.f).norm(); float angle = std::asin(theRobotDimensions.upperArmLength / length2ElbowJoint); Pose3f elbow; elbow.rotateY(-jointRequest.angles[joint + 0]) .rotateZ(jointRequest.angles[joint + 1]) .translate(length2ElbowJoint, 0.f, 0.f) .rotateZ(-angle) .translate(theRobotDimensions.yOffsetElbowToShoulder, 0.f, 0.f); jointRequest.angles[joint + 0] = std::atan2(elbow.translation.z(), elbow.translation.x()); jointRequest.angles[joint + 1] = std::atan2(elbow.translation.y(), std::sqrt(sqr(elbow.translation.x()) + sqr(elbow.translation.z()))); jointRequest.angles[joint + 0] = (jointRequest.angles[joint + 0] < pi) ? jointRequest.angles[joint + 0] : 0_deg; //clip special jointRequest.angles[joint + 0] *= -1.f; jointRequest.angles[joint + 1] *= sign; jointRequest.angles[joint + 2] = handRotAng.x(); jointRequest.angles[joint + 3] = handRotAng.y(); jointRequest.angles[joint + 4] = handRotAng.z(); jointRequest.angles[joint + 5] = 0.f; }
void TorsoMatrixProvider::update(TorsoMatrix& torsoMatrix) { const Vector3f axis(theInertialData.angle.x(), theInertialData.angle.y(), 0.0f); const RotationMatrix torsoRotation = Rotation::AngleAxis::unpack(axis); // calculate "center of hip" position from left foot Pose3f fromLeftFoot = Pose3f(torsoRotation) *= theRobotModel.soleLeft; fromLeftFoot.translation *= -1.; fromLeftFoot.rotation = torsoRotation; // calculate "center of hip" position from right foot Pose3f fromRightFoot = Pose3f(torsoRotation) *= theRobotModel.soleRight; fromRightFoot.translation *= -1.; fromRightFoot.rotation = torsoRotation; // get foot z-rotations const Pose3f& leftFootInverse(theRobotModel.limbs[Limbs::footLeft].inverse()); const Pose3f& rightFootInverse(theRobotModel.limbs[Limbs::footRight].inverse()); const float leftFootZRotation = leftFootInverse.rotation.getZAngle(); const float rightFootZRotation = rightFootInverse.rotation.getZAngle(); // determine used foot const bool useLeft = fromLeftFoot.translation.z() > fromRightFoot.translation.z(); torsoMatrix.leftSupportFoot = useLeft; // calculate foot span const Vector3f newFootSpan(fromRightFoot.translation - fromLeftFoot.translation); // and construct the matrix Pose3f newTorsoMatrix; newTorsoMatrix.translate(newFootSpan.x() / (useLeft ? 2.f : -2.f), newFootSpan.y() / (useLeft ? 2.f : -2.f), 0); newTorsoMatrix.conc(useLeft ? fromLeftFoot : fromRightFoot); // calculate torso offset if(torsoMatrix.translation.z() != 0) // the last torso matrix should be valid { Pose3f& torsoOffset = torsoMatrix.offset; torsoOffset = torsoMatrix.inverse(); torsoOffset.translate(lastFootSpan.x() / (useLeft ? 2.f : -2.f), lastFootSpan.y() / (useLeft ? 2.f : -2.f), 0); torsoOffset.rotateZ(useLeft ? float(leftFootZRotation - lastLeftFootZRotation) : float(rightFootZRotation - lastRightFootZRotation)); torsoOffset.translate(newFootSpan.x() / (useLeft ? -2.f : 2.f), newFootSpan.y() / (useLeft ? -2.f : 2.f), 0); torsoOffset.conc(newTorsoMatrix); } // adopt new matrix and footSpan static_cast<Pose3f&>(torsoMatrix) = newTorsoMatrix; lastLeftFootZRotation = leftFootZRotation; lastRightFootZRotation = rightFootZRotation; lastFootSpan = newFootSpan; // valid? torsoMatrix.isValid = theGroundContactState.contact; // PLOT("module:TorsoMatrixProvider:footSpanX", newFootSpan.x()); PLOT("module:TorsoMatrixProvider:footSpanY", newFootSpan.y()); PLOT("module:TorsoMatrixProvider:footSpanZ", newFootSpan.z()); }
Vector4f LIPStateEstimator::measure(SupportFoot supportFoot, const Vector2f& LIPOrigin) const { const Pose3f& supportFootToTorso = supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight; const Quaternionf& torsoToWorld = theInertialData.orientation2D; const Pose3f originToTorso = supportFootToTorso + Vector3f(LIPOrigin.x(), LIPOrigin.y(), 0.f); const Vector3f comInOrigin = originToTorso.inverse() * theRobotModel.centerOfMass; const Vector3f com = (torsoToWorld * originToTorso.rotation) * comInOrigin; PLOT("module:ZmpWalkingEngine:LIPStateEstimator:Estimate:measuredComHeight", com.z()); const Vector2f accInWorld = (torsoToWorld * theInertialData.acc * 1000.f).head<2>(); const Vector2f zmp = com.head<2>().array() - (accInWorld.array() / LIP3D(LIPHeights).getK().square()); return (Vector4f() << com.head<2>(), zmp).finished(); }
Pose3f KickViewMath::calculateFootPos(const JointAngles& jointAngles, const Joints::Joint& joint, const RobotDimensions& robotDimensions) { Pose3f footPos; float sign = joint == Joints::lHipYawPitch ? -1.f : 1.f; footPos.translate(0, -sign * (robotDimensions.yHipOffset), 0) .rotateX(-pi_4 * sign) .rotateZ(jointAngles.angles[joint]*sign) .rotateX(((jointAngles.angles[joint + 1] + pi_4)*sign)) .rotateY(jointAngles.angles[joint + 2]) .translate(0, 0, -robotDimensions.upperLegLength) .rotateY(jointAngles.angles[joint + 3]) .translate(0, 0, -robotDimensions.lowerLegLength); footPos.translation += Vector3f(0, 0, -robotDimensions.footHeight); //because inverse kinematics subtracts this return footPos; }
LIPStateEstimator::EstimatedState LIPStateEstimator::convertToOtherFoot(const EstimatedState& state) const { EstimatedState otherState(state); otherState.supportFoot = state.supportFoot == SupportFoot::left ? SupportFoot::right : SupportFoot::left; otherState.origin.y() *= -1.f; const Pose3f& soleToTorso = state.supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight; const Pose3f& otherSoleToTorso = state.supportFoot == SupportFoot::left ? theRobotModel.soleRight : theRobotModel.soleLeft; const Pose3f originToTorso = soleToTorso + Vector3f(state.origin.x(), state.origin.y(), 0.f); const Pose3f otherOriginToTorso = otherSoleToTorso + Vector3f(otherState.origin.x(), otherState.origin.y(), 0.f); //const Quaternionf& torsoToWorld = theInertiaData.orientation; //Pose3f oritinToOtherOrigin = Pose3f(torsoToWorld) * otherOriginToTorso.rotation * otherOriginToTorso.inverse() * originToTorso * originToTorso.rotation.inverse() *= torsoToWorld.inverse(); const Pose3f oritinToOtherOrigin = otherOriginToTorso.inverse() * originToTorso; otherState.com.position = (oritinToOtherOrigin * Vector3f(state.com.position.x(), state.com.position.y(), 0.f)).head<2>(); otherState.com.velocity = (oritinToOtherOrigin.rotation * Vector3f(state.com.velocity.x(), state.com.velocity.y(), 0.f)).head<2>(); otherState.zmp = (oritinToOtherOrigin * Vector3f(state.zmp.x(), state.zmp.y(), 0.f)).head<2>(); return otherState; }
Pose3f KickViewMath::calculateHandPos(const JointAngles& jointAngles, const Joints::Joint& joint, const RobotDimensions& robotDimensions) { Pose3f handPos; float sign = joint == Joints::lShoulderPitch ? 1.f : -1.f; handPos.translate(robotDimensions.armOffset.x(), sign * robotDimensions.armOffset.y(), robotDimensions.armOffset.z()); handPos.rotateY(-jointAngles.angles[joint]); handPos.rotateZ(sign * jointAngles.angles[joint + 1]); handPos.translate(robotDimensions.upperArmLength, 0, 0); handPos.rotateX(jointAngles.angles[joint + 2] * sign); handPos.rotateZ(sign * jointAngles.angles[joint + 3]); return handPos; }
void InertialDataFilter::update(InertialData& inertialData) { DECLARE_PLOT("module:InertialDataFilter:expectedAccX"); DECLARE_PLOT("module:InertialDataFilter:accX"); DECLARE_PLOT("module:InertialDataFilter:expectedAccY"); DECLARE_PLOT("module:InertialDataFilter:accY"); DECLARE_PLOT("module:InertialDataFilter:expectedAccZ"); DECLARE_PLOT("module:InertialDataFilter:accZ"); // check whether the filter shall be reset if(!lastTime || theFrameInfo.time <= lastTime) { if(theFrameInfo.time == lastTime) return; // weird log file replaying? reset(); } if(theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::playDead) { reset(); } // get foot positions Pose3f leftFoot = theRobotModel.limbs[Limbs::footLeft]; Pose3f rightFoot = theRobotModel.limbs[Limbs::footRight]; leftFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight); rightFoot.translate(0.f, 0.f, -theRobotDimensions.footHeight); const Pose3f leftFootInvert(leftFoot.inverse()); const Pose3f rightFootInvert(rightFoot.inverse()); // calculate rotation and position offset using the robot model (joint data) const Pose3f leftOffset(lastLeftFoot.translation.z() != 0.f ? Pose3f(lastLeftFoot).conc(leftFootInvert) : Pose3f()); const Pose3f rightOffset(lastRightFoot.translation.z() != 0.f ? Pose3f(lastRightFoot).conc(rightFootInvert) : Pose3f()); // detect the foot that is on ground bool useLeft = true; if(theMotionInfo.motion == MotionRequest::walk && theWalkingEngineOutput.speed.translation.x() != 0) { useLeft = theWalkingEngineOutput.speed.translation.x() > 0 ? (leftOffset.translation.x() > rightOffset.translation.x()) : (leftOffset.translation.x() < rightOffset.translation.x()); } else { Pose3f left(mean.rotation); Pose3f right(mean.rotation); left.conc(leftFoot); right.conc(rightFoot); useLeft = left.translation.z() < right.translation.z(); } // update the filter const float timeScale = theFrameInfo.cycleTime; predict(RotationMatrix::fromEulerAngles(theInertialSensorData.gyro.x() * timeScale, theInertialSensorData.gyro.y() * timeScale, 0)); // insert calculated rotation safeRawAngle = theInertialSensorData.angle.head<2>().cast<float>(); bool useFeet = true; MODIFY("module:InertialDataFilter:useFeet", useFeet); if(useFeet && (theMotionInfo.motion == MotionRequest::walk || theMotionInfo.motion == MotionRequest::stand || (theMotionInfo.motion == MotionRequest::specialAction && theMotionInfo.specialActionRequest.specialAction == SpecialActionRequest::standHigh)) && std::abs(safeRawAngle.x()) < calculatedAccLimit.x() && std::abs(safeRawAngle.y()) < calculatedAccLimit.y()) { const RotationMatrix& usedRotation(useLeft ? leftFootInvert.rotation : rightFootInvert.rotation); Vector3f accGravOnly(usedRotation.col(0).z(), usedRotation.col(1).z(), usedRotation.col(2).z()); accGravOnly *= Constants::g_1000; readingUpdate(accGravOnly); } else // insert acceleration sensor values readingUpdate(theInertialSensorData.acc); // fill the representation inertialData.angle = Vector2a(std::atan2(mean.rotation.col(1).z(), mean.rotation.col(2).z()), std::atan2(-mean.rotation.col(0).z(), mean.rotation.col(2).z())); inertialData.acc = theInertialSensorData.acc; inertialData.gyro = theInertialSensorData.gyro; inertialData.orientation = Rotation::removeZRotation(Quaternionf(mean.rotation)); // this keeps the rotation matrix orthogonal mean.rotation.normalize(); // store some data for the next iteration lastLeftFoot = leftFoot; lastRightFoot = rightFoot; lastTime = theFrameInfo.time; // plots Vector3f angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(inertialData.orientation)); PLOT("module:InertialDataFilter:angleX", toDegrees(angleAxisVec.x())); PLOT("module:InertialDataFilter:angleY", toDegrees(angleAxisVec.y())); PLOT("module:InertialDataFilter:angleZ", toDegrees(angleAxisVec.z())); PLOT("module:InertialDataFilter:unfilteredAngleX", theInertialSensorData.angle.x().toDegrees()); PLOT("module:InertialDataFilter:unfilteredAngleY", theInertialSensorData.angle.y().toDegrees()); angleAxisVec = Rotation::AngleAxis::pack(AngleAxisf(mean.rotation)); PLOT("module:InertialDataFilter:interlanAngleX", toDegrees(angleAxisVec.x())); PLOT("module:InertialDataFilter:interlanAngleY", toDegrees(angleAxisVec.y())); PLOT("module:InertialDataFilter:interlanAngleZ", toDegrees(angleAxisVec.z())); }