Choreonoid  1.5
JointPath.h
Go to the documentation of this file.
1 
7 #ifndef CNOID_BODY_JOINT_PATH_H
8 #define CNOID_BODY_JOINT_PATH_H
9 
10 #include "LinkPath.h"
11 #include "InverseKinematics.h"
12 #include <cnoid/Referenced>
13 #include <cnoid/EigenTypes>
14 #include <boost/function.hpp>
15 #include "exportdecl.h"
16 
17 namespace cnoid {
18 
19 class JointPathIkImpl;
20 
22 {
23 public:
24 
25  JointPath();
26  JointPath(Link* base, Link* end);
27  JointPath(Link* end);
28  virtual ~JointPath();
29 
30  bool setPath(Link* base, Link* end);
31  bool setPath(Link* end);
32 
34  bool find(Link* base, Link* end) { return setPath(base, end); }
36  bool find(Link* end) { return setPath(end); }
37 
38  bool empty() const {
39  return joints.empty();
40  }
41 
42  int numJoints() const {
43  return joints.size();
44  }
45 
46  Link* joint(int index) const {
47  return joints[index];
48  }
49 
50  Link* baseLink() const {
51  return linkPath.baseLink();
52  }
53 
54  Link* endLink() const {
55  return linkPath.endLink();
56  }
57 
58  bool isJointDownward(int index) const {
59  return (index >= numUpwardJointConnections);
60  }
61 
62  void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) const {
63  linkPath.calcForwardKinematics(calcVelocity, calcAcceleration);
64  }
65 
66  int indexOf(const Link* link) const;
67 
68  bool isBestEffortIKmode() const;
69  void setBestEffortIKmode(bool on);
70 
71  void setNumericalIKmaxIKerror(double e);
72  void setNumericalIKdeltaScale(double s);
73  void setNumericalIKmaxIterations(int n);
74  void setNumericalIKdampingConstant(double lambda);
75 
76  static double numericalIKdefaultDeltaScale();
77  static int numericalIKdefaultMaxIterations();
78  static double numericalIKdefaultMaxIKerror();
79  static double numericalIKdefaultDampingConstant();
80 
81  void customizeTarget(
82  int numTargetElements,
83  boost::function<double(VectorXd& out_error)> errorFunc,
84  boost::function<void(MatrixXd& out_Jacobian)> jacobianFunc);
85 
86  // InverseKinematics Interface
87  virtual bool hasAnalyticalIK() const;
88 
89  JointPath& storeCurrentPosition();
90 
91  JointPath& setGoal(const Vector3& end_p, const Matrix3& end_R) {
92  targetTranslationGoal = end_p;
93  targetRotationGoal = end_R;
94  return *this;
95  }
96 
97  JointPath& setGoal(const Vector3& base_p, const Matrix3& base_R, const Vector3& end_p, const Matrix3& end_R);
98 
99  virtual bool calcInverseKinematics();
100 
103  }
104 
105  int numIterations() const;
106 
108  void calcJacobian(Eigen::MatrixXd& out_J) const;
110  virtual bool calcInverseKinematics(const Vector3& end_p, const Matrix3& end_R);
112  bool calcInverseKinematics(
113  const Vector3& base_p, const Matrix3& base_R, const Vector3& end_p, const Matrix3& end_R);
115  void setNumericalIKtruncateRatio(double r);
117  static double numericalIKdefaultTruncateRatio();
118 
119 protected:
120 
121  virtual void onJointPathUpdated();
122 
123 private:
124 
125  JointPath(const JointPath& org);
126 
127  void initialize();
128  void extractJoints();
129  JointPathIkImpl* getOrCreateIK();
130 
131  LinkPath linkPath;
132  std::vector<Link*> joints;
133  int numUpwardJointConnections;
134  Vector3 targetTranslationGoal;
135  Matrix3 targetRotationGoal;
136  bool needForwardKinematicsBeforeIK;
137  JointPathIkImpl* ik;
138 };
139 
140 typedef boost::shared_ptr<JointPath> JointPathPtr;
141 
142 class Body;
143 
149 CNOID_EXPORT JointPathPtr getCustomJointPath(Body* body, Link* baseLink, Link* targetLink);
150 
151 };
152 
153 #endif
boost::shared_ptr< JointPath > JointPathPtr
Definition: BodyMotionPoseProvider.h:20
Definition: Body.h:28
Link * joint(int index) const
Definition: JointPath.h:46
Link * baseLink() const
Definition: JointPath.h:50
Definition: InverseKinematics.h:14
JointPath & setGoal(const Vector3 &end_p, const Matrix3 &end_R)
Definition: JointPath.h:91
bool find(Link *end)
Deprecated. Use "setPath()" instead of this.
Definition: JointPath.h:36
int numJoints() const
Definition: JointPath.h:42
virtual bool calcInverseKinematics()
Definition: JointPath.cpp:366
Link * endLink() const
Definition: JointPath.h:54
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false) const
Definition: JointPath.h:62
bool isJointDownward(int index) const
Definition: JointPath.h:58
Definition: JointPath.h:21
Definition: LinkPath.h:14
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
bool calcNumericalIK()
Definition: JointPath.h:101
bool find(Link *base, Link *end)
Deprecated. Use "setPath()" instead of this.
Definition: JointPath.h:34
bool empty() const
Definition: JointPath.h:38
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
CNOID_EXPORT JointPathPtr getCustomJointPath(Body *body, Link *baseLink, Link *targetLink)
Definition: JointPath.cpp:625
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57