fk.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  * fk.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.19
12  */
13 
14 #include "chain.h"
15 
17 {
19 }
20 
22 {
23  if(parent)
24  {
25 #if 0
26  // readable, but slower version
27  abs_pos = parent->abs_pos + parent->abs_att * rel_pos;
28  abs_att = parent->abs_att * rel_att;
29 #else
30  // faster version
31  abs_pos.mul(parent->abs_att, rel_pos);
32  abs_pos += parent->abs_pos;
33  abs_att.mul(parent->abs_att, rel_att);
34 #endif
35 // cout << name << ": " << abs_pos << endl;
36  }
37  child->calc_position();
38  brother->calc_position();
39 }
40 
42 {
44 }
45 
47 {
48  if(parent)
49  {
50  static fMat33 t_rel_att;
51  static fVec3 v1, v2;
52  t_rel_att.tran(rel_att);
53  // compute loc_lin_vel
54  v1.cross(parent->loc_ang_vel, rel_pos);
55  v2.mul(t_rel_att, parent->loc_lin_vel);
56  loc_lin_vel.mul(t_rel_att, v1);
57  loc_lin_vel += v2;
58  loc_lin_vel += rel_lin_vel;
59  // compute loc_ang_vel
60  loc_ang_vel.mul(t_rel_att, parent->loc_ang_vel);
61  loc_ang_vel += rel_ang_vel;
62  // compute loc_com_vel
63  v1.cross(loc_ang_vel, loc_com);
64  loc_com_vel.add(loc_lin_vel, v1);
65  }
66  else
67  {
68  loc_lin_vel.zero();
69  loc_ang_vel.zero();
70  }
71  child->calc_velocity();
72  brother->calc_velocity();
73 }
74 
76 {
78 }
79 
81 {
82  if(parent)
83  {
84  static fMat33 t_rel_att;
85  static fVec3 v1, v2, v3, v4;
86  t_rel_att.tran(rel_att);
87  // Éð»Ê±ñ1¡¦x
88  v1.mul(t_rel_att, parent->loc_ang_vel);
89 // v1 += loc_ang_vel;
90  v1 *= 2.0;
91  v2.cross(v1, rel_lin_vel);
92  loc_lin_acc.add(rel_lin_acc, v2);
93  v2.cross(parent->loc_ang_acc, rel_pos);
94  v1.add(parent->loc_lin_acc, v2);
95  v2.cross(parent->loc_ang_vel, rel_pos);
96  v3.cross(parent->loc_ang_vel, v2);
97  v1 += v3;
98  v4.mul(t_rel_att, v1);
99  loc_lin_acc += v4;
100  // ³Ñ±ñ1¡¦x
101 // v1.cross(loc_ang_vel, parent->rel_ang_vel);
102  v1.cross(loc_ang_vel, rel_ang_vel);
103  loc_ang_acc.add(rel_ang_acc, v1);
104  v1.mul(t_rel_att, parent->loc_ang_acc);
105  loc_ang_acc += v1;
106  // ½Å¿´£öÅð»Ê±ñ1¡¦x
107  v1.cross(loc_ang_acc, loc_com);
108  loc_com_acc.add(loc_lin_acc, v1);
109  v1.cross(loc_ang_vel, loc_com);
110  v2.cross(loc_ang_vel, v1);
111  loc_com_acc += v2;
112 
113 // cerr << name << ": loc_lin_acc = " << loc_lin_acc << endl;
114 // cerr << name << ": loc_ang_acc = " << loc_ang_acc << endl;
115 // if(real)
116 // cerr << name << ": acc = " << abs_att*loc_lin_acc << abs_att*loc_ang_acc << endl;
117  }
118  else
119  {
120  // ¥ë¡¼¥È¥ê¥ó¥¯
121  // loc_lin_acc£õ"¡¦dÍ÷)¡¦¡¦x£åÂð€¡¦Ãà¡à¡¦Ëࡦ±êù硦¡¦Çà
122  loc_ang_acc.zero();
123  loc_com_acc.zero();
124  }
125  child->calc_acceleration();
126  brother->calc_acceleration();
127 }
128 
129 double Chain::TotalCOM(fVec3& com, const char* chname)
130 {
131  com.zero();
132  double m = root->total_com(com, chname);
133  com /= m;
134  return m;
135 }
136 
137 double Joint::total_com(fVec3& com, const char* chname)
138 {
139  int is_target = false;
140  if(!chname)
141  {
142  is_target = true;
143  }
144  else
145  {
146  char* my_chname = CharName();
147  if(my_chname && !strcmp(my_chname, chname))
148  {
149  is_target = true;
150  }
151  }
152  fVec3 b_com, c_com;
153  b_com.zero();
154  c_com.zero();
155  double ret = brother->total_com(b_com, chname) + child->total_com(c_com, chname);
156 
157  com.add(b_com, c_com);
158  if(is_target)
159  {
160  static fVec3 abs_com_pos, my_com;
161  ret += mass;
162  abs_com_pos.mul(abs_att, loc_com);
163  abs_com_pos += abs_pos;
164  my_com.mul(abs_com_pos, mass);
165  com += my_com;
166  }
167  return ret;
168 }
169 
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
3x3 matrix class.
Definition: fMatrix3.h:29
double TotalCOM(fVec3 &com, const char *chname=0)
Center of mass of the chain.
Definition: fk.cpp:129
char * CharName(const char *_name)
Extracts the character name from a joint name.
Definition: chain.cpp:17
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
void calc_acceleration()
Definition: fk.cpp:80
Joint * root
Chain information.
Definition: chain.h:466
void CalcVelocity()
Definition: fk.cpp:41
void CalcPosition()
Forward kinematics.
Definition: fk.cpp:16
void calc_position()
Definition: fk.cpp:21
double total_com(fVec3 &com, const char *chname)
Definition: fk.cpp:137
Classes for defining open/closed kinematic chains.
void CalcAcceleration()
Definition: fk.cpp:75
void calc_velocity()
Definition: fk.cpp:46
3-element vector class.
Definition: fMatrix3.h:206
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916


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:21