Пример #1
0
 private Vector3 GetVectorFromPoints(Point3 p, Point3 q)
 {
     return(Vector3.FromPoints(p, q, InferredZPositionClamp));
 }
        /// <summary>
        /// Calculates the shoulder roll for the left arm.
        /// </summary>
        /// <param name="neckToSpine">The vector from the neck point to the spine point.</param>
        /// <param name="shoulderR2L">The vector from right to left shoulder.</param>
        /// <param name="shoulderToElbow">The vector from the left shoulder to the left elbow.</param>
        /// <param name="elbowToWrist">The vector from the left elbow to the left wrist.</param>
        /// <param name="inRadians">Whether to get the angle in radians instead of degrees. Defaults to false.</param>
        /// <returns>The angle of the shoulder roll.</returns>
        public static double GetShoulderRollLeft(Vector3 neckToSpine, Point3 shoulderL, Point3 shoulderR, Point3 elbow, Point3 wrist, bool inRadians = false)
        {
            Vector3 shoulderR2L = Vector3.FromPoints(shoulderR, shoulderL);

            // 1. Get Body plane (neck-spine and shoulder-shoulder).
            // 2. Get upper arm plane (shoulder-shoulder and shoulder-elbow).
            // 3. Get Arm plane (shoulder-elbow and elbow-wrist).
            // 4. Get angle of planes.
            // 5. Adapt angle to be in the range of 90 deg (pointing up) to -90
            // (pointing down).

            Plane upperArmPlane = Plane.FromPoints(shoulderR, shoulderL, elbow);
            Plane armPlane      = Plane.FromPoints(shoulderL, elbow, wrist);

            if (armPlane.AsVector().IsEmpty)
            {
                return(0);
            }

            double angle;

            if (upperArmPlane.AsVector().IsEmpty)
            {
                // When the elbow is point perpendicular to the body to the
                // side, the upper arm plane will be empty (as the shoulders
                // and upper arm vectors will line up). Use the body plane's
                // normal vector to create a new virtual shoulder to create an
                // upper arm plane.
                Plane   bodyPlane       = Plane.FromVectors(neckToSpine, shoulderR2L);
                Vector3 bodyPlaneNormal = bodyPlane.AsVector();
                Point3  shoulderOffset  = new Point3(shoulderL.X + bodyPlaneNormal.X, shoulderL.Y + bodyPlaneNormal.Y, shoulderL.Z + bodyPlaneNormal.Z);
                upperArmPlane = Plane.FromPoints(shoulderR, shoulderL, shoulderOffset);
            }

            angle = Plane.GetAngleBetweenPlanes(armPlane, upperArmPlane, inRadians);

            double distFromPlane = new Point3(wrist.X - elbow.X, wrist.Y - elbow.Y, wrist.Z - elbow.Z).DistanceFromPlane(upperArmPlane);

            if (distFromPlane > 0)
            {
                return(-angle);
            }
            return(angle);
        }
Пример #3
0
        /// <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
            }));
        }
        public static InverseKinematics GetInverseKinematicsLeft(Point3 neck, Point3 spine, Point3 shoulderL, Point3 shoulderR, Point3 elbow, Point3 wrist, bool inRadians = false)
        {
            Vector3 neckToSpine     = Vector3.FromPoints(neck, spine);
            Vector3 shoulderR2L     = Vector3.FromPoints(shoulderR, shoulderL);
            Vector3 shoulderToElbow = Vector3.FromPoints(shoulderL, elbow);
            Vector3 elbowToWrist    = Vector3.FromPoints(elbow, wrist);

            double shoulderYaw   = GetShoulderYaw(neckToSpine, shoulderR2L, shoulderToElbow, inRadians: inRadians);
            double shoulderPitch = -GetShoulderPitch(shoulderR2L, shoulderToElbow, inRadians: inRadians);
            double shoulderRoll  = GetShoulderRollLeft(neckToSpine, shoulderL, shoulderR, elbow, wrist, inRadians: inRadians);
            double elbowPitch    = -GetElbowAngle(shoulderToElbow, elbowToWrist, inRadians: inRadians);

            return(new InverseKinematics(shoulderYaw, shoulderPitch, shoulderRoll, elbowPitch, shoulderToElbow.Magnitude, elbowToWrist.Magnitude, isInRadians: inRadians));
        }