17 #include "libopenrave.h"
19 #include <boost/algorithm/string.hpp>
20 #include <boost/lexical_cast.hpp>
22 #include "fparsermulti.h"
27 for(
size_t i = 0; i <
_vaxes.size(); ++i) {
44 BOOST_ASSERT(rawcoeffs.size()==3);
47 polyroots2<dReal>(&rawcoeffs[0],&rawroots[0],numroots);
48 rawroots.resize(numroots);
54 BOOST_ASSERT(rawcoeffs.size()==D+1);
57 polyroots<dReal,D>(&rawcoeffs[0],&rawroots[0],numroots);
58 rawroots.resize(numroots);
62 static void fparser_sssa(std::vector<dReal>& res,
const vector<dReal>& coeffs)
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);
71 static void fparser_sasa(std::vector<dReal>& res,
const vector<dReal>& coeffs)
73 dReal a = coeffs[0], gamma = coeffs[1], b = coeffs[2];
79 static void fparser_sass(std::vector<dReal>& res,
const vector<dReal>& coeffs)
81 dReal a = coeffs[0], gamma = coeffs[1], b = coeffs[2];
89 #ifdef OPENRAVE_FPARSER_SETEPSILON
90 parser->setEpsilon(g_fEpsilonLinear);
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);
109 FOREACH(it,_dofbranches) {
112 for(
size_t i = 0; i < _vaxes.size(); ++i) {
113 _vaxes[i] =
Vector(0,0,1);
117 _bInitialized =
false;
128 switch(_info._type) {
137 return int(_info._type & 0xf);
142 return static_cast<bool>(_info._bIsCircular.at(iaxis));
150 return !(_info._type&(1<<(4+iaxis)));
158 return !!(_info._type&(1<<(4+iaxis)));
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() ) {
183 for(
int i = 0; i <
GetDOF(); ++i) {
184 if( IsCircular(i) ) {
187 if( _info._vlowerlimit.at(i) < _info._vupperlimit.at(i) ) {
201 pValues.push_back(GetValue(0));
205 Transform tjoint = _tinvLeft * _attachedbodies[0]->GetTransform().inverse() * _attachedbodies[1]->GetTransform() * _tinvRight;
207 switch(_info._type) {
209 Vector axis1cur = tjoint.rotate(_vaxes[0]), axis2cur = tjoint.rotate(_vaxes[1]);
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));
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));
232 pValues.push_back(_info._voffsets[1]+f+(_dofbranches[1]*2*PI));
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 ) {
240 pValues.push_back(tjoint.rot.y*fmult);
241 pValues.push_back(tjoint.rot.z*fmult);
242 pValues.push_back(tjoint.rot.w*fmult);
245 pValues.push_back(0);
246 pValues.push_back(0);
247 pValues.push_back(0);
257 for(
int i = 0; i <
GetDOF(); ++i) {
258 Vector vaxis = _vaxes.at(i);
259 if( IsRevolute(i) ) {
262 tjoint.rot = res.second;
263 if( res.first != 0 ) {
270 f = 2.0f*
RaveAtan2(tjoint.rot.y*vaxis.x+tjoint.rot.z*vaxis.y+tjoint.rot.w*vaxis.z, tjoint.rot.x);
279 pValues.push_back(_info._voffsets[i]+f+(_dofbranches[i]*2*
PI));
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);
285 tjoint.trans -= vaxis*f;
296 Transform tjoint = _tinvLeft * _attachedbodies[0]->GetTransform().inverse() * _attachedbodies[1]->GetTransform() * _tinvRight;
298 switch(_info._type) {
300 Vector axis1cur = tjoint.rotate(_vaxes[0]), axis2cur = tjoint.rotate(_vaxes[1]);
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));
313 return _info._voffsets[0]+f+(_dofbranches[0]*2*
PI);
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));
326 return _info._voffsets[1]+f+(_dofbranches[1]*2*
PI);
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 ) {
336 return tjoint.rot.y*fmult;
338 else if( iaxis == 1 ) {
339 return tjoint.rot.z*fmult;
341 else if( iaxis == 2 ) {
342 return tjoint.rot.w*fmult;
346 if((iaxis >= 0)&&(iaxis < 3)) {
354 vector<dReal> vsampledata;
355 dReal splitpercentage = 0.01;
356 dReal precision(1e-6);
357 dReal timemin = 0, timemax = _info._trajfollow->GetDuration();
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;
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;
380 timemin = max(timemin,besttime-timestep);
381 timemax = min(timemax, besttime+timestep);
382 splitpercentage = 0.1f;
385 return 0.5*(timemin+timemax);
393 return _info._voffsets[0]+(tjoint.trans.x*_vaxes[0].x+tjoint.trans.y*_vaxes[0].y+tjoint.trans.z*_vaxes[0].z);
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);
404 return _info._voffsets[0]+f+(_dofbranches[0]*2*
PI);
408 for(
int i = 0; i <
GetDOF(); ++i) {
409 Vector vaxis = _vaxes.at(i);
410 if( IsRevolute(i) ) {
413 tjoint.rot = res.second;
414 if( res.first != 0 ) {
421 f = 2.0f*
RaveAtan2(tjoint.rot.y*vaxis.x+tjoint.rot.z*vaxis.y+tjoint.rot.w*vaxis.z, tjoint.rot.x);
431 return _info._voffsets[i]+f+(_dofbranches[i]*2*
PI);
435 f = tjoint.trans.x*vaxis.x+tjoint.trans.y*vaxis.y+tjoint.trans.z*vaxis.z;
437 return _info._voffsets[i]+f;
440 tjoint.trans -= vaxis*f;
452 pVelocities.resize(0);
455 pVelocities.push_back(GetVelocity(0));
458 _GetVelocities(pVelocities,bAppend,_attachedbodies[0]->GetVelocity(), _attachedbodies[1]->GetVelocity());
464 return _GetVelocity(axis,_attachedbodies[0]->GetVelocity(), _attachedbodies[1]->GetVelocity());
467 void KinBody::Joint::_GetVelocities(std::vector<dReal>& pVelocities,
bool bAppend,
const std::pair<Vector,Vector>& linkparentvelocity,
const std::pair<Vector,Vector>& linkchildvelocity)
const
470 pVelocities.resize(0);
473 pVelocities.push_back(GetVelocity(0));
476 const Transform& linkparenttransform = _attachedbodies[0]->_info._t;
477 const Transform& linkchildtransform = _attachedbodies[1]->_info._t;
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);
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());
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());
511 const Transform& linkparenttransform = _attachedbodies[0]->_info._t;
512 const Transform& linkchildtransform = _attachedbodies[1]->_info._t;
516 switch(_info._type) {
518 Vector v =
quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second);
527 return _vaxes[0].dot3(
quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-linkparentvelocity.second.cross(linkchildtransform.trans-linkparenttransform.trans)));
530 return _vaxes[0].dot3(
quatRotate(quatdeltainv,linkchildvelocity.second-linkparentvelocity.second));
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));
541 angvelocitycovered +=
quatRotate(quatdelta,_vaxes[i]*fvelocity);
544 dReal fvelocity = _vaxes[i].dot3(
quatRotate(quatdeltainv,linkchildvelocity.first-linkparentvelocity.first-(linkparentvelocity.second-angvelocitycovered).cross(linkparenttransform.trans-linkchildtransform.trans)-linvelocitycovered));
548 linvelocitycovered +=
quatRotate(quatdelta,_vaxes[i]*fvelocity);
558 return _attachedbodies[0]->GetTransform() * _tLeft.trans;
563 return _attachedbodies[0]->GetTransform().rotate(_tLeft.rotate(_vaxes.at(iaxis)));
569 for(
int i = 0; i <
GetDOF(); ++i) {
576 _bInitialized =
false;
577 _attachedbodies[0] = plink0;
578 _attachedbodies[1] = plink1;
580 Vector vanchor=vanchorraw;
581 for(
size_t i = 0; i < vaxes.size(); ++i) {
582 _vaxes[i] = vaxes[i];
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]);
591 vanchor = tswap*vanchor;
592 swap(_attachedbodies[0],_attachedbodies[1]);
597 for(
size_t i = 0; i < vaxes.size(); ++i) {
598 _info._vaxes[i] = _vaxes[i];
600 _info._vanchor = vanchor;
602 tbody0 = _attachedbodies[0]->GetTransform();
603 tbody1 = _attachedbodies[1]->GetTransform();
604 trel = tbody0.inverse() * tbody1;
611 switch(_info._type) {
613 _tLeft.trans = vanchor;
614 _tRight.trans = -vanchor;
615 _tRight = _tRight * trel;
619 _tLeft.trans = vanchor;
620 _tRight.trans = -vanchor;
621 _tRight = _tRight * trel;
625 _tLeft.trans = vanchor;
626 _tRight.trans = -vanchor;
627 _tRight = _tRight * trel;
630 if( !_info._trajfollow ) {
633 _tRight = _tRight * trel;
638 _tLeftNoOffset = _tLeft;
639 _tRightNoOffset = _tRight;
643 _tLeftNoOffset.trans = vanchor;
644 _tRightNoOffset.trans = -vanchor;
645 _tRightNoOffset = _tRightNoOffset * trel;
651 _tLeftNoOffset = _tLeftNoOffset * trot.inverse();
652 _tRightNoOffset = trot*_tRightNoOffset;
653 _vaxes[0] =
Vector(0,0,1);
657 if( IsRevolute(0) ) {
661 toffset.trans = _vaxes[0]*_info._voffsets[0];
663 _tLeft = _tLeftNoOffset * toffset;
664 _tRight = _tRightNoOffset;
667 if( IsRevolute(
GetDOF()-1) ) {
671 _tRight.trans += _vaxes[
GetDOF()-1]*_info._voffsets[
GetDOF()-1];
676 if( vcurrentvalues.size() > 0 ) {
680 vector<dReal> vsampledata;
682 _info._trajfollow->Sample(vsampledata,0);
683 if( !_info._trajfollow->GetConfigurationSpecification().ExtractTransform(t0,vsampledata.begin(),
KinBodyConstPtr()) ) {
686 _info._trajfollow->Sample(vsampledata,vcurrentvalues.at(0));
687 if( !_info._trajfollow->GetConfigurationSpecification().ExtractTransform(t1,vsampledata.begin(),
KinBodyConstPtr()) ) {
690 toffset = t0*t1.inverse();
693 if( IsRevolute(0) ) {
697 toffset.trans = -_vaxes[0]*vcurrentvalues[0];
700 _tLeftNoOffset *= toffset;
702 if( vcurrentvalues.size() > 1 ) {
703 if( IsRevolute(
GetDOF()-1) ) {
707 toffset.trans = -_vaxes[
GetDOF()-1]*vcurrentvalues.at(
GetDOF()-1);
709 _tRightNoOffset = toffset * _tRightNoOffset;
710 _tRight = toffset * _tRight;
713 _tinvRight = _tRight.inverse();
714 _tinvLeft = _tLeft.inverse();
716 _vcircularlowerlimit = _info._vlowerlimit;
717 _vcircularupperlimit = _info._vupperlimit;
718 for(
int i = 0; i <
GetDOF(); ++i) {
719 if( IsCircular(i) ) {
721 _info._vlowerlimit.at(i) = -1e4;
722 _info._vupperlimit.at(i) = 1e4;
726 if( !!_attachedbodies[0] ) {
727 _info._linkname0 = _attachedbodies[0]->GetName();
730 _info._linkname0.clear();
732 if( !!_attachedbodies[1] ) {
733 _info._linkname1 = _attachedbodies[1]->GetName();
736 _info._linkname1.clear();
738 _info._vcurrentvalues = vcurrentvalues;
740 _bInitialized =
true;
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()));
749 return _attachedbodies[0];
754 return _attachedbodies[1];
759 return _vaxes.at(iaxis);
765 return _tLeftNoOffset;
771 return _tRightNoOffset;
777 vLowerLimit.resize(0);
778 vUpperLimit.resize(0);
780 for(
int i = 0; i <
GetDOF(); ++i) {
781 vLowerLimit.push_back(_info._vlowerlimit[i]);
782 vUpperLimit.push_back(_info._vupperlimit[i]);
788 return make_pair(_info._vlowerlimit.at(iaxis),_info._vupperlimit.at(iaxis));
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) ) {
797 _info._vlowerlimit[i] = vLowerLimit.at(i);
798 _info._vupperlimit[i] = vUpperLimit.at(i);
799 if( IsRevolute(i) && !IsCircular(i) ) {
801 if( _info._vlowerlimit[i] < -
PI || _info._vupperlimit[i] >
PI) {
802 SetWrapOffset(0.5f * (_info._vlowerlimit.at(i) + _info._vupperlimit.at(i)),i);
821 for(
int i = 0; i <
GetDOF(); ++i) {
822 vlower.push_back(-_info._vmaxvel[i]);
823 vupper.push_back(_info._vmaxvel[i]);
832 for(
int i = 0; i <
GetDOF(); ++i) {
833 vmax.push_back(_info._vmaxvel[i]);
839 return make_pair(-_info._vmaxvel.at(iaxis), _info._vmaxvel.at(iaxis));
844 for(
int i = 0; i <
GetDOF(); ++i) {
845 _info._vmaxvel[i] = vmaxvel.at(i);
855 for(
int i = 0; i <
GetDOF(); ++i) {
856 vmax.push_back(_info._vmaxaccel[i]);
862 return _info._vmaxaccel.at(iaxis);
867 for(
int i = 0; i <
GetDOF(); ++i) {
868 _info._vmaxaccel[i] = vmax.at(i);
878 for(
int i = 0; i <
GetDOF(); ++i) {
879 vmax.push_back(_info._vmaxtorque[i]);
885 for(
int i = 0; i <
GetDOF(); ++i) {
886 _info._vmaxtorque[i] = vmax.at(i);
893 if( _info._voffsets.at(iaxis) != newoffset ) {
894 _info._voffsets.at(iaxis) = newoffset;
897 if( IsRevolute(0) ) {
901 toffset.trans = _vaxes[0]*newoffset;
903 _tLeft = _tLeftNoOffset * toffset;
904 _tinvLeft = _tLeft.inverse();
907 _tRight = _tRightNoOffset;
909 if( IsRevolute(
GetDOF()-1) ) {
913 _tRight.trans += _vaxes[
GetDOF()-1]*newoffset;
915 _tinvRight = _tRight.inverse();
924 resolutions.resize(
GetDOF());
926 for(
int i = 0; i <
GetDOF(); ++i) {
927 resolutions.push_back(_info._vresolution[i]);
933 return _info._vresolution.at(iaxis);
938 _info._vresolution.at(iaxis) = resolution;
947 for(
int i = 0; i <
GetDOF(); ++i) {
948 weights.push_back(_info._vweights[i]);
954 return _info._vweights.at(iaxis);
959 for(
int i = 0; i <
GetDOF(); ++i) {
961 _info._vweights[i] = vweights.at(i);
968 for(
int i = 0; i <
GetDOF(); ++i) {
969 if( IsCircular(i) ) {
973 q1.at(i) -= q2.at(i);
980 if( IsCircular(iaxis) ) {
984 return value1-value2;
990 GetParent()->GetEnv()->GetPhysicsEngine()->AddJointTorque(shared_from_this(), pTorques);
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();
1005 RAVELOG_WARN(
"deprecated KinBody::Joint::GetMimicCoeffs(): could not deduce coeffs\n");
1006 std::vector<dReal> coeffs(2); coeffs[0] = 1; coeffs[1] = 0;
1013 return !!_vmimic.at(iaxis);
1015 for(
int i = 0; i <
GetDOF(); ++i) {
1016 if( !!_vmimic.at(i) ) {
1025 if( !_vmimic.at(iaxis) ) {
1028 if((format.size() == 0)||(format ==
"fparser")) {
1029 return _vmimic.at(iaxis)->_equations.at(itype);
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;
1036 FOREACHC(itdofformat, _vmimic.at(iaxis)->_vdofformat) {
1038 if( pjoint->GetDOF() > 1 ) {
1039 Vars.push_back(str(boost::format(
"<csymbol>%s_%d</csymbol>")%pjoint->GetName()%(int)itdofformat->axis));
1042 Vars.push_back(str(boost::format(
"<csymbol>%s</csymbol>")%pjoint->GetName()));
1046 _vmimic.at(iaxis)->_posfn->toMathML(sout,Vars);
1047 if((sout.size() > 9)&&(sout.substr(0,9) ==
"<csymbol>")) {
1049 sout = boost::str(boost::format(
"<apply>\n <plus/><cn type=\"real\">0</cn>\n %s\n </apply>")%sout);
1051 sout = str(mathfmt%sout);
1053 else if( itype == 1 ) {
1055 FOREACHC(itfn, _vmimic.at(iaxis)->_velfns) {
1056 (*itfn)->toMathML(stemp,Vars);
1057 sout += str(mathfmt%stemp);
1060 else if( itype == 2 ) {
1062 FOREACHC(itfn, _vmimic.at(iaxis)->_accelfns) {
1063 (*itfn)->toMathML(stemp,Vars);
1064 sout += str(mathfmt%stemp);
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);
1086 _vmimic.at(iaxis).reset();
1087 if( poseq.size() == 0 ) {
1091 std::vector<std::string> resultVars;
1093 mimic->_equations.at(0) = poseq;
1094 mimic->_equations.at(1) = veleq;
1095 mimic->_equations.at(2) = acceleq;
1098 if( !_info._vmimic.at(iaxis) ) {
1099 _info._vmimic.at(iaxis).reset(
new MimicInfo());
1101 _info._vmimic.at(iaxis)->_equations = mimic->_equations;
1104 mimic->_posfn = posfn;
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())));
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)));
1120 std::map<std::string,std::string> mapinvnames;
1121 FOREACH(itpair,jointnamepairs) {
1122 mapinvnames[itpair->second] = itpair->first;
1126 int ret = posfn->ParseAndDeduceVariables(
utils::SearchAndReplace(eq,mimic->_equations[0],jointnamepairs),resultVars);
1131 FOREACH(itvar,resultVars) {
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));
1140 dofformat.jointindex = boost::lexical_cast<uint16_t>(itvar->substr(5));
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);
1152 mimic->_vdofformat.push_back(dofformat);
1156 if( !resultVars.empty() ) {
1157 sVars << resultVars.at(0);
1158 for(
size_t i = 1; i < resultVars.size(); ++i) {
1159 sVars <<
"," << resultVars[i];
1163 for(
int itype = 1; itype < 3; ++itype) {
1164 if((itype == 2)&&(mimic->_equations[itype].size() == 0)) {
1168 std::vector<OpenRAVEFunctionParserRealPtr> vfns(resultVars.size());
1171 size_t index = eq.find(
'|');
1172 while(index != std::string::npos) {
1173 size_t startindex = index+1;
1174 index = eq.find(
'|',startindex);
1176 if( index != std::string::npos) {
1177 sequation = eq.substr(startindex,index-startindex);
1180 sequation = eq.substr(startindex);
1182 boost::trim(sequation);
1183 size_t nameendindex = sequation.find(
' ');
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;
1191 varname = sequation.substr(0,nameendindex);
1192 sequation = sequation.substr(nameendindex);
1194 vector<string>::iterator itnameindex = find(resultVars.begin(),resultVars.end(),varname);
1198 ret = fn->Parse(sequation,sVars.str());
1202 vfns.at(itnameindex-resultVars.begin()) = fn;
1205 for(
size_t j = 0; j < resultVars.size(); ++j) {
1208 RAVELOG_WARN(str(boost::format(
"SetMimicEquations: missing variable %s from partial derivatives of joint %s!")%mapinvnames[resultVars[j]]%_info._name));
1210 vfns[j]->Parse(
"0",
"");
1215 mimic->_velfns.swap(vfns);
1218 mimic->_accelfns.swap(vfns);
1221 _vmimic.at(iaxis) = mimic;
1227 vpartials.resize(0);
1228 if( dofindex >= 0 ) {
1229 vpartials.push_back(make_pair(dofindex+iaxis,1.0));
1234 MIMIC::DOFFormat thisdofformat;
1235 thisdofformat.dofindex = -1;
1236 thisdofformat.axis = iaxis;
1237 thisdofformat.jointindex = jointindex;
1238 if( jointindex < 0 ) {
1240 thisdofformat.jointindex = parent->GetJoints().size() + (find(parent->GetPassiveJoints().begin(),parent->GetPassiveJoints().end(),shared_from_this()) - parent->GetPassiveJoints().begin());
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() ) {
1249 if( vtempvalues.empty() ) {
1250 FOREACHC(itdofformat, _vmimic[iaxis]->_vdofformat) {
1251 vtempvalues.push_back(itdofformat->GetJoint(parent)->GetValue(itdofformat->axis));
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);
1259 FOREACHC(itpartial,vtemppartials) {
1260 if( itpartial->first == itmimicdof->dofindex ) {
1261 fpartial += itpartial->second;
1268 FOREACH(itpartial,vpartials) {
1269 if( itpartial->first == itmimicdof->dofindex ) {
1270 itpartial->second += fvel;
1276 vpartials.push_back(make_pair(itmimicdof->dofindex, fvel));
1281 FOREACH(itpartial,vpartials) {
1282 if( itpartial->first == itmimicdof->dofindex ) {
1288 vpartials.push_back(make_pair(itmimicdof->dofindex, it->second));
1294 int KinBody::Joint::_Eval(
int axis, uint32_t timederiv,
const std::vector<dReal>& vdependentvalues, std::vector<dReal>& voutput)
1296 if( timederiv == 0 ) {
1297 _vmimic.at(axis)->_posfn->EvalMulti(voutput, vdependentvalues.empty() ? NULL : &vdependentvalues[0]);
1298 return _vmimic.at(axis)->_posfn->EvalError();
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();
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();
1326 bool KinBody::Joint::MIMIC::DOFFormat::operator <(
const KinBody::Joint::MIMIC::DOFFormat& r)
const
1328 return jointindex < r.jointindex || (jointindex == r.jointindex && (dofindex < r.dofindex || (dofindex == r.dofindex && axis < r.axis)));
1331 bool KinBody::Joint::MIMIC::DOFFormat::operator ==(
const KinBody::Joint::MIMIC::DOFFormat& r)
const
1333 return jointindex == r.jointindex && dofindex == r.dofindex && axis == r.axis;
1338 int numjoints = (int)parent->GetJoints().size();
1339 return jointindex < numjoints ? parent->GetJoints().at(jointindex) : parent->GetPassiveJoints().at(jointindex-numjoints);
1344 int numjoints = (int)parent->GetJoints().size();
1345 return jointindex < numjoints ? parent->GetJoints().at(jointindex) : parent->GetPassiveJoints().at(jointindex-numjoints);
1350 if( parameters.size() > 0 ) {
1351 _info._mapFloatParameters[key] = parameters;
1354 _info._mapFloatParameters.erase(key);
1361 if( parameters.size() > 0 ) {
1362 _info._mapIntParameters[key] = parameters;
1365 _info._mapIntParameters.erase(key);
1372 _info._vcurrentvalues.resize(0);
1373 GetValues(_info._vcurrentvalues);
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) {
1390 o << (!_attachedbodies[0] ? -1 : _attachedbodies[0]->GetIndex()) <<
" " << (_attachedbodies[1]->GetIndex()) <<
" ";
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]);