JointPath.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
16 #include "JointPath.h"
17 #include "Link.h"
18 #include <hrpUtil/MatrixSolvers.h>
19 #include <algorithm>
20 
21 using namespace std;
22 using namespace hrp;
23 
24 
25 JointPath::JointPath()
26 {
27  initialize();
28 }
29 
30 
31 JointPath::JointPath(Link* base, Link* end)
32  : linkPath(base, end),
33  joints(linkPath.size())
34 {
35  initialize();
36  extractJoints();
37 }
38 
39 
41  : linkPath(end),
42  joints(linkPath.size())
43 {
44  initialize();
45  extractJoints();
46 }
47 
48 
50 {
51  maxIKErrorSqr = 1.0e-6 * 1.0e-6;
52  isBestEffortIKMode = false;
53 }
54 
55 
57 {
58 
59 }
60 
61 
62 bool JointPath::find(Link* base, Link* end)
63 {
64  if(linkPath.find(base, end)){
65  extractJoints();
66  }
68 
69  return (!joints.empty());
70 }
71 
72 
74 {
75  linkPath.find(end);
76  extractJoints();
78 
79  return !joints.empty();
80 }
81 
82 
84 {
86 
87  int n = linkPath.size();
88  if(n <= 1){
89  joints.clear();
90  } else {
91  int i = 0;
92  if(linkPath.isDownward(i)){
93  i++;
94  }
95  joints.resize(n); // reserve size n buffer
96  joints.clear();
97  int m = n - 1;
98  while(i < m){
99  Link* link = linkPath[i];
100  if(link->jointId >= 0){
102  joints.push_back(link);
103  if(!linkPath.isDownward(i)){
105  }
106  }
107  }
108  ++i;
109  }
110  if(linkPath.isDownward(m-1)){
111  Link* link = linkPath[m];
112  if(link->jointId >= 0){
114  joints.push_back(link);
115  }
116  }
117  }
118  }
119 }
120 
121 
123 {
124 
125 }
126 
127 
128 void JointPath::calcJacobian(dmatrix& out_J, const Vector3& local_p) const
129 {
130  const int n = joints.size();
131  out_J.resize(6, n);
132 
133  if(n > 0){
134 
135  Link* targetLink = linkPath.endLink();
136 
137  for(int i=0; i < n; ++i){
138 
139  Link* link = joints[i];
140 
141  switch(link->jointType){
142 
144  {
145  Vector3 omega(link->R * link->a);
146  Vector3 arm(targetLink->p + targetLink->R * local_p - link->p);
147  if(!isJointDownward(i)){
148  omega *= -1.0;
149  }
150  Vector3 dp(omega.cross(arm));
151  out_J.col(i) << dp, omega;
152  }
153  break;
154 
155  case Link::SLIDE_JOINT:
156  {
157  Vector3 dp(link->R * link->d);
158  if(!isJointDownward(i)){
159  dp *= -1.0;
160  }
161  out_J.col(i) << dp, Vector3::Zero();
162  }
163  break;
164 
165  default:
166  out_J.col(i).setZero();
167  }
168  }
169  }
170 }
171 
172 
173 void JointPath::calcJacobianDot(dmatrix& out_dJ, const Vector3& local_p) const
174 {
175  const int n = joints.size();
176  out_dJ.resize(6, n);
177 
178  if (n > 0) {
179 
180  Link* targetLink = linkPath.endLink();
181 
182  for (int i=0; i < n; ++i) {
183 
184  Link* link = joints[i];
185 
186  switch (link->jointType) {
187 
189  {
190  Vector3 omega(link->R * link->a);
191  Vector3 arm(targetLink->p + targetLink->R * local_p - link->p);
192 
193  Vector3 omegaDot(hat(link->w) * link->R * link->a);
194  Vector3 armDot(targetLink->v + hat(targetLink->w) * targetLink->R * local_p - link->v);
195 
196  if (!isJointDownward(i)) {
197  omega *= -1.0;
198  omegaDot *= -1.0;
199  }
200 
201  Vector3 ddp(omegaDot.cross(arm) + omega.cross(armDot));
202 
203  out_dJ.col(i) << ddp, omegaDot;
204  }
205  break;
206 
207  case Link::SLIDE_JOINT:
208  {
209  Vector3 ddp(hat(link->w) * link->R * link->d);
210  if (!isJointDownward(i)) {
211  ddp *= -1.0;
212  }
213  out_dJ.col(i) << ddp, Vector3::Zero();
214  }
215  break;
216 
217  default:
218  out_dJ.col(i).setZero();
219  }
220  }
221  }
222 }
223 
224 
226 {
227  maxIKErrorSqr = e * e;
228 }
229 
230 
232 {
233  isBestEffortIKMode = on;
234 }
235 
236 
238 (const Vector3& base_p, const Matrix33& base_R, const Vector3& end_p, const Matrix33& end_R)
239 {
240  Link* baseLink = linkPath.baseLink();
241  baseLink->p = base_p;
242  baseLink->R = base_R;
243 
244  if(!hasAnalyticalIK()){
245  calcForwardKinematics();
246  }
247 
248  return calcInverseKinematics(end_p, end_R);
249 }
250 
251 
252 bool JointPath::calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R)
253 {
254  static const int MAX_IK_ITERATION = 50;
255  static const double LAMBDA = 0.9;
256  static const double JOINT_LIMIT_MARGIN = 0.05;
257 
258  if(joints.empty()){
259  if(linkPath.empty()){
260  return false;
261  }
262  if(baseLink() == endLink()){
263  baseLink()->p = end_p;
264  baseLink()->R = end_R;
265  return true;
266  } else {
267  // \todo implement here
268  return false;
269  }
270  }
271 
272  const int n = numJoints();
273 
274  Link* target = linkPath.endLink();
275 
276  std::vector<double> qorg(n);
277  for(int i=0; i < n; ++i){
278  qorg[i] = joints[i]->q;
279  }
280 
281  dmatrix J(6, n);
282  dvector dq(n);
283  dvector v(6);
284 
285  double errsqr = maxIKErrorSqr * 100.0;
286  bool converged = false;
287 
288  for(int i=0; i < MAX_IK_ITERATION; i++){
289 
290  calcJacobian(J);
291 
292  Vector3 dp(end_p - target->p);
293  Vector3 omega(target->R * omegaFromRot(target->R.transpose() * end_R));
294 
295  if(isBestEffortIKMode){
296  const double errsqr0 = errsqr;
297  errsqr = dp.dot(dp) + omega.dot(omega);
298  if(fabs(errsqr - errsqr0) < maxIKErrorSqr){
299  converged = true;
300  break;
301  }
302  } else {
303  const double errsqr = dp.dot(dp) + omega.dot(omega);
304  if(errsqr < maxIKErrorSqr){
305  converged = true;
306  break;
307  }
308  }
309 
310  v << dp, omega;
311 
312  if(n == 6){
313  solveLinearEquationLU(J, v, dq);
314  } else {
315  solveLinearEquationSVD(J, v, dq); // dq = pseudoInverse(J) * v
316  }
317 
318  for(int j=0; j < n; ++j){
319  joints[j]->q += LAMBDA * dq(j);
320  if (joints[j]->q > joints[j]->ulimit){
321  joints[j]->q = joints[j]->ulimit - JOINT_LIMIT_MARGIN;
322  }else if(joints[j]->q < joints[j]->llimit){
323  joints[j]->q = joints[j]->llimit + JOINT_LIMIT_MARGIN;
324  }
325  }
326 
328  }
329 
330  if(!converged){
331  for(int i=0; i < n; ++i){
332  joints[i]->q = qorg[i];
333  }
335  }
336 
337  return converged;
338 }
339 
340 
342 {
343  return false;
344 }
345 
346 
347 std::ostream& operator<<(std::ostream& os, JointPath& path)
348 {
349  int n = path.numJoints();
350  for(int i=0; i < n; ++i){
351  Link* link = path.joint(i);
352  os << link->name;
353  if(i != n){
354  os << (path.isJointDownward(i) ? " => " : " <= ");
355  }
356  }
357  os << std::endl;
358  return os;
359 }
hrp::omegaFromRot
HRP_UTIL_EXPORT Vector3 omegaFromRot(const Matrix33 &r)
Definition: Eigen3d.cpp:63
size
png_uint_32 size
Definition: png.h:1518
i
png_uint_32 i
Definition: png.h:2732
hrp::JointPath::joint
Link * joint(int index) const
Definition: JointPath.h:46
hrp::LinkTraverse::empty
bool empty() const
Definition: LinkTraverse.h:44
hrp::JointPath::initialize
void initialize()
Definition: JointPath.cpp:49
hrp::JointPath::JointPath
JointPath()
Definition: JointPath.cpp:25
hrp::dvector
Eigen::VectorXd dvector
Definition: EigenTypes.h:14
hrp::JointPath::isBestEffortIKMode
bool isBestEffortIKMode
Definition: JointPath.h:90
operator<<
std::ostream & operator<<(std::ostream &os, JointPath &path)
Definition: JointPath.cpp:347
hrp::LinkPath::find
bool find(Link *base, Link *end)
Definition: LinkPath.cpp:50
autoplay.n
n
Definition: autoplay.py:12
hrp::JointPath::numUpwardJointConnections
int numUpwardJointConnections
Definition: JointPath.h:99
hrp::LinkTraverse::size
size_t size() const
Definition: LinkTraverse.h:48
hrp
Definition: ColdetModel.h:28
hrp::JointPath::extractJoints
void extractJoints()
Definition: JointPath.cpp:83
hrp::JointPath::numJoints
unsigned int numJoints() const
Definition: JointPath.h:42
MatrixSolvers.h
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
hrp::JointPath::calcForwardKinematics
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false) const
Definition: JointPath.h:62
hrp::JointPath::baseLink
Link * baseLink() const
Definition: JointPath.h:50
hrp::JointPath::linkPath
LinkPath linkPath
Definition: JointPath.h:97
hrp::JointPath::calcInverseKinematics
virtual bool calcInverseKinematics(const Vector3 &end_p, const Matrix33 &end_R)
Definition: JointPath.cpp:252
JointPath.h
The header file of the LinkPath and JointPath classes.
hrp::hat
Matrix33 hat(const Vector3 &c)
Definition: Eigen3d.h:80
hrp::JointPath::onJointPathUpdated
virtual void onJointPathUpdated()
Definition: JointPath.cpp:122
hrp::dmatrix
Eigen::MatrixXd dmatrix
Definition: EigenTypes.h:13
hrp::JointPath::setMaxIKError
virtual void setMaxIKError(double e)
Definition: JointPath.cpp:225
hrp::JointPath::find
bool find(Link *base, Link *end)
Definition: JointPath.cpp:62
hrp::JointPath::isJointDownward
bool isJointDownward(int index) const
Definition: JointPath.h:58
hrp::LinkPath::endLink
Link * endLink() const
Definition: LinkPath.h:38
hrp::JointPath::maxIKErrorSqr
double maxIKErrorSqr
Definition: JointPath.h:89
hrp::JointPath::setBestEffortIKMode
virtual void setBestEffortIKMode(bool on)
Definition: JointPath.cpp:231
hrp::JointPath::calcJacobian
void calcJacobian(dmatrix &out_J, const Vector3 &local_p=Vector3::Zero()) const
Definition: JointPath.cpp:128
hrp::solveLinearEquationSVD
int solveLinearEquationSVD(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
Definition: MatrixSolvers.cpp:157
hrp::JointPath::~JointPath
virtual ~JointPath()
Definition: JointPath.cpp:56
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
hrp::JointPath::hasAnalyticalIK
virtual bool hasAnalyticalIK()
Definition: JointPath.cpp:341
hrp::JointPath::calcJacobianDot
void calcJacobianDot(dmatrix &out_dJ, const Vector3 &local_p=Vector3::Zero()) const
Definition: JointPath.cpp:173
hrp::JointPath::joints
std::vector< Link * > joints
Definition: JointPath.h:98
hrp::LinkTraverse::isDownward
bool isDownward(int index) const
Definition: LinkTraverse.h:77
hrp::JointPath::endLink
Link * endLink() const
Definition: JointPath.h:54
hrp::solveLinearEquationLU
int solveLinearEquationLU(dmatrix a, const dmatrix &b, dmatrix &out_x)
Definition: MatrixSolvers.cpp:58
hrp::JointPath
Definition: JointPath.h:26


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03