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
23 Professeur Agrege
24 Departement de genie electrique
25 Ecole Polytechnique de Montreal
26 C.P. 6079, Succ. Centre-Ville
27 Montreal, Quebec, H3C 3A7
28 
29 email: richard.gourdeau@polymtl.ca
30 
31 -------------------------------------------------------------------------------
32 Revision_history:
33 
34 2003/14/05: Etienne Lachance
35  -Fix Robot::dq_torque.
36  -Added mRobot/mRobot_min_para::dq_torque.
37 
38 2004/07/01: Etienne Lachance
39  -Replace vec_x_prod by CrossProduct.
40 
41 2004/07/01: Ethan Tira-Thompson
42  -Added support for newmat's use_namespace #define, using ROBOOP namespace
43 
44 2004/07/02: Etienne Lachance
45  -Added Doxygen comments.
46 -------------------------------------------------------------------------------
47 */
48 
54 static const char rcsid[] = "$Id: comp_dq.cpp,v 1.17 2004/07/06 02:16:36 gourdeau Exp $";
56 
57 #include "robot.h"
58 
59 #ifdef use_namespace
60 namespace ROBOOP {
61  using namespace NEWMAT;
62 #endif
63 
64 
65 void Robot::dq_torque(const ColumnVector & q, const ColumnVector & qp,
66  const ColumnVector & qpp, const ColumnVector & dq,
67  ColumnVector & ltorque, ColumnVector & dtorque)
74 {
75  int i;
76  Matrix Rt, temp;
77  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
78  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
79  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
80  if(dq.Ncols() != 1 || qpp.Nrows() != dof) error("dq has wrong dimension");
81  ltorque = ColumnVector(dof);
82  dtorque = ColumnVector(dof);
83  set_q(q);
84 
85  vp[0] = gravity;
86  ColumnVector z0(3);
87  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
88  Matrix Q(3,3);
89  Q = 0.0;
90  Q(1,2) = -1.0;
91  Q(2,1) = 1.0;
92 
93  for(i = 1; i <= dof; i++)
94  {
95  Rt = links[i].R.t();
96  p[i] = ColumnVector(3);
97  p[i](1) = links[i].get_a();
98  p[i](2) = links[i].get_d() * Rt(2,3);
99  p[i](3) = links[i].get_d() * Rt(3,3);
100  if(links[i].get_joint_type() != 0)
101  {
102  dp[i] = ColumnVector(3);
103  dp[i](1) = 0.0;
104  dp[i](2) = Rt(2,3);
105  dp[i](3) = Rt(3,3);
106  }
107  if(links[i].get_joint_type() == 0)
108  {
109  w[i] = Rt*(w[i-1] + z0*qp(i));
110  dw[i] = Rt*(dw[i-1] - Q*w[i-1]*dq(i));
111  wp[i] = Rt*(wp[i-1] + z0*qpp(i)
112  + CrossProduct(w[i-1],z0*qp(i)));
113  dwp[i] = Rt*(dwp[i-1]
114  + CrossProduct(dw[i-1],z0*qp(i))
115  - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
116  *dq(i));
117  vp[i] = CrossProduct(wp[i],p[i])
118  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
119  + Rt*(vp[i-1]);
120  dvp[i] = CrossProduct(dwp[i],p[i])
121  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
122  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
123  + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
124  }
125  else
126  {
127  w[i] = Rt*w[i-1];
128  dw[i] = Rt*dw[i-1];
129  wp[i] = Rt*wp[i-1];
130  dwp[i] = Rt*dwp[i-1];
131  vp[i] = Rt*(vp[i-1] + z0*qpp(i)
132  + 2.0*CrossProduct(w[i],Rt*z0*qp(i)))
133  + CrossProduct(wp[i],p[i])
134  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
135  dvp[i] = Rt*(dvp[i-1]
136  + 2.0*CrossProduct(dw[i-1],z0*qp(i)))
137  + CrossProduct(dwp[i],p[i])
138  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
139  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
140  + (CrossProduct(wp[i],dp[i])
141  + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
142  *dq(i);
143  }
144  a[i] = CrossProduct(wp[i],links[i].r)
145  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
146  + vp[i];
147  da[i] = CrossProduct(dwp[i],links[i].r)
148  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
149  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
150  + dvp[i];
151  }
152 
153  for(i = dof; i >= 1; i--)
154  {
155  F[i] = a[i] * links[i].m;
156  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
157  dF[i] = da[i] * links[i].m;
158  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
159  + CrossProduct(w[i],links[i].I*dw[i]);
160  if(i == dof)
161  {
162  f[i] = F[i];
163  df[i] = dF[i];
164  n[i] = CrossProduct(p[i],f[i])
165  + CrossProduct(links[i].r,F[i]) + N[i];
166  dn[i] = CrossProduct(p[i],df[i])
167  + CrossProduct(links[i].r,dF[i]) + dN[i];
168  if(links[i].get_joint_type() != 0)
169  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
170  }
171  else
172  {
173  f[i] = links[i+1].R*f[i+1] + F[i];
174  df[i] = links[i+1].R*df[i+1] + dF[i];
175  if(links[i+1].get_joint_type() == 0)
176  df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
177  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
178  + CrossProduct(links[i].r,F[i]) + N[i];
179 
180  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
181  + CrossProduct(links[i].r,dF[i]) + dN[i];
182  if(links[i+1].get_joint_type() == 0)
183  dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
184  else
185  dn[i] += CrossProduct(dp[i],f[i])*dq(i);
186  }
187  if(links[i].get_joint_type() == 0)
188  {
189  temp = ((z0.t()*links[i].R)*n[i]);
190  ltorque(i) = temp(1,1);
191  temp = ((z0.t()*links[i].R)*dn[i]);
192  dtorque(i) = temp(1,1);
193  }
194  else
195  {
196  temp = ((z0.t()*links[i].R)*f[i]);
197  ltorque(i) = temp(1,1);
198  temp = ((z0.t()*links[i].R)*df[i]);
199  dtorque(i) = temp(1,1);
200  }
201  }
202 }
203 
204 
205 void mRobot::dq_torque(const ColumnVector & q, const ColumnVector & qp,
206  const ColumnVector & qpp, const ColumnVector & dq,
207  ColumnVector & ltorque, ColumnVector & dtorque)
214 {
215  int i;
216  Matrix Rt, temp;
217  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
218  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
219  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
220  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
221  ltorque = ColumnVector(dof);
222  dtorque = ColumnVector(dof);
223  set_q(q);
224 
225  vp[0] = gravity;
226  ColumnVector z0(3);
227  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
228  Matrix Q(3,3);
229  Q = 0.0;
230  Q(1,2) = -1.0;
231  Q(2,1) = 1.0;
232  for(i = 1; i <= dof; i++)
233  {
234  Rt = links[i].R.t();
235  p[i] = links[i].p;
236  if(links[i].get_joint_type() != 0)
237  {
238  dp[i] = ColumnVector(3);
239  dp[i](1) = 0.0;
240  dp[i](2) = Rt(2,3);
241  dp[i](3) = Rt(3,3);
242  }
243  if(links[i].get_joint_type() == 0)
244  {
245  w[i] = Rt*w[i-1] + z0*qp(i);
246  dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i);
247  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
248  + z0*qpp(i);
249  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
250  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i);
251  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
252  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
253  + vp[i-1]);
254  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
255  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
256  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
257  - Q*Rt*(CrossProduct(wp[i-1],p[i])
258  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
259  }
260  else
261  {
262  w[i] = Rt*w[i-1];
263  dw[i] = Rt*dw[i-1];
264  wp[i] = Rt*wp[i-1];
265  dwp[i] = Rt*dwp[i-1];
266  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
267  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
268  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
269  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
270  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
271  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
272  + (CrossProduct(wp[i-1],dp[i])
273  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
274  + 2*CrossProduct(dw[i],z0*qp(i));
275  }
276  a[i] = CrossProduct(wp[i],links[i].r)
277  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
278  + vp[i];
279  da[i] = CrossProduct(dwp[i],links[i].r)
280  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
281  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
282  + dvp[i];
283  }
284 
285  for(i = dof; i >= 1; i--) {
286  F[i] = a[i] * links[i].m;
287  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
288  dF[i] = da[i] * links[i].m;
289  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
290  + CrossProduct(w[i],links[i].I*dw[i]);
291 
292  if(i == dof)
293  {
294  f[i] = F[i];
295  df[i] = dF[i];
296  n[i] = CrossProduct(links[i].r,F[i]) + N[i];
297  dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
298  }
299  else
300  {
301  f[i] = links[i+1].R*f[i+1] + F[i];
302  df[i] = links[i+1].R*df[i+1] + dF[i];
303  if(links[i+1].get_joint_type() == 0)
304  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
305 
306  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
307  + CrossProduct(links[i].r,F[i]) + N[i];
308  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
309  + CrossProduct(links[i].r,dF[i]) + dN[i];
310  if(links[i+1].get_joint_type() == 0)
311  dn[i] += (links[i+1].R*Q*n[i+1] +
312  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
313  else
314  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
315  }
316 
317  if(links[i].get_joint_type() == 0)
318  {
319  temp = z0.t()*n[i];
320  ltorque(i) = temp(1,1);
321  temp = z0.t()*dn[i];
322  dtorque(i) = temp(1,1);
323  }
324  else
325  {
326  temp = z0.t()*f[i];
327  ltorque(i) = temp(1,1);
328  temp = z0.t()*df[i];
329  dtorque(i) = temp(1,1);
330  }
331  }
332 }
333 
334 
335 void mRobot_min_para::dq_torque(const ColumnVector & q, const ColumnVector & qp,
336  const ColumnVector & qpp, const ColumnVector & dq,
337  ColumnVector & ltorque, ColumnVector & dtorque)
344 {
345  int i;
346  Matrix Rt, temp;
347  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
348  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
349  if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
350  if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
351  ltorque = ColumnVector(dof);
352  dtorque = ColumnVector(dof);
353  set_q(q);
354 
355  vp[0] = gravity;
356  ColumnVector z0(3);
357  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
358  Matrix Q(3,3);
359  Q = 0.0;
360  Q(1,2) = -1.0;
361  Q(2,1) = 1.0;
362  for(i = 1; i <= dof; i++)
363  {
364  Rt = links[i].R.t();
365  p[i] = links[i].p;
366  if(links[i].get_joint_type() != 0)
367  {
368  dp[i] = ColumnVector(3);
369  dp[i](1) = 0.0;
370  dp[i](2) = Rt(2,3);
371  dp[i](3) = Rt(3,3);
372  }
373  if(links[i].get_joint_type() == 0)
374  {
375  w[i] = Rt*w[i-1] + z0*qp(i);
376  dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i);
377  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
378  + z0*qpp(i);
379  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
380  - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i);
381  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
382  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
383  + vp[i-1]);
384  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
385  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
386  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
387  - Q*Rt*(CrossProduct(wp[i-1],p[i])
388  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
389  }
390  else
391  {
392  w[i] = Rt*w[i-1];
393  dw[i] = Rt*dw[i-1];
394  wp[i] = Rt*wp[i-1];
395  dwp[i] = Rt*dwp[i-1];
396  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
397  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
398  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
399  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
400  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
401  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
402  + (CrossProduct(wp[i-1],dp[i])
403  + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i));
404  }
405  }
406 
407  for(i = dof; i >= 1; i--) {
408  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
409  CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
410  dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
411  + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
412  + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
413  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
414  - CrossProduct(vp[i], links[i].mc);
415  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
416  + CrossProduct(w[i],links[i].I*dw[i])
417  - CrossProduct(dvp[i],links[i].mc);
418 
419  if(i == dof)
420  {
421  f[i] = F[i];
422  df[i] = dF[i];
423  n[i] = N[i];
424  dn[i] = dN[i];
425  }
426  else
427  {
428  f[i] = links[i+1].R*f[i+1] + F[i];
429  df[i] = links[i+1].R*df[i+1] + dF[i];
430  if(links[i+1].get_joint_type() == 0)
431  df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
432 
433  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
434  + N[i];
435  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
436  + dN[i];
437  if(links[i+1].get_joint_type() == 0)
438  dn[i] += (links[i+1].R*Q*n[i+1] +
439  CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
440  else
441  dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
442  }
443 
444  if(links[i].get_joint_type() == 0)
445  {
446  temp = z0.t()*n[i];
447  ltorque(i) = temp(1,1);
448  temp = z0.t()*dn[i];
449  dtorque(i) = temp(1,1);
450  }
451  else
452  {
453  temp = z0.t()*f[i];
454  ltorque(i) = temp(1,1);
455  temp = z0.t()*df[i];
456  dtorque(i) = temp(1,1);
457  }
458  }
459 }
460 
461 #ifdef use_namespace
462 }
463 #endif
robot.h
Robots class definitions.
GeneralMatrix::Nrows
int Nrows() const
Definition: newmat.h:494
mRobot::dq_torque
virtual void dq_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)
Delta torque due to delta joint position.
Definition: comp_dq.cpp:205
BaseMatrix::t
TransposedMatrix t() const
Definition: newmat6.cpp:320
CrossProduct
Matrix CrossProduct(const Matrix &A, const Matrix &B)
Definition: newmat.h:2062
Matrix
The usual rectangular matrix.
Definition: newmat.h:625
a
FloatVector FloatVector * a
Definition: include/libKinematics.h:78
GeneralMatrix::Ncols
int Ncols() const
Definition: newmat.h:495
rcsid
static const char rcsid[]
RCS/CVS version.
Definition: comp_dq.cpp:55
mRobot_min_para::dq_torque
virtual void dq_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)
Delta torque due to delta joint position.
Definition: comp_dq.cpp:335
ColumnVector
Column vector.
Definition: newmat.h:1008
Robot::dq_torque
virtual void dq_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)
Delta torque due to delta joint position.
Definition: comp_dq.cpp:65

kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16