17 #include "libopenrave.h"
28 if((name != _pdata->GetXMLId())&&(name !=
"offsetlink")&&(name !=
"id")&&(name !=
"sid")&&(name !=
"translation")&&(name !=
"rotationmat")&&(name !=
"rotationaxis")&&(name !=
"quat")&&(name !=
"pretranslation")&&(name !=
"prerotation")&&(name !=
"prerotationaxis")&&(name !=
"prequat")) {
36 if( name ==
"offsetlink" ) {
37 ss >> _pdata->strOffsetLink;
39 else if( name ==
"id" ) {
42 else if( name ==
"sid" ) {
45 else if( name ==
"translation" ) {
46 ss >> _pdata->transOffset.trans.x >> _pdata->transOffset.trans.y >> _pdata->transOffset.trans.z;
48 else if( name ==
"rotationmat" ) {
50 ss >> m.m[0] >> m.m[1] >> m.m[2] >> m.m[4] >> m.m[5] >> m.m[6] >> m.m[8] >> m.m[9] >> m.m[10];
51 _pdata->transOffset.rot =
Transform(m).rot;
53 else if( name ==
"rotationaxis" ) {
55 ss >> axis.x >> axis.y >> axis.z >> fang;
58 else if( name ==
"quat" ) {
59 ss >> _pdata->transOffset.rot;
61 else if( name ==
"pretranslation") {
62 ss >> _pdata->transPreOffset.trans.x >> _pdata->transPreOffset.trans.y >> _pdata->transPreOffset.trans.z;
64 else if( name ==
"prerotationmat") {
66 ss >> m.m[0] >> m.m[1] >> m.m[2] >> m.m[4] >> m.m[5] >> m.m[6] >> m.m[8] >> m.m[9] >> m.m[10];
67 _pdata->transPreOffset.rot =
Transform(m).rot;
69 else if( name ==
"prerotationaxis") {
71 ss >> axis.x >> axis.y >> axis.z >> fang;
74 else if( name ==
"prequat") {
75 ss >> _pdata->transPreOffset.rot;
81 RAVELOG_WARN(str(boost::format(
"error parsing %s\n")%name));
117 boost::mutex::scoped_lock lock(
_mutex);
124 FOREACHC(itbody, vbodies) {
125 boost::shared_ptr<XMLData> pmocapdata = boost::dynamic_pointer_cast<
XMLData>((*itbody)->GetReadableInterface(
_xmlid));
137 BOOST_ASSERT(pbody->GetEnv()==
GetEnv());
138 boost::shared_ptr<XMLData const> pdata = boost::static_pointer_cast<
XMLData const>(_pdata);
140 pdata = boost::dynamic_pointer_cast<
XMLData const>(pbody->GetReadableInterface(
_xmlid));
142 RAVELOG_VERBOSE(str(boost::format(
"failed to find manage data for body %s\n")%pbody->GetName()));
147 boost::mutex::scoped_lock lock(
_mutex);
149 RAVELOG_WARN(str(boost::format(
"body %s already added\n")%pbody->GetName()));
156 RAVELOG_VERBOSE(str(boost::format(
"system adding body %s (%s), total: %d\n")%pbody->GetName()%pbody->GetURI()%
_mapbodies.size()));
163 boost::mutex::scoped_lock lock(
_mutex);
164 bool bSuccess =
_mapbodies.erase(pbody->GetEnvironmentId())>0;
165 RAVELOG_VERBOSE(str(boost::format(
"system removing body %s %s\n")%pbody->GetName()%(bSuccess ?
"succeeded" :
"failed")));
171 boost::mutex::scoped_lock lock(
_mutex);
177 boost::mutex::scoped_lock lock(
_mutex);
178 BODIES::iterator it =
_mapbodies.find(pbody->GetEnvironmentId());
180 RAVELOG_WARN(
"trying to %s body %s that is not in system\n", bEnable ?
"enable" :
"disable", pbody->GetName().c_str());
184 it->second->bEnabled = bEnable;
191 BODIES::iterator it =
_mapbodies.find(pbody1->GetEnvironmentId());
192 boost::shared_ptr<BodyData> pb1,pb2;
196 it =
_mapbodies.find(pbody2->GetEnvironmentId());
204 pb1->SetBody(pbody2);
207 pb2->SetBody(pbody1);
215 pnewdata->copy(pdata);
216 return boost::shared_ptr<BodyData>(
new BodyData(RaveInterfaceCast<SimpleSensorSystem>(shared_from_this()),pbody, pnewdata));
221 EnvironmentMutex::scoped_lock lockenv(
GetEnv()->GetMutex());
223 if( listbodies.size() > 0 ) {
225 FOREACH(it, listbodies) {
226 BOOST_ASSERT( it->first->IsEnabled() );
235 TransformMatrix toffset = tbase * tlink.inverse() * it->first->_initdata->transOffset;
236 TransformMatrix tfinal = toffset * it->second*it->first->_initdata->transPreOffset;
238 plink->GetParent()->SetTransform(tfinal);
239 it->first->lastupdated = curtime;
240 it->first->tnew = it->second;
242 if( !it->first->IsPresent() ) {
243 RAVELOG_VERBOSE(str(boost::format(
"updating body %s\n")%plink->GetParent()->GetName()));
245 it->first->bPresent =
true;
249 boost::mutex::scoped_lock lock(
_mutex);
253 if( !!plink &&(plink->GetParent()->GetEnvironmentId()==0)) {
258 if( !itbody->second->IsLocked() ) {
261 GetEnv()->Remove(plink->GetParent());
267 if( itbody->second->IsPresent() && !!plink ) {
268 RAVELOG_VERBOSE(str(boost::format(
"body %s not present\n")%plink->GetParent()->GetName()));
270 itbody->second->bPresent =
false;
279 list< SNAPSHOT > listbodies;