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


kni
Author(s): Martin Günther
autogenerated on Fri Jun 7 2019 22:06:44