move_ankle.cpp
Go to the documentation of this file.
2 #include <hrpUtil/Eigen3d.h>
3 #include <hrpModel/Link.h>
5 #include <hrpModel/JointPath.h>
7 
8 using namespace std;
9 using namespace hrp;
10 using namespace OpenHRP;
11 
12 enum {X, Y, Z};
13 #define deg2rad(x) ( 3.14159265358979 / 180*(x) )
14 
15 int
16 main(int argc, char* argv[])
17 {
18  int i;
19  string url = "file://";
20  // -urlでモデルのURLを指定 //
21  for(i=0; i < argc; i++) {
22  if( strcmp(argv[i], "-url") == 0 && i+1 < argc) url += argv[i+1];
23  }
24 
25  // モデルロード //
26  BodyPtr body(new Body());
27  if(!loadBodyFromModelLoader(body, url.c_str(), argc, argv)){
28  cerr << "ModelLoader: " << url << " cannot be loaded" << endl;
29  return 0;
30  }
31 
32  body->calcForwardKinematics();
33 
34  // OnlineViewer設定 //
35  OnlineViewer_var olv = getOnlineViewer(argc, argv);
36  try {
37  olv->load(body->modelName().c_str(), url.c_str());
38  olv->setLogName("move_ankle");
39  } catch (CORBA::SystemException& ex) {
40  cerr << "Failed to connect GrxUI." << endl;
41  return 1;
42  }
43 
44  // 特異点にならないよう最初は曲げておく //
45  body->joint(1)->q = deg2rad(-10);
46  body->joint(3)->q = deg2rad(20);
47  body->joint(4)->q = deg2rad(-10);
48  body->calcForwardKinematics();
49 
50  // 腰から足首までのパスを設定 //
51  Link* waist = body->link("WAIST");
52  Link* ankle = body->link("RLEG_ANKLE_R");
53  JointPathPtr path = body->getJointPath(waist, ankle);
54 
55  // WorldStateを作成する //
56  WorldState world;
57  world.characterPositions.length(1);
58  world.collisions.length(0);
59 
60  // SampleRobot用CharacterPosition //
61  //world.collisions.length(0);
62  CharacterPosition& robot = world.characterPositions[0];
63  robot.characterName = CORBA::string_dup("SampleRobot");
64 
65  // 時間は0 //
66  world.time=0;
67 
68  while (1) {
69  // 時間を進める //
70  world.time+=0.01;
71 
72  // 少し動かす //
73  Vector3 p = ankle->p;
74  Matrix33 R = ankle->R;
75  p(2) += 0.002;
76 
77  // もし逆運動学計算に失敗したら終わり //
78  if (!path->calcInverseKinematics(p, R)) {
79  break;
80  }
81 
82  // LinkをWorldStateにコピーする。 //
83  int n = body->numLinks();
84  robot.linkPositions.length(n);
85  for (int i=0; i<n; i++) {
86  Link* link = body->link(i);
87  setVector3(link->p, robot.linkPositions[i].p);
88  setMatrix33ToRowMajorArray(link->R, robot.linkPositions[i].R);
89  }
90 
91  // OnlineViewer アップデート //
92  try {
93  olv->update(world);
94  } catch (CORBA::SystemException& ex) {
95  std::cerr << "OnlineViewer could not be updated." << endl;
96  return 1;
97  }
98  }
99 
100  return 0;
101 }
The header file of the LinkTraverse class.
Definition: move_ankle.cpp:12
Definition: move_ankle.cpp:12
void setVector3(const Vector3 &v3, V &v, size_t top=0)
Definition: Eigen3d.h:130
png_uint_32 i
Definition: png.h:2735
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
void setMatrix33ToRowMajorArray(const Matrix33 &m33, Array &a, size_t top=0)
Definition: Eigen3d.h:105
#define deg2rad(x)
Definition: move_ankle.cpp:13
The header file of the LinkPath and JointPath classes.
int main(int argc, char *argv[])
Definition: move_ankle.cpp:16
path
Definition: Body.h:44
boost::shared_ptr< JointPath > JointPathPtr
Definition: Body.h:29
HRP_UTIL_EXPORT OpenHRP::OnlineViewer_var getOnlineViewer(int argc, char **argv)
Definition: move_ankle.cpp:12
HRPMODEL_API bool loadBodyFromModelLoader(BodyPtr body, const char *url, CORBA_ORB_var orb, bool loadGeometryForCollisionDetection=false)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04