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