RotationMatrix ForwardKinematics::calcChestFeetRotation(const KinematicChain& theKinematicChain)
{
RotationMatrix calculatedRotation;
// calculate rotation based on foot - torso transformation
const Pose3D& footLeft = theKinematicChain.theLinks[KinematicChain::LFoot].M;
const Pose3D& footRight = theKinematicChain.theLinks[KinematicChain::RFoot].M;
const Pose3D& body = theKinematicChain.theLinks[KinematicChain::Torso].M;
// local in chest
Pose3D localFootLeft(body.local(footLeft));
Pose3D localFootRight(body.local(footRight));
if(abs(localFootLeft.translation.z - localFootRight.translation.z) < 3.f/* magic number */)
{
// use average of the calculated rotation of each leg
double meanX = (localFootLeft.rotation.getXAngle() + localFootRight.rotation.getXAngle())*0.5;
double meanY = (localFootLeft.rotation.getYAngle() + localFootRight.rotation.getYAngle())*0.5;
//calculatedRotation.fromKardanRPY(0.0, meanY, meanX);
calculatedRotation.rotateX(meanX).rotateY(meanY);
}
else if(localFootLeft.translation.z > localFootRight.translation.z)
{
// use left foot
calculatedRotation = localFootLeft.rotation;
}
else
{
// use right foot
calculatedRotation = localFootRight.rotation;
}
return calculatedRotation.invert();
}//end calcChestFeetRotation
void KickEngineData::calcLegJoints(const JointData::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions)
{
float sign = joint == JointData::LHipYawPitch ? 1.f : -1.f;
float leg0, leg1, leg2, leg3, leg4, leg5;
const Vector3<>& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra];
const Vector3<>& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot];
RotationMatrix rotateBodyTilt = RotationMatrix().rotateX(comOffset.x);
Vector3<> anklePos = rotateBodyTilt * Vector3<>(footPos.x, footPos.y, footPos.z);
//we need just the leg length x and y have to stay the same
anklePos.y = footPos.y;
anklePos.x = footPos.x;
// for the translation of the footpos we only need to translate the anklepoint, which is the intersection of the axis leg4 and leg5
// the rotation of the foot will be made later by rotating the footpos around the anklepoint
anklePos -= Vector3<>(0.f, sign * (theRobotDimensions.lengthBetweenLegs / 2), -theRobotDimensions.heightLeg5Joint);
RotationMatrix rotateBecauseOfHip = RotationMatrix().rotateZ(footRotAng.z).rotateX(-sign * pi_4);
anklePos = rotateBecauseOfHip * anklePos;
float diagonal = anklePos.abs();
// 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(abs(a1) > 1.f) OUTPUT(idText, text, "clipped a1");
a1 = abs(a1) > 1.f ? 0.f : acos(a1);
float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength +
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength);
//if(abs(a2) > 1.f) OUTPUT(idText, text, "clipped a2");
a2 = abs(a2) > 1.f ? pi : acos(a2);
leg0 = footRotAng.z * sign;
leg2 = -a1 - atan2(anklePos.x, Vector2<>(anklePos.y, anklePos.z).abs() * -sgn(anklePos.z));
leg1 = anklePos.z == 0.0f ? 0.0f : atan(anklePos.y / -anklePos.z) * -sign;
leg3 = pi - a2;
RotationMatrix footRot = RotationMatrix().rotateX(leg1 * -sign).rotateY(leg2 + leg3);
footRot = footRot.invert() * rotateBecauseOfHip;
leg5 = asin(-footRot[2].y) * sign * -1;
leg4 = -atan2(footRot[2].x, footRot[2].z) * -1;
leg4 += footRotAng.y;
leg5 += footRotAng.x;
jointRequest.angles[joint] = leg0;
jointRequest.angles[joint + 1] = -pi_4 + leg1;
jointRequest.angles[joint + 2] = leg2;
jointRequest.angles[joint + 3] = leg3;
jointRequest.angles[joint + 4] = leg4;
jointRequest.angles[joint + 5] = leg5;
}
bool ViewBikeMath::calcLegJoints(const Vector3<>& footPos, const Vector3<>& 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;
Vector3<> anklePos; //FLOAT
anklePos = Vector3<>(footPos.x, footPos.y, footPos.z + robotDimensions.heightLeg5Joint);
anklePos -= Vector3<>(0.f, sign * (robotDimensions.lengthBetweenLegs / 2.f), 0.f);
RotationMatrix rotateBecauseOfHip = RotationMatrix().rotateZ(sign * footRotAng.z).rotateX(-sign * pi_4);
Vector3<> rotatedFootRotAng;
anklePos = rotateBecauseOfHip * anklePos;
leg0 = footRotAng.z;
rotatedFootRotAng = rotateBecauseOfHip * footRotAng;
leg2 = -std::atan2(anklePos.x, Vector2<>(anklePos.y, anklePos.z).abs());
float diagonal = anklePos.abs();
// 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);
const Range<float> clipping(-1.f, 1.f);
if(!clipping.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().rotateX(leg1 * -sign).rotateY(leg2 + leg3);
footRot = footRot.invert() * rotateBecauseOfHip;
leg5 = std::asin(-footRot[2].y) * -sign;
leg4 = -std::atan2(footRot[2].x, footRot[2].z) * -1;
leg4 += footRotAng.y;
leg5 += footRotAng.x;
return legTooShort;
}
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, float joint0, bool left, const RobotDimensions& robotDimensions) {
Pose3D target(position);
Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
const int sign(left ? -1 : 1);
target.translation.y += robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2 * sign; // translate to origin of leg
target = Pose3D().rotateZ(joint0 * -sign).rotateX(M_PI_4 * sign).conc(target); // compute residual transformation with fixed joint0
// use cosine theorem and arctan to compute first three joints
float length = target.translation.abs();
float sqrLength = length * length;
float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength];
float sqrUpperLeg = upperLeg * upperLeg;
float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength];
float sqrLowerLeg = lowerLeg * lowerLeg;
float cosUpperLeg = (sqrUpperLeg + sqrLength - sqrLowerLeg) / (2 * upperLeg * length);
float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
// clip for the case that target position is not reachable
const Range<float> clipping(-1.0f, 1.0f);
bool reachable = true;
if(!clipping.isInside(cosKnee) || clipping.isInside(upperLeg))
{
cosKnee = clipping.limit(cosKnee);
cosUpperLeg = clipping.limit(cosUpperLeg);
reachable = false;
}
float joint1 = target.translation.z == 0.0f ? 0.0f : atanf(target.translation.y / -target.translation.z) * sign;
float joint2 = -acos(cosUpperLeg);
joint2 -= atan2(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs() * -sgn(target.translation.z));
float joint3 = M_PI - acos(cosKnee);
RotationMatrix beforeFoot = RotationMatrix().rotateX(joint1 * sign).rotateY(joint2 + joint3);
joint1 -= M_PI_4; // because of the strange hip of Nao
// compute joints from rotation matrix using theorem of euler angles
// see http://www.geometrictools.com/Documentation/EulerAngles.pdf
// this is possible because of the known order of joints (y, x, z) where z is left open and is seen as failure
RotationMatrix foot = beforeFoot.invert() * target.rotation;
float joint5 = asin(-foot[2].y) * -sign * -1;
float joint4 = -atan2(foot[2].x, foot[2].z) * -1;
//float failure = atan2(foot[0].y, foot[1].y) * sign;
// set computed joints in jointAngles
jointAngles[firstJoint + 0] = joint0;
jointAngles[firstJoint + 1] = joint1;
jointAngles[firstJoint + 2] = joint2;
jointAngles[firstJoint + 3] = joint3;
jointAngles[firstJoint + 4] = joint4;
jointAngles[firstJoint + 5] = joint5;
return reachable;
}
bool Geometry::calculatePointByAngles
(
const Vector2<>& angles,
const CameraMatrix& cameraMatrix,
const CameraInfo& cameraInfo,
Vector2<int>& point
)
{
Vector3<> vectorToPointWorld, vectorToPoint;
vectorToPointWorld.x = (float) cos(angles.x);
vectorToPointWorld.y = (float) sin(angles.x);
vectorToPointWorld.z = (float) tan(angles.y);
RotationMatrix rotationMatrix = cameraMatrix.rotation;
vectorToPoint = rotationMatrix.invert() * vectorToPointWorld;
float factor = cameraInfo.focalLength;
float scale = factor / vectorToPoint.x;
point.x = (int)(0.5f + cameraInfo.opticalCenter.x - vectorToPoint.y * scale);
point.y = (int)(0.5f + cameraInfo.opticalCenter.y - vectorToPoint.z * scale);
return vectorToPoint.x > 0;
}
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, bool left, const RobotDimensions& robotDimensions) {
Pose3D target(position);
Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
int sign(left ? -1 : 1);
target.translation.y += (float) robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2.f * sign; // translate to origin of leg
// rotate by 45° around origin for Nao
// calculating sqrtf(2) is faster than calculating the resp. rotation matrix with getRotationX()
static const float sqrt2_2 = sqrtf(2.0f) * 0.5f;
RotationMatrix rotationX_pi_4 = RotationMatrix(Vector3<float>(1, 0, 0), Vector3<float>(0, sqrt2_2, sqrt2_2 * sign), Vector3<float>(0, sqrt2_2 * -sign, sqrt2_2));
target.translation = rotationX_pi_4 * target.translation;
target.rotation = rotationX_pi_4 * target.rotation;
target = target.invert(); // invert pose to get position of body relative to foot
// use geometrical solution to compute last three joints
float length = target.translation.abs();
float sqrLength = length * length;
float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength];
float sqrUpperLeg = upperLeg * upperLeg;
float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength];
float sqrLowerLeg = lowerLeg * lowerLeg;
float cosLowerLeg = (sqrLowerLeg + sqrLength - sqrUpperLeg) / (2 * lowerLeg * length);
float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
// clip for the case of unreachable position
const Range<float> clipping(-1.0f, 1.0f);
bool reachable = true;
if(!clipping.isInside(cosKnee) || clipping.isInside(cosLowerLeg))
{
cosKnee = clipping.limit(cosKnee);
cosLowerLeg = clipping.limit(cosLowerLeg);
reachable = false;
}
float joint3 = M_PI - acosf(cosKnee); // implicitly solve discrete ambiguousness (knee always moves forward)
float joint4 = -acosf(cosLowerLeg);
joint4 -= atan2f(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs());
float joint5 = atan2f(target.translation.y, target.translation.z) * sign;
// calulate rotation matrix before hip joints
RotationMatrix hipFromFoot;
hipFromFoot.rotateX(joint5 * -sign);
hipFromFoot.rotateY(-joint4 - joint3);
// compute rotation matrix for hip from rotation before hip and desired rotation
RotationMatrix hip = hipFromFoot.invert() * target.rotation;
// compute joints from rotation matrix using theorem of euler angles
// see http://www.geometrictools.com/Documentation/EulerAngles.pdf
// this is possible because of the known order of joints (z, x, y seen from body resp. y, x, z seen from foot)
float joint1 = asinf(-hip[2].y) * -sign;
joint1 -= M_PI_4; // because of the 45°-rotational construction of the Nao legs
float joint2 = -atan2f(hip[2].x, hip[2].z);
float joint0 = atan2f(hip[0].y, hip[1].y) * -sign;
// set computed joints in jointAngles
jointAngles[firstJoint + 0] = joint0;
jointAngles[firstJoint + 1] = joint1;
jointAngles[firstJoint + 2] = joint2;
jointAngles[firstJoint + 3] = joint3;
jointAngles[firstJoint + 4] = joint4;
jointAngles[firstJoint + 5] = joint5;
return reachable;
}
Vector2<double> AngleEstimator::RotationMatrix::operator-(const RotationMatrix& other) const
{
//return ((const RotationMatrix&)(other.invert() * *this)).getAngleAxis();
const Vector3<double>& result(other * ((const RotationMatrix&)(other.invert() * *this)).getAngleAxis());
return Vector2<double>(result.x, result.y);
}
