openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
kinbody.h
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/>.
22 #ifndef OPENRAVE_KINBODY_H
23 #define OPENRAVE_KINBODY_H
24 
25 namespace OpenRAVE {
26 
27 class OpenRAVEFunctionParserReal;
28 typedef boost::shared_ptr< OpenRAVEFunctionParserReal > OpenRAVEFunctionParserRealPtr;
29 
32  GT_None = 0,
33  GT_Box = 1,
34  GT_Sphere = 2,
37 };
38 
43 {
44 public:
47  Prop_JointMimic=0x1,
48  Prop_JointLimits=0x2,
49  Prop_JointOffset=0x4,
50  Prop_JointProperties=0x8,
51  Prop_JointAccelerationVelocityTorqueLimits=0x10,
52  Prop_Joints=Prop_JointMimic|Prop_JointLimits|Prop_JointOffset|Prop_JointProperties|Prop_JointAccelerationVelocityTorqueLimits,
53 
54  Prop_Name=0x20,
55  Prop_LinkDraw=0x40,
56  Prop_LinkGeometry=0x80,
57  // 0x100
58  // 0x200
59  Prop_LinkStatic=0x400,
60  Prop_LinkEnable=0x800,
61  Prop_LinkDynamics=0x1000,
62  Prop_Links=Prop_LinkDraw|Prop_LinkGeometry|Prop_LinkStatic|Prop_LinkEnable|Prop_LinkDynamics,
63  Prop_JointCustomParameters = 0x2000,
64  Prop_LinkCustomParameters = 0x4000,
65 
66  // robot only
67  // 0x00010000
68  Prop_RobotSensors = 0x00020000,
69  Prop_Sensors = 0x00020000,
70  Prop_RobotSensorPlacement = 0x00040000,
71  Prop_SensorPlacement = 0x00040000,
72  Prop_RobotActiveDOFs = 0x00080000,
73  Prop_RobotManipulatorTool = 0x00100000,
74  Prop_RobotManipulatorName = 0x00200000,
75  Prop_RobotManipulatorSolver = 0x00400000,
76  Prop_RobotManipulators = Prop_RobotManipulatorTool | Prop_RobotManipulatorName | Prop_RobotManipulatorSolver,
77  };
78 
81  CLA_Nothing = 0,
82  CLA_CheckLimits = 1,
83  CLA_CheckLimitsSilent = 2,
84  CLA_CheckLimitsThrow = 3,
85  };
86 
91  {
92 public:
93  GeometryInfo();
94  virtual ~GeometryInfo() {
95  }
96 
99  bool InitCollisionMesh(float fTessellation=1);
100 
101  inline dReal GetSphereRadius() const {
102  return _vGeomData.x;
103  }
104  inline dReal GetCylinderRadius() const {
105  return _vGeomData.x;
106  }
107  inline dReal GetCylinderHeight() const {
108  return _vGeomData.y;
109  }
110  inline const Vector& GetBoxExtents() const {
111  return _vGeomData;
112  }
113 
115  Vector _vGeomData;
116 
117 
118 
119  RaveVector<float> _vDiffuseColor, _vAmbientColor;
120 
127 
129 
134  std::string _filenamerender;
135 
139  std::string _filenamecollision;
140 
144  bool _bVisible;
146  };
147  typedef boost::shared_ptr<GeometryInfo> GeometryInfoPtr;
148  typedef boost::shared_ptr<GeometryInfo const> GeometryInfoConstPtr;
149 
152  {
153 public:
154  LinkInfo();
155  virtual ~LinkInfo() {
156  }
157  std::vector<GeometryInfoPtr> _vgeometryinfos;
159  std::string _name;
168  std::map<std::string, std::vector<dReal> > _mapFloatParameters;
169  std::map<std::string, std::vector<int> > _mapIntParameters;
170 
171  std::vector<std::string> _vForcedAdjacentLinks;
176  bool _bStatic;
177 
180  bool __padding0, __padding1; // for 4-byte alignment
181  };
182  typedef boost::shared_ptr<LinkInfo> LinkInfoPtr;
183  typedef boost::shared_ptr<LinkInfo const> LinkInfoConstPtr;
184 
186  class OPENRAVE_API Link : public boost::enable_shared_from_this<Link>
187  {
188 public:
189  Link(KinBodyPtr parent);
190  virtual ~Link();
191 
194  typedef boost::shared_ptr<KinBody::GeometryInfo> GeometryInfoPtr RAVE_DEPRECATED;
195  typedef TriMesh TRIMESH RAVE_DEPRECATED;
196  typedef GeometryType GeomType RAVE_DEPRECATED;
202 
205  {
206 public:
213 
214  Geometry(boost::shared_ptr<Link> parent, const KinBody::GeometryInfo& info);
215  virtual ~Geometry() {
216  }
217 
219  inline const Transform& GetTransform() const {
220  return _info._t;
221  }
222  inline GeometryType GetType() const {
223  return _info._type;
224  }
225  inline const Vector& GetRenderScale() const {
226  return _info._vRenderScale;
227  }
228 
229  inline const std::string& GetRenderFilename() const {
230  return _info._filenamerender;
231  }
232  inline float GetTransparency() const {
233  return _info._fTransparency;
234  }
236  inline bool IsDraw() const RAVE_DEPRECATED {
237  return _info._bVisible;
238  }
239  inline bool IsVisible() const {
240  return _info._bVisible;
241  }
242  inline bool IsModifiable() const {
243  return _info._bModifiable;
244  }
245 
246  inline dReal GetSphereRadius() const {
247  return _info._vGeomData.x;
248  }
249  inline dReal GetCylinderRadius() const {
250  return _info._vGeomData.x;
251  }
252  inline dReal GetCylinderHeight() const {
253  return _info._vGeomData.y;
254  }
255  inline const Vector& GetBoxExtents() const {
256  return _info._vGeomData;
257  }
258  inline const RaveVector<float>& GetDiffuseColor() const {
259  return _info._vDiffuseColor;
260  }
261  inline const RaveVector<float>& GetAmbientColor() const {
262  return _info._vAmbientColor;
263  }
264 
266  inline const TriMesh& GetCollisionMesh() const {
267  return _info._meshcollision;
268  }
269 
270  inline const KinBody::GeometryInfo& GetInfo() const {
271  return _info;
272  }
273 
275  virtual AABB ComputeAABB(const Transform& trans) const;
276  virtual void serialize(std::ostream& o, int options) const;
277 
279  virtual void SetCollisionMesh(const TriMesh& mesh);
283  virtual bool SetVisible(bool visible);
285  inline void SetDraw(bool bDraw) RAVE_DEPRECATED {
286  SetVisible(bDraw);
287  }
289  virtual void SetTransparency(float f);
291  virtual void SetDiffuseColor(const RaveVector<float>& color);
293  virtual void SetAmbientColor(const RaveVector<float>& color);
294 
300  virtual bool ValidateContactNormal(const Vector& position, Vector& normal) const;
301 
303  virtual void SetRenderFilename(const std::string& renderfilename);
304 
305 protected:
306  boost::weak_ptr<Link> _parent;
308 #ifdef RAVE_PRIVATE
309 #ifdef _MSC_VER
310  friend class OpenRAVEXMLParser::LinkXMLReader;
311  friend class OpenRAVEXMLParser::KinBodyXMLReader;
312  friend class XFileReader;
313 #else
314  friend class ::OpenRAVEXMLParser::LinkXMLReader;
315  friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
316  friend class ::XFileReader;
317 #endif
318 #endif
319  friend class ColladaReader;
320  friend class RobotBase;
321  friend class KinBody;
322  friend class KinBody::Link;
323  };
324 
325  typedef boost::shared_ptr<Geometry> GeometryPtr;
326  typedef boost::shared_ptr<Geometry const> GeometryConstPtr;
327  typedef Geometry GEOMPROPERTIES RAVE_DEPRECATED;
328 
329  inline const std::string& GetName() const {
330  return _info._name;
331  }
332 
337  inline bool IsStatic() const {
338  return _info._bStatic;
339  }
340 
342  virtual void Enable(bool enable);
343 
345  virtual bool IsEnabled() const;
346 
350  virtual bool SetVisible(bool visible);
351 
353  virtual bool IsVisible() const;
354 
356  inline KinBodyPtr GetParent() const {
357  return KinBodyPtr(_parent);
358  }
359 
361  inline int GetIndex() const {
362  return _index;
363  }
364  inline const TriMesh& GetCollisionData() const {
365  return _collision;
366  }
367 
369  virtual AABB ComputeLocalAABB() const;
370 
372  virtual AABB ComputeAABB() const;
373 
375  inline Transform GetTransform() const {
376  return _info._t;
377  }
378 
384  virtual void GetParentLinks(std::vector< boost::shared_ptr<Link> >& vParentLinks) const;
385 
390  virtual bool IsParentLink(boost::shared_ptr<Link const> plink) const;
391 
393  inline Vector GetLocalCOM() const {
394  return _info._tMassFrame.trans;
395  }
396 
398  inline Vector GetGlobalCOM() const {
399  return _info._t*_info._tMassFrame.trans;
400  }
401 
402  inline Vector GetCOMOffset() const {
403  return _info._tMassFrame.trans;
404  }
405 
407  virtual TransformMatrix GetLocalInertia() const;
408 
409  // \brief return inertia around the link's COM in the global coordinate frame.
410  virtual TransformMatrix GetGlobalInertia() const;
411 
414  RAVELOG_WARN("KinBody::Link::GetInertia is deprecated, use KinBody::Link::GetGlobalInertia\n");
415  return GetLocalInertia();
416  }
417 
419  virtual void SetLocalMassFrame(const Transform& massframe);
420 
422  virtual void SetPrincipalMomentsOfInertia(const Vector& inertiamoments);
423 
425  virtual void SetMass(dReal mass);
426 
428  inline const Transform& GetLocalMassFrame() const {
429  return _info._tMassFrame;
430  }
431 
433  inline Transform GetGlobalMassFrame() const {
434  return _info._t*_info._tMassFrame;
435  }
436 
438  inline const Vector& GetPrincipalMomentsOfInertia() const {
439  return _info._vinertiamoments;
440  }
441  inline dReal GetMass() const {
442  return _info._mass;
443  }
444 
448  virtual void SetStatic(bool bStatic);
449 
453  virtual void SetTransform(const Transform& transform);
454 
459  virtual void SetForce(const Vector& force, const Vector& pos, bool add);
460 
463  virtual void SetTorque(const Vector& torque, bool add);
464 
468  virtual void SetVelocity(const Vector& linearvel, const Vector& angularvel);
469 
473  virtual void GetVelocity(Vector& linearvel, Vector& angularvel) const;
474 
476  virtual std::pair<Vector,Vector> GetVelocity() const;
477 
479  inline const std::vector<GeometryPtr>& GetGeometries() const {
480  return _vGeometries;
481  }
482  virtual GeometryPtr GetGeometry(int index);
483 
488  virtual void InitGeometries(std::vector<KinBody::GeometryInfoConstPtr>& geometries);
489  virtual void InitGeometries(std::list<KinBody::GeometryInfo>& geometries);
490 
492  virtual void SwapGeometries(boost::shared_ptr<Link>& link);
493 
498  virtual bool ValidateContactNormal(const Vector& position, Vector& normal) const;
499 
501  virtual bool IsRigidlyAttached(boost::shared_ptr<Link const> plink) const;
502 
506  virtual void GetRigidlyAttachedLinks(std::vector<boost::shared_ptr<Link> >& vattachedlinks) const;
507 
508  virtual void serialize(std::ostream& o, int options) const;
509 
511  inline const std::map<std::string, std::vector<dReal> >& GetFloatParameters() const {
512  return _info._mapFloatParameters;
513  }
514 
518  virtual void SetFloatParameters(const std::string& key, const std::vector<dReal>& parameters);
519 
521  inline const std::map<std::string, std::vector<int> >& GetIntParameters() const {
522  return _info._mapIntParameters;
523  }
524 
528  virtual void SetIntParameters(const std::string& key, const std::vector<int>& parameters);
529 
531  virtual void UpdateInfo();
532 
537  inline const KinBody::LinkInfo& GetInfo() const {
538  return _info;
539  }
540 
543  UpdateInfo();
544  return _info;
545  }
546 
547 protected:
549  virtual void _Update();
550 
551  std::vector<GeometryPtr> _vGeometries;
552 
554 
555 private:
558 
559  int _index;
560  KinBodyWeakPtr _parent;
561  std::vector<int> _vParentLinks;
562  std::vector<int> _vRigidlyAttachedLinks;
563  TriMesh _collision;
564 
565 
566 #ifdef RAVE_PRIVATE
567 #ifdef _MSC_VER
568  friend class OpenRAVEXMLParser::LinkXMLReader;
569  friend class OpenRAVEXMLParser::KinBodyXMLReader;
570  friend class OpenRAVEXMLParser::RobotXMLReader;
571  friend class XFileReader;
572 #else
573  friend class ::OpenRAVEXMLParser::LinkXMLReader;
574  friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
575  friend class ::OpenRAVEXMLParser::RobotXMLReader;
576  friend class ::XFileReader;
577 #endif
578 #endif
579  friend class ColladaReader;
580  friend class KinBody;
581  friend class RobotBase;
582  };
583  typedef boost::shared_ptr<KinBody::Link> LinkPtr;
584  typedef boost::shared_ptr<KinBody::Link const> LinkConstPtr;
585  typedef boost::weak_ptr<KinBody::Link> LinkWeakPtr;
586 
595  enum JointType {
596  JointNone = 0,
597  JointHinge = 0x01,
598  JointRevolute = 0x01,
599  JointSlider = 0x11,
600  JointPrismatic = 0x11,
601  JointRR = 0x02,
602  JointRP = 0x12,
603  JointPR = 0x22,
604  JointPP = 0x32,
605  JointSpecialBit = 0x80000000,
606  JointUniversal = 0x80000001,
607  JointHinge2 = 0x80000002,
608  JointSpherical = 0x80000003,
609  JointTrajectory = 0x80000004,
610  };
611 
612  class Joint;
613 
618  {
619 public:
620  boost::array< std::string, 3> _equations;
621  };
622  typedef boost::shared_ptr<MimicInfo> MimicInfoPtr;
623  typedef boost::shared_ptr<MimicInfo const> MimicInfoConstPtr;
624 
626  {
627 public:
628  boost::array< std::string, 3> _equations;
629 
630  struct DOFFormat
631  {
632  int16_t jointindex;
633  int16_t dofindex : 14;
634  uint8_t axis : 2;
635  bool operator <(const DOFFormat& r) const;
636  bool operator ==(const DOFFormat& r) const;
637  boost::shared_ptr<Joint> GetJoint(KinBodyPtr parent) const;
638  boost::shared_ptr<Joint const> GetJoint(KinBodyConstPtr parent) const;
639  };
641  {
642  int16_t dofindex;
643  uint16_t dofformatindex;
644  bool operator ==(const DOFHierarchy& r) const {
645  return dofindex==r.dofindex && dofformatindex == r.dofformatindex;
646  }
647  };
648 
650 
651  std::vector< DOFFormat > _vdofformat;
652  std::vector<DOFHierarchy> _vmimicdofs;
654  std::vector<OpenRAVEFunctionParserRealPtr > _velfns, _accelfns;
655 
656  };
657  typedef boost::shared_ptr<Mimic> MimicPtr;
658  typedef boost::shared_ptr<Mimic const> MimicConstPtr;
659 
662  {
663 public:
664  JointInfo();
665  virtual ~JointInfo() {
666  }
667 
669  std::string _name;
670  std::string _linkname0, _linkname1;
672  boost::array<Vector,3> _vaxes;
673  std::vector<dReal> _vcurrentvalues;
674 
675  boost::array<dReal,3> _vresolution;
676  boost::array<dReal,3> _vmaxvel;
677  boost::array<dReal,3> _vhardmaxvel;
678  boost::array<dReal,3> _vmaxaccel;
679  boost::array<dReal,3> _vmaxtorque;
680  boost::array<dReal,3> _vweights;
681 
687  boost::array<dReal,3> _voffsets;
688  boost::array<dReal,3> _vlowerlimit, _vupperlimit;
690 
691  boost::array<MimicInfoPtr,3> _vmimic;
692 
693  std::map<std::string, std::vector<dReal> > _mapFloatParameters;
694  std::map<std::string, std::vector<int> > _mapIntParameters;
695 
702  boost::array<uint8_t,3> _bIsCircular;
703 
704  bool _bIsActive;
705  };
706  typedef boost::shared_ptr<JointInfo> JointInfoPtr;
707  typedef boost::shared_ptr<JointInfo const> JointInfoConstPtr;
708 
710  class OPENRAVE_API Joint : public boost::enable_shared_from_this<Joint>
711  {
712 public:
714  typedef Mimic MIMIC RAVE_DEPRECATED;
730 
732  virtual ~Joint();
733 
735  inline const std::string& GetName() const {
736  return _info._name;
737  }
738 
739  inline dReal GetMaxVel(int iaxis=0) const {
740  return _info._vmaxvel[iaxis];
741  }
742  inline dReal GetMaxAccel(int iaxis=0) const {
743  return _info._vmaxaccel[iaxis];
744  }
745  inline dReal GetMaxTorque(int iaxis=0) const {
746  return _info._vmaxtorque[iaxis];
747  }
748 
752  inline int GetDOFIndex() const {
753  return dofindex;
754  }
755 
757  inline int GetJointIndex() const {
758  return jointindex;
759  }
760 
761  inline KinBodyPtr GetParent() const {
762  return KinBodyPtr(_parent);
763  }
764 
765  inline LinkPtr GetFirstAttached() const {
766  return _attachedbodies[0];
767  }
768  inline LinkPtr GetSecondAttached() const {
769  return _attachedbodies[1];
770  }
771 
772  inline KinBody::JointType GetType() const {
773  return _info._type;
774  }
775 
779  virtual void GetResolutions(std::vector<dReal>& resolutions, bool bAppend=false) const;
780 
784  virtual dReal GetResolution(int iaxis=0) const;
785 
786  virtual void SetResolution(dReal resolution, int iaxis=0);
787 
789  virtual int GetDOF() const;
790 
797  virtual bool IsCircular(int iaxis) const;
798 
802  virtual bool IsRevolute(int iaxis) const;
803 
807  virtual bool IsPrismatic(int iaxis) const;
808 
810  virtual bool IsStatic() const;
811 
816  virtual void GetValues(std::vector<dReal>& values, bool bAppend=false) const;
817 
819  virtual dReal GetValue(int axis) const;
820 
825  virtual void GetVelocities(std::vector<dReal>& values, bool bAppend=false) const;
826 
828  virtual dReal GetVelocity(int axis) const;
829 
831  virtual void AddTorque(const std::vector<dReal>& torques);
832 
834  virtual Vector GetAnchor() const;
835 
839  virtual Vector GetAxis(int axis = 0) const;
840 
847  virtual void GetLimits(std::vector<dReal>& vLowerLimit, std::vector<dReal>& vUpperLimit, bool bAppend=false) const;
848 
850  virtual std::pair<dReal, dReal> GetLimit(int iaxis=0) const;
851 
853  virtual void SetLimits(const std::vector<dReal>& lower, const std::vector<dReal>& upper);
854 
860  virtual void GetVelocityLimits(std::vector<dReal>& vmax, bool bAppend=false) const;
861 
862  virtual void GetVelocityLimits(std::vector<dReal>& vlower, std::vector<dReal>& vupper, bool bAppend=false) const;
863 
865  virtual std::pair<dReal, dReal> GetVelocityLimit(int iaxis=0) const;
866 
868  virtual void SetVelocityLimits(const std::vector<dReal>& vmax);
869 
875  virtual void GetAccelerationLimits(std::vector<dReal>& vmax, bool bAppend=false) const;
876 
877  virtual dReal GetAccelerationLimit(int iaxis=0) const;
878 
880  virtual void SetAccelerationLimits(const std::vector<dReal>& vmax);
881 
887  virtual void GetTorqueLimits(std::vector<dReal>& vmax, bool bAppend=false) const;
888 
890  virtual void SetTorqueLimits(const std::vector<dReal>& vmax);
891 
895  virtual void GetWeights(std::vector<dReal>& weights, bool bAppend=false) const;
896 
898  virtual dReal GetWeight(int axis=0) const;
899 
901  virtual void SetWeights(const std::vector<dReal>& weights);
902 
906  virtual void SubtractValues(std::vector<dReal>& values1, const std::vector<dReal>& values2) const;
907 
911  virtual dReal SubtractValue(dReal value1, dReal value2, int iaxis) const;
912 
918  inline dReal GetWrapOffset(int iaxis=0) const {
919  return _info._voffsets.at(iaxis);
920  }
921 
922  inline dReal GetOffset(int iaxis=0) const RAVE_DEPRECATED {
923  return GetWrapOffset(iaxis);
924  }
925 
927  virtual void SetWrapOffset(dReal offset, int iaxis=0);
928 
929  virtual void serialize(std::ostream& o, int options) const;
930 
932 
933 
934  virtual LinkPtr GetHierarchyParentLink() const;
936  virtual LinkPtr GetHierarchyChildLink() const;
938  virtual Vector GetInternalHierarchyAxis(int axis = 0) const;
940  virtual Transform GetInternalHierarchyLeftTransform() const;
942  virtual Transform GetInternalHierarchyRightTransform() const;
944 
948 
949 
951  virtual int GetMimicJointIndex() const RAVE_DEPRECATED;
953  virtual const std::vector<dReal> GetMimicCoeffs() const RAVE_DEPRECATED;
954 
958  virtual bool IsMimic(int axis=-1) const;
959 
970  virtual std::string GetMimicEquation(int axis=0, int type=0, const std::string& format="") const;
971 
978  virtual void GetMimicDOFIndices(std::vector<int>& vmimicdofs, int axis=0) const;
979 
999  virtual void SetMimicEquations(int axis, const std::string& poseq, const std::string& veleq, const std::string& acceleq="");
1001 
1003  inline const std::map<std::string, std::vector<dReal> >& GetFloatParameters() const {
1004  return _info._mapFloatParameters;
1005  }
1006 
1010  virtual void SetFloatParameters(const std::string& key, const std::vector<dReal>& parameters);
1011 
1013  inline const std::map<std::string, std::vector<int> >& GetIntParameters() const {
1014  return _info._mapIntParameters;
1015  }
1016 
1020  virtual void SetIntParameters(const std::string& key, const std::vector<int>& parameters);
1021 
1023  virtual void UpdateInfo();
1024 
1028  inline const KinBody::JointInfo& GetInfo() const {
1029  return _info;
1030  }
1031 
1034  UpdateInfo();
1035  return _info;
1036  }
1037 
1038 protected:
1040 
1041  boost::array< MimicPtr,3> _vmimic;
1042 
1050  virtual void _ComputePartialVelocities(std::vector<std::pair<int,dReal> >& vpartials, int iaxis, std::map< std::pair<Mimic::DOFFormat, int>, dReal >& mapcachedpartials) const;
1051 
1063  virtual void _ComputeInternalInformation(LinkPtr plink0, LinkPtr plink1, const Vector& vanchor, const std::vector<Vector>& vaxes, const std::vector<dReal>& vcurrentvalues);
1064 
1072  virtual int _Eval(int axis, uint32_t timederiv, const std::vector<dReal>& vdependentvalues, std::vector<dReal>& voutput);
1073 
1075  virtual void _GetVelocities(std::vector<dReal>& values, bool bAppend, const std::pair<Vector,Vector>& linkparentvelocity, const std::pair<Vector,Vector>& linkchildvelocity) const;
1076 
1078  virtual dReal _GetVelocity(int axis, const std::pair<Vector,Vector>&linkparentvelocity, const std::pair<Vector,Vector>&linkchildvelocity) const;
1079 
1080  boost::array<int,3> _dofbranches;
1081 
1082 private:
1085 
1086  int dofindex;
1087  int jointindex;
1088  boost::array<dReal,3> _vcircularlowerlimit, _vcircularupperlimit;
1089 
1090  KinBodyWeakPtr _parent;
1091  boost::array<LinkPtr,2> _attachedbodies;
1092  boost::array<Vector,3> _vaxes;
1093  Transform _tRight, _tLeft;
1094  Transform _tRightNoOffset, _tLeftNoOffset;
1095  Transform _tinvRight, _tinvLeft;
1096  bool _bInitialized;
1098 #ifdef RAVE_PRIVATE
1099 #ifdef _MSC_VER
1100  friend class OpenRAVEXMLParser::JointXMLReader;
1101  friend class OpenRAVEXMLParser::KinBodyXMLReader;
1102  friend class OpenRAVEXMLParser::RobotXMLReader;
1103  friend class XFileReader;
1104 #else
1105  friend class ::OpenRAVEXMLParser::JointXMLReader;
1106  friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
1107  friend class ::OpenRAVEXMLParser::RobotXMLReader;
1108  friend class ::XFileReader;
1109 #endif
1110 #endif
1111  friend class ColladaReader;
1112  friend class ColladaWriter;
1113  friend class KinBody;
1114  };
1115  typedef boost::shared_ptr<KinBody::Joint> JointPtr;
1116  typedef boost::shared_ptr<KinBody::Joint const> JointConstPtr;
1117  typedef boost::weak_ptr<KinBody::Joint> JointWeakPtr;
1118 
1121  {
1122 public:
1123  BodyState() : environmentid(0){
1124  }
1125  virtual ~BodyState() {
1126  }
1128  std::vector<RaveTransform<dReal> > vectrans;
1129  std::vector<dReal> jointvalues;
1130  std::string strname;
1132  };
1133  typedef boost::shared_ptr<KinBody::BodyState> BodyStatePtr;
1134  typedef boost::shared_ptr<KinBody::BodyState const> BodyStateConstPtr;
1135 
1137  class OPENRAVE_API ManageData : public boost::enable_shared_from_this<ManageData>
1138  {
1139 public:
1140  ManageData(SensorSystemBasePtr psensorsystem) : _psensorsystem(psensorsystem) {
1141  }
1142  virtual ~ManageData() {
1143  }
1144 
1146  return SensorSystemBasePtr(_psensorsystem);
1147  }
1148 
1152  virtual XMLReadableConstPtr GetData() const = 0;
1153 
1156  virtual KinBody::LinkPtr GetOffsetLink() const = 0;
1157 
1159  virtual bool IsPresent() const = 0;
1160 
1162  virtual bool IsEnabled() const = 0;
1163 
1165  virtual bool IsLocked() const = 0;
1166 
1168  virtual bool Lock(bool bDoLock) = 0;
1169 
1170 private:
1173  SensorSystemBaseWeakPtr _psensorsystem;
1174  };
1175 
1176  typedef boost::shared_ptr<KinBody::ManageData> ManageDataPtr;
1177  typedef boost::shared_ptr<KinBody::ManageData const> ManageDataConstPtr;
1178 
1181  {
1182  Save_LinkTransformation=0x00000001,
1183  Save_LinkEnable=0x00000002,
1184  Save_LinkVelocities=0x00000004,
1185  Save_JointMaxVelocityAndAcceleration=0x00000008,
1186  Save_ActiveDOF=0x00010000,
1187  Save_ActiveManipulator=0x00020000,
1188  Save_GrabbedBodies=0x00040000,
1189  };
1190 
1195  {
1196 public:
1197  KinBodyStateSaver(KinBodyPtr pbody, int options = Save_LinkTransformation|Save_LinkEnable);
1198  virtual ~KinBodyStateSaver();
1199  inline KinBodyPtr GetBody() const {
1200  return _pbody;
1201  }
1202 
1207  virtual void Restore(boost::shared_ptr<KinBody> body=boost::shared_ptr<KinBody>());
1208 
1212  virtual void Release();
1213 protected:
1214  int _options;
1215  std::vector<Transform> _vLinkTransforms;
1216  std::vector<uint8_t> _vEnabledLinks;
1217  std::vector<std::pair<Vector,Vector> > _vLinkVelocities;
1218  std::vector<int> _vdofbranches;
1219  std::vector<dReal> _vMaxVelocities, _vMaxAccelerations;
1221 private:
1222  virtual void _RestoreKinBody(boost::shared_ptr<KinBody> body);
1223  };
1224 
1225  typedef boost::shared_ptr<KinBodyStateSaver> KinBodyStateSaverPtr;
1226 
1227  virtual ~KinBody();
1228 
1230  static inline InterfaceType GetInterfaceTypeStatic() {
1231  return PT_KinBody;
1232  }
1233 
1234  virtual void Destroy();
1235 
1240  virtual bool InitFromBoxes(const std::vector<AABB>& boxes, bool visible);
1241 
1246  virtual bool InitFromBoxes(const std::vector<OBB>& boxes, bool visible);
1247 
1252  virtual bool InitFromSpheres(const std::vector<Vector>& spheres, bool visible);
1253 
1258  virtual bool InitFromTrimesh(const TriMesh& trimesh, bool visible);
1259 
1264  virtual bool InitFromGeometries(const std::vector<KinBody::GeometryInfoConstPtr>& geometries);
1265  virtual bool InitFromGeometries(const std::list<KinBody::GeometryInfo>& geometries);
1266 
1271  virtual bool Init(const std::vector<LinkInfoConstPtr>& linkinfos, const std::vector<JointInfoConstPtr>& jointinfos);
1272 
1274  virtual const std::string& GetName() const {
1275  return _name;
1276  }
1277 
1279  virtual void SetName(const std::string& name);
1280 
1283 
1284 
1288  virtual int GetDOF() const;
1289 
1293  virtual void GetDOFValues(std::vector<dReal>& v, const std::vector<int>& dofindices = std::vector<int>()) const;
1294 
1298  virtual void GetDOFVelocities(std::vector<dReal>& v, const std::vector<int>& dofindices = std::vector<int>()) const;
1299 
1303  virtual void GetDOFLimits(std::vector<dReal>& lowerlimit, std::vector<dReal>& upperlimit, const std::vector<int>& dofindices = std::vector<int>()) const;
1304 
1308  virtual void GetDOFVelocityLimits(std::vector<dReal>& lowerlimit, std::vector<dReal>& upperlimit, const std::vector<int>& dofindices = std::vector<int>()) const;
1309 
1313  virtual void GetDOFVelocityLimits(std::vector<dReal>& maxvelocities, const std::vector<int>& dofindices = std::vector<int>()) const;
1314 
1318  virtual void GetDOFAccelerationLimits(std::vector<dReal>& maxaccelerations, const std::vector<int>& dofindices = std::vector<int>()) const;
1319 
1321  virtual void GetDOFTorqueLimits(std::vector<dReal>& maxaccelerations) const;
1322 
1324  virtual void GetDOFMaxVel(std::vector<dReal>& v) const RAVE_DEPRECATED {
1325  GetDOFVelocityLimits(v);
1326  }
1327  virtual void GetDOFMaxAccel(std::vector<dReal>& v) const RAVE_DEPRECATED {
1328  GetDOFAccelerationLimits(v);
1329  }
1330  virtual void GetDOFMaxTorque(std::vector<dReal>& v) const;
1331 
1335  virtual void GetDOFResolutions(std::vector<dReal>& v, const std::vector<int>& dofindices = std::vector<int>()) const;
1336 
1340  virtual void GetDOFWeights(std::vector<dReal>& v, const std::vector<int>& dofindices = std::vector<int>()) const;
1341 
1343  virtual void SetDOFVelocityLimits(const std::vector<dReal>& maxlimits);
1344 
1346  virtual void SetDOFAccelerationLimits(const std::vector<dReal>& maxlimits);
1347 
1349  virtual void SetDOFTorqueLimits(const std::vector<dReal>& maxlimits);
1350 
1354  virtual void SetDOFWeights(const std::vector<dReal>& weights, const std::vector<int>& dofindices = std::vector<int>());
1355 
1357  virtual void SetDOFLimits(const std::vector<dReal>& lower, const std::vector<dReal>& upper);
1358 
1360  const std::vector<JointPtr>& GetJoints() const {
1361  return _vecjoints;
1362  }
1363 
1370  const std::vector<JointPtr>& GetPassiveJoints() const {
1371  return _vPassiveJoints;
1372  }
1373 
1378  virtual const std::vector<JointPtr>& GetDependencyOrderedJoints() const;
1379 
1386  virtual const std::vector< std::vector< std::pair<LinkPtr, JointPtr> > >& GetClosedLoops() const;
1387 
1406  virtual bool GetChain(int linkindex1, int linkindex2, std::vector<JointPtr>& vjoints) const;
1407 
1409  virtual bool GetChain(int linkindex1, int linkindex2, std::vector<LinkPtr>& vlinks) const;
1410 
1416  virtual bool IsDOFInChain(int linkindex1, int linkindex2, int dofindex) const;
1417 
1419  virtual int GetJointIndex(const std::string& name) const;
1420 
1422  virtual JointPtr GetJoint(const std::string& name) const;
1423 
1427  virtual JointPtr GetJointFromDOFIndex(int dofindex) const;
1429 
1436  virtual void SubtractDOFValues(std::vector<dReal>& values1, const std::vector<dReal>& values2, const std::vector<int>& dofindices=std::vector<int>()) const;
1437 
1441  virtual void SetDOFTorques(const std::vector<dReal>& torques, bool add);
1442 
1444  virtual const std::vector<LinkPtr>& GetLinks() const {
1445  return _veclinks;
1446  }
1447 
1449  virtual LinkPtr GetLink(const std::string& name) const;
1450 
1452  virtual void SimulationStep(dReal fElapsedTime);
1453 
1455  virtual void GetLinkTransformations(std::vector<Transform>& transforms) const;
1456 
1460  virtual void GetLinkTransformations(std::vector<Transform>& transforms, std::vector<int>& dofbranches) const;
1461 
1463  virtual void GetBodyTransformations(std::vector<Transform>& transforms) const RAVE_DEPRECATED {
1464  GetLinkTransformations(transforms);
1465  }
1466 
1468  virtual Transform GetTransform() const;
1469 
1473  virtual bool SetVelocity(const Vector& linearvel, const Vector& angularvel);
1474 
1485  virtual void SetDOFVelocities(const std::vector<dReal>& dofvelocities, const Vector& linearvel, const Vector& angularvel,uint32_t checklimits = CLA_CheckLimits);
1486 
1493  virtual void SetDOFVelocities(const std::vector<dReal>& dofvelocities, uint32_t checklimits = CLA_CheckLimits, const std::vector<int>& dofindices = std::vector<int>());
1494 
1498  virtual void GetLinkVelocities(std::vector<std::pair<Vector,Vector> >& velocities) const;
1499 
1511  virtual void GetLinkAccelerations(const std::vector<dReal>& dofaccelerations, std::vector<std::pair<Vector,Vector> >& linkaccelerations) const;
1512 
1521  virtual void SetTransform(const Transform& transform);
1522 
1524  virtual AABB ComputeAABB() const;
1525 
1527  virtual Vector GetCenterOfMass() const;
1528 
1530  virtual void Enable(bool enable);
1531 
1533  virtual bool IsEnabled() const;
1534 
1538  virtual bool SetVisible(bool visible);
1539 
1541  virtual bool IsVisible() const;
1542 
1548  virtual void SetDOFValues(const std::vector<dReal>& values, uint32_t checklimits = CLA_CheckLimits, const std::vector<int>& dofindices = std::vector<int>());
1549 
1550  virtual void SetJointValues(const std::vector<dReal>& values, bool checklimits = true) {
1551  SetDOFValues(values,static_cast<uint32_t>(checklimits));
1552  }
1553 
1559  virtual void SetDOFValues(const std::vector<dReal>& values, const Transform& transform, uint32_t checklimits = CLA_CheckLimits);
1560 
1561  virtual void SetJointValues(const std::vector<dReal>& values, const Transform& transform, bool checklimits = true)
1562  {
1563  SetDOFValues(values,transform,static_cast<uint32_t>(checklimits));
1564  }
1565 
1567  virtual void SetLinkTransformations(const std::vector<Transform>& transforms);
1568 
1572  virtual void SetLinkTransformations(const std::vector<Transform>& transforms, const std::vector<int>& dofbranches);
1573 
1575  virtual void SetBodyTransformations(const std::vector<Transform>& transforms) RAVE_DEPRECATED {
1576  SetLinkTransformations(transforms);
1577  }
1578 
1580  virtual void SetLinkVelocities(const std::vector<std::pair<Vector,Vector> >& velocities);
1581 
1590  virtual void ComputeJacobianTranslation(int linkindex, const Vector& position, std::vector<dReal>& jacobian, const std::vector<int>& dofindices=std::vector<int>()) const;
1591 
1593  virtual void CalculateJacobian(int linkindex, const Vector& position, std::vector<dReal>& jacobian) const {
1594  ComputeJacobianTranslation(linkindex,position,jacobian);
1595  }
1596 
1598  virtual void CalculateJacobian(int linkindex, const Vector& position, boost::multi_array<dReal,2>& jacobian) const;
1599 
1605  virtual void CalculateRotationJacobian(int linkindex, const Vector& quat, std::vector<dReal>& jacobian) const;
1606 
1608  virtual void CalculateRotationJacobian(int linkindex, const Vector& quat, boost::multi_array<dReal,2>& jacobian) const;
1609 
1614  virtual void ComputeJacobianAxisAngle(int linkindex, std::vector<dReal>& jacobian, const std::vector<int>& dofindices=std::vector<int>()) const;
1615 
1617  virtual void CalculateAngularVelocityJacobian(int linkindex, std::vector<dReal>& jacobian) const {
1618  ComputeJacobianAxisAngle(linkindex,jacobian);
1619  }
1620 
1622  virtual void CalculateAngularVelocityJacobian(int linkindex, boost::multi_array<dReal,2>& jacobian) const;
1623 
1645 
1646  virtual void ComputeHessianTranslation(int linkindex, const Vector& position, std::vector<dReal>& hessian, const std::vector<int>& dofindices=std::vector<int>()) const;
1647 
1668 
1669  virtual void ComputeHessianAxisAngle(int linkindex, std::vector<dReal>& hessian, const std::vector<int>& dofindices=std::vector<int>()) const;
1670 
1672  typedef std::map<int, std::pair<Vector,Vector> > ForceTorqueMap;
1673 
1683  virtual void ComputeInverseDynamics(std::vector<dReal>& doftorques, const std::vector<dReal>& dofaccelerations, const ForceTorqueMap& externalforcetorque=ForceTorqueMap()) const;
1684 
1702  virtual void ComputeInverseDynamics(boost::array< std::vector<dReal>, 3>& doftorquecomponents, const std::vector<dReal>& dofaccelerations, const ForceTorqueMap& externalforcetorque=ForceTorqueMap()) const;
1703 
1705  virtual bool CheckSelfCollision(CollisionReportPtr report = CollisionReportPtr()) const;
1706 
1708  virtual bool IsAttached(KinBodyConstPtr body) const;
1709 
1713  virtual void GetAttached(std::set<KinBodyPtr>& setAttached) const;
1714 
1716  virtual bool IsRobot() const {
1717  return false;
1718  }
1719 
1724  virtual int GetEnvironmentId() const;
1725 
1734  virtual int8_t DoesAffect(int jointindex, int linkindex) const;
1735 
1738  return GetUserData("_viewer_");
1739  }
1740 
1743  {
1744  AO_Enabled = 1,
1745  AO_ActiveDOFs = 2,
1746  };
1747 
1750  virtual const std::set<int>& GetNonAdjacentLinks(int adjacentoptions=0) const;
1751 
1753  virtual const std::set<int>& GetAdjacentLinks() const;
1754 
1757  return GetUserData("_physics_");
1758  }
1761  return GetUserData("_collision_");
1762  }
1763  virtual ManageDataPtr GetManageData() const {
1764  return _pManageData;
1765  }
1766 
1772  virtual int GetUpdateStamp() const {
1773  return _nUpdateStampId;
1774  }
1775 
1776  virtual void Clone(InterfaceBaseConstPtr preference, int cloningoptions);
1777 
1785  virtual UserDataPtr RegisterChangeCallback(int properties, const boost::function<void()>& callback) const;
1786 
1787  void Serialize(BaseXMLWriterPtr writer, int options=0) const;
1788 
1795  virtual const std::string& GetKinematicsGeometryHash() const;
1796 
1802  virtual void SetZeroConfiguration();
1803 
1805  virtual void SetNonCollidingConfiguration();
1806 
1809 
1810 
1814  virtual ConfigurationSpecification GetConfigurationSpecification(const std::string& interpolation="") const;
1815 
1819  virtual ConfigurationSpecification GetConfigurationSpecificationIndices(const std::vector<int>& indices, const std::string& interpolation="") const;
1820 
1825  virtual void SetConfigurationValues(std::vector<dReal>::const_iterator itvalues, uint32_t checklimits = CLA_CheckLimits);
1826 
1828  virtual void GetConfigurationValues(std::vector<dReal>& v) const;
1829 
1831 
1833  virtual void serialize(std::ostream& o, int options) const;
1834 
1835 protected:
1837  KinBody(InterfaceType type, EnvironmentBasePtr penv);
1839  return boost::static_pointer_cast<KinBody>(shared_from_this());
1840  }
1842  return boost::static_pointer_cast<KinBody const>(shared_from_this());
1843  }
1844 
1847  SetUserData("_physics_", pdata);
1848  }
1851  SetUserData("_collision_", pdata);
1852  }
1853 
1856  SetUserData("_viewer_",pdata);
1857  }
1858  virtual void SetManageData(ManageDataPtr pdata) {
1859  _pManageData = pdata;
1860  }
1861 
1868  virtual void _ComputeInternalInformation();
1869 
1873  virtual void _ComputeDOFLinkVelocities(std::vector<dReal>& dofvelocities, std::vector<std::pair<Vector,Vector> >& linkvelocities, bool usebaselinkvelocity=true) const;
1874 
1880  virtual void _ComputeLinkAccelerations(const std::vector<dReal>& dofvelocities, const std::vector<dReal>& dofaccelerations, const std::vector< std::pair<Vector, Vector> >& linkvelocities, std::vector<std::pair<Vector,Vector> >& linkaccelerations, const Vector& gravity) const;
1881 
1886  virtual void _ParametersChanged(int parameters);
1887 
1889  virtual bool _IsAttached(KinBodyConstPtr body, std::set<KinBodyConstPtr>& setChecked) const;
1890 
1892  virtual void _AttachBody(KinBodyPtr body);
1893 
1897  virtual bool _RemoveAttachedBody(KinBodyPtr body);
1898 
1900  virtual void _ResetInternalCollisionCache();
1901 
1902  std::string _name;
1903  std::vector<JointPtr> _vecjoints;
1904  std::vector<JointPtr> _vTopologicallySortedJoints;
1905  std::vector<JointPtr> _vTopologicallySortedJointsAll;
1907  std::vector<JointPtr> _vDOFOrderedJoints;
1908  std::vector<LinkPtr> _veclinks;
1909  std::vector<int> _vDOFIndices;
1910  std::vector<std::pair<int16_t,int16_t> > _vAllPairsShortestPaths;
1911  std::vector<int8_t> _vJointsAffectingLinks;
1912  std::vector< std::vector< std::pair<LinkPtr,JointPtr> > > _vClosedLoops;
1913  std::vector< std::vector< std::pair<int16_t,int16_t> > > _vClosedLoopIndices;
1914  std::vector<JointPtr> _vPassiveJoints;
1915  std::set<int> _setAdjacentLinks;
1916 
1917  std::vector< std::pair<std::string, std::string> > _vForcedAdjacentLinks;
1918  std::list<KinBodyWeakPtr> _listAttachedBodies;
1919  mutable std::list<UserDataWeakPtr> _listRegisteredCallbacks;
1920 
1921  mutable boost::array<std::set<int>, 4> _setNonAdjacentLinks;
1923  std::vector<Transform> _vInitialLinkTransformations;
1924 
1926 
1928  mutable int _nUpdateStampId;
1933 private:
1934  mutable std::string __hashkinematics;
1935  mutable std::vector<dReal> _vTempJoints;
1936  virtual const char* GetHash() const {
1937  return OPENRAVE_KINBODY_HASH;
1938  }
1939 
1940 #ifdef RAVE_PRIVATE
1941 #ifdef _MSC_VER
1942  friend class Environment;
1943  friend class OpenRAVEXMLParser::KinBodyXMLReader;
1944  friend class OpenRAVEXMLParser::JointXMLReader;
1945  friend class XFileReader;
1946 #else
1947  friend class ::Environment;
1948  friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
1949  friend class ::OpenRAVEXMLParser::JointXMLReader;
1950  friend class ::XFileReader;
1951 #endif
1952 #endif
1953 
1954  friend class ColladaReader;
1955  friend class ColladaWriter;
1956  friend class PhysicsEngineBase;
1957  friend class CollisionCheckerBase;
1958  friend class ViewerBase;
1959  friend class SensorSystemBase;
1960  friend class RaveDatabase;
1961  friend class ChangeCallbackData;
1962 };
1963 
1964 } // end namespace OpenRAVE
1965 
1966 #endif