/// <summary>
 /// コンストラクタ
 /// </summary>
 /// <param name="linkLength">リンク長配列、要素数7</param>
 /// <param name="method">姿勢角表現方法列挙体</param>
 public RobotArmState(double[] linkLength, EXPRESSION_METHOD method)
 {
     if (linkLength.Length != 7)
     {
         throw new ArgumentException();
     }
     this.method          = method;                            //姿勢角表現方法の保存
     arm                  = new Kinematics(linkLength);
     this.jointAngle      = new double[] { 0, 0, 0, 0, 0, 0 }; //関節角度初期化
     this.rotMat          = new double[3, 3];
     this.linkPos         = new Kinematics.Position[7];
     this.linkPos         = arm.SolveFK(this.jointAngle, out rotMat); //リンク位置および手先の姿勢行列計算
     this.endPos          = this.linkPos[6];                          //手先の位置
     this.coordinateAngle = new double[] { 0, 0, 0 };
     this.coordinateAngle = ConvertRotMatToEuler(rotMat, method);     //手先姿勢計算
 }
 /// <summary>
 /// 指定された表現のオイラー角を3x3姿勢行列に変換
 /// </summary>
 /// <param name="rotAngle"></param>
 /// <param name="method"></param>
 /// <returns></returns>
 private double[,] ConvertEulerToRotMat(double[] rotAngle, EXPRESSION_METHOD method)
 {
     if (rotAngle.Length != 3)
     {
         throw new ArgumentException();
     }
     double[,] rotMat = new double[3, 3];
     switch (method)
     {
     case EXPRESSION_METHOD.EULER_ZYX:
         rotMat = (Kinematics.GenerateRotMat(rotAngle[0], Kinematics.AXIS.X) *
                   Kinematics.GenerateRotMat(rotAngle[1], Kinematics.AXIS.Y) *
                   Kinematics.GenerateRotMat(rotAngle[2], Kinematics.AXIS.Z)).ToArray();
         break;
     }
     return(rotMat);
 }
 /// <summary>
 /// 3x3姿勢行列を指定された表現のオイラー角に変換
 /// </summary>
 /// <param name="rotMat">3x3姿勢行列</param>
 /// <param name="method">姿勢角表現方法列挙体</param>
 /// <returns>オイラー角配列[rad](x,y,zの順)</returns>
 private double[] ConvertRotMatToEuler(double[,] rotMat, EXPRESSION_METHOD method)
 {
     if (rotMat.GetLength(0) != 3 || rotMat.GetLength(1) != 3)
     {
         throw new ArgumentException();
     }
     double[] rotAngle = new double[3];
     switch (method)
     {
     case EXPRESSION_METHOD.EULER_ZYX:
         rotAngle[0] = -Math.Atan2(rotMat[1, 2], rotMat[2, 2]);
         rotAngle[1] = -Math.Atan2(-rotMat[0, 2], Math.Sqrt(rotMat[1, 2] * rotMat[1, 2] + rotMat[2, 2] * rotMat[2, 2]));
         rotAngle[2] = -Math.Atan2(rotMat[0, 1], rotMat[0, 0]);
         break;
     }
     return(rotAngle);
 }