ROBOOP, A Robotics Object Oriented Package in C++
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, 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 Revision_history:
31 
32 2003/02/03: Etienne Lachance
33  -Rewrite mRobot::jacobian() since previous version was incorrect.
34  -Added functions kine_pd().
35  -Make sur that joint angles (qout) are in [-pi, pi] in inv_kin functions.
36 
37 2003/05/18: Etienne Lachance
38  -Added functions Robot_basic::jacobian_DLS_inv and
39  Robot/mRobot/mRobot_min_para::jacobian_dot
40 
41 2003/08/22: Etienne Lachance
42  -Added parameter converge in inv_kin prototype function. It indicates if the
43  inverse kinematics solution converge.
44 
45 2003/11/26: Etienne Lachance
46  -Use angle conditions only if it converge in inv_kin.
47 
48 2004/01/23: Etienne Lachance
49  -Added const in non reference argument for all functions.
50 
51 2004/03/12: Etienne Lachance
52  -Added logic to set q in inv_kin.
53 
54 2004/04/24: Etienne Lachance
55  -Moved inv_kin to invkine.cpp.
56 
57 2004/05/14: Etienne Lachance
58  -Replaced vec_x_prod by CrossProduct.
59 
60 2004/05/21: Etienne Lachance
61  -Added Doxygen comments.
62 
63 2004/07/01: Ethan Tira-Thompson
64  -Added support for newmat's use_namespace #define, using ROBOOP namespace
65 
66 2004/07/16: Ethan Tira-Thompson
67  -Supports Link::immobile flag so jacobians and deltas are 0 for immobile joints
68  -Jacobians will only contain entries for mobile joints - otherwise NaNs result
69  in later processing
70  -Added parameters to jacobian functions to generate for frames other than
71  the end effector
72 -------------------------------------------------------------------------------
73 */
74 
80 #include "robot.h"
81 
82 #ifdef use_namespace
83 namespace ROBOOP {
84  using namespace NEWMAT;
85 #endif
86 
87 
88 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos)const
94 {
95  kine(Rot,pos,dof+fix);
96 }
97 
98 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos, const int j)const
105 {
106  if(j < 1 || j > dof+fix) error("j must be 1 <= j <= dof+fix");
107 
108  Rot = links[1].R;
109  pos = links[1].p;
110  for (int i = 2; i <= j; i++) {
111  pos = pos + Rot*links[i].p;
112  Rot = Rot*links[i].R;
113  }
114 }
115 
116 ReturnMatrix Robot_basic::kine(void)const
118 {
119  Matrix thomo;
120 
121  thomo = kine(dof+fix);
122  thomo.Release(); return thomo;
123 }
124 
125 ReturnMatrix Robot_basic::kine(const int j)const
127 {
128  Matrix Rot, thomo(4,4);
129  ColumnVector pos;
130 
131  kine(Rot,pos,j);
132  thomo << fourbyfourident;
133  thomo.SubMatrix(1,3,1,3) = Rot;
134  thomo.SubMatrix(1,3,4,4) = pos;
135  thomo.Release(); return thomo;
136 }
137 
138 ReturnMatrix Robot_basic::kine_pd(const int j)const
149 {
150  Matrix temp(3,5), Rot;
151  ColumnVector pos, pos_dot;
152 
153  if(j < 1 || j > dof)
154  error("j must be 1 <= j <= dof");
155 
156  kine_pd(Rot, pos, pos_dot, j);
157 
158  temp.SubMatrix(1,3,1,3) = Rot;
159  temp.SubMatrix(1,3,4,4) = pos;
160  temp.SubMatrix(1,3,5,5) = pos_dot;
161  temp.Release();
162  return temp;
163 }
164 
165 ReturnMatrix Robot_basic::jacobian_DLS_inv(const double eps, const double lambda_max,
166  const int ref)const
196 {
197  Matrix jacob_inv_DLS, U, V;
198  DiagonalMatrix Q;
199  SVD(jacobian(ref), Q, U, V);
200 
201  if(Q(6,6) >= eps)
202  jacob_inv_DLS = V*Q.i()*U.t();
203  else
204  {
205  Q(6,6) += (1 - pow(Q(6,6)/eps,2))*lambda_max*lambda_max;
206  jacob_inv_DLS = V*Q.i()*U.t();
207  }
208 
209  jacob_inv_DLS.Release();
210  return(jacob_inv_DLS);
211 }
212 
213 // --------------------- R O B O T DH N O T A T I O N --------------------------
214 
215 void Robot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
216  const int j)const
225 {
226  if(j < 1 || j > dof)
227  error("j must be 1 <= j <= dof");
228 
229  if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
230  pos = ColumnVector(3);
231  if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
232  pos_dot = ColumnVector(3);
233 
234  pos = 0.0;
235  pos_dot = 0.0;
236  for(int i = 1; i <= j; i++)
237  {
238  R[i] = R[i-1]*links[i].R;
239  pos = pos + R[i-1]*links[i].p;
240  pos_dot = pos_dot + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
241  }
242  Rot = R[j];
243 }
244 
245 void Robot::dTdqi(Matrix & dRot, ColumnVector & dp,
246  const int i)
289 {
290  int j;
291  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
292  if(links[i].get_immobile()) {
293  dRot = Matrix(3,3);
294  dp = Matrix(3,1);
295  dRot = 0.0;
296  dp = 0.0;
297  } else if(links[i].get_joint_type() == 0) {
298  Matrix dR(3,3);
299  dR = 0.0;
300  Matrix R2 = links[i].R;
301  ColumnVector p2 = links[i].p;
302  dRot = Matrix(3,3);
303  dRot << threebythreeident;
304  for(j = 1; j < i; j++) {
305  dRot = dRot*links[j].R;
306  }
307  // dRot * Q
308  for(j = 1; j <= 3; j++) {
309  dR(j,1) = dRot(j,2);
310  dR(j,2) = -dRot(j,1);
311  }
312  for(j = i+1; j <= dof; j++) {
313  p2 = p2 + R2*links[j].p;
314  R2 = R2*links[j].R;
315  }
316  dp = dR*p2;
317  dRot = dR*R2;
318  } else {
319  dRot = Matrix(3,3);
320  dp = Matrix(3,1);
321  dRot = 0.0;
322  dp = 0.0;
323  dp(3) = 1.0;
324  for(j = i-1; j >= 1; j--) {
325  dp = links[j].R*dp;
326  }
327  }
328 }
329 
330 ReturnMatrix Robot::dTdqi(const int i)
337 {
338  Matrix dRot, thomo(4,4);
339  ColumnVector dpos;
340 
341  dTdqi(dRot, dpos, i);
342  thomo = (Real) 0.0;
343  thomo.SubMatrix(1,3,1,3) = dRot;
344  thomo.SubMatrix(1,3,4,4) = dpos;
345  thomo.Release(); return thomo;
346 }
347 
348 ReturnMatrix Robot::jacobian(const int endlink, const int ref)const
397 {
398  int i, j;
399  const int adof=get_available_dof(endlink);
400  Matrix jac(6,adof);
401  Matrix pr, temp(3,1);
402 
403  if(ref < 0 || ref > dof) error("invalid referential");
404 
405  for(i = 1; i <= dof; i++) {
406  R[i] = R[i-1]*links[i].R;
407  p[i] = p[i-1]+R[i-1]*links[i].p;
408  }
409 
410  for(i=1,j=1; j <= adof; i++) {
411  if(links[i].get_immobile())
412  continue;
413  if(links[i].get_joint_type() == 0) {
414  temp(1,1) = R[i-1](1,3);
415  temp(2,1) = R[i-1](2,3);
416  temp(3,1) = R[i-1](3,3);
417  pr = p[dof]-p[i-1];
418  temp = CrossProduct(temp,pr);
419  jac(1,j) = temp(1,1);
420  jac(2,j) = temp(2,1);
421  jac(3,j) = temp(3,1);
422  jac(4,j) = R[i-1](1,3);
423  jac(5,j) = R[i-1](2,3);
424  jac(6,j) = R[i-1](3,3);
425  } else {
426  jac(1,j) = R[i-1](1,3);
427  jac(2,j) = R[i-1](2,3);
428  jac(3,j) = R[i-1](3,3);
429  jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
430  }
431  j++;
432  }
433 
434  if(ref != 0) {
435  Matrix zeros(3,3);
436  zeros = (Real) 0.0;
437  Matrix RT = R[ref].t();
438  Matrix Rot;
439  Rot = ((RT & zeros) | (zeros & RT));
440  jac = Rot*jac;
441  }
442  jac.Release(); return jac;
443 }
444 
445 ReturnMatrix Robot::jacobian_dot(const int ref)const
493 {
494  int i, j;
495  const int adof=get_available_dof();
496  Matrix jacdot(6,adof);
497  ColumnVector e(3), temp, pr, ppr;
498 
499  if(ref < 0 || ref > dof)
500  error("invalid referential");
501 
502  for(i = 1; i <= dof; i++)
503  {
504  R[i] = R[i-1]*links[i].R;
505  p[i] = p[i-1] + R[i-1]*links[i].p;
506  pp[i] = pp[i-1] + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
507  }
508 
509  for(i=1,j=1; j <= adof; i++) {
510  if(links[i].get_immobile())
511  continue;
512  if(links[i].get_joint_type() == 0)
513  {
514  pr = p[dof]-p[i-1];
515  ppr = pp[dof]-pp[i-1];
516  e(1) = R[i-1](1,3);
517  e(2) = R[i-1](2,3);
518  e(3) = R[i-1](3,3);
519  temp = CrossProduct(R[i-1]*w[i-1], e);
520  jacdot(4,j) = temp(1); // d(e)/dt
521  jacdot(5,j) = temp(2);
522  jacdot(6,j) = temp(3);
523  temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
524  jacdot(1,j) = temp(1);
525  jacdot(2,j) = temp(2);
526  jacdot(3,j) = temp(3);
527  }
528  else
529  jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
530  jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
531  j++;
532  }
533 
534  if(ref != 0) {
535  Matrix zeros(3,3);
536  zeros = (Real) 0.0;
537  Matrix RT = R[ref].t();
538  Matrix Rot;
539  Rot = ((RT & zeros) | (zeros & RT));
540  jacdot = Rot*jacdot;
541  }
542 
543  jacdot.Release(); return jacdot;
544 }
545 
546 // ---------------- R O B O T M O D I F I E D DH N O T A T I O N --------------------------
547 
548 void mRobot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
549  const int j)const
558 {
559  if(j < 1 || j > dof+fix)
560  error("j must be 1 <= j <= dof+fix");
561 
562  if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
563  pos = ColumnVector(3);
564  if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
565  pos_dot = ColumnVector(3);
566 
567  pos = 0.0;
568  pos_dot = 0.0;
569  for(int i = 1; i <= j; i++)
570  {
571  pos = pos + R[i-1]*links[i].p;
572  pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
573  R[i] = R[i-1]*links[i].R;
574  }
575  Rot = R[j];
576 }
577 
578 void mRobot::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
614 {
615  int j;
616  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
617  if(links[i].get_immobile()) {
618  dRot = Matrix(3,3);
619  dp = Matrix(3,1);
620  dRot = 0.0;
621  dp = 0.0;
622  } else if(links[i].get_joint_type() == 0) {
623  Matrix dR(3,3), R2(3,3), p2(3,1);
624  dR = 0.0;
625  dRot = Matrix(3,3);
626  dRot << threebythreeident;
627  for(j = 1; j <= i; j++) {
628  dRot = dRot*links[j].R;
629  }
630  // dRot * Q
631  for(j = 1; j <= 3; j++) {
632  dR(j,1) = dRot(j,2);
633  dR(j,2) = -dRot(j,1);
634  }
635  if(i < dof) {
636  R2 = links[i+1].R;
637  p2 = links[i+1].p;
638  } else {
639  R2 << threebythreeident;
640  p2 = 0.0;
641  }
642  for(j = i+1; j <= dof; j++) {
643  p2 = p2 + R2*links[j].p;
644  R2 = R2*links[j].R;
645  }
646  dp = dR*p2;
647  dRot = dR*R2; // probleme ...
648  } else {
649  dRot = Matrix(3,3);
650  dp = Matrix(3,1);
651  dRot = 0.0;
652  dp = 0.0;
653  dp(3) = 1.0;
654  for(j = i; j >= 1; j--) {
655  dp = links[j].R*dp;
656  }
657  }
658 }
659 
660 ReturnMatrix mRobot::dTdqi(const int i)
667 {
668  Matrix dRot, thomo(4,4);
669  ColumnVector dpos;
670 
671  dTdqi(dRot, dpos, i);
672  thomo = (Real) 0.0;
673  thomo.SubMatrix(1,3,1,3) = dRot;
674  thomo.SubMatrix(1,3,4,4) = dpos;
675  thomo.Release(); return thomo;
676 }
677 
678 ReturnMatrix mRobot::jacobian(const int endlink, const int ref)const
684 {
685  int i, j;
686  const int adof=get_available_dof(endlink);
687  Matrix jac(6,adof);
688  ColumnVector pr(3), temp(3);
689 
690  if(ref < 0 || ref > dof+fix)
691  error("invalid referential");
692 
693  for(i = 1; i <= dof+fix; i++) {
694  R[i] = R[i-1]*links[i].R;
695  p[i] = p[i-1] + R[i-1]*links[i].p;
696  }
697 
698  for(i=1,j=1; j <= adof; i++) {
699  if(links[i].get_immobile())
700  continue;
701  if(links[i].get_joint_type() == 0){
702  temp(1) = R[i](1,3);
703  temp(2) = R[i](2,3);
704  temp(3) = R[i](3,3);
705  pr = p[dof+fix]-p[i];
706  temp = CrossProduct(temp,pr);
707  jac(1,j) = temp(1);
708  jac(2,j) = temp(2);
709  jac(3,j) = temp(3);
710  jac(4,j) = R[i](1,3);
711  jac(5,j) = R[i](2,3);
712  jac(6,j) = R[i](3,3);
713  } else {
714  jac(1,j) = R[i](1,3);
715  jac(2,j) = R[i](2,3);
716  jac(3,j) = R[i](3,3);
717  jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
718  }
719  j++;
720  }
721  if(ref != 0) {
722  Matrix zeros(3,3);
723  zeros = (Real) 0.0;
724  Matrix RT = R[ref].t();
725  Matrix Rot;
726  Rot = ((RT & zeros) | (zeros & RT));
727  jac = Rot*jac;
728  }
729  jac.Release(); return jac;
730 }
731 
732 ReturnMatrix mRobot::jacobian_dot(const int ref)const
738 {
739  int i, j;
740  const int adof=get_available_dof();
741  Matrix jacdot(6,adof);
742  ColumnVector e(3), temp, pr, ppr;
743 
744  if(ref < 0 || ref > dof+fix)
745  error("invalid referential");
746 
747  for(i = 1; i <= dof+fix; i++)
748  {
749  R[i] = R[i-1]*links[i].R;
750  p[i] = p[i-1] + R[i-1]*links[i].p;
751  pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
752  }
753 
754  for(i=1,j=1; j <= adof; i++) {
755  if(links[i].get_immobile())
756  continue;
757  if(links[i].get_joint_type() == 0)
758  {
759  pr = p[dof+fix]-p[i];
760  ppr = pp[dof+fix]-pp[i];
761 
762  e(1) = R[i](1,3);
763  e(2) = R[i](2,3);
764  e(3) = R[i](3,3);
765  temp = CrossProduct(R[i-1]*w[i-1], e);
766  jacdot(4,j) = temp(1); // d(e)/dt
767  jacdot(5,j) = temp(2);
768  jacdot(6,j) = temp(3);
769 
770  temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
771  jacdot(1,j) = temp(1);
772  jacdot(2,j) = temp(2);
773  jacdot(3,j) = temp(3);
774  }
775  else
776  jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
777  jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
778  j++;
779  }
780 
781  if(ref != 0) {
782  Matrix zeros(3,3);
783  zeros = (Real) 0.0;
784  Matrix RT = R[ref].t();
785  Matrix Rot;
786  Rot = ((RT & zeros) | (zeros & RT));
787  jacdot = Rot*jacdot;
788  }
789 
790  jacdot.Release(); return jacdot;
791 }
792 
793 // ------------- 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 ------------
794 
795 void mRobot_min_para::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
796  const int j)const
805 {
806  if(j < 1 || j > dof+fix)
807  error("j must be 1 <= j <= dof+fix");
808 
809  if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
810  pos = ColumnVector(3);
811  if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
812  pos_dot = ColumnVector(3);
813 
814  pos = 0.0;
815  pos_dot = 0.0;
816  for(int i = 1; i <= j; i++)
817  {
818  pos = pos + R[i-1]*links[i].p;
819  pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
820  R[i] = R[i-1]*links[i].R;
821  }
822  Rot = R[j];
823 }
824 
825 void mRobot_min_para::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
837 {
838  int j;
839  if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
840  if(links[i].get_immobile()) {
841  dRot = Matrix(3,3);
842  dp = Matrix(3,1);
843  dRot = 0.0;
844  dp = 0.0;
845  } else if(links[i].get_joint_type() == 0) {
846  Matrix dR(3,3), R2, p2(3,1);
847  dR = 0.0;
848  dRot = Matrix(3,3);
849  dRot << threebythreeident;
850  for(j = 1; j <= i; j++) {
851  dRot = dRot*links[j].R;
852  }
853  // dRot * Q
854  for(j = 1; j <= 3; j++) {
855  dR(j,1) = dRot(j,2);
856  dR(j,2) = -dRot(j,1);
857  }
858  if(i < dof) {
859  R2 = links[i+1].R;
860  p2 = links[i+1].p;
861  } else {
862  R2 << threebythreeident;
863  p2 = 0.0;
864  }
865  for(j = i+1; j <= dof; j++) {
866  p2 = p2 + R2*links[j].p;
867  R2 = R2*links[j].R;
868  }
869  dp = dR*p2;
870  dRot = dR*R2;
871  } else {
872  dRot = Matrix(3,3);
873  dp = Matrix(3,1);
874  dRot = 0.0;
875  dp = 0.0;
876  dp(3) = 1.0;
877  for(j = i; j >= 1; j--) {
878  dp = links[j].R*dp;
879  }
880  }
881 }
882 
883 ReturnMatrix mRobot_min_para::dTdqi(const int i)
890 {
891  Matrix dRot, thomo(4,4);
892  ColumnVector dpos;
893 
894  dTdqi(dRot, dpos, i);
895  thomo = (Real) 0.0;
896  thomo.SubMatrix(1,3,1,3) = dRot;
897  thomo.SubMatrix(1,3,4,4) = dpos;
898  thomo.Release(); return thomo;
899 }
900 
901 ReturnMatrix mRobot_min_para::jacobian(const int endlink, const int ref)const
907 {
908  int i, j;
909  const int adof=get_available_dof(endlink);
910  Matrix jac(6,adof);
911  ColumnVector pr(3), temp(3);
912 
913  if(ref < 0 || ref > dof+fix)
914  error("invalid referential");
915 
916  for(i = 1; i <= dof+fix; i++) {
917  R[i] = R[i-1]*links[i].R;
918  p[i] = p[i-1] + R[i-1]*links[i].p;
919  }
920 
921  for(i=1,j=1; j<=adof; i++) {
922  if(links[i].get_immobile())
923  continue;
924  if(links[i].get_joint_type() == 0){
925  temp(1) = R[i](1,3);
926  temp(2) = R[i](2,3);
927  temp(3) = R[i](3,3);
928 
929  pr = p[dof+fix]-p[i];
930  temp = CrossProduct(temp,pr);
931  jac(1,j) = temp(1);
932  jac(2,j) = temp(2);
933  jac(3,j) = temp(3);
934  jac(4,j) = R[i](1,3);
935  jac(5,j) = R[i](2,3);
936  jac(6,j) = R[i](3,3);
937  } else {
938  jac(1,j) = R[i](1,3);
939  jac(2,j) = R[i](2,3);
940  jac(3,j) = R[i](3,3);
941  jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
942  }
943  j++;
944  }
945  if(ref != 0) {
946  Matrix zeros(3,3);
947  zeros = (Real) 0.0;
948  Matrix RT = R[ref].t();
949  Matrix Rot;
950  Rot = ((RT & zeros) | (zeros & RT));
951  jac = Rot*jac;
952  }
953 
954  jac.Release(); return jac;
955 }
956 
957 ReturnMatrix mRobot_min_para::jacobian_dot(const int ref)const
963 {
964  int i, j;
965  const int adof=get_available_dof();
966  Matrix jacdot(6,adof);
967  ColumnVector e(3), temp, pr, ppr;
968 
969  if(ref < 0 || ref > dof+fix)
970  error("invalid referential");
971 
972  for(i = 1; i <= dof+fix; i++)
973  {
974  R[i] = R[i-1]*links[i].R;
975  p[i] = p[i-1] + R[i-1]*links[i].p;
976  pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
977  }
978 
979  for(i=1,j=1; j <= dof; i++) {
980  if(links[i].get_immobile())
981  continue;
982  if(links[i].get_joint_type() == 0)
983  {
984  pr = p[dof+fix]-p[i];
985  ppr = pp[dof+fix]-pp[i];
986 
987  e(1) = R[i](1,3);
988  e(2) = R[i](2,3);
989  e(3) = R[i](3,3);
990  temp = CrossProduct(R[i-1]*w[i-1], e);
991  jacdot(4,j) = temp(1); // d(e)/dt
992  jacdot(5,j) = temp(2);
993  jacdot(6,j) = temp(3);
994 
995  temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
996  jacdot(1,j) = temp(1);
997  jacdot(2,j) = temp(2);
998  jacdot(3,j) = temp(3);
999  }
1000  else
1001  jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
1002  jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
1003  j++;
1004  }
1005 
1006  if(ref != 0) {
1007  Matrix zeros(3,3);
1008  zeros = (Real) 0.0;
1009  Matrix RT = R[ref].t();
1010  Matrix Rot;
1011  Rot = ((RT & zeros) | (zeros & RT));
1012  jacdot = Rot*jacdot;
1013  }
1014 
1015  jacdot.Release(); return jacdot;
1016 }
1017 
1018 #ifdef use_namespace
1019 }
1020 #endif