/// <summary>
/// 関節角度から順運動学を解き、各種状態変数を更新
/// </summary>
/// <param name="jointAngle">関節角度配列[rad]、要素数6</param>
public void UpdateByFK(double[] jointAngle)
{
if (jointAngle.Length != 6)
{
throw new ArgumentException();
}
this.jointAngle = jointAngle;
this.linkPos = arm.SolveFK(this.jointAngle, out this.rotMat);
this.endPos = this.linkPos[6];
this.coordinateAngle = ConvertRotMatToEuler(this.rotMat, this.method);
}
/// <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); //手先姿勢計算
}