void KickEngineData::simpleCalcArmJoints(const JointData::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions, const Vector3<>& armPos, const Vector3<>& handRotAng)
{
float sign = joint == JointData::LShoulderPitch ? 1.f : -1.f;
Vector3<> target = armPos - Vector3<>(theRobotDimensions.armOffset.x,
theRobotDimensions.armOffset.y * sign,
theRobotDimensions.armOffset.z);
jointRequest.angles[joint + 0] = atan2(target.z, target.x);
jointRequest.angles[joint + 1] = atan2(target.y * sign, sqrt(sqr(target.x) + sqr(target.z)));
float length2ElbowJoint = Vector3<>(theRobotDimensions.upperArmLength, theRobotDimensions.yElbowShoulder, 0.f).abs();
float angle = asin(theRobotDimensions.upperArmLength / length2ElbowJoint);
Pose3D elbow;
elbow.rotateY(-jointRequest.angles[joint + 0])
.rotateZ(jointRequest.angles[joint + 1])
.translate(length2ElbowJoint, 0.f , 0.f)
.rotateZ(-angle)
.translate(theRobotDimensions.yElbowShoulder, 0.f, 0.f);
jointRequest.angles[joint + 0] = atan2(elbow.translation.z, elbow.translation.x);
jointRequest.angles[joint + 1] = atan2(elbow.translation.y, sqrt(sqr(elbow.translation.x) + sqr(elbow.translation.z)));
jointRequest.angles[joint + 0] = (jointRequest.angles[joint + 0] < pi) ? jointRequest.angles[joint + 0] : 0; //clip special case
Pose3D hand(elbow.translation);
hand.rotateZ(handRotAng.z * sign)
.rotateY(handRotAng.y)
.rotateX(handRotAng.x * sign)
.translate(theRobotDimensions.lowerArmLength, 0.f, 0.f);
InverseKinematic::calcJointsForElbowPos(elbow.translation, hand.translation, jointRequest, joint, theRobotDimensions);
}
Pose3D ViewBikeMath::calculateHandPos(const JointData& jointData, const JointData::Joint& joint, const RobotDimensions& robotDimensions)
{
Pose3D handPos;
float sign = joint == JointData::LShoulderPitch ? 1.f : -1.f;
handPos.translate(robotDimensions.armOffset.x, sign * robotDimensions.armOffset.y, robotDimensions.armOffset.z);
handPos.rotateY(-jointData.angles[joint]);
handPos.rotateZ(sign * jointData.angles[joint + 1]);
handPos.translate(robotDimensions.upperArmLength, 0, 0);
handPos.rotateX(jointData.angles[joint + 2] * sign);
handPos.rotateZ(sign * jointData.angles[joint + 3]);
return handPos;
}