ROBOOP, A Robotics Object Oriented Package in C++
comp_dqp.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  -Added function mRobot/mRobot_min_para::dqp_torque
35 
36 2004/07/01: Etienne Lachance
37  -Replaced vec_x_prod by CrossProduct.
38 
39 2004/07/01: Ethan Tira-Thompson
40  -Added support for newmat's use_namespace #define, using ROBOOP namespace
41 
42 2004/07/02: Etienne Lachance
43  -Added Doxygen comments.
44 -------------------------------------------------------------------------------
45 */
46 
52 #include "robot.h"
53 
54 #ifdef use_namespace
55 namespace ROBOOP {
56  using namespace NEWMAT;
57 #endif
58 
59 void Robot::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
60  const ColumnVector & dqp,
61  ColumnVector & ltorque, ColumnVector & dtorque)
68 {
69  int i;
70  Matrix Rt, temp;
71  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
72  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
73  if(dqp.Ncols() != 1 || qp.Nrows() != dof) error("dqp has wrong dimension");
74  ltorque = ColumnVector(dof);
75  dtorque = ColumnVector(dof);
76  set_q(q);
77 
78  vp[0] = gravity;
79  ColumnVector z0(3);
80  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
81  Matrix Q(3,3);
82  Q = 0.0;
83  Q(1,2) = -1.0;
84  Q(2,1) = 1.0;
85 
86  for(i = 1; i <= dof; i++)
87  {
88  Rt = links[i].R.t();
89  p[i] = ColumnVector(3);
90  p[i](1) = links[i].get_a();
91  p[i](2) = links[i].get_d() * Rt(2,3);
92  p[i](3) = links[i].get_d() * Rt(3,3);
93  if(links[i].get_joint_type() != 0)
94  {
95  dp[i] = ColumnVector(3);
96  dp[i](1) = 0.0;
97  dp[i](2) = Rt(2,3);
98  dp[i](3) = Rt(3,3);
99  }
100  if(links[i].get_joint_type() == 0)
101  {
102  w[i] = Rt*(w[i-1] + z0*qp(i));
103  dw[i] = Rt*(dw[i-1] + z0*dqp(i));
104  wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
105  dwp[i] = Rt*(dwp[i-1]
106  + CrossProduct(dw[i-1],z0*qp(i))
107  + CrossProduct(w[i-1],z0*dqp(i)));
108  vp[i] = CrossProduct(wp[i],p[i])
109  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
110  + Rt*(vp[i-1]);
111  dvp[i] = CrossProduct(dwp[i],p[i])
112  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
113  + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
114  + Rt*dvp[i-1];
115  }
116  else
117  {
118  w[i] = Rt*w[i-1];
119  dw[i] = Rt*dw[i-1];
120  wp[i] = Rt*wp[i-1];
121  dwp[i] = Rt*dwp[i-1];
122  vp[i] = Rt*(vp[i-1]
123  + 2.0*CrossProduct(w[i],Rt*z0*qp(i)))
124  + CrossProduct(wp[i],p[i])
125  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
126  dvp[i] = Rt*(dvp[i-1]
127  + 2.0*(CrossProduct(dw[i-1],z0*qp(i))
128  + CrossProduct(w[i-1],z0*dqp(i))))
129  + CrossProduct(dwp[i],p[i])
130  + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
131  + CrossProduct(w[i],CrossProduct(dw[i],p[i]));
132  }
133  a[i] = CrossProduct(wp[i],links[i].r)
134  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
135  + vp[i];
136  da[i] = CrossProduct(dwp[i],links[i].r)
137  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
138  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
139  + dvp[i];
140  }
141 
142  for(i = dof; i >= 1; i--)
143  {
144  F[i] = a[i] * links[i].m;
145  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
146  dF[i] = da[i] * links[i].m;
147  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
148  + CrossProduct(w[i],links[i].I*dw[i]);
149  if(i == dof)
150  {
151  f[i] = F[i];
152  n[i] = CrossProduct(p[i],f[i])
153  + CrossProduct(links[i].r,F[i]) + N[i];
154  df[i] = dF[i];
155  dn[i] = CrossProduct(p[i],df[i])
156  + CrossProduct(links[i].r,dF[i]) + dN[i];
157  }
158  else
159  {
160  f[i] = links[i+1].R*f[i+1] + F[i];
161  df[i] = links[i+1].R*df[i+1] + dF[i];
162  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
163  + CrossProduct(links[i].r,F[i]) + N[i];
164  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
165  + CrossProduct(links[i].r,dF[i]) + dN[i];
166  }
167  if(links[i].get_joint_type() == 0)
168  {
169  temp = ((z0.t()*links[i].R)*n[i]);
170  ltorque(i) = temp(1,1);
171  temp = ((z0.t()*links[i].R)*dn[i]);
172  dtorque(i) = temp(1,1);
173  }
174  else
175  {
176  temp = ((z0.t()*links[i].R)*f[i]);
177  ltorque(i) = temp(1,1);
178  temp = ((z0.t()*links[i].R)*df[i]);
179  dtorque(i) = temp(1,1);
180  }
181  }
182 }
183 
184 void mRobot::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
185  const ColumnVector & dqp, ColumnVector & ltorque,
186  ColumnVector & dtorque)
193 {
194  int i;
195  Matrix Rt, temp;
196  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
197  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
198  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
199  ltorque = ColumnVector(dof);
200  dtorque = ColumnVector(dof);
201  set_q(q);
202 
203  vp[0] = gravity;
204  ColumnVector z0(3);
205  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
206  Matrix Q(3,3);
207  Q = 0.0;
208  Q(1,2) = -1.0;
209  Q(2,1) = 1.0;
210  for(i = 1; i <= dof; i++)
211  {
212  Rt = links[i].R.t();
213  p[i] = links[i].p;
214  if(links[i].get_joint_type() != 0)
215  {
216  dp[i] = ColumnVector(3);
217  dp[i](1) = 0.0;
218  dp[i](2) = Rt(2,3);
219  dp[i](3) = Rt(3,3);
220  }
221  if(links[i].get_joint_type() == 0)
222  {
223  w[i] = Rt*w[i-1] + z0*qp(i);
224  dw[i] = Rt*dw[i-1] + z0*dqp(i);
225  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i));
226  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
227  + CrossProduct(Rt*w[i-1],z0*dqp(i));
228  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
229  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
230  + vp[i-1]);
231  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
232  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
233  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]);
234  }
235  else
236  {
237  w[i] = Rt*w[i-1];
238  dw[i] = Rt*dw[i-1];
239  wp[i] = Rt*wp[i-1];
240  dwp[i] = Rt*dwp[i-1];
241  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
242  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
243  + 2.0*CrossProduct(w[i],z0*qp(i));
244  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
245  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
246  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
247  + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)));
248  }
249  a[i] = CrossProduct(wp[i],links[i].r)
250  + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
251  + vp[i];
252  da[i] = CrossProduct(dwp[i],links[i].r)
253  + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
254  + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
255  + dvp[i];
256  }
257 
258  for(i = dof; i >= 1; i--) {
259  F[i] = a[i] * links[i].m;
260  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
261  dF[i] = da[i] * links[i].m;
262  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
263  + CrossProduct(w[i],links[i].I*dw[i]);
264 
265  if(i == dof)
266  {
267  f[i] = F[i];
268  df[i] = dF[i];
269  n[i] = CrossProduct(links[i].r,F[i]) + N[i];
270  dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
271  }
272  else
273  {
274  f[i] = links[i+1].R*f[i+1] + F[i];
275  df[i] = links[i+1].R*df[i+1] + dF[i];
276  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
277  + CrossProduct(links[i].r,F[i]) + N[i];
278  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
279  + CrossProduct(links[i].r,dF[i]) + dN[i];
280  }
281 
282  if(links[i].get_joint_type() == 0)
283  {
284  temp = z0.t()*n[i];
285  ltorque(i) = temp(1,1);
286  temp = z0.t()*dn[i];
287  dtorque(i) = temp(1,1);
288  }
289  else
290  {
291  temp = z0.t()*f[i];
292  ltorque(i) = temp(1,1);
293  temp = z0.t()*df[i];
294  dtorque(i) = temp(1,1);
295  }
296  }
297 }
298 
299 void mRobot_min_para::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
300  const ColumnVector & dqp, ColumnVector & ltorque,
301  ColumnVector & dtorque)
308 {
309  int i;
310  Matrix Rt, temp;
311  if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
312  if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
313  if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
314  ltorque = ColumnVector(dof);
315  dtorque = ColumnVector(dof);
316  set_q(q);
317 
318  vp[0] = gravity;
319  ColumnVector z0(3);
320  z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
321  Matrix Q(3,3);
322  Q = 0.0;
323  Q(1,2) = -1.0;
324  Q(2,1) = 1.0;
325  for(i = 1; i <= dof; i++)
326  {
327  Rt = links[i].R.t();
328  p[i] = links[i].p;
329  if(links[i].get_joint_type() != 0)
330  {
331  dp[i] = ColumnVector(3);
332  dp[i](1) = 0.0;
333  dp[i](2) = Rt(2,3);
334  dp[i](3) = Rt(3,3);
335  }
336  if(links[i].get_joint_type() == 0)
337  {
338  w[i] = Rt*w[i-1] + z0*qp(i);
339  dw[i] = Rt*dw[i-1] + z0*dqp(i);
340  wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i));
341  dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i));
342  vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
343  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
344  + vp[i-1]);
345  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
346  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
347  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]);
348  }
349  else
350  {
351  w[i] = Rt*w[i-1];
352  dw[i] = Rt*dw[i-1];
353  wp[i] = Rt*wp[i-1];
354  dwp[i] = Rt*dwp[i-1];
355  vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
356  + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
357  + 2.0*CrossProduct(w[i],z0*qp(i));
358  dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
359  + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
360  + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
361  + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)));
362  }
363  }
364 
365  for(i = dof; i >= 1; i--) {
366  F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
367  CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
368  dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
369  + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
370  + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
371  N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
372  - CrossProduct(vp[i], links[i].mc);
373  dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
374  + CrossProduct(w[i],links[i].I*dw[i])
375  - CrossProduct(dvp[i],links[i].mc);
376 
377  if(i == dof)
378  {
379  f[i] = F[i];
380  df[i] = dF[i];
381  n[i] = N[i];
382  dn[i] = dN[i];
383  }
384  else
385  {
386  f[i] = links[i+1].R*f[i+1] + F[i];
387  df[i] = links[i+1].R*df[i+1] + dF[i];
388  n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
389  dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1]) + dN[i];
390  }
391 
392  if(links[i].get_joint_type() == 0)
393  {
394  temp = z0.t()*n[i];
395  ltorque(i) = temp(1,1);
396  temp = z0.t()*dn[i];
397  dtorque(i) = temp(1,1);
398  }
399  else
400  {
401  temp = z0.t()*f[i];
402  ltorque(i) = temp(1,1);
403  temp = z0.t()*df[i];
404  dtorque(i) = temp(1,1);
405  }
406  }
407 }
408 
409 #ifdef use_namespace
410 }
411 #endif
412 
413 
414 
415 
416 
417 
418 
419 
420 
421 
422