Link.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 
43 #include "Link.h"
44 #include <stack>
45 
46 using namespace std;
47 using namespace hrp;
48 
49 
50 Link::Link()
51 {
52  body = 0;
53  index = -1;
54  jointId = -1;
55  jointType = FIXED_JOINT;
56  parent = 0;
57  sibling = 0;
58  child = 0;
59 
60  isHighGainMode = false;
61  isCrawler = false;
62 
63  defaultJointValue = 0.0;
64 }
65 
66 
67 Link::Link(const Link& org)
68  : name(org.name)
69 {
70  body = 0;
71  index = -1; // should be set by a Body object
72  jointId = org.jointId;
73  jointType = org.jointType;
74 
75  p = org.p;
76  R = org.R;
77  v = org.v;
78  w = org.w;
79  dv = org.dv;
80  dw = org.dw;
81 
82  q = org.q;
83  dq = org.dq;
84  ddq = org.ddq;
85  u = org.u;
86 
87  a = org.a;
88  d = org.d;
89  b = org.b;
90  Rs = org.Rs;
91  m = org.m;
92  I = org.I;
93  c = org.c;
94 
95  fext = org.fext;
96  tauext = org.tauext;
97 
98  Jm2 = org.Jm2;
99 
100  ulimit = org.ulimit;
101  llimit = org.llimit;
102  uvlimit = org.uvlimit;
103  lvlimit = org.lvlimit;
104 
106  torqueConst = org.torqueConst;
108  Ir = org.Ir;
109  gearRatio = org.gearRatio;
112 
114  isCrawler = org.isCrawler;
115 
116  if(org.coldetModel){
117  coldetModel = new ColdetModel(*org.coldetModel);
118  }
119 
120  parent = child = sibling = 0;
121 
122  if(org.child){
123  stack<Link*> children;
124  for(Link* orgChild = org.child; orgChild; orgChild = orgChild->sibling){
125  children.push(orgChild);
126  }
127  while(!children.empty()){
128  addChild(new Link(*children.top()));
129  children.pop();
130  }
131  }
132 
133 }
134 
135 
137 {
138  Link* link = child;
139  while(link){
140  Link* linkToDelete = link;
141  link = link->sibling;
142  delete linkToDelete;
143  }
144 }
145 
146 
147 void Link::addChild(Link* link)
148 {
149  if(link->parent){
150  link->parent->detachChild(link);
151  }
152 
153  link->sibling = child;
154  link->parent = this;
155  child = link;
156 
157  link->setBodyIter(body);
158 }
159 
160 
166 bool Link::detachChild(Link* childToRemove)
167 {
168  bool removed = false;
169 
170  Link* link = child;
171  Link* prevSibling = 0;
172  while(link){
173  if(link == childToRemove){
174  removed = true;
175  if(prevSibling){
176  prevSibling->sibling = link->sibling;
177  } else {
178  child = link->sibling;
179  }
180  break;
181  }
182  prevSibling = link;
183  link = link->sibling;
184  }
185 
186  if(removed){
187  childToRemove->parent = 0;
188  childToRemove->sibling = 0;
189  childToRemove->setBodyIter(0);
190  }
191 
192  return removed;
193 }
194 
195 
197 {
198  this->body = body;
199  for(Link* link = child; link; link = link->sibling){
200  link->setBodyIter(body);
201  }
202 }
203 
204 
205 std::ostream& operator<<(std::ostream &out, Link& link)
206 {
207  link.putInformation(out);
208  return out;
209 }
210 
211 void Link::putInformation(std::ostream& out)
212 {
213  out << "Link " << name << " Link Index = " << index << ", Joint ID = " << jointId << "\n";
214 
215  out << "Joint Type: ";
216 
217  switch(jointType) {
218  case FREE_JOINT:
219  out << "Free Joint\n";
220  break;
221  case FIXED_JOINT:
222  out << "Fixed Joint\n";
223  break;
224  case ROTATIONAL_JOINT:
225  out << "Rotational Joint\n";
226  out << "Axis = " << a << "\n";
227  break;
228  case SLIDE_JOINT:
229  out << "Slide Joint\n";
230  out << "Axis = " << d << "\n";
231  break;
232  }
233 
234  out << "parent = " << (parent ? parent->name : "null") << "\n";
235 
236  out << "child = ";
237  if(child){
238  Link* link = child;
239  while(true){
240  out << link->name;
241  link = link->sibling;
242  if(!link){
243  break;
244  }
245  out << ", ";
246  }
247  } else {
248  out << "null";
249  }
250  out << "\n";
251 
252  out << "b = " << b << "\n";
253  out << "Rs = " << Rs << "\n";
254  out << "c = " << c << "\n";
255  out << "m = " << m << "\n";
256  out << "Ir = " << Ir << "\n";
257  out << "I = " << I << "\n";
258  out << "torqueConst = " << torqueConst << "\n";
259  out << "encoderPulse = " << encoderPulse << "\n";
260  out << "gearRatio = " << gearRatio << "\n";
261  out << "gearEfficiency = " << gearEfficiency << "\n";
262  out << "Jm2 = " << Jm2 << "\n";
263  out << "ulimit = " << ulimit << "\n";
264  out << "llimit = " << llimit << "\n";
265  out << "uvlimit = " << uvlimit << "\n";
266  out << "lvlimit = " << lvlimit << "\n";
267 
268  if(false){
269  out << "R = " << R << "\n";
270  out << "p = " << p << ", wc = " << wc << "\n";
271  out << "v = " << v << ", vo = " << vo << ", dvo = " << dvo << "\n";
272  out << "w = " << w << ", dw = " << dw << "\n";
273 
274  out << "u = " << u << ", q = " << q << ", dq = " << dq << ", ddq = " << ddq << "\n";
275 
276  out << "fext = " << fext << ", tauext = " << tauext << "\n";
277 
278  out << "sw = " << sw << ", sv = " << sv << "\n";
279  out << "Ivv = " << Ivv << "\n";
280  out << "Iwv = " << Iwv << "\n";
281  out << "Iww = " << Iww << "\n";
282  out << "cv = " << cv << ", cw = " << cw << "\n";
283  out << "pf = " << pf << ", ptau = " << ptau << "\n";
284  out << "hhv = " << hhv << ", hhw = " << hhw << "\n";
285  out << "uu = " << uu << ", dd = " << dd << "\n";
286 
287  out << std::endl;
288  }
289 }
290 
292 {
293  subm = m;
294  submwc = m*wc;
295  if (child){
296  child->calcSubMassCM();
297  subm += child->subm;
298  submwc += child->submwc;
299  Link *l = child->sibling;
300  while (l){
301  l->calcSubMassCM();
302  subm += l->subm;
303  submwc += l->submwc;
304  l = l->sibling;
305  }
306  }
307  /*
308  std::cout << "calcSubMassCM() : " << name << ", subm = " << subm
309  << ", subCM = " << vector3(submwc/subm) << std::endl;
310  */
311 }
312 
314 {
315  subIw = R*I*R.transpose();
316  if (subm!=0.0) subIw += m*hat(wc - submwc/subm).transpose()*hat(wc - submwc/subm);
317  if (child){
318  Matrix33 childsubIw;
319  child->calcSubMassInertia(childsubIw);
320  subIw += childsubIw;
321  if (child->subm!=0.0) subIw += child->subm*hat(child->submwc/child->subm - submwc/subm).transpose()*hat(child->submwc/child->subm - submwc/subm);
322  Link *l = child->sibling;
323  while (l){
324  Matrix33 lsubIw;
325  l->calcSubMassInertia(lsubIw);
326  subIw += lsubIw;
327  if (l->subm!=0.0) subIw += l->subm*hat(l->submwc/l->subm - submwc/subm).transpose()*hat(l->submwc/l->subm - submwc/subm);
328  l = l->sibling;
329  }
330  }
331 }
png_infop png_charpp name
Definition: png.h:2382
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
list index
Matrix33 hat(const Vector3 &c)
Definition: Eigen3d.h:80
Definition: Body.h:44


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