コード例 #1
0
 /// <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);
 }
コード例 #2
0
 /// <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);
 }
コード例 #3
0
 /// <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);
 }
コード例 #4
0
 /// <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);     //手先姿勢計算
 }
コード例 #5
0
        /// <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);
        }
コード例 #6
0
 /// <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");
 }