openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
controller.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2011 Rosen Diankov (rosen.diankov@gmail.com)
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 
19 namespace OpenRAVE {
20 
21 MultiController::MultiController(EnvironmentBasePtr penv) : ControllerBase(penv), _nControlTransformation(0)
22 {
23 }
24 
26 {
27 }
28 
29 bool MultiController::Init(RobotBasePtr robot, const std::vector<int>& dofindices, int nControlTransformation)
30 {
31  boost::mutex::scoped_lock lock(_mutex);
32  _probot=robot;
33  if( !_probot ) {
34  return false;
35  }
36  _listcontrollers.clear();
37  _dofindices=dofindices;
38  // reverse the mapping
39  _dofreverseindices.resize(_probot->GetDOF());
40  FOREACH(it,_dofreverseindices) {
41  *it = -1;
42  }
43  int index = 0;
44  FOREACH(it,_dofindices) {
45  _dofreverseindices.at(*it) = index++;
46  }
47  _vcontrollersbydofs.resize(0); _vcontrollersbydofs.resize(_dofindices.size());
48  _nControlTransformation = nControlTransformation;
49  _ptransformcontroller.reset();
50  return true;
51 }
52 
53 const std::vector<int>& MultiController::GetControlDOFIndices() const {
54  return _dofindices;
55 }
58 }
60  return _probot;
61 }
62 
63 bool MultiController::AttachController(ControllerBasePtr controller, const std::vector<int>& dofindices, int nControlTransformation)
64 {
65  boost::mutex::scoped_lock lock(_mutex);
66  if( nControlTransformation && !!_ptransformcontroller ) {
67  throw openrave_exception("controller already attached for transformation",ORE_InvalidArguments);
68  }
69  FOREACHC(it,dofindices) {
70  if( !!_vcontrollersbydofs.at(*it) ) {
71  throw openrave_exception(str(boost::format("controller already attached to dof %d")%*it));
72  }
73  }
74  if( !controller->Init(_probot,dofindices,nControlTransformation) ) {
75  return false;
76  }
77  if( nControlTransformation ) {
79  }
80  FOREACHC(it,dofindices) {
82  }
83  _listcontrollers.push_back(controller);
84  return true;
85 }
86 
88 {
89  boost::mutex::scoped_lock lock(_mutex);
90  _listcontrollers.remove(controller);
91  if( _ptransformcontroller == controller ) {
92  _ptransformcontroller.reset();
93  }
94  FOREACH(it,_vcontrollersbydofs) {
95  if( *it == controller ) {
96  it->reset();
97  }
98  }
99 }
100 
102 {
103  boost::mutex::scoped_lock lock(_mutex);
104  if( dof < 0 ) {
105  return _ptransformcontroller;
106  }
107  int index=0;
108  FOREACHC(it,_dofindices) {
109  if( *it == dof ) {
110  return _vcontrollersbydofs.at(index);
111  }
112  index++;
113  }
114  return ControllerBasePtr();
115 }
116 
117 void MultiController::Reset(int options)
118 {
119  boost::mutex::scoped_lock lock(_mutex);
120  FOREACH(itcontroller,_listcontrollers) {
121  (*itcontroller)->Reset(options);
122  }
123 }
124 
125 bool MultiController::SetDesired(const std::vector<dReal>& values, TransformConstPtr trans)
126 {
127  boost::mutex::scoped_lock lock(_mutex);
128  vector<dReal> v;
129  bool bsuccess = true;
130  FOREACH(itcontroller,_listcontrollers) {
131  v.resize(0);
132  FOREACHC(it, (*itcontroller)->GetControlDOFIndices()) {
133  v.push_back(values.at(_dofreverseindices.at(*it)));
134  }
135  bsuccess &= (*itcontroller)->SetDesired(v,trans);
136  }
137  return bsuccess;
138 }
139 
141 {
142  boost::mutex::scoped_lock lock(_mutex);
143  bool bsuccess = true;
144  if( !ptraj ) {
145  FOREACH(itcontroller,_listcontrollers) {
146  bsuccess &= (*itcontroller)->SetPath(TrajectoryBaseConstPtr());
147  }
148  }
149  else {
150  if( !_ptraj ||(_ptraj->GetXMLId() != ptraj->GetXMLId())) {
151  _ptraj = RaveCreateTrajectory(ptraj->GetEnv(),ptraj->GetXMLId());
152  }
153  }
154  return bsuccess;
155 }
156 
158 {
159  boost::mutex::scoped_lock lock(_mutex);
160  FOREACH(it,_listcontrollers) {
161  (*it)->SimulationStep(fTimeElapsed);
162  }
163 }
164 
166 {
167  boost::mutex::scoped_lock lock(_mutex);
168  bool bdone=true;
169  FOREACH(it,_listcontrollers) {
170  bdone &= (*it)->IsDone();
171  }
172  return bdone;
173 }
174 
176 {
177  boost::mutex::scoped_lock lock(_mutex);
178  dReal t = 0;
179  FOREACHC(it,_listcontrollers) {
180  if( it == _listcontrollers.begin() ) {
181  t = (*it)->GetTime();
182  }
183  else {
184  dReal tnew = (*it)->GetTime();
185  if( RaveFabs(t-tnew) > 0.000001 ) {
186  RAVELOG_WARN(str(boost::format("multi-controller time is different! %f!=%f\n")%t%tnew));
187  }
188  t = max(t,tnew);
189  }
190  }
191  return t;
192 }
193 
194 void MultiController::GetVelocity(std::vector<dReal>& vel) const
195 {
196  boost::mutex::scoped_lock lock(_mutex);
197  vel.resize(_dofindices.size());
198  FOREACH(it,vel) {
199  *it = 0;
200  }
201  vector<dReal> v;
202  FOREACHC(itcontroller,_listcontrollers) {
203  (*itcontroller)->GetVelocity(v);
204  int index=0;
205  FOREACH(it,v) {
206  vel.at(_dofreverseindices.at((*itcontroller)->GetControlDOFIndices().at(index++))) = *it;
207  }
208  }
209 }
210 
211 void MultiController::GetTorque(std::vector<dReal>& torque) const
212 {
213  boost::mutex::scoped_lock lock(_mutex);
214  torque.resize(_dofindices.size());
215  FOREACH(it,torque) {
216  *it = 0;
217  }
218  vector<dReal> v;
219  FOREACHC(itcontroller,_listcontrollers) {
220  (*itcontroller)->GetTorque(v);
221  int index=0;
222  FOREACH(it,v) {
223  torque.at(_dofreverseindices.at((*itcontroller)->GetControlDOFIndices().at(index++))) = *it;
224  }
225  }
226 }
227 
228 
229 //void RobotBase::GetControlMaxTorques(std::vector<dReal>& maxtorques) const
230 //{
231 // if( _nActiveDOF < 0 ) {
232 // GetDOFMaxTorque(maxtorques);
233 // return;
234 // }
235 // maxtorques.resize(GetActiveDOF());
236 // if( maxtorques.size() == 0 ) {
237 // return;
238 // }
239 // dReal* pMaxTorques = &maxtorques[0];
240 //
241 // if( _vActiveJointIndices.size() != 0 ) {
242 // GetDOFMaxTorque(_vTempRobotJoints);
243 //
244 // FOREACHC(it, _vActiveJointIndices)
245 // *pMaxTorques++ = _vTempRobotJoints[*it];
246 // }
247 //
248 // if( _nAffineDOFs == DOF_NoTransform )
249 // return;
250 //
251 // if( _nAffineDOFs & DOF_X ) *pMaxTorques++ = 0;
252 // if( _nAffineDOFs & DOF_Y ) *pMaxTorques++ = 0;
253 // if( _nAffineDOFs & DOF_Z ) *pMaxTorques++ = 0;
254 // if( _nAffineDOFs & DOF_RotationAxis ) *pMaxTorques++ = 0;
255 // else if( _nAffineDOFs & DOF_Rotation3D ) {
256 // *pMaxTorques++ = 0;
257 // *pMaxTorques++ = 0;
258 // *pMaxTorques++ = 0;
259 // }
260 // else if( _nAffineDOFs & DOF_RotationQuat ) {
261 // *pMaxTorques++ = 0;
262 // *pMaxTorques++ = 0;
263 // *pMaxTorques++ = 0;
264 // *pMaxTorques++ = 0;
265 // }
266 //}
267 //
268 //void RobotBase::SetControlTorques(const std::vector<dReal>& vtorques)
269 //{
270 // if(_nActiveDOF < 0) {
271 // SetJointTorques(vtorques, false);
272 // return;
273 // }
274 //
275 // if( _vActiveJointIndices.size() > 0 ) {
276 // _vTempRobotJoints.resize(GetDOF());
277 // std::vector<dReal>::const_iterator ittorque = vtorques.begin();
278 // FOREACHC(it, _vActiveJointIndices)
279 // _vTempRobotJoints[*it] = *ittorque++;
280 // SetJointTorques(_vTempRobotJoints,false);
281 // }
282 //}
283 
284 }