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