kinemat.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 Revision_history:
32 
33 2003/02/03: Etienne Lachance
34  -Rewrite mRobot::jacobian() since previous version was incorrect.
35  -Added functions kine_pd().
36  -Make sur that joint angles (qout) are in [-pi, pi] in inv_kin functions.
37 
38 2003/05/18: Etienne Lachance
39  -Added functions Robot_basic::jacobian_DLS_inv and
40  Robot/mRobot/mRobot_min_para::jacobian_dot
41 
42 2003/08/22: Etienne Lachance
43  -Added parameter converge in inv_kin prototype function. It indicates if the
44  inverse kinematics solution converge.
45 
46 2003/11/26: Etienne Lachance
47  -Use angle conditions only if it converge in inv_kin.
48 
49 2004/01/23: Etienne Lachance
50  -Added const in non reference argument for all functions.
51 
52 2004/03/12: Etienne Lachance
53  -Added logic to set q in inv_kin.
54 
55 2004/04/24: Etienne Lachance
56  -Moved inv_kin to invkine.cpp.
57 
58 2004/05/14: Etienne Lachance
59  -Replaced vec_x_prod by CrossProduct.
60 
61 2004/05/21: Etienne Lachance
62  -Added Doxygen comments.
63 
64 2004/07/01: Ethan Tira-Thompson
65  -Added support for newmat's use_namespace #define, using ROBOOP namespace
66 
67 2004/07/16: Ethan Tira-Thompson
68  -Supports Link::immobile flag so jacobians and deltas are 0 for immobile joints
69  -Jacobians will only contain entries for mobile joints - otherwise NaNs result
70  in later processing
71  -Added parameters to jacobian functions to generate for frames other than
72  the end effector
73 -------------------------------------------------------------------------------
74 */
75 
81 static const char rcsid[] = "$Id: kinemat.cpp,v 1.31 2004/08/16 00:37:53 gourdeau Exp $";
83 
84 #include "robot.h"
85 
86 #ifdef use_namespace
87 namespace ROBOOP {
88  using namespace NEWMAT;
89 #endif
90 
91 
92 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos)const
98 {
99  kine(Rot,pos,dof+fix);
100 }
101 
102 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos, const int j)const
109 {
110  if(j < 1 || j > dof+fix) error("j must be 1 <= j <= dof+fix");
111 
112  Rot = links[1].R;
113  pos = links[1].p;
114  for (int i = 2; i <= j; i++) {
115  pos = pos + Rot*links[i].p;
116  Rot = Rot*links[i].R;
117  }
118 }
119 
122 {
123  Matrix thomo;
124 
125  thomo = kine(dof+fix);
126  thomo.Release(); return thomo;
127 }
128 
131 {
132  Matrix Rot, thomo(4,4);
133  ColumnVector pos;
134 
135  kine(Rot,pos,j);
136  thomo << fourbyfourident;
137  thomo.SubMatrix(1,3,1,3) = Rot;
138  thomo.SubMatrix(1,3,4,4) = pos;
139  thomo.Release(); return thomo;
140 }
141 
153 {
154  Matrix temp(3,5), Rot;
155  ColumnVector pos, pos_dot;
156 
157  if(j < 1 || j > dof)
158  error("j must be 1 <= j <= dof");
159 
160  kine_pd(Rot, pos, pos_dot, j);
161 
162  temp.SubMatrix(1,3,1,3) = Rot;
163  temp.SubMatrix(1,3,4,4) = pos;
164  temp.SubMatrix(1,3,5,5) = pos_dot;
165  temp.Release();
166  return temp;
167 }
168 
169 ReturnMatrix Robot_basic::jacobian_DLS_inv(const double eps, const double lambda_max,
170  const int ref)const
200 {
201  Matrix jacob_inv_DLS, U, V;
202  DiagonalMatrix Q;
203  SVD(jacobian(ref), Q, U, V);
204 
205  if(Q(6,6) >= eps)
206  jacob_inv_DLS = V*Q.i()*U.t();
207  else
208  {
209  Q(6,6) += (1 - pow(Q(6,6)/eps,2))*lambda_max*lambda_max;
210  jacob_inv_DLS = V*Q.i()*U.t();
211  }
212 
213  jacob_inv_DLS.Release();
214  return(jacob_inv_DLS);
215 }
216 
217 // --------------------- R O B O T DH N O T A T I O N --------------------------
218 
219 void Robot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
220  const int j)const
229 {
230  if(j < 1 || j > dof)
231  error("j must be 1 <= j <= dof");
232 
233  if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
234  pos = ColumnVector(3);
235  if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
236  pos_dot = ColumnVector(3);
237 
238  pos = 0.0;
239  pos_dot = 0.0;
240  for(int i = 1; i <= j; i++)
241  {
242  R[i] = R[i-1]*links[i].R;
243  pos = pos + R[i-1]*links[i].p;
244  pos_dot = pos_dot + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
245  }
246  Rot = R[j];
247 }
248 
249 void Robot::dTdqi(Matrix & dRot, ColumnVector & dp,
250  const int i)
293 {
294  int j;
295  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
296  if(links[i].get_immobile()) {
297  dRot = Matrix(3,3);
298  dp = Matrix(3,1);
299  dRot = 0.0;
300  dp = 0.0;
301  } else if(links[i].get_joint_type() == 0) {
302  Matrix dR(3,3);
303  dR = 0.0;
304  Matrix R2 = links[i].R;
305  ColumnVector p2 = links[i].p;
306  dRot = Matrix(3,3);
307  dRot << threebythreeident;
308  for(j = 1; j < i; j++) {
309  dRot = dRot*links[j].R;
310  }
311  // dRot * Q
312  for(j = 1; j <= 3; j++) {
313  dR(j,1) = dRot(j,2);
314  dR(j,2) = -dRot(j,1);
315  }
316  for(j = i+1; j <= dof; j++) {
317  p2 = p2 + R2*links[j].p;
318  R2 = R2*links[j].R;
319  }
320  dp = dR*p2;
321  dRot = dR*R2;
322  } else {
323  dRot = Matrix(3,3);
324  dp = Matrix(3,1);
325  dRot = 0.0;
326  dp = 0.0;
327  dp(3) = 1.0;
328  for(j = i-1; j >= 1; j--) {
329  dp = links[j].R*dp;
330  }
331  }
332 }
333 
341 {
342  Matrix dRot, thomo(4,4);
343  ColumnVector dpos;
344 
345  dTdqi(dRot, dpos, i);
346  thomo = (Real) 0.0;
347  thomo.SubMatrix(1,3,1,3) = dRot;
348  thomo.SubMatrix(1,3,4,4) = dpos;
349  thomo.Release(); return thomo;
350 }
351 
352 ReturnMatrix Robot::jacobian(const int endlink, const int ref)const
401 {
402  int i, j;
403  const int adof=get_available_dof(endlink);
404  Matrix jac(6,adof);
405  Matrix pr, temp(3,1);
406 
407  if(ref < 0 || ref > dof) error("invalid referential");
408 
409  for(i = 1; i <= dof; i++) {
410  R[i] = R[i-1]*links[i].R;
411  p[i] = p[i-1]+R[i-1]*links[i].p;
412  }
413 
414  for(i=1,j=1; j <= adof; i++) {
415  if(links[i].get_immobile())
416  continue;
417  if(links[i].get_joint_type() == 0) {
418  temp(1,1) = R[i-1](1,3);
419  temp(2,1) = R[i-1](2,3);
420  temp(3,1) = R[i-1](3,3);
421  pr = p[dof]-p[i-1];
422  temp = CrossProduct(temp,pr);
423  jac(1,j) = temp(1,1);
424  jac(2,j) = temp(2,1);
425  jac(3,j) = temp(3,1);
426  jac(4,j) = R[i-1](1,3);
427  jac(5,j) = R[i-1](2,3);
428  jac(6,j) = R[i-1](3,3);
429  } else {
430  jac(1,j) = R[i-1](1,3);
431  jac(2,j) = R[i-1](2,3);
432  jac(3,j) = R[i-1](3,3);
433  jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
434  }
435  j++;
436  }
437 
438  if(ref != 0) {
439  Matrix zeros(3,3);
440  zeros = (Real) 0.0;
441  Matrix RT = R[ref].t();
442  Matrix Rot;
443  Rot = ((RT & zeros) | (zeros & RT));
444  jac = Rot*jac;
445  }
446  jac.Release(); return jac;
447 }
448 
497 {
498  int i, j;
499  const int adof=get_available_dof();
500  Matrix jacdot(6,adof);
501  ColumnVector e(3), temp, pr, ppr;
502 
503  if(ref < 0 || ref > dof)
504  error("invalid referential");
505 
506  for(i = 1; i <= dof; i++)
507  {
508  R[i] = R[i-1]*links[i].R;
509  p[i] = p[i-1] + R[i-1]*links[i].p;
510  pp[i] = pp[i-1] + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
511  }
512 
513  for(i=1,j=1; j <= adof; i++) {
514  if(links[i].get_immobile())
515  continue;
516  if(links[i].get_joint_type() == 0)
517  {
518  pr = p[dof]-p[i-1];
519  ppr = pp[dof]-pp[i-1];
520  e(1) = R[i-1](1,3);
521  e(2) = R[i-1](2,3);
522  e(3) = R[i-1](3,3);
523  temp = CrossProduct(R[i-1]*w[i-1], e);
524  jacdot(4,j) = temp(1); // d(e)/dt
525  jacdot(5,j) = temp(2);
526  jacdot(6,j) = temp(3);
527  temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
528  jacdot(1,j) = temp(1);
529  jacdot(2,j) = temp(2);
530  jacdot(3,j) = temp(3);
531  }
532  else
533  jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
534  jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
535  j++;
536  }
537 
538  if(ref != 0) {
539  Matrix zeros(3,3);
540  zeros = (Real) 0.0;
541  Matrix RT = R[ref].t();
542  Matrix Rot;
543  Rot = ((RT & zeros) | (zeros & RT));
544  jacdot = Rot*jacdot;
545  }
546 
547  jacdot.Release(); return jacdot;
548 }
549 
550 // ---------------- R O B O T M O D I F I E D DH N O T A T I O N --------------------------
551 
552 void mRobot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
553  const int j)const
562 {
563  if(j < 1 || j > dof+fix)
564  error("j must be 1 <= j <= dof+fix");
565 
566  if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
567  pos = ColumnVector(3);
568  if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
569  pos_dot = ColumnVector(3);
570 
571  pos = 0.0;
572  pos_dot = 0.0;
573  for(int i = 1; i <= j; i++)
574  {
575  pos = pos + R[i-1]*links[i].p;
576  pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
577  R[i] = R[i-1]*links[i].R;
578  }
579  Rot = R[j];
580 }
581 
582 void mRobot::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
618 {
619  int j;
620  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
621  if(links[i].get_immobile()) {
622  dRot = Matrix(3,3);
623  dp = Matrix(3,1);
624  dRot = 0.0;
625  dp = 0.0;
626  } else if(links[i].get_joint_type() == 0) {
627  Matrix dR(3,3), R2(3,3), p2(3,1);
628  dR = 0.0;
629  dRot = Matrix(3,3);
630  dRot << threebythreeident;
631  for(j = 1; j <= i; j++) {
632  dRot = dRot*links[j].R;
633  }
634  // dRot * Q
635  for(j = 1; j <= 3; j++) {
636  dR(j,1) = dRot(j,2);
637  dR(j,2) = -dRot(j,1);
638  }
639  if(i < dof) {
640  R2 = links[i+1].R;
641  p2 = links[i+1].p;
642  } else {
643  R2 << threebythreeident;
644  p2 = 0.0;
645  }
646  for(j = i+1; j <= dof; j++) {
647  p2 = p2 + R2*links[j].p;
648  R2 = R2*links[j].R;
649  }
650  dp = dR*p2;
651  dRot = dR*R2; // probleme ...
652  } else {
653  dRot = Matrix(3,3);
654  dp = Matrix(3,1);
655  dRot = 0.0;
656  dp = 0.0;
657  dp(3) = 1.0;
658  for(j = i; j >= 1; j--) {
659  dp = links[j].R*dp;
660  }
661  }
662 }
663 
671 {
672  Matrix dRot, thomo(4,4);
673  ColumnVector dpos;
674 
675  dTdqi(dRot, dpos, i);
676  thomo = (Real) 0.0;
677  thomo.SubMatrix(1,3,1,3) = dRot;
678  thomo.SubMatrix(1,3,4,4) = dpos;
679  thomo.Release(); return thomo;
680 }
681 
682 ReturnMatrix mRobot::jacobian(const int endlink, const int ref)const
688 {
689  int i, j;
690  const int adof=get_available_dof(endlink);
691  Matrix jac(6,adof);
692  ColumnVector pr(3), temp(3);
693 
694  if(ref < 0 || ref > dof+fix)
695  error("invalid referential");
696 
697  for(i = 1; i <= dof+fix; i++) {
698  R[i] = R[i-1]*links[i].R;
699  p[i] = p[i-1] + R[i-1]*links[i].p;
700  }
701 
702  for(i=1,j=1; j <= adof; i++) {
703  if(links[i].get_immobile())
704  continue;
705  if(links[i].get_joint_type() == 0){
706  temp(1) = R[i](1,3);
707  temp(2) = R[i](2,3);
708  temp(3) = R[i](3,3);
709  pr = p[dof+fix]-p[i];
710  temp = CrossProduct(temp,pr);
711  jac(1,j) = temp(1);
712  jac(2,j) = temp(2);
713  jac(3,j) = temp(3);
714  jac(4,j) = R[i](1,3);
715  jac(5,j) = R[i](2,3);
716  jac(6,j) = R[i](3,3);
717  } else {
718  jac(1,j) = R[i](1,3);
719  jac(2,j) = R[i](2,3);
720  jac(3,j) = R[i](3,3);
721  jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
722  }
723  j++;
724  }
725  if(ref != 0) {
726  Matrix zeros(3,3);
727  zeros = (Real) 0.0;
728  Matrix RT = R[ref].t();
729  Matrix Rot;
730  Rot = ((RT & zeros) | (zeros & RT));
731  jac = Rot*jac;
732  }
733  jac.Release(); return jac;
734 }
735 
742 {
743  int i, j;
744  const int adof=get_available_dof();
745  Matrix jacdot(6,adof);
746  ColumnVector e(3), temp, pr, ppr;
747 
748  if(ref < 0 || ref > dof+fix)
749  error("invalid referential");
750 
751  for(i = 1; i <= dof+fix; i++)
752  {
753  R[i] = R[i-1]*links[i].R;
754  p[i] = p[i-1] + R[i-1]*links[i].p;
755  pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
756  }
757 
758  for(i=1,j=1; j <= adof; i++) {
759  if(links[i].get_immobile())
760  continue;
761  if(links[i].get_joint_type() == 0)
762  {
763  pr = p[dof+fix]-p[i];
764  ppr = pp[dof+fix]-pp[i];
765 
766  e(1) = R[i](1,3);
767  e(2) = R[i](2,3);
768  e(3) = R[i](3,3);
769  temp = CrossProduct(R[i-1]*w[i-1], e);
770  jacdot(4,j) = temp(1); // d(e)/dt
771  jacdot(5,j) = temp(2);
772  jacdot(6,j) = temp(3);
773 
774  temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
775  jacdot(1,j) = temp(1);
776  jacdot(2,j) = temp(2);
777  jacdot(3,j) = temp(3);
778  }
779  else
780  jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
781  jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
782  j++;
783  }
784 
785  if(ref != 0) {
786  Matrix zeros(3,3);
787  zeros = (Real) 0.0;
788  Matrix RT = R[ref].t();
789  Matrix Rot;
790  Rot = ((RT & zeros) | (zeros & RT));
791  jacdot = Rot*jacdot;
792  }
793 
794  jacdot.Release(); return jacdot;
795 }
796 
797 // ------------- R O B O T DH M O D I F I E D, M I N I M U M P A R A M E T E R S ------------
798 
800  const int j)const
809 {
810  if(j < 1 || j > dof+fix)
811  error("j must be 1 <= j <= dof+fix");
812 
813  if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
814  pos = ColumnVector(3);
815  if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
816  pos_dot = ColumnVector(3);
817 
818  pos = 0.0;
819  pos_dot = 0.0;
820  for(int i = 1; i <= j; i++)
821  {
822  pos = pos + R[i-1]*links[i].p;
823  pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
824  R[i] = R[i-1]*links[i].R;
825  }
826  Rot = R[j];
827 }
828 
829 void mRobot_min_para::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
841 {
842  int j;
843  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
844  if(links[i].get_immobile()) {
845  dRot = Matrix(3,3);
846  dp = Matrix(3,1);
847  dRot = 0.0;
848  dp = 0.0;
849  } else if(links[i].get_joint_type() == 0) {
850  Matrix dR(3,3), R2, p2(3,1);
851  dR = 0.0;
852  dRot = Matrix(3,3);
853  dRot << threebythreeident;
854  for(j = 1; j <= i; j++) {
855  dRot = dRot*links[j].R;
856  }
857  // dRot * Q
858  for(j = 1; j <= 3; j++) {
859  dR(j,1) = dRot(j,2);
860  dR(j,2) = -dRot(j,1);
861  }
862  if(i < dof) {
863  R2 = links[i+1].R;
864  p2 = links[i+1].p;
865  } else {
866  R2 << threebythreeident;
867  p2 = 0.0;
868  }
869  for(j = i+1; j <= dof; j++) {
870  p2 = p2 + R2*links[j].p;
871  R2 = R2*links[j].R;
872  }
873  dp = dR*p2;
874  dRot = dR*R2;
875  } else {
876  dRot = Matrix(3,3);
877  dp = Matrix(3,1);
878  dRot = 0.0;
879  dp = 0.0;
880  dp(3) = 1.0;
881  for(j = i; j >= 1; j--) {
882  dp = links[j].R*dp;
883  }
884  }
885 }
886 
894 {
895  Matrix dRot, thomo(4,4);
896  ColumnVector dpos;
897 
898  dTdqi(dRot, dpos, i);
899  thomo = (Real) 0.0;
900  thomo.SubMatrix(1,3,1,3) = dRot;
901  thomo.SubMatrix(1,3,4,4) = dpos;
902  thomo.Release(); return thomo;
903 }
904 
905 ReturnMatrix mRobot_min_para::jacobian(const int endlink, const int ref)const
911 {
912  int i, j;
913  const int adof=get_available_dof(endlink);
914  Matrix jac(6,adof);
915  ColumnVector pr(3), temp(3);
916 
917  if(ref < 0 || ref > dof+fix)
918  error("invalid referential");
919 
920  for(i = 1; i <= dof+fix; i++) {
921  R[i] = R[i-1]*links[i].R;
922  p[i] = p[i-1] + R[i-1]*links[i].p;
923  }
924 
925  for(i=1,j=1; j<=adof; i++) {
926  if(links[i].get_immobile())
927  continue;
928  if(links[i].get_joint_type() == 0){
929  temp(1) = R[i](1,3);
930  temp(2) = R[i](2,3);
931  temp(3) = R[i](3,3);
932 
933  pr = p[dof+fix]-p[i];
934  temp = CrossProduct(temp,pr);
935  jac(1,j) = temp(1);
936  jac(2,j) = temp(2);
937  jac(3,j) = temp(3);
938  jac(4,j) = R[i](1,3);
939  jac(5,j) = R[i](2,3);
940  jac(6,j) = R[i](3,3);
941  } else {
942  jac(1,j) = R[i](1,3);
943  jac(2,j) = R[i](2,3);
944  jac(3,j) = R[i](3,3);
945  jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
946  }
947  j++;
948  }
949  if(ref != 0) {
950  Matrix zeros(3,3);
951  zeros = (Real) 0.0;
952  Matrix RT = R[ref].t();
953  Matrix Rot;
954  Rot = ((RT & zeros) | (zeros & RT));
955  jac = Rot*jac;
956  }
957 
958  jac.Release(); return jac;
959 }
960 
967 {
968  int i, j;
969  const int adof=get_available_dof();
970  Matrix jacdot(6,adof);
971  ColumnVector e(3), temp, pr, ppr;
972 
973  if(ref < 0 || ref > dof+fix)
974  error("invalid referential");
975 
976  for(i = 1; i <= dof+fix; i++)
977  {
978  R[i] = R[i-1]*links[i].R;
979  p[i] = p[i-1] + R[i-1]*links[i].p;
980  pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
981  }
982 
983  for(i=1,j=1; j <= dof; i++) {
984  if(links[i].get_immobile())
985  continue;
986  if(links[i].get_joint_type() == 0)
987  {
988  pr = p[dof+fix]-p[i];
989  ppr = pp[dof+fix]-pp[i];
990 
991  e(1) = R[i](1,3);
992  e(2) = R[i](2,3);
993  e(3) = R[i](3,3);
994  temp = CrossProduct(R[i-1]*w[i-1], e);
995  jacdot(4,j) = temp(1); // d(e)/dt
996  jacdot(5,j) = temp(2);
997  jacdot(6,j) = temp(3);
998 
999  temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
1000  jacdot(1,j) = temp(1);
1001  jacdot(2,j) = temp(2);
1002  jacdot(3,j) = temp(3);
1003  }
1004  else
1005  jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
1006  jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
1007  j++;
1008  }
1009 
1010  if(ref != 0) {
1011  Matrix zeros(3,3);
1012  zeros = (Real) 0.0;
1013  Matrix RT = R[ref].t();
1014  Matrix Rot;
1015  Rot = ((RT & zeros) | (zeros & RT));
1016  jacdot = Rot*jacdot;
1017  }
1018 
1019  jacdot.Release(); return jacdot;
1020 }
1021 
1022 #ifdef use_namespace
1023 }
1024 #endif
void Release()
Definition: newmat.h:514
virtual ReturnMatrix jacobian(const int ref=0) const
Jacobian of mobile links expressed at frame ref.
Definition: robot.h:454
virtual void kine_pd(Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const
Direct kinematics with velocity.
Definition: kinemat.cpp:552
Robots class definitions.
ReturnMatrix kine(void) const
Return the end effector direct kinematics transform matrix.
Definition: kinemat.cpp:120
double Real
Definition: include.h:307
virtual void dTdqi(Matrix &dRot, ColumnVector &dp, const int i)
Partial derivative of the robot position (homogeneous transf.)
Definition: kinemat.cpp:249
TransposedMatrix t() const
Definition: newmat6.cpp:320
virtual ReturnMatrix jacobian_dot(const int ref=0) const
Jacobian derivative of mobile joints expressed at frame ref.
Definition: kinemat.cpp:736
Matrix CrossProduct(const Matrix &A, const Matrix &B)
Definition: newmat.h:2062
void SVD(const Matrix &, DiagonalMatrix &, Matrix &, Matrix &, bool=true, bool=true)
Definition: svd.cpp:30
The usual rectangular matrix.
Definition: newmat.h:625
InvertedMatrix i() const
Definition: newmat6.cpp:329
virtual ReturnMatrix jacobian_dot(const int ref=0) const
Jacobian derivative of mobile joints expressed at frame ref.
Definition: kinemat.cpp:961
virtual void dTdqi(Matrix &dRot, ColumnVector &dp, const int i)
Partial derivative of the robot position (homogeneous transf.)
Definition: kinemat.cpp:829
virtual void kine_pd(Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const
Direct kinematics with velocity.
Definition: kinemat.cpp:219
virtual ReturnMatrix jacobian(const int ref=0) const
Jacobian of mobile links expressed at frame ref.
Definition: robot.h:405
static const char rcsid[]
RCS/CVS version.
Definition: kinemat.cpp:82
Diagonal matrix.
Definition: newmat.h:896
ReturnMatrix kine_pd(const int ref=0) const
Direct kinematics with velocity.
Definition: kinemat.cpp:142
virtual ReturnMatrix jacobian_dot(const int ref=0) const
Jacobian derivative of mobile joints expressed at frame ref.
Definition: kinemat.cpp:449
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
ReturnMatrix jacobian_DLS_inv(const double eps, const double lambda_max, const int ref=0) const
Inverse Jacobian based on damped least squares inverse.
Definition: kinemat.cpp:169
virtual void kine_pd(Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref=0) const
Direct kinematics with velocity.
Definition: kinemat.cpp:799
Real threebythreeident[]
Used to initialize a matrix.
Definition: robot.cpp:136
Column vector.
Definition: newmat.h:1008
Real fourbyfourident[]
Used to initialize a matrix.
Definition: robot.cpp:133
virtual ReturnMatrix jacobian(const int ref=0) const
Jacobian of mobile links expressed at frame ref.
Definition: robot.h:357
virtual void dTdqi(Matrix &dRot, ColumnVector &dp, const int i)
Partial derivative of the robot position (homogeneous transf.)
Definition: kinemat.cpp:582


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