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 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());
}
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;
}
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()));
}
