17 #include "libopenrave.h"
19 #define CHECK_INTERNAL_COMPUTATION OPENRAVE_ASSERT_FORMAT(_nHierarchyComputed == 2, "robot %s internal structures need to be computed, current value is %d. Are you sure Environment::AddRobot/AddKinBody was called?", GetName()%_nHierarchyComputed, ORE_NotInitialized);
32 boost::function<void()> _fn;
45 pattachedlink.reset();
49 psensor->Clone(sensor.psensor,cloningoptions);
51 pdata = psensor->CreateSensorData();
55 int index =
LinkPtr(sensor.pattachedlink)->GetIndex();
56 if((index >= 0)&&(index < (
int)probot->GetLinks().size())) {
57 pattachedlink = probot->GetLinks().at(index);
67 if( psensor->GetSensorData(pdata) ) {
81 o << (pattachedlink.expired() ? -1 :
LinkPtr(pattachedlink)->GetIndex()) <<
" ";
82 SerializeRound(o,trelative);
83 o << (!pdata ? -1 : pdata->GetType()) <<
" ";
88 switch(prawgeom->GetType()) {
91 o << pgeom->
min_angle[0] <<
" " << pgeom->max_angle[0] <<
" " << pgeom->resolution[0] <<
" " << pgeom->max_range <<
" ";
96 o << pgeom->
KK.
fx <<
" " << pgeom->KK.fy <<
" " << pgeom->KK.cx <<
" " << pgeom->KK.cy <<
" " << pgeom->width <<
" " << pgeom->height <<
" ";
109 if( __hashstructure.size() == 0 ) {
111 ss << std::fixed << std::setprecision(SERIALIZATION_PRECISION);
115 return __hashstructure;
135 _RestoreRobot(_probot);
140 _RestoreRobot(!robot ? _probot : robot);
149 void RobotBase::RobotStateSaver::_RestoreRobot(boost::shared_ptr<RobotBase> probot)
154 if( probot->GetEnvironmentId() == 0 ) {
155 RAVELOG_WARN(str(boost::format(
"robot %s not added to environment, skipping restore")%_pbody->GetName()));
159 probot->SetActiveDOFs(vactivedofs, affinedofs, rotationaxis);
162 if( probot == _probot ) {
176 probot->ReleaseAllGrabbed();
179 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
180 KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
182 if( probot->GetEnv() == _probot->GetEnv() ) {
183 probot->_AttachBody(pbody);
184 probot->_vGrabbedBodies.push_back(*itgrabbed);
188 KinBodyPtr pnewbody = probot->GetEnv()->GetBodyFromEnvironmentId(pbody->GetEnvironmentId());
189 if( pbody->GetKinematicsGeometryHash() != pnewbody->GetKinematicsGeometryHash() ) {
190 RAVELOG_WARN(str(boost::format(
"body %s is not similar across environments")%pbody->GetName()));
193 GrabbedPtr pnewgrabbed(
new Grabbed(pnewbody,probot->GetLinks().at(
KinBody::LinkPtr(pgrabbed->_plinkrobot)->GetIndex())));
194 pnewgrabbed->_troot = pgrabbed->_troot;
195 pnewgrabbed->_listNonCollidingLinks.clear();
196 FOREACHC(itlinkref, pgrabbed->_listNonCollidingLinks) {
197 pnewgrabbed->_listNonCollidingLinks.push_back(probot->GetLinks().at((*itlinkref)->GetIndex()));
199 probot->_AttachBody(pnewbody);
200 probot->_vGrabbedBodies.push_back(pnewgrabbed);
257 bool RobotBase::Init(
const std::vector<KinBody::LinkInfoConstPtr>& linkinfos,
const std::vector<KinBody::JointInfoConstPtr>& jointinfos,
const std::vector<RobotBase::ManipulatorInfoConstPtr>& manipinfos,
const std::vector<RobotBase::AttachedSensorInfoConstPtr>& attachedsensorinfos)
263 FOREACHC(itmanipinfo, manipinfos) {
266 __hashrobotstructure.resize(0);
270 if( attachedsensorinfos.size() > 0 ) {
271 RAVELOG_WARN(
"currently do not support initializing from AttachedSensorInfo\n");
278 RAVELOG_DEBUG(
"default robot doesn't not support setting controllers (try GenericRobot)\n");
284 if(
_name != newname ) {
287 stringstream ss(itgroup->name);
288 string grouptype, oldname;
289 ss >> grouptype >> oldname;
292 itgroup->name = str(boost::format(
"%s %s %s")%grouptype%newname%buf.str());
298 void RobotBase::SetDOFValues(
const std::vector<dReal>& vJointValues, uint32_t bCheckLimits,
const std::vector<int>& dofindices)
356 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
357 KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
359 Transform t = pgrabbed->_plinkrobot->GetTransform();
360 pbody->SetTransform(t * pgrabbed->_troot);
362 std::pair<Vector, Vector> velocity = pgrabbed->_plinkrobot->GetVelocity();
363 velocity.first += velocity.second.cross(t.rotate(pgrabbed->_troot.trans));
364 pbody->SetVelocity(velocity.first, velocity.second);
377 if( !!(*itsensor)->psensor && !(*itsensor)->pattachedlink.expired() )
378 (*itsensor)->psensor->SetTransform(
LinkPtr((*itsensor)->pattachedlink)->GetTransform()*(*itsensor)->trelative);
497 FOREACHC(itj, vJointIndices) {
505 for(
size_t i = 0; i < vJointIndices.size(); ++i) {
522 ss <<
"joint_values " <<
GetName();
526 group.
name = ss.str();
585 if( values.size() == 0 ) {
588 vector<dReal>::iterator itvalues = values.begin();
592 *itvalues++ = _vTempRobotJoints[*it];
613 Vector linearvel, angularvel;
618 _veclinks.at(0)->GetVelocity(linearvel, angularvel);
627 angularvel.x = *pAffineValues++;
628 angularvel.y = *pAffineValues++;
629 angularvel.z = *pAffineValues++;
642 std::vector<dReal>::const_iterator itvel = velocities.begin();
644 _vTempRobotJoints[*it] = *itvel++;
663 if( velocities.size() == 0 )
665 dReal* pVelocities = &velocities[0];
669 *pVelocities++ = _vTempRobotJoints[*it];
676 Vector linearvel, angularvel;
677 _veclinks.at(0)->GetVelocity(linearvel, angularvel);
687 *pVelocities++ = angularvel.x;
688 *pVelocities++ = angularvel.y;
689 *pVelocities++ = angularvel.z;
703 dReal* pLowerLimit = &lower[0];
704 dReal* pUpperLimit = &upper[0];
705 vector<dReal> alllower,allupper;
715 *pLowerLimit++ = alllower[*it];
716 *pUpperLimit++ = allupper[*it];
724 *pLowerLimit++ = alllower[*it];
725 *pUpperLimit++ = allupper[*it];
758 *pLowerLimit++ = -fsin;
759 *pLowerLimit++ = -fsin;
760 *pLowerLimit++ = -fsin;
762 *pUpperLimit++ = fsin;
763 *pUpperLimit++ = fsin;
764 *pUpperLimit++ = fsin;
777 if( resolution.size() == 0 ) {
780 dReal* pResolution = &resolution[0];
784 *pResolution++ = _vTempRobotJoints[*it];
819 if( weights.size() == 0 ) {
822 dReal* pweight = &weights[0];
826 *pweight++ = _vTempRobotJoints[*it];
849 std::vector<dReal> dummy;
855 if( maxvel.size() == 0 ) {
858 dReal* pMaxVel = &maxvel[0];
862 *pMaxVel++ = _vTempRobotJoints[*it];
889 if( maxaccel.size() == 0 ) {
892 dReal* pMaxAccel = &maxaccel[0];
896 *pMaxAccel++ = _vTempRobotJoints[*it];
927 q1.at(index) = pjoint->SubtractValue(q1.at(index),q2.at(index),
_vActiveDOFIndices[index]-pjoint->GetDOFIndex());
931 q1.at(index) -= q2.at(index);
935 q1.at(index) -= q2.at(index);
939 q1.at(index) -= q2.at(index);
948 q1.at(index) -= q2.at(index); index++;
949 q1.at(index) -= q2.at(index); index++;
950 q1.at(index) -= q2.at(index); index++;
954 q1.at(index) -= q2.at(index); index++;
955 q1.at(index) -= q2.at(index); index++;
956 q1.at(index) -= q2.at(index); index++;
957 q1.at(index) -= q2.at(index); index++;
968 if( interpolation.size() == 0 ) {
973 itgroup->interpolation=interpolation;
986 vjacobian.resize(3*dofstride);
993 std::vector<dReal> vjacobianjoints;
995 for(
size_t i = 0; i < 3; ++i) {
1006 vjacobian[dofstride+ind] = 0;
1007 vjacobian[2*dofstride+ind] = 0;
1012 vjacobian[dofstride+ind] = 1;
1013 vjacobian[2*dofstride+ind] = 0;
1018 vjacobian[dofstride+ind] = 0;
1019 vjacobian[2*dofstride+ind] = 1;
1024 vjacobian[ind] = vj.x;
1025 vjacobian[dofstride+ind] = vj.y;
1026 vjacobian[2*dofstride+ind] = vj.z;
1034 dReal Qx = t.rot.x, Qy = t.rot.y, Qz = t.rot.z, Qw = t.rot.w;
1035 dReal Tx = offset.x-t.trans.x, Ty = offset.y-t.trans.y, Tz = offset.z-t.trans.z;
1038 dReal dRQ[12] = { 2*Qy*Ty+2*Qz*Tz, -4*Qy*Tx+2*Qx*Ty+2*Qw*Tz, -4*Qz*Tx-2*Qw*Ty+2*Qx*Tz, -2*Qz*Ty+2*Qy*Tz,
1039 2*Qy*Tx-4*Qx*Ty-2*Qw*Tz, 2*Qx*Tx+2*Qz*Tz, 2*Qw*Tx-4*Qz*Ty+2*Qy*Tz, 2*Qz*Tx-2*Qx*Tz,
1040 2*Qz*Tx+2*Qw*Ty-4*Qx*Tz, -2*Qw*Tx+2*Qz*Ty-4*Qy*Tz, 2*Qx*Tx+2*Qy*Ty, -2*Qy*Tx+2*Qx*Ty };
1043 dReal fsin = sqrt(t.rot.y * t.rot.y + t.rot.z * t.rot.z + t.rot.w * t.rot.w);
1044 dReal fcos = t.rot.x;
1045 dReal fangle = 2 * atan2(fsin, fcos);
1046 dReal normalizer = fangle / fsin;
1047 dReal Ax = normalizer * t.rot.y;
1048 dReal Ay = normalizer * t.rot.z;
1049 dReal Az = normalizer * t.rot.w;
1054 dReal fangle2 = fangle*fangle;
1055 dReal fiangle2 = 1/fangle2;
1056 dReal inormalizer = normalizer > 0 ? 1/normalizer : 0;
1057 dReal fconst = inormalizer*fiangle2;
1058 dReal fconst2 = fcos*fiangle2;
1059 dReal dQA[12] = { -0.5f*Ax*inormalizer, -0.5f*Ay*inormalizer, -0.5f*Az*inormalizer,
1060 inormalizer+0.5f*Ax*Ax*(fconst2-fconst), 0.5f*Ax*fconst2*Ay-Ax*fconst*Ay, 0.5f*Ax*fconst2*Az-Ax*fconst*Az,
1061 0.5f*Ax*fconst2*Ay-Ax*fconst*Ay, inormalizer+0.5f*Ay*Ay*(fconst2-fconst), 0.5f*Ay*fconst2*Az-Ay*fconst*Az,
1062 0.5f*Ax*fconst2*Az-Ax*fconst*Az, 0.5f*Ay*fconst2*Az-Ay*fconst*Az, inormalizer+0.5f*Az*Az*(fconst2-fconst)};
1064 for(
int i = 0; i < 3; ++i) {
1065 for(
int j = 0; j < 3; ++j) {
1066 vjacobian[i*dofstride+ind+j] = dRQ[4*i+0]*dQA[3*0+j] + dRQ[4*i+1]*dQA[3*1+j] + dRQ[4*i+2]*dQA[3*2+j] + dRQ[4*i+3]*dQA[3*3+j];
1075 dReal qw = t.rot[0], qx = t.rot[1], qy = t.rot[2], qz = t.rot[3];
1076 Vector offset_local = t.inverse() * offset;
1077 dReal x = offset_local.x, y = offset_local.y, z = offset_local.z;
1079 dReal dRQ[12] = {-2*qz*y + 2*qw*x + 2*qy*z,2*qx*x + 2*qy*y + 2*qz*z,-2*qy*x + 2*qw*z + 2*qx*y,-2*qw*y - 2*qz*x + 2*qx*z,-2*qx*z + 2*qw*y + 2*qz*x,-2*qw*z - 2*qx*y + 2*qy*x,2*qx*x + 2*qy*y + 2*qz*z,-2*qz*y + 2*qw*x + 2*qy*z,-2*qy*x + 2*qw*z + 2*qx*y,-2*qx*z + 2*qw*y + 2*qz*x,-2*qw*x - 2*qy*z + 2*qz*y,2*qx*x + 2*qy*y + 2*qz*z};
1080 for (
int i=0; i < 3; ++i) {
1081 double qdotrow = dRQ[4*i]*qw + dRQ[4*i+1]*qx + dRQ[4*i+2]*qy + dRQ[4*i+3]*qz;
1082 dRQ[4*i] -= qdotrow*qw;
1083 dRQ[4*i+1] -= qdotrow*qx;
1084 dRQ[4*i+2] -= qdotrow*qy;
1085 dRQ[4*i+3] -= qdotrow*qz;
1088 for(
int i = 0; i < 3; ++i) {
1089 for(
int j = 0; j < 4; ++j) {
1090 vjacobian[i*dofstride+ind + j] = dRQ[4*i+j];
1103 std::vector<dReal> vjacobian;
1107 vector<dReal>::const_iterator itsrc = vjacobian.begin();
1108 FOREACH(itdst,mjacobian) {
1121 vjacobian.resize(4*dofstride);
1123 std::vector<dReal> vjacobianjoints;
1127 vjacobian[dofstride+i] = vjacobianjoints[
GetDOF()+_vActiveDOFIndices[i]];
1128 vjacobian[2*dofstride+i] = vjacobianjoints[2*
GetDOF()+_vActiveDOFIndices[i]];
1129 vjacobian[3*dofstride+i] = vjacobianjoints[3*
GetDOF()+_vActiveDOFIndices[i]];
1140 vjacobian[dofstride+ind] = 0;
1141 vjacobian[2*dofstride+ind] = 0;
1142 vjacobian[3*dofstride+ind] = 0;
1147 vjacobian[dofstride+ind] = 0;
1148 vjacobian[2*dofstride+ind] = 0;
1149 vjacobian[3*dofstride+ind] = 0;
1154 vjacobian[dofstride+ind] = 0;
1155 vjacobian[2*dofstride+ind] = 0;
1156 vjacobian[3*dofstride+ind] = 0;
1161 vjacobian[ind] =
dReal(0.5)*(-q.y*v.x - q.z*v.y - q.w*v.z);
1162 vjacobian[dofstride+ind] =
dReal(0.5)*(q.x*v.x - q.z*v.z + q.w*v.y);
1163 vjacobian[2*dofstride+ind] =
dReal(0.5)*(q.x*v.y + q.y*v.z - q.w*v.x);
1164 vjacobian[3*dofstride+ind] =
dReal(0.5)*(q.x*v.z - q.y*v.y + q.z*v.x);
1183 std::vector<dReal> vjacobian;
1187 vector<dReal>::const_iterator itsrc = vjacobian.begin();
1188 FOREACH(itdst,mjacobian) {
1202 vjacobian.resize(3*dofstride);
1209 std::vector<dReal> vjacobianjoints;
1211 for(
size_t i = 0; i < 3; ++i) {
1222 vjacobian[dofstride+ind] = 0;
1223 vjacobian[2*dofstride+ind] = 0;
1228 vjacobian[dofstride+ind] = 0;
1229 vjacobian[2*dofstride+ind] = 0;
1234 vjacobian[dofstride+ind] = 0;
1235 vjacobian[2*dofstride+ind] = 0;
1240 vjacobian[ind] = v.x;
1241 vjacobian[dofstride+ind] = v.y;
1242 vjacobian[2*dofstride+ind] = v.z;
1254 dReal fnorm = t.rot.y*t.rot.y+t.rot.z*t.rot.z+t.rot.w*t.rot.w;
1257 vjacobian[ind] = t.rot.y*fnorm;
1258 vjacobian[dofstride+ind] = t.rot.z*fnorm;
1259 vjacobian[2*dofstride+ind] = t.rot.w*fnorm;
1263 vjacobian[dofstride+ind] = 0;
1264 vjacobian[2*dofstride+ind] = 0;
1277 std::vector<dReal> vjacobian;
1281 vector<dReal>::const_iterator itsrc = vjacobian.begin();
1282 FOREACH(itdst,mjacobian) {
1294 boost::array<uint8_t,4> compute={ { 0,0,0,0}};
1296 for(
size_t i = 0; i < compute.size(); ++i) {
1297 if( i & AO_Enabled ) {
1303 for(
size_t i = 0; i < compute.size(); ++i) {
1304 if( i & AO_ActiveDOFs ) {
1309 if( requestedoptions & ~(AO_Enabled|AO_ActiveDOFs) ) {
1314 if( compute.at(AO_Enabled) ) {
1318 if( plink1->IsEnabled() && plink2->IsEnabled() ) {
1323 if( compute.at(AO_ActiveDOFs) ) {
1334 if( compute.at(AO_Enabled|AO_ActiveDOFs) ) {
1338 if( plink1->IsEnabled() && plink2->IsEnabled() ) {
1354 return Grab(pbody, pmanip->GetEndEffector());
1363 return Grab(pbody, pmanip->GetEndEffector(), setRobotLinksToIgnore);
1376 GrabbedPtr pgrabbed(
new Grabbed(pbody,plink));
1378 Transform tbody = pbody->GetTransform();
1379 pgrabbed->_troot = t.inverse() * tbody;
1380 pgrabbed->_ProcessCollidingLinks(std::set<int>());
1382 std::pair<Vector, Vector> velocity = plink->GetVelocity();
1383 velocity.first += velocity.second.cross(tbody.trans - t.trans);
1384 pbody->SetVelocity(velocity.first, velocity.second);
1399 GrabbedPtr pgrabbed(
new Grabbed(pbody,pRobotLinkToGrabWith));
1400 Transform t = pRobotLinkToGrabWith->GetTransform();
1401 Transform tbody = pbody->GetTransform();
1402 pgrabbed->_troot = t.inverse() * tbody;
1403 pgrabbed->_ProcessCollidingLinks(setRobotLinksToIgnore);
1405 std::pair<Vector, Vector> velocity = pRobotLinkToGrabWith->GetVelocity();
1406 velocity.first += velocity.second.cross(tbody.trans - t.trans);
1407 pbody->SetVelocity(velocity.first, velocity.second);
1416 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
1417 if(
KinBodyPtr(pgrabbed->_pgrabbedbody) == pbody ) {
1424 RAVELOG_DEBUG(str(boost::format(
"Robot %s: body %s not grabbed\n")%
GetName()%pbody->GetName()));
1430 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
1431 KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
1442 std::vector<LinkPtr > vattachedlinks;
1444 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
1449 pgrabbed->_ProcessCollidingLinks(pgrabbed->_setRobotLinksToIgnore);
1456 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(_pgrabbed);
1457 KinBodyPtr pgrabbedbody = pgrabbed->_pgrabbedbody.lock();
1458 if( !!pgrabbedbody ) {
1463 pgrabbed->_ProcessCollidingLinks(pgrabbed->_setRobotLinksToIgnore);
1470 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
1472 return pgrabbed->_plinkrobot;
1482 GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbed);
1483 KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
1484 if( !!pbody && pbody->GetEnvironmentId() ) {
1485 vbodies.push_back(pbody);
1493 for(
size_t i = 0; i < vgrabbedinfo.size(); ++i) {
1496 vgrabbedinfo[i]->_grabbedname = pgrabbed->_pgrabbedbody.lock()->GetName();
1497 vgrabbedinfo[i]->_robotlinkname = pgrabbed->_plinkrobot->GetName();
1498 vgrabbedinfo[i]->_trelative = pgrabbed->_troot;
1499 vgrabbedinfo[i]->_setRobotLinksToIgnore = pgrabbed->_setRobotLinksToIgnore;
1501 if( find(pgrabbed->_listNonCollidingLinks.begin(), pgrabbed->_listNonCollidingLinks.end(), *itlink) == pgrabbed->_listNonCollidingLinks.end() ) {
1502 vgrabbedinfo[i]->_setRobotLinksToIgnore.insert((*itlink)->GetIndex());
1512 FOREACHC(itgrabbedinfo, vgrabbedinfo) {
1523 GrabbedPtr pgrabbed(
new Grabbed(pbody,pRobotLinkToGrabWith));
1524 pgrabbed->_troot = pgrabbedinfo->_trelative;
1525 pgrabbed->_ProcessCollidingLinks(pgrabbedinfo->_setRobotLinksToIgnore);
1526 Transform tlink = pRobotLinkToGrabWith->GetTransform();
1527 Transform tbody = tlink * pgrabbed->_troot;
1528 pbody->SetTransform(tbody);
1530 std::pair<Vector, Vector> velocity = pRobotLinkToGrabWith->GetVelocity();
1531 velocity.first += velocity.second.cross(tbody.trans - tlink.trans);
1532 pbody->SetVelocity(velocity.first, velocity.second);
1540 ignorelinks.clear();
1542 GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbed);
1543 KinBodyPtr grabbedbody = pgrabbed->_pgrabbedbody.lock();
1544 if( grabbedbody == body ) {
1546 if( find(pgrabbed->_listNonCollidingLinks.begin(), pgrabbed->_listNonCollidingLinks.end(), *itrobotlink) == pgrabbed->_listNonCollidingLinks.end() ) {
1547 ignorelinks.push_back(*itrobotlink);
1553 RAVELOG_WARN(str(boost::format(
"body %s is not currently grabbed")%body->GetName()));
1568 if( *itmanip == pmanip ) {
1579 if( manipname.size() > 0 ) {
1581 if( (*itmanip)->GetName() == manipname ) {
1616 if( (*itmanip)->GetName() == manipinfo.
_name ) {
1621 newmanip->_ComputeInternalInformation();
1623 __hashrobotstructure.resize(0);
1633 if( *itmanip == manip ) {
1635 __hashrobotstructure.resize(0);
1649 bool bCollision =
false;
1651 GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbed);
1656 FOREACHC(itrobotlink,pgrabbed->_listNonCollidingLinks) {
1658 FOREACHC(itbodylink,pbody->GetLinks()) {
1672 if( pbody->CheckSelfCollision(report) ) {
1681 GrabbedConstPtr pgrabbed2 = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbed2);
1683 if( pbody == pbody2 ) {
1686 FOREACHC(itlink2, pbody2->GetLinks()) {
1688 if( find(pgrabbed->_listNonCollidingLinks.begin(),pgrabbed->_listNonCollidingLinks.end(),*itlink2) != pgrabbed->_listNonCollidingLinks.end() ) {
1689 FOREACHC(itlink, pbody->GetLinks()) {
1690 if( find(pgrabbed2->_listNonCollidingLinks.begin(),pgrabbed2->_listNonCollidingLinks.end(),*itlink) != pgrabbed2->_listNonCollidingLinks.end() ) {
1715 if( bCollision && !!report ) {
1717 RAVELOG_VERBOSE(str(boost::format(
"Self collision: %s\n")%report->__str__()));
1718 std::vector<OpenRAVE::dReal> v;
1720 stringstream ss; ss << std::setprecision(std::numeric_limits<OpenRAVE::dReal>::digits10+1);
1721 for(
size_t i = 0; i < v.size(); ++i ) {
1726 ss <<
"colvalues=[" << v[i];
1740 if( plink->IsEnabled() ) {
1741 boost::shared_ptr<TransformSaver<LinkPtr> > linksaver(
new TransformSaver<LinkPtr>(plink));
1742 plink->SetTransform(tlinktrans);
1743 if( pchecker->CheckCollision(
LinkConstPtr(plink),report) ) {
1750 std::vector<KinBodyConstPtr> vbodyexcluded;
1751 std::vector<KinBody::LinkConstPtr> vlinkexcluded;
1753 GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbed);
1754 if( pgrabbed->_plinkrobot == plink ) {
1755 KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
1757 vbodyexcluded.resize(0);
1760 if( itgrabbed2 != itgrabbed ) {
1761 GrabbedConstPtr pgrabbed2 = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbed2);
1762 KinBodyPtr pbody2 = pgrabbed2->_pgrabbedbody.lock();
1764 vbodyexcluded.push_back(pbody2);
1769 pbody->SetTransform(tlinktrans * pgrabbed->_troot);
1770 if( pchecker->CheckCollision(
KinBodyConstPtr(pbody),vbodyexcluded, vlinkexcluded, report) ) {
1784 if( plink->IsEnabled() ) {
1785 boost::shared_ptr<TransformSaver<LinkPtr> > linksaver(
new TransformSaver<LinkPtr>(plink));
1786 plink->SetTransform(tlinktrans);
1787 if( pchecker->CheckSelfCollision(
LinkConstPtr(plink),report) ) {
1795 std::vector<KinBodyConstPtr> vbodyexcluded;
1796 std::vector<KinBody::LinkConstPtr> vlinkexcluded;
1798 GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbed);
1799 if( pgrabbed->_plinkrobot == plink ) {
1800 KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
1804 plink->Enable(
false);
1808 pbody->SetTransform(tlinktrans * pgrabbed->_troot);
1829 for(
int i = 0; i <
GetDOF(); ++i) {
1838 ss <<
"joint_values " <<
GetName();
1842 group.
name = ss.str();
1851 if( (*itmanip)->_info._name.size() == 0 ) {
1853 ss <<
"manip" << manipindex;
1854 RAVELOG_WARN(str(boost::format(
"robot %s has a manipulator with no name, setting to %s\n")%
GetName()%ss.str()));
1855 (*itmanip)->_info._name = ss.str();
1857 (*itmanip)->_ComputeInternalInformation();
1858 vector<ManipulatorPtr>::iterator itmanip2 = itmanip; ++itmanip2;
1860 if( (*itmanip)->GetName() == (*itmanip2)->GetName() ) {
1861 RAVELOG_WARN(str(boost::format(
"robot %s has two manipulators with the same name: %s!\n")%
GetName()%(*itmanip)->GetName()));
1876 if( (*itsensor)->GetName().size() == 0 ) {
1878 ss <<
"sensor" << sensorindex;
1879 RAVELOG_WARN(str(boost::format(
"robot %s has a sensor with no name, setting to %s\n")%
GetName()%ss.str()));
1880 (*itsensor)->_name = ss.str();
1885 if( !!(*itsensor)->GetSensor() ) {
1886 stringstream ss; ss <<
GetName() <<
"_" << (*itsensor)->GetName();
1887 (*itsensor)->GetSensor()->SetName(ss.str());
1893 __hashrobotstructure.resize(0);
1895 (*itsensor)->__hashstructure.resize(0);
1899 if(
ComputeAABB().extents.lengthsqr3() > 900.0f ) {
1900 RAVELOG_WARN(str(boost::format(
"Robot %s span is greater than 30 meaning that it is most likely defined in a unit other than meters. It is highly encouraged to define all OpenRAVE robots in meters since many metrics, database models, and solvers have been specifically optimized for this unit\n")%
GetName()));
1905 std::vector<int> dofindices;
1906 for(
int i = 0; i <
GetDOF(); ++i) {
1907 dofindices.push_back(i);
1927 (*itsensor)->__hashstructure.resize(0);
1932 (*itmanip)->__hashstructure.resize(0);
1933 (*itmanip)->__hashkinematicsstructure.resize(0);
1939 std::map<GrabbedPtr, list<KinBody::LinkConstPtr> > mapcheckcollisions;
1941 if( (*itlink)->IsEnabled() ) {
1943 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
1944 if( find(pgrabbed->GetRigidlyAttachedLinks().begin(),pgrabbed->GetRigidlyAttachedLinks().end(), *itlink) == pgrabbed->GetRigidlyAttachedLinks().end() ) {
1945 std::list<KinBody::LinkConstPtr>::iterator itnoncolliding = find(pgrabbed->_listNonCollidingLinks.begin(),pgrabbed->_listNonCollidingLinks.end(),*itlink);
1946 if( itnoncolliding != pgrabbed->_listNonCollidingLinks.end() ) {
1947 if( pgrabbed->WasLinkNonColliding(*itlink) == 0 ) {
1948 pgrabbed->_listNonCollidingLinks.erase(itnoncolliding);
1950 mapcheckcollisions[pgrabbed].push_back(*itlink);
1954 if( pgrabbed->WasLinkNonColliding(*itlink) == 1 ) {
1955 pgrabbed->_listNonCollidingLinks.push_back(*itlink);
1964 GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<
Grabbed>(*itgrabbed);
1965 if( find(pgrabbed->GetRigidlyAttachedLinks().begin(),pgrabbed->GetRigidlyAttachedLinks().end(), *itlink) == pgrabbed->GetRigidlyAttachedLinks().end() ) {
1966 if( find(pgrabbed->_listNonCollidingLinks.begin(),pgrabbed->_listNonCollidingLinks.end(),*itlink) == pgrabbed->_listNonCollidingLinks.end() ) {
1967 if( pgrabbed->WasLinkNonColliding(*itlink) != 0 ) {
1968 pgrabbed->_listNonCollidingLinks.push_back(*itlink);
1996 __hashrobotstructure = r->__hashrobotstructure;
1999 FOREACHC(itmanip, r->_vecManipulators) {
2002 if( !!r->GetActiveManipulator() && r->GetActiveManipulator()->GetName() == (*itmanip)->GetName() ) {
2008 FOREACHC(itsensor, r->_vecSensors) {
2040 RAVELOG_WARN(str(boost::format(
"failed to set %s controller for robot %s\n")%r->GetController()->GetXMLId()%
GetName()));
2045 std::vector<int> dofindices;
2046 for(
int i = 0; i <
GetDOF(); ++i) {
2047 dofindices.push_back(i);
2056 FOREACHC(itgrabbedref, r->_vGrabbedBodies) {
2057 GrabbedConstPtr pgrabbedref = boost::dynamic_pointer_cast<
Grabbed const>(*itgrabbedref);
2059 KinBodyPtr pbodyref(pgrabbedref->_pgrabbedbody);
2062 pgrabbedbody =
GetEnv()->GetBodyFromEnvironmentId(pbodyref->GetEnvironmentId());
2063 BOOST_ASSERT(pgrabbedbody->GetName() == pbodyref->GetName());
2066 pgrabbed->_troot = pgrabbedref->_troot;
2067 pgrabbed->_listNonCollidingLinks.clear();
2068 FOREACHC(itlinkref, pgrabbedref->_listNonCollidingLinks) {
2069 pgrabbed->_listNonCollidingLinks.push_back(
_veclinks.at((*itlinkref)->GetIndex()));
2081 (*itmanip)->serialize(o,options);
2086 (*itsensor)->serialize(o,options);
2094 if( __hashrobotstructure.size() == 0 ) {
2096 ss << std::fixed << std::setprecision(SERIALIZATION_PRECISION);
2100 return __hashrobotstructure;
2134 ss <<
"joint_values " <<
GetName();
2135 for(
int i = 0; i <
GetDOF(); ++i) {
2139 spec.
_vgroups[0].interpolation =
"linear";
2142 spec.
_vgroups[1].name =
"deltatime";
2143 if( !bOverwriteTransforms ) {
2148 spec.
_vgroups[2].interpolation =
"linear";
2150 pfulltraj->Init(spec);
2151 std::vector<dReal> vdata;
2152 pActiveTraj->GetWaypoints(0,pActiveTraj->GetNumWaypoints(),vdata);
2153 pfulltraj->Insert(0,vdata,pActiveTraj->GetConfigurationSpecification());