Choreonoid  1.1
JointPath.h
[詳解]
1 
7 #ifndef CNOID_BODY_JOINT_PATH_H_INCLUDED
8 #define CNOID_BODY_JOINT_PATH_H_INCLUDED
9 
10 #include "LinkPath.h"
11 #include "InverseKinematics.h"
12 #include <cnoid/EigenTypes>
13 #include <boost/shared_ptr.hpp>
14 #include "exportdecl.h"
15 
16 namespace cnoid {
17 
19  {
20  public:
21 
22  JointPath();
23  JointPath(Link* base, Link* end);
24  JointPath(Link* end);
25  virtual ~JointPath();
26 
27  bool find(Link* base, Link* end);
28  bool find(Link* end);
29 
30  inline bool empty() const {
31  return joints.empty();
32  }
33 
34  inline int numJoints() const {
35  return joints.size();
36  }
37 
38  inline Link* joint(int index) const {
39  return joints[index];
40  }
41 
42  inline Link* baseLink() const {
43  return linkPath.baseLink();
44  }
45 
46  inline Link* endLink() const {
47  return linkPath.endLink();
48  }
49 
50  inline bool isJointDownward(int index) const {
51  return (index >= numUpwardJointConnections);
52  }
53 
54  inline void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) const {
55  linkPath.calcForwardKinematics(calcVelocity, calcAcceleration);
56  }
57 
58  void calcJacobian(Eigen::MatrixXd& out_J) const;
59 
60  void setMaxIKerror(double e);
61  void setBestEffortIKMode(bool on);
62 
63  // InverseKinematics Interface
64  virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R);
65  virtual bool hasAnalyticalIK() const;
66 
67  bool calcInverseKinematics(
68  const Vector3& base_p, const Matrix3& base_R, const Vector3& end_p, const Matrix3& end_R);
69 
70  protected:
71 
72  virtual void onJointPathUpdated();
73 
74  double maxIKerrorSqr;
76 
77  private:
78 
79  void initialize();
80  void extractJoints();
81 
82  LinkPath linkPath;
83  std::vector<Link*> joints;
84  int numUpwardJointConnections;
85  };
86 
87  typedef boost::shared_ptr<JointPath> JointPathPtr;
88 
89 };
90 
91 
92 #endif
int numJoints() const
Definition: JointPath.h:34
boost::shared_ptr< JointPath > JointPathPtr
Definition: Body.h:25
Definition: InverseKinematics.h:13
Link * joint(int index) const
Definition: JointPath.h:38
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false) const
Definition: JointPath.h:54
Link * baseLink() const
Definition: JointPath.h:42
bool empty() const
Definition: JointPath.h:30
Link * endLink() const
Definition: JointPath.h:46
double maxIKerrorSqr
Definition: JointPath.h:74
Definition: JointPath.h:18
Definition: LinkPath.h:14
bool isJointDownward(int index) const
Definition: JointPath.h:50
bool isBestEffortIKMode
Definition: JointPath.h:75
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
#define CNOID_EXPORT
Definition: Util/exportdecl.h:13
bool initialize()
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25