handle.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  * The University of Tokyo
8  */
9 /*
10  * handle.cpp
11  * Create: Katsu Yamane, 03.07.10
12  */
13 
14 #include "ik.h"
15 
17 {
18  int i, j, count = 0;
19  int n_dof = ik->NumDOF();
20  static fMat Jtemp;
21  J.resize(n_const, n_dof);
22  Jtemp.resize(6, n_dof);
23  Jtemp.zero();
24  joint->CalcJacobian(Jtemp);
25  // compute other_joint's Jacobian and transform to other_joint's
26  // local frame
27  if(other_joint)
28  {
29  static fMat Jtemp_other;
30  Jtemp_other.resize(6, ik->NumDOF());
31  Jtemp_other.zero();
32  other_joint->CalcJacobian(Jtemp_other);
34  for(i=0; i<n_dof; i++)
35  {
36  double* a = Jtemp.data() + 6*i;
37  double* b = Jtemp_other.data() + 6*i;
38  double m0 = *a - *b;
39  double m1 = *(a+1) - *(b+1);
40  double m2 = *(a+2) - *(b+2);
41  double m3 = *(a+3) - *(b+3);
42  double m4 = *(a+4) - *(b+4);
43  double m5 = *(a+5) - *(b+5);
44  *a = abs_att(0,0)*m0 + abs_att(1,0)*m1 + abs_att(2,0)*m2;
45  *(a+1) = abs_att(0,1)*m0 + abs_att(1,1)*m1 + abs_att(2,1)*m2;
46  *(a+2) = abs_att(0,2)*m0 + abs_att(1,2)*m1 + abs_att(2,2)*m2;
47  *(a+3) = abs_att(0,0)*m3 + abs_att(1,0)*m4 + abs_att(2,0)*m5;
48  *(a+4) = abs_att(0,1)*m3 + abs_att(1,1)*m4 + abs_att(2,1)*m5;
49  *(a+5) = abs_att(0,2)*m3 + abs_att(1,2)*m4 + abs_att(2,2)*m5;
50  }
51  }
52  // copy to J
53  for(i=0; i<6; i++)
54  {
56  {
57  int a_row = J.row();
58  double* a = J.data() + count;
59  int b_row = Jtemp.row();
60  double* b = Jtemp.data() + i;
61  for(j=0; j<n_dof; a+=a_row, b+=b_row, j++)
62  {
63 // J(count, j) = Jtemp(i, j);
64  *a = *b;
65  }
66  count++;
67  }
68  }
69  return 0;
70 }
71 
73 {
74  // compute feedback velocity
75  if(n_const > 0)
76  {
77  static fVec3 fb_pos, fb_att;
78  static fVec3 cur_pos;
79  static fMat33 cur_att;
80  // absolute position / orientation
81  cur_att.mul(joint->abs_att, rel_att);
82  cur_pos.mul(joint->abs_att, rel_pos);
83  cur_pos += joint->abs_pos;
84  // relative position / orientation wrt other_joint
85  if(other_joint)
86  {
87  static fVec3 pp;
88  static fMat33 rt, catt;
90  catt.set(cur_att);
91  pp.sub(cur_pos, other_joint->abs_pos);
92  cur_pos.mul(rt, pp);
93  cur_att.mul(rt, catt);
94  }
95  fb_pos.sub(abs_pos, cur_pos);
96  fb_pos *= gain;
97  fb_att.rotation(abs_att, cur_att);
98  fb_att *= gain;
99  int i, count = 0;
100  for(i=0; i<3; i++)
101  {
103  {
104  fb(count) = fb_pos(i);
105  count++;
106  }
107  }
108  for(i=0; i<3; i++)
109  {
110  if(const_index[i+3] == IK::HAVE_CONSTRAINT)
111  {
112  fb(count) = fb_att(i);
113  count++;
114  }
115  }
116  }
117  return 0;
118 }
void rotation(const fMat33 &ref, const fMat33 &tgt)
Computes the rotation from tgt to ref.
Definition: fMatrix3.cpp:954
fVec3 rel_pos
Definition: ik.h:544
3x3 matrix class.
Definition: fMatrix3.h:29
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
int CalcJacobian(fMat &J)
The Jacobian matrix of position/orientation w.r.t. the joint values.
Definition: jacobi.cpp:119
fMat33 abs_att
current constraint orientation
Definition: ik.h:541
int calc_feedback()
compute the feedback velocity
Definition: handle.cpp:72
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
int row() const
Returns the number of rows.
Definition: fMatrix.h:158
Inverse kinematics (UTPoser) class.
double gain
priority
Definition: ik.h:396
png_uint_32 i
Definition: png.h:2735
long b
Definition: jpegint.h:371
with constraint
Definition: ik.h:67
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
Joint * joint
target joint
Definition: ik.h:391
def j(str, encoding="cp932")
int NumDOF()
Total degrees of freedom.
Definition: chain.h:343
fMat J
Definition: ik.h:402
fVec fb
Jacobian matrix (n_const x total DOF)
Definition: ik.h:403
fMat33 rel_att
Definition: ik.h:545
fMat33 abs_att
absolute orientation
Definition: chain.h:742
string a
fVec3 abs_pos
current constraint position
Definition: ik.h:540
double * data() const
Returns the pointer to the first element.
Definition: fMatrix.h:193
int calc_jacobian()
Computes the constraint Jacobian.
Definition: handle.cpp:16
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
IK * ik
Definition: ik.h:390
3-element vector class.
Definition: fMatrix3.h:206
fVec3 abs_pos
absolute position
Definition: chain.h:741
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
IK::ConstIndex const_index[6]
with/without constraint for each DOF
Definition: ik.h:543
int n_const
weight
Definition: ik.h:398
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
Joint * other_joint
connected to other joint: constrains the relative position and/or orientation w.r.t. other_joint&#39;s frame
Definition: ik.h:548


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:22