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