openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
kinbodyjoint.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 #include <boost/algorithm/string.hpp> // boost::trim
20 #include <boost/lexical_cast.hpp>
21 
22 #include "fparsermulti.h"
23 
24 namespace OpenRAVE {
25 
26 KinBody::JointInfo::JointInfo() : XMLReadable("joint"), _type(JointNone), _bIsActive(true) {
27  for(size_t i = 0; i < _vaxes.size(); ++i) {
28  _vaxes[i] = Vector(0,0,1);
29  }
30  std::fill(_vresolution.begin(), _vresolution.end(), 0.02);
31  std::fill(_vmaxvel.begin(), _vmaxvel.end(), 10);
32  std::fill(_vhardmaxvel.begin(), _vhardmaxvel.end(), 10);
33  std::fill(_vmaxaccel.begin(), _vmaxaccel.end(), 50);
34  std::fill(_vmaxtorque.begin(), _vmaxtorque.end(), 1e5);
35  std::fill(_vweights.begin(), _vweights.end(), 1);
36  std::fill(_voffsets.begin(), _voffsets.end(), 0);
37  std::fill(_vlowerlimit.begin(), _vlowerlimit.end(), 0);
38  std::fill(_vupperlimit.begin(), _vupperlimit.end(), 0);
39  std::fill(_bIsCircular.begin(), _bIsCircular.end(), 0);
40 }
41 
42 static void fparser_polyroots2(vector<dReal>& rawroots, const vector<dReal>& rawcoeffs)
43 {
44  BOOST_ASSERT(rawcoeffs.size()==3);
45  int numroots=0;
46  rawroots.resize(2);
47  polyroots2<dReal>(&rawcoeffs[0],&rawroots[0],numroots);
48  rawroots.resize(numroots);
49 }
50 
51 template <int D>
52 static void fparser_polyroots(vector<dReal>& rawroots, const vector<dReal>& rawcoeffs)
53 {
54  BOOST_ASSERT(rawcoeffs.size()==D+1);
55  int numroots=0;
56  rawroots.resize(D);
57  polyroots<dReal,D>(&rawcoeffs[0],&rawroots[0],numroots);
58  rawroots.resize(numroots);
59 }
60 
61 // take triangle 3 sides and compute the angle opposite the first side
62 static void fparser_sssa(std::vector<dReal>& res, const vector<dReal>& coeffs)
63 {
64  dReal a = coeffs.at(0), b = coeffs.at(1), c = coeffs.at(2);
65  dReal f = (a*a+b*b-c*c)/(2*b);
66  res.resize(1);
67  res[0] = RaveAtan2(RaveSqrt(a*a-f*f),b-f);
68 }
69 
71 static void fparser_sasa(std::vector<dReal>& res, const vector<dReal>& coeffs)
72 {
73  dReal a = coeffs[0], gamma = coeffs[1], b = coeffs[2];
74  res.resize(1);
75  res[0] = RaveAtan2(a*RaveSin(gamma),b-a*RaveCos(gamma));
76 }
77 
79 static void fparser_sass(std::vector<dReal>& res, const vector<dReal>& coeffs)
80 {
81  dReal a = coeffs[0], gamma = coeffs[1], b = coeffs[2];
82  res.resize(1);
83  res[0] = RaveSqrt(a*a+b*b-2*a*b*RaveCos(gamma));
84 }
85 
87 {
88  OpenRAVEFunctionParserRealPtr parser(new OpenRAVEFunctionParserReal());
89 #ifdef OPENRAVE_FPARSER_SETEPSILON
90  parser->setEpsilon(g_fEpsilonLinear);
91 #endif
92  // register commonly used functions
93  parser->AddBoostFunction("polyroots2",fparser_polyroots2,3);
94  parser->AddBoostFunction("polyroots3",fparser_polyroots<3>,4);
95  parser->AddBoostFunction("polyroots4",fparser_polyroots<4>,5);
96  parser->AddBoostFunction("polyroots5",fparser_polyroots<5>,6);
97  parser->AddBoostFunction("polyroots6",fparser_polyroots<6>,7);
98  parser->AddBoostFunction("polyroots7",fparser_polyroots<7>,8);
99  parser->AddBoostFunction("polyroots8",fparser_polyroots<8>,9);
100  parser->AddBoostFunction("SSSA",fparser_sssa,3);
101  parser->AddBoostFunction("SASA",fparser_sasa,3);
102  parser->AddBoostFunction("SASS",fparser_sass,3);
103  return parser;
104 }
105 
107 {
108  _parent = parent;
109  FOREACH(it,_dofbranches) {
110  *it = 0;
111  }
112  for(size_t i = 0; i < _vaxes.size(); ++i) {
113  _vaxes[i] = Vector(0,0,1);
114  }
115  jointindex=-1;
116  dofindex = -1; // invalid index
117  _bInitialized = false;
118  _info._type = type;
119 }
120 
122 {
123 }
124 
126 {
127  if( _info._type & KinBody::JointSpecialBit ) {
128  switch(_info._type) {
130  case KinBody::JointUniversal: return 2;
131  case KinBody::JointSpherical: return 3;
132  case KinBody::JointTrajectory: return 1;
133  default:
134  throw OPENRAVE_EXCEPTION_FORMAT("invalid joint type 0x%x", _info._type, ORE_Failed);
135  }
136  }
137  return int(_info._type & 0xf);
138 }
139 
140 bool KinBody::Joint::IsCircular(int iaxis) const
141 {
142  return static_cast<bool>(_info._bIsCircular.at(iaxis));
143 }
144 
145 bool KinBody::Joint::IsRevolute(int iaxis) const
146 {
147  if( _info._type & KinBody::JointSpecialBit ) {
148  return _info._type == KinBody::JointHinge2 || _info._type == KinBody::JointUniversal;
149  }
150  return !(_info._type&(1<<(4+iaxis)));
151 }
152 
153 bool KinBody::Joint::IsPrismatic(int iaxis) const
154 {
155  if( _info._type & KinBody::JointSpecialBit ) {
156  return false;
157  }
158  return !!(_info._type&(1<<(4+iaxis)));
159 }
160 
162 {
163  if( IsMimic() ) {
164  bool bstatic = true;
165  KinBodyConstPtr parent(_parent);
166  for(int i = 0; i < GetDOF(); ++i) {
167  if( !!_vmimic.at(i) ) {
168  FOREACHC(it, _vmimic.at(i)->_vmimicdofs) {
169  if( !parent->GetJointFromDOFIndex(it->dofindex)->IsStatic() ) {
170  bstatic = false;
171  break;
172  }
173  }
174  if( !bstatic ) {
175  break;
176  }
177  }
178  }
179  if( bstatic ) {
180  return true;
181  }
182  }
183  for(int i = 0; i < GetDOF(); ++i) {
184  if( IsCircular(i) ) {
185  return false;
186  }
187  if( _info._vlowerlimit.at(i) < _info._vupperlimit.at(i) ) {
188  return false;
189  }
190  }
191  return true;
192 }
193 
194 void KinBody::Joint::GetValues(vector<dReal>& pValues, bool bAppend) const
195 {
196  OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
197  if( !bAppend ) {
198  pValues.resize(0);
199  }
200  if( GetDOF() == 1 ) {
201  pValues.push_back(GetValue(0));
202  return;
203  }
204  dReal f;
205  Transform tjoint = _tinvLeft * _attachedbodies[0]->GetTransform().inverse() * _attachedbodies[1]->GetTransform() * _tinvRight;
206  if( _info._type & KinBody::JointSpecialBit ) {
207  switch(_info._type) {
208  case KinBody::JointHinge2: {
209  Vector axis1cur = tjoint.rotate(_vaxes[0]), axis2cur = tjoint.rotate(_vaxes[1]);
210  Vector vec1, vec2, vec3;
211  vec1 = (_vaxes[1] - _vaxes[0].dot3(_vaxes[1])*_vaxes[0]).normalize();
212  vec2 = (axis2cur - _vaxes[0].dot3(axis2cur)*_vaxes[0]).normalize();
213  vec3 = _vaxes[0].cross(vec1);
214  f = 2.0*RaveAtan2(vec3.dot3(vec2), vec1.dot3(vec2));
215  if( f < -PI ) {
216  f += 2*PI;
217  }
218  else if( f > PI ) {
219  f -= 2*PI;
220  }
221  pValues.push_back(_info._voffsets[0]+f+(_dofbranches[0]*2*PI));
222  vec1 = (_vaxes[0] - axis2cur.dot(_vaxes[0])*axis2cur).normalize();
223  vec2 = (axis1cur - axis2cur.dot(axis1cur)*axis2cur).normalize();
224  vec3 = axis2cur.cross(vec1);
225  f = 2.0*RaveAtan2(vec3.dot(vec2), vec1.dot(vec2));
226  if( f < -PI ) {
227  f += 2*PI;
228  }
229  else if( f > PI ) {
230  f -= 2*PI;
231  }
232  pValues.push_back(_info._voffsets[1]+f+(_dofbranches[1]*2*PI));
233  break;
234  }
236  dReal fsinang2 = tjoint.rot.y*tjoint.rot.y+tjoint.rot.z*tjoint.rot.z+tjoint.rot.w*tjoint.rot.w;
237  if( fsinang2 > 1e-10f ) {
238  dReal fsinang = RaveSqrt(fsinang2);
239  dReal fmult = 2*RaveAtan2(fsinang,tjoint.rot.x)/fsinang;
240  pValues.push_back(tjoint.rot.y*fmult);
241  pValues.push_back(tjoint.rot.z*fmult);
242  pValues.push_back(tjoint.rot.w*fmult);
243  }
244  else {
245  pValues.push_back(0);
246  pValues.push_back(0);
247  pValues.push_back(0);
248  }
249  break;
250  }
251  default:
252  throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x", _info._type, ORE_Failed);
253  }
254  }
255  else {
256  // chain of revolute and prismatic joints
257  for(int i = 0; i < GetDOF(); ++i) {
258  Vector vaxis = _vaxes.at(i);
259  if( IsRevolute(i) ) {
260  if( i+1 < GetDOF() ) {
261  std::pair<dReal, Vector > res = normalizeAxisRotation(vaxis,tjoint.rot);
262  tjoint.rot = res.second;
263  if( res.first != 0 ) {
264  // could speed up by checking if trans is ever needed after this
265  tjoint.trans = quatRotate(quatFromAxisAngle(vaxis,res.first),tjoint.trans);
266  }
267  f = -res.first;
268  }
269  else {
270  f = 2.0f*RaveAtan2(tjoint.rot.y*vaxis.x+tjoint.rot.z*vaxis.y+tjoint.rot.w*vaxis.z, tjoint.rot.x);
271  }
272  // expect values to be within -PI to PI range
273  if( f < -PI ) {
274  f += 2*PI;
275  }
276  else if( f > PI ) {
277  f -= 2*PI;
278  }
279  pValues.push_back(_info._voffsets[i]+f+(_dofbranches[i]*2*PI));
280  }
281  else { // prismatic
282  f = tjoint.trans.x*vaxis.x+tjoint.trans.y*vaxis.y+tjoint.trans.z*vaxis.z;
283  pValues.push_back(_info._voffsets[i]+f);
284  if( i+1 < GetDOF() ) {
285  tjoint.trans -= vaxis*f;
286  }
287  }
288  }
289  }
290 }
291 
293 {
294  OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
295  dReal f;
296  Transform tjoint = _tinvLeft * _attachedbodies[0]->GetTransform().inverse() * _attachedbodies[1]->GetTransform() * _tinvRight;
297  if( _info._type & KinBody::JointSpecialBit ) {
298  switch(_info._type) {
299  case KinBody::JointHinge2: {
300  Vector axis1cur = tjoint.rotate(_vaxes[0]), axis2cur = tjoint.rotate(_vaxes[1]);
301  Vector vec1, vec2, vec3;
302  if( iaxis == 0 ) {
303  vec1 = (_vaxes[1] - _vaxes[0].dot3(_vaxes[1])*_vaxes[0]).normalize();
304  vec2 = (axis2cur - _vaxes[0].dot3(axis2cur)*_vaxes[0]).normalize();
305  vec3 = _vaxes[0].cross(vec1);
306  f = 2.0*RaveAtan2(vec3.dot3(vec2), vec1.dot3(vec2));
307  if( f < -PI ) {
308  f += 2*PI;
309  }
310  else if( f > PI ) {
311  f -= 2*PI;
312  }
313  return _info._voffsets[0]+f+(_dofbranches[0]*2*PI);
314  }
315  else if( iaxis == 1 ) {
316  vec1 = (_vaxes[0] - axis2cur.dot(_vaxes[0])*axis2cur).normalize();
317  vec2 = (axis1cur - axis2cur.dot(axis1cur)*axis2cur).normalize();
318  vec3 = axis2cur.cross(vec1);
319  f = 2.0*RaveAtan2(vec3.dot(vec2), vec1.dot(vec2));
320  if( f < -PI ) {
321  f += 2*PI;
322  }
323  else if( f > PI ) {
324  f -= 2*PI;
325  }
326  return _info._voffsets[1]+f+(_dofbranches[1]*2*PI);
327  }
328  break;
329  }
331  dReal fsinang2 = tjoint.rot.y*tjoint.rot.y+tjoint.rot.z*tjoint.rot.z+tjoint.rot.w*tjoint.rot.w;
332  if( fsinang2 > 1e-10f ) {
333  dReal fsinang = RaveSqrt(fsinang2);
334  dReal fmult = 2*RaveAtan2(fsinang,tjoint.rot.x)/fsinang;
335  if( iaxis == 0 ) {
336  return tjoint.rot.y*fmult;
337  }
338  else if( iaxis == 1 ) {
339  return tjoint.rot.z*fmult;
340  }
341  else if( iaxis == 2 ) {
342  return tjoint.rot.w*fmult;
343  }
344  }
345  else {
346  if((iaxis >= 0)&&(iaxis < 3)) {
347  return 0;
348  }
349  }
350  break;
351  }
353  //uint64_t starttime = utils::GetMicroTime();
354  vector<dReal> vsampledata;
355  dReal splitpercentage = 0.01;
356  dReal precision(1e-6);
357  dReal timemin = 0, timemax = _info._trajfollow->GetDuration();
358  Transform tbest, ttest;
359  int totalcalls = 0;
360  while(timemin+precision < timemax) {
361  dReal timestep = (timemax-timemin)*splitpercentage;
362  dReal timeclosest = timemin;
363  dReal bestdist = 1e30, besttime=0;
364  for(; timeclosest < timemax; timeclosest += timestep ) {
365  if( timeclosest > timemax ) {
366  timeclosest = timemax;
367  }
368  totalcalls += 1;
369  _info._trajfollow->Sample(vsampledata,timeclosest);
370  if( _info._trajfollow->GetConfigurationSpecification().ExtractTransform(ttest,vsampledata.begin(),KinBodyConstPtr()) ) {
371  dReal fdist = TransformDistanceFast(ttest,tjoint,0.3);
372  if( bestdist > fdist ) {
373  besttime = timeclosest;
374  bestdist = fdist;
375  tbest = ttest;
376  }
377  }
378  }
379  OPENRAVE_ASSERT_OP_FORMAT(bestdist, <, 1e30, "failed to compute trajectory value for joint %s\n",GetName(),ORE_Assert);
380  timemin = max(timemin,besttime-timestep);
381  timemax = min(timemax, besttime+timestep);
382  splitpercentage = 0.1f;
383  //RAVELOG_INFO(str(boost::format("calls: %d time: %f")%totalcalls%((utils::GetMicroTime()-starttime)*1e-6)));
384  }
385  return 0.5*(timemin+timemax);
386  }
387  default:
388  break;
389  }
390  }
391  else {
392  if( _info._type == KinBody::JointPrismatic ) {
393  return _info._voffsets[0]+(tjoint.trans.x*_vaxes[0].x+tjoint.trans.y*_vaxes[0].y+tjoint.trans.z*_vaxes[0].z);
394  }
395  else if( _info._type == KinBody::JointRevolute ) {
396  f = 2.0f*RaveAtan2(tjoint.rot.y*_vaxes[0].x+tjoint.rot.z*_vaxes[0].y+tjoint.rot.w*_vaxes[0].z, tjoint.rot.x);
397  // expect values to be within -PI to PI range
398  if( f < -PI ) {
399  f += 2*PI;
400  }
401  else if( f > PI ) {
402  f -= 2*PI;
403  }
404  return _info._voffsets[0]+f+(_dofbranches[0]*2*PI);
405  }
406 
407  // chain of revolute and prismatic joints
408  for(int i = 0; i < GetDOF(); ++i) {
409  Vector vaxis = _vaxes.at(i);
410  if( IsRevolute(i) ) {
411  if( i+1 < GetDOF() ) {
412  std::pair<dReal, Vector > res = normalizeAxisRotation(vaxis,tjoint.rot);
413  tjoint.rot = res.second;
414  if( res.first != 0 ) {
415  // could speed up by checking if trans is ever needed after this
416  tjoint.trans = quatRotate(quatFromAxisAngle(vaxis,res.first),tjoint.trans);
417  }
418  f = -res.first;
419  }
420  else {
421  f = 2.0f*RaveAtan2(tjoint.rot.y*vaxis.x+tjoint.rot.z*vaxis.y+tjoint.rot.w*vaxis.z, tjoint.rot.x);
422  }
423  // expect values to be within -PI to PI range
424  if( f < -PI ) {
425  f += 2*PI;
426  }
427  else if( f > PI ) {
428  f -= 2*PI;
429  }
430  if( i == iaxis ) {
431  return _info._voffsets[i]+f+(_dofbranches[i]*2*PI);
432  }
433  }
434  else { // prismatic
435  f = tjoint.trans.x*vaxis.x+tjoint.trans.y*vaxis.y+tjoint.trans.z*vaxis.z;
436  if( i == iaxis ) {
437  return _info._voffsets[i]+f;
438  }
439  if( i+1 < GetDOF() ) {
440  tjoint.trans -= vaxis*f;
441  }
442  }
443  }
444  }
445  throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x axis %d\n", _info._type%iaxis, ORE_Failed);
446 }
447 
448 void KinBody::Joint::GetVelocities(std::vector<dReal>& pVelocities, bool bAppend) const
449 {
450  OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
451  if( !bAppend ) {
452  pVelocities.resize(0);
453  }
454  if( GetDOF() == 1 ) {
455  pVelocities.push_back(GetVelocity(0));
456  return;
457  }
458  _GetVelocities(pVelocities,bAppend,_attachedbodies[0]->GetVelocity(), _attachedbodies[1]->GetVelocity());
459 };
460 
462 {
463  OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
464  return _GetVelocity(axis,_attachedbodies[0]->GetVelocity(), _attachedbodies[1]->GetVelocity());
465 }
466 
467 void KinBody::Joint::_GetVelocities(std::vector<dReal>& pVelocities, bool bAppend, const std::pair<Vector,Vector>& linkparentvelocity, const std::pair<Vector,Vector>& linkchildvelocity) const
468 {
469  if( !bAppend ) {
470  pVelocities.resize(0);
471  }
472  if( GetDOF() == 1 ) {
473  pVelocities.push_back(GetVelocity(0));
474  return;
475  }
476  const Transform& linkparenttransform = _attachedbodies[0]->_info._t;
477  const Transform& linkchildtransform = _attachedbodies[1]->_info._t;
478  Vector quatdelta = quatMultiply(linkparenttransform.rot,_tLeft.rot);
479  Vector quatdeltainv = quatInverse(quatdelta);
480  if( _info._type & KinBody::JointSpecialBit ) {
481  switch(_info._type) {
483  Vector v = quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second);
484  pVelocities.push_back(v.x);
485  pVelocities.push_back(v.y);
486  pVelocities.push_back(v.z);
487  break;
488  }
489  default:
490  throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x", _info._type, ORE_InvalidArguments);
491  }
492  }
493  else {
494  // chain of revolute and prismatic joints
495  Vector angvelocitycovered, linvelocitycovered;
496  for(int i = 0; i < GetDOF(); ++i) {
497  if( IsRevolute(i) ) {
498  pVelocities.push_back(_vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second-angvelocitycovered)));
499  angvelocitycovered += quatRotate(quatdelta,_vaxes[i]*pVelocities.back());
500  }
501  else { // prismatic
502  pVelocities.push_back(_vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-(linkparentvelocity.second-angvelocitycovered).cross(linkchildtransform.trans-linkparenttransform.trans)-linvelocitycovered)));
503  linvelocitycovered += quatRotate(quatdelta,_vaxes[i]*pVelocities.back());
504  }
505  }
506  }
507 }
508 
509 dReal KinBody::Joint::_GetVelocity(int axis, const std::pair<Vector,Vector>&linkparentvelocity, const std::pair<Vector,Vector>&linkchildvelocity) const
510 {
511  const Transform& linkparenttransform = _attachedbodies[0]->_info._t;
512  const Transform& linkchildtransform = _attachedbodies[1]->_info._t;
513  Vector quatdelta = quatMultiply(linkparenttransform.rot,_tLeft.rot);
514  Vector quatdeltainv = quatInverse(quatdelta);
515  if( _info._type & KinBody::JointSpecialBit ) {
516  switch(_info._type) {
518  Vector v = quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second);
519  return v[axis];
520  }
521  default:
522  throw OPENRAVE_EXCEPTION_FORMAT("unknown joint type 0x%x", _info._type, ORE_InvalidArguments);
523  }
524  }
525  else {
526  if( _info._type == KinBody::JointPrismatic ) {
527  return _vaxes[0].dot3(quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-linkparentvelocity.second.cross(linkchildtransform.trans-linkparenttransform.trans)));
528  }
529  else if( _info._type == KinBody::JointRevolute ) {
530  return _vaxes[0].dot3(quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second));
531  }
532  else {
533  // chain of revolute and prismatic joints
534  Vector angvelocitycovered, linvelocitycovered;
535  for(int i = 0; i < GetDOF(); ++i) {
536  if( IsRevolute(i) ) {
537  dReal fvelocity = _vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second-angvelocitycovered));
538  if( i == axis ) {
539  return fvelocity;
540  }
541  angvelocitycovered += quatRotate(quatdelta,_vaxes[i]*fvelocity);
542  }
543  else { // prismatic
544  dReal fvelocity = _vaxes[i].dot3(quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-(linkparentvelocity.second-angvelocitycovered).cross(linkparenttransform.trans-linkchildtransform.trans)-linvelocitycovered));
545  if( i == axis ) {
546  return fvelocity;
547  }
548  linvelocitycovered += quatRotate(quatdelta,_vaxes[i]*fvelocity);
549  }
550  }
551  }
552  }
553  throw OPENRAVE_EXCEPTION_FORMAT("unsupported joint type 0x%x", _info._type, ORE_InvalidArguments);
554 }
555 
557 {
558  return _attachedbodies[0]->GetTransform() * _tLeft.trans;
559 }
560 
562 {
563  return _attachedbodies[0]->GetTransform().rotate(_tLeft.rotate(_vaxes.at(iaxis)));
564 }
565 
566 void KinBody::Joint::_ComputeInternalInformation(LinkPtr plink0, LinkPtr plink1, const Vector& vanchorraw, const std::vector<Vector>& vaxes, const std::vector<dReal>& vcurrentvalues)
567 {
568  OPENRAVE_ASSERT_OP_FORMAT(!!plink0,&&,!!plink1, "one or more attached _attachedbodies are invalid for joint %s", GetName(),ORE_InvalidArguments);
569  for(int i = 0; i < GetDOF(); ++i) {
570  OPENRAVE_ASSERT_OP_FORMAT(_info._vmaxvel[i], >=, 0, "joint %s[%d] max velocity is invalid",_info._name%i, ORE_InvalidArguments);
571  OPENRAVE_ASSERT_OP_FORMAT(_info._vmaxaccel[i], >=, 0, "joint %s[%d] max acceleration is invalid",_info._name%i, ORE_InvalidArguments);
572  OPENRAVE_ASSERT_OP_FORMAT(_info._vmaxtorque[i], >=, 0, "joint %s[%d] max acceleration is invalid",_info._name%i, ORE_InvalidArguments);
573  }
574 
575  KinBodyPtr parent(_parent);
576  _bInitialized = false;
577  _attachedbodies[0] = plink0;
578  _attachedbodies[1] = plink1;
579  Transform trel, tbody0, tbody1;
580  Vector vanchor=vanchorraw;
581  for(size_t i = 0; i < vaxes.size(); ++i) {
582  _vaxes[i] = vaxes[i];
583  }
584  // make sure first body is always closer to the root, unless the second body is static and the first body is not the root link
585  if( _attachedbodies[1]->IsStatic() && _attachedbodies[0]->GetIndex() > 0) {
586  if( !_attachedbodies[0]->IsStatic() ) {
587  Transform tswap = plink1->GetTransform().inverse() * plink0->GetTransform();
588  for(int i = 0; i < GetDOF(); ++i) {
589  _vaxes[i] = -tswap.rotate(_vaxes[i]);
590  }
591  vanchor = tswap*vanchor;
592  swap(_attachedbodies[0],_attachedbodies[1]);
593  }
594  }
595 
596  // update _info
597  for(size_t i = 0; i < vaxes.size(); ++i) {
598  _info._vaxes[i] = _vaxes[i];
599  }
600  _info._vanchor = vanchor;
601 
602  tbody0 = _attachedbodies[0]->GetTransform();
603  tbody1 = _attachedbodies[1]->GetTransform();
604  trel = tbody0.inverse() * tbody1;
605  _tLeft = Transform();
606  _tLeftNoOffset = Transform();
607  _tRight = Transform();
608  _tRightNoOffset = Transform();
609 
610  if( _info._type & KinBody::JointSpecialBit ) {
611  switch(_info._type) {
613  _tLeft.trans = vanchor;
614  _tRight.trans = -vanchor;
615  _tRight = _tRight * trel;
616  OPENRAVE_ASSERT_OP((int)vaxes.size(),==,2);
617  break;
619  _tLeft.trans = vanchor;
620  _tRight.trans = -vanchor;
621  _tRight = _tRight * trel;
622  OPENRAVE_ASSERT_OP((int)vaxes.size(),==,2);
623  break;
625  _tLeft.trans = vanchor;
626  _tRight.trans = -vanchor;
627  _tRight = _tRight * trel;
628  break;
630  if( !_info._trajfollow ) {
631  throw OPENRAVE_EXCEPTION_FORMAT0("trajectory joint requires Joint::_trajfollow to be initialized",ORE_InvalidState);
632  }
633  _tRight = _tRight * trel;
634  break;
635  default:
636  throw OPENRAVE_EXCEPTION_FORMAT("unrecognized joint type 0x%x", _info._type, ORE_InvalidArguments);
637  }
638  _tLeftNoOffset = _tLeft;
639  _tRightNoOffset = _tRight;
640  }
641  else {
642  OPENRAVE_ASSERT_OP((int)vaxes.size(),==,GetDOF());
643  _tLeftNoOffset.trans = vanchor;
644  _tRightNoOffset.trans = -vanchor;
645  _tRightNoOffset = _tRightNoOffset * trel;
646  if( GetDOF() == 1 ) {
647  // in the case of one axis, create a new coordinate system such that the axis rotates about (0,0,1)
648  // this is necessary in order to simplify the rotation matrices (for future symbolic computation)
649  // and suppress any floating-point error. The data structures are only setup for this to work in 1 DOF.
650  Transform trot; trot.rot = quatRotateDirection(_vaxes[0],Vector(0,0,1));
651  _tLeftNoOffset = _tLeftNoOffset * trot.inverse();
652  _tRightNoOffset = trot*_tRightNoOffset;
653  _vaxes[0] = Vector(0,0,1);
654  }
655 
656  Transform toffset;
657  if( IsRevolute(0) ) {
658  toffset.rot = quatFromAxisAngle(_vaxes[0], _info._voffsets[0]);
659  }
660  else {
661  toffset.trans = _vaxes[0]*_info._voffsets[0];
662  }
663  _tLeft = _tLeftNoOffset * toffset;
664  _tRight = _tRightNoOffset;
665  if( GetDOF() > 1 ) {
666  // right multiply by the offset of the last axis, might be buggy?
667  if( IsRevolute(GetDOF()-1) ) {
668  _tRight = matrixFromAxisAngle(_vaxes[GetDOF()-1], _info._voffsets[GetDOF()-1]) * _tRight;
669  }
670  else {
671  _tRight.trans += _vaxes[GetDOF()-1]*_info._voffsets[GetDOF()-1];
672  }
673  }
674  }
675 
676  if( vcurrentvalues.size() > 0 ) {
677  // see if any joints have offsets
678  Transform toffset;
679  if( _info._type == KinBody::JointTrajectory ) {
680  vector<dReal> vsampledata;
681  Transform t0, t1;
682  _info._trajfollow->Sample(vsampledata,0);
683  if( !_info._trajfollow->GetConfigurationSpecification().ExtractTransform(t0,vsampledata.begin(),KinBodyConstPtr()) ) {
684  throw OPENRAVE_EXCEPTION_FORMAT("failed to sample trajectory for joint %s",GetName(),ORE_Assert);
685  }
686  _info._trajfollow->Sample(vsampledata,vcurrentvalues.at(0));
687  if( !_info._trajfollow->GetConfigurationSpecification().ExtractTransform(t1,vsampledata.begin(),KinBodyConstPtr()) ) {
688  throw OPENRAVE_EXCEPTION_FORMAT("failed to sample trajectory for joint %s",GetName(),ORE_Assert);
689  }
690  toffset = t0*t1.inverse();
691  }
692  else if( !(_info._type&KinBody::JointSpecialBit) || _info._type == KinBody::JointUniversal || _info._type == KinBody::JointHinge2 ) {
693  if( IsRevolute(0) ) {
694  toffset.rot = quatFromAxisAngle(_vaxes[0], -vcurrentvalues[0]);
695  }
696  else {
697  toffset.trans = -_vaxes[0]*vcurrentvalues[0];
698  }
699  }
700  _tLeftNoOffset *= toffset;
701  _tLeft *= toffset;
702  if( vcurrentvalues.size() > 1 ) {
703  if( IsRevolute(GetDOF()-1) ) {
704  toffset.rot = quatFromAxisAngle(_vaxes[GetDOF()-1], -vcurrentvalues.at(GetDOF()-1));
705  }
706  else {
707  toffset.trans = -_vaxes[GetDOF()-1]*vcurrentvalues.at(GetDOF()-1);
708  }
709  _tRightNoOffset = toffset * _tRightNoOffset;
710  _tRight = toffset * _tRight;
711  }
712  }
713  _tinvRight = _tRight.inverse();
714  _tinvLeft = _tLeft.inverse();
715 
716  _vcircularlowerlimit = _info._vlowerlimit;
717  _vcircularupperlimit = _info._vupperlimit;
718  for(int i = 0; i < GetDOF(); ++i) {
719  if( IsCircular(i) ) {
720  // can rotate forever, so don't limit it. Unfortunately if numbers are too big precision will start getting lost
721  _info._vlowerlimit.at(i) = -1e4;
722  _info._vupperlimit.at(i) = 1e4;
723  }
724  }
725 
726  if( !!_attachedbodies[0] ) {
727  _info._linkname0 = _attachedbodies[0]->GetName();
728  }
729  else {
730  _info._linkname0.clear();
731  }
732  if( !!_attachedbodies[1] ) {
733  _info._linkname1 = _attachedbodies[1]->GetName();
734  }
735  else {
736  _info._linkname1.clear();
737  }
738  _info._vcurrentvalues = vcurrentvalues;
739 
740  _bInitialized = true;
741 
742  if( _attachedbodies[1]->IsStatic() && !IsStatic() ) {
743  RAVELOG_WARN(str(boost::format("joint %s: all attached links are static, but joint is not!\n")%GetName()));
744  }
745 }
746 
748 {
749  return _attachedbodies[0];
750 }
751 
753 {
754  return _attachedbodies[1];
755 }
756 
758 {
759  return _vaxes.at(iaxis);
760 }
761 
763 {
764  OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
765  return _tLeftNoOffset;
766 }
767 
769 {
770  OPENRAVE_ASSERT_FORMAT0(_bInitialized, "joint not initialized",ORE_NotInitialized);
771  return _tRightNoOffset;
772 }
773 
774 void KinBody::Joint::GetLimits(std::vector<dReal>& vLowerLimit, std::vector<dReal>& vUpperLimit, bool bAppend) const
775 {
776  if( !bAppend ) {
777  vLowerLimit.resize(0);
778  vUpperLimit.resize(0);
779  }
780  for(int i = 0; i < GetDOF(); ++i) {
781  vLowerLimit.push_back(_info._vlowerlimit[i]);
782  vUpperLimit.push_back(_info._vupperlimit[i]);
783  }
784 }
785 
786 std::pair<dReal, dReal> KinBody::Joint::GetLimit(int iaxis) const
787 {
788  return make_pair(_info._vlowerlimit.at(iaxis),_info._vupperlimit.at(iaxis));
789 }
790 
791 void KinBody::Joint::SetLimits(const std::vector<dReal>& vLowerLimit, const std::vector<dReal>& vUpperLimit)
792 {
793  bool bChanged = false;
794  for(int i = 0; i < GetDOF(); ++i) {
795  if( _info._vlowerlimit[i] != vLowerLimit.at(i) || _info._vupperlimit[i] != vUpperLimit.at(i) ) {
796  bChanged = true;
797  _info._vlowerlimit[i] = vLowerLimit.at(i);
798  _info._vupperlimit[i] = vUpperLimit.at(i);
799  if( IsRevolute(i) && !IsCircular(i) ) {
800  // TODO, necessary to set wrap?
801  if( _info._vlowerlimit[i] < -PI || _info._vupperlimit[i] > PI) {
802  SetWrapOffset(0.5f * (_info._vlowerlimit.at(i) + _info._vupperlimit.at(i)),i);
803  }
804  else {
805  SetWrapOffset(0,i);
806  }
807  }
808  }
809  }
810  if( bChanged ) {
811  GetParent()->_ParametersChanged(Prop_JointLimits);
812  }
813 }
814 
815 void KinBody::Joint::GetVelocityLimits(std::vector<dReal>& vlower, std::vector<dReal>& vupper, bool bAppend) const
816 {
817  if( !bAppend ) {
818  vlower.resize(0);
819  vupper.resize(0);
820  }
821  for(int i = 0; i < GetDOF(); ++i) {
822  vlower.push_back(-_info._vmaxvel[i]);
823  vupper.push_back(_info._vmaxvel[i]);
824  }
825 }
826 
827 void KinBody::Joint::GetVelocityLimits(std::vector<dReal>& vmax, bool bAppend) const
828 {
829  if( !bAppend ) {
830  vmax.resize(0);
831  }
832  for(int i = 0; i < GetDOF(); ++i) {
833  vmax.push_back(_info._vmaxvel[i]);
834  }
835 }
836 
837 std::pair<dReal, dReal> KinBody::Joint::GetVelocityLimit(int iaxis) const
838 {
839  return make_pair(-_info._vmaxvel.at(iaxis), _info._vmaxvel.at(iaxis));
840 }
841 
842 void KinBody::Joint::SetVelocityLimits(const std::vector<dReal>& vmaxvel)
843 {
844  for(int i = 0; i < GetDOF(); ++i) {
845  _info._vmaxvel[i] = vmaxvel.at(i);
846  }
847  GetParent()->_ParametersChanged(Prop_JointAccelerationVelocityTorqueLimits);
848 }
849 
850 void KinBody::Joint::GetAccelerationLimits(std::vector<dReal>& vmax, bool bAppend) const
851 {
852  if( !bAppend ) {
853  vmax.resize(0);
854  }
855  for(int i = 0; i < GetDOF(); ++i) {
856  vmax.push_back(_info._vmaxaccel[i]);
857  }
858 }
859 
861 {
862  return _info._vmaxaccel.at(iaxis);
863 }
864 
865 void KinBody::Joint::SetAccelerationLimits(const std::vector<dReal>& vmax)
866 {
867  for(int i = 0; i < GetDOF(); ++i) {
868  _info._vmaxaccel[i] = vmax.at(i);
869  }
870  GetParent()->_ParametersChanged(Prop_JointAccelerationVelocityTorqueLimits);
871 }
872 
873 void KinBody::Joint::GetTorqueLimits(std::vector<dReal>& vmax, bool bAppend) const
874 {
875  if( !bAppend ) {
876  vmax.resize(0);
877  }
878  for(int i = 0; i < GetDOF(); ++i) {
879  vmax.push_back(_info._vmaxtorque[i]);
880  }
881 }
882 
883 void KinBody::Joint::SetTorqueLimits(const std::vector<dReal>& vmax)
884 {
885  for(int i = 0; i < GetDOF(); ++i) {
886  _info._vmaxtorque[i] = vmax.at(i);
887  }
888  GetParent()->_ParametersChanged(Prop_JointAccelerationVelocityTorqueLimits);
889 }
890 
891 void KinBody::Joint::SetWrapOffset(dReal newoffset, int iaxis)
892 {
893  if( _info._voffsets.at(iaxis) != newoffset ) {
894  _info._voffsets.at(iaxis) = newoffset;
895  if( iaxis == 0 ) {
896  Transform toffset;
897  if( IsRevolute(0) ) {
898  toffset.rot = quatFromAxisAngle(_vaxes[0], newoffset);
899  }
900  else {
901  toffset.trans = _vaxes[0]*newoffset;
902  }
903  _tLeft = _tLeftNoOffset * toffset;
904  _tinvLeft = _tLeft.inverse();
905  }
906  if(GetDOF() > 1 && iaxis==GetDOF()-1 ) {
907  _tRight = _tRightNoOffset;
908  // right multiply by the offset of the last axis, might be buggy?
909  if( IsRevolute(GetDOF()-1) ) {
910  _tRight = matrixFromAxisAngle(_vaxes[GetDOF()-1], newoffset) * _tRight;
911  }
912  else {
913  _tRight.trans += _vaxes[GetDOF()-1]*newoffset;
914  }
915  _tinvRight = _tRight.inverse();
916  }
917  GetParent()->_ParametersChanged(Prop_JointOffset);
918  }
919 }
920 
921 void KinBody::Joint::GetResolutions(std::vector<dReal>& resolutions, bool bAppend) const
922 {
923  if( !bAppend ) {
924  resolutions.resize(GetDOF());
925  }
926  for(int i = 0; i < GetDOF(); ++i) {
927  resolutions.push_back(_info._vresolution[i]);
928  }
929 }
930 
932 {
933  return _info._vresolution.at(iaxis);
934 }
935 
936 void KinBody::Joint::SetResolution(dReal resolution, int iaxis)
937 {
938  _info._vresolution.at(iaxis) = resolution;
939  GetParent()->_ParametersChanged(Prop_JointProperties);
940 }
941 
942 void KinBody::Joint::GetWeights(std::vector<dReal>& weights, bool bAppend) const
943 {
944  if( !bAppend ) {
945  weights.resize(GetDOF());
946  }
947  for(int i = 0; i < GetDOF(); ++i) {
948  weights.push_back(_info._vweights[i]);
949  }
950 }
951 
953 {
954  return _info._vweights.at(iaxis);
955 }
956 
957 void KinBody::Joint::SetWeights(const std::vector<dReal>& vweights)
958 {
959  for(int i = 0; i < GetDOF(); ++i) {
960  OPENRAVE_ASSERT_OP(vweights.at(i),>,0);
961  _info._vweights[i] = vweights.at(i);
962  }
963  GetParent()->_ParametersChanged(Prop_JointProperties);
964 }
965 
966 void KinBody::Joint::SubtractValues(std::vector<dReal>& q1, const std::vector<dReal>& q2) const
967 {
968  for(int i = 0; i < GetDOF(); ++i) {
969  if( IsCircular(i) ) {
970  q1.at(i) = utils::NormalizeCircularAngle(q1.at(i)-q2.at(i),_vcircularlowerlimit.at(i),_vcircularupperlimit.at(i));
971  }
972  else {
973  q1.at(i) -= q2.at(i);
974  }
975  }
976 }
977 
978 dReal KinBody::Joint::SubtractValue(dReal value1, dReal value2, int iaxis) const
979 {
980  if( IsCircular(iaxis) ) {
981  return utils::NormalizeCircularAngle(value1-value2,_vcircularlowerlimit.at(iaxis),_vcircularupperlimit.at(iaxis));
982  }
983  else {
984  return value1-value2;
985  }
986 }
987 
988 void KinBody::Joint::AddTorque(const std::vector<dReal>& pTorques)
989 {
990  GetParent()->GetEnv()->GetPhysicsEngine()->AddJointTorque(shared_from_this(), pTorques);
991 }
992 
994 {
995  for(int i = 0; i < GetDOF(); ++i) {
996  if( !!_vmimic.at(i) &&(_vmimic.at(i)->_vmimicdofs.size() > 0)) {
997  return GetParent()->GetJointFromDOFIndex(_vmimic.at(i)->_vmimicdofs.front().dofindex)->GetJointIndex();
998  }
999  }
1000  return -1;
1001 }
1002 
1003 const std::vector<dReal> KinBody::Joint::GetMimicCoeffs() const
1004 {
1005  RAVELOG_WARN("deprecated KinBody::Joint::GetMimicCoeffs(): could not deduce coeffs\n");
1006  std::vector<dReal> coeffs(2); coeffs[0] = 1; coeffs[1] = 0;
1007  return coeffs;
1008 }
1009 
1010 bool KinBody::Joint::IsMimic(int iaxis) const
1011 {
1012  if( iaxis >= 0 ) {
1013  return !!_vmimic.at(iaxis);
1014  }
1015  for(int i = 0; i < GetDOF(); ++i) {
1016  if( !!_vmimic.at(i) ) {
1017  return true;
1018  }
1019  }
1020  return false;
1021 }
1022 
1023 std::string KinBody::Joint::GetMimicEquation(int iaxis, int itype, const std::string& format) const
1024 {
1025  if( !_vmimic.at(iaxis) ) {
1026  return "";
1027  }
1028  if((format.size() == 0)||(format == "fparser")) {
1029  return _vmimic.at(iaxis)->_equations.at(itype);
1030  }
1031  else if( format == "mathml" ) {
1032  boost::format mathfmt("<math xmlns=\"http://www.w3.org/1998/Math/MathML\">\n%s</math>\n");
1033  std::vector<std::string> Vars;
1034  std::string sout;
1035  KinBodyConstPtr parent(_parent);
1036  FOREACHC(itdofformat, _vmimic.at(iaxis)->_vdofformat) {
1037  JointConstPtr pjoint = itdofformat->GetJoint(parent);
1038  if( pjoint->GetDOF() > 1 ) {
1039  Vars.push_back(str(boost::format("<csymbol>%s_%d</csymbol>")%pjoint->GetName()%(int)itdofformat->axis));
1040  }
1041  else {
1042  Vars.push_back(str(boost::format("<csymbol>%s</csymbol>")%pjoint->GetName()));
1043  }
1044  }
1045  if( itype == 0 ) {
1046  _vmimic.at(iaxis)->_posfn->toMathML(sout,Vars);
1047  if((sout.size() > 9)&&(sout.substr(0,9) == "<csymbol>")) {
1048  // due to a bug in ROS robot_model, have to return with <apply> (remove this in 2012).
1049  sout = boost::str(boost::format("<apply>\n <plus/><cn type=\"real\">0</cn>\n %s\n </apply>")%sout);
1050  }
1051  sout = str(mathfmt%sout);
1052  }
1053  else if( itype == 1 ) {
1054  std::string stemp;
1055  FOREACHC(itfn, _vmimic.at(iaxis)->_velfns) {
1056  (*itfn)->toMathML(stemp,Vars);
1057  sout += str(mathfmt%stemp);
1058  }
1059  }
1060  else if( itype == 2 ) {
1061  std::string stemp;
1062  FOREACHC(itfn, _vmimic.at(iaxis)->_accelfns) {
1063  (*itfn)->toMathML(stemp,Vars);
1064  sout += str(mathfmt%stemp);
1065  }
1066  }
1067  return sout;
1068  }
1069  throw OPENRAVE_EXCEPTION_FORMAT("unsupported math format %s", format, ORE_InvalidArguments);
1070 }
1071 
1072 void KinBody::Joint::GetMimicDOFIndices(std::vector<int>& vmimicdofs, int iaxis) const
1073 {
1074  OPENRAVE_ASSERT_FORMAT(!!_vmimic.at(iaxis), "joint %s axis %d is not mimic", GetName()%iaxis,ORE_InvalidArguments);
1075  vmimicdofs.resize(0);
1076  FOREACHC(it, _vmimic.at(iaxis)->_vmimicdofs) {
1077  std::vector<int>::iterator itinsert = std::lower_bound(vmimicdofs.begin(),vmimicdofs.end(), it->dofindex);
1078  if((itinsert == vmimicdofs.end())||(*itinsert != it->dofindex)) {
1079  vmimicdofs.insert(itinsert,it->dofindex);
1080  }
1081  }
1082 }
1083 
1084 void KinBody::Joint::SetMimicEquations(int iaxis, const std::string& poseq, const std::string& veleq, const std::string& acceleq)
1085 {
1086  _vmimic.at(iaxis).reset();
1087  if( poseq.size() == 0 ) {
1088  return;
1089  }
1090  KinBodyPtr parent(_parent);
1091  std::vector<std::string> resultVars;
1092  MimicPtr mimic(new Mimic());
1093  mimic->_equations.at(0) = poseq;
1094  mimic->_equations.at(1) = veleq;
1095  mimic->_equations.at(2) = acceleq;
1096 
1097  // copy equations into the info
1098  if( !_info._vmimic.at(iaxis) ) {
1099  _info._vmimic.at(iaxis).reset(new MimicInfo());
1100  }
1101  _info._vmimic.at(iaxis)->_equations = mimic->_equations;
1102 
1104  mimic->_posfn = posfn;
1105  // because openrave joint names can hold symbols like '-' and '.' can affect the equation, so first do a search and replace
1106  std::vector< std::pair<std::string, std::string> > jointnamepairs; jointnamepairs.reserve(parent->GetJoints().size());
1107  FOREACHC(itjoint,parent->GetJoints()) {
1108  if( (*itjoint)->GetName().size() > 0 ) {
1109  jointnamepairs.push_back(make_pair((*itjoint)->GetName(),str(boost::format("joint%d")%(*itjoint)->GetJointIndex())));
1110  }
1111  }
1112  size_t index = parent->GetJoints().size();
1113  FOREACHC(itjoint,parent->GetPassiveJoints()) {
1114  if( (*itjoint)->GetName().size() > 0 ) {
1115  jointnamepairs.push_back(make_pair((*itjoint)->GetName(),str(boost::format("joint%d")%index)));
1116  }
1117  ++index;
1118  }
1119 
1120  std::map<std::string,std::string> mapinvnames;
1121  FOREACH(itpair,jointnamepairs) {
1122  mapinvnames[itpair->second] = itpair->first;
1123  }
1124 
1125  std::string eq;
1126  int ret = posfn->ParseAndDeduceVariables(utils::SearchAndReplace(eq,mimic->_equations[0],jointnamepairs),resultVars);
1127  if( ret >= 0 ) {
1128  throw OPENRAVE_EXCEPTION_FORMAT("failed to set equation '%s' on %s:%s, at %d. Error is %s\n", mimic->_equations[0]%parent->GetName()%GetName()%ret%posfn->ErrorMsg(),ORE_InvalidArguments);
1129  }
1130  // process the variables
1131  FOREACH(itvar,resultVars) {
1132  OPENRAVE_ASSERT_FORMAT(itvar->find("joint") == 0, "equation '%s' uses unknown variable", mimic->_equations[0], ORE_InvalidArguments);
1133  MIMIC::DOFFormat dofformat;
1134  size_t axisindex = itvar->find('_');
1135  if( axisindex != std::string::npos ) {
1136  dofformat.jointindex = boost::lexical_cast<uint16_t>(itvar->substr(5,axisindex-5));
1137  dofformat.axis = boost::lexical_cast<uint8_t>(itvar->substr(axisindex+1));
1138  }
1139  else {
1140  dofformat.jointindex = boost::lexical_cast<uint16_t>(itvar->substr(5));
1141  dofformat.axis = 0;
1142  }
1143  dofformat.dofindex = -1;
1144  JointPtr pjoint = dofformat.GetJoint(parent);
1145  if((pjoint->GetDOFIndex() >= 0)&& !pjoint->IsMimic(dofformat.axis) ) {
1146  dofformat.dofindex = pjoint->GetDOFIndex()+dofformat.axis;
1147  MIMIC::DOFHierarchy h;
1148  h.dofindex = dofformat.dofindex;
1149  h.dofformatindex = mimic->_vdofformat.size();
1150  mimic->_vmimicdofs.push_back(h);
1151  }
1152  mimic->_vdofformat.push_back(dofformat);
1153  }
1154 
1155  stringstream sVars;
1156  if( !resultVars.empty() ) {
1157  sVars << resultVars.at(0);
1158  for(size_t i = 1; i < resultVars.size(); ++i) {
1159  sVars << "," << resultVars[i];
1160  }
1161  }
1162 
1163  for(int itype = 1; itype < 3; ++itype) {
1164  if((itype == 2)&&(mimic->_equations[itype].size() == 0)) {
1165  continue;
1166  }
1167 
1168  std::vector<OpenRAVEFunctionParserRealPtr> vfns(resultVars.size());
1169  // extract the equations
1170  utils::SearchAndReplace(eq,mimic->_equations[itype],jointnamepairs);
1171  size_t index = eq.find('|');
1172  while(index != std::string::npos) {
1173  size_t startindex = index+1;
1174  index = eq.find('|',startindex);
1175  string sequation;
1176  if( index != std::string::npos) {
1177  sequation = eq.substr(startindex,index-startindex);
1178  }
1179  else {
1180  sequation = eq.substr(startindex);
1181  }
1182  boost::trim(sequation);
1183  size_t nameendindex = sequation.find(' ');
1184  string varname;
1185  if( nameendindex == std::string::npos ) {
1186  RAVELOG_WARN(str(boost::format("invalid equation syntax '%s' for joint %s")%sequation%_info._name));
1187  varname = sequation;
1188  sequation = "0";
1189  }
1190  else {
1191  varname = sequation.substr(0,nameendindex);
1192  sequation = sequation.substr(nameendindex);
1193  }
1194  vector<string>::iterator itnameindex = find(resultVars.begin(),resultVars.end(),varname);
1195  OPENRAVE_ASSERT_FORMAT(itnameindex != resultVars.end(), "variable %s from velocity equation is not referenced in the position, skipping...", mapinvnames[varname],ORE_InvalidArguments);
1196 
1198  ret = fn->Parse(sequation,sVars.str());
1199  if( ret >= 0 ) {
1200  throw OPENRAVE_EXCEPTION_FORMAT("failed to set equation '%s' on %s:%s, at %d. Error is %s", sequation%parent->GetName()%GetName()%ret%fn->ErrorMsg(),ORE_InvalidArguments);
1201  }
1202  vfns.at(itnameindex-resultVars.begin()) = fn;
1203  }
1204  // check if anything is missing
1205  for(size_t j = 0; j < resultVars.size(); ++j) {
1206  if( !vfns[j] ) {
1207  // print a message instead of throwing an exception since it might be common for only position equations to be specified
1208  RAVELOG_WARN(str(boost::format("SetMimicEquations: missing variable %s from partial derivatives of joint %s!")%mapinvnames[resultVars[j]]%_info._name));
1209  vfns[j] = CreateJointFunctionParser();
1210  vfns[j]->Parse("0","");
1211  }
1212  }
1213 
1214  if( itype == 1 ) {
1215  mimic->_velfns.swap(vfns);
1216  }
1217  else {
1218  mimic->_accelfns.swap(vfns);
1219  }
1220  }
1221  _vmimic.at(iaxis) = mimic;
1222  parent->_ParametersChanged(Prop_JointMimic);
1223 }
1224 
1225 void KinBody::Joint::_ComputePartialVelocities(std::vector<std::pair<int,dReal> >& vpartials, int iaxis, std::map< std::pair<MIMIC::DOFFormat, int>, dReal >& mapcachedpartials) const
1226 {
1227  vpartials.resize(0);
1228  if( dofindex >= 0 ) {
1229  vpartials.push_back(make_pair(dofindex+iaxis,1.0));
1230  return;
1231  }
1232  OPENRAVE_ASSERT_FORMAT(!!_vmimic.at(iaxis), "cannot compute partial velocities of joint %s", _info._name, ORE_Failed);
1233  KinBodyConstPtr parent(_parent);
1234  MIMIC::DOFFormat thisdofformat;
1235  thisdofformat.dofindex = -1; // always -1 since it is mimiced
1236  thisdofformat.axis = iaxis;
1237  thisdofformat.jointindex = jointindex;
1238  if( jointindex < 0 ) {
1239  // this is a weird computation... have to figure out the passive joint index given where it is in parent->GetPassiveJoints()
1240  thisdofformat.jointindex = parent->GetJoints().size() + (find(parent->GetPassiveJoints().begin(),parent->GetPassiveJoints().end(),shared_from_this()) - parent->GetPassiveJoints().begin());
1241  }
1242  std::vector<std::pair<int,dReal> > vtemppartials;
1243  vector<dReal> vtempvalues;
1244  FOREACHC(itmimicdof, _vmimic[iaxis]->_vmimicdofs) {
1245  std::pair<MIMIC::DOFFormat, int> key = make_pair(thisdofformat,itmimicdof->dofindex);
1246  std::map< std::pair<MIMIC::DOFFormat, int>, dReal >::iterator it = mapcachedpartials.find(key);
1247  if( it == mapcachedpartials.end() ) {
1248  // not in the cache so compute using the chain rule
1249  if( vtempvalues.empty() ) {
1250  FOREACHC(itdofformat, _vmimic[iaxis]->_vdofformat) {
1251  vtempvalues.push_back(itdofformat->GetJoint(parent)->GetValue(itdofformat->axis));
1252  }
1253  }
1254  dReal fvel = _vmimic[iaxis]->_velfns.at(itmimicdof->dofformatindex)->Eval(vtempvalues.empty() ? NULL : &vtempvalues[0]);
1255  const MIMIC::DOFFormat& dofformat = _vmimic[iaxis]->_vdofformat.at(itmimicdof->dofformatindex);
1256  if( dofformat.GetJoint(parent)->IsMimic(dofformat.axis) ) {
1257  dofformat.GetJoint(parent)->_ComputePartialVelocities(vtemppartials,dofformat.axis,mapcachedpartials);
1258  dReal fpartial = 0;
1259  FOREACHC(itpartial,vtemppartials) {
1260  if( itpartial->first == itmimicdof->dofindex ) {
1261  fpartial += itpartial->second;
1262  }
1263  }
1264  fvel *= fpartial;
1265  }
1266  // before pushing back, check for repetition
1267  bool badd = true;
1268  FOREACH(itpartial,vpartials) {
1269  if( itpartial->first == itmimicdof->dofindex ) {
1270  itpartial->second += fvel;
1271  badd = false;
1272  break;
1273  }
1274  }
1275  if( badd ) {
1276  vpartials.push_back(make_pair(itmimicdof->dofindex, fvel));
1277  }
1278  }
1279  else {
1280  bool badd = true;
1281  FOREACH(itpartial,vpartials) {
1282  if( itpartial->first == itmimicdof->dofindex ) {
1283  badd = false;
1284  break;
1285  }
1286  }
1287  if( badd ) {
1288  vpartials.push_back(make_pair(itmimicdof->dofindex, it->second));
1289  }
1290  }
1291  }
1292 }
1293 
1294 int KinBody::Joint::_Eval(int axis, uint32_t timederiv, const std::vector<dReal>& vdependentvalues, std::vector<dReal>& voutput)
1295 {
1296  if( timederiv == 0 ) {
1297  _vmimic.at(axis)->_posfn->EvalMulti(voutput, vdependentvalues.empty() ? NULL : &vdependentvalues[0]);
1298  return _vmimic.at(axis)->_posfn->EvalError();
1299  }
1300  else if( timederiv == 1 ) {
1301  voutput.resize(_vmimic.at(axis)->_velfns.size());
1302  for(size_t i = 0; i < voutput.size(); ++i) {
1303  voutput[i] = _vmimic.at(axis)->_velfns.at(i)->Eval(vdependentvalues.empty() ? NULL : &vdependentvalues[0]);
1304  int err = _vmimic.at(axis)->_velfns.at(i)->EvalError();
1305  if( err ) {
1306  return err;
1307  }
1308  }
1309  }
1310  else if( timederiv == 2 ) {
1311  voutput.resize(_vmimic.at(axis)->_accelfns.size());
1312  for(size_t i = 0; i < voutput.size(); ++i) {
1313  voutput[i] = _vmimic.at(axis)->_accelfns.at(i)->Eval(vdependentvalues.empty() ? NULL : &vdependentvalues[0]);
1314  int err = _vmimic.at(axis)->_accelfns.at(i)->EvalError();
1315  if( err ) {
1316  return err;
1317  }
1318  }
1319  }
1320  else {
1321  throw OPENRAVE_EXCEPTION_FORMAT("timederiv %d not supported",timederiv,ORE_InvalidArguments);
1322  }
1323  return 0;
1324 }
1325 
1326 bool KinBody::Joint::MIMIC::DOFFormat::operator <(const KinBody::Joint::MIMIC::DOFFormat& r) const
1327 {
1328  return jointindex < r.jointindex || (jointindex == r.jointindex && (dofindex < r.dofindex || (dofindex == r.dofindex && axis < r.axis)));
1329 }
1330 
1331 bool KinBody::Joint::MIMIC::DOFFormat::operator ==(const KinBody::Joint::MIMIC::DOFFormat& r) const
1332 {
1333  return jointindex == r.jointindex && dofindex == r.dofindex && axis == r.axis;
1334 }
1335 
1336 KinBody::JointPtr KinBody::Joint::MIMIC::DOFFormat::GetJoint(KinBodyPtr parent) const
1337 {
1338  int numjoints = (int)parent->GetJoints().size();
1339  return jointindex < numjoints ? parent->GetJoints().at(jointindex) : parent->GetPassiveJoints().at(jointindex-numjoints);
1340 }
1341 
1342 KinBody::JointConstPtr KinBody::Joint::MIMIC::DOFFormat::GetJoint(KinBodyConstPtr parent) const
1343 {
1344  int numjoints = (int)parent->GetJoints().size();
1345  return jointindex < numjoints ? parent->GetJoints().at(jointindex) : parent->GetPassiveJoints().at(jointindex-numjoints);
1346 }
1347 
1348 void KinBody::Joint::SetFloatParameters(const std::string& key, const std::vector<dReal>& parameters)
1349 {
1350  if( parameters.size() > 0 ) {
1351  _info._mapFloatParameters[key] = parameters;
1352  }
1353  else {
1354  _info._mapFloatParameters.erase(key);
1355  }
1356  GetParent()->_ParametersChanged(Prop_JointCustomParameters);
1357 }
1358 
1359 void KinBody::Joint::SetIntParameters(const std::string& key, const std::vector<int>& parameters)
1360 {
1361  if( parameters.size() > 0 ) {
1362  _info._mapIntParameters[key] = parameters;
1363  }
1364  else {
1365  _info._mapIntParameters.erase(key);
1366  }
1367  GetParent()->_ParametersChanged(Prop_JointCustomParameters);
1368 }
1369 
1371 {
1372  _info._vcurrentvalues.resize(0);
1373  GetValues(_info._vcurrentvalues);
1374 }
1375 
1376 void KinBody::Joint::serialize(std::ostream& o, int options) const
1377 {
1378  if( options & SO_Kinematics ) {
1379  o << dofindex << " " << jointindex << " " << _info._type << " ";
1380  SerializeRound(o,_tRightNoOffset);
1381  SerializeRound(o,_tLeftNoOffset);
1382  for(int i = 0; i < GetDOF(); ++i) {
1383  SerializeRound3(o,_vaxes[i]);
1384  if( !!_vmimic.at(i) ) {
1385  FOREACHC(iteq,_vmimic.at(i)->_equations) {
1386  o << *iteq << " ";
1387  }
1388  }
1389  }
1390  o << (!_attachedbodies[0] ? -1 : _attachedbodies[0]->GetIndex()) << " " << (_attachedbodies[1]->GetIndex()) << " ";
1391  }
1392  if( options & SO_Dynamics ) {
1393  for(int i = 0; i < GetDOF(); ++i) {
1394  SerializeRound(o,_info._vmaxvel[i]);
1395  SerializeRound(o,_info._vmaxaccel[i]);
1396  SerializeRound(o,_info._vmaxtorque[i]);
1397  SerializeRound(o,_info._vlowerlimit[i]);
1398  SerializeRound(o,_info._vupperlimit[i]);
1399  }
1400  }
1401 }
1402 
1403 }