openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
kinbody.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 Rosen Diankov (rosen.diankov@gmail.com)
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 #include <algorithm>
19 
20 // used for functions that are also used internally
21 #define CHECK_INTERNAL_COMPUTATION0 OPENRAVE_ASSERT_FORMAT(_nHierarchyComputed != 0, "body %s internal structures need to be computed, current value is %d. Are you sure Environment::AddRobot/AddKinBody was called?", GetName()%_nHierarchyComputed, ORE_NotInitialized);
22 #define CHECK_INTERNAL_COMPUTATION OPENRAVE_ASSERT_FORMAT(_nHierarchyComputed == 2, "body %s internal structures need to be computed, current value is %d. Are you sure Environment::AddRobot/AddKinBody was called?", GetName()%_nHierarchyComputed, ORE_NotInitialized);
23 
24 namespace OpenRAVE {
25 
27 {
28 public:
29  ChangeCallbackData(int properties, const boost::function<void()>& callback, KinBodyConstPtr pbody) : _properties(properties), _callback(callback), _pweakbody(pbody) {
30  }
31  virtual ~ChangeCallbackData() {
32  KinBodyConstPtr pbody = _pweakbody.lock();
33  if( !!pbody ) {
34  boost::unique_lock< boost::shared_mutex > lock(pbody->GetInterfaceMutex());
35  pbody->_listRegisteredCallbacks.erase(_iterator);
36  }
37  }
38 
39  list<UserDataWeakPtr>::iterator _iterator;
41  boost::function<void()> _callback;
42 protected:
43  boost::weak_ptr<KinBody const> _pweakbody;
44 };
45 
46 typedef boost::shared_ptr<ChangeCallbackData> ChangeCallbackDataPtr;
47 
48 KinBody::KinBodyStateSaver::KinBodyStateSaver(KinBodyPtr pbody, int options) : _options(options), _pbody(pbody)
49 {
51  _pbody->GetLinkTransformations(_vLinkTransforms, _vdofbranches);
52  }
53  if( _options & Save_LinkEnable ) {
54  _vEnabledLinks.resize(_pbody->GetLinks().size());
55  for(size_t i = 0; i < _vEnabledLinks.size(); ++i) {
56  _vEnabledLinks[i] = _pbody->GetLinks().at(i)->IsEnabled();
57  }
58  }
60  _pbody->GetLinkVelocities(_vLinkVelocities);
61  }
63  _pbody->GetDOFVelocityLimits(_vMaxVelocities);
64  _pbody->GetDOFAccelerationLimits(_vMaxAccelerations);
65  }
66 }
67 
69 {
70  _RestoreKinBody(_pbody);
71 }
72 
73 void KinBody::KinBodyStateSaver::Restore(boost::shared_ptr<KinBody> body)
74 {
75  _RestoreKinBody(!body ? _pbody : body);
76 }
77 
79 {
80  _pbody.reset();
81 }
82 
83 void KinBody::KinBodyStateSaver::_RestoreKinBody(boost::shared_ptr<KinBody> pbody)
84 {
85  if( !pbody ) {
86  return;
87  }
88  if( pbody->GetEnvironmentId() == 0 ) {
89  RAVELOG_WARN(str(boost::format("body %s not added to environment, skipping restore")%pbody->GetName()));
90  return;
91  }
92  if( _options & Save_LinkTransformation ) {
93  pbody->SetLinkTransformations(_vLinkTransforms, _vdofbranches);
94  }
95  if( _options & Save_LinkEnable ) {
96  // should first enable before calling the parameter callbacks
97  bool bchanged = false;
98  for(size_t i = 0; i < _vEnabledLinks.size(); ++i) {
99  if( pbody->GetLinks().at(i)->IsEnabled() != !!_vEnabledLinks[i] ) {
100  pbody->GetLinks().at(i)->_info._bIsEnabled = !!_vEnabledLinks[i];
101  bchanged = true;
102  }
103  }
104  if( bchanged ) {
105  pbody->_nNonAdjacentLinkCache &= ~AO_Enabled;
106  pbody->_ParametersChanged(Prop_LinkEnable);
107  }
108  }
109  if( _options & Save_JointMaxVelocityAndAcceleration ) {
110  pbody->SetDOFVelocityLimits(_vMaxVelocities);
111  pbody->SetDOFAccelerationLimits(_vMaxAccelerations);
112  }
113  if( _options & Save_LinkVelocities ) {
114  pbody->SetLinkVelocities(_vLinkVelocities);
115  }
116 }
117 
118 KinBody::KinBody(InterfaceType type, EnvironmentBasePtr penv) : InterfaceBase(type, penv)
119 {
123  _environmentid = 0;
124  _nNonAdjacentLinkCache = 0x80000000;
125  _nUpdateStampId = 0;
126 }
127 
129 {
130  RAVELOG_VERBOSE(str(boost::format("destroying kinbody: %s\n")%GetName()));
131  Destroy();
132 }
133 
135 {
136  if( _listAttachedBodies.size() > 0 ) {
137  // could be in the environment destructor?
138  stringstream ss; ss << GetName() << " still has attached bodies: ";
139  FOREACHC(it,_listAttachedBodies) {
140  KinBodyPtr pattached = it->lock();
141  if( !!pattached ) {
142  ss << pattached->GetName();
143  }
144  }
145  RAVELOG_VERBOSE(ss.str());
146  }
147  _listAttachedBodies.clear();
148 
149  _veclinks.clear();
150  _vecjoints.clear();
153  _vDOFOrderedJoints.clear();
154  _vPassiveJoints.clear();
155  _vJointsAffectingLinks.clear();
156  _vDOFIndices.clear();
157 
158  _setAdjacentLinks.clear();
160  _vAllPairsShortestPaths.clear();
161  _vClosedLoops.clear();
162  _vClosedLoopIndices.clear();
163  _vForcedAdjacentLinks.clear();
166  _pManageData.reset();
167 
169 }
170 
171 bool KinBody::InitFromBoxes(const std::vector<AABB>& vaabbs, bool visible)
172 {
173  OPENRAVE_ASSERT_FORMAT(GetEnvironmentId()==0, "%s: cannot Init a body while it is added to the environment", GetName(), ORE_Failed);
174  Destroy();
175  LinkPtr plink(new Link(shared_kinbody()));
176  plink->_index = 0;
177  plink->_info._name = "base";
178  plink->_info._bStatic = true;
179  size_t numvertices=0, numindices=0;
180  FOREACHC(itab, vaabbs) {
181  GeometryInfo info;
182  info._type = GT_Box;
183  info._t.trans = itab->pos;
184  info._bVisible = visible;
185  info._vGeomData = itab->extents;
186  info._vDiffuseColor=Vector(1,0.5f,0.5f,1);
187  info._vAmbientColor=Vector(0.1,0.0f,0.0f,0);
188  Link::GeometryPtr geom(new Link::Geometry(plink,info));
189  geom->_info.InitCollisionMesh();
190  numvertices += geom->GetCollisionMesh().vertices.size();
191  numindices += geom->GetCollisionMesh().indices.size();
192  plink->_vGeometries.push_back(geom);
193  }
194 
195  plink->_collision.vertices.reserve(numvertices);
196  plink->_collision.indices.reserve(numindices);
197  TriMesh trimesh;
198  FOREACH(itgeom,plink->_vGeometries) {
199  trimesh = (*itgeom)->GetCollisionMesh();
200  trimesh.ApplyTransform((*itgeom)->GetTransform());
201  plink->_collision.Append(trimesh);
202  }
203  _veclinks.push_back(plink);
204  return true;
205 }
206 
207 bool KinBody::InitFromBoxes(const std::vector<OBB>& vobbs, bool visible)
208 {
209  OPENRAVE_ASSERT_FORMAT(GetEnvironmentId()==0, "%s: cannot Init a body while it is added to the environment", GetName(), ORE_Failed);
210  Destroy();
211  LinkPtr plink(new Link(shared_kinbody()));
212  plink->_index = 0;
213  plink->_info._name = "base";
214  plink->_info._bStatic = true;
215  size_t numvertices=0, numindices=0;
216  FOREACHC(itobb, vobbs) {
217  TransformMatrix tm;
218  tm.trans = itobb->pos;
219  tm.m[0] = itobb->right.x; tm.m[1] = itobb->up.x; tm.m[2] = itobb->dir.x;
220  tm.m[4] = itobb->right.y; tm.m[5] = itobb->up.y; tm.m[6] = itobb->dir.y;
221  tm.m[8] = itobb->right.z; tm.m[9] = itobb->up.z; tm.m[10] = itobb->dir.z;
222  GeometryInfo info;
223  info._type = GT_Box;
224  info._t = tm;
225  info._bVisible = visible;
226  info._vGeomData = itobb->extents;
227  info._vDiffuseColor=Vector(1,0.5f,0.5f,1);
228  info._vAmbientColor=Vector(0.1,0.0f,0.0f,0);
229  Link::GeometryPtr geom(new Link::Geometry(plink,info));
230  geom->_info.InitCollisionMesh();
231  numvertices += geom->GetCollisionMesh().vertices.size();
232  numindices += geom->GetCollisionMesh().indices.size();
233  plink->_vGeometries.push_back(geom);
234  }
235 
236  plink->_collision.vertices.reserve(numvertices);
237  plink->_collision.indices.reserve(numindices);
238  TriMesh trimesh;
239  FOREACH(itgeom,plink->_vGeometries) {
240  trimesh = (*itgeom)->GetCollisionMesh();
241  trimesh.ApplyTransform((*itgeom)->GetTransform());
242  plink->_collision.Append(trimesh);
243  }
244  _veclinks.push_back(plink);
245  return true;
246 }
247 
248 bool KinBody::InitFromSpheres(const std::vector<Vector>& vspheres, bool visible)
249 {
250  OPENRAVE_ASSERT_FORMAT(GetEnvironmentId()==0, "%s: cannot Init a body while it is added to the environment", GetName(), ORE_Failed);
251  Destroy();
252  LinkPtr plink(new Link(shared_kinbody()));
253  plink->_index = 0;
254  plink->_info._name = "base";
255  plink->_info._bStatic = true;
256  TriMesh trimesh;
257  FOREACHC(itv, vspheres) {
258  GeometryInfo info;
259  info._type = GT_Sphere;
260  info._t.trans.x = itv->x; info._t.trans.y = itv->y; info._t.trans.z = itv->z;
261  info._bVisible = visible;
262  info._vGeomData.x = itv->w;
263  info._vDiffuseColor=Vector(1,0.5f,0.5f,1);
264  info._vAmbientColor=Vector(0.1,0.0f,0.0f,0);
265  Link::GeometryPtr geom(new Link::Geometry(plink,info));
266  geom->_info.InitCollisionMesh();
267  plink->_vGeometries.push_back(geom);
268  trimesh = geom->GetCollisionMesh();
269  trimesh.ApplyTransform(geom->GetTransform());
270  plink->_collision.Append(trimesh);
271  }
272  _veclinks.push_back(plink);
273  return true;
274 }
275 
276 bool KinBody::InitFromTrimesh(const TriMesh& trimesh, bool visible)
277 {
278  OPENRAVE_ASSERT_FORMAT(GetEnvironmentId()==0, "%s: cannot Init a body while it is added to the environment", GetName(), ORE_Failed);
279  Destroy();
280  LinkPtr plink(new Link(shared_kinbody()));
281  plink->_index = 0;
282  plink->_info._name = "base";
283  plink->_info._bStatic = true;
284  plink->_collision = trimesh;
285  GeometryInfo info;
286  info._type = GT_TriMesh;
287  info._bVisible = visible;
288  info._vDiffuseColor=Vector(1,0.5f,0.5f,1);
289  info._vAmbientColor=Vector(0.1,0.0f,0.0f,0);
290  info._meshcollision = trimesh;
291  Link::GeometryPtr geom(new Link::Geometry(plink,info));
292  plink->_vGeometries.push_back(geom);
293  _veclinks.push_back(plink);
294  return true;
295 }
296 
297 bool KinBody::InitFromGeometries(const std::list<KinBody::GeometryInfo>& geometries)
298 {
299  std::vector<GeometryInfoConstPtr> newgeometries; newgeometries.reserve(geometries.size());
300  FOREACHC(it, geometries) {
301  newgeometries.push_back(GeometryInfoConstPtr(&(*it), utils::null_deleter()));
302  }
303  return InitFromGeometries(newgeometries);
304 }
305 
306 bool KinBody::InitFromGeometries(const std::vector<KinBody::GeometryInfoConstPtr>& geometries)
307 {
308  OPENRAVE_ASSERT_FORMAT(GetEnvironmentId()==0, "%s: cannot Init a body while it is added to the environment", GetName(), ORE_Failed);
309  OPENRAVE_ASSERT_OP(geometries.size(),>,0);
310  Destroy();
311  LinkPtr plink(new Link(shared_kinbody()));
312  plink->_index = 0;
313  plink->_info._name = "base";
314  plink->_info._bStatic = true;
315  FOREACHC(itinfo,geometries) {
316  Link::GeometryPtr geom(new Link::Geometry(plink,**itinfo));
317  geom->_info.InitCollisionMesh();
318  plink->_vGeometries.push_back(geom);
319  plink->_collision.Append(geom->GetCollisionMesh(),geom->GetTransform());
320  }
321  _veclinks.push_back(plink);
322  return true;
323 }
324 
325 bool KinBody::Init(const std::vector<KinBody::LinkInfoConstPtr>& linkinfos, const std::vector<KinBody::JointInfoConstPtr>& jointinfos)
326 {
327  OPENRAVE_ASSERT_FORMAT(GetEnvironmentId()==0, "%s: cannot Init a body while it is added to the environment", GetName(), ORE_Failed);
328  OPENRAVE_ASSERT_OP(linkinfos.size(),>,0);
329  Destroy();
330  _veclinks.reserve(linkinfos.size());
331  FOREACHC(itlinkinfo, linkinfos) {
332  LinkInfoConstPtr rawinfo = *itlinkinfo;
333  LinkPtr plink(new Link(shared_kinbody()));
334  plink->_info = *rawinfo;
335  LinkInfo& info = plink->_info;
336  plink->_index = static_cast<int>(_veclinks.size());
337  FOREACHC(itgeominfo,info._vgeometryinfos) {
338  Link::GeometryPtr geom(new Link::Geometry(plink,**itgeominfo));
339  geom->_info.InitCollisionMesh();
340  plink->_vGeometries.push_back(geom);
341  plink->_collision.Append(geom->GetCollisionMesh(),geom->GetTransform());
342  }
343  FOREACHC(itadjacentname, info._vForcedAdjacentLinks) {
344  _vForcedAdjacentLinks.push_back(std::make_pair(info._name, *itadjacentname));
345  }
346  _veclinks.push_back(plink);
347  }
348  _vecjoints.reserve(jointinfos.size());
349  FOREACHC(itjointinfo, jointinfos) {
350  JointInfoConstPtr rawinfo = *itjointinfo;
351  JointPtr pjoint(new Joint(shared_kinbody(), rawinfo->_type));
352  pjoint->_info = *rawinfo;
353  JointInfo& info = pjoint->_info;
354  for(size_t i = 0; i < info._vmimic.size(); ++i) {
355  if( !!info._vmimic[i] ) {
356  pjoint->_vmimic[i].reset(new Mimic());
357  pjoint->_vmimic[i]->_equations = info._vmimic[i]->_equations;
358  }
359  }
360  LinkPtr plink0, plink1;
361  FOREACHC(itlink, _veclinks) {
362  if( (*itlink)->_info._name == info._linkname0 ) {
363  plink0 = *itlink;
364  if( !!plink1 ) {
365  break;
366  }
367  }
368  if( (*itlink)->_info._name == info._linkname1 ) {
369  plink1 = *itlink;
370  if( !!plink0 ) {
371  break;
372  }
373  }
374  }
375  OPENRAVE_ASSERT_FORMAT(!!plink0&&!!plink1, "cannot find links '%s' and '%s' of body '%s' joint %s ", info._linkname0%info._linkname1%GetName()%info._name, ORE_Failed);
376  std::vector<Vector> vaxes(pjoint->GetDOF());
377  std::copy(info._vaxes.begin(),info._vaxes.begin()+vaxes.size(), vaxes.begin());
378  pjoint->_ComputeInternalInformation(plink0, plink1, info._vanchor, vaxes, info._vcurrentvalues);
379  if( info._bIsActive ) {
380  _vecjoints.push_back(pjoint);
381  }
382  else {
383  _vPassiveJoints.push_back(pjoint);
384  }
385  }
386  return true;
387 }
388 
389 void KinBody::SetName(const std::string& newname)
390 {
391  OPENRAVE_ASSERT_OP(newname.size(), >, 0);
392  if( _name != newname ) {
393  // have to replace the 2nd word of all the groups with the robot name
394  FOREACH(itgroup, _spec._vgroups) {
395  stringstream ss(itgroup->name);
396  string grouptype, oldname;
397  ss >> grouptype >> oldname;
398  stringbuf buf;
399  ss.get(buf,0);
400  itgroup->name = str(boost::format("%s %s %s")%grouptype%newname%buf.str());
401  }
402  _name = newname;
404  }
405 }
406 
407 void KinBody::SetDOFTorques(const std::vector<dReal>& torques, bool bAdd)
408 {
409  OPENRAVE_ASSERT_OP_FORMAT((int)torques.size(), >=, GetDOF(), "not enough values %d<%d", torques.size()%GetDOF(),ORE_InvalidArguments);
410  if( !bAdd ) {
411  FOREACH(itlink, _veclinks) {
412  (*itlink)->SetForce(Vector(),Vector(),false);
413  (*itlink)->SetTorque(Vector(),false);
414  }
415  }
416  std::vector<dReal> jointtorques;
417  FOREACH(it, _vecjoints) {
418  jointtorques.resize((*it)->GetDOF());
419  std::copy(torques.begin()+(*it)->GetDOFIndex(),torques.begin()+(*it)->GetDOFIndex()+(*it)->GetDOF(),jointtorques.begin());
420  (*it)->AddTorque(jointtorques);
421  }
422 }
423 
424 int KinBody::GetDOF() const
425 {
426  return _vecjoints.size() > 0 ? _vecjoints.back()->GetDOFIndex()+_vecjoints.back()->GetDOF() : 0;
427 }
428 
429 void KinBody::GetDOFValues(std::vector<dReal>& v, const std::vector<int>& dofindices) const
430 {
432  if( dofindices.size() == 0 ) {
433  v.resize(0);
434  if( (int)v.capacity() < GetDOF() ) {
435  v.reserve(GetDOF());
436  }
437  FOREACHC(it, _vDOFOrderedJoints) {
438  int toadd = (*it)->GetDOFIndex()-(int)v.size();
439  if( toadd > 0 ) {
440  v.insert(v.end(),toadd,0);
441  }
442  else if( toadd < 0 ) {
443  throw OPENRAVE_EXCEPTION_FORMAT("dof indices mismatch joint %s, toadd=%d", (*it)->GetName()%toadd, ORE_InvalidState);
444  }
445  (*it)->GetValues(v,true);
446  }
447  }
448  else {
449  v.resize(dofindices.size());
450  for(size_t i = 0; i < dofindices.size(); ++i) {
451  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
452  v[i] = pjoint->GetValue(dofindices[i]-pjoint->GetDOFIndex());
453  }
454  }
455 }
456 
457 void KinBody::GetDOFVelocities(std::vector<dReal>& v, const std::vector<int>& dofindices) const
458 {
459  if( dofindices.size() == 0 ) {
460  v.resize(0);
461  if( (int)v.capacity() < GetDOF() ) {
462  v.reserve(GetDOF());
463  }
464  FOREACHC(it, _vDOFOrderedJoints) {
465  (*it)->GetVelocities(v,true);
466  }
467  }
468  else {
469  v.resize(dofindices.size());
470  for(size_t i = 0; i < dofindices.size(); ++i) {
471  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
472  v[i] = pjoint->GetVelocity(dofindices[i]-pjoint->GetDOFIndex());
473  }
474  }
475 }
476 
477 void KinBody::GetDOFLimits(std::vector<dReal>& vLowerLimit, std::vector<dReal>& vUpperLimit, const std::vector<int>& dofindices) const
478 {
479  if( dofindices.size() == 0 ) {
480  vLowerLimit.resize(0);
481  if( (int)vLowerLimit.capacity() < GetDOF() ) {
482  vLowerLimit.reserve(GetDOF());
483  }
484  vUpperLimit.resize(0);
485  if( (int)vUpperLimit.capacity() < GetDOF() ) {
486  vUpperLimit.reserve(GetDOF());
487  }
488  FOREACHC(it,_vDOFOrderedJoints) {
489  (*it)->GetLimits(vLowerLimit,vUpperLimit,true);
490  }
491  }
492  else {
493  vLowerLimit.resize(dofindices.size());
494  vUpperLimit.resize(dofindices.size());
495  for(size_t i = 0; i < dofindices.size(); ++i) {
496  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
497  std::pair<dReal, dReal> res = pjoint->GetLimit(dofindices[i]-pjoint->GetDOFIndex());
498  vLowerLimit[i] = res.first;
499  vUpperLimit[i] = res.second;
500  }
501  }
502 }
503 
504 void KinBody::GetDOFVelocityLimits(std::vector<dReal>& vlower, std::vector<dReal>& vupper, const std::vector<int>& dofindices) const
505 {
506  if( dofindices.size() == 0 ) {
507  vlower.resize(0);
508  vupper.resize(0);
509  if( (int)vlower.capacity() < GetDOF() ) {
510  vlower.reserve(GetDOF());
511  }
512  if( (int)vupper.capacity() < GetDOF() ) {
513  vupper.reserve(GetDOF());
514  }
515  FOREACHC(it,_vDOFOrderedJoints) {
516  (*it)->GetVelocityLimits(vlower,vupper,true);
517  }
518  }
519  else {
520  vlower.resize(dofindices.size());
521  vupper.resize(dofindices.size());
522  for(size_t i = 0; i < dofindices.size(); ++i) {
523  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
524  std::pair<dReal, dReal> res = pjoint->GetVelocityLimit(dofindices[i]-pjoint->GetDOFIndex());
525  vlower[i] = res.first;
526  vupper[i] = res.second;
527  }
528  }
529 }
530 
531 void KinBody::GetDOFVelocityLimits(std::vector<dReal>& v, const std::vector<int>& dofindices) const
532 {
533  if( dofindices.size() == 0 ) {
534  v.resize(0);
535  if( (int)v.capacity() < GetDOF() ) {
536  v.reserve(GetDOF());
537  }
538  FOREACHC(it, _vDOFOrderedJoints) {
539  (*it)->GetVelocityLimits(v,true);
540  }
541  }
542  else {
543  v.resize(dofindices.size());
544  for(size_t i = 0; i < dofindices.size(); ++i) {
545  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
546  v[i] = pjoint->GetMaxVel(dofindices[i]-pjoint->GetDOFIndex());
547  }
548  }
549 }
550 
551 void KinBody::GetDOFAccelerationLimits(std::vector<dReal>& v, const std::vector<int>& dofindices) const
552 {
553  if( dofindices.size() == 0 ) {
554  v.resize(0);
555  if( (int)v.capacity() < GetDOF() ) {
556  v.reserve(GetDOF());
557  }
558  FOREACHC(it, _vDOFOrderedJoints) {
559  (*it)->GetAccelerationLimits(v,true);
560  }
561  }
562  else {
563  v.resize(dofindices.size());
564  for(size_t i = 0; i < dofindices.size(); ++i) {
565  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
566  v[i] = pjoint->GetAccelerationLimit(dofindices[i]-pjoint->GetDOFIndex());
567  }
568  }
569 }
570 
571 void KinBody::GetDOFTorqueLimits(std::vector<dReal>& v) const
572 {
573  v.resize(0);
574  if( (int)v.capacity() < GetDOF() ) {
575  v.reserve(GetDOF());
576  }
577  FOREACHC(it, _vDOFOrderedJoints) {
578  (*it)->GetTorqueLimits(v,true);
579  }
580 }
581 
582 void KinBody::GetDOFMaxTorque(std::vector<dReal>& v) const
583 {
584  v.resize(0);
585  if( (int)v.capacity() < GetDOF() ) {
586  v.reserve(GetDOF());
587  }
588  FOREACHC(it, _vDOFOrderedJoints) {
589  v.insert(v.end(),(*it)->GetDOF(),(*it)->GetMaxTorque());
590  }
591 }
592 
593 void KinBody::GetDOFResolutions(std::vector<dReal>& v, const std::vector<int>& dofindices) const
594 {
595  if( dofindices.size() == 0 ) {
596  v.resize(0);
597  if( (int)v.capacity() < GetDOF() )
598  v.reserve(GetDOF());
599  FOREACHC(it, _vDOFOrderedJoints) {
600  v.insert(v.end(),(*it)->GetDOF(),(*it)->GetResolution());
601  }
602  }
603  else {
604  v.resize(dofindices.size());
605  for(size_t i = 0; i < dofindices.size(); ++i) {
606  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
607  v[i] = pjoint->GetResolution(dofindices[i]-pjoint->GetDOFIndex());
608  }
609  }
610 }
611 
612 void KinBody::GetDOFWeights(std::vector<dReal>& v, const std::vector<int>& dofindices) const
613 {
614  if( dofindices.size() == 0 ) {
615  v.resize(GetDOF());
616  std::vector<dReal>::iterator itv = v.begin();
617  FOREACHC(it, _vDOFOrderedJoints) {
618  for(int i = 0; i < (*it)->GetDOF(); ++i) {
619  *itv++ = (*it)->GetWeight(i);
620  }
621  }
622  }
623  else {
624  v.resize(dofindices.size());
625  for(size_t i = 0; i < dofindices.size(); ++i) {
626  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
627  v[i] = pjoint->GetWeight(dofindices[i]-pjoint->GetDOFIndex());
628  }
629  }
630 }
631 
632 void KinBody::SetDOFWeights(const std::vector<dReal>& v, const std::vector<int>& dofindices)
633 {
634  if( dofindices.size() == 0 ) {
635  OPENRAVE_ASSERT_OP((int)v.size(),>=,GetDOF());
636  for(int i = 0; i < GetDOF(); ++i) {
637  OPENRAVE_ASSERT_OP_FORMAT(v[i], >, 0, "dof %d weight %f has to be >= 0", i%v[i], ORE_InvalidArguments);
638  }
639  std::vector<dReal>::const_iterator itv = v.begin();
640  FOREACHC(it, _vDOFOrderedJoints) {
641  std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vweights.begin());
642  itv += (*it)->GetDOF();
643  }
644  }
645  else {
646  OPENRAVE_ASSERT_OP(v.size(),==,dofindices.size());
647  for(size_t i = 0; i < dofindices.size(); ++i) {
648  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
649  pjoint->_info._vweights.at(dofindices[i]-pjoint->GetDOFIndex()) = v[i];
650  }
651  }
653 }
654 
655 void KinBody::SetDOFLimits(const std::vector<dReal>& lower, const std::vector<dReal>& upper)
656 {
657  OPENRAVE_ASSERT_OP((int)lower.size(),==,GetDOF());
658  OPENRAVE_ASSERT_OP((int)upper.size(),==,GetDOF());
659  bool bChanged = false;
660  std::vector<dReal>::const_iterator itlower = lower.begin(), itupper = upper.begin();
661  FOREACHC(it, _vDOFOrderedJoints) {
662  for(int i = 0; i < (*it)->GetDOF(); ++i) {
663  if( (*it)->_info._vlowerlimit.at(i) != *(itlower+i) || (*it)->_info._vupperlimit.at(i) != *(itupper+i) ) {
664  bChanged = true;
665  std::copy(itlower,itlower+(*it)->GetDOF(), (*it)->_info._vlowerlimit.begin());
666  std::copy(itupper,itupper+(*it)->GetDOF(), (*it)->_info._vupperlimit.begin());
667  for(int i = 0; i < (*it)->GetDOF(); ++i) {
668  if( (*it)->IsRevolute(i) && !(*it)->IsCircular(i) ) {
669  // TODO, necessary to set wrap?
670  if( (*it)->_info._vlowerlimit.at(i) < -PI || (*it)->_info._vupperlimit.at(i) > PI) {
671  (*it)->SetWrapOffset(0.5f * ((*it)->_info._vlowerlimit.at(i) + (*it)->_info._vupperlimit.at(i)),i);
672  }
673  else {
674  (*it)->SetWrapOffset(0,i);
675  }
676  }
677  }
678  break;
679  }
680  }
681  itlower += (*it)->GetDOF();
682  itupper += (*it)->GetDOF();
683  }
684  if( bChanged ) {
686  }
687 }
688 
689 void KinBody::SetDOFVelocityLimits(const std::vector<dReal>& v)
690 {
691  std::vector<dReal>::const_iterator itv = v.begin();
692  FOREACHC(it, _vDOFOrderedJoints) {
693  std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vmaxvel.begin());
694  itv += (*it)->GetDOF();
695  }
697 }
698 
699 void KinBody::SetDOFAccelerationLimits(const std::vector<dReal>& v)
700 {
701  std::vector<dReal>::const_iterator itv = v.begin();
702  FOREACHC(it, _vDOFOrderedJoints) {
703  std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vmaxaccel.begin());
704  itv += (*it)->GetDOF();
705  }
707 }
708 
709 void KinBody::SetDOFTorqueLimits(const std::vector<dReal>& v)
710 {
711  std::vector<dReal>::const_iterator itv = v.begin();
712  FOREACHC(it, _vDOFOrderedJoints) {
713  std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vmaxtorque.begin());
714  itv += (*it)->GetDOF();
715  }
717 }
718 
719 void KinBody::SimulationStep(dReal fElapsedTime)
720 {
721 }
722 
723 void KinBody::SubtractDOFValues(std::vector<dReal>& q1, const std::vector<dReal>& q2, const std::vector<int>& dofindices) const
724 {
725  if( dofindices.size() == 0 ) {
726  FOREACHC(itjoint,_vecjoints) {
727  int dof = (*itjoint)->GetDOFIndex();
728  for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
729  if( (*itjoint)->IsCircular(i) ) {
730  q1.at(dof+i) = utils::NormalizeCircularAngle(q1.at(dof+i)-q2.at(dof+i),(*itjoint)->_vcircularlowerlimit.at(i), (*itjoint)->_vcircularupperlimit.at(i));
731  }
732  else {
733  q1.at(dof+i) -= q2.at(dof+i);
734  }
735  }
736  }
737  }
738  else {
739  for(size_t i = 0; i < dofindices.size(); ++i) {
740  JointPtr pjoint = GetJointFromDOFIndex(dofindices[i]);
741  if( pjoint->IsCircular(dofindices[i]-pjoint->GetDOFIndex()) ) {
742  int iaxis = dofindices[i]-pjoint->GetDOFIndex();
743  q1.at(i) = utils::NormalizeCircularAngle(q1.at(i)-q2.at(i), pjoint->_vcircularlowerlimit.at(iaxis), pjoint->_vcircularupperlimit.at(iaxis));
744  }
745  else {
746  q1.at(i) -= q2.at(i);
747  }
748  }
749  }
750 }
751 
752 // like apply transform except everything is relative to the first frame
754 {
755  if( _veclinks.size() == 0 ) {
756  return;
757  }
758  Transform tbaseinv = _veclinks.front()->GetTransform().inverse();
759  Transform tapply = trans * tbaseinv;
760  FOREACH(itlink, _veclinks) {
761  (*itlink)->SetTransform(tapply * (*itlink)->GetTransform());
762  }
763 }
764 
766 {
767  return _veclinks.size() > 0 ? _veclinks.front()->GetTransform() : Transform();
768 }
769 
770 bool KinBody::SetVelocity(const Vector& linearvel, const Vector& angularvel)
771 {
772  std::vector<std::pair<Vector,Vector> > velocities(_veclinks.size());
773  velocities.at(0).first = linearvel;
774  velocities.at(0).second = angularvel;
775  Vector vlinktrans = _veclinks.at(0)->GetTransform().trans;
776  for(size_t i = 1; i < _veclinks.size(); ++i) {
777  velocities[i].first = linearvel + angularvel.cross(_veclinks[i]->GetTransform().trans-vlinktrans);
778  velocities[i].second = angularvel;
779  }
780  return GetEnv()->GetPhysicsEngine()->SetLinkVelocities(shared_kinbody(),velocities);
781 }
782 
783 void KinBody::SetDOFVelocities(const std::vector<dReal>& vDOFVelocities, const Vector& linearvel, const Vector& angularvel, uint32_t checklimits)
784 {
786  OPENRAVE_ASSERT_OP_FORMAT((int)vDOFVelocities.size(), >=, GetDOF(), "not enough values %d!=%d", vDOFVelocities.size()%GetDOF(),ORE_InvalidArguments);
787  std::vector<std::pair<Vector,Vector> > velocities(_veclinks.size());
788  velocities.at(0).first = linearvel;
789  velocities.at(0).second = angularvel;
790 
791  vector<dReal> vlower,vupper,vtempvalues, veval;
792  if( checklimits != CLA_Nothing ) {
793  GetDOFVelocityLimits(vlower,vupper);
794  }
795 
796  // have to compute the velocities ahead of time since they are dependent on the link transformations
797  std::vector< std::vector<dReal> > vPassiveJointVelocities(_vPassiveJoints.size());
798  for(size_t i = 0; i < vPassiveJointVelocities.size(); ++i) {
799  if( !_vPassiveJoints[i]->IsMimic() ) {
800  _vPassiveJoints[i]->GetVelocities(vPassiveJointVelocities[i]);
801  }
802  else {
803  vPassiveJointVelocities[i].resize(_vPassiveJoints[i]->GetDOF(),0);
804  }
805  }
806 
807  std::vector<uint8_t> vlinkscomputed(_veclinks.size(),0);
808  vlinkscomputed[0] = 1;
809  boost::array<dReal,3> dummyvalues; // dummy values for a joint
810 
811  for(size_t ijoint = 0; ijoint < _vTopologicallySortedJointsAll.size(); ++ijoint) {
812  JointPtr pjoint = _vTopologicallySortedJointsAll[ijoint];
813  int jointindex = _vTopologicallySortedJointIndicesAll[ijoint];
814  int dofindex = pjoint->GetDOFIndex();
815  const dReal* pvalues=dofindex >= 0 ? &vDOFVelocities.at(dofindex) : NULL;
816  if( pjoint->IsMimic() ) {
817  for(int i = 0; i < pjoint->GetDOF(); ++i) {
818  if( pjoint->IsMimic(i) ) {
819  vtempvalues.resize(0);
820  const std::vector<Mimic::DOFFormat>& vdofformat = pjoint->_vmimic[i]->_vdofformat;
821  FOREACHC(itdof,vdofformat) {
822  JointPtr pj = itdof->jointindex < (int)_vecjoints.size() ? _vecjoints[itdof->jointindex] : _vPassiveJoints.at(itdof->jointindex-_vecjoints.size());
823  vtempvalues.push_back(pj->GetValue(itdof->axis));
824  }
825  dummyvalues[i] = 0;
826  int err = pjoint->_Eval(i,1,vtempvalues,veval);
827  if( err ) {
828  RAVELOG_WARN(str(boost::format("failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
829  }
830  else {
831  for(size_t ipartial = 0; ipartial < vdofformat.size(); ++ipartial) {
832  dReal partialvelocity;
833  if( vdofformat[ipartial].dofindex >= 0 ) {
834  partialvelocity = vDOFVelocities.at(vdofformat[ipartial].dofindex);
835  }
836  else {
837  partialvelocity = vPassiveJointVelocities.at(vdofformat[ipartial].jointindex-_vecjoints.size()).at(vdofformat[ipartial].axis);
838  }
839  dummyvalues[i] += veval.at(ipartial) * partialvelocity;
840  }
841  }
842 
843  // if joint is passive, update the stored joint values! This is necessary because joint value might be referenced in the future.
844  if( dofindex < 0 ) {
845  vPassiveJointVelocities.at(jointindex-(int)_vecjoints.size()).at(i) = dummyvalues[i];
846  }
847  }
848  else if( dofindex >= 0 ) {
849  dummyvalues[i] = vDOFVelocities.at(dofindex+i); // is this correct? what is a joint has a mimic and non-mimic axis?
850  }
851  else {
852  // preserve passive joint values
853  dummyvalues[i] = vPassiveJointVelocities.at(jointindex-(int)_vecjoints.size()).at(i);
854  }
855  }
856  pvalues = &dummyvalues[0];
857  }
858  // do the test after mimic computation!
859  if( vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] ) {
860  continue;
861  }
862  if( !pvalues ) {
863  // has to be a passive joint
864  pvalues = &vPassiveJointVelocities.at(jointindex-(int)_vecjoints.size()).at(0);
865  }
866 
867  if( checklimits &&(dofindex >= 0)) {
868  for(int i = 0; i < pjoint->GetDOF(); ++i) {
869  if( pvalues[i] < vlower.at(dofindex+i) ) {
870  if( checklimits == CLA_CheckLimits ) {
871  RAVELOG_WARN(str(boost::format("dof %d velocity is not in limits %.15e<%.15e")%i%pvalues[i]%vlower.at(dofindex+i)));
872  }
873  else if( checklimits == CLA_CheckLimitsThrow ) {
874  throw OPENRAVE_EXCEPTION_FORMAT("dof %d velocity is not in limits %.15e<%.15e", i%pvalues[i]%vlower.at(dofindex+i), ORE_InvalidArguments);
875  }
876  dummyvalues[i] = vlower[i];
877  }
878  else if( pvalues[i] > vupper.at(dofindex+i) ) {
879  if( checklimits == CLA_CheckLimits ) {
880  RAVELOG_WARN(str(boost::format("dof %d velocity is not in limits %.15e>%.15e")%i%pvalues[i]%vupper.at(dofindex+i)));
881  }
882  else if( checklimits == CLA_CheckLimitsThrow ) {
883  throw OPENRAVE_EXCEPTION_FORMAT("dof %d velocity is not in limits %.15e>%.15e", i%pvalues[i]%vupper.at(dofindex+i), ORE_InvalidArguments);
884  }
885  dummyvalues[i] = vupper[i];
886  }
887  else {
888  dummyvalues[i] = pvalues[i];
889  }
890  }
891  pvalues = &dummyvalues[0];
892  }
893 
894  // compute for global coordinate system
895  Vector vparent, wparent;
896  Transform tparent;
897  if( !pjoint->GetHierarchyParentLink() ) {
898  tparent = _veclinks.at(0)->GetTransform();
899  vparent = velocities.at(0).first;
900  wparent = velocities.at(0).second;
901  }
902  else {
903  tparent = pjoint->GetHierarchyParentLink()->GetTransform();
904  vparent = velocities[pjoint->GetHierarchyParentLink()->GetIndex()].first;
905  wparent = velocities[pjoint->GetHierarchyParentLink()->GetIndex()].second;
906  }
907 
908  int childindex = pjoint->GetHierarchyChildLink()->GetIndex();
909  Transform tchild = pjoint->GetHierarchyChildLink()->GetTransform();
910  Vector xyzdelta = tchild.trans - tparent.trans;
911  Transform tdelta = tparent * pjoint->GetInternalHierarchyLeftTransform();
912 // if( pjoint->GetType() & JointSpecialBit ) {
913 // switch(pjoint->GetType()) {
914 // case JointHinge2: {
915 // Transform tfirst;
916 // tfirst.rot = quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(0), pjoint->GetValue(0));
917 // w = pvalues[0]*pjoint->GetInternalHierarchyAxis(0) + tfirst.rotate(pvalues[1]*pjoint->GetInternalHierarchyAxis(1));
918 // break;
919 // }
920 // case JointSpherical:
921 // w.x = pvalues[0]; w.y = pvalues[1]; w.z = pvalues[2];
922 // break;
923 // default:
924 // RAVELOG_WARN(str(boost::format("forward kinematic type %d not supported")%pjoint->GetType()));
925 // break;
926 // }
927 // }
928 // else {
929  if( pjoint->GetType() == JointRevolute ) {
930  Vector gw = tdelta.rotate(pvalues[0]*pjoint->GetInternalHierarchyAxis(0));
931  velocities.at(childindex) = make_pair(vparent + wparent.cross(xyzdelta) + gw.cross(tchild.trans-tdelta.trans), wparent + gw);
932  }
933  else if( pjoint->GetType() == JointPrismatic ) {
934  velocities.at(childindex) = make_pair(vparent + wparent.cross(xyzdelta) + tdelta.rotate(pvalues[0]*pjoint->GetInternalHierarchyAxis(0)), wparent);
935  }
936  else if( pjoint->GetType() == JointTrajectory ) {
937  Transform tlocalvelocity, tlocal;
938  if( pjoint->IsMimic(0) ) {
939  // vtempvalues should already be init from previous _Eval call
940  int err = pjoint->_Eval(0,0,vtempvalues,veval);
941  if( err != 0 ) {
942  RAVELOG_WARN(str(boost::format("error with evaluation of joint %s")%pjoint->GetName()));
943  }
944  dReal fvalue = veval[0];
945  if( pjoint->IsCircular(0) ) {
946  fvalue = utils::NormalizeCircularAngle(fvalue,pjoint->_vcircularlowerlimit.at(0), pjoint->_vcircularupperlimit.at(0));
947  }
948  pjoint->_info._trajfollow->Sample(vtempvalues,fvalue);
949  }
950  else {
951  // calling GetValue() could be extremely slow
952  pjoint->_info._trajfollow->Sample(vtempvalues,pjoint->GetValue(0));
953  }
954  pjoint->_info._trajfollow->GetConfigurationSpecification().ExtractTransform(tlocal, vtempvalues.begin(), KinBodyConstPtr(),0);
955  pjoint->_info._trajfollow->GetConfigurationSpecification().ExtractTransform(tlocalvelocity, vtempvalues.begin(), KinBodyConstPtr(),1);
956  Vector gw = tdelta.rotate(quatMultiply(tlocalvelocity.rot, quatInverse(tlocal.rot))*2*pvalues[0]); // qvel = [0,axisangle] * qrot * 0.5 * vel
957  gw = Vector(gw.y,gw.z,gw.w);
958  Vector gv = tdelta.rotate(tlocalvelocity.trans*pvalues[0]);
959  velocities.at(childindex) = make_pair(vparent + wparent.cross(xyzdelta) + gw.cross(tchild.trans-tdelta.trans) + gv, wparent + gw);
960  }
961  else {
962  throw OPENRAVE_EXCEPTION_FORMAT("joint %s not supported for querying velocities",pjoint->GetType(),ORE_Assert);
963 // //todo
964 // Transform tjoint;
965 // for(int iaxis = 0; iaxis < pjoint->GetDOF(); ++iaxis) {
966 // Transform tdelta;
967 // if( pjoint->IsRevolute(iaxis) ) {
968 // w += tjoint.rotate(pvalues[iaxis]*pjoint->GetInternalHierarchyAxis(iaxis));
969 // tdelta.rot = quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(iaxis), pvalues[iaxis]);
970 // }
971 // else {
972 // tdelta.trans = pjoint->GetInternalHierarchyAxis(iaxis) * pvalues[iaxis];
973 // v += tjoint.rotate(pvalues[iaxis]*pjoint->GetInternalHierarchyAxis(iaxis)) + w.cross(tdelta.trans);
974 // }
975 // tjoint = tjoint * tdelta;
976 // }
977  }
978 // }
979 
980 
981  vlinkscomputed[childindex] = 1;
982  }
983  SetLinkVelocities(velocities);
984 }
985 
986 void KinBody::SetDOFVelocities(const std::vector<dReal>& vDOFVelocities, uint32_t checklimits, const std::vector<int>& dofindices)
987 {
988  Vector linearvel,angularvel;
989  _veclinks.at(0)->GetVelocity(linearvel,angularvel);
990  if( dofindices.size() == 0 ) {
991  return SetDOFVelocities(vDOFVelocities,linearvel,angularvel,checklimits);
992  }
993 
994  // check if all dofindices are supplied
995  if( (int)dofindices.size() == GetDOF() ) {
996  bool bordereddof = true;
997  for(size_t i = 0; i < dofindices.size(); ++i) {
998  if( dofindices[i] != (int)i ) {
999  bordereddof = false;
1000  break;
1001  }
1002  }
1003  if( bordereddof ) {
1004  return SetDOFVelocities(vDOFVelocities,linearvel,angularvel,checklimits);
1005  }
1006  }
1007  OPENRAVE_ASSERT_OP_FORMAT0(vDOFVelocities.size(),==,dofindices.size(),"index sizes do not match", ORE_InvalidArguments);
1008  // have to recreate the correct vector
1009  std::vector<dReal> vfulldof(GetDOF());
1010  std::vector<int>::const_iterator it;
1011  for(size_t i = 0; i < dofindices.size(); ++i) {
1012  it = find(dofindices.begin(), dofindices.end(), i);
1013  if( it != dofindices.end() ) {
1014  vfulldof[i] = vDOFVelocities.at(static_cast<size_t>(it-dofindices.begin()));
1015  }
1016  else {
1017  JointPtr pjoint = GetJointFromDOFIndex(i);
1018  if( !!pjoint ) {
1019  vfulldof[i] = _vecjoints.at(_vDOFIndices.at(i))->GetVelocity(i-_vDOFIndices.at(i));
1020  }
1021  }
1022  }
1023  return SetDOFVelocities(vfulldof,linearvel,angularvel,checklimits);
1024 }
1025 
1026 void KinBody::GetLinkVelocities(std::vector<std::pair<Vector,Vector> >& velocities) const
1027 {
1028  GetEnv()->GetPhysicsEngine()->GetLinkVelocities(shared_kinbody_const(),velocities);
1029 }
1030 
1031 void KinBody::GetLinkTransformations(vector<Transform>& vtrans) const
1032 {
1034  RAVELOG_VERBOSE("GetLinkTransformations should be called with dofbranches\n");
1035  }
1036  vtrans.resize(_veclinks.size());
1037  vector<Transform>::iterator it;
1038  vector<LinkPtr>::const_iterator itlink;
1039  for(it = vtrans.begin(), itlink = _veclinks.begin(); it != vtrans.end(); ++it, ++itlink) {
1040  *it = (*itlink)->GetTransform();
1041  }
1042 }
1043 
1044 void KinBody::GetLinkTransformations(vector<Transform>& vtrans, std::vector<int>& dofbranches) const
1045 {
1046  vtrans.resize(_veclinks.size());
1047  vector<Transform>::iterator it;
1048  vector<LinkPtr>::const_iterator itlink;
1049  for(it = vtrans.begin(), itlink = _veclinks.begin(); it != vtrans.end(); ++it, ++itlink) {
1050  *it = (*itlink)->GetTransform();
1051  }
1052 
1053  dofbranches.resize(0);
1054  if( (int)dofbranches.capacity() < GetDOF() ) {
1055  dofbranches.reserve(GetDOF());
1056  }
1057  FOREACHC(it, _vDOFOrderedJoints) {
1058  int toadd = (*it)->GetDOFIndex()-(int)dofbranches.size();
1059  if( toadd > 0 ) {
1060  dofbranches.insert(dofbranches.end(),toadd,0);
1061  }
1062  else if( toadd < 0 ) {
1063  throw OPENRAVE_EXCEPTION_FORMAT("dof indices mismatch joint %s, toadd=%d", (*it)->GetName()%toadd, ORE_InvalidState);
1064  }
1065  for(int i = 0; i < (*it)->GetDOF(); ++i) {
1066  dofbranches.push_back((*it)->_dofbranches[i]);
1067  }
1068  }
1069 }
1070 
1072 {
1073  return _vecjoints.at(_vDOFIndices.at(dofindex));
1074 }
1075 
1077 {
1078  Vector vmin, vmax;
1079  bool binitialized=false;
1080  AABB ab;
1081  FOREACHC(itlink,_veclinks) {
1082  ab = (*itlink)->ComputeAABB();
1083  if((ab.extents.x == 0)&&(ab.extents.y == 0)&&(ab.extents.z == 0)) {
1084  continue;
1085  }
1086  Vector vnmin = ab.pos - ab.extents;
1087  Vector vnmax = ab.pos + ab.extents;
1088  if( !binitialized ) {
1089  vmin = vnmin;
1090  vmax = vnmax;
1091  binitialized = true;
1092  }
1093  else {
1094  if( vmin.x > vnmin.x ) {
1095  vmin.x = vnmin.x;
1096  }
1097  if( vmin.y > vnmin.y ) {
1098  vmin.y = vnmin.y;
1099  }
1100  if( vmin.z > vnmin.z ) {
1101  vmin.z = vnmin.z;
1102  }
1103  if( vmax.x < vnmax.x ) {
1104  vmax.x = vnmax.x;
1105  }
1106  if( vmax.y < vnmax.y ) {
1107  vmax.y = vnmax.y;
1108  }
1109  if( vmax.z < vnmax.z ) {
1110  vmax.z = vnmax.z;
1111  }
1112  }
1113  }
1114  if( !binitialized ) {
1115  ab.pos = GetTransform().trans;
1116  ab.extents = Vector(0,0,0);
1117  }
1118  else {
1119  ab.pos = (dReal)0.5 * (vmin + vmax);
1120  ab.extents = vmax - ab.pos;
1121  }
1122  return ab;
1123 }
1124 
1126 {
1127  // find center of mass and set the outer transform to it
1128  Vector center;
1129  dReal fTotalMass = 0;
1130 
1131  FOREACHC(itlink, _veclinks) {
1132  center += ((*itlink)->GetTransform() * (*itlink)->GetCOMOffset() * (*itlink)->GetMass());
1133  fTotalMass += (*itlink)->GetMass();
1134  }
1135 
1136  if( fTotalMass > 0 ) {
1137  center /= fTotalMass;
1138  }
1139  return center;
1140 }
1141 
1142 void KinBody::SetLinkTransformations(const std::vector<Transform>& vbodies)
1143 {
1145  RAVELOG_WARN("SetLinkTransformations should be called with dofbranches, setting all branches to 0\n");
1146  }
1147  else {
1148  RAVELOG_DEBUG("SetLinkTransformations should be called with dofbranches, setting all branches to 0\n");
1149  }
1150  OPENRAVE_ASSERT_OP_FORMAT(vbodies.size(), >=, _veclinks.size(), "not enough links %d<%d", vbodies.size()%_veclinks.size(),ORE_InvalidArguments);
1151  vector<Transform>::const_iterator it;
1152  vector<LinkPtr>::iterator itlink;
1153  for(it = vbodies.begin(), itlink = _veclinks.begin(); it != vbodies.end(); ++it, ++itlink) {
1154  (*itlink)->SetTransform(*it);
1155  }
1156  FOREACH(itjoint,_vecjoints) {
1157  for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
1158  (*itjoint)->_dofbranches[i] = 0;
1159  }
1160  }
1161  _nUpdateStampId++;
1162 }
1163 
1164 void KinBody::SetLinkTransformations(const std::vector<Transform>& transforms, const std::vector<int>& dofbranches)
1165 {
1166  OPENRAVE_ASSERT_OP_FORMAT(transforms.size(), >=, _veclinks.size(), "not enough links %d<%d", transforms.size()%_veclinks.size(),ORE_InvalidArguments);
1167  vector<Transform>::const_iterator it;
1168  vector<LinkPtr>::iterator itlink;
1169  for(it = transforms.begin(), itlink = _veclinks.begin(); it != transforms.end(); ++it, ++itlink) {
1170  (*itlink)->SetTransform(*it);
1171  }
1172  FOREACH(itjoint,_vecjoints) {
1173  for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
1174  (*itjoint)->_dofbranches[i] = dofbranches.at((*itjoint)->GetDOFIndex()+i);
1175  }
1176  }
1177  _nUpdateStampId++;
1178 }
1179 
1180 void KinBody::SetLinkVelocities(const std::vector<std::pair<Vector,Vector> >& velocities)
1181 {
1182  GetEnv()->GetPhysicsEngine()->SetLinkVelocities(shared_kinbody(),velocities);
1183 }
1184 
1185 void KinBody::SetDOFValues(const std::vector<dReal>& vJointValues, const Transform& transBase, uint32_t checklimits)
1186 {
1187  if( _veclinks.size() == 0 ) {
1188  return;
1189  }
1190  Transform tbase = transBase*_veclinks.at(0)->GetTransform().inverse();
1191  _veclinks.at(0)->SetTransform(transBase);
1192 
1193  // apply the relative transformation to all links!! (needed for passive joints)
1194  for(size_t i = 1; i < _veclinks.size(); ++i) {
1195  _veclinks[i]->SetTransform(tbase*_veclinks[i]->GetTransform());
1196  }
1197  SetDOFValues(vJointValues,checklimits);
1198 }
1199 
1200 void KinBody::SetDOFValues(const std::vector<dReal>& vJointValues, uint32_t checklimits, const std::vector<int>& dofindices)
1201 {
1203  _nUpdateStampId++;
1204  if( vJointValues.size() == 0 || _veclinks.size() == 0) {
1205  return;
1206  }
1207  int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() : GetDOF();
1208  OPENRAVE_ASSERT_OP_FORMAT((int)vJointValues.size(),>=,expecteddof, "not enough values %d<%d", vJointValues.size()%GetDOF(),ORE_InvalidArguments);
1209 
1210  const dReal* pJointValues = &vJointValues[0];
1211  if( checklimits != CLA_Nothing || dofindices.size() > 0 ) {
1212  _vTempJoints.resize(GetDOF());
1213  if( dofindices.size() > 0 ) {
1214  // user only set a certain number of indices, so have to fill the temporary array with the full set of values first
1215  // and then overwrite with the user set values
1216  GetDOFValues(_vTempJoints);
1217  for(size_t i = 0; i < dofindices.size(); ++i) {
1218  _vTempJoints.at(dofindices[i]) = pJointValues[i];
1219  }
1220  pJointValues = &_vTempJoints[0];
1221  }
1222  dReal* ptempjoints = &_vTempJoints[0];
1223 
1224  // check the limits
1225  vector<dReal> upperlim, lowerlim;
1226  FOREACHC(it, _vecjoints) {
1227  const dReal* p = pJointValues+(*it)->GetDOFIndex();
1228  OPENRAVE_ASSERT_OP( (*it)->GetDOF(), <=, 3 );
1229  (*it)->GetLimits(lowerlim, upperlim);
1230  if( (*it)->GetType() == JointSpherical ) {
1231  dReal fcurang = fmod(RaveSqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]),2*PI);
1232  if( fcurang < lowerlim[0] ) {
1233  if( fcurang < 1e-10 ) {
1234  *ptempjoints++ = lowerlim[0]; *ptempjoints++ = 0; *ptempjoints++ = 0;
1235  }
1236  else {
1237  dReal fmult = lowerlim[0]/fcurang;
1238  *ptempjoints++ = p[0]*fmult; *ptempjoints++ = p[1]*fmult; *ptempjoints++ = p[2]*fmult;
1239  }
1240  }
1241  else if( fcurang > upperlim[0] ) {
1242  if( fcurang < 1e-10 ) {
1243  *ptempjoints++ = upperlim[0]; *ptempjoints++ = 0; *ptempjoints++ = 0;
1244  }
1245  else {
1246  dReal fmult = upperlim[0]/fcurang;
1247  *ptempjoints++ = p[0]*fmult; *ptempjoints++ = p[1]*fmult; *ptempjoints++ = p[2]*fmult;
1248  }
1249  }
1250  else {
1251  *ptempjoints++ = p[0]; *ptempjoints++ = p[1]; *ptempjoints++ = p[2];
1252  }
1253  }
1254  else {
1255  for(int i = 0; i < (*it)->GetDOF(); ++i) {
1256  if( (*it)->IsCircular(i) ) {
1257  // don't normalize since user is expecting the values he sets are exactly returned via GetDOFValues
1258  *ptempjoints++ = p[i]; //utils::NormalizeCircularAngle(p[i],(*it)->_vcircularlowerlimit[i],(*it)->_vcircularupperlimit[i]);
1259  }
1260  else {
1261  if( p[i] < lowerlim[i] ) {
1262  if( p[i] < lowerlim[i]-g_fEpsilonEvalJointLimit ) {
1263  if( checklimits == CLA_CheckLimits ) {
1264  RAVELOG_WARN(str(boost::format("dof %d value is not in limits %e<%e")%((*it)->GetDOFIndex()+i)%p[i]%lowerlim[i]));
1265  }
1266  else if( checklimits == CLA_CheckLimitsThrow ) {
1267  throw OPENRAVE_EXCEPTION_FORMAT("dof %d value is not in limits %e<%e", ((*it)->GetDOFIndex()+i)%p[i]%lowerlim[i], ORE_InvalidArguments);
1268  }
1269  }
1270  *ptempjoints++ = lowerlim[i];
1271  }
1272  else if( p[i] > upperlim[i] ) {
1273  if( p[i] > upperlim[i]+g_fEpsilonEvalJointLimit ) {
1274  if( checklimits == CLA_CheckLimits ) {
1275  RAVELOG_WARN(str(boost::format("dof %d value is not in limits %e<%e")%((*it)->GetDOFIndex()+i)%p[i]%upperlim[i]));
1276  }
1277  else if( checklimits == CLA_CheckLimitsThrow ) {
1278  throw OPENRAVE_EXCEPTION_FORMAT("dof %d value is not in limits %e<%e",((*it)->GetDOFIndex()+i)%p[i]%upperlim[i], ORE_InvalidArguments);
1279  }
1280  }
1281  *ptempjoints++ = upperlim[i];
1282  }
1283  else {
1284  *ptempjoints++ = p[i];
1285  }
1286  }
1287  }
1288  }
1289  }
1290  pJointValues = &_vTempJoints[0];
1291  }
1292 
1293  boost::array<dReal,3> dummyvalues; // dummy values for a joint
1294  std::vector<dReal> vtempvalues, veval;
1295 
1296  // have to compute the angles ahead of time since they are dependent on the link transformations
1297  std::vector< std::vector<dReal> > vPassiveJointValues(_vPassiveJoints.size());
1298  for(size_t i = 0; i < vPassiveJointValues.size(); ++i) {
1299  if( !_vPassiveJoints[i]->IsMimic() ) {
1300  _vPassiveJoints[i]->GetValues(vPassiveJointValues[i]);
1301  // check if out of limits!
1302  for(size_t j = 0; j < vPassiveJointValues[i].size(); ++j) {
1303  if( !_vPassiveJoints[i]->IsCircular(j) ) {
1304  if( vPassiveJointValues[i][j] < _vPassiveJoints[i]->_info._vlowerlimit.at(j) ) {
1305  if( vPassiveJointValues[i][j] < _vPassiveJoints[i]->_info._vlowerlimit.at(j)-5e-4f ) {
1306  RAVELOG_WARN(str(boost::format("dummy joint out of lower limit! %e < %e\n")%_vPassiveJoints[i]->_info._vlowerlimit.at(j)%vPassiveJointValues[i][j]));
1307  }
1308  vPassiveJointValues[i][j] = _vPassiveJoints[i]->_info._vlowerlimit.at(j);
1309  }
1310  else if( vPassiveJointValues[i][j] > _vPassiveJoints[i]->_info._vupperlimit.at(j) ) {
1311  if( vPassiveJointValues[i][j] > _vPassiveJoints[i]->_info._vupperlimit.at(j)+5e-4f ) {
1312  RAVELOG_WARN(str(boost::format("dummy joint out of upper limit! %e > %e\n")%_vPassiveJoints[i]->_info._vupperlimit.at(j)%vPassiveJointValues[i][j]));
1313  }
1314  vPassiveJointValues[i][j] = _vPassiveJoints[i]->_info._vupperlimit.at(j);
1315  }
1316  }
1317  }
1318  }
1319  else {
1320  vPassiveJointValues[i].reserve(_vPassiveJoints[i]->GetDOF()); // do not resize so that we can catch hierarchy errors
1321  }
1322  }
1323 
1324  std::vector<uint8_t> vlinkscomputed(_veclinks.size(),0);
1325  vlinkscomputed[0] = 1;
1326 
1327  for(size_t ijoint = 0; ijoint < _vTopologicallySortedJointsAll.size(); ++ijoint) {
1328  JointPtr pjoint = _vTopologicallySortedJointsAll[ijoint];
1329  int jointindex = _vTopologicallySortedJointIndicesAll[ijoint];
1330  int dofindex = pjoint->GetDOFIndex();
1331  const dReal* pvalues=dofindex >= 0 ? pJointValues + dofindex : NULL;
1332  if( pjoint->IsMimic() ) {
1333  for(int i = 0; i < pjoint->GetDOF(); ++i) {
1334  if( pjoint->IsMimic(i) ) {
1335  vtempvalues.resize(0);
1336  const std::vector<Mimic::DOFFormat>& vdofformat = pjoint->_vmimic[i]->_vdofformat;
1337  FOREACHC(itdof,vdofformat) {
1338  if( itdof->dofindex >= 0 ) {
1339  vtempvalues.push_back(pJointValues[itdof->dofindex]);
1340  }
1341  else {
1342  vtempvalues.push_back(vPassiveJointValues.at(itdof->jointindex-_vecjoints.size()).at(itdof->axis));
1343  }
1344  }
1345  int err = pjoint->_Eval(i, 0, vtempvalues, veval);
1346  if( err ) {
1347  RAVELOG_WARN(str(boost::format("failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
1348  }
1349  else {
1350  vector<dReal> vevalcopy = veval;
1351  vector<dReal>::iterator iteval = veval.begin();
1352  while(iteval != veval.end()) {
1353  bool removevalue = false;
1354  if( pjoint->GetType() == JointSpherical || pjoint->IsCircular(i) ) {
1355  }
1356  else if( *iteval < pjoint->_info._vlowerlimit[i] ) {
1357  if(*iteval >= pjoint->_info._vlowerlimit[i]-g_fEpsilonJointLimit ) {
1358  *iteval = pjoint->_info._vlowerlimit[i];
1359  }
1360  else {
1361  removevalue=true;
1362  }
1363  }
1364  else if( *iteval > pjoint->_info._vupperlimit[i] ) {
1365  if(*iteval <= pjoint->_info._vupperlimit[i]+g_fEpsilonJointLimit ) {
1366  *iteval = pjoint->_info._vupperlimit[i];
1367  }
1368  else {
1369  removevalue=true;
1370  }
1371  }
1372 
1373  if( removevalue ) {
1374  iteval = veval.erase(iteval); // invalid value so remove from candidates
1375  }
1376  else {
1377  ++iteval;
1378  }
1379  }
1380 
1381  if( veval.empty() ) {
1382  FORIT(iteval,vevalcopy) {
1383  if( checklimits == CLA_Nothing || pjoint->GetType() == JointSpherical || pjoint->IsCircular(i) ) {
1384  veval.push_back(*iteval);
1385  }
1386  else if( *iteval < pjoint->_info._vlowerlimit[i]-g_fEpsilonEvalJointLimit ) {
1387  veval.push_back(pjoint->_info._vlowerlimit[i]);
1388  if( checklimits == CLA_CheckLimits ) {
1389  RAVELOG_WARN(str(boost::format("joint %s: lower limit (%e) is not followed: %e")%pjoint->GetName()%pjoint->_info._vlowerlimit[i]%*iteval));
1390  }
1391  else if( checklimits == CLA_CheckLimitsThrow ) {
1392  throw OPENRAVE_EXCEPTION_FORMAT("joint %s: lower limit (%e) is not followed: %e", pjoint->GetName()%pjoint->_info._vlowerlimit[i]%*iteval, ORE_InvalidArguments);
1393  }
1394  }
1395  else if( *iteval > pjoint->_info._vupperlimit[i]+g_fEpsilonEvalJointLimit ) {
1396  veval.push_back(pjoint->_info._vupperlimit[i]);
1397  if( checklimits == CLA_CheckLimits ) {
1398  RAVELOG_WARN(str(boost::format("joint %s: upper limit (%e) is not followed: %e")%pjoint->GetName()%pjoint->_info._vupperlimit[i]%*iteval));
1399  }
1400  else if( checklimits == CLA_CheckLimitsThrow ) {
1401  throw OPENRAVE_EXCEPTION_FORMAT("joint %s: upper limit (%e) is not followed: %e", pjoint->GetName()%pjoint->_info._vupperlimit[i]%*iteval, ORE_InvalidArguments);
1402  }
1403  }
1404  else {
1405  veval.push_back(*iteval);
1406  }
1407  }
1408  OPENRAVE_ASSERT_FORMAT(!veval.empty(), "no valid values for joint %s", pjoint->GetName(),ORE_Assert);
1409  }
1410  if( veval.size() > 1 ) {
1411  stringstream ss; ss << std::setprecision(std::numeric_limits<dReal>::digits10+1);
1412  ss << "multiplie values for joint " << pjoint->GetName() << ": ";
1413  FORIT(iteval,veval) {
1414  ss << *iteval << " ";
1415  }
1416  RAVELOG_WARN(ss.str());
1417  }
1418  dummyvalues[i] = veval.at(0);
1419  }
1420 
1421  // if joint is passive, update the stored joint values! This is necessary because joint value might be referenced in the future.
1422  if( dofindex < 0 ) {
1423  vPassiveJointValues.at(jointindex-(int)_vecjoints.size()).resize(pjoint->GetDOF());
1424  vPassiveJointValues.at(jointindex-(int)_vecjoints.size()).at(i) = dummyvalues[i];
1425  }
1426  }
1427  else if( dofindex >= 0 ) {
1428  dummyvalues[i] = pvalues[dofindex+i]; // is this correct? what is a joint has a mimic and non-mimic axis?
1429  }
1430  else {
1431  // preserve passive joint values
1432  dummyvalues[i] = vPassiveJointValues.at(jointindex-(int)_vecjoints.size()).at(i);
1433  }
1434  }
1435  pvalues = &dummyvalues[0];
1436  }
1437  // do the test after mimic computation!
1438  if( vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] ) {
1439  continue;
1440  }
1441  if( !pvalues ) {
1442  // has to be a passive joint
1443  pvalues = &vPassiveJointValues.at(jointindex-(int)_vecjoints.size()).at(0);
1444  }
1445 
1446  Transform tjoint;
1447  if( pjoint->GetType() & JointSpecialBit ) {
1448  switch(pjoint->GetType()) {
1449  case JointHinge2: {
1450  Transform tfirst;
1451  tfirst.rot = quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(0), pvalues[0]);
1452  Transform tsecond;
1453  tsecond.rot = quatFromAxisAngle(tfirst.rotate(pjoint->GetInternalHierarchyAxis(1)), pvalues[1]);
1454  tjoint = tsecond * tfirst;
1455  pjoint->_dofbranches[0] = CountCircularBranches(pvalues[0]-pjoint->GetWrapOffset(0));
1456  pjoint->_dofbranches[1] = CountCircularBranches(pvalues[1]-pjoint->GetWrapOffset(1));
1457  break;
1458  }
1459  case JointSpherical: {
1460  dReal fang = pvalues[0]*pvalues[0]+pvalues[1]*pvalues[1]+pvalues[2]*pvalues[2];
1461  if( fang > 0 ) {
1462  fang = RaveSqrt(fang);
1463  dReal fiang = 1/fang;
1464  tjoint.rot = quatFromAxisAngle(Vector(pvalues[0]*fiang,pvalues[1]*fiang,pvalues[2]*fiang),fang);
1465  }
1466  break;
1467  }
1468  case JointTrajectory: {
1469  vector<dReal> vdata;
1470  tjoint = Transform();
1471  dReal fvalue = pvalues[0];
1472  if( pjoint->IsCircular(0) ) {
1473  // need to normalize the value
1474  fvalue = utils::NormalizeCircularAngle(fvalue,pjoint->_vcircularlowerlimit.at(0), pjoint->_vcircularupperlimit.at(0));
1475  }
1476  pjoint->_info._trajfollow->Sample(vdata,fvalue);
1477  if( !pjoint->_info._trajfollow->GetConfigurationSpecification().ExtractTransform(tjoint,vdata.begin(),KinBodyConstPtr()) ) {
1478  RAVELOG_WARN(str(boost::format("trajectory sampling for joint %s failed")%pjoint->GetName()));
1479  }
1480  pjoint->_dofbranches[0] = 0;
1481  break;
1482  }
1483  default:
1484  RAVELOG_WARN(str(boost::format("forward kinematic type 0x%x not supported")%pjoint->GetType()));
1485  break;
1486  }
1487  }
1488  else {
1489  if( pjoint->GetType() == JointRevolute ) {
1490  tjoint.rot = quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(0), pvalues[0]);
1491  pjoint->_dofbranches[0] = CountCircularBranches(pvalues[0]-pjoint->GetWrapOffset(0));
1492  }
1493  else if( pjoint->GetType() == JointPrismatic ) {
1494  tjoint.trans = pjoint->GetInternalHierarchyAxis(0) * pvalues[0];
1495  }
1496  else {
1497  for(int iaxis = 0; iaxis < pjoint->GetDOF(); ++iaxis) {
1498  Transform tdelta;
1499  if( pjoint->IsRevolute(iaxis) ) {
1500  tdelta.rot = quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(iaxis), pvalues[iaxis]);
1501  pjoint->_dofbranches[iaxis] = CountCircularBranches(pvalues[iaxis]-pjoint->GetWrapOffset(iaxis));
1502  }
1503  else {
1504  tdelta.trans = pjoint->GetInternalHierarchyAxis(iaxis) * pvalues[iaxis];
1505  }
1506  tjoint = tjoint * tdelta;
1507  }
1508  }
1509  }
1510 
1511  Transform t = pjoint->GetInternalHierarchyLeftTransform() * tjoint * pjoint->GetInternalHierarchyRightTransform();
1512  if( !pjoint->GetHierarchyParentLink() ) {
1513  t = _veclinks.at(0)->GetTransform() * t;
1514  }
1515  else {
1516  t = pjoint->GetHierarchyParentLink()->GetTransform() * t;
1517  }
1518  pjoint->GetHierarchyChildLink()->SetTransform(t);
1519  vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] = 1;
1520  }
1521 }
1522 
1523 KinBody::LinkPtr KinBody::GetLink(const std::string& linkname) const
1524 {
1525  for(std::vector<LinkPtr>::const_iterator it = _veclinks.begin(); it != _veclinks.end(); ++it) {
1526  if ( (*it)->GetName() == linkname ) {
1527  return *it;
1528  }
1529  }
1530  return LinkPtr();
1531 }
1532 
1533 const std::vector<KinBody::JointPtr>& KinBody::GetDependencyOrderedJoints() const
1534 {
1537 }
1538 
1539 const std::vector< std::vector< std::pair<KinBody::LinkPtr, KinBody::JointPtr> > >& KinBody::GetClosedLoops() const
1540 {
1542  return _vClosedLoops;
1543 }
1544 
1545 bool KinBody::GetChain(int linkindex1, int linkindex2, std::vector<JointPtr>& vjoints) const
1546 {
1548  OPENRAVE_ASSERT_FORMAT(linkindex1>=0 && linkindex1<(int)_veclinks.size(), "body %s linkindex1 %d invalid (num links %d)", GetName()%linkindex1%_veclinks.size(), ORE_InvalidArguments);
1549  OPENRAVE_ASSERT_FORMAT(linkindex2>=0 && linkindex2<(int)_veclinks.size(), "body %s linkindex2 %d invalid (num links %d)", GetName()%linkindex2%_veclinks.size(), ORE_InvalidArguments);
1550  vjoints.resize(0);
1551  if( linkindex1 == linkindex2 ) {
1552  return true;
1553  }
1554  int offset = linkindex2*_veclinks.size();
1555  int curlink = linkindex1;
1556  while(_vAllPairsShortestPaths[offset+curlink].first>=0) {
1557  int jointindex = _vAllPairsShortestPaths[offset+curlink].second;
1558  vjoints.push_back(jointindex < (int)_vecjoints.size() ? _vecjoints.at(jointindex) : _vPassiveJoints.at(jointindex-_vecjoints.size()));
1559  int prevlink = curlink;
1560  curlink = _vAllPairsShortestPaths[offset+curlink].first;
1561  OPENRAVE_ASSERT_OP(prevlink,!=,curlink); // avoid loops
1562  }
1563  return vjoints.size()>0; // otherwise disconnected
1564 }
1565 
1566 bool KinBody::GetChain(int linkindex1, int linkindex2, std::vector<LinkPtr>& vlinks) const
1567 {
1569  OPENRAVE_ASSERT_FORMAT(linkindex1>=0 && linkindex1<(int)_veclinks.size(), "body %s linkindex1 %d invalid (num links %d)", GetName()%linkindex1%_veclinks.size(), ORE_InvalidArguments);
1570  OPENRAVE_ASSERT_FORMAT(linkindex2>=0 && linkindex2<(int)_veclinks.size(), "body %s linkindex2 %d invalid (num links %d)", GetName()%linkindex2%_veclinks.size(), ORE_InvalidArguments);
1571  vlinks.resize(0);
1572  int offset = linkindex2*_veclinks.size();
1573  int curlink = linkindex1;
1574  if( _vAllPairsShortestPaths[offset+curlink].first < 0 ) {
1575  return false;
1576  }
1577  vlinks.push_back(_veclinks.at(linkindex1));
1578  if( linkindex1 == linkindex2 ) {
1579  return true;
1580  }
1581  while(_vAllPairsShortestPaths[offset+curlink].first != linkindex2) {
1582  curlink = _vAllPairsShortestPaths[offset+curlink].first;
1583  if( curlink < 0 ) {
1584  vlinks.resize(0);
1585  return false;
1586  }
1587  vlinks.push_back(_veclinks.at(curlink));
1588  }
1589  vlinks.push_back(_veclinks.at(linkindex2));
1590  return true; // otherwise disconnected
1591 }
1592 
1593 bool KinBody::IsDOFInChain(int linkindex1, int linkindex2, int dofindex) const
1594 {
1596  int jointindex = _vDOFIndices.at(dofindex);
1597  return (DoesAffect(jointindex,linkindex1)==0) != (DoesAffect(jointindex,linkindex2)==0);
1598 }
1599 
1600 int KinBody::GetJointIndex(const std::string& jointname) const
1601 {
1602  int index = 0;
1603  FOREACHC(it,_vecjoints) {
1604  if ((*it)->GetName() == jointname ) {
1605  return index;
1606  }
1607  ++index;
1608  }
1609  return -1;
1610 }
1611 
1612 KinBody::JointPtr KinBody::GetJoint(const std::string& jointname) const
1613 {
1614  FOREACHC(it,_vecjoints) {
1615  if ((*it)->GetName() == jointname ) {
1616  return *it;
1617  }
1618  }
1619  FOREACHC(it,_vPassiveJoints) {
1620  if ((*it)->GetName() == jointname ) {
1621  return *it;
1622  }
1623  }
1624  return JointPtr();
1625 }
1626 
1627 void KinBody::ComputeJacobianTranslation(int linkindex, const Vector& position, vector<dReal>& vjacobian,const std::vector<int>& dofindices) const
1628 {
1630  OPENRAVE_ASSERT_FORMAT(linkindex >= 0 && linkindex < (int)_veclinks.size(), "body %s bad link index %d (num links %d)", GetName()%linkindex%_veclinks.size(),ORE_InvalidArguments);
1631  size_t dofstride=0;
1632  if( dofindices.size() > 0 ) {
1633  dofstride = dofindices.size();
1634  }
1635  else {
1636  dofstride = GetDOF();
1637  }
1638  vjacobian.resize(3*dofstride);
1639  if( dofstride == 0 ) {
1640  return;
1641  }
1642  std::fill(vjacobian.begin(),vjacobian.end(),0);
1643 
1644  Vector v;
1645  int offset = linkindex*_veclinks.size();
1646  int curlink = 0;
1647  std::vector<std::pair<int,dReal> > vpartials;
1648  std::vector<int> vpartialindices;
1649  std::map< std::pair<Mimic::DOFFormat, int>, dReal > mapcachedpartials;
1650  while(_vAllPairsShortestPaths[offset+curlink].first>=0) {
1651  int jointindex = _vAllPairsShortestPaths[offset+curlink].second;
1652  if( jointindex < (int)_vecjoints.size() ) {
1653  // active joint
1654  JointPtr pjoint = _vecjoints.at(jointindex);
1655  int dofindex = pjoint->GetDOFIndex();
1656  int8_t affect = DoesAffect(pjoint->GetJointIndex(), linkindex);
1657  for(int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1658  if( affect != 0 ) {
1659  if( pjoint->IsRevolute(dof) ) {
1660  v = pjoint->GetAxis(dof).cross(position-pjoint->GetAnchor());
1661  }
1662  else if( pjoint->IsPrismatic(dof) ) {
1663  v = pjoint->GetAxis(dof);
1664  }
1665  else {
1666  RAVELOG_WARN("ComputeJacobianTranslation joint %d not supported\n", pjoint->GetType());
1667  continue;
1668  }
1669  if( dofindices.size() > 0 ) {
1670  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),dofindex+dof);
1671  if( itindex != dofindices.end() ) {
1672  size_t index = itindex-dofindices.begin();
1673  vjacobian[index] += v.x; vjacobian[index+dofstride] += v.y; vjacobian[index+2*dofstride] += v.z;
1674  }
1675  }
1676  else {
1677  vjacobian[dofindex+dof] += v.x; vjacobian[dofstride+dofindex+dof] += v.y; vjacobian[2*dofstride+dofindex+dof] += v.z;
1678  }
1679  }
1680  }
1681  }
1682  else {
1683  // add in the contributions from the passive joint
1684  JointPtr pjoint = _vPassiveJoints.at(jointindex-_vecjoints.size());
1685  for(int idof = 0; idof < pjoint->GetDOF(); ++idof) {
1686  if( pjoint->IsMimic(idof) ) {
1687  bool bhas = dofindices.size() == 0;
1688  if( !bhas ) {
1689  FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
1690  if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
1691  bhas = true;
1692  break;
1693  }
1694  }
1695  }
1696  if( bhas ) {
1697  Vector vaxis;
1698  if( pjoint->IsRevolute(idof) ) {
1699  vaxis = pjoint->GetAxis(idof).cross(position-pjoint->GetAnchor());
1700  }
1701  else if( pjoint->IsPrismatic(idof) ) {
1702  vaxis = pjoint->GetAxis(idof);
1703  }
1704  else {
1705  RAVELOG_WARN("ComputeJacobianTranslation joint %d not supported\n", pjoint->GetType());
1706  continue;
1707  }
1708  pjoint->_ComputePartialVelocities(vpartials,idof,mapcachedpartials);
1709  FOREACH(itpartial,vpartials) {
1710  Vector v = vaxis * itpartial->second;
1711  int index = itpartial->first;
1712  if( dofindices.size() > 0 ) {
1713  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),itpartial->first);
1714  if( itindex == dofindices.end() ) {
1715  continue;
1716  }
1717  index = itindex-dofindices.begin();
1718  }
1719  vjacobian[index] += v.x;
1720  vjacobian[dofstride+index] += v.y;
1721  vjacobian[2*dofstride+index] += v.z;
1722  }
1723  }
1724  }
1725  }
1726  }
1727  curlink = _vAllPairsShortestPaths[offset+curlink].first;
1728  }
1729 }
1730 
1731 void KinBody::CalculateJacobian(int linkindex, const Vector& trans, boost::multi_array<dReal,2>& mjacobian) const
1732 {
1733  mjacobian.resize(boost::extents[3][GetDOF()]);
1734  if( GetDOF() == 0 ) {
1735  return;
1736  }
1737  std::vector<dReal> vjacobian;
1738  ComputeJacobianTranslation(linkindex,trans,vjacobian);
1739  OPENRAVE_ASSERT_OP((int)vjacobian.size(),==,3*GetDOF());
1740  vector<dReal>::const_iterator itsrc = vjacobian.begin();
1741  FOREACH(itdst,mjacobian) {
1742  std::copy(itsrc,itsrc+GetDOF(),itdst->begin());
1743  itsrc += GetDOF();
1744  }
1745 }
1746 
1747 void KinBody::CalculateRotationJacobian(int linkindex, const Vector& q, std::vector<dReal>& vjacobian) const
1748 {
1750  OPENRAVE_ASSERT_FORMAT(linkindex >= 0 && linkindex < (int)_veclinks.size(), "body %s bad link index %d (num links %d)", GetName()%linkindex%_veclinks.size(),ORE_InvalidArguments);
1751  int dofstride = GetDOF();
1752  vjacobian.resize(4*dofstride);
1753  if( dofstride == 0 ) {
1754  return;
1755  }
1756  std::fill(vjacobian.begin(),vjacobian.end(),0);
1757  Vector v;
1758  int offset = linkindex*_veclinks.size();
1759  int curlink = 0;
1760  std::vector<std::pair<int,dReal> > vpartials;
1761  std::map< std::pair<Mimic::DOFFormat, int>, dReal > mapcachedpartials;
1762  while(_vAllPairsShortestPaths[offset+curlink].first>=0) {
1763  int jointindex = _vAllPairsShortestPaths[offset+curlink].second;
1764  if( jointindex < (int)_vecjoints.size() ) {
1765  // active joint
1766  JointPtr pjoint = _vecjoints.at(jointindex);
1767  int dofindex = pjoint->GetDOFIndex();
1768  int8_t affect = DoesAffect(pjoint->GetJointIndex(), linkindex);
1769  for(int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1770  if( affect == 0 ) {
1771  RAVELOG_WARN(str(boost::format("link %s should be affected by joint %s")%_veclinks.at(linkindex)->GetName()%pjoint->GetName()));
1772  }
1773  else {
1774  if( pjoint->IsRevolute(dof) ) {
1775  v = pjoint->GetAxis(dof);
1776  }
1777  else if( pjoint->IsPrismatic(dof) ) {
1778  v = Vector(0,0,0);
1779  }
1780  else {
1781  RAVELOG_WARN("CalculateRotationJacobian joint %d not supported\n", pjoint->GetType());
1782  v = Vector(0,0,0);
1783  }
1784  vjacobian[dofindex+dof] += dReal(0.5)*(-q.y*v.x - q.z*v.y - q.w*v.z);
1785  vjacobian[dofstride+dofindex+dof] += dReal(0.5)*(q.x*v.x - q.z*v.z + q.w*v.y);
1786  vjacobian[2*dofstride+dofindex+dof] += dReal(0.5)*(q.x*v.y + q.y*v.z - q.w*v.x);
1787  vjacobian[3*dofstride+dofindex+dof] += dReal(0.5)*(q.x*v.z - q.y*v.y + q.z*v.x);
1788  }
1789  }
1790  }
1791  else {
1792  // add in the contributions from the passive joint
1793  JointPtr pjoint = _vPassiveJoints.at(jointindex-_vecjoints.size());
1794  for(int idof = 0; idof < pjoint->GetDOF(); ++idof) {
1795  if( pjoint->IsMimic(idof) ) {
1796  Vector vaxis;
1797  if( pjoint->IsRevolute(idof) ) {
1798  vaxis = pjoint->GetAxis(idof);
1799  }
1800  else if( pjoint->IsPrismatic(idof) ) {
1801  vaxis = Vector(0,0,0);
1802  }
1803  else {
1804  RAVELOG_WARN("CalculateRotationJacobian joint %d not supported\n", pjoint->GetType());
1805  continue;
1806  }
1807  pjoint->_ComputePartialVelocities(vpartials,idof,mapcachedpartials);
1808  FOREACH(itpartial,vpartials) {
1809  int dofindex = itpartial->first;
1810  Vector v = vaxis * itpartial->second;
1811  vjacobian[dofindex] += dReal(0.5)*(-q.y*v.x - q.z*v.y - q.w*v.z);
1812  vjacobian[dofstride+dofindex] += dReal(0.5)*(q.x*v.x - q.z*v.z + q.w*v.y);
1813  vjacobian[2*dofstride+dofindex] += dReal(0.5)*(q.x*v.y + q.y*v.z - q.w*v.x);
1814  vjacobian[3*dofstride+dofindex] += dReal(0.5)*(q.x*v.z - q.y*v.y + q.z*v.x);
1815  }
1816  }
1817  }
1818  }
1819  curlink = _vAllPairsShortestPaths[offset+curlink].first;
1820  }
1821 }
1822 
1823 void KinBody::CalculateRotationJacobian(int linkindex, const Vector& q, boost::multi_array<dReal,2>& mjacobian) const
1824 {
1825  mjacobian.resize(boost::extents[4][GetDOF()]);
1826  if( GetDOF() == 0 ) {
1827  return;
1828  }
1829  std::vector<dReal> vjacobian;
1830  CalculateRotationJacobian(linkindex,q,vjacobian);
1831  OPENRAVE_ASSERT_OP((int)vjacobian.size(),==,4*GetDOF());
1832  vector<dReal>::const_iterator itsrc = vjacobian.begin();
1833  FOREACH(itdst,mjacobian) {
1834  std::copy(itsrc,itsrc+GetDOF(),itdst->begin());
1835  itsrc += GetDOF();
1836  }
1837 }
1838 
1839 void KinBody::ComputeJacobianAxisAngle(int linkindex, std::vector<dReal>& vjacobian, const std::vector<int>& dofindices) const
1840 {
1842  OPENRAVE_ASSERT_FORMAT(linkindex >= 0 && linkindex < (int)_veclinks.size(), "body %s bad link index %d (num links %d)", GetName()%linkindex%_veclinks.size(),ORE_InvalidArguments);
1843  size_t dofstride=0;
1844  if( dofindices.size() > 0 ) {
1845  dofstride = dofindices.size();
1846  }
1847  else {
1848  dofstride = GetDOF();
1849  }
1850  vjacobian.resize(3*dofstride);
1851  if( dofstride == 0 ) {
1852  return;
1853  }
1854  std::fill(vjacobian.begin(),vjacobian.end(),0);
1855 
1856  Vector v, anchor, axis;
1857  int offset = linkindex*_veclinks.size();
1858  int curlink = 0;
1859  std::vector<std::pair<int,dReal> > vpartials;
1860  std::map< std::pair<Mimic::DOFFormat, int>, dReal > mapcachedpartials;
1861  while(_vAllPairsShortestPaths[offset+curlink].first>=0) {
1862  int jointindex = _vAllPairsShortestPaths[offset+curlink].second;
1863  if( jointindex < (int)_vecjoints.size() ) {
1864  // active joint
1865  JointPtr pjoint = _vecjoints.at(jointindex);
1866  int dofindex = pjoint->GetDOFIndex();
1867  int8_t affect = DoesAffect(pjoint->GetJointIndex(), linkindex);
1868  for(int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1869  if( affect != 0 ) {
1870  if( pjoint->IsRevolute(dof) ) {
1871  v = pjoint->GetAxis(dof);
1872  }
1873  else if( pjoint->IsPrismatic(dof) ) {
1874  continue;
1875  }
1876  else {
1877  RAVELOG_WARN("ComputeJacobianAxisAngle joint %d not supported\n", pjoint->GetType());
1878  continue;
1879  }
1880  if( dofindices.size() > 0 ) {
1881  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),dofindex+dof);
1882  if( itindex != dofindices.end() ) {
1883  size_t index = itindex-dofindices.begin();
1884  vjacobian[index] += v.x; vjacobian[index+dofstride] += v.y; vjacobian[index+2*dofstride] += v.z;
1885  }
1886  }
1887  else {
1888  vjacobian[dofindex+dof] += v.x; vjacobian[dofstride+dofindex+dof] += v.y; vjacobian[2*dofstride+dofindex+dof] += v.z;
1889  }
1890  }
1891  }
1892  }
1893  else {
1894  // add in the contributions from the passive joint
1895  JointPtr pjoint = _vPassiveJoints.at(jointindex-_vecjoints.size());
1896  for(int idof = 0; idof < pjoint->GetDOF(); ++idof) {
1897  if( pjoint->IsMimic(idof) ) {
1898  bool bhas = dofindices.size() == 0;
1899  if( !bhas ) {
1900  FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
1901  if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
1902  bhas = true;
1903  break;
1904  }
1905  }
1906  }
1907  if( bhas ) {
1908  Vector vaxis;
1909  if( pjoint->IsRevolute(idof) ) {
1910  vaxis = pjoint->GetAxis(idof);
1911  }
1912  else if( pjoint->IsPrismatic(idof) ) {
1913  continue;
1914  }
1915  else {
1916  RAVELOG_WARN("ComputeJacobianAxisAngle joint %d not supported\n", pjoint->GetType());
1917  continue;
1918  }
1919  pjoint->_ComputePartialVelocities(vpartials,idof,mapcachedpartials);
1920  FOREACH(itpartial,vpartials) {
1921  Vector v = vaxis * itpartial->second;
1922  int index = itpartial->first;
1923  if( dofindices.size() > 0 ) {
1924  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),itpartial->first);
1925  if( itindex == dofindices.end() ) {
1926  continue;
1927  }
1928  index = itindex-dofindices.begin();
1929  }
1930  vjacobian[index] += v.x;
1931  vjacobian[dofstride+index] += v.y;
1932  vjacobian[2*dofstride+index] += v.z;
1933  }
1934  }
1935  }
1936  }
1937  }
1938  curlink = _vAllPairsShortestPaths[offset+curlink].first;
1939  }
1940 }
1941 
1942 void KinBody::CalculateAngularVelocityJacobian(int linkindex, boost::multi_array<dReal,2>& mjacobian) const
1943 {
1944  mjacobian.resize(boost::extents[3][GetDOF()]);
1945  if( GetDOF() == 0 ) {
1946  return;
1947  }
1948  std::vector<dReal> vjacobian;
1949  ComputeJacobianAxisAngle(linkindex,vjacobian);
1950  OPENRAVE_ASSERT_OP((int)vjacobian.size(),==,3*GetDOF());
1951  vector<dReal>::const_iterator itsrc = vjacobian.begin();
1952  FOREACH(itdst,mjacobian) {
1953  std::copy(itsrc,itsrc+GetDOF(),itdst->begin());
1954  itsrc += GetDOF();
1955  }
1956 }
1957 
1958 void KinBody::ComputeHessianTranslation(int linkindex, const Vector& position, std::vector<dReal>& hessian, const std::vector<int>& dofindices) const
1959 {
1961  OPENRAVE_ASSERT_FORMAT(linkindex >= 0 && linkindex < (int)_veclinks.size(), "body %s bad link index %d (num links %d)", GetName()%linkindex%_veclinks.size(),ORE_InvalidArguments);
1962  size_t dofstride=0;
1963  if( dofindices.size() > 0 ) {
1964  dofstride = dofindices.size();
1965  }
1966  else {
1967  dofstride = GetDOF();
1968  }
1969  hessian.resize(dofstride*3*dofstride);
1970  if( dofstride == 0 ) {
1971  return;
1972  }
1973  std::fill(hessian.begin(),hessian.end(),0);
1974 
1975  int offset = linkindex*_veclinks.size();
1976  int curlink = 0;
1977  std::vector<Vector> vaxes, vjacobian; vaxes.reserve(dofstride); vjacobian.reserve(dofstride);
1978  std::vector<int> vpartialindices;
1979  std::map< std::pair<Mimic::DOFFormat, int>, dReal > mapcachedpartials;
1980  std::vector<int> vinsertedindices; vinsertedindices.reserve(dofstride);
1981  typedef std::pair< std::vector<Vector>, std::vector<std::pair<int,dReal> > > PartialInfo;
1982  std::map<size_t, PartialInfo > mappartialsinserted; // if vinsertedindices has -1, that index will be here
1983  while(_vAllPairsShortestPaths[offset+curlink].first>=0) {
1984  int jointindex = _vAllPairsShortestPaths[offset+curlink].second;
1985  if( jointindex < (int)_vecjoints.size() ) {
1986  // active joint
1987  JointPtr pjoint = _vecjoints.at(jointindex);
1988  int dofindex = pjoint->GetDOFIndex();
1989  int8_t affect = DoesAffect(pjoint->GetJointIndex(), linkindex);
1990  for(int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1991  if( affect == 0 ) {
1992  RAVELOG_WARN(str(boost::format("link %s should be affected by joint %s")%_veclinks.at(linkindex)->GetName()%pjoint->GetName()));
1993  }
1994  else {
1995  size_t index = dofindex+dof;
1996  if( dofindices.size() > 0 ) {
1997  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),dofindex+dof);
1998  if( itindex != dofindices.end() ) {
1999  index = itindex-dofindices.begin();
2000  }
2001  else {
2002  continue;
2003  }
2004  }
2005 
2006  if( pjoint->IsRevolute(dof) ) {
2007  vaxes.push_back(pjoint->GetAxis(dof));
2008  vjacobian.push_back(pjoint->GetAxis(dof).cross(position-pjoint->GetAnchor()));
2009  }
2010  else if( pjoint->IsPrismatic(dof) ) {
2011  vaxes.push_back(Vector());
2012  vjacobian.push_back(pjoint->GetAxis(dof));
2013  }
2014  else {
2015  vaxes.push_back(Vector());
2016  vjacobian.push_back(Vector());
2017  RAVELOG_WARN("ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2018  }
2019  vinsertedindices.push_back(index);
2020  }
2021  }
2022  }
2023  else {
2024  // add in the contributions from the passive joint
2025  JointPtr pjoint = _vPassiveJoints.at(jointindex-_vecjoints.size());
2026  for(int idof = 0; idof < pjoint->GetDOF(); ++idof) {
2027  if( pjoint->IsMimic(idof) ) {
2028  bool bhas = dofindices.size() == 0;
2029  if( !bhas ) {
2030  FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
2031  if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
2032  bhas = true;
2033  break;
2034  }
2035  }
2036  }
2037  if( bhas ) {
2038  Vector vaxis;
2039  if( pjoint->IsRevolute(idof) ) {
2040  vaxes.push_back(pjoint->GetAxis(idof));
2041  vjacobian.push_back(pjoint->GetAxis(idof).cross(position-pjoint->GetAnchor()));
2042  }
2043  else if( pjoint->IsPrismatic(idof) ) {
2044  vjacobian.push_back(pjoint->GetAxis(idof));
2045  vaxes.push_back(Vector());
2046  }
2047  else {
2048  vaxes.push_back(Vector());
2049  vjacobian.push_back(Vector());
2050  RAVELOG_WARN("ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2051  }
2052  PartialInfo& partialinfo = mappartialsinserted[vinsertedindices.size()];
2053  partialinfo.first.resize(vinsertedindices.size());
2054  pjoint->_ComputePartialVelocities(partialinfo.second,idof,mapcachedpartials);
2055  vinsertedindices.push_back(-1);
2056  }
2057  }
2058  }
2059  }
2060  curlink = _vAllPairsShortestPaths[offset+curlink].first;
2061  }
2062 
2063  for(size_t i = 0; i < vaxes.size(); ++i) {
2064  if( vinsertedindices[i] < 0 ) {
2065  PartialInfo& partialinfo = mappartialsinserted[i];
2066  FOREACH(itpartial,partialinfo.second) {
2067  int index = itpartial->first;
2068  if( dofindices.size() > 0 ) {
2069  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),itpartial->first);
2070  if( itindex == dofindices.end() ) {
2071  continue;
2072  }
2073  index = itindex-dofindices.begin();
2074  }
2075 
2076  for(size_t j = 0; j < i; ++j) {
2077  Vector v = partialinfo.first.at(j)*itpartial->second;
2078  if( vinsertedindices[j] < 0 ) {
2079  //RAVELOG_WARN("hessian unhandled condition with mimic\n");
2080  PartialInfo& partialinfo2 = mappartialsinserted[j];
2081  FOREACH(itpartial2,partialinfo2.second) {
2082  int index2 = itpartial2->first;
2083  if( dofindices.size() > 0 ) {
2084  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),itpartial->first);
2085  if( itindex == dofindices.end() ) {
2086  continue;
2087  }
2088  index2 = itindex-dofindices.begin();
2089  }
2090 
2091  Vector v2 = v*itpartial2->second;
2092  size_t indexoffset = 3*dofstride*index2+index;
2093  hessian[indexoffset+0] += v2.x;
2094  hessian[indexoffset+dofstride] += v2.y;
2095  hessian[indexoffset+2*dofstride] += v2.z;
2096  if( j != i ) {
2097  // symmetric
2098  indexoffset = 3*dofstride*index+index2;
2099  hessian[indexoffset+0] += v2.x;
2100  hessian[indexoffset+dofstride] += v2.y;
2101  hessian[indexoffset+2*dofstride] += v2.z;
2102  }
2103  }
2104  }
2105  else {
2106  size_t indexoffset = 3*dofstride*index+vinsertedindices[j];
2107  hessian[indexoffset+0] += v.x;
2108  hessian[indexoffset+dofstride] += v.y;
2109  hessian[indexoffset+2*dofstride] += v.z;
2110  if( j != i ) {
2111  // symmetric
2112  indexoffset = 3*dofstride*vinsertedindices[j]+index;
2113  hessian[indexoffset+0] += v.x;
2114  hessian[indexoffset+dofstride] += v.y;
2115  hessian[indexoffset+2*dofstride] += v.z;
2116  }
2117  }
2118  }
2119 
2120  for(size_t j = i; j < vaxes.size(); ++j) {
2121  Vector v = vaxes[i].cross(vjacobian[j]);
2122  if( j == i ) {
2123  dReal f = itpartial->second*itpartial->second;
2124  size_t indexoffset = 3*dofstride*index+index;
2125  hessian[indexoffset+0] += v.x*f;
2126  hessian[indexoffset+dofstride] += v.y*f;
2127  hessian[indexoffset+2*dofstride] += v.z*f;
2128  continue;
2129  }
2130 
2131  if( vinsertedindices[j] < 0 ) {
2132  // only add the first time, do not multiply by itpartial->second yet?
2133  if( itpartial == partialinfo.second.begin() ) {
2134  mappartialsinserted[j].first.at(i) += v; // will get to it later
2135  }
2136  }
2137  else {
2138  v *= itpartial->second;
2139  size_t indexoffset = 3*dofstride*index+vinsertedindices[j];
2140  hessian[indexoffset+0] += v.x;
2141  hessian[indexoffset+dofstride] += v.y;
2142  hessian[indexoffset+2*dofstride] += v.z;
2143  if( j != i ) {
2144  // symmetric
2145  indexoffset = 3*dofstride*vinsertedindices[j]+index;
2146  hessian[indexoffset+0] += v.x;
2147  hessian[indexoffset+dofstride] += v.y;
2148  hessian[indexoffset+2*dofstride] += v.z;
2149  }
2150  }
2151  }
2152  }
2153  }
2154  else {
2155  size_t ioffset = 3*dofstride*vinsertedindices[i];
2156  for(size_t j = i; j < vaxes.size(); ++j) {
2157  Vector v = vaxes[i].cross(vjacobian[j]);
2158  if( vinsertedindices[j] < 0 ) {
2159  mappartialsinserted[j].first.at(i) = v; // we'll get to it later
2160  }
2161  else {
2162  size_t indexoffset = ioffset+vinsertedindices[j];
2163  hessian[indexoffset+0] += v.x;
2164  hessian[indexoffset+dofstride] += v.y;
2165  hessian[indexoffset+2*dofstride] += v.z;
2166  if( j != i ) {
2167  // symmetric
2168  indexoffset = 3*dofstride*vinsertedindices[j]+vinsertedindices[i];
2169  hessian[indexoffset+0] += v.x;
2170  hessian[indexoffset+dofstride] += v.y;
2171  hessian[indexoffset+2*dofstride] += v.z;
2172  }
2173  }
2174  }
2175  }
2176  }
2177 }
2178 
2179 void KinBody::ComputeHessianAxisAngle(int linkindex, std::vector<dReal>& hessian, const std::vector<int>& dofindices) const
2180 {
2182  OPENRAVE_ASSERT_FORMAT(linkindex >= 0 && linkindex < (int)_veclinks.size(), "body %s bad link index %d (num links %d)", GetName()%linkindex%_veclinks.size(),ORE_InvalidArguments);
2183  size_t dofstride=0;
2184  if( dofindices.size() > 0 ) {
2185  dofstride = dofindices.size();
2186  }
2187  else {
2188  dofstride = GetDOF();
2189  }
2190  hessian.resize(dofstride*3*dofstride);
2191  if( dofstride == 0 ) {
2192  return;
2193  }
2194  std::fill(hessian.begin(),hessian.end(),0);
2195 
2196  int offset = linkindex*_veclinks.size();
2197  int curlink = 0;
2198  std::vector<Vector> vaxes; vaxes.reserve(dofstride);
2199  std::vector<int> vpartialindices;
2200  std::map< std::pair<Mimic::DOFFormat, int>, dReal > mapcachedpartials;
2201  std::vector<int> vinsertedindices; vinsertedindices.reserve(dofstride);
2202  typedef std::pair< std::vector<Vector>, std::vector<std::pair<int,dReal> > > PartialInfo;
2203  std::map<size_t, PartialInfo > mappartialsinserted; // if vinsertedindices has -1, that index will be here
2204  while(_vAllPairsShortestPaths[offset+curlink].first>=0) {
2205  int jointindex = _vAllPairsShortestPaths[offset+curlink].second;
2206  if( jointindex < (int)_vecjoints.size() ) {
2207  // active joint
2208  JointPtr pjoint = _vecjoints.at(jointindex);
2209  int dofindex = pjoint->GetDOFIndex();
2210  int8_t affect = DoesAffect(pjoint->GetJointIndex(), linkindex);
2211  for(int dof = 0; dof < pjoint->GetDOF(); ++dof) {
2212  if( affect == 0 ) {
2213  RAVELOG_WARN(str(boost::format("link %s should be affected by joint %s")%_veclinks.at(linkindex)->GetName()%pjoint->GetName()));
2214  }
2215  else {
2216  size_t index = dofindex+dof;
2217  if( dofindices.size() > 0 ) {
2218  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),dofindex+dof);
2219  if( itindex != dofindices.end() ) {
2220  index = itindex-dofindices.begin();
2221  }
2222  else {
2223  continue;
2224  }
2225  }
2226 
2227  if( pjoint->IsRevolute(dof) ) {
2228  vaxes.push_back(pjoint->GetAxis(dof));
2229  }
2230  else if( pjoint->IsPrismatic(dof) ) {
2231  vaxes.push_back(Vector());
2232  }
2233  else {
2234  vaxes.push_back(Vector());
2235  RAVELOG_WARN("ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2236  }
2237  vinsertedindices.push_back(index);
2238  }
2239  }
2240  }
2241  else {
2242  // add in the contributions from the passive joint
2243  JointPtr pjoint = _vPassiveJoints.at(jointindex-_vecjoints.size());
2244  for(int idof = 0; idof < pjoint->GetDOF(); ++idof) {
2245  if( pjoint->IsMimic(idof) ) {
2246  bool bhas = dofindices.size() == 0;
2247  if( !bhas ) {
2248  FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
2249  if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
2250  bhas = true;
2251  break;
2252  }
2253  }
2254  }
2255  if( bhas ) {
2256  Vector vaxis;
2257  if( pjoint->IsRevolute(idof) ) {
2258  vaxes.push_back(pjoint->GetAxis(idof));
2259  }
2260  else if( pjoint->IsPrismatic(idof) ) {
2261  vaxes.push_back(Vector());
2262  }
2263  else {
2264  vaxes.push_back(Vector());
2265  RAVELOG_WARN("ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2266  }
2267  PartialInfo& partialinfo = mappartialsinserted[vinsertedindices.size()];
2268  partialinfo.first.resize(vinsertedindices.size());
2269  pjoint->_ComputePartialVelocities(partialinfo.second,idof,mapcachedpartials);
2270  vinsertedindices.push_back(-1);
2271  }
2272  }
2273  }
2274  }
2275  curlink = _vAllPairsShortestPaths[offset+curlink].first;
2276  }
2277 
2278  for(size_t i = 0; i < vaxes.size(); ++i) {
2279  if( vinsertedindices[i] < 0 ) {
2280  PartialInfo& partialinfo = mappartialsinserted[i];
2281  FOREACH(itpartial,partialinfo.second) {
2282  int index = itpartial->first;
2283  if( dofindices.size() > 0 ) {
2284  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),itpartial->first);
2285  if( itindex == dofindices.end() ) {
2286  continue;
2287  }
2288  index = itindex-dofindices.begin();
2289  }
2290 
2291  for(size_t j = 0; j < i; ++j) {
2292  Vector v = partialinfo.first.at(j)*itpartial->second;
2293  if( vinsertedindices[j] < 0 ) {
2294  //RAVELOG_WARN("hessian unhandled condition with mimic\n");
2295  PartialInfo& partialinfo2 = mappartialsinserted[j];
2296  FOREACH(itpartial2,partialinfo2.second) {
2297  int index2 = itpartial2->first;
2298  if( dofindices.size() > 0 ) {
2299  std::vector<int>::const_iterator itindex = find(dofindices.begin(),dofindices.end(),itpartial->first);
2300  if( itindex == dofindices.end() ) {
2301  continue;
2302  }
2303  index2 = itindex-dofindices.begin();
2304  }
2305 
2306  Vector v2 = v*itpartial2->second;
2307  size_t indexoffset = 3*dofstride*index2+index;
2308  hessian[indexoffset+0] += v2.x;
2309  hessian[indexoffset+dofstride] += v2.y;
2310  hessian[indexoffset+2*dofstride] += v2.z;
2311  if( j != i ) {
2312  // symmetric
2313  indexoffset = 3*dofstride*index+index2;
2314  hessian[indexoffset+0] += v2.x;
2315  hessian[indexoffset+dofstride] += v2.y;
2316  hessian[indexoffset+2*dofstride] += v2.z;
2317  }
2318  }
2319  }
2320  else {
2321  size_t indexoffset = 3*dofstride*index+vinsertedindices[j];
2322  hessian[indexoffset+0] += v.x;
2323  hessian[indexoffset+dofstride] += v.y;
2324  hessian[indexoffset+2*dofstride] += v.z;
2325  if( j != i ) {
2326  // symmetric
2327  indexoffset = 3*dofstride*vinsertedindices[j]+index;
2328  hessian[indexoffset+0] += v.x;
2329  hessian[indexoffset+dofstride] += v.y;
2330  hessian[indexoffset+2*dofstride] += v.z;
2331  }
2332  }
2333  }
2334 
2335  for(size_t j = i+1; j < vaxes.size(); ++j) {
2336  Vector v = vaxes[i].cross(vaxes[j]);
2337  if( j == i ) {
2338  dReal f = itpartial->second*itpartial->second;
2339  size_t indexoffset = 3*dofstride*index+index;
2340  hessian[indexoffset+0] += v.x*f;
2341  hessian[indexoffset+dofstride] += v.y*f;
2342  hessian[indexoffset+2*dofstride] += v.z*f;
2343  continue;
2344  }
2345 
2346  if( vinsertedindices[j] < 0 ) {
2347  // only add the first time, do not multiply by itpartial->second yet?
2348  if( itpartial == partialinfo.second.begin() ) {
2349  mappartialsinserted[j].first.at(i) += v; // will get to it later
2350  }
2351  }
2352  else {
2353  v *= itpartial->second;
2354  size_t indexoffset = 3*dofstride*index+vinsertedindices[j];
2355  hessian[indexoffset+0] += v.x;
2356  hessian[indexoffset+dofstride] += v.y;
2357  hessian[indexoffset+2*dofstride] += v.z;
2358  // symmetric
2359  indexoffset = 3*dofstride*vinsertedindices[j]+index;
2360  hessian[indexoffset+0] += v.x;
2361  hessian[indexoffset+dofstride] += v.y;
2362  hessian[indexoffset+2*dofstride] += v.z;
2363  }
2364  }
2365  }
2366  }
2367  else {
2368  size_t ioffset = 3*dofstride*vinsertedindices[i];
2369  for(size_t j = i+1; j < vaxes.size(); ++j) {
2370  Vector v = vaxes[i].cross(vaxes[j]);
2371  if( vinsertedindices[j] < 0 ) {
2372  mappartialsinserted[j].first.at(i) = v; // we'll get to it later
2373  }
2374  else {
2375  size_t indexoffset = ioffset+vinsertedindices[j];
2376  hessian[indexoffset+0] += v.x;
2377  hessian[indexoffset+dofstride] += v.y;
2378  hessian[indexoffset+2*dofstride] += v.z;
2379  // symmetric
2380  indexoffset = 3*dofstride*vinsertedindices[j]+vinsertedindices[i];
2381  hessian[indexoffset+0] += v.x;
2382  hessian[indexoffset+dofstride] += v.y;
2383  hessian[indexoffset+2*dofstride] += v.z;
2384  }
2385  }
2386  }
2387  }
2388 }
2389 
2390 void KinBody::ComputeInverseDynamics(std::vector<dReal>& doftorques, const std::vector<dReal>& vDOFAccelerations, const KinBody::ForceTorqueMap& mapExternalForceTorque) const
2391 {
2393  doftorques.resize(GetDOF());
2394  if( _vecjoints.size() == 0 ) {
2395  return;
2396  }
2397 
2398  Vector vgravity = GetEnv()->GetPhysicsEngine()->GetGravity();
2399  std::vector<dReal> vDOFVelocities;
2400  std::vector<pair<Vector, Vector> > vLinkVelocities, vLinkAccelerations; // linear, angular
2401  _ComputeDOFLinkVelocities(vDOFVelocities, vLinkVelocities);
2402  // check if all velocities are 0, if yes, then can simplify some computations since only have contributions from dofacell and external forces
2403  bool bHasVelocity = false;
2404  FOREACH(it,vDOFVelocities) {
2405  if( RaveFabs(*it) > g_fEpsilonLinear ) {
2406  bHasVelocity = true;
2407  break;
2408  }
2409  }
2410  if( !bHasVelocity ) {
2411  vDOFVelocities.resize(0);
2412  }
2413  _ComputeLinkAccelerations(vDOFVelocities, vDOFAccelerations, vLinkVelocities, vLinkAccelerations, vgravity);
2414 
2415  // all valuess are in the global coordinate system
2416  // Given the velocity/acceleration of the object is on point A, to change to B do:
2417  // v_B = v_A + angularvel x (B-A)
2418  // a_B = a_A + angularaccel x (B-A) + angularvel x (angularvel x (B-A))
2419  // forward recursion
2420  std::vector<Vector> vLinkCOMLinearAccelerations(_veclinks.size()), vLinkCOMMomentOfInertia(_veclinks.size());
2421  for(size_t i = 0; i < vLinkVelocities.size(); ++i) {
2422  Vector vglobalcomfromlink = _veclinks.at(i)->GetGlobalCOM() - _veclinks.at(i)->_info._t.trans;
2423  Vector vangularaccel = vLinkAccelerations.at(i).second;
2424  Vector vangularvelocity = vLinkVelocities.at(i).second;
2425  vLinkCOMLinearAccelerations[i] = vLinkAccelerations.at(i).first + vangularaccel.cross(vglobalcomfromlink) + vangularvelocity.cross(vangularvelocity.cross(vglobalcomfromlink));
2426  TransformMatrix tm = _veclinks.at(i)->GetGlobalInertia();
2427  vLinkCOMMomentOfInertia[i] = tm.rotate(vangularaccel) + vangularvelocity.cross(tm.rotate(vangularvelocity));
2428  }
2429 
2430  // backward recursion
2431  std::vector< std::pair<Vector, Vector> > vLinkForceTorques(_veclinks.size());
2432  FOREACHC(it,mapExternalForceTorque) {
2433  vLinkForceTorques.at(it->first) = it->second;
2434  }
2435  std::fill(doftorques.begin(),doftorques.end(),0);
2436 
2437  std::vector<std::pair<int,dReal> > vpartials;
2438  std::map< std::pair<Mimic::DOFFormat, int>, dReal > mapcachedpartials;
2439 
2440  // go backwards
2441  for(size_t ijoint = 0; ijoint < _vTopologicallySortedJointsAll.size(); ++ijoint) {
2443  int childindex = pjoint->GetHierarchyChildLink()->GetIndex();
2444  Vector vcomforce = vLinkCOMLinearAccelerations[childindex]*pjoint->GetHierarchyChildLink()->GetMass() + vLinkForceTorques.at(childindex).first;
2445  Vector vjointtorque = vLinkForceTorques.at(childindex).second + vLinkCOMMomentOfInertia.at(childindex);
2446 
2447  if( !!pjoint->GetHierarchyParentLink() ) {
2448  Vector vchildcomtoparentcom = pjoint->GetHierarchyChildLink()->GetGlobalCOM() - pjoint->GetHierarchyParentLink()->GetGlobalCOM();
2449  int parentindex = pjoint->GetHierarchyParentLink()->GetIndex();
2450  vLinkForceTorques.at(parentindex).first += vcomforce;
2451  vLinkForceTorques.at(parentindex).second += vjointtorque + vchildcomtoparentcom.cross(vcomforce);
2452  }
2453 
2454  Vector vcomtoanchor = pjoint->GetHierarchyChildLink()->GetGlobalCOM() - pjoint->GetAnchor();
2455  if( pjoint->GetDOFIndex() >= 0 ) {
2456  if( pjoint->GetType() == JointHinge ) {
2457  doftorques.at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2458  }
2459  else if( pjoint->GetType() == JointSlider ) {
2460  doftorques.at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vcomforce);
2461  }
2462  else {
2463  throw OPENRAVE_EXCEPTION_FORMAT("joint 0x%x not supported", pjoint->GetType(), ORE_Assert);
2464  }
2465  }
2466  else if( pjoint->IsMimic(0) ) {
2467  // passive joint, so have to transfer the torque to its dependent joints.
2468  // TODO if there's more than one dependent joint, how do we split?
2469  dReal faxistorque;
2470  if( pjoint->GetType() == JointHinge ) {
2471  faxistorque = pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2472  }
2473  else if( pjoint->GetType() == JointSlider ) {
2474  faxistorque = pjoint->GetAxis(0).dot3(vcomforce);
2475  }
2476  else {
2477  throw OPENRAVE_EXCEPTION_FORMAT("joint 0x%x not supported", pjoint->GetType(), ORE_Assert);
2478  }
2479 
2480  pjoint->_ComputePartialVelocities(vpartials,0,mapcachedpartials);
2481  FOREACH(itpartial,vpartials) {
2482  int dofindex = itpartial->first;
2483  doftorques.at(dofindex) += itpartial->second*faxistorque;
2484  }
2485  }
2486  else {
2487  // joint should be static
2488  BOOST_ASSERT(pjoint->IsStatic());
2489  }
2490  }
2491 }
2492 
2493 void KinBody::ComputeInverseDynamics(boost::array< std::vector<dReal>, 3>& vDOFTorqueComponents, const std::vector<dReal>& vDOFAccelerations, const KinBody::ForceTorqueMap& mapExternalForceTorque) const
2494 {
2496  FOREACH(itdoftorques,vDOFTorqueComponents) {
2497  itdoftorques->resize(GetDOF());
2498  }
2499  if( _vecjoints.size() == 0 ) {
2500  return;
2501  }
2502 
2503  Vector vgravity = GetEnv()->GetPhysicsEngine()->GetGravity();
2504  std::vector<dReal> vDOFVelocities;
2505  boost::array< std::vector<pair<Vector, Vector> >, 3> vLinkVelocities; // [0] = all zeros, [1] = dof velocities only, [2] = only velocities due to base link
2506  boost::array< std::vector<pair<Vector, Vector> >, 3> vLinkAccelerations; // [0] = dofaccel only, [1] = dofvel only, [2] - gravity + external only (dofaccel=0, dofvel=0)
2507  boost::array<int,3> linkaccelsimilar = {{-1,-1,-1}}; // used for tracking which vLinkAccelerations indices are similar to each other (to avoid computation)
2508 
2509  vLinkVelocities[0].resize(_veclinks.size());
2510  _ComputeDOFLinkVelocities(vDOFVelocities, vLinkVelocities[1], false);
2511  // check if all velocities are 0, if yes, then can simplify some computations since only have contributions from dofacell and external forces
2512  bool bHasVelocity = false;
2513  FOREACH(it,vDOFVelocities) {
2514  if( RaveFabs(*it) > g_fEpsilonLinear ) {
2515  bHasVelocity = true;
2516  break;
2517  }
2518  }
2519  if( !bHasVelocity ) {
2520  vDOFVelocities.resize(0);
2521  }
2522 
2523  // all valuess are in the global coordinate system
2524  // try to compute as little as possible by checking what is non-zero
2525  Vector vbaselinear, vbaseangular;
2526  _veclinks.at(0)->GetVelocity(vbaselinear,vbaseangular);
2527  bool bHasGravity = vgravity.lengthsqr3() > g_fEpsilonLinear*g_fEpsilonLinear;
2528  bool bHasBaseLinkAccel = vbaseangular.lengthsqr3() > g_fEpsilonLinear*g_fEpsilonLinear;
2529  if( bHasBaseLinkAccel || bHasGravity ) {
2530  if( bHasBaseLinkAccel ) {
2531  // remove the base link velocity frame
2532  // v_B = v_A + angularvel x (B-A)
2533  vLinkVelocities[2].resize(_veclinks.size());
2534  Vector vbasepos = _veclinks.at(0)->_info._t.trans;
2535  for(size_t i = 1; i < vLinkVelocities[0].size(); ++i) {
2536  Vector voffset = _veclinks.at(i)->_info._t.trans - vbasepos;
2537  vLinkVelocities[2][i].first = vbaselinear + vbaseangular.cross(voffset);
2538  vLinkVelocities[2][i].second = vbaseangular;
2539  }
2540  }
2541  else {
2542  vLinkVelocities[2] = vLinkVelocities[0];
2543  }
2544  _ComputeLinkAccelerations(std::vector<dReal>(), std::vector<dReal>(), vLinkVelocities[2], vLinkAccelerations[2], vgravity);
2545  if( bHasVelocity ) {
2546  _ComputeLinkAccelerations(vDOFVelocities, std::vector<dReal>(), vLinkVelocities[1], vLinkAccelerations[1], Vector(0,0,0));
2547  if( vDOFAccelerations.size() > 0 ) {
2548  _ComputeLinkAccelerations(std::vector<dReal>(), vDOFAccelerations, vLinkVelocities[0], vLinkAccelerations[0], Vector(0,0,0));
2549  }
2550  else {
2551  linkaccelsimilar[0] = 1;
2552  }
2553  }
2554  else {
2555  if( vDOFAccelerations.size() > 0 ) {
2556  _ComputeLinkAccelerations(std::vector<dReal>(), vDOFAccelerations, vLinkVelocities[0], vLinkAccelerations[0], Vector(0,0,0));
2557  }
2558  }
2559  }
2560  else {
2561  // no external forces
2562  vLinkVelocities[2] = vLinkVelocities[0];
2563  if( bHasVelocity ) {
2564  _ComputeLinkAccelerations(vDOFVelocities, std::vector<dReal>(), vLinkVelocities[1], vLinkAccelerations[1], Vector(0,0,0));
2565  if( vDOFAccelerations.size() > 0 ) {
2566  _ComputeLinkAccelerations(std::vector<dReal>(), vDOFAccelerations, vLinkVelocities[0], vLinkAccelerations[0], Vector(0,0,0));
2567  }
2568  else {
2569  linkaccelsimilar[0] = 1;
2570  }
2571  }
2572  else {
2573  if( vDOFAccelerations.size() > 0 ) {
2574  _ComputeLinkAccelerations(std::vector<dReal>(), vDOFAccelerations, vLinkVelocities[0], vLinkAccelerations[0], Vector(0,0,0));
2575  }
2576  }
2577  }
2578 
2579  boost::array< std::vector<Vector>, 3> vLinkCOMLinearAccelerations, vLinkCOMMomentOfInertia;
2580  boost::array< std::vector< std::pair<Vector, Vector> >, 3> vLinkForceTorques;
2581  for(size_t j = 0; j < 3; ++j) {
2582  if( vLinkAccelerations[j].size() > 0 ) {
2583  vLinkCOMLinearAccelerations[j].resize(_veclinks.size());
2584  vLinkCOMMomentOfInertia[j].resize(_veclinks.size());
2585  vLinkForceTorques[j].resize(_veclinks.size());
2586  }
2587  }
2588 
2589  for(size_t i = 0; i < _veclinks.size(); ++i) {
2590  Vector vglobalcomfromlink = _veclinks.at(i)->GetGlobalCOM() - _veclinks.at(i)->_info._t.trans;
2591  TransformMatrix tm = _veclinks.at(i)->GetGlobalInertia();
2592  for(size_t j = 0; j < 3; ++j) {
2593  if( vLinkAccelerations[j].size() > 0 ) {
2594  Vector vangularaccel = vLinkAccelerations[j].at(i).second;
2595  Vector vangularvelocity = vLinkVelocities[j].at(i).second;
2596  vLinkCOMLinearAccelerations[j][i] = vLinkAccelerations[j].at(i).first + vangularaccel.cross(vglobalcomfromlink) + vangularvelocity.cross(vangularvelocity.cross(vglobalcomfromlink));
2597  vLinkCOMMomentOfInertia[j][i] = tm.rotate(vangularaccel) + vangularvelocity.cross(tm.rotate(vangularvelocity));
2598  }
2599  }
2600  }
2601 
2602  FOREACH(itdoftorques,vDOFTorqueComponents) {
2603  std::fill(itdoftorques->begin(),itdoftorques->end(),0);
2604  }
2605 
2606  // backward recursion
2607  vLinkForceTorques[2].resize(_veclinks.size());
2608  FOREACHC(it,mapExternalForceTorque) {
2609  vLinkForceTorques[2].at(it->first) = it->second;
2610  }
2611 
2612  std::vector<std::pair<int,dReal> > vpartials;
2613  std::map< std::pair<Mimic::DOFFormat, int>, dReal > mapcachedpartials;
2614 
2615  // go backwards
2616  for(size_t ijoint = 0; ijoint < _vTopologicallySortedJointsAll.size(); ++ijoint) {
2618  int childindex = pjoint->GetHierarchyChildLink()->GetIndex();
2619  Vector vchildcomtoparentcom;
2620  int parentindex = -1;
2621  if( !!pjoint->GetHierarchyParentLink() ) {
2622  vchildcomtoparentcom = pjoint->GetHierarchyChildLink()->GetGlobalCOM() - pjoint->GetHierarchyParentLink()->GetGlobalCOM();
2623  parentindex = pjoint->GetHierarchyParentLink()->GetIndex();
2624  }
2625 
2626  bool bIsMimic = pjoint->GetDOFIndex() < 0 && pjoint->IsMimic(0);
2627  if( bIsMimic ) {
2628  pjoint->_ComputePartialVelocities(vpartials,0,mapcachedpartials);
2629  }
2630 
2631  dReal mass = pjoint->GetHierarchyChildLink()->GetMass();
2632  Vector vcomtoanchor = pjoint->GetHierarchyChildLink()->GetGlobalCOM() - pjoint->GetAnchor();
2633  for(size_t j = 0; j < 3; ++j) {
2634  if( vLinkForceTorques[j].size() == 0 ) {
2635  continue;
2636  }
2637  Vector vcomforce = vLinkForceTorques[j].at(childindex).first;
2638  Vector vjointtorque = vLinkForceTorques[j].at(childindex).second;
2639  if( vLinkCOMLinearAccelerations[j].size() > 0 ) {
2640  vcomforce += vLinkCOMLinearAccelerations[j][childindex]*mass;
2641  vjointtorque += vLinkCOMMomentOfInertia[j].at(childindex);
2642  }
2643 
2644  if( parentindex >= 0 ) {
2645  vLinkForceTorques[j].at(parentindex).first += vcomforce;
2646  vLinkForceTorques[j].at(parentindex).second += vjointtorque + vchildcomtoparentcom.cross(vcomforce);
2647  }
2648 
2649  if( pjoint->GetDOFIndex() >= 0 ) {
2650  if( pjoint->GetType() == JointHinge ) {
2651  vDOFTorqueComponents[j].at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2652  }
2653  else if( pjoint->GetType() == JointSlider ) {
2654  vDOFTorqueComponents[j].at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vcomforce);
2655  }
2656  else {
2657  throw OPENRAVE_EXCEPTION_FORMAT("joint 0x%x not supported", pjoint->GetType(), ORE_Assert);
2658  }
2659  }
2660  else if( bIsMimic ) {
2661  // passive joint, so have to transfer the torque to its dependent joints.
2662  // TODO if there's more than one dependent joint, how do we split?
2663  dReal faxistorque;
2664  if( pjoint->GetType() == JointHinge ) {
2665  faxistorque = pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2666  }
2667  else if( pjoint->GetType() == JointSlider ) {
2668  faxistorque = pjoint->GetAxis(0).dot3(vcomforce);
2669  }
2670  else {
2671  throw OPENRAVE_EXCEPTION_FORMAT("joint 0x%x not supported", pjoint->GetType(), ORE_Assert);
2672  }
2673 
2674  FOREACH(itpartial,vpartials) {
2675  int dofindex = itpartial->first;
2676  vDOFTorqueComponents[j].at(dofindex) += itpartial->second*faxistorque;
2677  }
2678  }
2679  else {
2680  // joint should be static
2681  BOOST_ASSERT(pjoint->IsStatic());
2682  }
2683  }
2684  }
2685 }
2686 
2687 void KinBody::GetLinkAccelerations(const std::vector<dReal>&vDOFAccelerations, std::vector<std::pair<Vector,Vector> >&vLinkAccelerations) const
2688 {
2690  if( _veclinks.size() == 0 ) {
2691  vLinkAccelerations.resize(0);
2692  }
2693  else {
2694  std::vector<dReal> vDOFVelocities;
2695  std::vector<pair<Vector, Vector> > vLinkVelocities;
2696  _ComputeDOFLinkVelocities(vDOFVelocities,vLinkVelocities);
2697  _ComputeLinkAccelerations(vDOFVelocities, vDOFAccelerations, vLinkVelocities, vLinkAccelerations, GetEnv()->GetPhysicsEngine()->GetGravity());
2698  }
2699 }
2700 
2701 void KinBody::_ComputeDOFLinkVelocities(std::vector<dReal>& dofvelocities, std::vector<std::pair<Vector,Vector> >& vLinkVelocities, bool usebaselinkvelocity) const
2702 {
2703  GetEnv()->GetPhysicsEngine()->GetLinkVelocities(shared_kinbody_const(),vLinkVelocities);
2704  if( _veclinks.size() <= 1 ) {
2705  dofvelocities.resize(GetDOF());
2706  if( !usebaselinkvelocity && _veclinks.size() > 0 ) {
2707  vLinkVelocities[0].first = Vector();
2708  vLinkVelocities[0].second = Vector();
2709  }
2710  return;
2711  }
2712  if( !usebaselinkvelocity ) {
2713  Vector vbasepos = _veclinks.at(0)->_info._t.trans;
2714  // v_B = v_A + angularvel x (B-A)
2715  for(size_t i = 1; i < vLinkVelocities.size(); ++i) {
2716  Vector voffset = _veclinks.at(i)->_info._t.trans - vbasepos;
2717  vLinkVelocities[i].first -= vLinkVelocities[0].first + vLinkVelocities[0].second.cross(voffset);
2718  vLinkVelocities[i].second -= vLinkVelocities[0].second;
2719  }
2720  vLinkVelocities[0].first = Vector();
2721  vLinkVelocities[0].second = Vector();
2722  }
2723  dofvelocities.resize(0);
2724  if( (int)dofvelocities.capacity() < GetDOF() ) {
2725  dofvelocities.reserve(GetDOF());
2726  }
2727  FOREACHC(it, _vDOFOrderedJoints) {
2728  int parentindex = 0;
2729  if( !!(*it)->_attachedbodies[0] ) {
2730  parentindex = (*it)->_attachedbodies[0]->GetIndex();
2731  }
2732  int childindex = (*it)->_attachedbodies[0]->GetIndex();
2733  (*it)->_GetVelocities(dofvelocities,true,vLinkVelocities.at(parentindex),vLinkVelocities.at(childindex));
2734  }
2735 }
2736 
2737 void KinBody::_ComputeLinkAccelerations(const std::vector<dReal>& vDOFVelocities, const std::vector<dReal>& vDOFAccelerations, const std::vector< std::pair<Vector, Vector> >& vLinkVelocities, std::vector<std::pair<Vector,Vector> >& vLinkAccelerations, const Vector& vGravity) const
2738 {
2739  vLinkAccelerations.resize(_veclinks.size());
2740  if( _veclinks.size() == 0 ) {
2741  return;
2742  }
2743 
2744  vector<dReal> vtempvalues, veval;
2745  boost::array<dReal,3> dummyvelocities = {{0,0,0}}, dummyaccelerations={{0,0,0}}; // dummy values for a joint
2746 
2747  // set accelerations of first link to 0 since it isn't actuated
2748  vLinkAccelerations.at(0).first = -vGravity + vLinkVelocities.at(0).second.cross(vLinkVelocities.at(0).first);
2749  vLinkAccelerations.at(0).second = Vector();
2750 
2751  // have to compute the velocities and accelerations ahead of time since they are dependent on the link transformations
2752  std::vector< std::vector<dReal> > vPassiveJointVelocities(_vPassiveJoints.size()), vPassiveJointAccelerations(_vPassiveJoints.size());
2753  for(size_t i = 0; i <_vPassiveJoints.size(); ++i) {
2754  if( vDOFAccelerations.size() > 0 ) {
2755  vPassiveJointAccelerations[i].resize(_vPassiveJoints[i]->GetDOF(),0);
2756  }
2757  if( vDOFVelocities.size() > 0 ) {
2758  if( !_vPassiveJoints[i]->IsMimic() ) {
2759  _vPassiveJoints[i]->GetVelocities(vPassiveJointVelocities[i]);
2760  }
2761  else {
2762  vPassiveJointVelocities[i].resize(_vPassiveJoints[i]->GetDOF(),0);
2763  }
2764  }
2765  }
2766 
2767  Transform tdelta;
2768  Vector vlocalaxis;
2769  std::vector<uint8_t> vlinkscomputed(_veclinks.size(),0);
2770  vlinkscomputed[0] = 1;
2771 
2772  // compute the link accelerations going through topological order
2773  for(size_t ijoint = 0; ijoint < _vTopologicallySortedJointsAll.size(); ++ijoint) {
2774  JointPtr pjoint = _vTopologicallySortedJointsAll[ijoint];
2775  int jointindex = _vTopologicallySortedJointIndicesAll[ijoint];
2776  int dofindex = pjoint->GetDOFIndex();
2777 
2778  // have to compute the partial accelerations for each mimic dof
2779  const dReal* pdofaccelerations=NULL, *pdofvelocities=NULL;
2780  if( dofindex >= 0 ) {
2781  if( vDOFAccelerations.size() ) {
2782  pdofaccelerations = &vDOFAccelerations.at(dofindex);
2783  }
2784  if( vDOFVelocities.size() > 0 ) {
2785  pdofvelocities=&vDOFVelocities.at(dofindex);
2786  }
2787  }
2788  if( pjoint->IsMimic() && (vDOFAccelerations.size() > 0 || vDOFVelocities.size() > 0) ) {
2789  // compute both partial velocity and acceleration information
2790  for(int i = 0; i < pjoint->GetDOF(); ++i) {
2791  if( pjoint->IsMimic(i) ) {
2792  vtempvalues.resize(0);
2793  const std::vector<Mimic::DOFFormat>& vdofformat = pjoint->_vmimic[i]->_vdofformat;
2794  FOREACHC(itdof,vdofformat) {
2795  JointPtr pj = itdof->jointindex < (int)_vecjoints.size() ? _vecjoints[itdof->jointindex] : _vPassiveJoints.at(itdof->jointindex-_vecjoints.size());
2796  vtempvalues.push_back(pj->GetValue(itdof->axis));
2797  }
2798  dummyvelocities[i] = 0;
2799  dummyaccelerations[i] = 0;
2800 
2801  // velocity
2802  if( vDOFVelocities.size() > 0 ) {
2803  int err = pjoint->_Eval(i,1,vtempvalues,veval);
2804  if( err ) {
2805  RAVELOG_WARN(str(boost::format("failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
2806  }
2807  else {
2808  for(size_t ipartial = 0; ipartial < vdofformat.size(); ++ipartial) {
2809  dReal partialvelocity;
2810  if( vdofformat[ipartial].dofindex >= 0 ) {
2811  partialvelocity = vDOFVelocities.at(vdofformat[ipartial].dofindex);
2812  }
2813  else {
2814  partialvelocity = vPassiveJointVelocities.at(vdofformat[ipartial].jointindex-_vecjoints.size()).at(vdofformat[ipartial].axis);
2815  }
2816  dummyvelocities[i] += veval.at(ipartial) * partialvelocity;
2817  }
2818  }
2819  // if joint is passive, update the stored joint values! This is necessary because joint value might be referenced in the future.
2820  if( dofindex < 0 ) {
2821  vPassiveJointVelocities.at(jointindex-(int)_vecjoints.size()).at(i) = dummyvelocities[i];
2822  }
2823  }
2824 
2825  // acceleration
2826  if( vDOFAccelerations.size() > 0 ) {
2827  int err = pjoint->_Eval(i,2,vtempvalues,veval);
2828  if( err ) {
2829  RAVELOG_WARN(str(boost::format("failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
2830  }
2831  else {
2832  for(size_t ipartial = 0; ipartial < vdofformat.size(); ++ipartial) {
2833  dReal partialacceleration;
2834  if( vdofformat[ipartial].dofindex >= 0 ) {
2835  partialacceleration = vDOFAccelerations.at(vdofformat[ipartial].dofindex);
2836  }
2837  else {
2838  partialacceleration = vPassiveJointAccelerations.at(vdofformat[ipartial].jointindex-_vecjoints.size()).at(vdofformat[ipartial].axis);
2839  }
2840  dummyaccelerations[i] += veval.at(ipartial) * partialacceleration;
2841  }
2842  }
2843  // if joint is passive, update the stored joint values! This is necessary because joint value might be referenced in the future.
2844  if( dofindex < 0 ) {
2845  vPassiveJointAccelerations.at(jointindex-(int)_vecjoints.size()).at(i) = dummyaccelerations[i];
2846  }
2847  }
2848  }
2849  else if( dofindex >= 0 ) {
2850  // is this correct? what is a joint has a mimic and non-mimic axis?
2851  dummyvelocities[i] = vDOFVelocities.at(dofindex+i);
2852  dummyaccelerations[i] = vDOFAccelerations.at(dofindex+i);
2853  }
2854  else {
2855  // preserve passive joint values
2856  dummyvelocities[i] = vPassiveJointVelocities.at(jointindex-(int)_vecjoints.size()).at(i);
2857  dummyaccelerations[i] = vPassiveJointAccelerations.at(jointindex-(int)_vecjoints.size()).at(i);
2858  }
2859  }
2860  pdofvelocities = &dummyvelocities[0];
2861  pdofaccelerations = &dummyaccelerations[0];
2862  }
2863 
2864  // do the test after mimic computation!?
2865  if( vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] ) {
2866  continue;
2867  }
2868 
2869  if( vDOFVelocities.size() > 0 && !pdofvelocities ) {
2870  // has to be a passive joint
2871  pdofvelocities = &vPassiveJointVelocities.at(jointindex-(int)_vecjoints.size()).at(0);
2872  }
2873  if( vDOFAccelerations.size() > 0 && !pdofaccelerations ) {
2874  // has to be a passive joint
2875  pdofaccelerations = &vPassiveJointAccelerations.at(jointindex-(int)_vecjoints.size()).at(0);
2876  }
2877 
2878  int childindex = pjoint->GetHierarchyChildLink()->GetIndex();
2879  const Transform& tchild = pjoint->GetHierarchyChildLink()->GetTransform();
2880  const pair<Vector, Vector>& vChildVelocities = vLinkVelocities.at(childindex);
2881  pair<Vector, Vector>& vChildAccelerations = vLinkAccelerations.at(childindex);
2882 
2883  int parentindex = 0;
2884  if( !!pjoint->GetHierarchyParentLink() ) {
2885  parentindex = pjoint->GetHierarchyParentLink()->GetIndex();
2886  }
2887 
2888  const pair<Vector, Vector>& vParentVelocities = vLinkVelocities.at(parentindex);
2889  const pair<Vector, Vector>& vParentAccelerations = vLinkAccelerations.at(parentindex);
2890  Vector xyzdelta = tchild.trans - _veclinks.at(parentindex)->_info._t.trans;
2891  if( !!pdofaccelerations || !!pdofvelocities ) {
2892  tdelta = _veclinks.at(parentindex)->_info._t * pjoint->GetInternalHierarchyLeftTransform();
2893  vlocalaxis = pjoint->GetInternalHierarchyAxis(0);
2894  }
2895 
2896  // check out: http://en.wikipedia.org/wiki/Rotating_reference_frame
2897  // compute for global coordinate system
2898  // code for symbolic computation (python sympy)
2899  // t=Symbol('t'); q=Function('q')(t); dq=diff(q,t); axis=Matrix(3,1,symbols('ax,ay,az')); delta=Matrix(3,1,[Function('dx')(t), Function('dy')(t), Function('dz')(t)]); vparent=Matrix(3,1,[Function('vparentx')(t),Function('vparenty')(t),Function('vparentz')(t)]); wparent=Matrix(3,1,[Function('wparentx')(t),Function('wparenty')(t),Function('wparentz')(t)]); Mparent=Matrix(3,4,[Function('m%d%d'%(i,j))(t) for i in range(3) for j in range(4)]); c = Matrix(3,1,symbols('cx,cy,cz'))
2900  // hinge joints:
2901  // p = Mparent[0:3,3] + Mparent[0:3,0:3]*(Left[0:3,3] + Left[0:3,0:3]*Rot[0:3,0:3]*Right[0:3,3])
2902  // v = vparent + wparent.cross(p-Mparent[0:3,3]) + Mparent[0:3,0:3] * Left[0:3,0:3] * dq * axis.cross(Rot[0:3,0:3]*Right)
2903  // wparent.cross(v) = wparent.cross(vparent) + wparent.cross(wparent.cross(p-Mparent[0:3,3])) + wparent.cross(p-Mparent[0:3,3])
2904  // dv = vparent.diff(t) + wparent.diff(t).cross(p-Mparent[0:3,3]).transpose() + wparent.cross(v-vparent) + wparent.cross(v-vparent-wparent.cross(wparent.cross(p-Mparent[0:3,3]))) + Mparent[0:3,0:3] * Left[0:3,0:3] * (ddq * axis.cross(Rot[0:3,0:3]*Right) + dq * axis.cross(dq*axis.cross(Rot[0:3,0:3]*Right)))
2905  // w = wparent + Mparent[0:3,0:3]*Left[0:3,0:3]*axis*dq
2906  // dw = wparent.diff(t) + wparent.cross(Mparent[0:3,0:3]*Left[0:3,0:3]*axis*dq).transpose() + Mparent[0:3,0:3]*Left[0:3,0:3]*axis*ddq
2907  // slider:
2908  // v = vparent + wparent.cross(p-Mparent[0:3,3]) + Mparent[0:3,0:3]*Left[0:3,0:3]*dq*axis
2909  // dv = vparent.diff(t) + wparent.diff(t).cross(p-Mparent[0:3,3]).transpose() + wparent.cross(v-vparent) + wparent.cross(Mparent[0:3,0:3]*Left[0:3,0:3]*dq*axis) + Mparent[0:3,0:3]*Left[0:3,0:3]*ddq*axis
2910  // w = wparent
2911  // dw = wparent.diff(t)
2912  if( pjoint->GetType() == JointRevolute ) {
2913  vChildAccelerations.first = vParentAccelerations.first + vParentAccelerations.second.cross(xyzdelta) + vParentVelocities.second.cross((vChildVelocities.first-vParentVelocities.first)*2-vParentVelocities.second.cross(xyzdelta));
2914  vChildAccelerations.second = vParentAccelerations.second;
2915  if( !!pdofvelocities ) {
2916  Vector gw = tdelta.rotate(vlocalaxis*pdofvelocities[0]);
2917  vChildAccelerations.first += gw.cross(gw.cross(tchild.trans-tdelta.trans));
2918  vChildAccelerations.second += vParentVelocities.second.cross(gw);
2919  }
2920  if( !!pdofaccelerations ) {
2921  Vector gdw = tdelta.rotate(vlocalaxis*pdofaccelerations[0]);
2922  vChildAccelerations.first += gdw.cross(tchild.trans-tdelta.trans);
2923  vChildAccelerations.second += gdw;
2924  }
2925  }
2926  else if( pjoint->GetType() == JointPrismatic ) {
2927  Vector w = tdelta.rotate(vlocalaxis);
2928  vChildAccelerations.first = vParentAccelerations.first + vParentAccelerations.second.cross(xyzdelta);
2929  Vector angularveloctiycontrib = vChildVelocities.first-vParentVelocities.first;
2930  if( !!pdofvelocities ) {
2931  angularveloctiycontrib += w*pdofvelocities[0];
2932  }
2933  vChildAccelerations.first += vParentVelocities.second.cross(angularveloctiycontrib);
2934  if( !!pdofaccelerations ) {
2935  vChildAccelerations.first += w*pdofaccelerations[0];
2936  }
2937  vChildAccelerations.second = vParentAccelerations.second;
2938  }
2939  else {
2940  throw OPENRAVE_EXCEPTION_FORMAT("joint type 0x%x not supported for getting link acceleration",pjoint->GetType(),ORE_Assert);
2941  }
2942  vlinkscomputed[childindex] = 1;
2943  }
2944 }
2945 
2947 {
2948  if( GetEnv()->CheckSelfCollision(shared_kinbody_const(), report) ) {
2949  if( !!report ) {
2950  RAVELOG_VERBOSE(str(boost::format("Self collision: %s\n")%report->__str__()));
2951  }
2952  return true;
2953  }
2954  return false;
2955 }
2956 
2958 {
2959  uint64_t starttime = utils::GetMicroTime();
2960  _nHierarchyComputed = 1;
2961 
2962  int lindex=0;
2963  FOREACH(itlink,_veclinks) {
2964  (*itlink)->_index = lindex; // always reset, necessary since index cannot be initialized by custom links
2965  (*itlink)->_vParentLinks.clear();
2966  if((_veclinks.size() > 1)&&((*itlink)->GetName().size() == 0)) {
2967  RAVELOG_WARN(str(boost::format("%s link index %d has no name")%GetName()%lindex));
2968  }
2969  lindex++;
2970  }
2971 
2972  {
2973  // move any enabled passive joints to the regular joints list
2974  vector<JointPtr>::iterator itjoint = _vPassiveJoints.begin();
2975  while(itjoint != _vPassiveJoints.end()) {
2976  bool bmimic = false;
2977  for(int idof = 0; idof < (*itjoint)->GetDOF(); ++idof) {
2978  if( !!(*itjoint)->_vmimic[idof] ) {
2979  bmimic = true;
2980  }
2981  }
2982  if( !bmimic && (*itjoint)->_info._bIsActive ) {
2983  _vecjoints.push_back(*itjoint);
2984  itjoint = _vPassiveJoints.erase(itjoint);
2985  }
2986  else {
2987  ++itjoint;
2988  }
2989  }
2990  // move any mimic joints to the passive joints
2991  itjoint = _vecjoints.begin();
2992  while(itjoint != _vecjoints.end()) {
2993  bool bmimic = false;
2994  for(int idof = 0; idof < (*itjoint)->GetDOF(); ++idof) {
2995  if( !!(*itjoint)->_vmimic[idof] ) {
2996  bmimic = true;
2997  break;
2998  }
2999  }
3000  if( bmimic || !(*itjoint)->_info._bIsActive ) {
3001  _vPassiveJoints.push_back(*itjoint);
3002  itjoint = _vecjoints.erase(itjoint);
3003  }
3004  else {
3005  ++itjoint;
3006  }
3007  }
3008  int jointindex=0;
3009  int dofindex=0;
3010  FOREACH(itjoint,_vecjoints) {
3011  (*itjoint)->jointindex = jointindex++;
3012  (*itjoint)->dofindex = dofindex;
3013  (*itjoint)->_info._bIsActive = true;
3014  dofindex += (*itjoint)->GetDOF();
3015  }
3016  FOREACH(itjoint,_vPassiveJoints) {
3017  (*itjoint)->jointindex = -1;
3018  (*itjoint)->dofindex = -1;
3019  (*itjoint)->_info._bIsActive = false;
3020  }
3021  }
3022 
3023  vector<size_t> vorder(_vecjoints.size());
3024  vector<int> vJointIndices(_vecjoints.size());
3025  _vDOFIndices.resize(GetDOF());
3026  for(size_t i = 0; i < _vecjoints.size(); ++i) {
3027  vJointIndices[i] = _vecjoints[i]->dofindex;
3028  for(int idof = 0; idof < _vecjoints[i]->GetDOF(); ++idof) {
3029  _vDOFIndices.at(vJointIndices[i]+idof) = i;
3030  }
3031  vorder[i] = i;
3032  }
3033  sort(vorder.begin(), vorder.end(), utils::index_cmp<vector<int>&>(vJointIndices));
3034  _vDOFOrderedJoints.resize(0);
3035  FOREACH(it,vorder) {
3036  _vDOFOrderedJoints.push_back(_vecjoints.at(*it));
3037  }
3038 
3039  try {
3040  // initialize all the mimic equations
3041  for(int ijoints = 0; ijoints < 2; ++ijoints) {
3042  vector<JointPtr>& vjoints = ijoints ? _vPassiveJoints : _vecjoints;
3043  FOREACH(itjoint,vjoints) {
3044  for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
3045  if( !!(*itjoint)->_vmimic[i] ) {
3046  std::string poseq = (*itjoint)->_vmimic[i]->_equations[0], veleq = (*itjoint)->_vmimic[i]->_equations[1], acceleq = (*itjoint)->_vmimic[i]->_equations[2]; // have to copy since memory can become invalidated
3047  (*itjoint)->SetMimicEquations(i,poseq,veleq,acceleq);
3048  }
3049  }
3050  }
3051  }
3052  // fill Mimic::_vmimicdofs, check that there are no circular dependencies between the mimic joints
3053  std::map<Mimic::DOFFormat, boost::shared_ptr<Mimic> > mapmimic;
3054  for(int ijoints = 0; ijoints < 2; ++ijoints) {
3055  vector<JointPtr>& vjoints = ijoints ? _vPassiveJoints : _vecjoints;
3056  int jointindex=0;
3057  FOREACH(itjoint,vjoints) {
3058  Mimic::DOFFormat dofformat;
3059  if( ijoints ) {
3060  dofformat.dofindex = -1;
3061  dofformat.jointindex = jointindex+(int)_vecjoints.size();
3062  }
3063  else {
3064  dofformat.dofindex = (*itjoint)->GetDOFIndex();
3065  dofformat.jointindex = (*itjoint)->GetJointIndex();
3066  }
3067  for(int idof = 0; idof < (*itjoint)->GetDOF(); ++idof) {
3068  dofformat.axis = idof;
3069  if( !!(*itjoint)->_vmimic[idof] ) {
3070  // only add if depends on mimic joints
3071  FOREACH(itdofformat,(*itjoint)->_vmimic[idof]->_vdofformat) {
3072  JointPtr pjoint = itdofformat->GetJoint(shared_kinbody());
3073  if( pjoint->IsMimic(itdofformat->axis) ) {
3074  mapmimic[dofformat] = (*itjoint)->_vmimic[idof];
3075  break;
3076  }
3077  }
3078  }
3079  }
3080  ++jointindex;
3081  }
3082  }
3083  bool bchanged = true;
3084  while(bchanged) {
3085  bchanged = false;
3086  FOREACH(itmimic,mapmimic) {
3087  boost::shared_ptr<Mimic> mimic = itmimic->second;
3089  h.dofformatindex = 0;
3090  FOREACH(itdofformat,mimic->_vdofformat) {
3091  if( mapmimic.find(*itdofformat) == mapmimic.end() ) {
3092  continue; // this is normal, just means that the parent is a regular dof
3093  }
3094  boost::shared_ptr<Mimic> mimicparent = mapmimic[*itdofformat];
3095  FOREACH(itmimicdof, mimicparent->_vmimicdofs) {
3096  if( mimicparent->_vdofformat[itmimicdof->dofformatindex] == itmimic->first ) {
3097  JointPtr pjoint = itmimic->first.GetJoint(shared_kinbody());
3098  JointPtr pjointparent = itdofformat->GetJoint(shared_kinbody());
3099  throw OPENRAVE_EXCEPTION_FORMAT("joint index %s uses a mimic joint %s that also depends on %s! this is not allowed", pjoint->GetName()%pjointparent->GetName()%pjoint->GetName(), ORE_Failed);
3100  }
3101  h.dofindex = itmimicdof->dofindex;
3102  if( find(mimic->_vmimicdofs.begin(),mimic->_vmimicdofs.end(),h) == mimic->_vmimicdofs.end() ) {
3103  mimic->_vmimicdofs.push_back(h);
3104  bchanged = true;
3105  }
3106  }
3107  ++h.dofformatindex;
3108  }
3109  }
3110  }
3111  }
3112  catch(const std::exception& ex) {
3113  RAVELOG_ERROR(str(boost::format("failed to set mimic equations on kinematics body %s: %s\n")%GetName()%ex.what()));
3114  for(int ijoints = 0; ijoints < 2; ++ijoints) {
3115  vector<JointPtr>& vjoints = ijoints ? _vPassiveJoints : _vecjoints;
3116  FOREACH(itjoint,vjoints) {
3117  for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
3118  (*itjoint)->_vmimic[i].reset();
3119  }
3120  }
3121  }
3122  }
3123 
3124  _vTopologicallySortedJoints.resize(0);
3127  _vJointsAffectingLinks.resize(_vecjoints.size()*_veclinks.size());
3128 
3129  // compute the all-pairs shortest paths
3130  {
3131  _vAllPairsShortestPaths.resize(_veclinks.size()*_veclinks.size());
3132  FOREACH(it,_vAllPairsShortestPaths) {
3133  it->first = -1;
3134  it->second = -1;
3135  }
3136  vector<uint32_t> vcosts(_veclinks.size()*_veclinks.size(),0x3fffffff); // initialize to 2^30-1 since we'll be adding
3137  for(size_t i = 0; i < _veclinks.size(); ++i) {
3138  vcosts[i*_veclinks.size()+i] = 0;
3139  }
3140  FOREACHC(itjoint,_vecjoints) {
3141  if( !!(*itjoint)->GetFirstAttached() && !!(*itjoint)->GetSecondAttached() ) {
3142  int index = (*itjoint)->GetFirstAttached()->GetIndex()*_veclinks.size()+(*itjoint)->GetSecondAttached()->GetIndex();
3143  _vAllPairsShortestPaths[index] = std::pair<int16_t,int16_t>((*itjoint)->GetFirstAttached()->GetIndex(),(*itjoint)->GetJointIndex());
3144  vcosts[index] = 1;
3145  index = (*itjoint)->GetSecondAttached()->GetIndex()*_veclinks.size()+(*itjoint)->GetFirstAttached()->GetIndex();
3146  _vAllPairsShortestPaths[index] = std::pair<int16_t,int16_t>((*itjoint)->GetSecondAttached()->GetIndex(),(*itjoint)->GetJointIndex());
3147  vcosts[index] = 1;
3148  }
3149  }
3150  int jointindex = (int)_vecjoints.size();
3151  FOREACHC(itjoint,_vPassiveJoints) {
3152  if( !!(*itjoint)->GetFirstAttached() && !!(*itjoint)->GetSecondAttached() ) {
3153  int index = (*itjoint)->GetFirstAttached()->GetIndex()*_veclinks.size()+(*itjoint)->GetSecondAttached()->GetIndex();
3154  _vAllPairsShortestPaths[index] = std::pair<int16_t,int16_t>((*itjoint)->GetFirstAttached()->GetIndex(),jointindex);
3155  vcosts[index] = 1;
3156  index = (*itjoint)->GetSecondAttached()->GetIndex()*_veclinks.size()+(*itjoint)->GetFirstAttached()->GetIndex();
3157  _vAllPairsShortestPaths[index] = std::pair<int16_t,int16_t>((*itjoint)->GetSecondAttached()->GetIndex(),jointindex);
3158  vcosts[index] = 1;
3159  }
3160  ++jointindex;
3161  }
3162  for(size_t k = 0; k < _veclinks.size(); ++k) {
3163  for(size_t i = 0; i < _veclinks.size(); ++i) {
3164  if( i == k ) {
3165  continue;
3166  }
3167  for(size_t j = 0; j < _veclinks.size(); ++j) {
3168  if((j == i)||(j == k)) {
3169  continue;
3170  }
3171  uint32_t kcost = vcosts[k*_veclinks.size()+i] + vcosts[j*_veclinks.size()+k];
3172  if( vcosts[j*_veclinks.size()+i] > kcost ) {
3173  vcosts[j*_veclinks.size()+i] = kcost;
3175  }
3176  }
3177  }
3178  }
3179  }
3180 
3181  // Use the APAC algorithm to initialize the kinematics hierarchy: _vTopologicallySortedJoints, _vJointsAffectingLinks, Link::_vParentLinks.
3182  // SIMOES, Ricardo. APAC: An exact algorithm for retrieving cycles and paths in all kinds of graphs. TĂ©khne, Dec. 2009, no.12, p.39-55. ISSN 1654-9911.
3183  if((_veclinks.size() > 0)&&(_vecjoints.size() > 0)) {
3184  std::vector< std::vector<int> > vlinkadjacency(_veclinks.size());
3185  // joints with only one attachment are attached to a static link, which is attached to link 0
3186  FOREACHC(itjoint,_vecjoints) {
3187  vlinkadjacency.at((*itjoint)->GetFirstAttached()->GetIndex()).push_back((*itjoint)->GetSecondAttached()->GetIndex());
3188  vlinkadjacency.at((*itjoint)->GetSecondAttached()->GetIndex()).push_back((*itjoint)->GetFirstAttached()->GetIndex());
3189  }
3190  FOREACHC(itjoint,_vPassiveJoints) {
3191  vlinkadjacency.at((*itjoint)->GetFirstAttached()->GetIndex()).push_back((*itjoint)->GetSecondAttached()->GetIndex());
3192  vlinkadjacency.at((*itjoint)->GetSecondAttached()->GetIndex()).push_back((*itjoint)->GetFirstAttached()->GetIndex());
3193  }
3194  FOREACH(it,vlinkadjacency) {
3195  sort(it->begin(), it->end());
3196  }
3197 
3198  // all unique paths starting at the root link or static links
3199  std::vector< std::list< std::list<int> > > vuniquepaths(_veclinks.size());
3200  std::list< std::list<int> > closedloops;
3201  int s = 0;
3202  std::list< std::list<int> > S;
3203  FOREACH(itv,vlinkadjacency[s]) {
3204  std::list<int> P;
3205  P.push_back(s);
3206  P.push_back(*itv);
3207  S.push_back(P);
3208  vuniquepaths[*itv].push_back(P);
3209  }
3210  while(!S.empty()) {
3211  std::list<int>& P = S.front();
3212  int u = P.back();
3213  FOREACH(itv,vlinkadjacency[u]) {
3214  std::list<int>::iterator itfound = find(P.begin(),P.end(),*itv);
3215  if( itfound == P.end() ) {
3216  S.push_back(P);
3217  S.back().push_back(*itv);
3218  vuniquepaths[*itv].push_back(S.back());
3219  }
3220  else {
3221  // found a cycle
3222  std::list<int> cycle;
3223  while(itfound != P.end()) {
3224  cycle.push_back(*itfound);
3225  ++itfound;
3226  }
3227  if( cycle.size() > 2 ) {
3228  // sort the cycle so that it starts with the lowest link index and the direction is the next lowest index
3229  // this way the cycle becomes unique and can be compared for duplicates
3230  itfound = cycle.begin();
3231  std::list<int>::iterator itmin = itfound++;
3232  while(itfound != cycle.end()) {
3233  if( *itmin > *itfound ) {
3234  itmin = itfound;
3235  }
3236  itfound++;
3237  }
3238  if( itmin != cycle.begin() ) {
3239  cycle.splice(cycle.end(),cycle,cycle.begin(),itmin);
3240  }
3241  if( *++cycle.begin() > cycle.back() ) {
3242  // reverse the cycle
3243  cycle.reverse();
3244  cycle.push_front(cycle.back());
3245  cycle.pop_back();
3246  }
3247  if( find(closedloops.begin(),closedloops.end(),cycle) == closedloops.end() ) {
3248  closedloops.push_back(cycle);
3249  }
3250  }
3251  }
3252  }
3253  S.pop_front();
3254  }
3255  // fill each link's parent links
3256  FOREACH(itlink,_veclinks) {
3257  if( (*itlink)->GetIndex() > 0 && vuniquepaths.at((*itlink)->GetIndex()).size() == 0 ) {
3258  RAVELOG_WARN(str(boost::format("_ComputeInternalInformation: %s has incomplete kinematics! link %s not connected to root %s")%GetName()%(*itlink)->GetName()%_veclinks.at(0)->GetName()));
3259  }
3260  FOREACH(itpath, vuniquepaths.at((*itlink)->GetIndex())) {
3261  OPENRAVE_ASSERT_OP(itpath->back(),==,(*itlink)->GetIndex());
3262  int parentindex = *---- itpath->end();
3263  if( find((*itlink)->_vParentLinks.begin(),(*itlink)->_vParentLinks.end(),parentindex) == (*itlink)->_vParentLinks.end() ) {
3264  (*itlink)->_vParentLinks.push_back(parentindex);
3265  }
3266  }
3267  }
3268  // find the link depths (minimum path length to the root)
3269  vector<int> vlinkdepths(_veclinks.size(),-1);
3270  vlinkdepths.at(0) = 0;
3271  for(size_t i = 0; i < _veclinks.size(); ++i) {
3272  if( _veclinks[i]->IsStatic() ) {
3273  vlinkdepths[i] = 0;
3274  }
3275  }
3276  bool changed = true;
3277  while(changed) {
3278  changed = false;
3279  FOREACH(itlink,_veclinks) {
3280  if( vlinkdepths[(*itlink)->GetIndex()] == -1 ) {
3281  int bestindex = -1;
3282  FOREACH(itparent, (*itlink)->_vParentLinks) {
3283  if( vlinkdepths[*itparent] >= 0 ) {
3284  if( bestindex == -1 || (bestindex >= 0 && vlinkdepths[*itparent] < bestindex) ) {
3285  bestindex = vlinkdepths[*itparent]+1;
3286  }
3287  }
3288  }
3289  if( bestindex >= 0 ) {
3290  vlinkdepths[(*itlink)->GetIndex()] = bestindex;
3291  changed = true;
3292  }
3293  }
3294  }
3295  }
3296 
3297 
3298  if( IS_DEBUGLEVEL(Level_Verbose) ) {
3299  FOREACH(itlink, _veclinks) {
3300  std::stringstream ss; ss << GetName() << ":" << (*itlink)->GetName() << " depth=" << vlinkdepths.at((*itlink)->GetIndex()) << ", parents=[";
3301  FOREACHC(itparentlink, (*itlink)->_vParentLinks) {
3302  ss << _veclinks.at(*itparentlink)->GetName() << ", ";
3303  }
3304  ss << "]";
3305  RAVELOG_VERBOSE(ss.str());
3306  }
3307  }
3308 
3309  // build up a directed graph of joint dependencies
3310  int numjoints = (int)(_vecjoints.size()+_vPassiveJoints.size());
3311  // build the adjacency list
3312  vector<int> vjointadjacency(numjoints*numjoints,0);
3313  for(int ij0 = 0; ij0 < numjoints; ++ij0) {
3314  JointPtr j0 = ij0 < (int)_vecjoints.size() ? _vecjoints[ij0] : _vPassiveJoints[ij0-_vecjoints.size()];
3315  bool bj0hasstatic = (!j0->GetFirstAttached() || j0->GetFirstAttached()->IsStatic()) || (!j0->GetSecondAttached() || j0->GetSecondAttached()->IsStatic());
3316  // mimic joint sorting is the hardest limit
3317  if( j0->IsMimic() ) {
3318  for(int i = 0; i < j0->GetDOF(); ++i) {
3319  if(j0->IsMimic(i)) {
3320  FOREACH(itdofformat, j0->_vmimic[i]->_vdofformat) {
3321  if( itdofformat->dofindex < 0 ) {
3322  vjointadjacency[itdofformat->jointindex*numjoints+ij0] = 1;
3323  }
3324  }
3325  }
3326  }
3327  }
3328 
3329  for(int ij1 = ij0+1; ij1 < numjoints; ++ij1) {
3330  JointPtr j1 = ij1 < (int)_vecjoints.size() ? _vecjoints[ij1] : _vPassiveJoints[ij1-_vecjoints.size()];
3331  bool bj1hasstatic = (!j1->GetFirstAttached() || j1->GetFirstAttached()->IsStatic()) || (!j1->GetSecondAttached() || j1->GetSecondAttached()->IsStatic());
3332 
3333  // test if connected to world, next in priority to mimic joints
3334  if( bj0hasstatic && bj1hasstatic ) {
3335  continue;
3336  }
3337  if( vjointadjacency[ij1*numjoints+ij0] || vjointadjacency[ij0*numjoints+ij1] ) {
3338  // already have an edge, so no reason to add any more
3339  continue;
3340  }
3341 
3342  // sort by link depth
3343  int j0l0 = vlinkdepths[j0->GetFirstAttached()->GetIndex()];
3344  int j0l1 = vlinkdepths[j0->GetSecondAttached()->GetIndex()];
3345  int j1l0 = vlinkdepths[j1->GetFirstAttached()->GetIndex()];
3346  int j1l1 = vlinkdepths[j1->GetSecondAttached()->GetIndex()];
3347  int diff = min(j0l0,j0l1) - min(j1l0,j1l1);
3348  if( diff < 0 ) {
3349  OPENRAVE_ASSERT_OP(min(j0l0,j0l1),<,100);
3350  vjointadjacency[ij0*numjoints+ij1] = 100-min(j0l0,j0l1);
3351  continue;
3352  }
3353  if( diff > 0 ) {
3354  OPENRAVE_ASSERT_OP(min(j1l0,j1l1),<,100);
3355  vjointadjacency[ij1*numjoints+ij0] = 100-min(j1l0,j1l1);
3356  continue;
3357  }
3358  diff = max(j0l0,j0l1) - max(j1l0,j1l1);
3359  if( diff < 0 ) {
3360  OPENRAVE_ASSERT_OP(max(j0l0,j0l1),<,100);
3361  vjointadjacency[ij0*numjoints+ij1] = 100-max(j0l0,j0l1);
3362  continue;
3363  }
3364  if( diff > 0 ) {
3365  OPENRAVE_ASSERT_OP(max(j1l0,j1l1),<,100);
3366  vjointadjacency[ij1*numjoints+ij0] = 100-max(j1l0,j1l1);
3367  continue;
3368  }
3369  }
3370  }
3371  // topologically sort the joints
3373  std::list<int> noincomingedges;
3374  for(int i = 0; i < numjoints; ++i) {
3375  bool hasincoming = false;
3376  for(int j = 0; j < numjoints; ++j) {
3377  if( vjointadjacency[j*numjoints+i] ) {
3378  hasincoming = true;
3379  break;
3380  }
3381  }
3382  if( !hasincoming ) {
3383  noincomingedges.push_back(i);
3384  }
3385  }
3386  bool bcontinuesorting = true;
3387  while(bcontinuesorting) {
3388  bcontinuesorting = false;
3389  while(!noincomingedges.empty()) {
3390  int n = noincomingedges.front();
3391  noincomingedges.pop_front();
3393  for(int i = 0; i < numjoints; ++i) {
3394  if( vjointadjacency[n*numjoints+i] ) {
3395  vjointadjacency[n*numjoints+i] = 0;
3396  bool hasincoming = false;
3397  for(int j = 0; j < numjoints; ++j) {
3398  if( vjointadjacency[j*numjoints+i] ) {
3399  hasincoming = true;
3400  break;
3401  }
3402  }
3403  if( !hasincoming ) {
3404  noincomingedges.push_back(i);
3405  }
3406  }
3407  }
3408  }
3409 
3410  // go backwards so we prioritize moving joints towards the end rather than the beginning (not a formal heurstic)
3411  int imaxadjind = vjointadjacency[numjoints*numjoints-1];
3412  for(int ijoint = numjoints*numjoints-1; ijoint >= 0; --ijoint) {
3413  if( vjointadjacency[ijoint] > vjointadjacency[imaxadjind] ) {
3414  imaxadjind = ijoint;
3415  }
3416  }
3417  if( vjointadjacency[imaxadjind] != 0 ) {
3418  bcontinuesorting = true;
3419  int ifirst = imaxadjind/numjoints;
3420  int isecond = imaxadjind%numjoints;
3421  if( vjointadjacency[imaxadjind] <= 2 ) { // level 1 - static constraint violated, level 2 - mimic constraint
3422  JointPtr pji = ifirst < (int)_vecjoints.size() ? _vecjoints[ifirst] : _vPassiveJoints.at(ifirst-_vecjoints.size());
3423  JointPtr pjj = isecond < (int)_vecjoints.size() ? _vecjoints[isecond] : _vPassiveJoints.at(isecond-_vecjoints.size());
3424  RAVELOG_WARN(str(boost::format("cannot sort joints topologically %d for robot %s joints %s:%s!! forward kinematics might be buggy\n")%vjointadjacency[imaxadjind]%GetName()%pji->GetName()%pjj->GetName()));
3425  }
3426  // remove this edge
3427  vjointadjacency[imaxadjind] = 0;
3428  bool hasincoming = false;
3429  for(int j = 0; j < numjoints; ++j) {
3430  if( vjointadjacency[j*numjoints+isecond] ) {
3431  hasincoming = true;
3432  break;
3433  }
3434  }
3435  if( !hasincoming ) {
3436  noincomingedges.push_back(isecond);
3437  }
3438  }
3439  }
3440  OPENRAVE_ASSERT_OP((int)_vTopologicallySortedJointIndicesAll.size(),==,numjoints);
3441  FOREACH(itindex,_vTopologicallySortedJointIndicesAll) {
3442  JointPtr pj = *itindex < (int)_vecjoints.size() ? _vecjoints[*itindex] : _vPassiveJoints.at(*itindex-_vecjoints.size());
3443  if( *itindex < (int)_vecjoints.size() ) {
3444  _vTopologicallySortedJoints.push_back(pj);
3445  }
3446  _vTopologicallySortedJointsAll.push_back(pj);
3447  //RAVELOG_INFO(str(boost::format("top: %s")%pj->GetName()));
3448  }
3449 
3450  // based on this topological sorting, find the parent link for each joint
3451  FOREACH(itjoint,_vTopologicallySortedJointsAll) {
3452  int parentlinkindex = -1;
3453  if( !(*itjoint)->GetFirstAttached() || (*itjoint)->GetFirstAttached()->IsStatic() ) {
3454  if( !!(*itjoint)->GetSecondAttached() && !(*itjoint)->GetSecondAttached()->IsStatic() ) {
3455  parentlinkindex = (*itjoint)->GetSecondAttached()->GetIndex();
3456  }
3457  }
3458  else if( !(*itjoint)->GetSecondAttached() || (*itjoint)->GetSecondAttached()->IsStatic() ) {
3459  parentlinkindex = (*itjoint)->GetFirstAttached()->GetIndex();
3460  }
3461  else {
3462  // NOTE: possibly try to choose roots that do not involve mimic joints. ikfast might have problems
3463  // dealing with very complex formulas
3464  LinkPtr plink0 = (*itjoint)->GetFirstAttached(), plink1 = (*itjoint)->GetSecondAttached();
3465  if( vlinkdepths[plink0->GetIndex()] < vlinkdepths[plink1->GetIndex()] ) {
3466  parentlinkindex = plink0->GetIndex();
3467  }
3468  else if( vlinkdepths[plink0->GetIndex()] > vlinkdepths[plink1->GetIndex()] ) {
3469  parentlinkindex = plink1->GetIndex();
3470  }
3471  else {
3472  // depths are the same, so check the adjacent joints of each link
3473  size_t link0pos=_vTopologicallySortedJointIndicesAll.size(), link1pos=_vTopologicallySortedJointIndicesAll.size();
3474  FOREACHC(itparentlink,plink0->_vParentLinks) {
3475  int jointindex = _vAllPairsShortestPaths[plink0->GetIndex()*_veclinks.size()+*itparentlink].second;
3477  link0pos = min(link0pos,pos);
3478  }
3479  FOREACHC(itparentlink,plink1->_vParentLinks) {
3480  int jointindex = _vAllPairsShortestPaths[plink1->GetIndex()*_veclinks.size()+*itparentlink].second;
3482  link1pos = min(link1pos,pos);
3483  }
3484  if( link0pos < link1pos ) {
3485  parentlinkindex = plink0->GetIndex();
3486  }
3487  else if( link0pos > link1pos ) {
3488  parentlinkindex = plink1->GetIndex();
3489  }
3490  else {
3491  RAVELOG_WARN(str(boost::format("links %s and %s have joints on the same depth %d and %d?")%plink0->GetName()%plink1->GetName()%link0pos%link1pos));
3492  }
3493  }
3494  }
3495  if( parentlinkindex == -1 ) {
3496  RAVELOG_WARN(str(boost::format("could not compute parent link for joint %s")%(*itjoint)->GetName()));
3497  }
3498  else if( parentlinkindex != (*itjoint)->GetFirstAttached()->GetIndex() ) {
3499  RAVELOG_VERBOSE(str(boost::format("swapping link order of joint %s(%d)")%(*itjoint)->GetName()%(*itjoint)->GetJointIndex()));
3500  // have to swap order
3501  Transform tswap = (*itjoint)->GetInternalHierarchyRightTransform().inverse();
3502  std::vector<Vector> vaxes((*itjoint)->GetDOF());
3503  std::vector<dReal> vcurrentvalues;
3504  (*itjoint)->GetValues(vcurrentvalues);
3505  for(size_t i = 0; i < vaxes.size(); ++i) {
3506  vaxes[i] = -tswap.rotate((*itjoint)->GetInternalHierarchyAxis(i));
3507  }
3508  // have to reset the link transformations temporarily in order to avoid setting a joint offset
3509  TransformSaver<LinkPtr> linksaver0((*itjoint)->GetFirstAttached());
3510  TransformSaver<LinkPtr> linksaver1((*itjoint)->GetSecondAttached());
3511  (*itjoint)->GetFirstAttached()->SetTransform(Transform());
3512  (*itjoint)->GetSecondAttached()->SetTransform((*itjoint)->GetInternalHierarchyLeftTransform()*(*itjoint)->GetInternalHierarchyRightTransform());
3513  (*itjoint)->_ComputeInternalInformation((*itjoint)->GetSecondAttached(),(*itjoint)->GetFirstAttached(),tswap.trans,vaxes,vcurrentvalues);
3514  }
3515  }
3516  // find out what links are affected by what joints.
3517  FOREACH(it,_vJointsAffectingLinks) {
3518  *it = 0;
3519  }
3520  vector<int8_t> vusedlinks;
3521  for(int i = 0; i < (int)_veclinks.size(); ++i) {
3522  vusedlinks.resize(0); vusedlinks.resize(_veclinks.size());
3523  FOREACH(itpath,vuniquepaths[i]) {
3524  FOREACH(itlink,*itpath) {
3525  vusedlinks[*itlink] = 1;
3526  }
3527  }
3528  for(int j = 0; j < (int)_veclinks.size(); ++j) {
3529  if( vusedlinks[j] &&(i != j)) {
3530  int jointindex = _vAllPairsShortestPaths[i*_veclinks.size()+j].second;
3531  OPENRAVE_ASSERT_OP( jointindex, >=, 0 );
3532  JointPtr pjoint = jointindex < (int)_vecjoints.size() ? _vecjoints[jointindex] : _vPassiveJoints.at(jointindex-_vecjoints.size());
3533  if( jointindex < (int)_vecjoints.size() ) {
3534  _vJointsAffectingLinks[jointindex*_veclinks.size()+i] = pjoint->GetHierarchyParentLink()->GetIndex() == i ? -1 : 1;
3535  }
3536  if( pjoint->IsMimic() ) {
3537  for(int idof = 0; idof < pjoint->GetDOF(); ++idof) {
3538  if( pjoint->IsMimic(idof) ) {
3539  FOREACHC(itmimicdof,pjoint->_vmimic[idof]->_vmimicdofs) {
3540  JointPtr pjoint2 = GetJointFromDOFIndex(itmimicdof->dofindex);
3541  _vJointsAffectingLinks[pjoint2->GetJointIndex()*_veclinks.size()+i] = pjoint2->GetHierarchyParentLink()->GetIndex() == i ? -1 : 1;
3542  }
3543  }
3544  }
3545  }
3546  }
3547  }
3548  }
3549 
3550  // process the closed loops, note that determining 'degrees of freedom' of the loop is very difficult and should be left to the 'fkfast' tool
3551  _vClosedLoopIndices.resize(0); _vClosedLoopIndices.reserve(closedloops.size());
3552  _vClosedLoops.resize(0); _vClosedLoops.reserve(closedloops.size());
3553  FOREACH(itclosedloop,closedloops) {
3554  _vClosedLoopIndices.push_back(vector< std::pair<int16_t, int16_t> >());
3555  _vClosedLoopIndices.back().reserve(itclosedloop->size());
3556  _vClosedLoops.push_back(vector< std::pair<LinkPtr, JointPtr> >());
3557  _vClosedLoops.back().reserve(itclosedloop->size());
3558  // fill the links
3559  FOREACH(itlinkindex,*itclosedloop) {
3560  _vClosedLoopIndices.back().push_back(make_pair<int16_t,int16_t>(*itlinkindex,0));
3561  _vClosedLoops.back().push_back(make_pair<LinkPtr,JointPtr>(_veclinks.at(*itlinkindex),JointPtr()));
3562  }
3563  // fill the joints
3564  for(size_t i = 0; i < _vClosedLoopIndices.back().size(); ++i) {
3565  int nextlink = i+1 < _vClosedLoopIndices.back().size() ? _vClosedLoopIndices.back()[i+1].first : _vClosedLoopIndices.back()[0].first;
3566  int jointindex = _vAllPairsShortestPaths[nextlink*_veclinks.size()+_vClosedLoopIndices.back()[i].first].second;
3567  _vClosedLoopIndices.back()[i].second = jointindex;
3568  if( jointindex < (int)_vecjoints.size() ) {
3569  _vClosedLoops.back().at(i).second = _vecjoints.at(jointindex);
3570  }
3571  else {
3572  _vClosedLoops.back().at(i).second = _vPassiveJoints.at(jointindex-_vecjoints.size());
3573  }
3574  }
3575 
3576  if( IS_DEBUGLEVEL(Level_Verbose) ) {
3577  stringstream ss;
3578  ss << GetName() << " closedloop found: ";
3579  FOREACH(itlinkindex,*itclosedloop) {
3580  LinkPtr plink = _veclinks.at(*itlinkindex);
3581  ss << plink->GetName() << "(" << plink->GetIndex() << ") ";
3582  }
3583  RAVELOG_VERBOSE(ss.str());
3584  }
3585  }
3586  }
3587 
3588  // compute the rigidly attached links
3589  for(size_t ilink = 0; ilink < _veclinks.size(); ++ilink) {
3590  vector<int>& vattachedlinks = _veclinks[ilink]->_vRigidlyAttachedLinks;
3591  vattachedlinks.resize(0);
3592  vattachedlinks.push_back(ilink);
3593  if((ilink == 0)|| _veclinks[ilink]->IsStatic() ) {
3594  FOREACHC(itlink,_veclinks) {
3595  if( (*itlink)->IsStatic() ) {
3596  if( (*itlink)->GetIndex() != (int)ilink ) {
3597  vattachedlinks.push_back((*itlink)->GetIndex());
3598  }
3599  }
3600  }
3601  FOREACHC(itjoint, GetJoints()) {
3602  if( (*itjoint)->IsStatic() ) {
3603  if( !(*itjoint)->GetFirstAttached() && !!(*itjoint)->GetSecondAttached() && !(*itjoint)->GetSecondAttached()->IsStatic() ) {
3604  vattachedlinks.push_back((*itjoint)->GetSecondAttached()->GetIndex());
3605  }
3606  if( !(*itjoint)->GetSecondAttached() && !!(*itjoint)->GetFirstAttached() && !(*itjoint)->GetFirstAttached()->IsStatic() ) {
3607  vattachedlinks.push_back((*itjoint)->GetFirstAttached()->GetIndex());
3608  }
3609  }
3610  }
3611  FOREACHC(itpassive, GetPassiveJoints()) {
3612  if( (*itpassive)->IsStatic() ) {
3613  if( !(*itpassive)->GetFirstAttached() && !!(*itpassive)->GetSecondAttached() && !(*itpassive)->GetSecondAttached()->IsStatic() ) {
3614  vattachedlinks.push_back((*itpassive)->GetSecondAttached()->GetIndex());
3615  }
3616  if( !(*itpassive)->GetSecondAttached() && !!(*itpassive)->GetFirstAttached() && !(*itpassive)->GetFirstAttached()->IsStatic() ) {
3617  vattachedlinks.push_back((*itpassive)->GetFirstAttached()->GetIndex());
3618  }
3619  }
3620  }
3621  }
3622 
3623  // breadth first search for rigid links
3624  for(size_t icurlink = 0; icurlink<vattachedlinks.size(); ++icurlink) {
3625  LinkPtr plink=_veclinks.at(vattachedlinks[icurlink]);
3626  FOREACHC(itjoint, _vecjoints) {
3627  if( (*itjoint)->IsStatic() ) {
3628  if(((*itjoint)->GetFirstAttached() == plink)&& !!(*itjoint)->GetSecondAttached() &&(find(vattachedlinks.begin(),vattachedlinks.end(),(*itjoint)->GetSecondAttached()->GetIndex()) == vattachedlinks.end())) {
3629  vattachedlinks.push_back((*itjoint)->GetSecondAttached()->GetIndex());
3630  }
3631  if(((*itjoint)->GetSecondAttached() == plink)&& !!(*itjoint)->GetFirstAttached() &&(find(vattachedlinks.begin(),vattachedlinks.end(),(*itjoint)->GetFirstAttached()->GetIndex()) == vattachedlinks.end())) {
3632  vattachedlinks.push_back((*itjoint)->GetFirstAttached()->GetIndex());
3633  }
3634  }
3635  }
3636  FOREACHC(itpassive, _vPassiveJoints) {
3637  if( (*itpassive)->IsStatic() ) {
3638  if(((*itpassive)->GetFirstAttached() == plink)&& !!(*itpassive)->GetSecondAttached() &&(find(vattachedlinks.begin(),vattachedlinks.end(),(*itpassive)->GetSecondAttached()->GetIndex()) == vattachedlinks.end())) {
3639  vattachedlinks.push_back((*itpassive)->GetSecondAttached()->GetIndex());
3640  }
3641  if(((*itpassive)->GetSecondAttached() == plink)&& !!(*itpassive)->GetFirstAttached() &&(find(vattachedlinks.begin(),vattachedlinks.end(),(*itpassive)->GetFirstAttached()->GetIndex()) == vattachedlinks.end())) {
3642  vattachedlinks.push_back((*itpassive)->GetFirstAttached()->GetIndex());
3643  }
3644  }
3645  }
3646  }
3647  }
3648 
3649  for(size_t ijoint = 0; ijoint < _vecjoints.size(); ++ijoint ) {
3650  if( _vecjoints[ijoint]->GetName().size() == 0 ) {
3651  RAVELOG_WARN(str(boost::format("%s joint index %d has no name")%GetName()%ijoint));
3652  }
3653  }
3654  for(size_t ijoint = 0; ijoint < _vPassiveJoints.size(); ++ijoint ) {
3655  if( _vPassiveJoints[ijoint]->GetName().size() == 0 ) {
3656  RAVELOG_WARN(str(boost::format("%s passive joint index %d has no name")%GetName()%ijoint));
3657  }
3658  }
3659  for(size_t ijoint0 = 0; ijoint0 < _vTopologicallySortedJointsAll.size(); ++ijoint0 ) {
3660  JointPtr pjoint0 = _vTopologicallySortedJointsAll[ijoint0];
3661  for(size_t ijoint1 = ijoint0+1; ijoint1 < _vTopologicallySortedJointsAll.size(); ++ijoint1 ) {
3662  JointPtr pjoint1 = _vTopologicallySortedJointsAll[ijoint1];
3663  if( pjoint0->GetName() == pjoint1->GetName() && (pjoint0->GetJointIndex() >= 0 || pjoint1->GetJointIndex() >= 0) ) {
3664  throw OPENRAVE_EXCEPTION_FORMAT("joint indices %d and %d share the same name '%s'", pjoint0->GetJointIndex()%pjoint1->GetJointIndex()%pjoint0->GetName(), ORE_InvalidState);
3665  }
3666  }
3667  }
3668 
3669  __hashkinematics.resize(0);
3670 
3671  // create the adjacency list
3672  {
3673  _setAdjacentLinks.clear();
3674  FOREACH(itadj, _vForcedAdjacentLinks) {
3675  LinkPtr pl0 = GetLink(itadj->first);
3676  LinkPtr pl1 = GetLink(itadj->second);
3677  if( !!pl0 && !!pl1 ) {
3678  int ind0 = pl0->GetIndex();
3679  int ind1 = pl1->GetIndex();
3680  if( ind1 < ind0 ) {
3681  _setAdjacentLinks.insert(ind1|(ind0<<16));
3682  }
3683  else {
3684  _setAdjacentLinks.insert(ind0|(ind1<<16));
3685  }
3686  }
3687  }
3688 
3689  // make no-geometry links adjacent to all other links
3690  FOREACH(itlink0, _veclinks) {
3691  if( (*itlink0)->GetGeometries().size() == 0 ) {
3692  int ind0 = (*itlink0)->GetIndex();
3693  FOREACH(itlink1,_veclinks) {
3694  if( *itlink0 != *itlink1 ) {
3695  int ind1 = (*itlink1)->GetIndex();
3696  if( ind1 < ind0 ) {
3697  _setAdjacentLinks.insert(ind1|(ind0<<16));
3698  }
3699  else {
3700  _setAdjacentLinks.insert(ind0|(ind1<<16));
3701  }
3702  }
3703  }
3704  }
3705  }
3706 
3708  FOREACH(itj, _vecjoints) {
3709  int ind0 = (*itj)->_attachedbodies[0]->GetIndex();
3710  int ind1 = (*itj)->_attachedbodies[1]->GetIndex();
3711  if( ind1 < ind0 ) {
3712  _setAdjacentLinks.insert(ind1|(ind0<<16));
3713  }
3714  else {
3715  _setAdjacentLinks.insert(ind0|(ind1<<16));
3716  }
3717  }
3718 
3719  FOREACH(itj, _vPassiveJoints) {
3720  int ind0 = (*itj)->_attachedbodies[0]->GetIndex();
3721  int ind1 = (*itj)->_attachedbodies[1]->GetIndex();
3722  if( ind1 < ind0 ) {
3723  _setAdjacentLinks.insert(ind1|(ind0<<16));
3724  }
3725  else {
3726  _setAdjacentLinks.insert(ind0|(ind1<<16));
3727  }
3728  }
3729 
3730  // if a pair links has exactly one non-static joint in the middle, then make the pair adjacent
3731  vector<JointPtr> vjoints;
3732  for(int i = 0; i < (int)_veclinks.size()-1; ++i) {
3733  for(int j = i+1; j < (int)_veclinks.size(); ++j) {
3734  GetChain(i,j,vjoints);
3735  size_t numstatic = 0;
3736  FOREACH(it,vjoints) {
3737  numstatic += (*it)->IsStatic();
3738  }
3739  if( numstatic+1 >= vjoints.size() ) {
3740  if( i < j ) {
3741  _setAdjacentLinks.insert(i|(j<<16));
3742  }
3743  else {
3744  _setAdjacentLinks.insert(j|(i<<16));
3745  }
3746  }
3747  }
3748  }
3749  }
3751  }
3752  _nHierarchyComputed = 2;
3753  // because of mimic joints, need to call SetDOFValues at least once, also use this to check for links that are off
3754  {
3755  vector<Transform> vprevtrans, vnewtrans;
3756  vector<int> vprevdofbranches, vnewdofbranches;
3757  GetLinkTransformations(vprevtrans, vprevdofbranches);
3758  vector<dReal> vcurrentvalues;
3759  // unfortunately if SetDOFValues is overloaded by the robot, it could call the robot's _UpdateGrabbedBodies, which is a problem during environment cloning since the grabbed bodies might not be initialized. Therefore, call KinBody::SetDOFValues
3760  GetDOFValues(vcurrentvalues);
3761  KinBody::SetDOFValues(vcurrentvalues,true, std::vector<int>());
3762  GetLinkTransformations(vnewtrans, vnewdofbranches);
3763  for(size_t i = 0; i < vprevtrans.size(); ++i) {
3764  if( TransformDistanceFast(vprevtrans[i],vnewtrans[i]) > 1e-5 ) {
3765  RAVELOG_VERBOSE(str(boost::format("link %d has different transformation after SetDOFValues (error=%f), this could be due to mimic joint equations kicking into effect.")%_veclinks.at(i)->GetName()%TransformDistanceFast(vprevtrans[i],vnewtrans[i])));
3766  }
3767  }
3768  for(int i = 0; i < GetDOF(); ++i) {
3769  if( vprevdofbranches.at(i) != vnewdofbranches.at(i) ) {
3770  RAVELOG_VERBOSE(str(boost::format("dof %d has different branches after SetDOFValues %d!=%d, this could be due to mimic joint equations kicking into effect.")%i%vprevdofbranches.at(i)%vnewdofbranches.at(i)));
3771  }
3772  }
3773  _vInitialLinkTransformations = vnewtrans;
3774  }
3775 
3776  {
3777  // do not initialize interpolation, since it implies a motion sampling strategy
3778  int offset = 0;
3779  _spec._vgroups.resize(0);
3780  if( GetDOF() > 0 ) {
3782  stringstream ss;
3783  ss << "joint_values " << GetName();
3784  for(int i = 0; i < GetDOF(); ++i) {
3785  ss << " " << i;
3786  }
3787  group.name = ss.str();
3788  group.dof = GetDOF();
3789  group.offset = offset;
3790  offset += group.dof;
3791  _spec._vgroups.push_back(group);
3792  }
3793 
3795  group.name = str(boost::format("affine_transform %s %d")%GetName()%DOF_Transform);
3796  group.offset = offset;
3798  _spec._vgroups.push_back(group);
3799  }
3800 
3801  // notify any callbacks of the changes
3802  if( _nParametersChanged ) {
3803  std::list<UserDataWeakPtr> listRegisteredCallbacks;
3804  {
3805  boost::shared_lock< boost::shared_mutex > lock(GetInterfaceMutex());
3806  listRegisteredCallbacks = _listRegisteredCallbacks; // copy since it can be changed
3807  }
3808  FOREACH(it,listRegisteredCallbacks) {
3809  ChangeCallbackDataPtr pdata = boost::dynamic_pointer_cast<ChangeCallbackData>(it->lock());
3810  if( !!pdata && (pdata->_properties & _nParametersChanged) ) {
3811  pdata->_callback();
3812  }
3813  }
3814  _nParametersChanged = 0;
3815  }
3816  RAVELOG_DEBUG(str(boost::format("_ComputeInternalInformation %s in %fs")%GetName()%(1e-6*(utils::GetMicroTime()-starttime))));
3817 }
3818 
3820 {
3821  if( shared_kinbody_const() == pbody ) {
3822  return true;
3823  }
3824  std::set<KinBodyConstPtr> dummy;
3825  return _IsAttached(pbody,dummy);
3826 }
3827 
3828 void KinBody::GetAttached(std::set<KinBodyPtr>&setAttached) const
3829 {
3830  setAttached.insert(boost::const_pointer_cast<KinBody>(shared_kinbody_const()));
3831  FOREACHC(itbody,_listAttachedBodies) {
3832  KinBodyPtr pattached = itbody->lock();
3833  if( !!pattached && setAttached.insert(pattached).second ) {
3834  pattached->GetAttached(setAttached);
3835  }
3836  }
3837 }
3838 
3839 bool KinBody::_IsAttached(KinBodyConstPtr pbody, std::set<KinBodyConstPtr>&setChecked) const
3840 {
3841  if( !setChecked.insert(shared_kinbody_const()).second ) {
3842  return false;
3843  }
3844  FOREACHC(itbody,_listAttachedBodies) {
3845  KinBodyConstPtr pattached = itbody->lock();
3846  if( !!pattached && ((pattached == pbody)|| pattached->_IsAttached(pbody,setChecked)) ) {
3847  return true;
3848  }
3849  }
3850  return false;
3851 }
3852 
3854 {
3855  _listAttachedBodies.push_back(pbody);
3856  pbody->_listAttachedBodies.push_back(shared_kinbody());
3857 }
3858 
3860 {
3861  int numremoved = 0;
3862  FOREACH(it,_listAttachedBodies) {
3863  if( it->lock() == pbody ) {
3864  _listAttachedBodies.erase(it);
3865  numremoved++;
3866  break;
3867  }
3868  }
3869 
3870  KinBodyPtr pthisbody = shared_kinbody();
3871  FOREACH(it,pbody->_listAttachedBodies) {
3872  if( it->lock() == pthisbody ) {
3873  pbody->_listAttachedBodies.erase(it);
3874  numremoved++;
3875  break;
3876  }
3877  }
3878 
3879  return numremoved==2;
3880 }
3881 
3882 void KinBody::Enable(bool bEnable)
3883 {
3884  bool bchanged = false;
3885  FOREACH(it, _veclinks) {
3886  if( (*it)->_info._bIsEnabled != bEnable ) {
3887  (*it)->_info._bIsEnabled = bEnable;
3889  bchanged = true;
3890  }
3891  }
3892  if( bchanged ) {
3894  }
3895 }
3896 
3898 {
3899  FOREACHC(it, _veclinks) {
3900  if((*it)->IsEnabled()) {
3901  return true;
3902  }
3903  }
3904  return false;
3905 }
3906 
3907 bool KinBody::SetVisible(bool visible)
3908 {
3909  bool bchanged = false;
3910  FOREACH(it, _veclinks) {
3911  FOREACH(itgeom,(*it)->_vGeometries) {
3912  if( (*itgeom)->IsVisible() != visible ) {
3913  (*itgeom)->_info._bVisible = visible;
3914  bchanged = true;
3915  }
3916  }
3917  }
3918  if( bchanged ) {
3920  return true;
3921  }
3922  return false;
3923 }
3924 
3926 {
3927  FOREACHC(it, _veclinks) {
3928  if((*it)->IsVisible()) {
3929  return true;
3930  }
3931  }
3932  return false;
3933 }
3934 
3936 {
3937  return _environmentid;
3938 }
3939 
3940 int8_t KinBody::DoesAffect(int jointindex, int linkindex ) const
3941 {
3943  OPENRAVE_ASSERT_FORMAT(jointindex >= 0 && jointindex < (int)_vecjoints.size(), "body %s jointindex %d invalid (num joints %d)", GetName()%jointindex%_vecjoints.size(), ORE_InvalidArguments);
3944  OPENRAVE_ASSERT_FORMAT(linkindex >= 0 && linkindex < (int)_veclinks.size(), "body %s linkindex %d invalid (num links %d)", GetName()%linkindex%_veclinks.size(), ORE_InvalidArguments);
3945  return _vJointsAffectingLinks.at(jointindex*_veclinks.size()+linkindex);
3946 }
3947 
3949 {
3951  vector<int> vdofbranches;
3953 }
3954 
3956 {
3957  _nNonAdjacentLinkCache = 0x80000000;
3958  FOREACH(it,_setNonAdjacentLinks) {
3959  it->clear();
3960  }
3961 }
3962 
3963 const std::set<int>& KinBody::GetNonAdjacentLinks(int adjacentoptions) const
3964 {
3965  class TransformsSaver
3966  {
3967 public:
3968  TransformsSaver(KinBodyConstPtr pbody) : _pbody(pbody) {
3969  _pbody->GetLinkTransformations(vcurtrans, _vdofbranches);
3970  }
3971  ~TransformsSaver() {
3972  for(size_t i = 0; i < _pbody->_veclinks.size(); ++i) {
3973  boost::static_pointer_cast<Link>(_pbody->_veclinks[i])->_info._t = vcurtrans.at(i);
3974  }
3975  for(size_t i = 0; i < _pbody->_vecjoints.size(); ++i) {
3976  for(int j = 0; j < _pbody->_vecjoints[i]->GetDOF(); ++j) {
3977  _pbody->_vecjoints[i]->_dofbranches[j] = _vdofbranches.at(_pbody->_vecjoints[i]->GetDOFIndex()+j);
3978  }
3979  }
3980  }
3981 private:
3982  KinBodyConstPtr _pbody;
3983  std::vector<Transform> vcurtrans;
3984  std::vector<int> _vdofbranches;
3985  };
3986 
3988  if( _nNonAdjacentLinkCache & 0x80000000 ) {
3989  // Check for colliding link pairs given the initial pose _vInitialLinkTransformations
3990  // this is actually weird, we need to call the individual link collisions on a const body. in order to pull this off, we need to be very careful with the body state.
3991  TransformsSaver saver(shared_kinbody_const());
3992  CollisionOptionsStateSaver colsaver(GetEnv()->GetCollisionChecker(),0); // have to reset the collision options
3993  for(size_t i = 0; i < _veclinks.size(); ++i) {
3994  boost::static_pointer_cast<Link>(_veclinks[i])->_info._t = _vInitialLinkTransformations.at(i);
3995  }
3996  _nUpdateStampId++; // because transforms were modified
3997  for(size_t i = 0; i < _veclinks.size(); ++i) {
3998  for(size_t j = i+1; j < _veclinks.size(); ++j) {
3999  if((_setAdjacentLinks.find(i|(j<<16)) == _setAdjacentLinks.end())&& !GetEnv()->CheckCollision(LinkConstPtr(_veclinks[i]), LinkConstPtr(_veclinks[j])) ) {
4000  _setNonAdjacentLinks[0].insert(i|(j<<16));
4001  }
4002  }
4003  }
4004  _nUpdateStampId++; // because transforms were modified
4006  }
4007  if( (_nNonAdjacentLinkCache&adjacentoptions) != adjacentoptions ) {
4008  int requestedoptions = (~_nNonAdjacentLinkCache)&adjacentoptions;
4009  // find out what needs to computed
4010  if( requestedoptions & AO_Enabled ) {
4011  _setNonAdjacentLinks.at(AO_Enabled).clear();
4012  FOREACHC(itset, _setNonAdjacentLinks[0]) {
4013  KinBody::LinkConstPtr plink1(_veclinks.at(*itset&0xffff)), plink2(_veclinks.at(*itset>>16));
4014  if( plink1->IsEnabled() && plink2->IsEnabled() ) {
4015  _setNonAdjacentLinks[AO_Enabled].insert(*itset);
4016  }
4017  }
4019  }
4020  else {
4021  throw OPENRAVE_EXCEPTION_FORMAT("no support for adjacentoptions %d", adjacentoptions,ORE_InvalidArguments);
4022  }
4023  }
4024  return _setNonAdjacentLinks.at(adjacentoptions);
4025 }
4026 
4027 const std::set<int>& KinBody::GetAdjacentLinks() const
4028 {
4030  return _setAdjacentLinks;
4031 }
4032 
4033 void KinBody::Clone(InterfaceBaseConstPtr preference, int cloningoptions)
4034 {
4035  InterfaceBase::Clone(preference,cloningoptions);
4036  KinBodyConstPtr r = RaveInterfaceConstCast<KinBody>(preference);
4037 
4038  _name = r->_name;
4039  _nHierarchyComputed = r->_nHierarchyComputed;
4040  _bMakeJoinedLinksAdjacent = r->_bMakeJoinedLinksAdjacent;
4041  __hashkinematics = r->__hashkinematics;
4042  _vTempJoints = r->_vTempJoints;
4043 
4044  _veclinks.resize(0); _veclinks.reserve(r->_veclinks.size());
4045  FOREACHC(itlink, r->_veclinks) {
4046  LinkPtr pnewlink(new Link(shared_kinbody()));
4047  *pnewlink = **itlink; // be careful of copying pointers
4048  pnewlink->_parent = shared_kinbody();
4049  _veclinks.push_back(pnewlink);
4050  }
4051 
4052  _vecjoints.resize(0); _vecjoints.reserve(r->_vecjoints.size());
4053  FOREACHC(itjoint, r->_vecjoints) {
4054  JointPtr pnewjoint(new Joint(shared_kinbody()));
4055  *pnewjoint = **itjoint; // be careful of copying pointers!
4056  pnewjoint->_parent = shared_kinbody();
4057  pnewjoint->_attachedbodies[0] = _veclinks.at((*itjoint)->_attachedbodies[0]->GetIndex());
4058  pnewjoint->_attachedbodies[1] = _veclinks.at((*itjoint)->_attachedbodies[1]->GetIndex());
4059  _vecjoints.push_back(pnewjoint);
4060  }
4061 
4062  _vPassiveJoints.resize(0); _vPassiveJoints.reserve(r->_vPassiveJoints.size());
4063  FOREACHC(itjoint, r->_vPassiveJoints) {
4064  JointPtr pnewjoint(new Joint(shared_kinbody()));
4065  *pnewjoint = **itjoint; // be careful of copying pointers!
4066  pnewjoint->_parent = shared_kinbody();
4067  pnewjoint->_attachedbodies[0] = _veclinks.at((*itjoint)->_attachedbodies[0]->GetIndex());
4068  pnewjoint->_attachedbodies[1] = _veclinks.at((*itjoint)->_attachedbodies[1]->GetIndex());
4069  _vPassiveJoints.push_back(pnewjoint);
4070  }
4071 
4072  _vTopologicallySortedJoints.resize(0); _vTopologicallySortedJoints.resize(r->_vTopologicallySortedJoints.size());
4073  FOREACHC(itjoint, r->_vTopologicallySortedJoints) {
4074  _vTopologicallySortedJoints.push_back(_vecjoints.at((*itjoint)->GetJointIndex()));
4075  }
4076  _vTopologicallySortedJointsAll.resize(0); _vTopologicallySortedJointsAll.resize(r->_vTopologicallySortedJointsAll.size());
4077  FOREACHC(itjoint, r->_vTopologicallySortedJointsAll) {
4078  std::vector<JointPtr>::const_iterator it = find(r->_vecjoints.begin(),r->_vecjoints.end(),*itjoint);
4079  if( it != r->_vecjoints.end() ) {
4080  _vTopologicallySortedJointsAll.push_back(_vecjoints.at(it-r->_vecjoints.begin()));
4081  }
4082  else {
4083  it = find(r->_vPassiveJoints.begin(), r->_vPassiveJoints.end(),*itjoint);
4084  if( it != r->_vPassiveJoints.end() ) {
4085  _vTopologicallySortedJointsAll.push_back(_vPassiveJoints.at(it-r->_vPassiveJoints.begin()));
4086  }
4087  else {
4088  throw OPENRAVE_EXCEPTION_FORMAT("joint %s doesn't belong to anythong?",(*itjoint)->GetName(), ORE_Assert);
4089  }
4090  }
4091  }
4092  _vDOFOrderedJoints = r->_vDOFOrderedJoints;
4093  _vJointsAffectingLinks = r->_vJointsAffectingLinks;
4094  _vDOFIndices = r->_vDOFIndices;
4095 
4096  _setAdjacentLinks = r->_setAdjacentLinks;
4097  _vInitialLinkTransformations = r->_vInitialLinkTransformations;
4098  _vForcedAdjacentLinks = r->_vForcedAdjacentLinks;
4099  _vAllPairsShortestPaths = r->_vAllPairsShortestPaths;
4100  _vClosedLoopIndices = r->_vClosedLoopIndices;
4101  _vClosedLoops.resize(0); _vClosedLoops.reserve(r->_vClosedLoops.size());
4102  FOREACHC(itloop,_vClosedLoops) {
4103  _vClosedLoops.push_back(std::vector< std::pair<LinkPtr,JointPtr> >());
4104  FOREACHC(it,*itloop) {
4105  _vClosedLoops.back().push_back(make_pair(_veclinks.at(it->first->GetIndex()),JointPtr()));
4106  // the joint might be in _vPassiveJoints
4107  std::vector<JointPtr>::const_iterator itjoint = find(r->_vecjoints.begin(),r->_vecjoints.end(),it->second);
4108  if( itjoint != r->_vecjoints.end() ) {
4109  _vClosedLoops.back().back().second = _vecjoints.at(itjoint-r->_vecjoints.begin());
4110  }
4111  else {
4112  itjoint = find(r->_vPassiveJoints.begin(), r->_vPassiveJoints.end(),it->second);
4113  if( itjoint != r->_vPassiveJoints.end() ) {
4114  _vClosedLoops.back().back().second = _vPassiveJoints.at(itjoint-r->_vPassiveJoints.begin());
4115  }
4116  else {
4117  throw OPENRAVE_EXCEPTION_FORMAT("joint %s in closed loop doesn't belong to anythong?",(*itjoint)->GetName(), ORE_Assert);
4118  }
4119  }
4120  }
4121  }
4122 
4123  _listAttachedBodies.clear(); // will be set in the environment
4124  FOREACHC(itatt, r->_listAttachedBodies) {
4125  KinBodyConstPtr pattref = itatt->lock();
4126  if( !!pattref ) {
4127  _listAttachedBodies.push_back(GetEnv()->GetBodyFromEnvironmentId(pattref->GetEnvironmentId()));
4128  }
4129  }
4130 
4131  // cannot copy the velocities since it requires the physics engine to be initialized with this kinbody, which might not happen before the clone..?
4132 // std::vector<std::pair<Vector,Vector> > velocities;
4133 // r->GetLinkVelocities(velocities);
4134 // SetLinkVelocities(velocities);
4135 
4136  // do not force-reset the callbacks!! since the ChangeCallbackData destructors will crash
4137  //_listRegisteredCallbacks.clear();
4138 
4139  // cache
4141  _nUpdateStampId++; // update the stamp instead of copying
4142 }
4143 
4144 void KinBody::_ParametersChanged(int parameters)
4145 {
4146  _nUpdateStampId++;
4147  if( _nHierarchyComputed == 1 ) {
4148  _nParametersChanged |= parameters;
4149  return;
4150  }
4151 
4152  if( (parameters & Prop_JointMimic) == Prop_JointMimic || (parameters & Prop_LinkStatic) == Prop_LinkStatic) {
4153  KinBodyStateSaver saver(shared_kinbody(),Save_LinkTransformation);
4154  vector<dReal> vzeros(GetDOF(),0);
4155  SetDOFValues(vzeros,Transform(),true);
4157  }
4158  // do not change hash if geometry changed!
4159  boost::array<int,3> hashproperties = {{Prop_LinkDynamics, Prop_LinkGeometry, Prop_JointMimic }};
4160  FOREACH(it, hashproperties) {
4161  if( (parameters & *it) == *it ) {
4162  __hashkinematics.resize(0);
4163  break;
4164  }
4165  }
4166 
4167  std::list<UserDataWeakPtr> listRegisteredCallbacks;
4168  {
4169  boost::shared_lock< boost::shared_mutex > lock(GetInterfaceMutex());
4170  listRegisteredCallbacks = _listRegisteredCallbacks; // copy since it can be changed
4171  }
4172  FOREACH(it,listRegisteredCallbacks) {
4173  ChangeCallbackDataPtr pdata = boost::dynamic_pointer_cast<ChangeCallbackData>(it->lock());
4174  if( !!pdata && (pdata->_properties & parameters) ) {
4175  pdata->_callback();
4176  }
4177  }
4178 }
4179 
4180 void KinBody::Serialize(BaseXMLWriterPtr writer, int options) const
4181 {
4182  InterfaceBase::Serialize(writer,options);
4183 }
4184 
4185 void KinBody::serialize(std::ostream& o, int options) const
4186 {
4187  o << _veclinks.size() << " ";
4188  FOREACHC(it,_veclinks) {
4189  (*it)->serialize(o,options);
4190  }
4191  o << _vecjoints.size() << " ";
4192  FOREACHC(it,_vecjoints) {
4193  (*it)->serialize(o,options);
4194  }
4195  o << _vPassiveJoints.size() << " ";
4196  FOREACHC(it,_vPassiveJoints) {
4197  (*it)->serialize(o,options);
4198  }
4199 }
4200 
4202 {
4203  std::vector<Vector> vaxes;
4204  FOREACH(itjoint,_vecjoints) {
4205  vaxes.resize((*itjoint)->GetDOF());
4206  for(size_t i = 0; i < vaxes.size(); ++i) {
4207  vaxes[i] = (*itjoint)->GetInternalHierarchyLeftTransform().rotate((*itjoint)->GetInternalHierarchyAxis(i));
4208  }
4209  (*itjoint)->_ComputeInternalInformation((*itjoint)->GetFirstAttached(), (*itjoint)->GetSecondAttached(),(*itjoint)->GetInternalHierarchyLeftTransform().trans,vaxes,std::vector<dReal>());
4210  }
4211 }
4212 
4213 const std::string& KinBody::GetKinematicsGeometryHash() const
4214 {
4216  if( __hashkinematics.size() == 0 ) {
4217  ostringstream ss;
4218  ss << std::fixed << std::setprecision(SERIALIZATION_PRECISION);
4220  __hashkinematics = utils::GetMD5HashString(ss.str());
4221  }
4222  return __hashkinematics;
4223 }
4224 
4225 void KinBody::SetConfigurationValues(std::vector<dReal>::const_iterator itvalues, uint32_t checklimits)
4226 {
4227  vector<dReal> vdofvalues(GetDOF());
4228  if( GetDOF() > 0 ) {
4229  std::copy(itvalues,itvalues+GetDOF(),vdofvalues.begin());
4230  }
4231  Transform t;
4233  SetDOFValues(vdofvalues,t,checklimits);
4234 }
4235 
4236 void KinBody::GetConfigurationValues(std::vector<dReal>&v) const
4237 {
4238  GetDOFValues(v);
4239  v.resize(GetDOF()+RaveGetAffineDOF(DOF_Transform));
4241 }
4242 
4244 {
4246  if( interpolation.size() == 0 ) {
4247  return _spec;
4248  }
4250  FOREACH(itgroup,spec._vgroups) {
4251  itgroup->interpolation=interpolation;
4252  }
4253  return spec;
4254 }
4255 
4256 ConfigurationSpecification KinBody::GetConfigurationSpecificationIndices(const std::vector<int>&indices, const std::string& interpolation) const
4257 {
4260  if( indices.size() > 0 ) {
4261  spec._vgroups.resize(1);
4262  stringstream ss;
4263  ss << "joint_values " << GetName();
4264  FOREACHC(it,indices) {
4265  ss << " " << *it;
4266  }
4267  spec._vgroups[0].name = ss.str();
4268  spec._vgroups[0].dof = indices.size();
4269  spec._vgroups[0].offset = 0;
4270  spec._vgroups[0].interpolation=interpolation;
4271  }
4272  return spec;
4273 }
4274 
4275 UserDataPtr KinBody::RegisterChangeCallback(int properties, const boost::function<void()>&callback) const
4276 {
4277  ChangeCallbackDataPtr pdata(new ChangeCallbackData(properties,callback,shared_kinbody_const()));
4278  boost::unique_lock< boost::shared_mutex > lock(GetInterfaceMutex());
4279  pdata->_iterator = _listRegisteredCallbacks.insert(_listRegisteredCallbacks.end(),pdata);
4280  return pdata;
4281 }
4282 
4283 } // end namespace OpenRAVE