Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00015 #ifndef HRPMODEL_JOINT_PATH_H_INCLUDED
00016 #define HRPMODEL_JOINT_PATH_H_INCLUDED
00017
00018 #include <boost/shared_ptr.hpp>
00019 #include <hrpUtil/Eigen3d.h>
00020 #include "LinkPath.h"
00021 #include "InverseKinematics.h"
00022 #include "Config.h"
00023
00024 namespace hrp {
00025
00026 class HRPMODEL_API JointPath : public InverseKinematics
00027 {
00028 public:
00029
00030 JointPath();
00031 JointPath(Link* base, Link* end);
00032 JointPath(Link* end);
00033 virtual ~JointPath();
00034
00035 bool find(Link* base, Link* end);
00036 bool find(Link* end);
00037
00038 inline bool empty() const {
00039 return joints.empty();
00040 }
00041
00042 inline unsigned int numJoints() const {
00043 return joints.size();
00044 }
00045
00046 inline Link* joint(int index) const {
00047 return joints[index];
00048 }
00049
00050 inline Link* baseLink() const {
00051 return linkPath.baseLink();
00052 }
00053
00054 inline Link* endLink() const {
00055 return linkPath.endLink();
00056 }
00057
00058 inline bool isJointDownward(int index) const {
00059 return (index >= numUpwardJointConnections);
00060 }
00061
00062 inline void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) const {
00063 linkPath.calcForwardKinematics(calcVelocity, calcAcceleration);
00064 }
00065
00066 void calcJacobian(dmatrix& out_J) const;
00067
00068 inline dmatrix Jacobian() const {
00069 dmatrix J;
00070 calcJacobian(J);
00071 return J;
00072 }
00073
00074
00075 virtual void setMaxIKError(double e);
00076 virtual void setBestEffortIKMode(bool on);
00077 virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R);
00078 virtual bool hasAnalyticalIK();
00079
00080 bool calcInverseKinematics(
00081 const Vector3& base_p, const Matrix33& base_R, const Vector3& end_p, const Matrix33& end_R);
00082
00083 protected:
00084
00085 virtual void onJointPathUpdated();
00086
00087 double maxIKErrorSqr;
00088 bool isBestEffortIKMode;
00089
00090 private:
00091
00092 void initialize();
00093 void extractJoints();
00094
00095 LinkPath linkPath;
00096 std::vector<Link*> joints;
00097 int numUpwardJointConnections;
00098 };
00099
00100 typedef boost::shared_ptr<JointPath> JointPathPtr;
00101
00102 };
00103
00104
00105 #endif