예제 #1
0
        ///<summary>
        /// Compute some angle using rotation matrix
        ///</summary>
        private void angleMatrix(Skeleton skel)
        {
            //HEAD
            Matrix3x3   m_head = new Matrix3x3(skel.BoneOrientations[JointType.Head].HierarchicalRotation.Matrix);
            Vecto3Float v_head = Kinematic.computeEulerFromMatrixXYZ(m_head);

            //Yaw
            this.jointAngles[NAOConversion.HeadYaw] = this.jointAngles[NAOConversion.HeadYaw]
                                                      + NAOConversion.convertAngle(NAOConversion.HeadYaw, v_head.Z) / this.avg;
            //Pitch
            this.jointAngles[NAOConversion.HeadPitch] = this.jointAngles[NAOConversion.HeadPitch]
                                                        + NAOConversion.convertAngle(NAOConversion.HeadPitch, v_head.X) / this.avg;
        }
예제 #2
0
        ///<summary>
        /// Compute angle between the joint according to the position of the joint
        ///</summary>
        private void angleVector()
        {
            //ELBOWROLL
            //Left
            float elbowLeftRollAngle = Kinematic.getAngle(this.avgPoint[JointType.ShoulderLeft],
                                                          this.avgPoint[JointType.WristLeft],
                                                          this.avgPoint[JointType.ElbowLeft],
                                                          false);


            this.jointAngles[NAOConversion.LElbowRoll] = NAOConversion.convertAngle(NAOConversion.LElbowRoll, elbowLeftRollAngle);
            //Right
            float elbowRighttRollAngle = Kinematic.getAngle(this.avgPoint[JointType.ShoulderRight],
                                                            this.avgPoint[JointType.WristRight],
                                                            this.avgPoint[JointType.ElbowRight],
                                                            false);

            this.jointAngles[NAOConversion.RElbowRoll] = NAOConversion.convertAngle(NAOConversion.RElbowRoll, elbowRighttRollAngle);

            //SHOULDERROLL
            //Left
            float shoulderLeftRollAngle = Kinematic.getAngleZX(this.avgPoint[JointType.HipLeft],
                                                               this.avgPoint[JointType.ShoulderLeft],
                                                               this.avgPoint[JointType.ElbowLeft]);

            this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll, shoulderLeftRollAngle);
            //Right
            float shoulderRightRollAngle = Kinematic.getAngleZX(this.avgPoint[JointType.ElbowRight],
                                                                this.avgPoint[JointType.ShoulderRight],
                                                                this.avgPoint[JointType.HipRight]);

            this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll, shoulderRightRollAngle);

            //SHOULDERPITCH
            //Left
            if (this.avgPoint[JointType.ElbowLeft].Z < this.avgPoint[JointType.Spine].Z ||
                this.avgPoint[JointType.ElbowLeft].Y >= this.avgPoint[JointType.Spine].Y)
            {
                float shoulderLeftPitchAngle = Kinematic.getAngleZY(this.avgPoint[JointType.ElbowLeft],
                                                                    this.avgPoint[JointType.ShoulderLeft],
                                                                    this.avgPoint[JointType.HipLeft]);
                this.jointAngles[NAOConversion.LShoulderPitch] = NAOConversion.convertAngle(NAOConversion.LShoulderPitch, shoulderLeftPitchAngle);
            }
            else
            {
                this.jointAngles[NAOConversion.LShoulderPitch] = (float)Math.PI / 2;
            }



            //Right
            if (this.avgPoint[JointType.ElbowRight].Z < this.avgPoint[JointType.Spine].Z ||
                this.avgPoint[JointType.ElbowRight].Y >= this.avgPoint[JointType.Spine].Y)
            {
                float shoulderRightPitchAngle = Kinematic.getAngleZY(this.avgPoint[JointType.HipRight],
                                                                     this.avgPoint[JointType.ShoulderRight],
                                                                     this.avgPoint[JointType.ElbowRight]);
                this.jointAngles[NAOConversion.RShoulderPitch] = NAOConversion.convertAngle(NAOConversion.RShoulderPitch, shoulderRightPitchAngle);
            }
            else
            {
                this.jointAngles[NAOConversion.RShoulderPitch] = (float)Math.PI / 2;
            }
        }
