public static JointAngles DoInverseKinematics(Vector3 targetWristPoint)
{
JointAngles jointAngles = new JointAngles();
jointAngles.BaseAngle = Math.Atan2(targetWristPoint.Y, targetWristPoint.X) * 180.0 / Math.PI;
double distanceFromZAxis = Math.Sqrt(targetWristPoint.X * targetWristPoint.X + targetWristPoint.Y * targetWristPoint.Y);
double heightAboveShoulder = targetWristPoint.Z - ShoulderHeight;
// Inverse kinematics for 2R planar manipulator
double cosTheta2 =
(distanceFromZAxis * distanceFromZAxis + heightAboveShoulder * heightAboveShoulder -
s_UpperArmLengthSquared - s_ForearmLengthSquared) /
(2.0 * UpperArmLength * ForearmLength);
double sinTheta2 = -Math.Sqrt(1 - cosTheta2 * cosTheta2);
double theta2 = Math.Atan2(sinTheta2, cosTheta2) * 180.0 / Math.PI;
jointAngles.ElbowAngle = theta2 + 90;
double k1 = UpperArmLength + ForearmLength * cosTheta2;
double k2 = ForearmLength * sinTheta2;
double theta1 = (Math.Atan2(heightAboveShoulder, distanceFromZAxis) - Math.Atan2(k2, k1)) * 180.0 / Math.PI;
jointAngles.ShoulderAngle = 90 - theta1;
return(jointAngles);
}
public static JointAngles DoInverseKinematics(Vector3 targetWristPoint)
{
JointAngles jointAngles = new JointAngles();
jointAngles.BaseAngle = Math.Atan2(targetWristPoint.Y, targetWristPoint.X) * 180.0 / Math.PI;
double distanceFromZAxis = Math.Sqrt(targetWristPoint.X * targetWristPoint.X + targetWristPoint.Y * targetWristPoint.Y);
double heightAboveShoulder = targetWristPoint.Z - ShoulderHeight;
// Inverse kinematics for 2R planar manipulator
double cosTheta2 =
(distanceFromZAxis * distanceFromZAxis + heightAboveShoulder * heightAboveShoulder -
s_UpperArmLengthSquared - s_ForearmLengthSquared) /
(2.0 * UpperArmLength * ForearmLength);
double sinTheta2 = -Math.Sqrt(1 - cosTheta2 * cosTheta2);
double theta2 = Math.Atan2(sinTheta2, cosTheta2) * 180.0 / Math.PI;
jointAngles.ElbowAngle = theta2 + 90;
double k1 = UpperArmLength + ForearmLength * cosTheta2;
double k2 = ForearmLength * sinTheta2;
double theta1 = (Math.Atan2(heightAboveShoulder, distanceFromZAxis) - Math.Atan2(k2, k1)) * 180.0 / Math.PI;
jointAngles.ShoulderAngle = 90 - theta1;
return jointAngles;
}
private double GetWristTiltAngleForVertical(JointAngles jointAngles)
{
return -90 + jointAngles.ShoulderAngle - jointAngles.ElbowAngle;
}