void KickEngineData::calcLegJoints(const Joints::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions, const DamageConfigurationBody& theDamageConfigurationBody)
{
const int sign = joint == Joints::lHipYawPitch ? 1 : -1;
const Vector3f& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra];
const Vector3f& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot];
const RotationMatrix rotateBodyTilt = RotationMatrix::aroundX(bodyAngle.x()).rotateY(bodyAngle.y());
Vector3f anklePos = rotateBodyTilt * (footPos - Vector3f(0.f, 0, -theRobotDimensions.footHeight));
anklePos -= Vector3f(0.f, sign * (theRobotDimensions.yHipOffset), 0);
//const RotationMatrix zRot = RotationMatrix::aroundZ(footRotAng.z()).rotateX(sign * pi_4);
//anklePos = zRot * anklePos;
const float leg0 = 0;//std::atan2(-anklePos.x(), anklePos.y());
const float diagonal = anklePos.norm();
// upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
float a1 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength -
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength + diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * diagonal);
//if(std::abs(a1) > 1.f) OUTPUT_TEXT("clipped a1");
a1 = std::abs(a1) > 1.f ? 0.f : std::acos(a1);
float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength +
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength);
//if(std::abs(a2) > 1.f) OUTPUT_TEXT("clipped a2");
a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);
const float leg2 = -a1 - std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm() * -sgn(anklePos.z()));
const float leg1 = anklePos.z() == 0.0f ? 0.0f : (std::atan(anklePos.y() / -anklePos.z()));
const float leg3 = pi - a2;
const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(0).rotateX(bodyAngle.x()).rotateY(bodyAngle.y());
//calculate inverse foot rotation so that they are flat to the ground
RotationMatrix footRot = RotationMatrix::aroundX(leg1).rotateY(leg2 + leg3);
footRot = footRot.inverse() /* * zRot*/ * rotateBecauseOfHip;
//and add additonal foot rotation (which is probably not flat to the ground)
const float leg4 = std::atan2(footRot(0, 2), footRot(2, 2)) + footRotAng.y();
const float leg5 = std::asin(-footRot(1, 2)) + footRotAng.x() ;
jointRequest.angles[joint] = leg0;
jointRequest.angles[joint + 1] = (/*-pi_4 * sign + */leg1);
jointRequest.angles[joint + 2] = leg2;
jointRequest.angles[joint + 3] = leg3;
jointRequest.angles[joint + 4] = leg4;
jointRequest.angles[joint + 5] = leg5;
//quickhack which allows calibration, but works only if the left foot is the support foot in .kmc file
Vector2f tiltCalibration = currentKickRequest.mirror ? theDamageConfigurationBody.startTiltRight : theDamageConfigurationBody.startTiltLeft;
if(currentKickRequest.mirror)
tiltCalibration.x() *= -1.f;
jointRequest.angles[Joints::lAnkleRoll] += tiltCalibration.x();
jointRequest.angles[Joints::lAnklePitch] += tiltCalibration.y();
}
bool KickViewMath::calcLegJoints(const Vector3f& footPos, const Vector3f& footRotAng, const bool& left, const RobotDimensions& robotDimensions,
float& leg0, float& leg1, float& leg2, float& leg3, float& leg4, float& leg5)
{
float sign = (left) ? 1.f : -1.f;
bool legTooShort = false;
Vector3f anklePos; //FLOAT
anklePos = Vector3f(footPos.x(), footPos.y(), footPos.z() + robotDimensions.footHeight);
anklePos -= Vector3f(0.f, sign * (robotDimensions.yHipOffset), 0.f);
const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(sign * footRotAng.z()).rotateX(-sign * pi_4);
Vector3f rotatedFootRotAng;
anklePos = rotateBecauseOfHip * anklePos;
leg0 = footRotAng.z();
rotatedFootRotAng = rotateBecauseOfHip * footRotAng;
leg2 = -std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm());
float diagonal = anklePos.norm();
// upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
float a1 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength -
robotDimensions.lowerLegLength * robotDimensions.lowerLegLength + diagonal * diagonal) /
(2 * robotDimensions.upperLegLength * diagonal);
a1 = std::abs(a1) > 1.f ? 0 : std::acos(a1);
float a2 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength +
robotDimensions.lowerLegLength * robotDimensions.lowerLegLength - diagonal * diagonal) /
(2 * robotDimensions.upperLegLength * robotDimensions.lowerLegLength);
if(!Rangef::OneRange().isInside(a2))
{
legTooShort = true;
}
a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);
leg1 = (-sign * std::atan2(anklePos.y(), std::abs(anklePos.z()))) - (pi / 4);
leg2 -= a1;
leg3 = pi - a2;
RotationMatrix footRot = RotationMatrix::aroundX(leg1 * -sign).rotateY(leg2 + leg3);
footRot = footRot.inverse() * rotateBecauseOfHip;
leg5 = std::asin(-footRot.col(2).y()) * -sign;
leg4 = -std::atan2(footRot.col(2).x(), footRot.col(2).z()) * -1;
leg4 += footRotAng.y();
leg5 += footRotAng.x();
return legTooShort;
}
