openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
robot.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 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 #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);
20 
21 namespace OpenRAVE {
22 
24 {
25 public:
26  CallOnDestruction(const boost::function<void()>& fn) : _fn(fn) {
27  }
29  _fn();
30  }
31 private:
32  boost::function<void()> _fn;
33 };
34 
36 {
37 }
38 
40 {
41  *this = sensor;
42  _probot = probot;
43  psensor.reset();
44  pdata.reset();
45  pattachedlink.reset();
46  if( (cloningoptions&Clone_Sensors) && !!sensor.psensor ) {
47  psensor = RaveCreateSensor(probot->GetEnv(), sensor.psensor->GetXMLId());
48  if( !!psensor ) {
49  psensor->Clone(sensor.psensor,cloningoptions);
50  if( !!psensor ) {
51  pdata = psensor->CreateSensorData();
52  }
53  }
54  }
55  int index = LinkPtr(sensor.pattachedlink)->GetIndex();
56  if((index >= 0)&&(index < (int)probot->GetLinks().size())) {
57  pattachedlink = probot->GetLinks().at(index);
58  }
59 }
60 
62 {
63 }
64 
66 {
67  if( psensor->GetSensorData(pdata) ) {
68  return pdata;
69  }
71 }
72 
74 {
75  trelative = t;
76  GetRobot()->_ParametersChanged(Prop_SensorPlacement);
77 }
78 
79 void RobotBase::AttachedSensor::serialize(std::ostream& o, int options) const
80 {
81  o << (pattachedlink.expired() ? -1 : LinkPtr(pattachedlink)->GetIndex()) << " ";
82  SerializeRound(o,trelative);
83  o << (!pdata ? -1 : pdata->GetType()) << " ";
84  // it is also important to serialize some of the geom parameters for the sensor (in case models are cached to it)
85  if( !!psensor ) {
86  SensorBase::SensorGeometryPtr prawgeom = psensor->GetSensorGeometry();
87  if( !!prawgeom ) {
88  switch(prawgeom->GetType()) {
89  case SensorBase::ST_Laser: {
90  boost::shared_ptr<SensorBase::LaserGeomData> pgeom = boost::static_pointer_cast<SensorBase::LaserGeomData>(prawgeom);
91  o << pgeom->min_angle[0] << " " << pgeom->max_angle[0] << " " << pgeom->resolution[0] << " " << pgeom->max_range << " ";
92  break;
93  }
94  case SensorBase::ST_Camera: {
95  boost::shared_ptr<SensorBase::CameraGeomData> pgeom = boost::static_pointer_cast<SensorBase::CameraGeomData>(prawgeom);
96  o << pgeom->KK.fx << " " << pgeom->KK.fy << " " << pgeom->KK.cx << " " << pgeom->KK.cy << " " << pgeom->width << " " << pgeom->height << " ";
97  break;
98  }
99  default:
100  // don't support yet
101  break;
102  }
103  }
104  }
105 }
106 
108 {
109  if( __hashstructure.size() == 0 ) {
110  ostringstream ss;
111  ss << std::fixed << std::setprecision(SERIALIZATION_PRECISION);
113  __hashstructure = utils::GetMD5HashString(ss.str());
114  }
115  return __hashstructure;
116 }
117 
118 RobotBase::RobotStateSaver::RobotStateSaver(RobotBasePtr probot, int options) : KinBodyStateSaver(probot, options), _probot(probot)
119 {
120  if( _options & Save_ActiveDOF ) {
121  vactivedofs = _probot->GetActiveDOFIndices();
122  affinedofs = _probot->GetAffineDOF();
123  rotationaxis = _probot->GetAffineRotationAxis();
124  }
125  if( _options & Save_ActiveManipulator ) {
126  _pManipActive = _probot->GetActiveManipulator();
127  }
128  if( _options & Save_GrabbedBodies ) {
129  _vGrabbedBodies = _probot->_vGrabbedBodies;
130  }
131 }
132 
134 {
135  _RestoreRobot(_probot);
136 }
137 
138 void RobotBase::RobotStateSaver::Restore(boost::shared_ptr<RobotBase> robot)
139 {
140  _RestoreRobot(!robot ? _probot : robot);
141  KinBodyStateSaver::Restore(!robot ? KinBodyPtr(_probot) : KinBodyPtr(robot));
142 }
143 
145 {
146  _probot.reset();
148 }
149 void RobotBase::RobotStateSaver::_RestoreRobot(boost::shared_ptr<RobotBase> probot)
150 {
151  if( !probot ) {
152  return;
153  }
154  if( probot->GetEnvironmentId() == 0 ) {
155  RAVELOG_WARN(str(boost::format("robot %s not added to environment, skipping restore")%_pbody->GetName()));
156  return;
157  }
158  if( _options & Save_ActiveDOF ) {
159  probot->SetActiveDOFs(vactivedofs, affinedofs, rotationaxis);
160  }
161  if( _options & Save_ActiveManipulator ) {
162  if( probot == _probot ) {
163  probot->SetActiveManipulator(_pManipActive);
164  }
165  else {
166  if( !_pManipActive ) {
167  probot->SetActiveManipulator(ManipulatorPtr());
168  }
169  else {
170  probot->SetActiveManipulator(_pManipActive->GetName());
171  }
172  }
173  }
174  if( _options & Save_GrabbedBodies ) {
175  // have to release all grabbed first
176  probot->ReleaseAllGrabbed();
177  OPENRAVE_ASSERT_OP(probot->_vGrabbedBodies.size(),==,0);
178  FOREACH(itgrabbed, _vGrabbedBodies) {
179  GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
180  KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
181  if( !!pbody ) {
182  if( probot->GetEnv() == _probot->GetEnv() ) {
183  probot->_AttachBody(pbody);
184  probot->_vGrabbedBodies.push_back(*itgrabbed);
185  }
186  else {
187  // pgrabbed points to a different environment, so have to re-initialize
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()));
191  }
192  else {
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()));
198  }
199  probot->_AttachBody(pnewbody);
200  probot->_vGrabbedBodies.push_back(pnewgrabbed);
201  }
202  }
203  }
204  }
205  }
206 }
207 
209 {
210  _nAffineDOFs = 0;
211  _nActiveDOF = -1;
213 
214  //set limits for the affine DOFs
215  _vTranslationLowerLimits = Vector(-100,-100,-100);
216  _vTranslationUpperLimits = Vector(100,100,100);
217  _vTranslationMaxVels = Vector(1.0f,1.0f,1.0f);
218  _vTranslationResolutions = Vector(0.001f,0.001f,0.001f);
219  _vTranslationWeights = Vector(2.0f,2.0f,2.0f);
220 
221  // rotation axis has infinite movement, so make sure the limits are big
222  _vRotationAxisLowerLimits = Vector(-10000,-10000,-10000,10000);
223  _vRotationAxisUpperLimits = Vector(10000,10000,10000,10000);
224 
225  _vRotationAxisMaxVels = Vector(0.4f,0.4f,0.4f,0.4f);
226  _vRotationAxisResolutions = Vector(0.01f,0.01f,0.01f,0.01f);
227  _vRotationAxisWeights = Vector(2.0f,2.0f,2.0f,2.0f);
228 
229  _vRotation3DLowerLimits = Vector(-10000,-10000,-10000);
230  _vRotation3DUpperLimits = Vector(10000,10000,10000);
231  _vRotation3DMaxVels = Vector(0.07f,0.07f,0.07f);
232  _vRotation3DResolutions = Vector(0.01f,0.01f,0.01f);
233  _vRotation3DWeights = Vector(1.0f,1.0f,1.0f);
234 
235  _vRotationQuatLimitStart = Vector(1,0,0,0);
238  _fQuatAngleResolution = 0.01f;
239  _fQuatAngleWeight = 0.4f;
240 }
241 
243 {
244  Destroy();
245 }
246 
248 {
250  _vecManipulators.clear();
251  _vecSensors.clear();
252  SetController(ControllerBasePtr(),std::vector<int>(),0);
253 
255 }
256 
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)
258 {
259  if( !KinBody::Init(linkinfos, jointinfos) ) {
260  return false;
261  }
262  _vecManipulators.resize(0);
263  FOREACHC(itmanipinfo, manipinfos) {
264  ManipulatorPtr newmanip(new Manipulator(shared_robot(),**itmanipinfo));
265  _vecManipulators.push_back(newmanip);
266  __hashrobotstructure.resize(0);
267  }
268  // TODO: sensors
269  _vecSensors.resize(0);
270  if( attachedsensorinfos.size() > 0 ) {
271  RAVELOG_WARN("currently do not support initializing from AttachedSensorInfo\n");
272  }
273  return true;
274 }
275 
276 bool RobotBase::SetController(ControllerBasePtr controller, const std::vector<int>& jointindices, int nControlTransformation)
277 {
278  RAVELOG_DEBUG("default robot doesn't not support setting controllers (try GenericRobot)\n");
279  return false;
280 }
281 
282 void RobotBase::SetName(const std::string& newname)
283 {
284  if( _name != newname ) {
285  // have to replace the 2nd word of all the groups with the robot name
286  FOREACH(itgroup, _activespec._vgroups) {
287  stringstream ss(itgroup->name);
288  string grouptype, oldname;
289  ss >> grouptype >> oldname;
290  stringbuf buf;
291  ss.get(buf,0);
292  itgroup->name = str(boost::format("%s %s %s")%grouptype%newname%buf.str());
293  }
294  KinBody::SetName(newname);
295  }
296 }
297 
298 void RobotBase::SetDOFValues(const std::vector<dReal>& vJointValues, uint32_t bCheckLimits, const std::vector<int>& dofindices)
299 {
300  KinBody::SetDOFValues(vJointValues, bCheckLimits,dofindices);
303 }
304 
305 void RobotBase::SetDOFValues(const std::vector<dReal>& vJointValues, const Transform& transbase, uint32_t bCheckLimits)
306 {
307  KinBody::SetDOFValues(vJointValues, transbase, bCheckLimits); // should call RobotBase::SetDOFValues, so no need to upgrade grabbed bodies, attached sensors
308 }
309 
310 void RobotBase::SetLinkTransformations(const std::vector<Transform>& transforms)
311 {
315 }
316 
317 void RobotBase::SetLinkTransformations(const std::vector<Transform>& transforms, const std::vector<int>& dofbranches)
318 {
319  KinBody::SetLinkTransformations(transforms,dofbranches);
322 }
323 
325 {
326  KinBody::SetTransform(trans);
329 }
330 
331 bool RobotBase::SetVelocity(const Vector& linearvel, const Vector& angularvel)
332 {
333  if( !KinBody::SetVelocity(linearvel,angularvel) ) {
334  return false;
335  }
337  return true;
338 }
339 
340 void RobotBase::SetDOFVelocities(const std::vector<dReal>& dofvelocities, const Vector& linearvel, const Vector& angularvel,uint32_t checklimits)
341 {
342  KinBody::SetDOFVelocities(dofvelocities,linearvel,angularvel,checklimits);
344  // do sensors need to have their velocities updated?
345 }
346 
347 void RobotBase::SetDOFVelocities(const std::vector<dReal>& dofvelocities, uint32_t checklimits, const std::vector<int>& dofindices)
348 {
349  KinBody::SetDOFVelocities(dofvelocities,checklimits, dofindices); // RobotBase::SetDOFVelocities should be called internally
350 }
351 
353 {
354  vector<UserDataPtr>::iterator itgrabbed = _vGrabbedBodies.begin();
355  while(itgrabbed != _vGrabbedBodies.end() ) {
356  GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
357  KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
358  if( !!pbody ) {
359  Transform t = pgrabbed->_plinkrobot->GetTransform();
360  pbody->SetTransform(t * pgrabbed->_troot);
361  // set the correct velocity
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);
365  ++itgrabbed;
366  }
367  else {
368  RAVELOG_DEBUG(str(boost::format("erasing invaliding grabbed body from %s")%GetName()));
369  itgrabbed = _vGrabbedBodies.erase(itgrabbed);
370  }
371  }
372 }
373 
375 {
376  FOREACH(itsensor, _vecSensors) {
377  if( !!(*itsensor)->psensor && !(*itsensor)->pattachedlink.expired() )
378  (*itsensor)->psensor->SetTransform(LinkPtr((*itsensor)->pattachedlink)->GetTransform()*(*itsensor)->trelative);
379  }
380 }
381 
382 void RobotBase::SetAffineTranslationLimits(const Vector& lower, const Vector& upper)
383 {
384  _vTranslationLowerLimits = lower;
385  _vTranslationUpperLimits = upper;
386 }
387 
388 void RobotBase::SetAffineRotationAxisLimits(const Vector& lower, const Vector& upper)
389 {
392 }
393 
394 void RobotBase::SetAffineRotation3DLimits(const Vector& lower, const Vector& upper)
395 {
396  _vRotation3DLowerLimits = lower;
397  _vRotation3DUpperLimits = upper;
398 }
399 
401 {
402  _fQuatLimitMaxAngle = RaveSqrt(quatangle.lengthsqr4());
403  if( _fQuatLimitMaxAngle > 0 ) {
405  }
406  else {
408  }
409 }
410 
412 {
413  _vTranslationMaxVels = vels;
414 }
415 
417 {
418  _vRotationAxisMaxVels = vels;
419 }
420 
422 {
423  _vRotation3DMaxVels = vels;
424 }
425 
427 {
428  _fQuatMaxAngleVelocity = anglevelocity;
429 }
430 
432 {
433  _vTranslationResolutions = resolution;
434 }
435 
437 {
438  _vRotationAxisResolutions = resolution;
439 }
440 
442 {
443  _vRotation3DResolutions = resolution;
444 }
445 
447 {
448  _fQuatAngleResolution = angleresolution;
449 }
450 
452 {
453  _vTranslationWeights = weights;
454 }
455 
457 {
458  _vRotationAxisWeights = weights;
459 }
460 
462 {
463  _vRotation3DWeights = weights;
464 }
465 
467 {
468  _fQuatAngleWeight = angleweight;
469 }
470 
472 {
473  lower = _vTranslationLowerLimits;
474  upper = _vTranslationUpperLimits;
475 }
476 
478 {
481 }
482 
484 {
485  lower = _vRotation3DLowerLimits;
486  upper = _vRotation3DUpperLimits;
487 }
488 
489 void RobotBase::SetActiveDOFs(const std::vector<int>& vJointIndices, int nAffineDOFBitmask, const Vector& vRotationAxis)
490 {
491  vActvAffineRotationAxis = vRotationAxis;
492  SetActiveDOFs(vJointIndices,nAffineDOFBitmask);
493 }
494 
495 void RobotBase::SetActiveDOFs(const std::vector<int>& vJointIndices, int nAffineDOFBitmask)
496 {
497  FOREACHC(itj, vJointIndices) {
498  OPENRAVE_ASSERT_FORMAT(*itj>=0 && *itj<GetDOF(), "bad indices %d (dof=%d)",*itj%GetDOF(),ORE_InvalidArguments);
499  }
500  // only reset the cache if the dof values are different
501  if( _vActiveDOFIndices.size() != vJointIndices.size() ) {
503  }
504  else {
505  for(size_t i = 0; i < vJointIndices.size(); ++i) {
506  if( _vActiveDOFIndices[i] != vJointIndices[i] ) {
508  break;
509  }
510  }
511  }
512  _vActiveDOFIndices = vJointIndices;
513  _nAffineDOFs = nAffineDOFBitmask;
514  _nActiveDOF = vJointIndices.size() + RaveGetAffineDOF(_nAffineDOFs);
515 
516  // do not initialize interpolation, since it implies a motion sampling strategy
517  int offset = 0;
518  _activespec._vgroups.resize(0);
519  if( GetActiveDOFIndices().size() > 0 ) {
521  stringstream ss;
522  ss << "joint_values " << GetName();
523  FOREACHC(it,GetActiveDOFIndices()) {
524  ss << " " << *it;
525  }
526  group.name = ss.str();
527  group.dof = (int)GetActiveDOFIndices().size();
528  group.offset = offset;
529  offset += group.dof;
530  _activespec._vgroups.push_back(group);
531  }
532  if( GetAffineDOF() > 0 ) {
534  group.name = str(boost::format("affine_transform %s %d")%GetName()%GetAffineDOF());
535  group.offset = offset;
536  group.dof = RaveGetAffineDOF(GetAffineDOF());
537  _activespec._vgroups.push_back(group);
538  }
539 
541 }
542 
543 void RobotBase::SetActiveDOFValues(const std::vector<dReal>& values, uint32_t bCheckLimits)
544 {
545  if(_nActiveDOF < 0) {
546  SetDOFValues(values,bCheckLimits);
547  return;
548  }
549  OPENRAVE_ASSERT_OP_FORMAT((int)values.size(),>=,GetActiveDOF(), "not enough values %d<%d",values.size()%GetActiveDOF(),ORE_InvalidArguments);
550 
551  Transform t;
552  if( (int)_vActiveDOFIndices.size() < _nActiveDOF ) {
553  t = GetTransform();
556  t.rot = quatMultiply(_vRotationQuatLimitStart, t.rot);
557  }
558  if( _vActiveDOFIndices.size() == 0 ) {
559  SetTransform(t);
560  }
561  }
562 
563  if( _vActiveDOFIndices.size() > 0 ) {
564  GetDOFValues(_vTempRobotJoints);
565  for(size_t i = 0; i < _vActiveDOFIndices.size(); ++i) {
566  _vTempRobotJoints[_vActiveDOFIndices[i]] = values[i];
567  }
568  if( (int)_vActiveDOFIndices.size() < _nActiveDOF ) {
569  SetDOFValues(_vTempRobotJoints, t, bCheckLimits);
570  }
571  else {
572  SetDOFValues(_vTempRobotJoints, bCheckLimits);
573  }
574  }
575 }
576 
577 void RobotBase::GetActiveDOFValues(std::vector<dReal>& values) const
578 {
579  if( _nActiveDOF < 0 ) {
580  GetDOFValues(values);
581  return;
582  }
583 
584  values.resize(GetActiveDOF());
585  if( values.size() == 0 ) {
586  return;
587  }
588  vector<dReal>::iterator itvalues = values.begin();
589  if( _vActiveDOFIndices.size() != 0 ) {
590  GetDOFValues(_vTempRobotJoints);
591  FOREACHC(it, _vActiveDOFIndices) {
592  *itvalues++ = _vTempRobotJoints[*it];
593  }
594  }
595 
597  return;
598  }
599  Transform t = GetTransform();
602  }
604 }
605 
606 void RobotBase::SetActiveDOFVelocities(const std::vector<dReal>& velocities, uint32_t bCheckLimits)
607 {
608  if(_nActiveDOF < 0) {
609  SetDOFVelocities(velocities,true);
610  return;
611  }
612 
613  Vector linearvel, angularvel;
614  if( (int)_vActiveDOFIndices.size() < _nActiveDOF ) {
615  // first set the affine transformation of the first link before setting joints
616  const dReal* pAffineValues = &velocities[_vActiveDOFIndices.size()];
617 
618  _veclinks.at(0)->GetVelocity(linearvel, angularvel);
619 
620  if( _nAffineDOFs & OpenRAVE::DOF_X ) linearvel.x = *pAffineValues++;
621  if( _nAffineDOFs & OpenRAVE::DOF_Y ) linearvel.y = *pAffineValues++;
622  if( _nAffineDOFs & OpenRAVE::DOF_Z ) linearvel.z = *pAffineValues++;
624  angularvel = vActvAffineRotationAxis * *pAffineValues++;
625  }
627  angularvel.x = *pAffineValues++;
628  angularvel.y = *pAffineValues++;
629  angularvel.z = *pAffineValues++;
630  }
632  throw OPENRAVE_EXCEPTION_FORMAT0("quaternions not supported",ORE_InvalidArguments);
633  }
634 
635  if( _vActiveDOFIndices.size() == 0 ) {
636  SetVelocity(linearvel, angularvel);
637  }
638  }
639 
640  if( _vActiveDOFIndices.size() > 0 ) {
641  GetDOFVelocities(_vTempRobotJoints);
642  std::vector<dReal>::const_iterator itvel = velocities.begin();
643  FOREACHC(it, _vActiveDOFIndices) {
644  _vTempRobotJoints[*it] = *itvel++;
645  }
646  if( (int)_vActiveDOFIndices.size() < _nActiveDOF ) {
647  SetDOFVelocities(_vTempRobotJoints,linearvel,angularvel,bCheckLimits);
648  }
649  else {
650  SetDOFVelocities(_vTempRobotJoints,bCheckLimits);
651  }
652  }
653 }
654 
655 void RobotBase::GetActiveDOFVelocities(std::vector<dReal>& velocities) const
656 {
657  if( _nActiveDOF < 0 ) {
658  GetDOFVelocities(velocities);
659  return;
660  }
661 
662  velocities.resize(GetActiveDOF());
663  if( velocities.size() == 0 )
664  return;
665  dReal* pVelocities = &velocities[0];
666  if( _vActiveDOFIndices.size() != 0 ) {
667  GetDOFVelocities(_vTempRobotJoints);
668  FOREACHC(it, _vActiveDOFIndices) {
669  *pVelocities++ = _vTempRobotJoints[*it];
670  }
671  }
672 
674  return;
675  }
676  Vector linearvel, angularvel;
677  _veclinks.at(0)->GetVelocity(linearvel, angularvel);
678 
679  if( _nAffineDOFs & OpenRAVE::DOF_X ) *pVelocities++ = linearvel.x;
680  if( _nAffineDOFs & OpenRAVE::DOF_Y ) *pVelocities++ = linearvel.y;
681  if( _nAffineDOFs & OpenRAVE::DOF_Z ) *pVelocities++ = linearvel.z;
683 
684  *pVelocities++ = vActvAffineRotationAxis.dot3(angularvel);
685  }
687  *pVelocities++ = angularvel.x;
688  *pVelocities++ = angularvel.y;
689  *pVelocities++ = angularvel.z;
690  }
692  throw OPENRAVE_EXCEPTION_FORMAT0("quaternions not supported",ORE_InvalidArguments);
693  }
694 }
695 
696 void RobotBase::GetActiveDOFLimits(std::vector<dReal>& lower, std::vector<dReal>& upper) const
697 {
698  lower.resize(GetActiveDOF());
699  upper.resize(GetActiveDOF());
700  if( GetActiveDOF() == 0 ) {
701  return;
702  }
703  dReal* pLowerLimit = &lower[0];
704  dReal* pUpperLimit = &upper[0];
705  vector<dReal> alllower,allupper;
706 
707  if( _nAffineDOFs == 0 ) {
708  if( _nActiveDOF < 0 ) {
709  GetDOFLimits(lower,upper);
710  return;
711  }
712  else {
713  GetDOFLimits(alllower,allupper);
714  FOREACHC(it, _vActiveDOFIndices) {
715  *pLowerLimit++ = alllower[*it];
716  *pUpperLimit++ = allupper[*it];
717  }
718  }
719  }
720  else {
721  if( _vActiveDOFIndices.size() > 0 ) {
722  GetDOFLimits(alllower,allupper);
723  FOREACHC(it, _vActiveDOFIndices) {
724  *pLowerLimit++ = alllower[*it];
725  *pUpperLimit++ = allupper[*it];
726  }
727  }
728 
729  if( _nAffineDOFs & OpenRAVE::DOF_X ) {
730  *pLowerLimit++ = _vTranslationLowerLimits.x;
731  *pUpperLimit++ = _vTranslationUpperLimits.x;
732  }
733  if( _nAffineDOFs & OpenRAVE::DOF_Y ) {
734  *pLowerLimit++ = _vTranslationLowerLimits.y;
735  *pUpperLimit++ = _vTranslationUpperLimits.y;
736  }
737  if( _nAffineDOFs & OpenRAVE::DOF_Z ) {
738  *pLowerLimit++ = _vTranslationLowerLimits.z;
739  *pUpperLimit++ = _vTranslationUpperLimits.z;
740  }
741 
743  *pLowerLimit++ = _vRotationAxisLowerLimits.x;
744  *pUpperLimit++ = _vRotationAxisUpperLimits.x;
745  }
747  *pLowerLimit++ = _vRotation3DLowerLimits.x;
748  *pLowerLimit++ = _vRotation3DLowerLimits.y;
749  *pLowerLimit++ = _vRotation3DLowerLimits.z;
750  *pUpperLimit++ = _vRotation3DUpperLimits.x;
751  *pUpperLimit++ = _vRotation3DUpperLimits.y;
752  *pUpperLimit++ = _vRotation3DUpperLimits.z;
753  }
755  // this is actually difficult to do correctly...
757  *pLowerLimit++ = RaveCos(_fQuatLimitMaxAngle);
758  *pLowerLimit++ = -fsin;
759  *pLowerLimit++ = -fsin;
760  *pLowerLimit++ = -fsin;
761  *pUpperLimit++ = 1;
762  *pUpperLimit++ = fsin;
763  *pUpperLimit++ = fsin;
764  *pUpperLimit++ = fsin;
765  }
766  }
767 }
768 
769 void RobotBase::GetActiveDOFResolutions(std::vector<dReal>& resolution) const
770 {
771  if( _nActiveDOF < 0 ) {
772  GetDOFResolutions(resolution);
773  return;
774  }
775 
776  resolution.resize(GetActiveDOF());
777  if( resolution.size() == 0 ) {
778  return;
779  }
780  dReal* pResolution = &resolution[0];
781 
782  GetDOFResolutions(_vTempRobotJoints);
783  FOREACHC(it, _vActiveDOFIndices) {
784  *pResolution++ = _vTempRobotJoints[*it];
785  }
786  // set some default limits
787  if( _nAffineDOFs & OpenRAVE::DOF_X ) {
788  *pResolution++ = _vTranslationResolutions.x;
789  }
790  if( _nAffineDOFs & OpenRAVE::DOF_Y ) {
791  *pResolution++ = _vTranslationResolutions.y;
792  }
793  if( _nAffineDOFs & OpenRAVE::DOF_Z ) { *pResolution++ = _vTranslationResolutions.z; }
794 
796  *pResolution++ = _vRotationAxisResolutions.x;
797  }
799  *pResolution++ = _vRotation3DResolutions.x;
800  *pResolution++ = _vRotation3DResolutions.y;
801  *pResolution++ = _vRotation3DResolutions.z;
802  }
804  *pResolution++ = _fQuatLimitMaxAngle;
805  *pResolution++ = _fQuatLimitMaxAngle;
806  *pResolution++ = _fQuatLimitMaxAngle;
807  *pResolution++ = _fQuatLimitMaxAngle;
808  }
809 }
810 
811 void RobotBase::GetActiveDOFWeights(std::vector<dReal>& weights) const
812 {
813  if( _nActiveDOF < 0 ) {
814  GetDOFWeights(weights);
815  return;
816  }
817 
818  weights.resize(GetActiveDOF());
819  if( weights.size() == 0 ) {
820  return;
821  }
822  dReal* pweight = &weights[0];
823 
824  GetDOFWeights(_vTempRobotJoints);
825  FOREACHC(it, _vActiveDOFIndices) {
826  *pweight++ = _vTempRobotJoints[*it];
827  }
828  // set some default limits
829  if( _nAffineDOFs & OpenRAVE::DOF_X ) { *pweight++ = _vTranslationWeights.x; }
830  if( _nAffineDOFs & OpenRAVE::DOF_Y ) { *pweight++ = _vTranslationWeights.y; }
831  if( _nAffineDOFs & OpenRAVE::DOF_Z ) { *pweight++ = _vTranslationWeights.z; }
832 
835  *pweight++ = _vRotation3DWeights.x;
836  *pweight++ = _vRotation3DWeights.y;
837  *pweight++ = _vRotation3DWeights.z;
838  }
840  *pweight++ = _fQuatAngleWeight;
841  *pweight++ = _fQuatAngleWeight;
842  *pweight++ = _fQuatAngleWeight;
843  *pweight++ = _fQuatAngleWeight;
844  }
845 }
846 
847 void RobotBase::GetActiveDOFVelocityLimits(std::vector<dReal>& maxvel) const
848 {
849  std::vector<dReal> dummy;
850  if( _nActiveDOF < 0 ) {
851  GetDOFVelocityLimits(dummy,maxvel);
852  return;
853  }
854  maxvel.resize(GetActiveDOF());
855  if( maxvel.size() == 0 ) {
856  return;
857  }
858  dReal* pMaxVel = &maxvel[0];
859 
860  GetDOFVelocityLimits(dummy,_vTempRobotJoints);
861  FOREACHC(it, _vActiveDOFIndices) {
862  *pMaxVel++ = _vTempRobotJoints[*it];
863  }
864  if( _nAffineDOFs & OpenRAVE::DOF_X ) { *pMaxVel++ = _vTranslationMaxVels.x; }
865  if( _nAffineDOFs & OpenRAVE::DOF_Y ) { *pMaxVel++ = _vTranslationMaxVels.y; }
866  if( _nAffineDOFs & OpenRAVE::DOF_Z ) { *pMaxVel++ = _vTranslationMaxVels.z; }
867 
870  *pMaxVel++ = _vRotation3DMaxVels.x;
871  *pMaxVel++ = _vRotation3DMaxVels.y;
872  *pMaxVel++ = _vRotation3DMaxVels.z;
873  }
875  *pMaxVel++ = _fQuatMaxAngleVelocity;
876  *pMaxVel++ = _fQuatMaxAngleVelocity;
877  *pMaxVel++ = _fQuatMaxAngleVelocity;
878  *pMaxVel++ = _fQuatMaxAngleVelocity;
879  }
880 }
881 
882 void RobotBase::GetActiveDOFAccelerationLimits(std::vector<dReal>& maxaccel) const
883 {
884  if( _nActiveDOF < 0 ) {
885  GetDOFAccelerationLimits(maxaccel);
886  return;
887  }
888  maxaccel.resize(GetActiveDOF());
889  if( maxaccel.size() == 0 ) {
890  return;
891  }
892  dReal* pMaxAccel = &maxaccel[0];
893 
894  GetDOFAccelerationLimits(_vTempRobotJoints);
895  FOREACHC(it, _vActiveDOFIndices) {
896  *pMaxAccel++ = _vTempRobotJoints[*it];
897  }
898  if( _nAffineDOFs & OpenRAVE::DOF_X ) { *pMaxAccel++ = _vTranslationMaxVels.x; } // wrong
899  if( _nAffineDOFs & OpenRAVE::DOF_Y ) { *pMaxAccel++ = _vTranslationMaxVels.y; } // wrong
900  if( _nAffineDOFs & OpenRAVE::DOF_Z ) { *pMaxAccel++ = _vTranslationMaxVels.z; } // wrong
901 
902  if( _nAffineDOFs & OpenRAVE::DOF_RotationAxis ) { *pMaxAccel++ = _vRotationAxisMaxVels.x; } // wrong
904  *pMaxAccel++ = _vRotation3DMaxVels.x; // wrong
905  *pMaxAccel++ = _vRotation3DMaxVels.y; // wrong
906  *pMaxAccel++ = _vRotation3DMaxVels.z; // wrong
907  }
909  *pMaxAccel++ = _fQuatMaxAngleVelocity; // wrong
910  *pMaxAccel++ = _fQuatMaxAngleVelocity; // wrong
911  *pMaxAccel++ = _fQuatMaxAngleVelocity; // wrong
912  *pMaxAccel++ = _fQuatMaxAngleVelocity; // wrong
913  }
914 }
915 
916 void RobotBase::SubtractActiveDOFValues(std::vector<dReal>& q1, const std::vector<dReal>& q2) const
917 {
918  if( _nActiveDOF < 0 ) {
919  SubtractDOFValues(q1,q2);
920  return;
921  }
922 
923  // go through all active joints
924  size_t index = 0;
925  for(; index < _vActiveDOFIndices.size(); ++index) {
927  q1.at(index) = pjoint->SubtractValue(q1.at(index),q2.at(index),_vActiveDOFIndices[index]-pjoint->GetDOFIndex());
928  }
929 
930  if( _nAffineDOFs & OpenRAVE::DOF_X ) {
931  q1.at(index) -= q2.at(index);
932  index++;
933  }
934  if( _nAffineDOFs & OpenRAVE::DOF_Y ) {
935  q1.at(index) -= q2.at(index);
936  index++;
937  }
938  if( _nAffineDOFs & OpenRAVE::DOF_Z ) {
939  q1.at(index) -= q2.at(index);
940  index++;
941  }
942 
944  q1.at(index) = utils::SubtractCircularAngle(q1.at(index),q2.at(index));
945  index++;
946  }
948  q1.at(index) -= q2.at(index); index++;
949  q1.at(index) -= q2.at(index); index++;
950  q1.at(index) -= q2.at(index); index++;
951  }
953  // would like to do q2^-1 q1, but that might break rest of planners...?
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++;
958  }
959 }
960 
961 const std::vector<int>& RobotBase::GetActiveDOFIndices() const
962 {
964 }
965 
967 {
968  if( interpolation.size() == 0 ) {
969  return _activespec;
970  }
972  FOREACH(itgroup,spec._vgroups) {
973  itgroup->interpolation=interpolation;
974  }
975  return spec;
976 }
977 
978 void RobotBase::CalculateActiveJacobian(int index, const Vector& offset, vector<dReal>& vjacobian) const
979 {
980  if( _nActiveDOF < 0 ) {
981  CalculateJacobian(index, offset, vjacobian);
982  return;
983  }
984 
985  int dofstride = GetActiveDOF();
986  vjacobian.resize(3*dofstride);
987  if( _vActiveDOFIndices.size() != 0 ) {
989  ComputeJacobianTranslation(index, offset, vjacobian, _vActiveDOFIndices);
990  return;
991  }
992  // have to copy
993  std::vector<dReal> vjacobianjoints;
994  ComputeJacobianTranslation(index, offset, vjacobianjoints, _vActiveDOFIndices);
995  for(size_t i = 0; i < 3; ++i) {
996  std::copy(vjacobianjoints.begin()+i*_vActiveDOFIndices.size(),vjacobianjoints.begin()+(i+1)*_vActiveDOFIndices.size(),vjacobian.begin()+i*dofstride);
997  }
998  }
999 
1001  return;
1002  }
1003  size_t ind = _vActiveDOFIndices.size();
1004  if( _nAffineDOFs & OpenRAVE::DOF_X ) {
1005  vjacobian[ind] = 1;
1006  vjacobian[dofstride+ind] = 0;
1007  vjacobian[2*dofstride+ind] = 0;
1008  ind++;
1009  }
1010  if( _nAffineDOFs & OpenRAVE::DOF_Y ) {
1011  vjacobian[ind] = 0;
1012  vjacobian[dofstride+ind] = 1;
1013  vjacobian[2*dofstride+ind] = 0;
1014  ind++;
1015  }
1016  if( _nAffineDOFs & OpenRAVE::DOF_Z ) {
1017  vjacobian[ind] = 0;
1018  vjacobian[dofstride+ind] = 0;
1019  vjacobian[2*dofstride+ind] = 1;
1020  ind++;
1021  }
1023  Vector vj = vActvAffineRotationAxis.cross(offset-GetTransform().trans);
1024  vjacobian[ind] = vj.x;
1025  vjacobian[dofstride+ind] = vj.y;
1026  vjacobian[2*dofstride+ind] = vj.z;
1027  ind++;
1028  }
1029  else if( _nAffineDOFs & OpenRAVE::DOF_Rotation3D ) {
1030  // have to take the partial derivative dT/dA of the axis*angle representation with respect to the transformation it induces
1031  // can introduce converting to quaternions in the middle, then by chain rule, dT/dA = dT/tQ * dQ/dA
1032  // for questions on derivation email rdiankov@cs.cmu.edu
1033  Transform t = GetTransform();
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;
1036 
1037  // after some math, the dT/dQ looks like:
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 };
1041 
1042  // calc dQ/dA
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;
1050 
1051  if( RaveFabs(fangle) < 1e-8f )
1052  fangle = 1e-8f;
1053 
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)};
1063 
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];
1067  }
1068  }
1069  ind += 3;
1070  }
1072  Transform t; t.identity(); t.rot = quatInverse(_vRotationQuatLimitStart);
1073  t = t * GetTransform();
1074  // note: qw, qx, qy, qz here follow the standard quaternion convention, not the openrave one
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;
1078 
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;
1086  }
1087 
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];
1091  }
1092  }
1093  ind += 4;
1094  }
1095 }
1096 
1097 void RobotBase::CalculateActiveJacobian(int linkindex, const Vector& offset, boost::multi_array<dReal,2>& mjacobian) const
1098 {
1099  if( _nActiveDOF < 0 ) {
1100  CalculateJacobian(linkindex, offset, mjacobian);
1101  return;
1102  }
1103  std::vector<dReal> vjacobian;
1104  RobotBase::CalculateActiveJacobian(linkindex,offset,vjacobian);
1105  OPENRAVE_ASSERT_OP((int)vjacobian.size(),==,3*GetActiveDOF());
1106  mjacobian.resize(boost::extents[3][GetActiveDOF()]);
1107  vector<dReal>::const_iterator itsrc = vjacobian.begin();
1108  FOREACH(itdst,mjacobian) {
1109  std::copy(itsrc,itsrc+GetActiveDOF(),itdst->begin());
1110  itsrc += GetActiveDOF();
1111  }
1112 }
1113 
1114 void RobotBase::CalculateActiveRotationJacobian(int index, const Vector& q, std::vector<dReal>& vjacobian) const
1115 {
1116  if( _nActiveDOF < 0 ) {
1117  CalculateRotationJacobian(index, q, vjacobian);
1118  return;
1119  }
1120  int dofstride = GetActiveDOF();
1121  vjacobian.resize(4*dofstride);
1122  if( _vActiveDOFIndices.size() != 0 ) {
1123  std::vector<dReal> vjacobianjoints;
1124  CalculateRotationJacobian(index, q, vjacobianjoints);
1125  for(size_t i = 0; i < _vActiveDOFIndices.size(); ++i) {
1126  vjacobian[i] = vjacobianjoints[_vActiveDOFIndices[i]];
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]];
1130  }
1131  }
1132 
1134  return;
1135  }
1136 
1137  size_t ind = _vActiveDOFIndices.size();
1138  if( _nAffineDOFs & OpenRAVE::DOF_X ) {
1139  vjacobian[ind] = 0;
1140  vjacobian[dofstride+ind] = 0;
1141  vjacobian[2*dofstride+ind] = 0;
1142  vjacobian[3*dofstride+ind] = 0;
1143  ind++;
1144  }
1145  if( _nAffineDOFs & OpenRAVE::DOF_Y ) {
1146  vjacobian[ind] = 0;
1147  vjacobian[dofstride+ind] = 0;
1148  vjacobian[2*dofstride+ind] = 0;
1149  vjacobian[3*dofstride+ind] = 0;
1150  ind++;
1151  }
1152  if( _nAffineDOFs & OpenRAVE::DOF_Z ) {
1153  vjacobian[ind] = 0;
1154  vjacobian[dofstride+ind] = 0;
1155  vjacobian[2*dofstride+ind] = 0;
1156  vjacobian[3*dofstride+ind] = 0;
1157  ind++;
1158  }
1159  if( _nAffineDOFs & OpenRAVE::DOF_RotationAxis ) {
1160  const Vector& v = vActvAffineRotationAxis;
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);
1165  ind++;
1166  }
1167  else if( _nAffineDOFs & OpenRAVE::DOF_Rotation3D ) {
1168  throw OPENRAVE_EXCEPTION_FORMAT("robot %s rotation 3d not supported, affine=%d",GetName()%_nAffineDOFs,ORE_NotImplemented);
1169  ind += 3;
1170  }
1171  else if( _nAffineDOFs & OpenRAVE::DOF_RotationQuat ) {
1172  throw OPENRAVE_EXCEPTION_FORMAT("robot %s quaternion not supported, affine=%d",GetName()%_nAffineDOFs,ORE_NotImplemented);
1173  ind += 4;
1174  }
1175 }
1176 
1177 void RobotBase::CalculateActiveRotationJacobian(int linkindex, const Vector& q, boost::multi_array<dReal,2>& mjacobian) const
1178 {
1179  if( _nActiveDOF < 0 ) {
1180  CalculateRotationJacobian(linkindex, q, mjacobian);
1181  return;
1182  }
1183  std::vector<dReal> vjacobian;
1184  RobotBase::CalculateActiveRotationJacobian(linkindex,q,vjacobian);
1185  OPENRAVE_ASSERT_OP((int)vjacobian.size(),==,4*GetActiveDOF());
1186  mjacobian.resize(boost::extents[4][GetActiveDOF()]);
1187  vector<dReal>::const_iterator itsrc = vjacobian.begin();
1188  FOREACH(itdst,mjacobian) {
1189  std::copy(itsrc,itsrc+GetActiveDOF(),itdst->begin());
1190  itsrc += GetActiveDOF();
1191  }
1192 }
1193 
1194 void RobotBase::CalculateActiveAngularVelocityJacobian(int index, std::vector<dReal>& vjacobian) const
1195 {
1196  if( _nActiveDOF < 0 ) {
1197  ComputeJacobianAxisAngle(index, vjacobian);
1198  return;
1199  }
1200 
1201  int dofstride = GetActiveDOF();
1202  vjacobian.resize(3*dofstride);
1203  if( _vActiveDOFIndices.size() != 0 ) {
1205  ComputeJacobianAxisAngle(index, vjacobian, _vActiveDOFIndices);
1206  return;
1207  }
1208  // have to copy
1209  std::vector<dReal> vjacobianjoints;
1210  ComputeJacobianAxisAngle(index, vjacobianjoints, _vActiveDOFIndices);
1211  for(size_t i = 0; i < 3; ++i) {
1212  std::copy(vjacobianjoints.begin()+i*_vActiveDOFIndices.size(),vjacobianjoints.begin()+(i+1)*_vActiveDOFIndices.size(),vjacobianjoints.begin()+i*dofstride);
1213  }
1214  }
1215 
1217  return;
1218  }
1219  size_t ind = _vActiveDOFIndices.size();
1220  if( _nAffineDOFs & OpenRAVE::DOF_X ) {
1221  vjacobian[ind] = 0;
1222  vjacobian[dofstride+ind] = 0;
1223  vjacobian[2*dofstride+ind] = 0;
1224  ind++;
1225  }
1226  if( _nAffineDOFs & OpenRAVE::DOF_Y ) {
1227  vjacobian[ind] = 0;
1228  vjacobian[dofstride+ind] = 0;
1229  vjacobian[2*dofstride+ind] = 0;
1230  ind++;
1231  }
1232  if( _nAffineDOFs & OpenRAVE::DOF_Z ) {
1233  vjacobian[ind] = 0;
1234  vjacobian[dofstride+ind] = 0;
1235  vjacobian[2*dofstride+ind] = 0;
1236  ind++;
1237  }
1238  if( _nAffineDOFs & OpenRAVE::DOF_RotationAxis ) {
1239  const Vector& v = vActvAffineRotationAxis;
1240  vjacobian[ind] = v.x;
1241  vjacobian[dofstride+ind] = v.y;
1242  vjacobian[2*dofstride+ind] = v.z;
1243 
1244  }
1245  else if( _nAffineDOFs & OpenRAVE::DOF_Rotation3D ) {
1246  throw OPENRAVE_EXCEPTION_FORMAT("robot %s rotation 3d not supported, affine=%d",GetName()%_nAffineDOFs,ORE_NotImplemented);
1247  }
1248  else if( _nAffineDOFs & OpenRAVE::DOF_RotationQuat ) {
1249  throw OPENRAVE_EXCEPTION_FORMAT("robot %s quaternion not supported, affine=%d",GetName()%_nAffineDOFs,ORE_NotImplemented);
1250 
1251  // most likely wrong
1253  t = t * GetTransform();
1254  dReal fnorm = t.rot.y*t.rot.y+t.rot.z*t.rot.z+t.rot.w*t.rot.w;
1255  if( fnorm > 0 ) {
1256  fnorm = dReal(1)/RaveSqrt(fnorm);
1257  vjacobian[ind] = t.rot.y*fnorm;
1258  vjacobian[dofstride+ind] = t.rot.z*fnorm;
1259  vjacobian[2*dofstride+ind] = t.rot.w*fnorm;
1260  }
1261  else {
1262  vjacobian[ind] = 0;
1263  vjacobian[dofstride+ind] = 0;
1264  vjacobian[2*dofstride+ind] = 0;
1265  }
1266 
1267  ++ind;
1268  }
1269 }
1270 
1271 void RobotBase::CalculateActiveAngularVelocityJacobian(int linkindex, boost::multi_array<dReal,2>& mjacobian) const
1272 {
1273  if( _nActiveDOF < 0 ) {
1274  CalculateAngularVelocityJacobian(linkindex, mjacobian);
1275  return;
1276  }
1277  std::vector<dReal> vjacobian;
1278  CalculateActiveAngularVelocityJacobian(linkindex,vjacobian);
1279  OPENRAVE_ASSERT_OP((int)vjacobian.size(),==,3*GetActiveDOF());
1280  mjacobian.resize(boost::extents[3][GetActiveDOF()]);
1281  vector<dReal>::const_iterator itsrc = vjacobian.begin();
1282  FOREACH(itdst,mjacobian) {
1283  std::copy(itsrc,itsrc+GetActiveDOF(),itdst->begin());
1284  itsrc += GetActiveDOF();
1285  }
1286 }
1287 
1288 const std::set<int>& RobotBase::GetNonAdjacentLinks(int adjacentoptions) const
1289 {
1290  KinBody::GetNonAdjacentLinks(0); // need to call to set the cache
1291  if( (_nNonAdjacentLinkCache&adjacentoptions) != adjacentoptions ) {
1292  int requestedoptions = (~_nNonAdjacentLinkCache)&adjacentoptions;
1293  // find out what needs to computed
1294  boost::array<uint8_t,4> compute={ { 0,0,0,0}};
1295  if( requestedoptions & AO_Enabled ) {
1296  for(size_t i = 0; i < compute.size(); ++i) {
1297  if( i & AO_Enabled ) {
1298  compute[i] = 1;
1299  }
1300  }
1301  }
1302  if( requestedoptions & AO_ActiveDOFs ) {
1303  for(size_t i = 0; i < compute.size(); ++i) {
1304  if( i & AO_ActiveDOFs ) {
1305  compute[i] = 1;
1306  }
1307  }
1308  }
1309  if( requestedoptions & ~(AO_Enabled|AO_ActiveDOFs) ) {
1310  throw OPENRAVE_EXCEPTION_FORMAT("does not support adjacentoptions %d",adjacentoptions,ORE_InvalidArguments);
1311  }
1312 
1313  // compute it
1314  if( compute.at(AO_Enabled) ) {
1315  _setNonAdjacentLinks.at(AO_Enabled).clear();
1316  FOREACHC(itset, _setNonAdjacentLinks[0]) {
1317  KinBody::LinkConstPtr plink1(_veclinks.at(*itset&0xffff)), plink2(_veclinks.at(*itset>>16));
1318  if( plink1->IsEnabled() && plink2->IsEnabled() ) {
1319  _setNonAdjacentLinks[AO_Enabled].insert(*itset);
1320  }
1321  }
1322  }
1323  if( compute.at(AO_ActiveDOFs) ) {
1324  _setNonAdjacentLinks.at(AO_ActiveDOFs).clear();
1325  FOREACHC(itset, _setNonAdjacentLinks[0]) {
1326  FOREACHC(it, GetActiveDOFIndices()) {
1327  if( IsDOFInChain(*itset&0xffff,*itset>>16,*it) ) {
1328  _setNonAdjacentLinks[AO_ActiveDOFs].insert(*itset);
1329  break;
1330  }
1331  }
1332  }
1333  }
1334  if( compute.at(AO_Enabled|AO_ActiveDOFs) ) {
1335  _setNonAdjacentLinks.at(AO_Enabled|AO_ActiveDOFs).clear();
1336  FOREACHC(itset, _setNonAdjacentLinks[AO_ActiveDOFs]) {
1337  KinBody::LinkConstPtr plink1(_veclinks.at(*itset&0xffff)), plink2(_veclinks.at(*itset>>16));
1338  if( plink1->IsEnabled() && plink2->IsEnabled() ) {
1339  _setNonAdjacentLinks[AO_Enabled|AO_ActiveDOFs].insert(*itset);
1340  }
1341  }
1342  }
1343  _nNonAdjacentLinkCache |= requestedoptions;
1344  }
1345  return _setNonAdjacentLinks.at(adjacentoptions);
1346 }
1347 
1349 {
1351  if( !pmanip ) {
1352  return false;
1353  }
1354  return Grab(pbody, pmanip->GetEndEffector());
1355 }
1356 
1357 bool RobotBase::Grab(KinBodyPtr pbody, const std::set<int>& setRobotLinksToIgnore)
1358 {
1360  if( !pmanip ) {
1361  return false;
1362  }
1363  return Grab(pbody, pmanip->GetEndEffector(), setRobotLinksToIgnore);
1364 }
1365 
1367 {
1368  OPENRAVE_ASSERT_FORMAT(!!pbody, "robot %s invalid bod to grab",GetName(),ORE_InvalidArguments);
1369  OPENRAVE_ASSERT_FORMAT(!!plink && plink->GetParent() == shared_kinbody(), "robot %s grabbing link needs to be part of robot",GetName(),ORE_InvalidArguments);
1370  OPENRAVE_ASSERT_FORMAT(pbody != shared_kinbody(),"robot %s cannot grab itself",GetName(), ORE_InvalidArguments);
1371  if( IsGrabbing(pbody) ) {
1372  RAVELOG_VERBOSE(str(boost::format("Robot %s: body %s already grabbed\n")%GetName()%pbody->GetName()));
1373  return true;
1374  }
1375 
1376  GrabbedPtr pgrabbed(new Grabbed(pbody,plink));
1377  Transform t = plink->GetTransform();
1378  Transform tbody = pbody->GetTransform();
1379  pgrabbed->_troot = t.inverse() * tbody;
1380  pgrabbed->_ProcessCollidingLinks(std::set<int>());
1381  // set velocity
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);
1385  _vGrabbedBodies.push_back(pgrabbed);
1386  _AttachBody(pbody);
1387  return true;
1388 }
1389 
1390 bool RobotBase::Grab(KinBodyPtr pbody, LinkPtr pRobotLinkToGrabWith, const std::set<int>& setRobotLinksToIgnore)
1391 {
1392  OPENRAVE_ASSERT_FORMAT(!!pbody && !!pRobotLinkToGrabWith && pRobotLinkToGrabWith->GetParent() == shared_kinbody(), "robot %s invalid grab arguments",GetName(), ORE_InvalidArguments);
1393  OPENRAVE_ASSERT_FORMAT(pbody != shared_kinbody(), "robot %s cannot grab itself",pbody->GetName(), ORE_InvalidArguments);
1394  if( IsGrabbing(pbody) ) {
1395  RAVELOG_VERBOSE(str(boost::format("Robot %s: body %s already grabbed\n")%GetName()%pbody->GetName()));
1396  return true;
1397  }
1398 
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);
1404  // set velocity
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);
1408  _vGrabbedBodies.push_back(pgrabbed);
1409  _AttachBody(pbody);
1410  return true;
1411 }
1412 
1414 {
1415  FOREACH(itgrabbed, _vGrabbedBodies) {
1416  GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
1417  if( KinBodyPtr(pgrabbed->_pgrabbedbody) == pbody ) {
1418  _vGrabbedBodies.erase(itgrabbed);
1419  _RemoveAttachedBody(pbody);
1420  return;
1421  }
1422  }
1423 
1424  RAVELOG_DEBUG(str(boost::format("Robot %s: body %s not grabbed\n")%GetName()%pbody->GetName()));
1425 }
1426 
1428 {
1429  FOREACH(itgrabbed, _vGrabbedBodies) {
1430  GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
1431  KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
1432  if( !!pbody ) {
1433  _RemoveAttachedBody(pbody);
1434  }
1435  }
1436  _vGrabbedBodies.clear();
1437 }
1438 
1440 {
1441  CollisionOptionsStateSaver colsaver(GetEnv()->GetCollisionChecker(),0); // have to reset the collision options
1442  std::vector<LinkPtr > vattachedlinks;
1443  FOREACH(itgrabbed, _vGrabbedBodies) {
1444  GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
1445  KinBodyPtr pbody(pgrabbed->_pgrabbedbody);
1446  if( !!pbody ) {
1447  _RemoveAttachedBody(pbody);
1448  CallOnDestruction destructionhook(boost::bind(&RobotBase::_AttachBody,this,pbody));
1449  pgrabbed->_ProcessCollidingLinks(pgrabbed->_setRobotLinksToIgnore);
1450  }
1451  }
1452 }
1453 
1455 {
1456  GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(_pgrabbed);
1457  KinBodyPtr pgrabbedbody = pgrabbed->_pgrabbedbody.lock();
1458  if( !!pgrabbedbody ) {
1459  // have to re-grab the body, which means temporarily resetting the collision checker and attachment
1460  CollisionOptionsStateSaver colsaver(GetEnv()->GetCollisionChecker(),0); // have to reset the collision options
1461  _RemoveAttachedBody(pgrabbedbody);
1462  CallOnDestruction destructionhook(boost::bind(&RobotBase::_AttachBody,this,pgrabbedbody));
1463  pgrabbed->_ProcessCollidingLinks(pgrabbed->_setRobotLinksToIgnore);
1464  }
1465 }
1466 
1468 {
1469  FOREACHC(itgrabbed, _vGrabbedBodies) {
1470  GrabbedPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed>(*itgrabbed);
1471  if( KinBodyConstPtr(pgrabbed->_pgrabbedbody) == pbody ) {
1472  return pgrabbed->_plinkrobot;
1473  }
1474  }
1475  return LinkPtr();
1476 }
1477 
1478 void RobotBase::GetGrabbed(std::vector<KinBodyPtr>& vbodies) const
1479 {
1480  vbodies.resize(0);
1481  FOREACHC(itgrabbed, _vGrabbedBodies) {
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);
1486  }
1487  }
1488 }
1489 
1490 void RobotBase::GetGrabbedInfo(std::vector<RobotBase::GrabbedInfoPtr>& vgrabbedinfo) const
1491 {
1492  vgrabbedinfo.resize(_vGrabbedBodies.size());
1493  for(size_t i = 0; i < vgrabbedinfo.size(); ++i) {
1494  GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(_vGrabbedBodies[i]);
1495  vgrabbedinfo[i].reset(new GrabbedInfo());
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;
1500  FOREACHC(itlink, _veclinks) {
1501  if( find(pgrabbed->_listNonCollidingLinks.begin(), pgrabbed->_listNonCollidingLinks.end(), *itlink) == pgrabbed->_listNonCollidingLinks.end() ) {
1502  vgrabbedinfo[i]->_setRobotLinksToIgnore.insert((*itlink)->GetIndex());
1503  }
1504  }
1505  }
1506 }
1507 
1508 void RobotBase::ResetGrabbed(const std::vector<RobotBase::GrabbedInfoConstPtr>& vgrabbedinfo)
1509 {
1511  CollisionOptionsStateSaver colsaver(GetEnv()->GetCollisionChecker(),0); // have to reset the collision options
1512  FOREACHC(itgrabbedinfo, vgrabbedinfo) {
1513  GrabbedInfoConstPtr pgrabbedinfo = *itgrabbedinfo;
1514  KinBodyPtr pbody = GetEnv()->GetKinBody(pgrabbedinfo->_grabbedname);
1515  KinBody::LinkPtr pRobotLinkToGrabWith = GetLink(pgrabbedinfo->_robotlinkname);
1516  OPENRAVE_ASSERT_FORMAT(!!pbody && !!pRobotLinkToGrabWith, "robot %s invalid grab arguments",GetName(), ORE_InvalidArguments);
1517  OPENRAVE_ASSERT_FORMAT(pbody != shared_kinbody(), "robot %s cannot grab itself",pbody->GetName(), ORE_InvalidArguments);
1518  if( IsGrabbing(pbody) ) {
1519  RAVELOG_VERBOSE(str(boost::format("Robot %s: body %s already grabbed\n")%GetName()%pbody->GetName()));
1520  continue;
1521  }
1522 
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);
1529  // set velocity
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);
1533  _vGrabbedBodies.push_back(pgrabbed);
1534  _AttachBody(pbody);
1535  }
1536 }
1537 
1538 void RobotBase::GetIgnoredLinksOfGrabbed(KinBodyConstPtr body, std::list<KinBody::LinkConstPtr>& ignorelinks) const
1539 {
1540  ignorelinks.clear();
1541  FOREACHC(itgrabbed, _vGrabbedBodies) {
1542  GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed);
1543  KinBodyPtr grabbedbody = pgrabbed->_pgrabbedbody.lock();
1544  if( grabbedbody == body ) {
1545  FOREACHC(itrobotlink, _veclinks) {
1546  if( find(pgrabbed->_listNonCollidingLinks.begin(), pgrabbed->_listNonCollidingLinks.end(), *itrobotlink) == pgrabbed->_listNonCollidingLinks.end() ) {
1547  ignorelinks.push_back(*itrobotlink);
1548  }
1549  }
1550  return;
1551  }
1552  }
1553  RAVELOG_WARN(str(boost::format("body %s is not currently grabbed")%body->GetName()));
1554 }
1555 
1557 {
1558  _pManipActive = _vecManipulators.at(index);
1559 }
1560 
1562 {
1563  if( !pmanip ) {
1564  _pManipActive.reset();
1565  }
1566  else {
1567  FOREACH(itmanip,_vecManipulators) {
1568  if( *itmanip == pmanip ) {
1569  _pManipActive = *itmanip;
1570  return;
1571  }
1572  }
1573  throw OPENRAVE_EXCEPTION_FORMAT("failed to find manipulator with name: %s", pmanip->GetName(), ORE_InvalidArguments);
1574  }
1575 }
1576 
1578 {
1579  if( manipname.size() > 0 ) {
1580  FOREACH(itmanip,_vecManipulators) {
1581  if( (*itmanip)->GetName() == manipname ) {
1582  _pManipActive = *itmanip;
1583  return _pManipActive;
1584  }
1585  }
1586  throw OPENRAVE_EXCEPTION_FORMAT("failed to find manipulator with name: %s", manipname, ORE_InvalidArguments);
1587  }
1588  _pManipActive.reset();
1589  return _pManipActive;
1590 }
1591 
1593 {
1594  return _pManipActive;
1595 }
1596 
1598 {
1599  return _pManipActive;
1600 }
1601 
1603 {
1604  for(size_t i = 0; i < _vecManipulators.size(); ++i) {
1605  if( _pManipActive == _vecManipulators[i] ) {
1606  return (int)i;
1607  }
1608  }
1609  return -1;
1610 }
1611 
1613 {
1614  OPENRAVE_ASSERT_OP(manipinfo._name.size(),>,0);
1615  FOREACH(itmanip,_vecManipulators) {
1616  if( (*itmanip)->GetName() == manipinfo._name ) {
1617  throw OPENRAVE_EXCEPTION_FORMAT("manipulator with name %s already exists",manipinfo._name,ORE_InvalidArguments);
1618  }
1619  }
1620  ManipulatorPtr newmanip(new Manipulator(shared_robot(),manipinfo));
1621  newmanip->_ComputeInternalInformation();
1622  _vecManipulators.push_back(newmanip);
1623  __hashrobotstructure.resize(0);
1624  return newmanip;
1625 }
1626 
1628 {
1629  if( _pManipActive == manip ) {
1630  _pManipActive.reset();
1631  }
1632  FOREACH(itmanip,_vecManipulators) {
1633  if( *itmanip == manip ) {
1634  _vecManipulators.erase(itmanip);
1635  __hashrobotstructure.resize(0);
1636  return;
1637  }
1638  }
1639 }
1640 
1643 {
1644  if( KinBody::CheckSelfCollision(report) ) {
1645  return true;
1646  }
1647  CollisionCheckerBasePtr pchecker = GetEnv()->GetCollisionChecker();
1648  // check all grabbed bodies with (TODO: support CO_ActiveDOFs option)
1649  bool bCollision = false;
1650  FOREACHC(itgrabbed, _vGrabbedBodies) {
1651  GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed);
1652  KinBodyPtr pbody(pgrabbed->_pgrabbedbody);
1653  if( !pbody ) {
1654  continue;
1655  }
1656  FOREACHC(itrobotlink,pgrabbed->_listNonCollidingLinks) {
1657  // have to use link/link collision since link/body checks attached bodies
1658  FOREACHC(itbodylink,pbody->GetLinks()) {
1659  if( pchecker->CheckCollision(*itrobotlink,KinBody::LinkConstPtr(*itbodylink),report) ) {
1660  bCollision = true;
1661  break;
1662  }
1663  }
1664  if( bCollision ) {
1665  break;
1666  }
1667  }
1668  if( bCollision ) {
1669  break;
1670  }
1671 
1672  if( pbody->CheckSelfCollision(report) ) {
1673  bCollision = true;
1674  break;
1675  }
1676 
1677  // check attached bodies with each other, this is actually tricky since they are attached "with each other", so regular CheckCollision will not work.
1678  // Instead, we will compare each of the body's links with every other
1679  if( _vGrabbedBodies.size() > 1 ) {
1680  FOREACHC(itgrabbed2, _vGrabbedBodies) {
1681  GrabbedConstPtr pgrabbed2 = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed2);
1682  KinBodyPtr pbody2(pgrabbed2->_pgrabbedbody);
1683  if( pbody == pbody2 ) {
1684  continue;
1685  }
1686  FOREACHC(itlink2, pbody2->GetLinks()) {
1687  // make sure the two bodies were not initially colliding
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() ) {
1691  if( pchecker->CheckCollision(KinBody::LinkConstPtr(*itlink),KinBody::LinkConstPtr(*itlink2),report) ) {
1692  bCollision = true;
1693  break;
1694  }
1695  }
1696  if( bCollision ) {
1697  break;
1698  }
1699  }
1700  if( bCollision ) {
1701  break;
1702  }
1703  }
1704  }
1705  if( bCollision ) {
1706  break;
1707  }
1708  }
1709  if( bCollision ) {
1710  break;
1711  }
1712  }
1713  }
1714 
1715  if( bCollision && !!report ) {
1716  if( IS_DEBUGLEVEL(Level_Verbose) ) {
1717  RAVELOG_VERBOSE(str(boost::format("Self collision: %s\n")%report->__str__()));
1718  std::vector<OpenRAVE::dReal> v;
1719  GetDOFValues(v);
1720  stringstream ss; ss << std::setprecision(std::numeric_limits<OpenRAVE::dReal>::digits10+1);
1721  for(size_t i = 0; i < v.size(); ++i ) {
1722  if( i > 0 ) {
1723  ss << "," << v[i];
1724  }
1725  else {
1726  ss << "colvalues=[" << v[i];
1727  }
1728  }
1729  ss << "]";
1730  RAVELOG_VERBOSE(ss.str());
1731  }
1732  }
1733  return bCollision;
1734 }
1735 
1736 bool RobotBase::CheckLinkCollision(int ilinkindex, const Transform& tlinktrans, CollisionReportPtr report)
1737 {
1738  LinkPtr plink = _veclinks.at(ilinkindex);
1739  CollisionCheckerBasePtr pchecker = GetEnv()->GetCollisionChecker();
1740  if( plink->IsEnabled() ) {
1741  boost::shared_ptr<TransformSaver<LinkPtr> > linksaver(new TransformSaver<LinkPtr>(plink)); // gcc optimization bug when linksaver is on stack?
1742  plink->SetTransform(tlinktrans);
1743  if( pchecker->CheckCollision(LinkConstPtr(plink),report) ) {
1744  return true;
1745  }
1746  }
1747 
1748  // check if any grabbed bodies are attached to this link, and if so check their collisions with the environment
1749  // it is important to make sure to add all other attached bodies in the ignored list!
1750  std::vector<KinBodyConstPtr> vbodyexcluded;
1751  std::vector<KinBody::LinkConstPtr> vlinkexcluded;
1752  FOREACHC(itgrabbed,_vGrabbedBodies) {
1753  GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed);
1754  if( pgrabbed->_plinkrobot == plink ) {
1755  KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
1756  if( !!pbody ) {
1757  vbodyexcluded.resize(0);
1758  vbodyexcluded.push_back(shared_kinbody_const());
1759  FOREACHC(itgrabbed2,_vGrabbedBodies) {
1760  if( itgrabbed2 != itgrabbed ) {
1761  GrabbedConstPtr pgrabbed2 = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed2);
1762  KinBodyPtr pbody2 = pgrabbed2->_pgrabbedbody.lock();
1763  if( !!pbody2 ) {
1764  vbodyexcluded.push_back(pbody2);
1765  }
1766  }
1767  }
1768  KinBodyStateSaver bodysaver(pbody,Save_LinkTransformation);
1769  pbody->SetTransform(tlinktrans * pgrabbed->_troot);
1770  if( pchecker->CheckCollision(KinBodyConstPtr(pbody),vbodyexcluded, vlinkexcluded, report) ) {
1771  return true;
1772  }
1773  }
1774  }
1775  }
1776  return false;
1777 }
1778 
1779 bool RobotBase::CheckLinkSelfCollision(int ilinkindex, const Transform& tlinktrans, CollisionReportPtr report)
1780 {
1781  // TODO: have to consider rigidly attached links??
1782  CollisionCheckerBasePtr pchecker = GetEnv()->GetCollisionChecker();
1783  LinkPtr plink = _veclinks.at(ilinkindex);
1784  if( plink->IsEnabled() ) {
1785  boost::shared_ptr<TransformSaver<LinkPtr> > linksaver(new TransformSaver<LinkPtr>(plink)); // gcc optimization bug when linksaver is on stack?
1786  plink->SetTransform(tlinktrans);
1787  if( pchecker->CheckSelfCollision(LinkConstPtr(plink),report) ) {
1788  return true;
1789  }
1790  }
1791 
1792  KinBodyStateSaverPtr linksaver;
1793  // check if any grabbed bodies are attached to this link, and if so check their collisions with the environment
1794  // it is important to make sure to add all other attached bodies in the ignored list!
1795  std::vector<KinBodyConstPtr> vbodyexcluded;
1796  std::vector<KinBody::LinkConstPtr> vlinkexcluded;
1797  FOREACHC(itgrabbed,_vGrabbedBodies) {
1798  GrabbedConstPtr pgrabbed = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbed);
1799  if( pgrabbed->_plinkrobot == plink ) {
1800  KinBodyPtr pbody = pgrabbed->_pgrabbedbody.lock();
1801  if( !!pbody ) {
1802  if( !linksaver ) {
1803  linksaver.reset(new KinBodyStateSaver(shared_kinbody()));
1804  plink->Enable(false);
1805  // also disable rigidly attached links?
1806  }
1807  KinBodyStateSaver bodysaver(pbody,Save_LinkTransformation);
1808  pbody->SetTransform(tlinktrans * pgrabbed->_troot);
1809  if( pchecker->CheckCollision(shared_kinbody_const(), KinBodyConstPtr(pbody),report) ) {
1810  return true;
1811  }
1812  }
1813  }
1814  }
1815  return false;
1816 }
1817 
1819 {
1820  KinBody::SimulationStep(fElapsedTime);
1823 }
1824 
1826 {
1828  _vAllDOFIndices.resize(GetDOF());
1829  for(int i = 0; i < GetDOF(); ++i) {
1830  _vAllDOFIndices[i] = i;
1831  }
1832 
1833  _activespec._vgroups.reserve(2);
1834  _activespec._vgroups.resize(0);
1835  if( _vAllDOFIndices.size() > 0 ) {
1837  stringstream ss;
1838  ss << "joint_values " << GetName();
1839  FOREACHC(it,_vAllDOFIndices) {
1840  ss << " " << *it;
1841  }
1842  group.name = ss.str();
1843  group.dof = (int)_vAllDOFIndices.size();
1844  group.offset = 0;
1845  // do not initialize interpolation, since it implies a motion sampling strategy
1846  _activespec._vgroups.push_back(group);
1847  }
1848 
1849  int manipindex=0;
1850  FOREACH(itmanip,_vecManipulators) {
1851  if( (*itmanip)->_info._name.size() == 0 ) {
1852  stringstream ss;
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();
1856  }
1857  (*itmanip)->_ComputeInternalInformation();
1858  vector<ManipulatorPtr>::iterator itmanip2 = itmanip; ++itmanip2;
1859  for(; itmanip2 != _vecManipulators.end(); ++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()));
1862  }
1863  }
1864  manipindex++;
1865  }
1866  // set active manipulator to first manipulator
1867  if( _vecManipulators.size() > 0 ) {
1869  }
1870  else {
1871  _pManipActive.reset();
1872  }
1873 
1874  int sensorindex=0;
1875  FOREACH(itsensor,_vecSensors) {
1876  if( (*itsensor)->GetName().size() == 0 ) {
1877  stringstream ss;
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();
1881  }
1882  else if( !utils::IsValidName((*itsensor)->GetName()) ) {
1883  throw OPENRAVE_EXCEPTION_FORMAT("sensor name \"%s\" is not valid", (*itsensor)->GetName(), ORE_Failed);
1884  }
1885  if( !!(*itsensor)->GetSensor() ) {
1886  stringstream ss; ss << GetName() << "_" << (*itsensor)->GetName(); // global unique name?
1887  (*itsensor)->GetSensor()->SetName(ss.str());
1888  }
1889  sensorindex++;
1890  }
1891 
1892  {
1893  __hashrobotstructure.resize(0);
1894  FOREACH(itsensor,_vecSensors) {
1895  (*itsensor)->__hashstructure.resize(0);
1896  }
1897  }
1898 
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()));
1901  }
1902 
1903  if( !GetController() ) {
1904  RAVELOG_VERBOSE(str(boost::format("no default controller set on robot %s\n")%GetName()));
1905  std::vector<int> dofindices;
1906  for(int i = 0; i < GetDOF(); ++i) {
1907  dofindices.push_back(i);
1908  }
1909  SetController(RaveCreateController(GetEnv(), "IdealController"),dofindices,1);
1910  }
1911 
1912  // reset the power on the sensors
1913  FOREACH(itsensor,_vecSensors) {
1914  SensorBasePtr psensor = (*itsensor)->GetSensor();
1915  if( !!psensor ) {
1916  int ispower = psensor->Configure(SensorBase::CC_PowerCheck);
1917  psensor->Configure(ispower ? SensorBase::CC_PowerOn : SensorBase::CC_PowerOff);
1918  }
1919  }
1920 }
1921 
1922 void RobotBase::_ParametersChanged(int parameters)
1923 {
1924  KinBody::_ParametersChanged(parameters);
1925  if( parameters & (Prop_Sensors|Prop_SensorPlacement) ) {
1926  FOREACH(itsensor,_vecSensors) {
1927  (*itsensor)->__hashstructure.resize(0);
1928  }
1929  }
1930  if( parameters & Prop_RobotManipulatorTool ) {
1931  FOREACH(itmanip,_vecManipulators) {
1932  (*itmanip)->__hashstructure.resize(0);
1933  (*itmanip)->__hashkinematicsstructure.resize(0);
1934  }
1935  }
1936 
1937  if( (parameters&Prop_LinkEnable) == Prop_LinkEnable ) {
1938  // check if any regrabbed bodies have the link in _listNonCollidingLinks and the link is enabled, or are missing the link in _listNonCollidingLinks and the link is disabled
1939  std::map<GrabbedPtr, list<KinBody::LinkConstPtr> > mapcheckcollisions;
1940  FOREACH(itlink,_veclinks) {
1941  if( (*itlink)->IsEnabled() ) {
1942  FOREACH(itgrabbed,_vGrabbedBodies) {
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);
1949  }
1950  mapcheckcollisions[pgrabbed].push_back(*itlink);
1951  }
1952  else {
1953  // try to restore
1954  if( pgrabbed->WasLinkNonColliding(*itlink) == 1 ) {
1955  pgrabbed->_listNonCollidingLinks.push_back(*itlink);
1956  }
1957  }
1958  }
1959  }
1960  }
1961  else {
1962  // add since it is disabled?
1963  FOREACH(itgrabbed,_vGrabbedBodies) {
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);
1969  }
1970  }
1971  }
1972  }
1973  }
1974  }
1975 
1976 // if( mapcheckcollisions.size() > 0 ) {
1977 // CollisionOptionsStateSaver colsaver(GetEnv()->GetCollisionChecker(),0); // have to reset the collision options
1978 // FOREACH(itgrabbed, mapcheckcollisions) {
1979 // KinBodyPtr pgrabbedbody(itgrabbed->first->_pgrabbedbody);
1980 // _RemoveAttachedBody(pgrabbedbody);
1981 // CallOnDestruction destructionhook(boost::bind(&RobotBase::_AttachBody,this,pgrabbedbody));
1982 // FOREACH(itlink, itgrabbed->second) {
1983 // if( pchecker->CheckCollision(*itlink, KinBodyConstPtr(pgrabbedbody)) ) {
1984 // itgrabbed->first->_listNonCollidingLinks.remove(*itlink);
1985 // }
1986 // }
1987 // }
1988 // }
1989  }
1990 }
1991 
1992 void RobotBase::Clone(InterfaceBaseConstPtr preference, int cloningoptions)
1993 {
1994  KinBody::Clone(preference,cloningoptions);
1995  RobotBaseConstPtr r = RaveInterfaceConstCast<RobotBase>(preference);
1996  __hashrobotstructure = r->__hashrobotstructure;
1997  _vecManipulators.clear();
1998  _pManipActive.reset();
1999  FOREACHC(itmanip, r->_vecManipulators) {
2000  ManipulatorPtr pmanip(new Manipulator(shared_robot(),*itmanip));
2001  _vecManipulators.push_back(pmanip);
2002  if( !!r->GetActiveManipulator() && r->GetActiveManipulator()->GetName() == (*itmanip)->GetName() ) {
2003  _pManipActive = pmanip;
2004  }
2005  }
2006 
2007  _vecSensors.clear();
2008  FOREACHC(itsensor, r->_vecSensors) {
2009  _vecSensors.push_back(AttachedSensorPtr(new AttachedSensor(shared_robot(),**itsensor,cloningoptions)));
2010  }
2012 
2013  _vActiveDOFIndices = r->_vActiveDOFIndices;
2014  _vAllDOFIndices = r->_vAllDOFIndices;
2015  vActvAffineRotationAxis = r->vActvAffineRotationAxis;
2016  _nActiveDOF = r->_nActiveDOF;
2017  _nAffineDOFs = r->_nAffineDOFs;
2018 
2019  _vTranslationLowerLimits = r->_vTranslationLowerLimits;
2020  _vTranslationUpperLimits = r->_vTranslationUpperLimits;
2021  _vTranslationMaxVels = r->_vTranslationMaxVels;
2022  _vTranslationResolutions = r->_vTranslationResolutions;
2023  _vRotationAxisLowerLimits = r->_vRotationAxisLowerLimits;
2024  _vRotationAxisUpperLimits = r->_vRotationAxisUpperLimits;
2025  _vRotationAxisMaxVels = r->_vRotationAxisMaxVels;
2026  _vRotationAxisResolutions = r->_vRotationAxisResolutions;
2027  _vRotation3DLowerLimits = r->_vRotation3DLowerLimits;
2028  _vRotation3DUpperLimits = r->_vRotation3DUpperLimits;
2029  _vRotation3DMaxVels = r->_vRotation3DMaxVels;
2030  _vRotation3DResolutions = r->_vRotation3DResolutions;
2031  _vRotationQuatLimitStart = r->_vRotationQuatLimitStart;
2032  _fQuatLimitMaxAngle = r->_fQuatLimitMaxAngle;
2033  _fQuatMaxAngleVelocity = r->_fQuatMaxAngleVelocity;
2034  _fQuatAngleResolution = r->_fQuatAngleResolution;
2035  _fQuatAngleWeight = r->_fQuatAngleWeight;
2036 
2037  // clone the controller
2038  if( (cloningoptions&Clone_RealControllers) && !!r->GetController() ) {
2039  if( !SetController(RaveCreateController(GetEnv(), r->GetController()->GetXMLId()),r->GetController()->GetControlDOFIndices(),r->GetController()->IsControlTransformation()) ) {
2040  RAVELOG_WARN(str(boost::format("failed to set %s controller for robot %s\n")%r->GetController()->GetXMLId()%GetName()));
2041  }
2042  }
2043 
2044  if( !GetController() ) {
2045  std::vector<int> dofindices;
2046  for(int i = 0; i < GetDOF(); ++i) {
2047  dofindices.push_back(i);
2048  }
2049  if( !SetController(RaveCreateController(GetEnv(), "IdealController"),dofindices, 1) ) {
2050  RAVELOG_WARN("failed to set IdealController\n");
2051  }
2052  }
2053 
2054  // clone the grabbed bodies, note that this can fail if the new cloned environment hasn't added the bodies yet (check out Environment::Clone)
2055  _vGrabbedBodies.resize(0);
2056  FOREACHC(itgrabbedref, r->_vGrabbedBodies) {
2057  GrabbedConstPtr pgrabbedref = boost::dynamic_pointer_cast<Grabbed const>(*itgrabbedref);
2058 
2059  KinBodyPtr pbodyref(pgrabbedref->_pgrabbedbody);
2060  KinBodyPtr pgrabbedbody;
2061  if( !!pbodyref ) {
2062  pgrabbedbody = GetEnv()->GetBodyFromEnvironmentId(pbodyref->GetEnvironmentId());
2063  BOOST_ASSERT(pgrabbedbody->GetName() == pbodyref->GetName());
2064 
2065  GrabbedPtr pgrabbed(new Grabbed(pgrabbedbody,_veclinks.at(KinBody::LinkPtr(pgrabbedref->_plinkrobot)->GetIndex())));
2066  pgrabbed->_troot = pgrabbedref->_troot;
2067  pgrabbed->_listNonCollidingLinks.clear();
2068  FOREACHC(itlinkref, pgrabbedref->_listNonCollidingLinks) {
2069  pgrabbed->_listNonCollidingLinks.push_back(_veclinks.at((*itlinkref)->GetIndex()));
2070  }
2071  _vGrabbedBodies.push_back(pgrabbed);
2072  }
2073  }
2074 }
2075 
2076 void RobotBase::serialize(std::ostream& o, int options) const
2077 {
2078  KinBody::serialize(o,options);
2079  if( options & SO_RobotManipulators ) {
2080  FOREACHC(itmanip,_vecManipulators) {
2081  (*itmanip)->serialize(o,options);
2082  }
2083  }
2084  if( options & SO_RobotSensors ) {
2085  FOREACHC(itsensor,_vecSensors) {
2086  (*itsensor)->serialize(o,options);
2087  }
2088  }
2089 }
2090 
2091 const std::string& RobotBase::GetRobotStructureHash() const
2092 {
2094  if( __hashrobotstructure.size() == 0 ) {
2095  ostringstream ss;
2096  ss << std::fixed << std::setprecision(SERIALIZATION_PRECISION);
2098  __hashrobotstructure = utils::GetMD5HashString(ss.str());
2099  }
2100  return __hashrobotstructure;
2101 }
2102 
2104 {
2105  if( !!GetController() ) {
2106  return GetController()->SetPath(ptraj);
2107  }
2108  return false;
2109 }
2110 
2112 {
2113  if( !!GetController() ) {
2114  return GetController()->SetPath(ptraj);
2115  }
2116  return false;
2117 }
2118 
2120 {
2121  if( !!GetController() ) {
2122  return GetController()->SetPath(ptraj);
2123  }
2124  return false;
2125 }
2126 
2127 void RobotBase::GetFullTrajectoryFromActive(TrajectoryBasePtr pfulltraj, TrajectoryBaseConstPtr pActiveTraj, bool bOverwriteTransforms)
2128 {
2130  spec._vgroups.resize(2);
2131  spec._vgroups[0].offset = 0;
2132  spec._vgroups[0].dof = GetDOF();
2133  stringstream ss;
2134  ss << "joint_values " << GetName();
2135  for(int i = 0; i < GetDOF(); ++i) {
2136  ss << " " << i;
2137  }
2138  spec._vgroups[0].name = ss.str();
2139  spec._vgroups[0].interpolation = "linear";
2140  spec._vgroups[1].offset = GetDOF();
2141  spec._vgroups[1].dof = 1;
2142  spec._vgroups[1].name = "deltatime";
2143  if( !bOverwriteTransforms ) {
2144  spec._vgroups.resize(3);
2145  spec._vgroups[2].offset = GetDOF()+1;
2146  spec._vgroups[2].dof = RaveGetAffineDOF(DOF_Transform);
2147  spec._vgroups[2].name = str(boost::format("affine_transform %s %d")%GetName()%DOF_Transform);
2148  spec._vgroups[2].interpolation = "linear";
2149  }
2150  pfulltraj->Init(spec);
2151  std::vector<dReal> vdata;
2152  pActiveTraj->GetWaypoints(0,pActiveTraj->GetNumWaypoints(),vdata);
2153  pfulltraj->Insert(0,vdata,pActiveTraj->GetConfigurationSpecification());
2154 }
2155 
2156 } // end namespace OpenRAVE