17 #include "libopenrave.h"
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);
34 boost::unique_lock< boost::shared_mutex > lock(pbody->GetInterfaceMutex());
35 pbody->_listRegisteredCallbacks.erase(
_iterator);
70 _RestoreKinBody(_pbody);
75 _RestoreKinBody(!body ? _pbody : body);
83 void KinBody::KinBodyStateSaver::_RestoreKinBody(boost::shared_ptr<KinBody> pbody)
88 if( pbody->GetEnvironmentId() == 0 ) {
89 RAVELOG_WARN(str(boost::format(
"body %s not added to environment, skipping restore")%pbody->GetName()));
93 pbody->SetLinkTransformations(_vLinkTransforms, _vdofbranches);
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];
110 pbody->SetDOFVelocityLimits(_vMaxVelocities);
111 pbody->SetDOFAccelerationLimits(_vMaxAccelerations);
114 pbody->SetLinkVelocities(_vLinkVelocities);
138 stringstream ss; ss <<
GetName() <<
" still has attached bodies: ";
142 ss << pattached->GetName();
177 plink->_info._name =
"base";
178 plink->_info._bStatic =
true;
179 size_t numvertices=0, numindices=0;
180 FOREACHC(itab, vaabbs) {
183 info.
_t.trans = itab->pos;
189 geom->_info.InitCollisionMesh();
190 numvertices += geom->GetCollisionMesh().vertices.size();
191 numindices += geom->GetCollisionMesh().indices.size();
192 plink->_vGeometries.push_back(geom);
195 plink->_collision.vertices.reserve(numvertices);
196 plink->_collision.indices.reserve(numindices);
198 FOREACH(itgeom,plink->_vGeometries) {
199 trimesh = (*itgeom)->GetCollisionMesh();
201 plink->_collision.Append(trimesh);
213 plink->_info._name =
"base";
214 plink->_info._bStatic =
true;
215 size_t numvertices=0, numindices=0;
216 FOREACHC(itobb, vobbs) {
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;
230 geom->_info.InitCollisionMesh();
231 numvertices += geom->GetCollisionMesh().vertices.size();
232 numindices += geom->GetCollisionMesh().indices.size();
233 plink->_vGeometries.push_back(geom);
236 plink->_collision.vertices.reserve(numvertices);
237 plink->_collision.indices.reserve(numindices);
239 FOREACH(itgeom,plink->_vGeometries) {
240 trimesh = (*itgeom)->GetCollisionMesh();
242 plink->_collision.Append(trimesh);
254 plink->_info._name =
"base";
255 plink->_info._bStatic =
true;
257 FOREACHC(itv, vspheres) {
260 info.
_t.trans.x = itv->x; info.
_t.trans.y = itv->y; info.
_t.trans.z = itv->z;
266 geom->_info.InitCollisionMesh();
267 plink->_vGeometries.push_back(geom);
268 trimesh = geom->GetCollisionMesh();
270 plink->_collision.Append(trimesh);
282 plink->_info._name =
"base";
283 plink->_info._bStatic =
true;
284 plink->_collision = trimesh;
292 plink->_vGeometries.push_back(geom);
299 std::vector<GeometryInfoConstPtr> newgeometries; newgeometries.reserve(geometries.size());
300 FOREACHC(it, geometries) {
313 plink->_info._name =
"base";
314 plink->_info._bStatic =
true;
315 FOREACHC(itinfo,geometries) {
317 geom->_info.InitCollisionMesh();
318 plink->_vGeometries.push_back(geom);
319 plink->_collision.Append(geom->GetCollisionMesh(),geom->GetTransform());
325 bool KinBody::Init(
const std::vector<KinBody::LinkInfoConstPtr>& linkinfos,
const std::vector<KinBody::JointInfoConstPtr>& jointinfos)
331 FOREACHC(itlinkinfo, linkinfos) {
334 plink->_info = *rawinfo;
336 plink->_index =
static_cast<int>(
_veclinks.size());
339 geom->_info.InitCollisionMesh();
340 plink->_vGeometries.push_back(geom);
341 plink->_collision.Append(geom->GetCollisionMesh(),geom->GetTransform());
349 FOREACHC(itjointinfo, jointinfos) {
352 pjoint->_info = *rawinfo;
354 for(
size_t i = 0; i < info.
_vmimic.size(); ++i) {
357 pjoint->_vmimic[i]->_equations = info.
_vmimic[i]->_equations;
362 if( (*itlink)->_info._name == info.
_linkname0 ) {
368 if( (*itlink)->_info._name == info.
_linkname1 ) {
376 std::vector<Vector> vaxes(pjoint->GetDOF());
377 std::copy(info.
_vaxes.begin(),info.
_vaxes.begin()+vaxes.size(), vaxes.begin());
392 if(
_name != newname ) {
395 stringstream ss(itgroup->name);
396 string grouptype, oldname;
397 ss >> grouptype >> oldname;
400 itgroup->name = str(boost::format(
"%s %s %s")%grouptype%newname%buf.str());
413 (*itlink)->SetTorque(
Vector(),
false);
416 std::vector<dReal> jointtorques;
418 jointtorques.resize((*it)->GetDOF());
419 std::copy(torques.begin()+(*it)->GetDOFIndex(),torques.begin()+(*it)->GetDOFIndex()+(*it)->GetDOF(),jointtorques.begin());
420 (*it)->AddTorque(jointtorques);
432 if( dofindices.size() == 0 ) {
434 if( (
int)v.capacity() <
GetDOF() ) {
438 int toadd = (*it)->GetDOFIndex()-(int)v.size();
440 v.insert(v.end(),toadd,0);
442 else if( toadd < 0 ) {
445 (*it)->GetValues(v,
true);
449 v.resize(dofindices.size());
450 for(
size_t i = 0; i < dofindices.size(); ++i) {
452 v[i] = pjoint->GetValue(dofindices[i]-pjoint->GetDOFIndex());
459 if( dofindices.size() == 0 ) {
461 if( (
int)v.capacity() <
GetDOF() ) {
465 (*it)->GetVelocities(v,
true);
469 v.resize(dofindices.size());
470 for(
size_t i = 0; i < dofindices.size(); ++i) {
472 v[i] = pjoint->GetVelocity(dofindices[i]-pjoint->GetDOFIndex());
477 void KinBody::GetDOFLimits(std::vector<dReal>& vLowerLimit, std::vector<dReal>& vUpperLimit,
const std::vector<int>& dofindices)
const
479 if( dofindices.size() == 0 ) {
480 vLowerLimit.resize(0);
481 if( (
int)vLowerLimit.capacity() <
GetDOF() ) {
482 vLowerLimit.reserve(
GetDOF());
484 vUpperLimit.resize(0);
485 if( (
int)vUpperLimit.capacity() <
GetDOF() ) {
486 vUpperLimit.reserve(
GetDOF());
489 (*it)->GetLimits(vLowerLimit,vUpperLimit,
true);
493 vLowerLimit.resize(dofindices.size());
494 vUpperLimit.resize(dofindices.size());
495 for(
size_t i = 0; i < dofindices.size(); ++i) {
497 std::pair<dReal, dReal> res = pjoint->GetLimit(dofindices[i]-pjoint->GetDOFIndex());
498 vLowerLimit[i] = res.first;
499 vUpperLimit[i] = res.second;
506 if( dofindices.size() == 0 ) {
509 if( (
int)vlower.capacity() <
GetDOF() ) {
512 if( (
int)vupper.capacity() <
GetDOF() ) {
516 (*it)->GetVelocityLimits(vlower,vupper,
true);
520 vlower.resize(dofindices.size());
521 vupper.resize(dofindices.size());
522 for(
size_t i = 0; i < dofindices.size(); ++i) {
524 std::pair<dReal, dReal> res = pjoint->GetVelocityLimit(dofindices[i]-pjoint->GetDOFIndex());
525 vlower[i] = res.first;
526 vupper[i] = res.second;
533 if( dofindices.size() == 0 ) {
535 if( (
int)v.capacity() <
GetDOF() ) {
539 (*it)->GetVelocityLimits(v,
true);
543 v.resize(dofindices.size());
544 for(
size_t i = 0; i < dofindices.size(); ++i) {
546 v[i] = pjoint->GetMaxVel(dofindices[i]-pjoint->GetDOFIndex());
553 if( dofindices.size() == 0 ) {
555 if( (
int)v.capacity() <
GetDOF() ) {
559 (*it)->GetAccelerationLimits(v,
true);
563 v.resize(dofindices.size());
564 for(
size_t i = 0; i < dofindices.size(); ++i) {
566 v[i] = pjoint->GetAccelerationLimit(dofindices[i]-pjoint->GetDOFIndex());
574 if( (
int)v.capacity() <
GetDOF() ) {
578 (*it)->GetTorqueLimits(v,
true);
585 if( (
int)v.capacity() <
GetDOF() ) {
589 v.insert(v.end(),(*it)->GetDOF(),(*it)->GetMaxTorque());
595 if( dofindices.size() == 0 ) {
597 if( (
int)v.capacity() <
GetDOF() )
600 v.insert(v.end(),(*it)->GetDOF(),(*it)->GetResolution());
604 v.resize(dofindices.size());
605 for(
size_t i = 0; i < dofindices.size(); ++i) {
607 v[i] = pjoint->GetResolution(dofindices[i]-pjoint->GetDOFIndex());
614 if( dofindices.size() == 0 ) {
616 std::vector<dReal>::iterator itv = v.begin();
618 for(
int i = 0; i < (*it)->GetDOF(); ++i) {
619 *itv++ = (*it)->GetWeight(i);
624 v.resize(dofindices.size());
625 for(
size_t i = 0; i < dofindices.size(); ++i) {
627 v[i] = pjoint->GetWeight(dofindices[i]-pjoint->GetDOFIndex());
634 if( dofindices.size() == 0 ) {
636 for(
int i = 0; i <
GetDOF(); ++i) {
639 std::vector<dReal>::const_iterator itv = v.begin();
641 std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vweights.begin());
642 itv += (*it)->GetDOF();
647 for(
size_t i = 0; i < dofindices.size(); ++i) {
649 pjoint->_info._vweights.at(dofindices[i]-pjoint->GetDOFIndex()) = v[i];
659 bool bChanged =
false;
660 std::vector<dReal>::const_iterator itlower = lower.begin(), itupper = upper.begin();
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) ) {
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) ) {
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);
674 (*it)->SetWrapOffset(0,i);
681 itlower += (*it)->GetDOF();
682 itupper += (*it)->GetDOF();
691 std::vector<dReal>::const_iterator itv = v.begin();
693 std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vmaxvel.begin());
694 itv += (*it)->GetDOF();
701 std::vector<dReal>::const_iterator itv = v.begin();
703 std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vmaxaccel.begin());
704 itv += (*it)->GetDOF();
711 std::vector<dReal>::const_iterator itv = v.begin();
713 std::copy(itv,itv+(*it)->GetDOF(), (*it)->_info._vmaxtorque.begin());
714 itv += (*it)->GetDOF();
725 if( dofindices.size() == 0 ) {
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));
733 q1.at(dof+i) -= q2.at(dof+i);
739 for(
size_t i = 0; i < dofindices.size(); ++i) {
741 if( pjoint->IsCircular(dofindices[i]-pjoint->GetDOFIndex()) ) {
742 int iaxis = dofindices[i]-pjoint->GetDOFIndex();
746 q1.at(i) -= q2.at(i);
761 (*itlink)->SetTransform(tapply * (*itlink)->GetTransform());
772 std::vector<std::pair<Vector,Vector> > velocities(
_veclinks.size());
773 velocities.at(0).first = linearvel;
774 velocities.at(0).second = angularvel;
776 for(
size_t i = 1; i <
_veclinks.size(); ++i) {
778 velocities[i].second = angularvel;
787 std::vector<std::pair<Vector,Vector> > velocities(
_veclinks.size());
788 velocities.at(0).first = linearvel;
789 velocities.at(0).second = angularvel;
791 vector<dReal> vlower,vupper,vtempvalues, veval;
797 std::vector< std::vector<dReal> > vPassiveJointVelocities(
_vPassiveJoints.size());
798 for(
size_t i = 0; i < vPassiveJointVelocities.size(); ++i) {
807 std::vector<uint8_t> vlinkscomputed(
_veclinks.size(),0);
808 vlinkscomputed[0] = 1;
809 boost::array<dReal,3> dummyvalues;
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) {
823 vtempvalues.push_back(pj->GetValue(itdof->axis));
826 int err = pjoint->_Eval(i,1,vtempvalues,veval);
828 RAVELOG_WARN(str(boost::format(
"failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
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);
837 partialvelocity = vPassiveJointVelocities.at(vdofformat[ipartial].jointindex-
_vecjoints.size()).at(vdofformat[ipartial].axis);
839 dummyvalues[i] += veval.at(ipartial) * partialvelocity;
845 vPassiveJointVelocities.at(jointindex-(
int)
_vecjoints.size()).at(i) = dummyvalues[i];
848 else if( dofindex >= 0 ) {
849 dummyvalues[i] = vDOFVelocities.at(dofindex+i);
853 dummyvalues[i] = vPassiveJointVelocities.at(jointindex-(
int)
_vecjoints.size()).at(i);
856 pvalues = &dummyvalues[0];
859 if( vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] ) {
864 pvalues = &vPassiveJointVelocities.at(jointindex-(
int)
_vecjoints.size()).at(0);
867 if( checklimits &&(dofindex >= 0)) {
868 for(
int i = 0; i < pjoint->GetDOF(); ++i) {
869 if( pvalues[i] < vlower.at(dofindex+i) ) {
871 RAVELOG_WARN(str(boost::format(
"dof %d velocity is not in limits %.15e<%.15e")%i%pvalues[i]%vlower.at(dofindex+i)));
876 dummyvalues[i] = vlower[i];
878 else if( pvalues[i] > vupper.at(dofindex+i) ) {
880 RAVELOG_WARN(str(boost::format(
"dof %d velocity is not in limits %.15e>%.15e")%i%pvalues[i]%vupper.at(dofindex+i)));
885 dummyvalues[i] = vupper[i];
888 dummyvalues[i] = pvalues[i];
891 pvalues = &dummyvalues[0];
897 if( !pjoint->GetHierarchyParentLink() ) {
898 tparent =
_veclinks.at(0)->GetTransform();
899 vparent = velocities.at(0).first;
900 wparent = velocities.at(0).second;
903 tparent = pjoint->GetHierarchyParentLink()->GetTransform();
904 vparent = velocities[pjoint->GetHierarchyParentLink()->GetIndex()].first;
905 wparent = velocities[pjoint->GetHierarchyParentLink()->GetIndex()].second;
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();
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);
934 velocities.at(childindex) = make_pair(vparent + wparent.cross(xyzdelta) + tdelta.rotate(pvalues[0]*pjoint->GetInternalHierarchyAxis(0)), wparent);
938 if( pjoint->IsMimic(0) ) {
940 int err = pjoint->_Eval(0,0,vtempvalues,veval);
942 RAVELOG_WARN(str(boost::format(
"error with evaluation of joint %s")%pjoint->GetName()));
944 dReal fvalue = veval[0];
945 if( pjoint->IsCircular(0) ) {
948 pjoint->_info._trajfollow->Sample(vtempvalues,fvalue);
952 pjoint->_info._trajfollow->Sample(vtempvalues,pjoint->GetValue(0));
954 pjoint->_info._trajfollow->GetConfigurationSpecification().ExtractTransform(tlocal, vtempvalues.begin(),
KinBodyConstPtr(),0);
955 pjoint->_info._trajfollow->GetConfigurationSpecification().ExtractTransform(tlocalvelocity, vtempvalues.begin(),
KinBodyConstPtr(),1);
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);
981 vlinkscomputed[childindex] = 1;
988 Vector linearvel,angularvel;
989 _veclinks.at(0)->GetVelocity(linearvel,angularvel);
990 if( dofindices.size() == 0 ) {
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 ) {
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()));
1034 RAVELOG_VERBOSE(
"GetLinkTransformations should be called with dofbranches\n");
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();
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();
1053 dofbranches.resize(0);
1054 if( (
int)dofbranches.capacity() <
GetDOF() ) {
1055 dofbranches.reserve(
GetDOF());
1058 int toadd = (*it)->GetDOFIndex()-(int)dofbranches.size();
1060 dofbranches.insert(dofbranches.end(),toadd,0);
1062 else if( toadd < 0 ) {
1065 for(
int i = 0; i < (*it)->GetDOF(); ++i) {
1066 dofbranches.push_back((*it)->_dofbranches[i]);
1079 bool binitialized=
false;
1082 ab = (*itlink)->ComputeAABB();
1088 if( !binitialized ) {
1091 binitialized =
true;
1094 if( vmin.x > vnmin.x ) {
1097 if( vmin.y > vnmin.y ) {
1100 if( vmin.z > vnmin.z ) {
1103 if( vmax.x < vnmax.x ) {
1106 if( vmax.y < vnmax.y ) {
1109 if( vmax.z < vnmax.z ) {
1114 if( !binitialized ) {
1119 ab.
pos = (
dReal)0.5 * (vmin + vmax);
1129 dReal fTotalMass = 0;
1132 center += ((*itlink)->GetTransform() * (*itlink)->GetCOMOffset() * (*itlink)->GetMass());
1133 fTotalMass += (*itlink)->GetMass();
1136 if( fTotalMass > 0 ) {
1137 center /= fTotalMass;
1145 RAVELOG_WARN(
"SetLinkTransformations should be called with dofbranches, setting all branches to 0\n");
1148 RAVELOG_DEBUG(
"SetLinkTransformations should be called with dofbranches, setting all branches to 0\n");
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);
1157 for(
int i = 0; i < (*itjoint)->GetDOF(); ++i) {
1158 (*itjoint)->_dofbranches[i] = 0;
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);
1173 for(
int i = 0; i < (*itjoint)->GetDOF(); ++i) {
1174 (*itjoint)->_dofbranches[i] = dofbranches.at((*itjoint)->GetDOFIndex()+i);
1191 _veclinks.at(0)->SetTransform(transBase);
1194 for(
size_t i = 1; i <
_veclinks.size(); ++i) {
1200 void KinBody::SetDOFValues(
const std::vector<dReal>& vJointValues, uint32_t checklimits,
const std::vector<int>& dofindices)
1204 if( vJointValues.size() == 0 ||
_veclinks.size() == 0) {
1207 int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() :
GetDOF();
1210 const dReal* pJointValues = &vJointValues[0];
1211 if( checklimits !=
CLA_Nothing || dofindices.size() > 0 ) {
1212 _vTempJoints.resize(
GetDOF());
1213 if( dofindices.size() > 0 ) {
1217 for(
size_t i = 0; i < dofindices.size(); ++i) {
1218 _vTempJoints.at(dofindices[i]) = pJointValues[i];
1220 pJointValues = &_vTempJoints[0];
1222 dReal* ptempjoints = &_vTempJoints[0];
1225 vector<dReal> upperlim, lowerlim;
1227 const dReal* p = pJointValues+(*it)->GetDOFIndex();
1229 (*it)->GetLimits(lowerlim, upperlim);
1232 if( fcurang < lowerlim[0] ) {
1233 if( fcurang < 1e-10 ) {
1234 *ptempjoints++ = lowerlim[0]; *ptempjoints++ = 0; *ptempjoints++ = 0;
1237 dReal fmult = lowerlim[0]/fcurang;
1238 *ptempjoints++ = p[0]*fmult; *ptempjoints++ = p[1]*fmult; *ptempjoints++ = p[2]*fmult;
1241 else if( fcurang > upperlim[0] ) {
1242 if( fcurang < 1e-10 ) {
1243 *ptempjoints++ = upperlim[0]; *ptempjoints++ = 0; *ptempjoints++ = 0;
1246 dReal fmult = upperlim[0]/fcurang;
1247 *ptempjoints++ = p[0]*fmult; *ptempjoints++ = p[1]*fmult; *ptempjoints++ = p[2]*fmult;
1251 *ptempjoints++ = p[0]; *ptempjoints++ = p[1]; *ptempjoints++ = p[2];
1255 for(
int i = 0; i < (*it)->GetDOF(); ++i) {
1256 if( (*it)->IsCircular(i) ) {
1258 *ptempjoints++ = p[i];
1261 if( p[i] < lowerlim[i] ) {
1262 if( p[i] < lowerlim[i]-g_fEpsilonEvalJointLimit ) {
1264 RAVELOG_WARN(str(boost::format(
"dof %d value is not in limits %e<%e")%((*it)->GetDOFIndex()+i)%p[i]%lowerlim[i]));
1270 *ptempjoints++ = lowerlim[i];
1272 else if( p[i] > upperlim[i] ) {
1273 if( p[i] > upperlim[i]+g_fEpsilonEvalJointLimit ) {
1275 RAVELOG_WARN(str(boost::format(
"dof %d value is not in limits %e<%e")%((*it)->GetDOFIndex()+i)%p[i]%upperlim[i]));
1281 *ptempjoints++ = upperlim[i];
1284 *ptempjoints++ = p[i];
1290 pJointValues = &_vTempJoints[0];
1293 boost::array<dReal,3> dummyvalues;
1294 std::vector<dReal> vtempvalues, veval;
1297 std::vector< std::vector<dReal> > vPassiveJointValues(
_vPassiveJoints.size());
1298 for(
size_t i = 0; i < vPassiveJointValues.size(); ++i) {
1302 for(
size_t j = 0; j < vPassiveJointValues[i].size(); ++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]));
1308 vPassiveJointValues[i][j] =
_vPassiveJoints[i]->_info._vlowerlimit.at(j);
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]));
1314 vPassiveJointValues[i][j] =
_vPassiveJoints[i]->_info._vupperlimit.at(j);
1324 std::vector<uint8_t> vlinkscomputed(
_veclinks.size(),0);
1325 vlinkscomputed[0] = 1;
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]);
1342 vtempvalues.push_back(vPassiveJointValues.at(itdof->jointindex-
_vecjoints.size()).at(itdof->axis));
1345 int err = pjoint->_Eval(i, 0, vtempvalues, veval);
1347 RAVELOG_WARN(str(boost::format(
"failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
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) ) {
1356 else if( *iteval < pjoint->_info._vlowerlimit[i] ) {
1357 if(*iteval >= pjoint->_info._vlowerlimit[i]-g_fEpsilonJointLimit ) {
1358 *iteval = pjoint->_info._vlowerlimit[i];
1364 else if( *iteval > pjoint->_info._vupperlimit[i] ) {
1365 if(*iteval <= pjoint->_info._vupperlimit[i]+g_fEpsilonJointLimit ) {
1366 *iteval = pjoint->_info._vupperlimit[i];
1374 iteval = veval.erase(iteval);
1381 if( veval.empty() ) {
1382 FORIT(iteval,vevalcopy) {
1384 veval.push_back(*iteval);
1386 else if( *iteval < pjoint->_info._vlowerlimit[i]-g_fEpsilonEvalJointLimit ) {
1387 veval.push_back(pjoint->_info._vlowerlimit[i]);
1389 RAVELOG_WARN(str(boost::format(
"joint %s: lower limit (%e) is not followed: %e")%pjoint->GetName()%pjoint->_info._vlowerlimit[i]%*iteval));
1395 else if( *iteval > pjoint->_info._vupperlimit[i]+g_fEpsilonEvalJointLimit ) {
1396 veval.push_back(pjoint->_info._vupperlimit[i]);
1398 RAVELOG_WARN(str(boost::format(
"joint %s: upper limit (%e) is not followed: %e")%pjoint->GetName()%pjoint->_info._vupperlimit[i]%*iteval));
1405 veval.push_back(*iteval);
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 <<
" ";
1418 dummyvalues[i] = veval.at(0);
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];
1427 else if( dofindex >= 0 ) {
1428 dummyvalues[i] = pvalues[dofindex+i];
1432 dummyvalues[i] = vPassiveJointValues.at(jointindex-(
int)
_vecjoints.size()).at(i);
1435 pvalues = &dummyvalues[0];
1438 if( vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] ) {
1443 pvalues = &vPassiveJointValues.at(jointindex-(
int)
_vecjoints.size()).at(0);
1448 switch(pjoint->GetType()) {
1451 tfirst.rot =
quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(0), pvalues[0]);
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));
1460 dReal fang = pvalues[0]*pvalues[0]+pvalues[1]*pvalues[1]+pvalues[2]*pvalues[2];
1463 dReal fiang = 1/fang;
1469 vector<dReal> vdata;
1471 dReal fvalue = pvalues[0];
1472 if( pjoint->IsCircular(0) ) {
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()));
1480 pjoint->_dofbranches[0] = 0;
1484 RAVELOG_WARN(str(boost::format(
"forward kinematic type 0x%x not supported")%pjoint->GetType()));
1490 tjoint.rot =
quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(0), pvalues[0]);
1491 pjoint->_dofbranches[0] = CountCircularBranches(pvalues[0]-pjoint->GetWrapOffset(0));
1494 tjoint.trans = pjoint->GetInternalHierarchyAxis(0) * pvalues[0];
1497 for(
int iaxis = 0; iaxis < pjoint->GetDOF(); ++iaxis) {
1499 if( pjoint->IsRevolute(iaxis) ) {
1500 tdelta.rot =
quatFromAxisAngle(pjoint->GetInternalHierarchyAxis(iaxis), pvalues[iaxis]);
1501 pjoint->_dofbranches[iaxis] = CountCircularBranches(pvalues[iaxis]-pjoint->GetWrapOffset(iaxis));
1504 tdelta.trans = pjoint->GetInternalHierarchyAxis(iaxis) * pvalues[iaxis];
1506 tjoint = tjoint * tdelta;
1511 Transform t = pjoint->GetInternalHierarchyLeftTransform() * tjoint * pjoint->GetInternalHierarchyRightTransform();
1512 if( !pjoint->GetHierarchyParentLink() ) {
1513 t =
_veclinks.at(0)->GetTransform() * t;
1516 t = pjoint->GetHierarchyParentLink()->GetTransform() * t;
1518 pjoint->GetHierarchyChildLink()->SetTransform(t);
1519 vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] = 1;
1525 for(std::vector<LinkPtr>::const_iterator it =
_veclinks.begin(); it !=
_veclinks.end(); ++it) {
1526 if ( (*it)->GetName() == linkname ) {
1551 if( linkindex1 == linkindex2 ) {
1554 int offset = linkindex2*
_veclinks.size();
1555 int curlink = linkindex1;
1559 int prevlink = curlink;
1563 return vjoints.size()>0;
1572 int offset = linkindex2*
_veclinks.size();
1573 int curlink = linkindex1;
1577 vlinks.push_back(
_veclinks.at(linkindex1));
1578 if( linkindex1 == linkindex2 ) {
1587 vlinks.push_back(
_veclinks.at(curlink));
1589 vlinks.push_back(
_veclinks.at(linkindex2));
1604 if ((*it)->GetName() == jointname ) {
1615 if ((*it)->GetName() == jointname ) {
1620 if ((*it)->GetName() == jointname ) {
1632 if( dofindices.size() > 0 ) {
1633 dofstride = dofindices.size();
1638 vjacobian.resize(3*dofstride);
1639 if( dofstride == 0 ) {
1642 std::fill(vjacobian.begin(),vjacobian.end(),0);
1645 int offset = linkindex*
_veclinks.size();
1647 std::vector<std::pair<int,dReal> > vpartials;
1648 std::vector<int> vpartialindices;
1649 std::map< std::pair<Mimic::DOFFormat, int>,
dReal > mapcachedpartials;
1655 int dofindex = pjoint->GetDOFIndex();
1656 int8_t affect =
DoesAffect(pjoint->GetJointIndex(), linkindex);
1657 for(
int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1659 if( pjoint->IsRevolute(dof) ) {
1660 v = pjoint->GetAxis(dof).cross(position-pjoint->GetAnchor());
1662 else if( pjoint->IsPrismatic(dof) ) {
1663 v = pjoint->GetAxis(dof);
1666 RAVELOG_WARN(
"ComputeJacobianTranslation joint %d not supported\n", pjoint->GetType());
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;
1677 vjacobian[dofindex+dof] += v.x; vjacobian[dofstride+dofindex+dof] += v.y; vjacobian[2*dofstride+dofindex+dof] += v.z;
1685 for(
int idof = 0; idof < pjoint->GetDOF(); ++idof) {
1686 if( pjoint->IsMimic(idof) ) {
1687 bool bhas = dofindices.size() == 0;
1689 FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
1690 if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
1698 if( pjoint->IsRevolute(idof) ) {
1699 vaxis = pjoint->GetAxis(idof).cross(position-pjoint->GetAnchor());
1701 else if( pjoint->IsPrismatic(idof) ) {
1702 vaxis = pjoint->GetAxis(idof);
1705 RAVELOG_WARN(
"ComputeJacobianTranslation joint %d not supported\n", pjoint->GetType());
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() ) {
1717 index = itindex-dofindices.begin();
1719 vjacobian[index] += v.x;
1720 vjacobian[dofstride+index] += v.y;
1721 vjacobian[2*dofstride+index] += v.z;
1733 mjacobian.resize(boost::extents[3][
GetDOF()]);
1737 std::vector<dReal> vjacobian;
1740 vector<dReal>::const_iterator itsrc = vjacobian.begin();
1741 FOREACH(itdst,mjacobian) {
1742 std::copy(itsrc,itsrc+
GetDOF(),itdst->begin());
1751 int dofstride =
GetDOF();
1752 vjacobian.resize(4*dofstride);
1753 if( dofstride == 0 ) {
1756 std::fill(vjacobian.begin(),vjacobian.end(),0);
1758 int offset = linkindex*
_veclinks.size();
1760 std::vector<std::pair<int,dReal> > vpartials;
1761 std::map< std::pair<Mimic::DOFFormat, int>,
dReal > mapcachedpartials;
1767 int dofindex = pjoint->GetDOFIndex();
1768 int8_t affect =
DoesAffect(pjoint->GetJointIndex(), linkindex);
1769 for(
int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1771 RAVELOG_WARN(str(boost::format(
"link %s should be affected by joint %s")%
_veclinks.at(linkindex)->GetName()%pjoint->GetName()));
1774 if( pjoint->IsRevolute(dof) ) {
1775 v = pjoint->GetAxis(dof);
1777 else if( pjoint->IsPrismatic(dof) ) {
1781 RAVELOG_WARN(
"CalculateRotationJacobian joint %d not supported\n", pjoint->GetType());
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);
1794 for(
int idof = 0; idof < pjoint->GetDOF(); ++idof) {
1795 if( pjoint->IsMimic(idof) ) {
1797 if( pjoint->IsRevolute(idof) ) {
1798 vaxis = pjoint->GetAxis(idof);
1800 else if( pjoint->IsPrismatic(idof) ) {
1804 RAVELOG_WARN(
"CalculateRotationJacobian joint %d not supported\n", pjoint->GetType());
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);
1825 mjacobian.resize(boost::extents[4][
GetDOF()]);
1829 std::vector<dReal> vjacobian;
1832 vector<dReal>::const_iterator itsrc = vjacobian.begin();
1833 FOREACH(itdst,mjacobian) {
1834 std::copy(itsrc,itsrc+
GetDOF(),itdst->begin());
1844 if( dofindices.size() > 0 ) {
1845 dofstride = dofindices.size();
1850 vjacobian.resize(3*dofstride);
1851 if( dofstride == 0 ) {
1854 std::fill(vjacobian.begin(),vjacobian.end(),0);
1857 int offset = linkindex*
_veclinks.size();
1859 std::vector<std::pair<int,dReal> > vpartials;
1860 std::map< std::pair<Mimic::DOFFormat, int>,
dReal > mapcachedpartials;
1866 int dofindex = pjoint->GetDOFIndex();
1867 int8_t affect =
DoesAffect(pjoint->GetJointIndex(), linkindex);
1868 for(
int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1870 if( pjoint->IsRevolute(dof) ) {
1871 v = pjoint->GetAxis(dof);
1873 else if( pjoint->IsPrismatic(dof) ) {
1877 RAVELOG_WARN(
"ComputeJacobianAxisAngle joint %d not supported\n", pjoint->GetType());
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;
1888 vjacobian[dofindex+dof] += v.x; vjacobian[dofstride+dofindex+dof] += v.y; vjacobian[2*dofstride+dofindex+dof] += v.z;
1896 for(
int idof = 0; idof < pjoint->GetDOF(); ++idof) {
1897 if( pjoint->IsMimic(idof) ) {
1898 bool bhas = dofindices.size() == 0;
1900 FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
1901 if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
1909 if( pjoint->IsRevolute(idof) ) {
1910 vaxis = pjoint->GetAxis(idof);
1912 else if( pjoint->IsPrismatic(idof) ) {
1916 RAVELOG_WARN(
"ComputeJacobianAxisAngle joint %d not supported\n", pjoint->GetType());
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() ) {
1928 index = itindex-dofindices.begin();
1930 vjacobian[index] += v.x;
1931 vjacobian[dofstride+index] += v.y;
1932 vjacobian[2*dofstride+index] += v.z;
1944 mjacobian.resize(boost::extents[3][
GetDOF()]);
1948 std::vector<dReal> vjacobian;
1951 vector<dReal>::const_iterator itsrc = vjacobian.begin();
1952 FOREACH(itdst,mjacobian) {
1953 std::copy(itsrc,itsrc+
GetDOF(),itdst->begin());
1963 if( dofindices.size() > 0 ) {
1964 dofstride = dofindices.size();
1969 hessian.resize(dofstride*3*dofstride);
1970 if( dofstride == 0 ) {
1973 std::fill(hessian.begin(),hessian.end(),0);
1975 int offset = linkindex*
_veclinks.size();
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;
1988 int dofindex = pjoint->GetDOFIndex();
1989 int8_t affect =
DoesAffect(pjoint->GetJointIndex(), linkindex);
1990 for(
int dof = 0; dof < pjoint->GetDOF(); ++dof) {
1992 RAVELOG_WARN(str(boost::format(
"link %s should be affected by joint %s")%
_veclinks.at(linkindex)->GetName()%pjoint->GetName()));
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();
2006 if( pjoint->IsRevolute(dof) ) {
2007 vaxes.push_back(pjoint->GetAxis(dof));
2008 vjacobian.push_back(pjoint->GetAxis(dof).cross(position-pjoint->GetAnchor()));
2010 else if( pjoint->IsPrismatic(dof) ) {
2011 vaxes.push_back(
Vector());
2012 vjacobian.push_back(pjoint->GetAxis(dof));
2015 vaxes.push_back(
Vector());
2016 vjacobian.push_back(
Vector());
2017 RAVELOG_WARN(
"ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2019 vinsertedindices.push_back(index);
2026 for(
int idof = 0; idof < pjoint->GetDOF(); ++idof) {
2027 if( pjoint->IsMimic(idof) ) {
2028 bool bhas = dofindices.size() == 0;
2030 FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
2031 if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
2039 if( pjoint->IsRevolute(idof) ) {
2040 vaxes.push_back(pjoint->GetAxis(idof));
2041 vjacobian.push_back(pjoint->GetAxis(idof).cross(position-pjoint->GetAnchor()));
2043 else if( pjoint->IsPrismatic(idof) ) {
2044 vjacobian.push_back(pjoint->GetAxis(idof));
2045 vaxes.push_back(
Vector());
2048 vaxes.push_back(
Vector());
2049 vjacobian.push_back(
Vector());
2050 RAVELOG_WARN(
"ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2052 PartialInfo& partialinfo = mappartialsinserted[vinsertedindices.size()];
2053 partialinfo.first.resize(vinsertedindices.size());
2054 pjoint->_ComputePartialVelocities(partialinfo.second,idof,mapcachedpartials);
2055 vinsertedindices.push_back(-1);
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() ) {
2073 index = itindex-dofindices.begin();
2076 for(
size_t j = 0; j < i; ++j) {
2077 Vector v = partialinfo.first.at(j)*itpartial->second;
2078 if( vinsertedindices[j] < 0 ) {
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() ) {
2088 index2 = itindex-dofindices.begin();
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;
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;
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;
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;
2120 for(
size_t j = i; j < vaxes.size(); ++j) {
2121 Vector v = vaxes[i].cross(vjacobian[j]);
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;
2131 if( vinsertedindices[j] < 0 ) {
2133 if( itpartial == partialinfo.second.begin() ) {
2134 mappartialsinserted[j].first.at(i) += v;
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;
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;
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;
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;
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;
2184 if( dofindices.size() > 0 ) {
2185 dofstride = dofindices.size();
2190 hessian.resize(dofstride*3*dofstride);
2191 if( dofstride == 0 ) {
2194 std::fill(hessian.begin(),hessian.end(),0);
2196 int offset = linkindex*
_veclinks.size();
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;
2209 int dofindex = pjoint->GetDOFIndex();
2210 int8_t affect =
DoesAffect(pjoint->GetJointIndex(), linkindex);
2211 for(
int dof = 0; dof < pjoint->GetDOF(); ++dof) {
2213 RAVELOG_WARN(str(boost::format(
"link %s should be affected by joint %s")%
_veclinks.at(linkindex)->GetName()%pjoint->GetName()));
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();
2227 if( pjoint->IsRevolute(dof) ) {
2228 vaxes.push_back(pjoint->GetAxis(dof));
2230 else if( pjoint->IsPrismatic(dof) ) {
2231 vaxes.push_back(
Vector());
2234 vaxes.push_back(
Vector());
2235 RAVELOG_WARN(
"ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2237 vinsertedindices.push_back(index);
2244 for(
int idof = 0; idof < pjoint->GetDOF(); ++idof) {
2245 if( pjoint->IsMimic(idof) ) {
2246 bool bhas = dofindices.size() == 0;
2248 FOREACHC(itmimicdof, pjoint->_vmimic[idof]->_vmimicdofs) {
2249 if( find(dofindices.begin(),dofindices.end(),itmimicdof->dofindex) != dofindices.end() ) {
2257 if( pjoint->IsRevolute(idof) ) {
2258 vaxes.push_back(pjoint->GetAxis(idof));
2260 else if( pjoint->IsPrismatic(idof) ) {
2261 vaxes.push_back(
Vector());
2264 vaxes.push_back(
Vector());
2265 RAVELOG_WARN(
"ComputeHessianTranslation joint %d not supported\n", pjoint->GetType());
2267 PartialInfo& partialinfo = mappartialsinserted[vinsertedindices.size()];
2268 partialinfo.first.resize(vinsertedindices.size());
2269 pjoint->_ComputePartialVelocities(partialinfo.second,idof,mapcachedpartials);
2270 vinsertedindices.push_back(-1);
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() ) {
2288 index = itindex-dofindices.begin();
2291 for(
size_t j = 0; j < i; ++j) {
2292 Vector v = partialinfo.first.at(j)*itpartial->second;
2293 if( vinsertedindices[j] < 0 ) {
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() ) {
2303 index2 = itindex-dofindices.begin();
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;
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;
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;
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;
2335 for(
size_t j = i+1; j < vaxes.size(); ++j) {
2336 Vector v = vaxes[i].cross(vaxes[j]);
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;
2346 if( vinsertedindices[j] < 0 ) {
2348 if( itpartial == partialinfo.second.begin() ) {
2349 mappartialsinserted[j].first.at(i) += v;
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;
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;
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;
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;
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;
2393 doftorques.resize(
GetDOF());
2398 Vector vgravity =
GetEnv()->GetPhysicsEngine()->GetGravity();
2399 std::vector<dReal> vDOFVelocities;
2400 std::vector<pair<Vector, Vector> > vLinkVelocities, vLinkAccelerations;
2403 bool bHasVelocity =
false;
2404 FOREACH(it,vDOFVelocities) {
2405 if(
RaveFabs(*it) > g_fEpsilonLinear ) {
2406 bHasVelocity =
true;
2410 if( !bHasVelocity ) {
2411 vDOFVelocities.resize(0);
2420 std::vector<Vector> vLinkCOMLinearAccelerations(
_veclinks.size()), vLinkCOMMomentOfInertia(
_veclinks.size());
2421 for(
size_t i = 0; i < vLinkVelocities.size(); ++i) {
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));
2427 vLinkCOMMomentOfInertia[i] = tm.rotate(vangularaccel) + vangularvelocity.cross(tm.rotate(vangularvelocity));
2431 std::vector< std::pair<Vector, Vector> > vLinkForceTorques(
_veclinks.size());
2432 FOREACHC(it,mapExternalForceTorque) {
2433 vLinkForceTorques.at(it->first) = it->second;
2435 std::fill(doftorques.begin(),doftorques.end(),0);
2437 std::vector<std::pair<int,dReal> > vpartials;
2438 std::map< std::pair<Mimic::DOFFormat, int>,
dReal > mapcachedpartials;
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);
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);
2454 Vector vcomtoanchor = pjoint->GetHierarchyChildLink()->GetGlobalCOM() - pjoint->GetAnchor();
2455 if( pjoint->GetDOFIndex() >= 0 ) {
2457 doftorques.at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2460 doftorques.at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vcomforce);
2466 else if( pjoint->IsMimic(0) ) {
2471 faxistorque = pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2474 faxistorque = pjoint->GetAxis(0).dot3(vcomforce);
2480 pjoint->_ComputePartialVelocities(vpartials,0,mapcachedpartials);
2481 FOREACH(itpartial,vpartials) {
2482 int dofindex = itpartial->first;
2483 doftorques.at(dofindex) += itpartial->second*faxistorque;
2488 BOOST_ASSERT(pjoint->IsStatic());
2496 FOREACH(itdoftorques,vDOFTorqueComponents) {
2497 itdoftorques->resize(
GetDOF());
2503 Vector vgravity =
GetEnv()->GetPhysicsEngine()->GetGravity();
2504 std::vector<dReal> vDOFVelocities;
2505 boost::array< std::vector<pair<Vector, Vector> >, 3> vLinkVelocities;
2506 boost::array< std::vector<pair<Vector, Vector> >, 3> vLinkAccelerations;
2507 boost::array<int,3> linkaccelsimilar = {{-1,-1,-1}};
2509 vLinkVelocities[0].resize(
_veclinks.size());
2512 bool bHasVelocity =
false;
2513 FOREACH(it,vDOFVelocities) {
2514 if(
RaveFabs(*it) > g_fEpsilonLinear ) {
2515 bHasVelocity =
true;
2519 if( !bHasVelocity ) {
2520 vDOFVelocities.resize(0);
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 ) {
2533 vLinkVelocities[2].resize(
_veclinks.size());
2535 for(
size_t i = 1; i < vLinkVelocities[0].size(); ++i) {
2537 vLinkVelocities[2][i].first = vbaselinear + vbaseangular.cross(voffset);
2538 vLinkVelocities[2][i].second = vbaseangular;
2542 vLinkVelocities[2] = vLinkVelocities[0];
2544 _ComputeLinkAccelerations(std::vector<dReal>(), std::vector<dReal>(), vLinkVelocities[2], vLinkAccelerations[2], vgravity);
2545 if( bHasVelocity ) {
2547 if( vDOFAccelerations.size() > 0 ) {
2551 linkaccelsimilar[0] = 1;
2555 if( vDOFAccelerations.size() > 0 ) {
2562 vLinkVelocities[2] = vLinkVelocities[0];
2563 if( bHasVelocity ) {
2565 if( vDOFAccelerations.size() > 0 ) {
2569 linkaccelsimilar[0] = 1;
2573 if( vDOFAccelerations.size() > 0 ) {
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());
2589 for(
size_t i = 0; i <
_veclinks.size(); ++i) {
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));
2602 FOREACH(itdoftorques,vDOFTorqueComponents) {
2603 std::fill(itdoftorques->begin(),itdoftorques->end(),0);
2607 vLinkForceTorques[2].resize(
_veclinks.size());
2608 FOREACHC(it,mapExternalForceTorque) {
2609 vLinkForceTorques[2].at(it->first) = it->second;
2612 std::vector<std::pair<int,dReal> > vpartials;
2613 std::map< std::pair<Mimic::DOFFormat, int>,
dReal > mapcachedpartials;
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();
2626 bool bIsMimic = pjoint->GetDOFIndex() < 0 && pjoint->IsMimic(0);
2628 pjoint->_ComputePartialVelocities(vpartials,0,mapcachedpartials);
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 ) {
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);
2644 if( parentindex >= 0 ) {
2645 vLinkForceTorques[j].at(parentindex).first += vcomforce;
2646 vLinkForceTorques[j].at(parentindex).second += vjointtorque + vchildcomtoparentcom.cross(vcomforce);
2649 if( pjoint->GetDOFIndex() >= 0 ) {
2651 vDOFTorqueComponents[j].at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2654 vDOFTorqueComponents[j].at(pjoint->GetDOFIndex()) += pjoint->GetAxis(0).dot3(vcomforce);
2660 else if( bIsMimic ) {
2665 faxistorque = pjoint->GetAxis(0).dot3(vjointtorque + vcomtoanchor.cross(vcomforce));
2668 faxistorque = pjoint->GetAxis(0).dot3(vcomforce);
2674 FOREACH(itpartial,vpartials) {
2675 int dofindex = itpartial->first;
2676 vDOFTorqueComponents[j].at(dofindex) += itpartial->second*faxistorque;
2681 BOOST_ASSERT(pjoint->IsStatic());
2691 vLinkAccelerations.resize(0);
2694 std::vector<dReal> vDOFVelocities;
2695 std::vector<pair<Vector, Vector> > vLinkVelocities;
2705 dofvelocities.resize(
GetDOF());
2706 if( !usebaselinkvelocity &&
_veclinks.size() > 0 ) {
2707 vLinkVelocities[0].first =
Vector();
2708 vLinkVelocities[0].second =
Vector();
2712 if( !usebaselinkvelocity ) {
2715 for(
size_t i = 1; i < vLinkVelocities.size(); ++i) {
2717 vLinkVelocities[i].first -= vLinkVelocities[0].first + vLinkVelocities[0].second.cross(voffset);
2718 vLinkVelocities[i].second -= vLinkVelocities[0].second;
2720 vLinkVelocities[0].first =
Vector();
2721 vLinkVelocities[0].second =
Vector();
2723 dofvelocities.resize(0);
2724 if( (
int)dofvelocities.capacity() <
GetDOF() ) {
2725 dofvelocities.reserve(
GetDOF());
2728 int parentindex = 0;
2729 if( !!(*it)->_attachedbodies[0] ) {
2730 parentindex = (*it)->_attachedbodies[0]->GetIndex();
2732 int childindex = (*it)->_attachedbodies[0]->GetIndex();
2733 (*it)->_GetVelocities(dofvelocities,
true,vLinkVelocities.at(parentindex),vLinkVelocities.at(childindex));
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
2739 vLinkAccelerations.resize(
_veclinks.size());
2744 vector<dReal> vtempvalues, veval;
2745 boost::array<dReal,3> dummyvelocities = {{0,0,0}}, dummyaccelerations={{0,0,0}};
2748 vLinkAccelerations.at(0).first = -vGravity + vLinkVelocities.at(0).second.cross(vLinkVelocities.at(0).first);
2749 vLinkAccelerations.at(0).second =
Vector();
2754 if( vDOFAccelerations.size() > 0 ) {
2757 if( vDOFVelocities.size() > 0 ) {
2769 std::vector<uint8_t> vlinkscomputed(
_veclinks.size(),0);
2770 vlinkscomputed[0] = 1;
2776 int dofindex = pjoint->GetDOFIndex();
2779 const dReal* pdofaccelerations=NULL, *pdofvelocities=NULL;
2780 if( dofindex >= 0 ) {
2781 if( vDOFAccelerations.size() ) {
2782 pdofaccelerations = &vDOFAccelerations.at(dofindex);
2784 if( vDOFVelocities.size() > 0 ) {
2785 pdofvelocities=&vDOFVelocities.at(dofindex);
2788 if( pjoint->IsMimic() && (vDOFAccelerations.size() > 0 || vDOFVelocities.size() > 0) ) {
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) {
2796 vtempvalues.push_back(pj->GetValue(itdof->axis));
2798 dummyvelocities[i] = 0;
2799 dummyaccelerations[i] = 0;
2802 if( vDOFVelocities.size() > 0 ) {
2803 int err = pjoint->_Eval(i,1,vtempvalues,veval);
2805 RAVELOG_WARN(str(boost::format(
"failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
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);
2814 partialvelocity = vPassiveJointVelocities.at(vdofformat[ipartial].jointindex-
_vecjoints.size()).at(vdofformat[ipartial].axis);
2816 dummyvelocities[i] += veval.at(ipartial) * partialvelocity;
2820 if( dofindex < 0 ) {
2821 vPassiveJointVelocities.at(jointindex-(
int)
_vecjoints.size()).at(i) = dummyvelocities[i];
2826 if( vDOFAccelerations.size() > 0 ) {
2827 int err = pjoint->_Eval(i,2,vtempvalues,veval);
2829 RAVELOG_WARN(str(boost::format(
"failed to evaluate joint %s, fparser error %d")%pjoint->GetName()%err));
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);
2838 partialacceleration = vPassiveJointAccelerations.at(vdofformat[ipartial].jointindex-
_vecjoints.size()).at(vdofformat[ipartial].axis);
2840 dummyaccelerations[i] += veval.at(ipartial) * partialacceleration;
2844 if( dofindex < 0 ) {
2845 vPassiveJointAccelerations.at(jointindex-(
int)
_vecjoints.size()).at(i) = dummyaccelerations[i];
2849 else if( dofindex >= 0 ) {
2851 dummyvelocities[i] = vDOFVelocities.at(dofindex+i);
2852 dummyaccelerations[i] = vDOFAccelerations.at(dofindex+i);
2856 dummyvelocities[i] = vPassiveJointVelocities.at(jointindex-(
int)
_vecjoints.size()).at(i);
2857 dummyaccelerations[i] = vPassiveJointAccelerations.at(jointindex-(
int)
_vecjoints.size()).at(i);
2860 pdofvelocities = &dummyvelocities[0];
2861 pdofaccelerations = &dummyaccelerations[0];
2865 if( vlinkscomputed[pjoint->GetHierarchyChildLink()->GetIndex()] ) {
2869 if( vDOFVelocities.size() > 0 && !pdofvelocities ) {
2871 pdofvelocities = &vPassiveJointVelocities.at(jointindex-(
int)
_vecjoints.size()).at(0);
2873 if( vDOFAccelerations.size() > 0 && !pdofaccelerations ) {
2875 pdofaccelerations = &vPassiveJointAccelerations.at(jointindex-(
int)
_vecjoints.size()).at(0);
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);
2883 int parentindex = 0;
2884 if( !!pjoint->GetHierarchyParentLink() ) {
2885 parentindex = pjoint->GetHierarchyParentLink()->GetIndex();
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);
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);
2920 if( !!pdofaccelerations ) {
2921 Vector gdw = tdelta.rotate(vlocalaxis*pdofaccelerations[0]);
2922 vChildAccelerations.first += gdw.cross(tchild.trans-tdelta.trans);
2923 vChildAccelerations.second += gdw;
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];
2933 vChildAccelerations.first += vParentVelocities.second.cross(angularveloctiycontrib);
2934 if( !!pdofaccelerations ) {
2935 vChildAccelerations.first += w*pdofaccelerations[0];
2937 vChildAccelerations.second = vParentAccelerations.second;
2942 vlinkscomputed[childindex] = 1;
2950 RAVELOG_VERBOSE(str(boost::format(
"Self collision: %s\n")%report->__str__()));
2964 (*itlink)->_index = lindex;
2965 (*itlink)->_vParentLinks.clear();
2966 if((
_veclinks.size() > 1)&&((*itlink)->GetName().size() == 0)) {
2976 bool bmimic =
false;
2977 for(
int idof = 0; idof < (*itjoint)->GetDOF(); ++idof) {
2978 if( !!(*itjoint)->_vmimic[idof] ) {
2982 if( !bmimic && (*itjoint)->_info._bIsActive ) {
2993 bool bmimic =
false;
2994 for(
int idof = 0; idof < (*itjoint)->GetDOF(); ++idof) {
2995 if( !!(*itjoint)->_vmimic[idof] ) {
3000 if( bmimic || !(*itjoint)->_info._bIsActive ) {
3011 (*itjoint)->jointindex = jointindex++;
3012 (*itjoint)->dofindex = dofindex;
3013 (*itjoint)->_info._bIsActive =
true;
3014 dofindex += (*itjoint)->GetDOF();
3017 (*itjoint)->jointindex = -1;
3018 (*itjoint)->dofindex = -1;
3019 (*itjoint)->_info._bIsActive =
false;
3024 vector<int> vJointIndices(
_vecjoints.size());
3026 for(
size_t i = 0; i <
_vecjoints.size(); ++i) {
3028 for(
int idof = 0; idof <
_vecjoints[i]->GetDOF(); ++idof) {
3035 FOREACH(it,vorder) {
3041 for(
int ijoints = 0; ijoints < 2; ++ijoints) {
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];
3047 (*itjoint)->SetMimicEquations(i,poseq,veleq,acceleq);
3053 std::map<Mimic::DOFFormat, boost::shared_ptr<Mimic> > mapmimic;
3054 for(
int ijoints = 0; ijoints < 2; ++ijoints) {
3057 FOREACH(itjoint,vjoints) {
3064 dofformat.
dofindex = (*itjoint)->GetDOFIndex();
3065 dofformat.
jointindex = (*itjoint)->GetJointIndex();
3067 for(
int idof = 0; idof < (*itjoint)->GetDOF(); ++idof) {
3068 dofformat.
axis = idof;
3069 if( !!(*itjoint)->_vmimic[idof] ) {
3071 FOREACH(itdofformat,(*itjoint)->_vmimic[idof]->_vdofformat) {
3073 if( pjoint->IsMimic(itdofformat->axis) ) {
3074 mapmimic[dofformat] = (*itjoint)->_vmimic[idof];
3083 bool bchanged =
true;
3086 FOREACH(itmimic,mapmimic) {
3087 boost::shared_ptr<Mimic> mimic = itmimic->second;
3090 FOREACH(itdofformat,mimic->_vdofformat) {
3091 if( mapmimic.find(*itdofformat) == mapmimic.end() ) {
3094 boost::shared_ptr<Mimic> mimicparent = mapmimic[*itdofformat];
3095 FOREACH(itmimicdof, mimicparent->_vmimicdofs) {
3096 if( mimicparent->_vdofformat[itmimicdof->dofformatindex] == itmimic->first ) {
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);
3102 if( find(mimic->_vmimicdofs.begin(),mimic->_vmimicdofs.end(),h) == mimic->_vmimicdofs.end() ) {
3103 mimic->_vmimicdofs.push_back(h);
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) {
3116 FOREACH(itjoint,vjoints) {
3117 for(
int i = 0; i < (*itjoint)->GetDOF(); ++i) {
3118 (*itjoint)->_vmimic[i].reset();
3137 for(
size_t i = 0; i <
_veclinks.size(); ++i) {
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());
3145 index = (*itjoint)->GetSecondAttached()->GetIndex()*
_veclinks.size()+(*itjoint)->GetFirstAttached()->GetIndex();
3146 _vAllPairsShortestPaths[index] = std::pair<int16_t,int16_t>((*itjoint)->GetSecondAttached()->GetIndex(),(*itjoint)->GetJointIndex());
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);
3156 index = (*itjoint)->GetSecondAttached()->GetIndex()*
_veclinks.size()+(*itjoint)->GetFirstAttached()->GetIndex();
3157 _vAllPairsShortestPaths[index] = std::pair<int16_t,int16_t>((*itjoint)->GetSecondAttached()->GetIndex(),jointindex);
3162 for(
size_t k = 0; k <
_veclinks.size(); ++k) {
3163 for(
size_t i = 0; i <
_veclinks.size(); ++i) {
3167 for(
size_t j = 0; j <
_veclinks.size(); ++j) {
3168 if((j == i)||(j == k)) {
3172 if( vcosts[j*
_veclinks.size()+i] > kcost ) {
3184 std::vector< std::vector<int> > vlinkadjacency(
_veclinks.size());
3187 vlinkadjacency.at((*itjoint)->GetFirstAttached()->GetIndex()).push_back((*itjoint)->GetSecondAttached()->GetIndex());
3188 vlinkadjacency.at((*itjoint)->GetSecondAttached()->GetIndex()).push_back((*itjoint)->GetFirstAttached()->GetIndex());
3191 vlinkadjacency.at((*itjoint)->GetFirstAttached()->GetIndex()).push_back((*itjoint)->GetSecondAttached()->GetIndex());
3192 vlinkadjacency.at((*itjoint)->GetSecondAttached()->GetIndex()).push_back((*itjoint)->GetFirstAttached()->GetIndex());
3194 FOREACH(it,vlinkadjacency) {
3195 sort(it->begin(), it->end());
3199 std::vector< std::list< std::list<int> > > vuniquepaths(
_veclinks.size());
3200 std::list< std::list<int> > closedloops;
3202 std::list< std::list<int> > S;
3203 FOREACH(itv,vlinkadjacency[s]) {
3208 vuniquepaths[*itv].push_back(P);
3211 std::list<int>& P = S.front();
3213 FOREACH(itv,vlinkadjacency[u]) {
3214 std::list<int>::iterator itfound = find(P.begin(),P.end(),*itv);
3215 if( itfound == P.end() ) {
3217 S.back().push_back(*itv);
3218 vuniquepaths[*itv].push_back(S.back());
3222 std::list<int> cycle;
3223 while(itfound != P.end()) {
3224 cycle.push_back(*itfound);
3227 if( cycle.size() > 2 ) {
3230 itfound = cycle.begin();
3231 std::list<int>::iterator itmin = itfound++;
3232 while(itfound != cycle.end()) {
3233 if( *itmin > *itfound ) {
3238 if( itmin != cycle.begin() ) {
3239 cycle.splice(cycle.end(),cycle,cycle.begin(),itmin);
3241 if( *++cycle.begin() > cycle.back() ) {
3244 cycle.push_front(cycle.back());
3247 if( find(closedloops.begin(),closedloops.end(),cycle) == closedloops.end() ) {
3248 closedloops.push_back(cycle);
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()));
3260 FOREACH(itpath, vuniquepaths.at((*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);
3269 vector<int> vlinkdepths(
_veclinks.size(),-1);
3270 vlinkdepths.at(0) = 0;
3271 for(
size_t i = 0; i <
_veclinks.size(); ++i) {
3276 bool changed =
true;
3280 if( vlinkdepths[(*itlink)->GetIndex()] == -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;
3289 if( bestindex >= 0 ) {
3290 vlinkdepths[(*itlink)->GetIndex()] = bestindex;
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() <<
", ";
3312 vector<int> vjointadjacency(numjoints*numjoints,0);
3313 for(
int ij0 = 0; ij0 < numjoints; ++ij0) {
3315 bool bj0hasstatic = (!j0->GetFirstAttached() || j0->GetFirstAttached()->IsStatic()) || (!j0->GetSecondAttached() || j0->GetSecondAttached()->IsStatic());
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;
3329 for(
int ij1 = ij0+1; ij1 < numjoints; ++ij1) {
3331 bool bj1hasstatic = (!j1->GetFirstAttached() || j1->GetFirstAttached()->IsStatic()) || (!j1->GetSecondAttached() || j1->GetSecondAttached()->IsStatic());
3334 if( bj0hasstatic && bj1hasstatic ) {
3337 if( vjointadjacency[ij1*numjoints+ij0] || vjointadjacency[ij0*numjoints+ij1] ) {
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);
3350 vjointadjacency[ij0*numjoints+ij1] = 100-min(j0l0,j0l1);
3355 vjointadjacency[ij1*numjoints+ij0] = 100-min(j1l0,j1l1);
3358 diff = max(j0l0,j0l1) - max(j1l0,j1l1);
3361 vjointadjacency[ij0*numjoints+ij1] = 100-max(j0l0,j0l1);
3366 vjointadjacency[ij1*numjoints+ij0] = 100-max(j1l0,j1l1);
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] ) {
3382 if( !hasincoming ) {
3383 noincomingedges.push_back(i);
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] ) {
3403 if( !hasincoming ) {
3404 noincomingedges.push_back(i);
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;
3417 if( vjointadjacency[imaxadjind] != 0 ) {
3418 bcontinuesorting =
true;
3419 int ifirst = imaxadjind/numjoints;
3420 int isecond = imaxadjind%numjoints;
3421 if( vjointadjacency[imaxadjind] <= 2 ) {
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()));
3427 vjointadjacency[imaxadjind] = 0;
3428 bool hasincoming =
false;
3429 for(
int j = 0; j < numjoints; ++j) {
3430 if( vjointadjacency[j*numjoints+isecond] ) {
3435 if( !hasincoming ) {
3436 noincomingedges.push_back(isecond);
3452 int parentlinkindex = -1;
3453 if( !(*itjoint)->GetFirstAttached() || (*itjoint)->GetFirstAttached()->IsStatic() ) {
3454 if( !!(*itjoint)->GetSecondAttached() && !(*itjoint)->GetSecondAttached()->IsStatic() ) {
3455 parentlinkindex = (*itjoint)->GetSecondAttached()->GetIndex();
3458 else if( !(*itjoint)->GetSecondAttached() || (*itjoint)->GetSecondAttached()->IsStatic() ) {
3459 parentlinkindex = (*itjoint)->GetFirstAttached()->GetIndex();
3464 LinkPtr plink0 = (*itjoint)->GetFirstAttached(), plink1 = (*itjoint)->GetSecondAttached();
3465 if( vlinkdepths[plink0->GetIndex()] < vlinkdepths[plink1->GetIndex()] ) {
3466 parentlinkindex = plink0->GetIndex();
3468 else if( vlinkdepths[plink0->GetIndex()] > vlinkdepths[plink1->GetIndex()] ) {
3469 parentlinkindex = plink1->GetIndex();
3474 FOREACHC(itparentlink,plink0->_vParentLinks) {
3477 link0pos = min(link0pos,pos);
3479 FOREACHC(itparentlink,plink1->_vParentLinks) {
3482 link1pos = min(link1pos,pos);
3484 if( link0pos < link1pos ) {
3485 parentlinkindex = plink0->GetIndex();
3487 else if( link0pos > link1pos ) {
3488 parentlinkindex = plink1->GetIndex();
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));
3495 if( parentlinkindex == -1 ) {
3496 RAVELOG_WARN(str(boost::format(
"could not compute parent link for joint %s")%(*itjoint)->GetName()));
3498 else if( parentlinkindex != (*itjoint)->GetFirstAttached()->GetIndex() ) {
3499 RAVELOG_VERBOSE(str(boost::format(
"swapping link order of joint %s(%d)")%(*itjoint)->GetName()%(*itjoint)->GetJointIndex()));
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));
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);
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;
3528 for(
int j = 0; j < (int)
_veclinks.size(); ++j) {
3529 if( vusedlinks[j] &&(i != j)) {
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) {
3553 FOREACH(itclosedloop,closedloops) {
3556 _vClosedLoops.push_back(vector< std::pair<LinkPtr, JointPtr> >());
3559 FOREACH(itlinkindex,*itclosedloop) {
3578 ss <<
GetName() <<
" closedloop found: ";
3579 FOREACH(itlinkindex,*itclosedloop) {
3581 ss << plink->GetName() <<
"(" << plink->GetIndex() <<
") ";
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() ) {
3595 if( (*itlink)->IsStatic() ) {
3596 if( (*itlink)->GetIndex() != (int)ilink ) {
3597 vattachedlinks.push_back((*itlink)->GetIndex());
3602 if( (*itjoint)->IsStatic() ) {
3603 if( !(*itjoint)->GetFirstAttached() && !!(*itjoint)->GetSecondAttached() && !(*itjoint)->GetSecondAttached()->IsStatic() ) {
3604 vattachedlinks.push_back((*itjoint)->GetSecondAttached()->GetIndex());
3606 if( !(*itjoint)->GetSecondAttached() && !!(*itjoint)->GetFirstAttached() && !(*itjoint)->GetFirstAttached()->IsStatic() ) {
3607 vattachedlinks.push_back((*itjoint)->GetFirstAttached()->GetIndex());
3612 if( (*itpassive)->IsStatic() ) {
3613 if( !(*itpassive)->GetFirstAttached() && !!(*itpassive)->GetSecondAttached() && !(*itpassive)->GetSecondAttached()->IsStatic() ) {
3614 vattachedlinks.push_back((*itpassive)->GetSecondAttached()->GetIndex());
3616 if( !(*itpassive)->GetSecondAttached() && !!(*itpassive)->GetFirstAttached() && !(*itpassive)->GetFirstAttached()->IsStatic() ) {
3617 vattachedlinks.push_back((*itpassive)->GetFirstAttached()->GetIndex());
3624 for(
size_t icurlink = 0; icurlink<vattachedlinks.size(); ++icurlink) {
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());
3631 if(((*itjoint)->GetSecondAttached() == plink)&& !!(*itjoint)->GetFirstAttached() &&(find(vattachedlinks.begin(),vattachedlinks.end(),(*itjoint)->GetFirstAttached()->GetIndex()) == vattachedlinks.end())) {
3632 vattachedlinks.push_back((*itjoint)->GetFirstAttached()->GetIndex());
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());
3641 if(((*itpassive)->GetSecondAttached() == plink)&& !!(*itpassive)->GetFirstAttached() &&(find(vattachedlinks.begin(),vattachedlinks.end(),(*itpassive)->GetFirstAttached()->GetIndex()) == vattachedlinks.end())) {
3642 vattachedlinks.push_back((*itpassive)->GetFirstAttached()->GetIndex());
3649 for(
size_t ijoint = 0; ijoint <
_vecjoints.size(); ++ijoint ) {
3656 RAVELOG_WARN(str(boost::format(
"%s passive joint index %d has no name")%
GetName()%ijoint));
3663 if( pjoint0->GetName() == pjoint1->GetName() && (pjoint0->GetJointIndex() >= 0 || pjoint1->GetJointIndex() >= 0) ) {
3669 __hashkinematics.resize(0);
3677 if( !!pl0 && !!pl1 ) {
3678 int ind0 = pl0->GetIndex();
3679 int ind1 = pl1->GetIndex();
3691 if( (*itlink0)->GetGeometries().size() == 0 ) {
3692 int ind0 = (*itlink0)->GetIndex();
3694 if( *itlink0 != *itlink1 ) {
3695 int ind1 = (*itlink1)->GetIndex();
3709 int ind0 = (*itj)->_attachedbodies[0]->GetIndex();
3710 int ind1 = (*itj)->_attachedbodies[1]->GetIndex();
3720 int ind0 = (*itj)->_attachedbodies[0]->GetIndex();
3721 int ind1 = (*itj)->_attachedbodies[1]->GetIndex();
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) {
3735 size_t numstatic = 0;
3736 FOREACH(it,vjoints) {
3737 numstatic += (*it)->IsStatic();
3739 if( numstatic+1 >= vjoints.size() ) {
3755 vector<Transform> vprevtrans, vnewtrans;
3756 vector<int> vprevdofbranches, vnewdofbranches;
3758 vector<dReal> vcurrentvalues;
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])));
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)));
3783 ss <<
"joint_values " <<
GetName();
3784 for(
int i = 0; i <
GetDOF(); ++i) {
3787 group.
name = ss.str();
3790 offset += group.
dof;
3803 std::list<UserDataWeakPtr> listRegisteredCallbacks;
3808 FOREACH(it,listRegisteredCallbacks) {
3824 std::set<KinBodyConstPtr> dummy;
3833 if( !!pattached && setAttached.insert(pattached).second ) {
3834 pattached->GetAttached(setAttached);
3846 if( !!pattached && ((pattached == pbody)|| pattached->_IsAttached(pbody,setChecked)) ) {
3863 if( it->lock() == pbody ) {
3871 FOREACH(it,pbody->_listAttachedBodies) {
3872 if( it->lock() == pthisbody ) {
3873 pbody->_listAttachedBodies.erase(it);
3879 return numremoved==2;
3884 bool bchanged =
false;
3886 if( (*it)->_info._bIsEnabled != bEnable ) {
3887 (*it)->_info._bIsEnabled = bEnable;
3900 if((*it)->IsEnabled()) {
3909 bool bchanged =
false;
3911 FOREACH(itgeom,(*it)->_vGeometries) {
3912 if( (*itgeom)->IsVisible() != visible ) {
3913 (*itgeom)->_info._bVisible = visible;
3928 if((*it)->IsVisible()) {
3951 vector<int> vdofbranches;
3965 class TransformsSaver
3969 _pbody->GetLinkTransformations(vcurtrans, _vdofbranches);
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);
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);
3983 std::vector<Transform> vcurtrans;
3984 std::vector<int> _vdofbranches;
3993 for(
size_t i = 0; i <
_veclinks.size(); ++i) {
3997 for(
size_t i = 0; i <
_veclinks.size(); ++i) {
3998 for(
size_t j = i+1; j <
_veclinks.size(); ++j) {
4014 if( plink1->IsEnabled() && plink2->IsEnabled() ) {
4041 __hashkinematics = r->__hashkinematics;
4042 _vTempJoints = r->_vTempJoints;
4045 FOREACHC(itlink, r->_veclinks) {
4047 *pnewlink = **itlink;
4053 FOREACHC(itjoint, r->_vecjoints) {
4055 *pnewjoint = **itjoint;
4057 pnewjoint->_attachedbodies[0] =
_veclinks.at((*itjoint)->_attachedbodies[0]->GetIndex());
4058 pnewjoint->_attachedbodies[1] =
_veclinks.at((*itjoint)->_attachedbodies[1]->GetIndex());
4063 FOREACHC(itjoint, r->_vPassiveJoints) {
4065 *pnewjoint = **itjoint;
4067 pnewjoint->_attachedbodies[0] =
_veclinks.at((*itjoint)->_attachedbodies[0]->GetIndex());
4068 pnewjoint->_attachedbodies[1] =
_veclinks.at((*itjoint)->_attachedbodies[1]->GetIndex());
4073 FOREACHC(itjoint, r->_vTopologicallySortedJoints) {
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() ) {
4083 it = find(r->_vPassiveJoints.begin(), r->_vPassiveJoints.end(),*itjoint);
4084 if( it != r->_vPassiveJoints.end() ) {
4103 _vClosedLoops.push_back(std::vector< std::pair<LinkPtr,JointPtr> >());
4104 FOREACHC(it,*itloop) {
4107 std::vector<JointPtr>::const_iterator itjoint = find(r->_vecjoints.begin(),r->_vecjoints.end(),it->second);
4108 if( itjoint != r->_vecjoints.end() ) {
4112 itjoint = find(r->_vPassiveJoints.begin(), r->_vPassiveJoints.end(),it->second);
4113 if( itjoint != r->_vPassiveJoints.end() ) {
4124 FOREACHC(itatt, r->_listAttachedBodies) {
4154 vector<dReal> vzeros(
GetDOF(),0);
4160 FOREACH(it, hashproperties) {
4161 if( (parameters & *it) == *it ) {
4162 __hashkinematics.resize(0);
4167 std::list<UserDataWeakPtr> listRegisteredCallbacks;
4172 FOREACH(it,listRegisteredCallbacks) {
4174 if( !!pdata && (pdata->_properties & parameters) ) {
4189 (*it)->serialize(o,options);
4193 (*it)->serialize(o,options);
4197 (*it)->serialize(o,options);
4203 std::vector<Vector> vaxes;
4205 vaxes.resize((*itjoint)->GetDOF());
4206 for(
size_t i = 0; i < vaxes.size(); ++i) {
4207 vaxes[i] = (*itjoint)->GetInternalHierarchyLeftTransform().rotate((*itjoint)->GetInternalHierarchyAxis(i));
4209 (*itjoint)->_ComputeInternalInformation((*itjoint)->GetFirstAttached(), (*itjoint)->GetSecondAttached(),(*itjoint)->GetInternalHierarchyLeftTransform().trans,vaxes,std::vector<dReal>());
4216 if( __hashkinematics.size() == 0 ) {
4218 ss << std::fixed << std::setprecision(SERIALIZATION_PRECISION);
4222 return __hashkinematics;
4227 vector<dReal> vdofvalues(
GetDOF());
4229 std::copy(itvalues,itvalues+
GetDOF(),vdofvalues.begin());
4246 if( interpolation.size() == 0 ) {
4251 itgroup->interpolation=interpolation;
4260 if( indices.size() > 0 ) {
4263 ss <<
"joint_values " <<
GetName();
4264 FOREACHC(it,indices) {
4268 spec.
_vgroups[0].dof = indices.size();
4270 spec.
_vgroups[0].interpolation=interpolation;