17 #include "libopenrave.h"
31 boost::mutex::scoped_lock lock(
_mutex);
65 boost::mutex::scoped_lock lock(
_mutex);
69 FOREACHC(it,dofindices) {
71 throw openrave_exception(str(boost::format(
"controller already attached to dof %d")%*it));
74 if( !controller->Init(
_probot,dofindices,nControlTransformation) ) {
77 if( nControlTransformation ) {
80 FOREACHC(it,dofindices) {
89 boost::mutex::scoped_lock lock(
_mutex);
95 if( *it == controller ) {
103 boost::mutex::scoped_lock lock(
_mutex);
119 boost::mutex::scoped_lock lock(
_mutex);
121 (*itcontroller)->Reset(options);
127 boost::mutex::scoped_lock lock(
_mutex);
129 bool bsuccess =
true;
132 FOREACHC(it, (*itcontroller)->GetControlDOFIndices()) {
135 bsuccess &= (*itcontroller)->SetDesired(v,trans);
142 boost::mutex::scoped_lock lock(
_mutex);
143 bool bsuccess =
true;
150 if( !
_ptraj ||(
_ptraj->GetXMLId() != ptraj->GetXMLId())) {
159 boost::mutex::scoped_lock lock(
_mutex);
161 (*it)->SimulationStep(fTimeElapsed);
167 boost::mutex::scoped_lock lock(
_mutex);
170 bdone &= (*it)->IsDone();
177 boost::mutex::scoped_lock lock(
_mutex);
181 t = (*it)->GetTime();
184 dReal tnew = (*it)->GetTime();
186 RAVELOG_WARN(str(boost::format(
"multi-controller time is different! %f!=%f\n")%t%tnew));
196 boost::mutex::scoped_lock lock(
_mutex);
203 (*itcontroller)->GetVelocity(v);
206 vel.at(
_dofreverseindices.at((*itcontroller)->GetControlDOFIndices().at(index++))) = *it;
213 boost::mutex::scoped_lock lock(
_mutex);
220 (*itcontroller)->GetTorque(v);
223 torque.at(
_dofreverseindices.at((*itcontroller)->GetControlDOFIndices().at(index++))) = *it;