server/UtDynamicsSimulator/ModelLoaderUtil.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: nil; tab-width: 4; c-basic-offset: 4; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * The University of Tokyo
9  * National Institute of Advanced Industrial Science and Technology (AIST)
10  * General Robotix Inc.
11  */
16 #include <hrpCorba/OpenHRPCommon.hh>
17 #include <stack>
18 #include "ModelLoaderUtil.h"
19 #include "Sensor.h"
20 
21 #include "World.h"
22 #include "psim.h"
23 
24 using namespace OpenHRP;
25 using namespace std;
26 
27 //#include <fstream>
28 //static std::ofstream logfile("model.log");
29 //static std::ofstream logfile;
30 
31 static void array_to_mat33(const DblArray4& a, fMat33& mat)
32 {
33  const double sth = sin(a[3]);
34  const double vth = 1.0 - cos(a[3]);
35 
36  double ax = a[0];
37  double ay = a[1];
38  double az = a[2];
39 
40  const double axx = ax*ax*vth;
41  const double ayy = ay*ay*vth;
42  const double azz = az*az*vth;
43  const double axy = ax*ay*vth;
44  const double ayz = ay*az*vth;
45  const double azx = az*ax*vth;
46 
47  ax *= sth;
48  ay *= sth;
49  az *= sth;
50 
51  mat(0,0) = 1.0 - azz - ayy;
52  mat(0,1) = -az + axy;
53  mat(0,2) = ay + azx;
54  mat(1,0) = az + axy;
55  mat(1,1) = 1.0 - azz - axx;
56  mat(1,2) = -ax + ayz;
57  mat(2,0) = -ay + azx;
58  mat(2,1) = ax + ayz;
59  mat(2,2) = 1.0 - ayy - axx;
60 }
61 
62 static void array_to_mat33(const DblArray9& a, fMat33& mat)
63 {
64  mat(0,0) = a[0];
65  mat(1,0) = a[1];
66  mat(2,0) = a[2];
67  mat(0,1) = a[3];
68  mat(1,1) = a[4];
69  mat(2,1) = a[5];
70  mat(0,2) = a[6];
71  mat(1,2) = a[7];
72  mat(2,2) = a[8];
73 }
74 
75 static void array_to_vec3(const DblArray3& a, fVec3& vec)
76 {
77  vec(0) = a[0];
78  vec(1) = a[1];
79  vec(2) = a[2];
80 }
81 
82 static void createSensors(::World* world, Joint* jnt, SensorInfoSequence iSensors)
83 {
84  int numSensors = iSensors.length();
85 
86  for(int i=0; i < numSensors; ++i)
87  {
88  SensorInfo iSensor = iSensors[i];
89 
90  //int id = iSensor->id();
91  int id = iSensor.id;
92 
93  if(id < 0){
94  std::cerr << "Warning: sensor ID is not given to sensor " << iSensor.name
95  << "of character " << jnt->CharName() << "." << std::endl;
96  } else {
97 
98  int sensorType = Sensor::COMMON;
99 
100  //switch(iSensor.type) {
101  //case ::FORCE_SENSOR: sensorType = Sensor::FORCE; break;
102  //case ::RATE_GYRO: sensorType = Sensor::RATE_GYRO; break;
103  //case ::ACCELERATION_SENSOR: sensorType = Sensor::ACCELERATION; break;
104  //case ::PRESSURE_SENSOR: sensorType = Sensor::PRESSURE; break;
105  //case ::PHOTO_INTERRUPTER: sensorType = Sensor::PHOTO_INTERRUPTER; break;
106  //case ::VISION_SENSOR: sensorType = Sensor::VISION; break;
107  //case ::TORQUE_SENSOR: sensorType = Sensor::TORQUE; break;
108  //}
109 
110  CORBA::String_var type0 = iSensor.type;
111  string type(type0);
112 
113  if( type == "Force" ) { sensorType = Sensor::FORCE; } // 6Ž²ÍEZ¥ó¥µ
114  else if( type == "RateGyro" ) { sensorType = Sensor::RATE_GYRO; } // ¥E¼¥È¥¸¥ã¥¤¥úÁ»¥ó¥µ
115  else if( type == "Acceleration" ) { sensorType = Sensor::ACCELERATION; } // ±E¡¦x¥»¥ó¥µ
116  else if( type == "Vision" ) { sensorType = Sensor::VISION; } // ¥Ó¥¸¥ç¥ó¥»¥ó¥µ
117 
118 
119  CORBA::String_var name0 = iSensor.name;
120  string name(name0);
121  const DblArray3& p = iSensor.translation;
122  static fVec3 localPos;
123  static fMat33 localR;
124  array_to_vec3(p, localPos);
125  const DblArray4& rot = iSensor.rotation;
126  array_to_mat33(rot, localR);
127  world->addSensor(jnt, sensorType, id, name, localPos, localR);
128  }
129  }
130 }
131 
132 static inline double getLimitValue(DblSequence_var limitseq, double defaultValue)
133 {
134  return (limitseq->length() == 0) ? defaultValue : limitseq[0];
135 }
136 
137 
138 static Joint* createLink(::World* world, const char* charname, int index, LinkInfoSequence_var iLinks, Joint* pjoint)
139 {
140  Chain* _chain = (Chain*)world->Chain();
141  LinkInfo iLink = iLinks[index];
142 
143 // logfile << "create: " << iLink->name() << ", jointId = " << iLink->jointId() << endl;
144  CORBA::String_var name = iLink.name;
145  std::string myname;
146  char sep[2];
147  sep[0] = charname_separator;
148  sep[1] = '\0';
149  myname = std::string(name) + std::string(sep) + std::string(charname);
150  Joint* jnt = new Joint(myname.c_str());
151 
152  _chain->AddJoint(jnt, pjoint);
153 
154  int jointId = iLink.jointId;
155  jnt->i_joint = jointId;
156 
157  CORBA::String_var jointType = iLink.jointType;
158  const std::string jt(jointType);
159 
160  if(jt == "fixed")
161  {
162  jnt->j_type = ::JFIXED;
163  }
164  else if(jt == "free")
165  {
166  jnt->j_type = ::JFREE;
167  }
168  else if(jt == "rotate")
169  {
170  jnt->j_type = ::JROTATE;
171  }
172  else if(jt == "slide")
173  {
174  jnt->j_type = ::JSLIDE;
175  }
176  else
177  {
178  jnt->j_type = ::JFREE;
179  }
180 
181  if(jointId < 0)
182  {
183  if(jnt->j_type == ::JROTATE || jnt->j_type == ::JSLIDE)
184  {
185  std::cerr << "Warning: Joint ID is not given to joint " << jnt->name
186  << " of character " << charname << "." << std::endl;
187  }
188  }
189 
190  const DblArray3& t =iLink.translation;
191  static fVec3 rel_pos;
192  array_to_vec3(t, rel_pos);
193 
194  const DblArray4& r = iLink.rotation;
195  static fMat33 rel_att;
196  array_to_mat33(r, rel_att);
197 
198  // joint axis is always set to z axis; use init_att as the origin
199  // of the joint axis
200  if(jnt->j_type == ::JROTATE || jnt->j_type == ::JSLIDE)
201  {
202  const DblArray3& a = iLink.jointAxis;
203  static fVec3 loc_axis;
204  array_to_vec3(a, loc_axis);
205 // logfile << "loc_axis = " << loc_axis << endl;
206 // logfile << "rel_att = " << rel_att << endl;
207 // logfile << "rel_pos = " << rel_pos << endl;
208 #if 0
209  static fMat33 init_att;
210  static fVec3 p_axis;
211  p_axis.mul(rel_att, loc_axis); // joint axis in parent frame -> z axis
212  static fVec3 x, y;
213  x.set(1.0, 0.0, 0.0);
214  y.set(0.0, 1.0, 0.0);
215  double zx = p_axis*x;
216  x -= zx * p_axis;
217  double xlen = x.length();
218  if(xlen > 1e-8)
219  {
220  x /= xlen;
221  y.cross(p_axis, x);
222  }
223  else
224  {
225  double yz = y*p_axis;
226  y -= yz * p_axis;
227  double ylen = y.length();
228  y /= ylen;
229  x.cross(y, p_axis);
230  }
231  init_att(0,0) = x(0); init_att(1,0) = x(1); init_att(2,0) = x(2);
232  init_att(0,1) = y(0); init_att(1,1) = y(1); init_att(2,1) = y(2);
233  init_att(0,2) = p_axis(0); init_att(1,2) = p_axis(1); init_att(2,2) = p_axis(2);
234  if(jnt->j_type == JROTATE)
235  jnt->SetRotateJointType(rel_pos, init_att, AXIS_Z);
236  else if(jnt->j_type == JSLIDE)
237  jnt->SetSlideJointType(rel_pos, init_att, AXIS_Z);
238 // logfile << "init_att = " << init_att << endl;
239 #else
240  AxisIndex axis = AXIS_NULL;
241  if(loc_axis(0) > 0.95) axis = AXIS_X;
242  else if(loc_axis(1) > 0.95) axis = AXIS_Y;
243  else if(loc_axis(2) > 0.95) axis = AXIS_Z;
244  else{
245  std::cerr << "unsupported joint axis for " << myname
246  << ", only X, Y and Z axes are supported" << std::endl;
247  }
248  assert(axis != AXIS_NULL);
249  if(jnt->j_type == JROTATE)
250  jnt->SetRotateJointType(rel_pos, rel_att, axis);
251  else if(jnt->j_type == JSLIDE)
252  jnt->SetSlideJointType(rel_pos, rel_att, axis);
253 #endif
254 // logfile << "n_dof = " << jnt->n_dof << endl;
255  }
256  else if(jnt->j_type == ::JSPHERE)
257  {
258  jnt->SetSphereJointType(rel_pos, rel_att);
259  }
260  else if(jnt->j_type == ::JFIXED)
261  {
262  jnt->SetFixedJointType(rel_pos, rel_att);
263  }
264  else if(jnt->j_type == ::JFREE)
265  {
266 // logfile << "rel_pos = " << rel_pos << endl;
267 // logfile << "rel_att = " << rel_att << endl;
268  jnt->SetFreeJointType(rel_pos, rel_att);
269  }
270 
271  jnt->mass = iLink.mass;
272 
273  //double equivalentInertia = iLink.equivalentInertia();
274 
275  //if(equivalentInertia == 0.0){
276  // jnt->rotor_inertia = iLink.rotorInertia;
277  // jnt->gear_ratio = iLink.gearRatio;
278  //} else {
279  // //jnt->rotor_inertia = equivalentInertia;
280  // jnt->gear_ratio = 1.0;
281  //}
282 
283  //link->Jm2 = iLink.equivalentInertia();
284  //link->torqueConst = iLink.torqueConst();
285  //if (link->Jm2 == 0){
286  // link->Jm2 = link->Ir * link->gearRatio * link->gearRatio;
287  //}
288  //link->encoderPulse = iLink.encoderPulse();
289 
290  //DblSequence_var ulimit = iLink.ulimit();
291  //DblSequence_var llimit = iLink.llimit();
292  //DblSequence_var uvlimit = iLink.uvlimit();
293  //DblSequence_var lvlimit = iLink.lvlimit();
294 
295  //double maxlimit = numeric_limits<double>::max();
296 
297  //link->ulimit = getLimitValue(ulimit, +maxlimit);
298  //link->llimit = getLimitValue(llimit, -maxlimit);
299  //link->uvlimit = getLimitValue(uvlimit, +maxlimit);
300  //link->lvlimit = getLimitValue(lvlimit, -maxlimit);
301 
302  const DblArray3& rc = iLink.centerOfMass;
303  static fVec3 com;
304  array_to_vec3(rc, com);
305  jnt->loc_com.set(com);
306 
307  const DblArray9& I = iLink.inertia;
308  static fMat33 inertia;
309  array_to_mat33(I, inertia);
310  jnt->inertia.set(inertia);
311 
312  //int sindex = iLinks[index].sister();
313  // createLink(world, charname, sindex, iLinks, pjoint);
314 
315  for( unsigned int i = 0 ; i < iLinks[index].childIndices.length() ; i++ )
316  {
317  if( 0 <= iLinks[index].childIndices[i] )
318  {
319  createLink(world, charname, iLinks[index].childIndices[i], iLinks, jnt);
320  }
321  }
322 
323  createSensors(world, jnt, iLink.sensors);
324 
325  return jnt;
326 }
327 
328 
329 int loadBodyFromBodyInfo(::World* world, const char* _name, BodyInfo_ptr bodyInfo)
330 {
331 // logfile << "loadBody(" << _name << ")" << endl;
332  pSim* _chain = world->Chain();
333  _chain->BeginCreateChain(true);
334  // no root
335  if(!_chain->Root())
336  {
337  _chain->AddRoot("space");
338  }
339 
340  CORBA::String_var name = _name;
341 
342  int n = bodyInfo->links()->length();
343  LinkInfoSequence_var iLinks = bodyInfo->links();
344  int failed = true;
345 
346  for(int i=0; i < n; ++i)
347  {
348  if(iLinks[i].parentIndex < 0) // root of the character
349  {
350  static fMat33 Rs;
351  Rs.identity();
352  Joint* r = createLink(world, name, i, iLinks, _chain->Root());
353  world->addCharacter(r, _name, bodyInfo->links());
354  failed = false;
355  break;
356  }
357  }
358 #if 0
359  int rootIndex = -1;
360  if(body){
361  matrix33 Rs(tvmet::identity<matrix33>());
362  Link* rootLink = createLink(body, rootIndex, iLinks, Rs, importLinkShape);
363  body->setRootLink(rootLink);
364 
365  DblArray3_var p = iLinks[rootIndex]->translation();
366  vector3 pos(p[0u], p[1u], p[2u]);
367  const DblArray4 &R = iLinks[rootIndex]->rotation();
368  matrix33 att;
370  body->setDefaultRootPosition(pos, att);
371 
372  body->installCustomizer();
373 
374  body->initializeConfiguration();
375  }
376 #endif
377  _chain->EndCreateChain();
378 // logfile << "end of loadBody" << endl;
379 // logfile << "total dof = " << _chain->NumDOF() << endl;
380  return failed;
381 }
382 
383 
384 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, CosNaming::NamingContext_var cxt)
385 {
386  CosNaming::Name ncName;
387  ncName.length(1);
388  ncName[0].id = CORBA::string_dup("ModelLoader");
389  ncName[0].kind = CORBA::string_dup("");
390  ModelLoader_var modelLoader = NULL;
391  try {
392  modelLoader = ModelLoader::_narrow(cxt->resolve(ncName));
393  } catch(const CosNaming::NamingContext::NotFound &exc) {
394  std::cerr << "ModelLoader not found: ";
395  switch(exc.why) {
396  case CosNaming::NamingContext::missing_node:
397  std::cerr << "Missing Node" << std::endl;
398  case CosNaming::NamingContext::not_context:
399  std::cerr << "Not Context" << std::endl;
400  break;
401  case CosNaming::NamingContext::not_object:
402  std::cerr << "Not Object" << std::endl;
403  break;
404  }
405  return 0;
406  } catch(CosNaming::NamingContext::CannotProceed &exc) {
407  std::cerr << "Resolve ModelLoader CannotProceed" << std::endl;
408  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
409  std::cerr << "Resolve ModelLoader InvalidName" << std::endl;
410  }
411 
412  BodyInfo_var bodyInfo;
413  try {
414  bodyInfo = modelLoader->getBodyInfo(url);
415  } catch(CORBA::SystemException& ex) {
416  std::cerr << "CORBA::SystemException raised by ModelLoader: " << ex._rep_id() << std::endl;
417  return 0;
418  } catch(ModelLoader::ModelLoaderException& ex){
419  std::cerr << "ModelLoaderException ( " << ex.description << ") : " << ex.description << std::endl;
420  }
421 
422  if(CORBA::is_nil(bodyInfo)){
423  return 0;
424  }
425 
426  return loadBodyFromBodyInfo(world, name, bodyInfo);
427 }
428 
429 
430 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, CORBA_ORB_var orb)
431 {
432  CosNaming::NamingContext_var cxt;
433  try {
434  CORBA::Object_var nS = orb->resolve_initial_references("NameService");
435  cxt = CosNaming::NamingContext::_narrow(nS);
436  } catch(CORBA::SystemException& ex) {
437  std::cerr << "NameService doesn't exist" << std::endl;
438  return 0;
439  }
440 
441  return loadBodyFromModelLoader(world, name, url, cxt);
442 }
443 
444 
445 int loadBodyFromModelLoader(::World* world, const char* name, const char *url, int argc, char *argv[])
446 {
447  CORBA::ORB_var orb = CORBA::ORB_init(argc, argv);
448  return loadBodyFromModelLoader(world, name, url, orb);
449 }
450 
451 
452 int loadBodyFromModelLoader(::World* world, const char* name, const char *URL, istringstream &strm)
453 {
454  vector<string> argvec;
455  while (!strm.eof()){
456  string arg;
457  strm >> arg;
458  argvec.push_back(arg);
459  }
460  int argc = argvec.size();
461  char **argv = new char *[argc];
462  for (int i=0; i<argc; i++){
463  argv[i] = (char *)argvec[i].c_str();
464  }
465 
466  int ret = loadBodyFromModelLoader(world, name, URL, argc, argv);
467 
468  delete [] argv;
469 
470  return ret;
471 }
NotFound
Definition: hrpPrep.py:129
3x3 matrix class.
Definition: fMatrix3.h:29
double mass
mass
Definition: chain.h:704
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
* x
Definition: IceUtils.h:98
Joint * AddRoot(const char *name=0, const fVec3 &grav=fVec3(0.0, 0.0, 9.8))
Add the (unique) root joint.
Definition: edit.cpp:60
static void array_to_mat33(const DblArray4 &a, fMat33 &mat)
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
Forward dynamics computation based on Assembly-Disassembly Algorithm.
void addCharacter(Joint *rjoint, const std::string &_name, OpenHRP::LinkInfoSequence_var links)
Definition: chain.h:51
png_infop png_charpp name
Definition: png.h:2382
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
Definition: chain.h:52
void SetFixedJointType(const fVec3 &rpos, const fMat33 &ratt)
Set joint type to fixed.
Definition: joint.cpp:56
void identity()
Identity matrix.
Definition: fMatrix3.cpp:230
int BeginCreateChain(int append=false)
Indicates begining of creating a kinematic chain.
Definition: edit.cpp:19
void getMatrix33FromRowMajorArray(Matrix33 &m33, const Array &a, size_t top=0)
Definition: Eigen3d.h:118
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
png_uint_32 i
Definition: png.h:2735
static char charname_separator
Definition: dims_common.h:19
Definition: chain.h:50
spherical (3DOF)
Definition: chain.h:43
#define CORBA_ORB_var
Definition: ORBwrap.h:17
static double getLimitValue(DblSequence_var limitseq, double defaultValue)
JointType j_type
joint type
Definition: chain.h:694
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
string a
char * name
joint name (including the character name)
Definition: chain.h:691
t
void SetSlideJointType(const fVec3 &rpos, const fMat33 &ratt, AxisIndex ai)
Set joint type to prismatic.
Definition: joint.cpp:43
int EndCreateChain(SceneGraph *sg=NULL)
End editing a chain.
Definition: edit.cpp:225
Joint * Root()
Definition: chain.h:151
void SetSphereJointType(const fVec3 &rpos, const fMat33 &ratt)
Set joint type to spherical.
Definition: joint.cpp:68
static void array_to_vec3(const DblArray3 &a, fVec3 &vec)
int addSensor(Joint *jnt, int sensorType, int id, const std::string name, const fVec3 &_localPos, const fMat33 &_localR)
fVec3 loc_com
center of mass in local frame
Definition: chain.h:706
int AddJoint(Joint *target, Joint *p)
Add a new joint target as a child of joint p.
Definition: edit.cpp:117
fixed (0DOF)
Definition: chain.h:40
void SetRotateJointType(const fVec3 &rpos, const fMat33 &ratt, AxisIndex ai)
Set joint type to rotational.
Definition: joint.cpp:30
prismatic (1DOF)
Definition: chain.h:42
The class representing the whole mechanism. May contain multiple characters.
Definition: chain.h:144
char * CharName() const
Returns the character name.
Definition: chain.h:648
friend double length(const fVec3 &v)
Returns the length of a vector.
Definition: fMatrix3.h:293
static Joint * createLink(::World *world, const char *charname, int index, LinkInfoSequence_var iLinks, Joint *pjoint)
AxisIndex
Direction of a 1-DOF joint.
Definition: chain.h:48
3-element vector class.
Definition: fMatrix3.h:206
rotational (1DOF)
Definition: chain.h:41
int loadBodyFromBodyInfo(::World *world, const char *_name, BodyInfo_ptr bodyInfo)
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
Main class for forward dynamics computation.
Definition: psim.h:446
char * arg
Definition: cdjpeg.h:136
The class for representing a joint.
Definition: chain.h:538
int i_joint
index of the joint
Definition: chain.h:722
* y
Definition: IceUtils.h:97
fMat33 inertia
intertia
Definition: chain.h:705
free (6DOF)
Definition: chain.h:44
static void createSensors(::World *world, Joint *jnt, SensorInfoSequence iSensors)
void SetFreeJointType(const fVec3 &rpos, const fMat33 &ratt)
Set joint type to free.
Definition: joint.cpp:80


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