/// <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); //手先姿勢計算 }