예제 #3
0
        ///<summary>
        /// Compute angle between the joint according to the position of the joint
        ///</summary>
        private void angleVector()
        {
            SkeletonPoint shoulderLeft   = this.avgPoint[JointType.ShoulderLeft];
            SkeletonPoint shoulderCenter = this.avgPoint[JointType.ShoulderCenter];
            SkeletonPoint shoulderRight  = this.avgPoint[JointType.ShoulderRight];

            SkeletonPoint elbowLeft  = this.avgPoint[JointType.ElbowLeft];
            SkeletonPoint elbowRight = this.avgPoint[JointType.ElbowRight];

            SkeletonPoint wristLeft  = this.avgPoint[JointType.WristLeft];
            SkeletonPoint wristRight = this.avgPoint[JointType.WristRight];

            SkeletonPoint hipLeft   = this.avgPoint[JointType.HipLeft];
            SkeletonPoint hipCenter = this.avgPoint[JointType.HipCenter];
            SkeletonPoint hipRight  = this.avgPoint[JointType.HipRight];

            Vecto3Float upperArmLeft = new Vecto3Float(shoulderLeft.X - elbowLeft.X,
                                                       shoulderLeft.Y - elbowLeft.Y,
                                                       shoulderLeft.Z - elbowLeft.Z);

            Vecto3Float lowerArmLeft = new Vecto3Float(wristLeft.X - elbowLeft.X,
                                                       wristLeft.Y - elbowLeft.Y,
                                                       wristLeft.Z - elbowLeft.Z);

            Vecto3Float upperArmRight = new Vecto3Float(shoulderRight.X - elbowRight.X,
                                                        shoulderRight.Y - elbowRight.Y,
                                                        shoulderRight.Z - elbowRight.Z);

            Vecto3Float lowerArmRight = new Vecto3Float(wristRight.X - elbowRight.X,
                                                        wristRight.Y - elbowRight.Y,
                                                        wristRight.Z - elbowRight.Z);


            Vecto3Float shoulderLToHipLeft = new Vecto3Float(shoulderLeft.X - hipLeft.X,
                                                             shoulderLeft.Y - hipLeft.Y,
                                                             shoulderLeft.Z - hipLeft.Z);

            Vecto3Float shoulderRToHipRight = new Vecto3Float(shoulderRight.X - hipRight.X,
                                                              shoulderRight.Y - hipRight.Y,
                                                              shoulderRight.Z - hipRight.Z);

            Vecto3Float shoulderLToShoulderC = new Vecto3Float(shoulderLeft.X - shoulderCenter.X,
                                                               shoulderLeft.Y - shoulderCenter.Y,
                                                               shoulderLeft.Z - shoulderCenter.Z);

            Vecto3Float shoulderRToShoulderC = new Vecto3Float(shoulderRight.X - shoulderCenter.X,
                                                               shoulderRight.Y - shoulderCenter.Y,
                                                               shoulderRight.Z - shoulderCenter.Z);


            if (hipLeft.Z + 0.15 > elbowLeft.Z)   //if the elbow is in front of the hip
            {
                //Shoulder Roll
                //Left

                if (Math.Abs(shoulderLeft.Z - elbowLeft.Z) < 0.1 &&
                    Math.Abs(shoulderLeft.X - elbowLeft.X) < 0.1)
                {
                    this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll,
                                                                                               Kinematic.getAngleNew(upperArmLeft.projectionOntoXY(),
                                                                                                                     shoulderLToHipLeft.projectionOntoXY()));
                }
                else
                {
                    this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll,
                                                                                               Kinematic.getAngleNew(upperArmLeft.projectionOntoZX(),
                                                                                                                     shoulderLToHipLeft.projectionOntoZX()));
                }


                //Shoulder Pitch
                //Left
                this.jointAngles[NAOConversion.LShoulderPitch] = NAOConversion.convertAngle(NAOConversion.LShoulderPitch,
                                                                                            Kinematic.getAngleNew(upperArmLeft.projectionOntoZY(),
                                                                                                                  shoulderLToHipLeft.projectionOntoZY()));
            }

            if (hipRight.Z + 0.15 > elbowRight.Z)//if the elbow is in front of the hip

            {
                //Shoulder Roll
                //Right
                if (Math.Abs(shoulderRight.Z - elbowRight.Z) < 0.1 &&
                    Math.Abs(shoulderRight.X - elbowRight.X) < 0.1)
                {
                    this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll,
                                                                                               Kinematic.getAngleNew(upperArmRight.projectionOntoXY(),
                                                                                                                     shoulderRToHipRight.projectionOntoXY()));
                }
                else
                {
                    this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll,
                                                                                               Kinematic.getAngleNew(upperArmRight.projectionOntoZX(),
                                                                                                                     shoulderRToHipRight.projectionOntoZX()));
                }
                //Shoulder Pitch
                //Right
                this.jointAngles[NAOConversion.RShoulderPitch] = NAOConversion.convertAngle(NAOConversion.RShoulderPitch,
                                                                                            Kinematic.getAngleNew(shoulderRToHipRight.projectionOntoZY(),
                                                                                                                  upperArmRight.projectionOntoZY()));
            }


            //Elbow Roll
            //Left
            this.jointAngles[NAOConversion.LElbowRoll] = NAOConversion.convertAngle(NAOConversion.LElbowRoll,
                                                                                    Kinematic.getAngleNew(upperArmLeft, lowerArmLeft));
            //Right
            this.jointAngles[NAOConversion.RElbowRoll] = NAOConversion.convertAngle(NAOConversion.RElbowRoll,
                                                                                    Kinematic.getAngleNew(upperArmRight, lowerArmRight));

            //Elbow Yaw
            ////Left
            Vecto3Float shoulderLTohipCenter = new Vecto3Float(shoulderLeft.X - hipCenter.X,
                                                               shoulderLeft.Y - hipCenter.Y,
                                                               shoulderLeft.Z - hipCenter.Z);
            Vecto3Float lCrossCenterArm = upperArmLeft.cross(shoulderLTohipCenter);
            Vecto3Float lCrossArms      = upperArmLeft.cross(lowerArmLeft);
            float       lElbowYaw       = Kinematic.getAngleNew(lCrossCenterArm, lCrossArms);

            this.jointAngles[NAOConversion.LElbowYaw] = NAOConversion.convertAngle(NAOConversion.LElbowYaw, lElbowYaw);


            //Right
            Vecto3Float shoulderRTohipCenter = new Vecto3Float(shoulderRight.X - hipCenter.X,
                                                               shoulderRight.Y - hipCenter.Y,
                                                               shoulderRight.Z - hipCenter.Z);
            Vecto3Float rCrossCenterArm = upperArmRight.cross(shoulderRTohipCenter);
            Vecto3Float rCrossArms      = upperArmRight.cross(lowerArmRight);
            float       rElbowYaw       = Kinematic.getAngleNew(rCrossCenterArm, rCrossArms);

            this.jointAngles[NAOConversion.RElbowYaw] = NAOConversion.convertAngle(NAOConversion.RElbowYaw, rElbowYaw);
        }