運動学プログラミング

目次

順運動学の実装(シミュレータ)

概要
順運動学のプログラミングをシミュレータ上のロボットを使って演習する.
  1. ロボットアームRM3の順運動学をプログラムせよ.
    • 任意の関節角 (q1,q2,q3) に対してエンドエフェクタの3次元座標および姿勢を計算するプログラムを書け.
    • 計算したエンドエフェクタの位置に半透明のボックスを描画し,計算結果が正しいことを確かめよ.
    • 計算には同次変換行列を用いること(手計算で展開する必要はない).
    • 適当にロボットを動かし,任意の姿勢に対して計算できることを確かめよ.

逆運動学の実装(シミュレータ)

概要
逆運動学のプログラミングをシミュレータ上のロボットを使って演習する.

数値的解法編か解析的解法編を選んで解くこと.

数値的解法編

  1. ヤコビ行列を用いた逆キネマティクスの数値解法アルゴリズムを書き下せ.
  2. ロボットアームRM3の関節角とエンドエフェクタの位置について,ヤコビ行列を計算せよ.
    • ヤコビ行列が計算されていることを可視化し,確かめよ.
  3. ロボットアームRM3の逆運動学(数値的解法)をプログラムせよ.
    • 任意の3次元位置を指定したときに,それを実現する関節角を計算するプログラムを書け.
    • 目標位置にボックスを描画し,さらに計算した関節角の姿勢をロボットに取らせて,計算結果が正しいことを確かめよ.
    • キーボード入力で目標位置を変更できるようにし,任意の目標位置に対して計算できているか検証せよ.
  • 参考資料:
    • 梶田: ヒューマノイドロボット, オーム社, 2005.

解析的解法編

  1. ロボットアームRM3の逆運動学(解析的解法)をプログラムせよ.
    • 任意の3次元位置を指定したときに,それを実現する関節角を計算するプログラムを書け.
    • 目標位置にボックスを描画し,さらに計算した関節角の姿勢をロボットに取らせて,計算結果が正しいことを確かめよ.
    • キーボード入力で目標位置を変更できるようにし,任意の目標位置に対して計算できているか検証せよ.
  • 参考資料:
    • 梶田: ヒューマノイドロボット, オーム社, 2005.

逆運動学の実装(実機)

概要
実機のロボットについて,逆運動学(いずれの解法でもよい)をプログラムする.
  1. 各自で選択した実機のロボットに対して,逆運動学を実装せよ.






ヒント集

擬似逆行列の計算方法

Eigen を使う場合

Eigen のテキスト を参照.

サンプルプログラム:

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
#include <Eigen/Dense>
template <typename t_matrix>
t_matrix PseudoInverse(const t_matrix& m, const double &tolerance=1.e-6)
{
  using namespace Eigen;
  typedef JacobiSVD<t_matrix> TSVD;
  unsigned int svd_opt(ComputeThinU | ComputeThinV);
  if(m.RowsAtCompileTime!=Dynamic || m.ColsAtCompileTime!=Dynamic)
    svd_opt= ComputeFullU | ComputeFullV;
  TSVD svd(m, svd_opt);
  const typename TSVD::SingularValuesType &sigma(svd.singularValues());
  typename TSVD::SingularValuesType sigma_inv(sigma.size());

  for(long i=0; i<sigma.size(); ++i)
  {
    if(sigma(i) > tolerance)
      sigma_inv(i)= 1.0/sigma(i);
    else
      sigma_inv(i)= 0.0;
  }
  return svd.matrixV()*sigma_inv.asDiagonal()*svd.matrixU().transpose();
}

#include <iostream>
using namespace Eigen;
using namespace std;

int main(int,char**)
{
  MatrixXd m(2,3);
  m<< 1,2,3,
      3,2,1;
  cout<<"m= "<<endl<<m<<endl;
  cout<<"PseudoInverse(m)= "<<endl<<PseudoInverse(m)<<endl;
  cout<<"m*PseudoInverse(m)= "<<endl<<m*PseudoInverse(m)<<endl;
  return 0;
}

