22 #ifndef OPENRAVE_ROBOT_H
23 #define OPENRAVE_ROBOT_H
59 Manipulator(
const Manipulator &r);
62 Manipulator(
RobotBasePtr probot, boost::shared_ptr<Manipulator const> r);
65 virtual ~Manipulator();
79 virtual std::pair<Vector,Vector> GetVelocity()
const;
82 return GetTransform();
85 virtual const std::string&
GetName()
const {
114 return _info._tLocalTool;
120 virtual void SetLocalToolTransform(
const Transform& t);
125 virtual void SetName(
const std::string& name);
129 return GetLocalToolTransform();
134 return __vgripperdofindices;
141 return __varmdofindices;
146 return _info._vClosingDirection;
152 virtual void SetLocalToolDirection(
const Vector& direction);
156 return _info._vdirection;
161 return GetLocalToolDirection();
172 virtual bool FindIKSolution(
const IkParameterization& param, std::vector<dReal>& solution,
int filteroptions)
const;
173 virtual bool FindIKSolution(
const IkParameterization& param,
const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution,
int filteroptions)
const;
175 virtual bool FindIKSolution(
const IkParameterization& param,
const std::vector<dReal>& vFreeParameters,
int filteroptions,
IkReturnPtr ikreturn)
const;
182 virtual bool FindIKSolutions(
const IkParameterization& param, std::vector<std::vector<dReal> >& solutions,
int filteroptions)
const;
183 virtual bool FindIKSolutions(
const IkParameterization& param,
const std::vector<dReal>& vFreeParameters, std::vector<std::vector<dReal> >& solutions,
int filteroptions)
const;
184 virtual bool FindIKSolutions(
const IkParameterization& param,
int filteroptions, std::vector<IkReturnPtr>& vikreturns)
const;
185 virtual bool FindIKSolutions(
const IkParameterization& param,
const std::vector<dReal>& vFreeParameters,
int filteroptions, std::vector<IkReturnPtr>& vikreturns)
const;
217 virtual void GetChildJoints(std::vector<JointPtr>& vjoints)
const;
220 virtual void GetChildDOFIndices(std::vector<int>& vdofndices)
const;
230 virtual void GetChildLinks(std::vector<LinkPtr>& vlinks)
const;
236 virtual void GetIndependentLinks(std::vector<LinkPtr>& vlinks)
const;
295 virtual void CalculateJacobian(std::vector<dReal>& jacobian)
const;
298 virtual void CalculateJacobian(boost::multi_array<dReal,2>& jacobian)
const;
301 virtual void CalculateRotationJacobian(std::vector<dReal>& jacobian)
const;
304 virtual void CalculateRotationJacobian(boost::multi_array<dReal,2>& jacobian)
const;
307 virtual void CalculateAngularVelocityJacobian(std::vector<dReal>& jacobian)
const;
310 virtual void CalculateAngularVelocityJacobian(boost::multi_array<dReal,2>& jacobian)
const;
317 virtual void serialize(std::ostream& o,
int options)
const;
320 virtual const std::string& GetStructureHash()
const;
325 virtual const std::string& GetKinematicsStructureHash()
const;
328 virtual void _ComputeInternalInformation();
334 std::vector<int> __vgripperdofindices, __varmdofindices;
337 mutable std::string __hashstructure, __hashkinematicsstructure;
341 friend class OpenRAVEXMLParser::ManipulatorXMLReader;
342 friend class OpenRAVEXMLParser::RobotXMLReader;
343 friend class XFileReader;
345 friend class ::OpenRAVEXMLParser::ManipulatorXMLReader;
346 friend class ::OpenRAVEXMLParser::RobotXMLReader;
347 friend class ::XFileReader;
350 friend class ColladaReader;
395 return LinkPtr(pattachedlink)->GetTransform()*trelative;
407 virtual void SetRelativeTransform(
const Transform& t);
409 virtual void serialize(std::ostream& o,
int options)
const;
412 virtual const std::string& GetStructureHash()
const;
420 mutable std::string __hashstructure;
423 friend class OpenRAVEXMLParser::AttachedSensorXMLReader;
424 friend class OpenRAVEXMLParser::RobotXMLReader;
425 friend class XFileReader;
427 friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;
428 friend class ::OpenRAVEXMLParser::RobotXMLReader;
429 friend class ::XFileReader;
432 friend class ColladaReader;
463 virtual void Restore(boost::shared_ptr<RobotBase> robot=boost::shared_ptr<RobotBase>());
468 virtual void Release();
478 virtual void _RestoreRobot(boost::shared_ptr<RobotBase> robot);
490 virtual void Destroy();
497 virtual bool Init(
const std::vector<LinkInfoConstPtr>& linkinfos,
const std::vector<JointInfoConstPtr>& jointinfos,
const std::vector<ManipulatorInfoConstPtr>& manipinfos,
const std::vector<AttachedSensorInfoConstPtr>& attachedsensorinfos);
502 return _vecManipulators;
509 virtual void SetName(
const std::string& name);
511 virtual void SetDOFValues(
const std::vector<dReal>& vJointValues, uint32_t checklimits = 1,
const std::vector<int>& dofindices = std::vector<int>());
512 virtual void SetDOFValues(
const std::vector<dReal>& vJointValues,
const Transform& transbase, uint32_t checklimits = 1);
514 virtual void SetLinkTransformations(
const std::vector<Transform>& transforms);
515 virtual void SetLinkTransformations(
const std::vector<Transform>& transforms,
const std::vector<int>& dofbranches);
517 virtual bool SetVelocity(
const Vector& linearvel,
const Vector& angularvel);
518 virtual void SetDOFVelocities(
const std::vector<dReal>& dofvelocities,
const Vector& linearvel,
const Vector& angularvel,uint32_t checklimits = 1);
519 virtual void SetDOFVelocities(
const std::vector<dReal>& dofvelocities, uint32_t checklimits = 1,
const std::vector<int>& dofindices = std::vector<int>());
523 virtual void SetTransform(
const Transform& trans);
559 virtual void SetActiveDOFs(
const std::vector<int>& dofindices,
int affine,
const Vector& rotationaxis);
561 return _nActiveDOF >= 0 ? _nActiveDOF : GetDOF();
578 virtual const std::vector<int>& GetActiveDOFIndices()
const;
581 return vActvAffineRotationAxis;
583 virtual void SetAffineTranslationLimits(
const Vector& lower,
const Vector& upper);
584 virtual void SetAffineRotationAxisLimits(
const Vector& lower,
const Vector& upper);
585 virtual void SetAffineRotation3DLimits(
const Vector& lower,
const Vector& upper);
591 virtual void SetAffineRotationQuatLimits(
const Vector& quatangle);
592 virtual void SetAffineTranslationMaxVels(
const Vector& vels);
593 virtual void SetAffineRotationAxisMaxVels(
const Vector& vels);
594 virtual void SetAffineRotation3DMaxVels(
const Vector& vels);
595 virtual void SetAffineRotationQuatMaxVels(
dReal vels);
596 virtual void SetAffineTranslationResolution(
const Vector& resolution);
597 virtual void SetAffineRotationAxisResolution(
const Vector& resolution);
598 virtual void SetAffineRotation3DResolution(
const Vector& resolution);
599 virtual void SetAffineRotationQuatResolution(
dReal resolution);
600 virtual void SetAffineTranslationWeights(
const Vector& weights);
601 virtual void SetAffineRotationAxisWeights(
const Vector& weights);
602 virtual void SetAffineRotation3DWeights(
const Vector& weights);
603 virtual void SetAffineRotationQuatWeights(
dReal weights);
605 virtual void GetAffineTranslationLimits(
Vector& lower,
Vector& upper)
const;
606 virtual void GetAffineRotationAxisLimits(
Vector& lower,
Vector& upper)
const;
607 virtual void GetAffineRotation3DLimits(
Vector& lower,
Vector& upper)
const;
613 return _vRotationQuatLimitStart * _fQuatLimitMaxAngle;
616 return _vTranslationMaxVels;
619 return _vRotationAxisMaxVels;
622 return _vRotation3DMaxVels;
625 return _fQuatMaxAngleVelocity;
628 return _vTranslationResolutions;
631 return _vRotationAxisResolutions;
634 return _vRotation3DResolutions;
637 return _fQuatAngleResolution;
640 return _vTranslationWeights;
643 return _vRotationAxisWeights;
646 return _vRotation3DWeights;
649 return _fQuatAngleResolution;
652 virtual void SetActiveDOFValues(
const std::vector<dReal>& values, uint32_t checklimits=1);
653 virtual void GetActiveDOFValues(std::vector<dReal>& v)
const;
654 virtual void SetActiveDOFVelocities(
const std::vector<dReal>& velocities, uint32_t checklimits=1);
655 virtual void GetActiveDOFVelocities(std::vector<dReal>& velocities)
const;
656 virtual void GetActiveDOFLimits(std::vector<dReal>& lower, std::vector<dReal>& upper)
const;
657 virtual void GetActiveDOFResolutions(std::vector<dReal>& v)
const;
658 virtual void GetActiveDOFWeights(std::vector<dReal>& v)
const;
659 virtual void GetActiveDOFVelocityLimits(std::vector<dReal>& v)
const;
660 virtual void GetActiveDOFAccelerationLimits(std::vector<dReal>& v)
const;
662 return GetActiveDOFVelocityLimits(v);
665 return GetActiveDOFAccelerationLimits(v);
669 virtual void SubtractActiveDOFValues(std::vector<dReal>& q1,
const std::vector<dReal>& q2)
const;
678 virtual ManipulatorPtr SetActiveManipulator(
const std::string& manipname);
679 virtual void SetActiveManipulator(ManipulatorConstPtr pmanip);
680 virtual ManipulatorPtr GetActiveManipulator();
681 virtual ManipulatorConstPtr GetActiveManipulator()
const;
689 virtual ManipulatorPtr AddManipulator(
const ManipulatorInfo& manipinfo);
695 virtual void RemoveManipulator(ManipulatorPtr manip);
716 virtual
void CalculateActiveJacobian(
int index, const
Vector& offset, std::vector<
dReal>& jacobian) const;
717 virtual
void CalculateActiveJacobian(
int index, const
Vector& offset, boost::multi_array<
dReal,2>& jacobian) const;
719 virtual
void CalculateActiveRotationJacobian(
int index, const
Vector& qInitialRot, std::vector<dReal>& jacobian) const;
720 virtual
void CalculateActiveRotationJacobian(
int index, const
Vector& qInitialRot, boost::multi_array<dReal,2>& jacobian) const;
728 virtual
void CalculateActiveAngularVelocityJacobian(
int index, std::vector<dReal>& jacobian) const;
729 virtual
void CalculateActiveAngularVelocityJacobian(
int index, boost::multi_array<dReal,2>& jacobian) const;
731 virtual const std::set<
int>& GetNonAdjacentLinks(
int adjacentoptions=0) const;
752 virtual
bool Grab(
KinBodyPtr body, LinkPtr pRobotLinkToGrabWith, const std::set<
int>& setRobotLinksToIgnore);
760 virtual
bool Grab(
KinBodyPtr body, LinkPtr pRobotLinkToGrabWith);
769 virtual
bool Grab(
KinBodyPtr body, const std::set<
int>& setRobotLinksToIgnore);
785 virtual
void ReleaseAllGrabbed();
791 virtual
void RegrabAll();
803 virtual
void GetGrabbed(std::vector<
KinBodyPtr>& vbodies) const;
809 virtual
void GetGrabbedInfo(std::vector<GrabbedInfoPtr>& vgrabbedinfo) const;
816 virtual
void ResetGrabbed(const std::vector<GrabbedInfoConstPtr>& vgrabbedinfo);
823 virtual
void GetIgnoredLinksOfGrabbed(
KinBodyConstPtr body, std::list<
KinBody::LinkConstPtr>& ignorelinks) const;
831 virtual
void SimulationStep(dReal fElapsedTime);
853 virtual
bool CheckLinkSelfCollision(
int ilinkindex, const
Transform& tlinktrans, CollisionReportPtr report = CollisionReportPtr());
859 virtual
bool IsRobot()
const {
863 virtual void serialize(std::ostream& o,
int options)
const;
867 virtual const std::string& GetRobotStructureHash()
const;
886 return boost::static_pointer_cast<
RobotBase>(shared_from_this());
889 return boost::static_pointer_cast<
RobotBase const>(shared_from_this());
896 virtual void _ComputeInternalInformation();
901 virtual void _ParametersChanged(
int parameters);
904 virtual void _UpdateGrabbedBodies();
905 virtual void _UpdateAttachedSensors();
925 virtual const char* GetHash()
const {
928 virtual const char* GetKinBodyHash()
const {
931 mutable std::string __hashrobotstructure;
932 mutable std::vector<dReal> _vTempRobotJoints;
936 friend class Environment;
937 friend class OpenRAVEXMLParser::RobotXMLReader;
938 friend class OpenRAVEXMLParser::ManipulatorXMLReader;
939 friend class OpenRAVEXMLParser::AttachedSensorXMLReader;
940 friend class XFileReader;
942 friend class ::Environment;
943 friend class ::OpenRAVEXMLParser::RobotXMLReader;
944 friend class ::OpenRAVEXMLParser::ManipulatorXMLReader;
945 friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;
946 friend class ::XFileReader;
949 friend class ColladaWriter;
950 friend class ColladaReader;
951 friend class RaveDatabase;
952 friend class Grabbed;