ロボティクス特論I

Advanced Robotics I

講義の概要については,シラバスを参照のこと.

About the outline of this course, please refer to the syllabus.

内容

  • forward kinematics
  • #ref(Li_FK.cpp)
    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.
      1 #include "simpleode/lib/arm.h"
      2 #include <iostream>
      3 #include <Eigen/Dense>
      4 using namespace std;
      5 using namespace Eigen;
      6
      7 static double targetAngle[3];
      8 static double actuatorPos[3];
      9 static double actuatorRot[12];
     10 static const Vector4d links((Vector4d() << 0.1, 0.1, 0.6, 0.6).finished());
     11
     12 //------------------------------------------------------------------------
     13
     14 //Translation 
     15 Matrix4d trans(const double& x, const double& y, const double& z){
     16     Matrix4d trans;
     17     trans << 1.0, 0.0, 0.0, x,
     18              0.0, 1.0, 0.0, y,
     19              0.0, 0.0, 1.0, z,
     20              0.0, 0.0, 0.0, 1.0;
     21     return trans;
     22 }
     23 //------------------------------------------------------------------------
     24
     25 //Euler angles for rotation
     26 Matrix4d row(const double& theta){
     27     Matrix4d row;
     28     row << 1.0, 0.0, 0.0, 0.0,
     29              0.0, cos(theta), -sin(theta), 0.0,
     30              0.0, sin(theta), cos(theta), 0.0,
     31              0.0, 0.0, 0.0, 1.0;
     32     return row;
     33 }
     34
     35 Matrix4d yaw(const double& phi){
     36     Matrix4d yaw;
     37     yaw << cos(phi), 0.0, sin(phi), 0.0,
     38            0.0, 1.0, 0.0, 0.0,
     39            -sin(phi), 0.0, cos(phi), 0.0,
     40            0.0, 0.0, 0.0, 1.0;
     41     return yaw;
     42 }
     43
     44 Matrix4d pitch(const double& psi){
     45     Matrix4d pitch;
     46     pitch << cos(psi), -sin(psi), 0.0, 0.0,
     47              sin(psi), cos(psi), 0.0, 0.0,
     48              0.0, 0.0, 1.0, 0.0,
     49              0.0, 0.0, 0.0, 1.0;
     50     return pitch;
     51 }
     52
     53 //------------------------------------------------------------------------
     54
     55 //FK matrix
     56 Matrix4d FKMat(const double& theta1, const double& theta2, const double& theta3){
     57     const double& l1 = links[0], l2 = links[1], l3 = links[2], l4 = links[3];
     58     return trans(0,0,l1) * pitch(theta1) * trans(0,0,l2) * yaw(theta2) * trans(0,0,l3) * yaw(theta3) * trans(0,0,l4);
     59 }
     60
     61 //------------------------------------------------------------------------
     62
     63 //ODE callbacks
     64 void test_ctrl(xode::TEnvironment& env, xode::TDynRobot& robot){
     65     const double kp(5.0);
     66     for(int i=0; i<3; ++i)
     67         robot.SetAngVelHinge(i, kp*( targetAngle[i] - robot.GetAngleHinge(i) ));
     68     Matrix4d actuator = FKMat(robot.GetAngleHinge(0), robot.GetAngleHinge(1), robot.GetAngleHinge(2));
     69     for(int i=0; i<3; ++i){
     70         for(int j=0; j<3; ++j)
     71             actuatorRot[i*4+j] = actuator(i, j);
     72         actuatorPos[i] = actuator(i, 3);
     73     }
     74     cout<<robot.GetAngleHinge(0)<<" "<<robot.GetAngleHinge(1)<<" "<<robot.GetAngleHinge(2)<<endl;
     75 }
     76
     77 void test_draw(xode::TEnvironment& env, xode::TDynRobot& robot){
     78     dReal sides[3] = {0.3, 0.3, 0.3};
     79     dsSetColorAlpha(0.0, 1.0, 1.0, 0.5);
     80     dsDrawBox(actuatorPos, actuatorRot, sides);
     81     dsSetColorAlpha(1.0, 0.0, 1.0, 0.5);
     82 }
     83
     84 void test_keyevent(xode::TEnvironment& env, xode::TDynRobot& robot, int cmd){
     85     dReal d(0.05);
     86     switch(cmd){
     87         case 'd': targetAngle[0]+=d; break;
     88         case 'a': targetAngle[0]-=d; break;
     89         case 'e': targetAngle[1]+=d; break;
     90         case 'q': targetAngle[1]-=d; break;
     91         case 'w': targetAngle[2]+=d; break;
     92         case 's': targetAngle[2]-=d; break;
     93     }
     94 }
     95
     96 //------------------------------------------------------------------------
     97
     98 int main(int argc, char **argv){
     99     xode::TEnvironment env;
    100     dsFunctions fn = xode::SimInit("textures", env);
    101     xode::ControlCallback = &test_ctrl;
    102     xode::DrawCallback = &test_draw;
    103     xode::KeyEventCallback = &test_keyevent;
    104     dsSimulationLoop(argc, argv, 400, 400, &fn);
    105     dCloseODE();
    106     return 0;
    107 }
  • inverse kinematics
  • #ref(Li_IK.cpp)
    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.
      1 #include "simpleode/lib/arm.h"
      2 #include <iostream>
      3 #include <Eigen/Dense>
      4 #include <Eigen/QR>  
      5 using namespace std;
      6 using namespace Eigen;
      7
      8 static double actuatorPos[3];
      9 static double actuatorRot[12];
     10 static bool initialized = false;
     11 static const Vector4d links((Vector4d() << 0.1, 0.1, 0.6, 0.6).finished());
     12
     13 //------------------------------------------------------------------------
     14
     15 //pseudoInverse
     16 template <typename t_matrix>
     17 t_matrix PseudoInverse(const t_matrix& m, const double &tolerance=1.e-6){
     18     using namespace Eigen;
     19     typedef JacobiSVD<t_matrix> TSVD;
     20     unsigned int svd_opt(ComputeThinU | ComputeThinV);
     21     if(m.RowsAtCompileTime!=Dynamic || m.ColsAtCompileTime!=Dynamic)
     22         svd_opt=ComputeFullU | ComputeFullV;
     23     TSVD svd(m, svd_opt);
     24     const typename TSVD::SingularValuesType &sigma(svd.singularValues());
     25     typename TSVD::SingularValuesType sigma_inv(sigma.size());
     26     for(long i=0; i<sigma.size(); i++){
     27         if(sigma(i) > tolerance)
     28             sigma_inv(i) = 1.0/sigma(i);
     29         else
     30             sigma_inv(i) = 0.0;
     31     }
     32     return svd.matrixV()*sigma_inv.asDiagonal()*svd.matrixU().transpose();
     33 }
     34
     35 //------------------------------------------------------------------------
     36
     37 //Translation 
     38 Matrix4d trans(const double& x, const double& y, const double& z){
     39     Matrix4d trans;
     40     trans << 1.0, 0.0, 0.0, x,
     41              0.0, 1.0, 0.0, y,
     42              0.0, 0.0, 1.0, z,
     43              0.0, 0.0, 0.0, 1.0;
     44     return trans;
     45 }
     46 //------------------------------------------------------------------------
     47
     48 //Euler angles for rotation
     49 Matrix4d row(const double& theta){
     50     Matrix4d row;
     51     row << 1.0, 0.0, 0.0, 0.0,
     52              0.0, cos(theta), -sin(theta), 0.0,
     53              0.0, sin(theta), cos(theta), 0.0,
     54              0.0, 0.0, 0.0, 1.0;
     55     return row;
     56 }
     57
     58 Matrix4d yaw(const double& phi){
     59     Matrix4d yaw;
     60     yaw << cos(phi), 0.0, sin(phi), 0.0,
     61            0.0, 1.0, 0.0, 0.0,
     62            -sin(phi), 0.0, cos(phi), 0.0,
     63            0.0, 0.0, 0.0, 1.0;
     64     return yaw;
     65 }
     66
     67 Matrix4d pitch(const double& psi){
     68     Matrix4d pitch;
     69     pitch << cos(psi), -sin(psi), 0.0, 0.0,
     70              sin(psi), cos(psi), 0.0, 0.0,
     71              0.0, 0.0, 1.0, 0.0,
     72              0.0, 0.0, 0.0, 1.0;
     73     return pitch;
     74 }
     75
     76 //------------------------------------------------------------------------
     77
     78 //FK matrix
     79 Matrix4d FKMat(const double& theta1, const double& theta2, const double& theta3){
     80     const double& l1 = links[0], l2 = links[1], l3 = links[2], l4 = links[3];
     81     return trans(0,0,l1) * pitch(theta1) * trans(0,0,l2) * yaw(theta2) * trans(0,0,l3) * yaw(theta3) * trans(0,0,l4);
     82 }
     83
     84 //------------------------------------------------------------------------
     85
     86 //calculate Jacobian of the actuator
     87 Matrix3d calcJacobian(const Vector3d& q){
     88     Matrix4d mat;
     89     Vector3d jointRot1, jointRot2, jointRot3;
     90     Vector3d jointPos1, jointPos2, jointPos3, actuatorPos;
     91     Vector3d jointDiff1, jointDiff2, jointDiff3;
     92     Vector3d jac1, jac2, jac3;
     93     Matrix3d jacobian;
     94     //joint1
     95     mat = trans(0, 0, links[0]) * pitch(q[0]);
     96     jointRot1 = mat.block(0, 2, 3, 1);
     97     jointPos1 = mat.block(0, 3, 3, 1);
     98     //joint2
     99     mat = mat * trans(0, 0, links[1]) * yaw(q[1]);
    100     jointRot2 = mat.block(0, 1, 3, 1);
    101     jointPos2 = mat.block(0, 3, 3, 1);
    102     //joint3
    103     mat = mat * trans(0, 0, links[2]) * yaw(q[2]);
    104     jointRot3 = mat.block(0, 1, 3, 1);
    105     jointPos3 = mat.block(0, 3, 3, 1);
    106     mat = mat * trans(0, 0, links[3]);
    107     actuatorPos = mat.block(0, 3, 3, 1);
    108     jointDiff1 = actuatorPos - jointPos1;
    109     jointDiff2 = actuatorPos - jointPos2;
    110     jointDiff3 = actuatorPos - jointPos3;
    111     jac1 = jointRot1.cross(jointDiff1);
    112     jac2 = jointRot2.cross(jointDiff2);
    113     jac3 = jointRot3.cross(jointDiff3);
    114     jacobian << jac1, jac2, jac3;
    115     return jacobian;
    116 }
    117
    118 //------------------------------------------------------------------------
    119
    120 Vector3d IKsolver(const Vector3d& q, const Vector3d& actuatorPos){
    121     const double ERR_THRESH = 1e-6;
    122     const int MAX_ITER = 10000;
    123     int iter = 0;
    124     Matrix4d actuator = FKMat(q(0), q(1), q(2));
    125     Vector3d currPos = actuator.block(0, 3, 3, 1);
    126     Vector3d diffPos = actuatorPos - currPos;
    127     Vector3d retQ = q;
    128     do{
    129         MatrixXd inverseJac = PseudoInverse(calcJacobian(retQ));
    130         Vector3d diffQ = inverseJac * diffPos;
    131         retQ += diffQ;
    132         actuator = FKMat(retQ(0), retQ(1), retQ(2));
    133         currPos = actuator.block(0, 3, 3, 1);
    134         diffPos = actuatorPos - currPos;
    135         ++iter;
    136     }while((iter < MAX_ITER) && (diffPos.norm() > ERR_THRESH));
    137     return retQ;
    138 }
    139
    140 //------------------------------------------------------------------------
    141
    142 //ODE callbacks
    143 void test_ctrl(xode::TEnvironment& env, xode::TDynRobot& robot){
    144     const double kp(5.0);
    145     if(!initialized){
    146         for(int i = 0; i < 3; ++i)
    147             actuatorPos[i] = robot.GetHandPos()[i];
    148         for(int i = 0; i < 12; ++i)
    149             actuatorRot[i] = robot.GetHandRot()[i];
    150         initialized = true;
    151     }
    152     Vector3d targetActuatorPos(actuatorPos);
    153     Vector3d q(robot.GetAngleHinge(0), robot.GetAngleHinge(1), robot.GetAngleHinge(2));
    154     Vector3d ret_q = IKsolver(q, targetActuatorPos);
    155     for(int i=0; i<3; ++i)
    156         robot.SetAngVelHinge(i, kp*(ret_q[i] - q[i]));
    157 }
    158
    159 void test_draw(xode::TEnvironment& env, xode::TDynRobot& robot){
    160     dReal sides[3] = {0.3, 0.3, 0.3};
    161     dsSetColorAlpha(0.0, 1.0, 1.0, 0.5);
    162     dsDrawBox(actuatorPos, actuatorRot, sides);
    163     dsSetColorAlpha(1.0, 0.0, 1.0, 0.5);
    164 }
    165
    166 void test_keyevent(xode::TEnvironment& env, xode::TDynRobot& robot, int cmd){
    167     dReal d(0.05);
    168     switch(cmd){
    169         case 'd': actuatorPos[0]+=d; break;
    170         case 'a': actuatorPos[0]-=d; break;
    171         case 'e': actuatorPos[1]+=d; break;
    172         case 'q': actuatorPos[1]-=d; break;
    173         case 'w': actuatorPos[2]+=d; break;
    174         case 's': actuatorPos[2]-=d; break;
    175     }
    176 }
    177
    178 //------------------------------------------------------------------------
    179
    180 int main(int argc, char **argv){
    181     xode::TEnvironment env;
    182     dsFunctions fn = xode::SimInit("textures", env);
    183     xode::ControlCallback = &test_ctrl;
    184     xode::DrawCallback = &test_draw;
    185     xode::KeyEventCallback = &test_keyevent;
    186     dsSimulationLoop(argc, argv, 400, 400, &fn);
    187     dCloseODE();
    188     return 0;
    189 }

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