public static ForwardKinematics GetForwardKinematicsLeft(Point3 shoulderPosition, InverseKinematics ik) { // The angles are required to be in radians. Just in case they are not, convert to radians. InverseKinematics inRadians = ik.ToRadians(); return(GetForwardKinematicsLeft(shoulderPosition, inRadians.ShoulderYaw, inRadians.ShoulderPitch, inRadians.ShoulderRoll, inRadians.ElbowPitch, inRadians.UpperArmLength, inRadians.LowerArmLength)); }
/// <summary> /// Gets the angle of the shoulder joint pitch and yaw. /// </summary> /// <param name="joints">List of joint positions.</param> private void TestShoulderToElbow(ref IReadOnlyDictionary <JointType, Joint> joints) { // 1. Get elbow and shoulder points Point3 pointForShoulderRight = Point3.FromCameraSpacePoint(joints[JointType.ShoulderRight].Position); Point3 pointForShoulderLeft = Point3.FromCameraSpacePoint(joints[JointType.ShoulderLeft].Position); Point3 pointForElbowRight = Point3.FromCameraSpacePoint(joints[JointType.ElbowRight].Position); Point3 pointForElbowLeft = Point3.FromCameraSpacePoint(joints[JointType.ElbowLeft].Position); Point3 pointForWristRight = Point3.FromCameraSpacePoint(joints[JointType.WristRight].Position); Point3 pointForWristLeft = Point3.FromCameraSpacePoint(joints[JointType.WristLeft].Position); Point3 pointForNeck = Point3.FromCameraSpacePoint(joints[JointType.Neck].Position); Point3 pointForSpine = Point3.FromCameraSpacePoint(joints[JointType.SpineShoulder].Position); // 2. Calculate the inverse kinematics (angles of each joint). InverseKinematics inverseKinematicsRight = InverseKinematics.GetInverseKinematicsRight( pointForNeck, pointForSpine, pointForShoulderLeft, pointForShoulderRight, pointForElbowRight, pointForWristRight); InverseKinematics inverseKinematicsLeft = InverseKinematics.GetInverseKinematicsLeft( pointForNeck, pointForSpine, pointForShoulderLeft, pointForShoulderRight, pointForElbowLeft, pointForWristLeft); // 3. Display the angles on the screen using (DrawingContext dc = this.projectedGroup.Open()) { // Using matrices and forward kinematics. InverseKinematics ikRadiansRight = inverseKinematicsRight.ToRadians(); InverseKinematics ikRadiansLeft = inverseKinematicsLeft.ToRadians(); ForwardKinematics fkRight = ForwardKinematics.GetForwardKinematicsRight(pointForShoulderRight, ikRadiansRight); ForwardKinematics fkLeft = ForwardKinematics.GetForwardKinematicsLeft(pointForShoulderLeft, ikRadiansLeft); // Draw the skeleton (neck/spine and shoulders) // Draw the projected vectors Pen drawingPen = bodyColors[2]; // new Pen(new SolidColorBrush(Color.FromArgb(255, 68, 192, 68)), 3); Pen skeletonPen = this.inferredBonePen; // Use the angles to create projected vectors of joints. Use Forward Kinematics. IReadOnlyDictionary <JointType, Joint> rightArmPoints = new Dictionary <JointType, Joint> { [JointType.ShoulderRight] = new Joint() { JointType = JointType.ShoulderRight, Position = pointForShoulderRight.ToCameraSpacePoint(), TrackingState = TrackingState.Tracked, }, [JointType.ElbowRight] = new Joint() { JointType = JointType.ElbowRight, Position = fkRight.Elbow.ToCameraSpacePoint(), TrackingState = TrackingState.Tracked, }, [JointType.WristRight] = new Joint() { JointType = JointType.WristRight, Position = fkRight.Wrist.ToCameraSpacePoint(), TrackingState = TrackingState.Tracked, } }; IReadOnlyDictionary <JointType, Joint> leftArmPoints = new Dictionary <JointType, Joint> { [JointType.ShoulderLeft] = new Joint() { JointType = JointType.ShoulderLeft, Position = pointForShoulderLeft.ToCameraSpacePoint(), TrackingState = TrackingState.Tracked, }, [JointType.ElbowLeft] = new Joint() { JointType = JointType.ElbowLeft, Position = fkLeft.Elbow.ToCameraSpacePoint(), TrackingState = TrackingState.Tracked, }, [JointType.WristLeft] = new Joint() { JointType = JointType.WristLeft, Position = fkLeft.Wrist.ToCameraSpacePoint(), TrackingState = TrackingState.Tracked, } }; // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); // convert the joint points to depth (display) space Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>(); foreach (JointType jointType in rightArmPoints.Keys) { // Get the 2D representation of the depth point. DepthSpacePoint depthSpacePoint = this.GetDepthSpacePoint(rightArmPoints[jointType].Position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } foreach (JointType jointType in leftArmPoints.Keys) { // Get the 2D representation of the depth point. DepthSpacePoint depthSpacePoint = this.GetDepthSpacePoint(leftArmPoints[jointType].Position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } this.DrawBody(new Dictionary <JointType, Joint> { [JointType.ShoulderLeft] = leftArmPoints[JointType.ShoulderLeft], [JointType.ShoulderRight] = rightArmPoints[JointType.ShoulderRight] }, jointPoints, dc, inferredBonePen); this.DrawBody(rightArmPoints, jointPoints, dc, bodyColors[4]); this.DrawBody(leftArmPoints, jointPoints, dc, bodyColors[1]); // prevent drawing outside of our render area this.projectedGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); AnglesR.Text = String.Format( "Shoulder {0}" + Environment.NewLine + "Elbow {1}" + Environment.NewLine + "Wrist {2}" + Environment.NewLine + "U Arm L {3}" + Environment.NewLine + "L Arm L {4}" + Environment.NewLine + "Shoulder {5}, {6}, {7}" + Environment.NewLine + "Elbow {8}", SpacePointToString(rightArmPoints[JointType.ShoulderRight].Position), SpacePointToString(rightArmPoints[JointType.ElbowRight].Position), SpacePointToString(rightArmPoints[JointType.WristRight].Position), inverseKinematicsRight.UpperArmLength, inverseKinematicsRight.LowerArmLength, (int)inverseKinematicsRight.ShoulderYaw, (int)inverseKinematicsRight.ShoulderPitch, (int)inverseKinematicsRight.ShoulderRoll, (int)inverseKinematicsRight.ElbowPitch); AnglesL.Text = String.Format( "Shoulder {0}" + Environment.NewLine + "Elbow {1}" + Environment.NewLine + "Wrist {2}" + Environment.NewLine + "U Arm L {3}" + Environment.NewLine + "L Arm L {4}" + Environment.NewLine + "Shoulder {5}, {6}, {7}" + Environment.NewLine + "Elbow {8}", SpacePointToString(leftArmPoints[JointType.ShoulderLeft].Position), SpacePointToString(leftArmPoints[JointType.ElbowLeft].Position), SpacePointToString(leftArmPoints[JointType.WristLeft].Position), inverseKinematicsLeft.UpperArmLength, inverseKinematicsLeft.LowerArmLength, (int)inverseKinematicsLeft.ShoulderYaw, (int)inverseKinematicsLeft.ShoulderPitch, (int)inverseKinematicsLeft.ShoulderRoll, (int)inverseKinematicsLeft.ElbowPitch); } // 4. Send the angles to the motors SerialWrite(FilterAngles(new double[] { inverseKinematicsRight.ShoulderYaw, inverseKinematicsRight.ShoulderPitch, inverseKinematicsRight.ShoulderRoll, inverseKinematicsRight.ElbowPitch, inverseKinematicsLeft.ShoulderYaw, inverseKinematicsLeft.ShoulderPitch, inverseKinematicsLeft.ShoulderRoll, inverseKinematicsLeft.ElbowPitch })); }