ROBOOP, A Robotics Object Oriented Package in C++
comp_dq.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau, Professeur
23 Departement de genie electrique
24 Ecole Polytechnique de Montreal
25 C.P. 6079, Succ. Centre-Ville
26 Montreal, Quebec, H3C 3A7
27 
28 email: richard.gourdeau@polymtl.ca
29 
30 -------------------------------------------------------------------------------
31 Revision_history:
32 
33 2003/14/05: Etienne Lachance
34  -Fix Robot::dq_torque.
35  -Added mRobot/mRobot_min_para::dq_torque.
36 
37 2004/07/01: Etienne Lachance
38  -Replace vec_x_prod by CrossProduct.
39 
40 2004/07/01: Ethan Tira-Thompson
41  -Added support for newmat's use_namespace #define, using ROBOOP namespace
42 
43 2004/07/02: Etienne Lachance
44  -Added Doxygen comments.
45 -------------------------------------------------------------------------------
46 */
47 
53 #include "robot.h"
54 
55 #ifdef use_namespace
56 namespace ROBOOP {
57  using namespace NEWMAT;
58 #endif
59 
60 
61 void Robot::dq_torque(const ColumnVector & q, const ColumnVector & qp,
62  const ColumnVector & qpp, const ColumnVector & dq,
63  ColumnVector & ltorque, ColumnVector & dtorque)
70 {
71  int i;
72  Matrix Rt, temp;
73  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
74  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
75  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
76  if(dq.Ncols() != 1 || qpp.Nrows() != dof) error("dq has wrong dimension");
77  ltorque = ColumnVector(dof);
78  dtorque = ColumnVector(dof);
79  set_q(q);
80 
81  vp[0] = gravity;
82  ColumnVector z0(3);
83  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
84  Matrix Q(3,3);
85  Q = 0.0;
86  Q(1,2) = -1.0;
87  Q(2,1) = 1.0;
88 
89  for(i = 1; i <= dof; i++)
90  {
91  Rt = links[i].R.t();
92  p[i] = ColumnVector(3);
93  p[i](1) = links[i].get_a();
94  p[i](2) = links[i].get_d() * Rt(2,3);
95  p[i](3) = links[i].get_d() * Rt(3,3);
96  if(links[i].get_joint_type() != 0)
97  {
98  dp[i] = ColumnVector(3);
99  dp[i](1) = 0.0;
100  dp[i](2) = Rt(2,3);
101  dp[i](3) = Rt(3,3);
102  }
103  if(links[i].get_joint_type() == 0)
104  {
105  w[i] = Rt*(w[i-1] + z0*qp(i));
106  dw[i] = Rt*(dw[i-1] - Q*w[i-1]*dq(i));
107  wp[i] = Rt*(wp[i-1] + z0*qpp(i)
108  + CrossProduct(w[i-1],z0*qp(i)));
109  dwp[i] = Rt*(dwp[i-1]
110  + CrossProduct(dw[i-1],z0*qp(i))
111  - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
112  *dq(i));
113  vp[i] = CrossProduct(wp[i],p[i])
114  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
115  + Rt*(vp[i-1]);
116  dvp[i] = CrossProduct(dwp[i],p[i])
117  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
118  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
119  + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
120  }
121  else
122  {
123  w[i] = Rt*w[i-1];
124  dw[i] = Rt*dw[i-1];
125  wp[i] = Rt*wp[i-1];
126  dwp[i] = Rt*dwp[i-1];
127  vp[i] = Rt*(vp[i-1] + z0*qpp(i)
128  + 2.0*CrossProduct(w[i],Rt*z0*qp(i)))
129  + CrossProduct(wp[i],p[i])
130  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
131  dvp[i] = Rt*(dvp[i-1]
132  + 2.0*CrossProduct(dw[i-1],z0*qp(i)))
133  + CrossProduct(dwp[i],p[i])
134  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
135  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
136  + (CrossProduct(wp[i],dp[i])
137  + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
138  *dq(i);
139  }
140  a[i] = CrossProduct(wp[i],links[i].r)
141  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
142  + vp[i];
143  da[i] = CrossProduct(dwp[i],links[i].r)
144  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
145  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
146  + dvp[i];
147  }
148 
149  for(i = dof; i >= 1; i--)
150  {
151  F[i] = a[i] * links[i].m;
152  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
153  dF[i] = da[i] * links[i].m;
154  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
155  + CrossProduct(w[i],links[i].I*dw[i]);
156  if(i == dof)
157  {
158  f[i] = F[i];
159  df[i] = dF[i];
160  n[i] = CrossProduct(p[i],f[i])
161  + CrossProduct(links[i].r,F[i]) + N[i];
162  dn[i] = CrossProduct(p[i],df[i])
163  + CrossProduct(links[i].r,dF[i]) + dN[i];
164  if(links[i].get_joint_type() != 0)
165  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
166  }
167  else
168  {
169  f[i] = links[i+1].R*f[i+1] + F[i];
170  df[i] = links[i+1].R*df[i+1] + dF[i];
171  if(links[i+1].get_joint_type() == 0)
172  df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
173  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
174  + CrossProduct(links[i].r,F[i]) + N[i];
175 
176  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
177  + CrossProduct(links[i].r,dF[i]) + dN[i];
178  if(links[i+1].get_joint_type() == 0)
179  dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
180  else
181  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
182  }
183  if(links[i].get_joint_type() == 0)
184  {
185  temp = ((z0.t()*links[i].R)*n[i]);
186  ltorque(i) = temp(1,1);
187  temp = ((z0.t()*links[i].R)*dn[i]);
188  dtorque(i) = temp(1,1);
189  }
190  else
191  {
192  temp = ((z0.t()*links[i].R)*f[i]);
193  ltorque(i) = temp(1,1);
194  temp = ((z0.t()*links[i].R)*df[i]);
195  dtorque(i) = temp(1,1);
196  }
197  }
198 }
199 
200 
201 void mRobot::dq_torque(const ColumnVector & q, const ColumnVector & qp,
202  const ColumnVector & qpp, const ColumnVector & dq,
203  ColumnVector & ltorque, ColumnVector & dtorque)
210 {
211  int i;
212  Matrix Rt, temp;
213  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
214  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
215  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
216  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
217  ltorque = ColumnVector(dof);
218  dtorque = ColumnVector(dof);
219  set_q(q);
220 
221  vp[0] = gravity;
222  ColumnVector z0(3);
223  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
224  Matrix Q(3,3);
225  Q = 0.0;
226  Q(1,2) = -1.0;
227  Q(2,1) = 1.0;
228  for(i = 1; i <= dof; i++)
229  {
230  Rt = links[i].R.t();
231  p[i] = links[i].p;
232  if(links[i].get_joint_type() != 0)
233  {
234  dp[i] = ColumnVector(3);
235  dp[i](1) = 0.0;
236  dp[i](2) = Rt(2,3);
237  dp[i](3) = Rt(3,3);
238  }
239  if(links[i].get_joint_type() == 0)
240  {
241  w[i] = Rt*w[i-1] + z0*qp(i);
242  dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i);
243  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
244  + z0*qpp(i);
245  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
246  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i);
247  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
248  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
249  + vp[i-1]);
250  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
251  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
252  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
253  - Q*Rt*(CrossProduct(wp[i-1],p[i])
254  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
255  }
256  else
257  {
258  w[i] = Rt*w[i-1];
259  dw[i] = Rt*dw[i-1];
260  wp[i] = Rt*wp[i-1];
261  dwp[i] = Rt*dwp[i-1];
262  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
263  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
264  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
265  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
266  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
267  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
268  + (CrossProduct(wp[i-1],dp[i])
269  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
270  + 2*CrossProduct(dw[i],z0*qp(i));
271  }
272  a[i] = CrossProduct(wp[i],links[i].r)
273  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
274  + vp[i];
275  da[i] = CrossProduct(dwp[i],links[i].r)
276  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
277  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
278  + dvp[i];
279  }
280 
281  for(i = dof; i >= 1; i--) {
282  F[i] = a[i] * links[i].m;
283  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
284  dF[i] = da[i] * links[i].m;
285  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
286  + CrossProduct(w[i],links[i].I*dw[i]);
287 
288  if(i == dof)
289  {
290  f[i] = F[i];
291  df[i] = dF[i];
292  n[i] = CrossProduct(links[i].r,F[i]) + N[i];
293  dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
294  }
295  else
296  {
297  f[i] = links[i+1].R*f[i+1] + F[i];
298  df[i] = links[i+1].R*df[i+1] + dF[i];
299  if(links[i+1].get_joint_type() == 0)
300  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
301 
302  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
303  + CrossProduct(links[i].r,F[i]) + N[i];
304  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
305  + CrossProduct(links[i].r,dF[i]) + dN[i];
306  if(links[i+1].get_joint_type() == 0)
307  dn[i] += (links[i+1].R*Q*n[i+1] +
308  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
309  else
310  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
311  }
312 
313  if(links[i].get_joint_type() == 0)
314  {
315  temp = z0.t()*n[i];
316  ltorque(i) = temp(1,1);
317  temp = z0.t()*dn[i];
318  dtorque(i) = temp(1,1);
319  }
320  else
321  {
322  temp = z0.t()*f[i];
323  ltorque(i) = temp(1,1);
324  temp = z0.t()*df[i];
325  dtorque(i) = temp(1,1);
326  }
327  }
328 }
329 
330 
331 void mRobot_min_para::dq_torque(const ColumnVector & q, const ColumnVector & qp,
332  const ColumnVector & qpp, const ColumnVector & dq,
333  ColumnVector & ltorque, ColumnVector & dtorque)
340 {
341  int i;
342  Matrix Rt, temp;
343  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
344  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
345  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
346  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
347  ltorque = ColumnVector(dof);
348  dtorque = ColumnVector(dof);
349  set_q(q);
350 
351  vp[0] = gravity;
352  ColumnVector z0(3);
353  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
354  Matrix Q(3,3);
355  Q = 0.0;
356  Q(1,2) = -1.0;
357  Q(2,1) = 1.0;
358  for(i = 1; i <= dof; i++)
359  {
360  Rt = links[i].R.t();
361  p[i] = links[i].p;
362  if(links[i].get_joint_type() != 0)
363  {
364  dp[i] = ColumnVector(3);
365  dp[i](1) = 0.0;
366  dp[i](2) = Rt(2,3);
367  dp[i](3) = Rt(3,3);
368  }
369  if(links[i].get_joint_type() == 0)
370  {
371  w[i] = Rt*w[i-1] + z0*qp(i);
372  dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i);
373  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
374  + z0*qpp(i);
375  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
376  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i);
377  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
378  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
379  + vp[i-1]);
380  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
381  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
382  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
383  - Q*Rt*(CrossProduct(wp[i-1],p[i])
384  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
385  }
386  else
387  {
388  w[i] = Rt*w[i-1];
389  dw[i] = Rt*dw[i-1];
390  wp[i] = Rt*wp[i-1];
391  dwp[i] = Rt*dwp[i-1];
392  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
393  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
394  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
395  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
396  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
397  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
398  + (CrossProduct(wp[i-1],dp[i])
399  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i));
400  }
401  }
402 
403  for(i = dof; i >= 1; i--) {
404  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
405  CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
406  dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
407  + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
408  + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
409  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
410  - CrossProduct(vp[i], links[i].mc);
411  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
412  + CrossProduct(w[i],links[i].I*dw[i])
413  - CrossProduct(dvp[i],links[i].mc);
414 
415  if(i == dof)
416  {
417  f[i] = F[i];
418  df[i] = dF[i];
419  n[i] = N[i];
420  dn[i] = dN[i];
421  }
422  else
423  {
424  f[i] = links[i+1].R*f[i+1] + F[i];
425  df[i] = links[i+1].R*df[i+1] + dF[i];
426  if(links[i+1].get_joint_type() == 0)
427  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
428 
429  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
430  + N[i];
431  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
432  + dN[i];
433  if(links[i+1].get_joint_type() == 0)
434  dn[i] += (links[i+1].R*Q*n[i+1] +
435  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
436  else
437  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
438  }
439 
440  if(links[i].get_joint_type() == 0)
441  {
442  temp = z0.t()*n[i];
443  ltorque(i) = temp(1,1);
444  temp = z0.t()*dn[i];
445  dtorque(i) = temp(1,1);
446  }
447  else
448  {
449  temp = z0.t()*f[i];
450  ltorque(i) = temp(1,1);
451  temp = z0.t()*df[i];
452  dtorque(i) = temp(1,1);
453  }
454  }
455 }
456 
457 #ifdef use_namespace
458 }
459 #endif