LinkTraverse.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  * General Robotix Inc.
9  */
10 
18 #include "LinkTraverse.h"
19 #include "Link.h"
20 #include <hrpUtil/Eigen3d.h>
21 
22 using namespace std;
23 using namespace hrp;
24 
25 
26 LinkTraverse::LinkTraverse()
27 {
28 
29 }
30 
31 
32 LinkTraverse::LinkTraverse(int size)
33  : links(size)
34 {
35  links.clear();
36 }
37 
38 
39 LinkTraverse::LinkTraverse(Link* root, bool doUpward, bool doDownward)
40 {
41  find(root, doUpward, doDownward);
42 }
43 
44 
46 {
47 
48 }
49 
50 
51 void LinkTraverse::find(Link* root, bool doUpward, bool doDownward)
52 {
54  links.clear();
55  traverse(root, doUpward, doDownward, false, 0);
56 }
57 
58 
59 void LinkTraverse::traverse(Link* link, bool doUpward, bool doDownward, bool isUpward, Link* prev)
60 {
61  links.push_back(link);
62  if(isUpward){
64  }
65 
66  if(doUpward && link->parent){
67  traverse(link->parent, doUpward, true, true, link);
68  }
69  if(doDownward){
70  for(Link* child = link->child; child; child = child->sibling){
71  if(child != prev){
72  traverse(child, false, true, false, 0);
73  }
74  }
75  }
76 }
77 
78 
79 void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const
80 {
81  Vector3 arm;
82  int i;
83  for(i=1; i <= numUpwardConnections; ++i){
84 
85  Link* link = links[i];
86  Link* child = links[i-1];
87 
88  switch(child->jointType){
89 
91  link->R.noalias() = child->R * rodrigues(child->a, child->q).transpose();
92  arm.noalias() = link->R * child->b;
93  link->p = child->p - arm;
94 
95  if(calcVelocity){
96  child->sw.noalias() = link->R * child->a;
97  link->w = child->w - child->dq * child->sw;
98  link->v = child->v - link->w.cross(arm);
99 
100  if(calcAcceleration){
101  link->dw = child->dw - child->dq * child->w.cross(child->sw) - (child->ddq * child->sw);
102  link->dv = child->dv - child->w.cross(child->w.cross(arm)) - child->dw.cross(arm);
103  }
104  }
105  break;
106 
107  case Link::SLIDE_JOINT:
108  link->R = child->R;
109  arm.noalias() = link->R * (child->b + child->q * child->d);
110  link->p = child->p - arm;
111 
112  if(calcVelocity){
113  child->sv.noalias() = link->R * child->d;
114  link->w = child->w;
115  link->v = child->v - child->dq * child->sv;
116 
117  if(calcAcceleration){
118  link->dw = child->dw;
119  link->dv = child->dv - child->w.cross(child->w.cross(arm)) - child->dw.cross(arm)
120  - 2.0 * child->dq * child->w.cross(child->sv) - child->ddq * child->sv;
121  }
122  }
123  break;
124 
125  case Link::FIXED_JOINT:
126  default:
127  link->R = child->R;
128  link->p = child->p - (link->R * child->b);
129 
130  if(calcVelocity){
131  link->w = child->w;
132  link->v = child->v;
133 
134  if(calcAcceleration){
135  link->dw = child->dw;
136  link->dv = child->dv;
137  }
138  }
139  break;
140  }
141  }
142 
143  int n = links.size();
144  for( ; i < n; ++i){
145 
146  Link* link = links[i];
147  Link* parent = link->parent;
148 
149  switch(link->jointType){
150 
152  link->R.noalias() = parent->R * rodrigues(link->a, link->q);
153  arm.noalias() = parent->R * link->b;
154  link->p = parent->p + arm;
155 
156  if(calcVelocity){
157  link->sw.noalias() = parent->R * link->a;
158  link->w = parent->w + link->sw * link->dq;
159  link->v = parent->v + parent->w.cross(arm);
160 
161  if(calcAcceleration){
162  link->dw = parent->dw + link->dq * parent->w.cross(link->sw) + (link->ddq * link->sw);
163  link->dv = parent->dv + parent->w.cross(parent->w.cross(arm)) + parent->dw.cross(arm);
164  }
165  }
166  break;
167 
168  case Link::SLIDE_JOINT:
169  link->R = parent->R;
170  arm.noalias() = parent->R * (link->b + link->q * link->d);
171  link->p = parent->p + arm;
172 
173  if(calcVelocity){
174  link->sv.noalias() = parent->R * link->d;
175  link->w = parent->w;
176  link->v = parent->v + link->sv * link->dq;
177 
178  if(calcAcceleration){
179  link->dw = parent->dw;
180  link->dv = parent->dv + parent->w.cross(parent->w.cross(arm)) + parent->dw.cross(arm)
181  + 2.0 * link->dq * parent->w.cross(link->sv) + link->ddq * link->sv;
182  }
183  }
184  break;
185 
186  case Link::FIXED_JOINT:
187  default:
188  link->R = parent->R;
189  link->p = parent->R * link->b + parent->p;
190 
191  if(calcVelocity){
192  link->w = parent->w;
193  link->v = parent->v;
194 
195  if(calcAcceleration){
196  link->dw = parent->dw;
197  link->dv = parent->dv;
198  }
199  }
200  break;
201  }
202  }
203 }
204 
205 
207 {
208  totalMass_ = 0.0;
209 
210  for(int i = 0; i < numLinks(); ++i) {
211  totalMass_ += links[i]->m;
212  }
213 
214  return totalMass_;
215 }
216 
217 
218 std::ostream& operator<<(std::ostream& os, LinkTraverse& traverse)
219 {
220  int n = traverse.numLinks();
221  for(int i=0; i < n; ++i){
222  Link* link = traverse[i];
223  os << link->name;
224  if(i != n){
225  os << (traverse.isDownward(i) ? " => " : " <= ");
226  }
227  }
228  os << std::endl;
229 
230  return os;
231 }
The header file of the LinkTraverse class.
void traverse(Link *link, bool doUpward, bool doDownward, bool isUpward, Link *prev)
png_uint_32 size
Definition: png.h:1521
Link * link(int index) const
Definition: LinkTraverse.h:56
png_uint_32 i
Definition: png.h:2735
std::ostream & operator<<(std::ostream &os, LinkTraverse &traverse)
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
void calcForwardKinematics(bool calcVelocity=false, bool calcAcceleration=false) const
virtual void find(Link *root, bool doUpward=false, bool doDownward=true)
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
virtual ~LinkTraverse()
bool isDownward(int index) const
Definition: LinkTraverse.h:77
std::vector< Link * > links
Definition: LinkTraverse.h:91
unsigned int numLinks() const
Definition: LinkTraverse.h:40


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04