/// <summary>
/// テキストボックスから手先位置を読み取り
/// </summary>
/// <returns>手先位置構造体</returns>
public Kinematics.Position ReadEndPosFromTextBox()
{
Kinematics.Position endPos = new Kinematics.Position(
double.Parse(txtbPosX.Text),
double.Parse(txtbPosY.Text),
double.Parse(txtbPosZ.Text));
return(endPos);
}
/// <summary>
/// 入力された手先位置構造体、3x3姿勢行列から逆運動学を解き各種状態変数を更新
/// </summary>
/// <param name="endPos">手先位置構造体</param>
/// <param name="coordinateAngle">オイラー角(x,y,zの順)</param>
public void UpdateByIK(Kinematics.Position endPos, double[] coordinateAngle)
{
if (coordinateAngle.Length != 3)
{
throw new ArgumentException();
}
double[,] rotMat = new double[3, 3];
rotMat = ConvertEulerToRotMat(coordinateAngle, this.method);
UpdateByIK(endPos, rotMat);
}
/// <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); //手先姿勢計算
}
/// <summary>
/// 入力された手先位置構造体、3x3姿勢行列から逆運動学を解き各種状態変数を更新
/// </summary>
/// <param name="endPos">手先位置構造体</param>
/// <param name="rotMat">3x3姿勢行列</param>
public void UpdateByIK(Kinematics.Position endPos, double[,] rotMat)
{
if (rotMat.GetLength(0) != 3 || rotMat.GetLength(1) != 3)
{
throw new ArgumentException();
}
//try
//{
this.jointAngle = arm.SolveIK(endPos, rotMat);
this.endPos = endPos;
this.rotMat = rotMat;
//}
//catch(Exception)
//{
// throw new ArgumentOutOfRangeException();
//}
this.linkPos = arm.SolveFK(this.jointAngle);
this.endPos = this.linkPos[6];
this.coordinateAngle = ConvertRotMatToEuler(this.rotMat, this.method);
}
/// <summary>
/// テキストボックスに手先位置を表示
/// </summary>
/// <param name="endPos">手先位置構造体</param>
public void WriteEndPosToTextBox(Kinematics.Position endPos)
{
txtbPosX.Text = endPos.x.ToString("F2");
txtbPosY.Text = endPos.y.ToString("F2");
txtbPosZ.Text = endPos.z.ToString("F2");
}