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