コンパイル:

g++ -g -Wall -O3 pinv.cpp -I/usr/include/eigen3


OpenCV を使う場合

OpenCV のテキスト を参照.

サンプルプログラム:

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
#include <opencv/cv.h>
#include <iostream>
using namespace cv;
using namespace std;

#define _M(x_m,r,c) (x_m.at<double>(r,c))

std::ostream& operator<<(std::ostream &lhs, const cv::Mat &rhs)
{
  if(rhs.type()!=CV_64FC1)  {lhs<<"---"<<endl;return lhs;}
  for(int r(0); r< rhs.rows; ++r)
  {
    const double *ptr= rhs.ptr<double>(r);
    for(int c(0); c< rhs.cols; ++c,++ptr)
      cout<<" "<<(*ptr);
    cout<<endl;
  }
  return lhs;
}

int main(int,char**)
{
  Mat m(3,4,CV_64FC1); // 3x4,double,1-channel
  _M(m,0,0)=3; _M(m,0,1)=1;  _M(m,0,2)=3;  _M(m,0,3)=12;
  _M(m,1,0)=2; _M(m,1,1)=3;  _M(m,1,2)=3;  _M(m,1,3)=0;
  _M(m,2,0)=1; _M(m,2,1)=-2; _M(m,2,2)=10; _M(m,2,3)=1;

  cout<<"m="<<endl<<m<<endl;
  cout<<"pinv(m)="<<endl<<m.inv(DECOMP_SVD)<<endl;
  cout<<"m*pinv(m)="<<endl<<m*m.inv(DECOMP_SVD)<<endl;
  return 0;
}

コンパイル:

g++ -g -Wall -O3 sample-pinv-opencv.cpp  -I/usr/include `pkg-config opencv --cflags` -lm -L/usr/local/lib `pkg-config opencv --libs`


liboctave を使う場合

liboctave のテキスト を参照.

サンプルプログラム:

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
int main()
#include <octave/config.h>
#include <octave/Matrix.h>
#include <iostream>
using namespace std;

int main(int,char**)
{
  Matrix m(3,4);
  m(0,0)=3; m(0,1)=1; m(0,2)=3; m(0,3)=12;
  m(1,0)=2; m(1,1)=3; m(1,2)=3; m(1,3)=0;
  m(2,0)=1; m(2,1)=-2; m(2,2)=10; m(2,3)=1;
  cout<<"m="<<endl<<m<<endl;
  cout<<"pinv(m)="<<endl<<m.pseudo_inverse()<<endl;
  cout<<"m*pinv(m)="<<endl<<m*m.pseudo_inverse()<<endl;
  return 0;
}

コンパイル:

g++ -g -Wall -O3 sample-pinv-octave.cpp  -I/usr/include -I/usr/local/include -I/usr/include/octave-`octave-config -v` -lm -L/usr/lib/octave-`octave-config -v` -loctave -lcruft -Wl,-rpath /usr/lib/octave-`octave-config -v` -ldl


逆運動学の数値的解法アルゴリズム

以下のコードはあくまで一例なので,各自のコードと完全に合致するわけではない.

まったく手がつかない人のためのサンプル.

length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1360.
length() used on @lines (did you mean "scalar(@lines)"?) at /usr/bin/code2html line 1370.
//!\brief execute inverse kinematics (from current pose)
void TKinematicRobot::ExecInverseKinematics (
    const DReal &tol=1.0e-6,
    int MaxItr=100,
    const DReal &step_size=0.2)
{
  int count(0);
  initialize_error_vector (error vector);
  get_error_vector (err_vector);
  do
  {
    get_current_jacobian (J);
    ReviseJacobian(J);  // optional; for singular matrix case
    dq= J.pseudo_inverse() * err_vector;
    dq*= step_size;
    add_to_joint_angles (dq);
    get_error_vector (err_vector);
    ++count;
  } while (GetNorm(err_vector)>tol && count<MaxItr);
}

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2018-08-30 (木) 07:17:06 (75d)