joint.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
9 /*
10  * joint.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.10.21
12  */
13 
14 #include "chain.h"
15 
16 double Joint::TotalMass()
17 {
18  if(!child) return mass;
19  return (mass + child->total_mass());
20 }
21 
22 double Joint::total_mass()
23 {
24  if(!child) return mass;
25  double b = 0.0;
26  if(brother) b = brother->total_mass();
27  return mass + b + child->total_mass();
28 }
29 
30 void Joint::SetRotateJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai)
31 {
32  j_type = JROTATE;
33  if(t_given) n_dof = 1;
34  else n_thrust = 1;
35  init_pos.set(rpos);
36  init_att.set(ratt);
37  rel_pos.set(rpos);
38  rel_att.set(ratt);
39  axis.zero();
40  if(ai != AXIS_NULL) axis(ai) = 1.0;
41 }
42 
43 void Joint::SetSlideJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai)
44 {
45  j_type = JSLIDE;
46  if(t_given) n_dof = 1;
47  else n_thrust = 1;
48  init_pos.set(rpos);
49  init_att.set(ratt);
50  rel_pos.set(rpos);
51  rel_att.set(ratt);
52  axis.zero();
53  if(ai != AXIS_NULL) axis(ai) = 1.0;
54 }
55 
56 void Joint::SetFixedJointType(const fVec3& rpos, const fMat33& ratt)
57 {
58  j_type = JFIXED;
59  n_dof = 0;
60  n_thrust = 0;
61  init_pos.set(rpos);
62  init_att.set(ratt);
63  rel_pos.set(rpos);
64  rel_att.set(ratt);
65  rel_ep.set(rel_att);
66 }
67 
68 void Joint::SetSphereJointType(const fVec3& rpos, const fMat33& ratt)
69 {
70  j_type = JSPHERE;
71  if(t_given) n_dof = 3;
72  else n_thrust = 3;
73  init_pos.set(rpos);
74  init_att.set(ratt);
75  rel_pos.set(rpos);
76  rel_att.set(ratt);
77  rel_ep.set(rel_att);
78 }
79 
80 void Joint::SetFreeJointType(const fVec3& rpos, const fMat33& ratt)
81 {
82  j_type = JFREE;
83  if(t_given) n_dof = 6;
84  else n_thrust = 6;
85  init_pos.set(rpos);
86  init_att.set(ratt);
87  rel_pos.set(rpos);
88  rel_att.set(ratt);
89  rel_ep.set(rel_att);
90 }
91 
92 void Joint::UpdateJointType()
93 {
94  n_dof = 0;
95  n_thrust = 0;
96  switch(j_type)
97  {
98  case JROTATE:
99  case JSLIDE:
100  if(t_given) n_dof = 1;
101  else n_thrust = 1;
102  break;
103  case JSPHERE:
104  if(t_given) n_dof = 3;
105  else n_thrust = 3;
106  break;
107  case JFREE:
108  if(t_given) n_dof = 6;
109  else n_thrust = 6;
110  break;
111  case JFIXED:
112  break;
113  default:
114  break;
115  }
116  init_pos.set(rel_pos);
117  init_att.set(rel_att);
118  rel_ep.set(rel_att);
119 }
120 
121 /*
122  * get joint values, vels, accs
123  */
124 int Chain::GetJointValue(fVec& values)
125 {
126  values.resize(n_value);
127  root->get_joint_value(values);
128  return 0;
129 }
130 
131 int Joint::get_joint_value(fVec& values)
132 {
133  static fVec3 p;
134  static fMat33 r;
135  static fEulerPara ep;
136  if(i_value >= 0)
137  {
138  switch(j_type)
139  {
140  case JROTATE:
141  case JSLIDE:
142  GetJointValue(values(i_value));
143  break;
144  case JSPHERE:
145  GetJointValue(r);
146  ep.set(r);
147  values(i_value) = ep(0);
148  values(i_value+1) = ep(1);
149  values(i_value+2) = ep(2);
150  values(i_value+3) = ep(3);
151  break;
152  case JFREE:
153  GetJointValue(p, r);
154  ep.set(r);
155  values(i_value) = p(0);
156  values(i_value+1) = p(1);
157  values(i_value+2) = p(2);
158  values(i_value+3) = ep(0);
159  values(i_value+4) = ep(1);
160  values(i_value+5) = ep(2);
161  values(i_value+6) = ep(3);
162  break;
163  default:
164  break;
165  }
166  }
167  child->get_joint_value(values);
168  brother->get_joint_value(values);
169  return 0;
170 }
171 
172 int Chain::GetJointVel(fVec& vels)
173 {
174  vels.resize(n_dof);
175  root->get_joint_vel(vels);
176  return 0;
177 }
178 
179 int Joint::get_joint_vel(fVec& vels)
180 {
181  static fVec3 pd, rd;
182  if(i_dof >= 0)
183  {
184  switch(j_type)
185  {
186  case JROTATE:
187  case JSLIDE:
188  GetJointVel(vels(i_dof));
189  break;
190  case JSPHERE:
191  GetJointVel(rd);
192  vels(i_dof) = rd(0);
193  vels(i_dof+1) = rd(1);
194  vels(i_dof+2) = rd(2);
195  break;
196  case JFREE:
197  GetJointVel(pd, rd);
198  vels(i_dof) = pd(0);
199  vels(i_dof+1) = pd(1);
200  vels(i_dof+2) = pd(2);
201  vels(i_dof+3) = rd(0);
202  vels(i_dof+4) = rd(1);
203  vels(i_dof+5) = rd(2);
204  break;
205  default:
206  break;
207  }
208  }
209  brother->get_joint_vel(vels);
210  child->get_joint_vel(vels);
211  return 0;
212 }
213 
214 int Chain::GetJointAcc(fVec& accs)
215 {
216  accs.resize(n_dof);
217  root->get_joint_acc(accs);
218  return 0;
219 }
220 
221 int Joint::get_joint_acc(fVec& accs)
222 {
223  static fVec3 pdd, rdd;
224  if(i_dof >= 0)
225  {
226  switch(j_type)
227  {
228  case JROTATE:
229  case JSLIDE:
230  GetJointAcc(accs(i_dof));
231  break;
232  case JSPHERE:
233  GetJointAcc(rdd);
234  accs(i_dof) = rdd(0);
235  accs(i_dof+1) = rdd(1);
236  accs(i_dof+2) = rdd(2);
237  break;
238  case JFREE:
239  GetJointAcc(pdd, rdd);
240  accs(i_dof) = pdd(0);
241  accs(i_dof+1) = pdd(1);
242  accs(i_dof+2) = pdd(2);
243  accs(i_dof+3) = rdd(0);
244  accs(i_dof+4) = rdd(1);
245  accs(i_dof+5) = rdd(2);
246  break;
247  default:
248  break;
249  }
250  }
251  brother->get_joint_acc(accs);
252  child->get_joint_acc(accs);
253  return 0;
254 }
255 
256 int Joint::GetJointValue(double& _q)
257 {
258  if(j_type == JROTATE || j_type == JSLIDE)
259  {
260  _q = q;
261  }
262  else
263  {
264  return -1;
265  }
266  return 0;
267 }
268 
269 int Joint::GetJointVel(double& _qd)
270 {
271  if(j_type == JROTATE || j_type == JSLIDE)
272  {
273  _qd = qd;
274  }
275  else
276  {
277  return -1;
278  }
279  return 0;
280 }
281 
282 int Joint::GetJointAcc(double& _qdd)
283 {
284  if(j_type == JROTATE || j_type == JSLIDE)
285  {
286  _qdd = qdd;
287  }
288  else
289  {
290  return -1;
291  }
292  return 0;
293 }
294 
295 int Joint::GetJointValue(fMat33& r)
296 {
297  if(j_type == JSPHERE || j_type == JFREE)
298  {
299  r.set(rel_att);
300  }
301  else
302  {
303  return -1;
304  }
305  return 0;
306 }
307 
308 int Joint::GetJointVel(fVec3& rd)
309 {
310  if(j_type == JSPHERE || j_type == JFREE)
311  {
312  rd.set(rel_ang_vel);
313  }
314  else
315  {
316  return -1;
317  }
318  return 0;
319 }
320 
321 int Joint::GetJointAcc(fVec3& rdd)
322 {
323  if(j_type == JSPHERE || j_type == JFREE)
324  {
325  rdd.set(rel_ang_acc);
326  }
327  else
328  {
329  return -1;
330  }
331  return 0;
332 }
333 
334 int Joint::GetJointValue(fVec3& p, fMat33& r)
335 {
336  if(j_type == JFREE || j_type == JFIXED)
337  {
338  p.set(rel_pos);
339  r.set(rel_att);
340  }
341  else
342  {
343  return -1;
344  }
345  return 0;
346 }
347 
348 int Joint::GetJointVel(fVec3& pd, fVec3& rd)
349 {
350  if(j_type == JFREE)
351  {
352  pd.set(rel_lin_vel);
353  rd.set(rel_ang_vel);
354  }
355  else
356  {
357  return -1;
358  }
359  return 0;
360 }
361 
362 int Joint::GetJointAcc(fVec3& pdd, fVec3& rdd)
363 {
364  if(j_type == JFREE)
365  {
366  pdd.set(rel_lin_acc);
367  rdd.set(rel_ang_acc);
368  }
369  else
370  {
371  return -1;
372  }
373  return 0;
374 }
375 
376 /*
377  * set joint position/orientation
378  */
379 int Joint::SetJointPosition(const fVec3& p)
380 {
381  rel_pos.set(p);
382  return 0;
383 }
384 
385 int Joint::SetJointOrientation(const fMat33& r)
386 {
387  rel_att.set(r);
388  rel_ep.set(r);
389  return 0;
390 }
391 
392 /*
393  * compute relative from absolute position/orientation
394  * call CalcPosition() first
395  */
396 int Chain::set_abs_position_orientation(Joint* jnt, const fVec3& abs_pos, const fMat33& abs_att)
397 {
398  if(!jnt->parent) return -1;
399  fVec3 p_pos(jnt->parent->abs_pos);
400  fMat33 p_att(jnt->parent->abs_att);
401  static fVec3 pp, r_pos;
402  static fMat33 tR, r_att;
403  pp.sub(abs_pos, p_pos);
404  tR.tran(p_att);
405  r_pos.mul(tR, pp);
406  r_att.mul(tR, abs_att);
407  jnt->SetJointPosition(r_pos);
408  jnt->SetJointOrientation(r_att);
409  return 0;
410 }
411 
412 /*
413  * set joint values, vels, accs
414  */
415 int Chain::SetJointValue(const fVec& values)
416 {
417  if(values.size() != n_value)
418  {
419  cerr << "Chain::SetJointValue: error - size of the argument should be n_value (" << n_value << ")" << endl;
420  return -1;
421  }
422  root->set_joint_value(values);
423  return 0;
424 }
425 
426 int Joint::set_joint_value(const fVec& values)
427 {
428  static fVec3 p;
429  static fEulerPara ep;
430  if(i_value >= 0)
431  {
432  switch(j_type)
433  {
434  case JROTATE:
435  case JSLIDE:
436  SetJointValue(values(i_value));
437  break;
438  case JSPHERE:
439  ep.set(values(i_value), values(i_value+1), values(i_value+2), values(i_value+3));
440  SetJointValue(ep);
441  break;
442  case JFREE:
443  p.set(values(i_value), values(i_value+1), values(i_value+2));
444  ep.set(values(i_value+3), values(i_value+4), values(i_value+5), values(i_value+6));
445  SetJointValue(p, ep);
446  break;
447  default:
448  break;
449  }
450  }
451  child->set_joint_value(values);
452  brother->set_joint_value(values);
453  return 0;
454 }
455 
456 int Chain::SetJointVel(const fVec& vels)
457 {
458  if(vels.size() != n_dof)
459  {
460  cerr << "Chain::SetJointVel: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
461  return -1;
462  }
463  root->set_joint_vel(vels);
464  return 0;
465 }
466 
467 int Joint::set_joint_vel(const fVec& vels)
468 {
469  static fVec3 pd;
470  static fVec3 rd;
471  if(i_dof >= 0)
472  {
473  switch(j_type)
474  {
475  case JROTATE:
476  case JSLIDE:
477  SetJointVel(vels(i_dof));
478  break;
479  case JSPHERE:
480  rd.set(vels(i_dof), vels(i_dof+1), vels(i_dof+2));
481  SetJointVel(rd);
482  break;
483  case JFREE:
484  pd.set(vels(i_dof), vels(i_dof+1), vels(i_dof+2));
485  rd.set(vels(i_dof+3), vels(i_dof+4), vels(i_dof+5));
486  SetJointVel(pd, rd);
487  break;
488  default:
489  break;
490  }
491  }
492  child->set_joint_vel(vels);
493  brother->set_joint_vel(vels);
494  return 0;
495 }
496 
497 int Chain::SetJointAcc(const fVec& accs)
498 {
499  if(accs.size() != n_dof)
500  {
501  cerr << "Chain::SetJointAcc: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
502  return -1;
503  }
504  root->set_joint_acc(accs);
505  return 0;
506 }
507 
508 int Joint::set_joint_acc(const fVec& accs)
509 {
510  static fVec3 pdd;
511  static fVec3 rdd;
512  if(i_dof >= 0)
513  {
514  switch(j_type)
515  {
516  case JROTATE:
517  case JSLIDE:
518  SetJointAcc(accs(i_dof));
519  break;
520  case JSPHERE:
521  rdd.set(accs(i_dof), accs(i_dof+1), accs(i_dof+2));
522  SetJointAcc(rdd);
523  break;
524  case JFREE:
525  pdd.set(accs(i_dof), accs(i_dof+1), accs(i_dof+2));
526  rdd.set(accs(i_dof+3), accs(i_dof+4), accs(i_dof+5));
527  SetJointAcc(pdd, rdd);
528  break;
529  default:
530  break;
531  }
532  }
533  child->set_joint_acc(accs);
534  brother->set_joint_acc(accs);
535  return 0;
536 }
537 
538 int Joint::SetJointValue(double _q)
539 {
540  q = _q;
541  if(j_type == JROTATE)
542  {
543  static fMat33 tmp;
544  tmp.rot2mat(axis, q);
545  rel_att.mul(init_att, tmp);
546  rel_ep.set(rel_att);
547  }
548  else if(j_type == JSLIDE)
549  {
550  static fVec3 tmp, tmp1;
551  tmp.mul(q, axis);
552  tmp1.mul(rel_att, tmp);
553  rel_pos.add(tmp1, init_pos);
554  }
555  else
556  {
557  return -1;
558  }
559  return 0;
560 }
561 
562 int Joint::SetJointVel(double _qd)
563 {
564  qd = _qd;
565  if(j_type == JROTATE)
566  {
567  rel_ang_vel.mul(qd, axis);
568  }
569  else if(j_type == JSLIDE)
570  {
571  rel_lin_vel.mul(qd, axis);
572  }
573  else
574  {
575  return -1;
576  }
577  return 0;
578 }
579 
580 int Joint::SetJointAcc(double _qdd)
581 {
582  qdd = _qdd;
583  if(j_type == JROTATE)
584  {
585  rel_ang_acc.mul(qdd, axis);
586  }
587  else if(j_type == JSLIDE)
588  {
589  rel_lin_acc.mul(qdd, axis);
590  }
591  else
592  {
593  return -1;
594  }
595  return 0;
596 }
597 
598 int Joint::SetJointValue(const fMat33& r)
599 {
600  if(j_type == JSPHERE || j_type == JFREE)
601  {
602  rel_att.set(r);
603  rel_ep.set(rel_att);
604  }
605  else
606  {
607  return -1;
608  }
609  return 0;
610 }
611 
612 int Joint::SetJointValue(const fEulerPara& ep)
613 {
614  if(j_type == JSPHERE || j_type == JFREE)
615  {
616  rel_ep.set(ep);
617  rel_att.set(rel_ep);
618  }
619  else
620  {
621  return -1;
622  }
623  return 0;
624 }
625 
626 int Joint::SetJointVel(const fVec3& rd)
627 {
628  if(j_type == JSPHERE || j_type == JFREE)
629  {
630  rel_ang_vel.set(rd);
631  }
632  else
633  {
634  return -1;
635  }
636  return 0;
637 }
638 
639 int Joint::SetJointAcc(const fVec3& rdd)
640 {
641  if(j_type == JSPHERE || j_type == JFREE)
642  {
643  rel_ang_acc.set(rdd);
644  }
645  else
646  {
647  return -1;
648  }
649  return 0;
650 }
651 
652 int Joint::SetJointAcc(double ax, double ay, double az)
653 {
654  if(j_type == JSPHERE || j_type == JFREE)
655  {
656  rel_ang_acc(0) = ax;
657  rel_ang_acc(1) = ay;
658  rel_ang_acc(2) = az;
659  }
660  else
661  {
662  return -1;
663  }
664  return 0;
665 }
666 
667 int Joint::SetJointValue(const fVec3& p, const fMat33& r)
668 {
669  if(j_type == JFREE || j_type == JFIXED)
670  {
671  rel_pos.set(p);
672  rel_att.set(r);
673  rel_ep.set(rel_att);
674  }
675  else
676  {
677  return -1;
678  }
679  return 0;
680 }
681 
682 int Joint::SetJointValue(const fVec3& p, const fEulerPara& ep)
683 {
684  if(j_type == JFREE || j_type == JFIXED)
685  {
686  rel_pos.set(p);
687  rel_ep.set(ep);
688  rel_att.set(rel_ep);
689  }
690  else
691  {
692  return -1;
693  }
694  return 0;
695 }
696 
697 int Joint::SetJointVel(const fVec3& pd, const fVec3& rd)
698 {
699  if(j_type == JFREE)
700  {
701  rel_lin_vel.set(pd);
702  rel_ang_vel.set(rd);
703  }
704  else
705  {
706  return -1;
707  }
708  return 0;
709 }
710 
711 int Joint::SetJointAcc(const fVec3& pdd, const fVec3& rdd)
712 {
713  if(j_type == JFREE)
714  {
715  rel_lin_acc.set(pdd);
716  rel_ang_acc.set(rdd);
717  }
718  else
719  {
720  return -1;
721  }
722  return 0;
723 }
724 
725 int Joint::SetJointAcc(double lx, double ly, double lz, double ax, double ay, double az)
726 {
727  if(j_type == JFREE)
728  {
729  rel_lin_acc(0) = lx;
730  rel_lin_acc(1) = ly;
731  rel_lin_acc(2) = lz;
732  rel_ang_acc(0) = ax;
733  rel_ang_acc(1) = ay;
734  rel_ang_acc(2) = az;
735  }
736  else
737  {
738  return -1;
739  }
740  return 0;
741 }
742 
743 /*
744  * set joint froce/torque
745  */
746 int Chain::SetJointForce(const fVec& forces)
747 {
748  if(forces.size() != n_dof)
749  {
750  cerr << "Chain::SetJointForce: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
751  return -1;
752  }
753  root->set_joint_force(forces);
754  return 0;
755 }
756 
757 int Joint::set_joint_force(const fVec& forces)
758 {
759  if(i_dof >= 0)
760  {
761  switch(j_type)
762  {
763  case JROTATE:
764  case JSLIDE:
765  SetJointForce(forces(i_dof));
766  break;
767  case JSPHERE:
768  SetJointForce(fVec3(forces(i_dof), forces(i_dof+1), forces(i_dof+2)));
769  break;
770  case JFREE:
771  SetJointForce(fVec3(forces(i_dof), forces(i_dof+1), forces(i_dof+2)),
772  fVec3(forces(i_dof+3), forces(i_dof+4), forces(i_dof+5)));
773  break;
774  default:
775  break;
776  }
777  }
778  child->set_joint_force(forces);
779  brother->set_joint_force(forces);
780  return 0;
781 }
782 
783 int Joint::SetJointForce(double _tau)
784 {
785  if(j_type == JROTATE || j_type == JSLIDE)
786  {
787  tau = _tau;
788  }
789  else
790  {
791  return -1;
792  }
793  return 0;
794 }
795 
796 int Joint::GetJointForce(double& _tau)
797 {
798  if(j_type == JROTATE || j_type == JSLIDE)
799  {
800  _tau = tau;
801  }
802  else
803  {
804  return -1;
805  }
806  return 0;
807 }
808 
809 int Joint::SetJointForce(const fVec3& _n3)
810 {
811  if(j_type == JSPHERE)
812  {
813  tau_n.set(_n3);
814  }
815  else
816  {
817  return -1;
818  }
819  return 0;
820 }
821 
822 int Joint::GetJointForce(fVec3& _n3)
823 {
824  if(j_type == JSPHERE)
825  {
826  _n3.set(tau_n);
827  }
828  else
829  {
830  return -1;
831  }
832  return 0;
833 }
834 
835 int Joint::SetJointForce(const fVec3& _f3, const fVec3& _n3)
836 {
837  if(j_type == JFREE)
838  {
839  tau_f.set(_f3);
840  tau_n.set(_n3);
841  }
842  else
843  {
844  return -1;
845  }
846  return 0;
847 }
848 
849 int Joint::GetJointForce(fVec3& _f3, fVec3& _n3)
850 {
851  if(j_type == JFREE)
852  {
853  _f3.set(tau_f);
854  _n3.set(tau_n);
855  }
856  else
857  {
858  return -1;
859  }
860  return 0;
861 }
862 
863 /*
864  * get joint force/torque
865  */
866 int Chain::GetJointForce(fVec& forces)
867 {
868  if(forces.size() != n_dof)
869  {
870  cerr << "Chain::GetJointForce: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
871  return -1;
872  }
873  root->get_joint_force(forces);
874  return 0;
875 }
876 
877 int Joint::get_joint_force(fVec& forces)
878 {
879  if(i_dof >= 0)
880  {
881  switch(j_type)
882  {
883  case JROTATE:
884  case JSLIDE:
885  forces(i_dof) = tau;
886  break;
887  case JSPHERE:
888  forces(i_dof) = tau_n(0);
889  forces(i_dof+1) = tau_n(1);
890  forces(i_dof+2) = tau_n(2);
891  break;
892  case JFREE:
893  forces(i_dof) = tau_f(0);
894  forces(i_dof+1) = tau_f(1);
895  forces(i_dof+2) = tau_f(2);
896  forces(i_dof+3) = tau_n(0);
897  forces(i_dof+4) = tau_n(1);
898  forces(i_dof+5) = tau_n(2);
899  break;
900  default:
901  break;
902  }
903  }
904  child->get_joint_force(forces);
905  brother->get_joint_force(forces);
906  return 0;
907 }
908 
909 /*
910  * clear joint force/torque and external force/moment
911  */
912 int Chain::ClearJointForce()
913 {
914  root->clear_joint_force();
915  return 0;
916 }
917 
918 int Joint::clear_joint_force()
919 {
920  tau = 0.0;
921  tau_f.zero();
922  tau_n.zero();
923  brother->clear_joint_force();
924  child->clear_joint_force();
925  return 0;
926 }
927 
928 int Chain::ClearExtForce()
929 {
930  root->clear_ext_force();
931  return 0;
932 }
933 
934 int Joint::clear_ext_force()
935 {
936  ext_force.zero();
937  ext_moment.zero();
938  brother->clear_ext_force();
939  child->clear_ext_force();
940  return 0;
941 }
fVec3::add
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
Chain::ClearJointForce
int ClearJointForce()
Clear the joint forces/torques.
Definition: joint.cpp:912
fMat33
3x3 matrix class.
Definition: fMatrix3.h:29
Joint::mass
double mass
mass
Definition: chain.h:704
Joint::UpdateJointType
void UpdateJointType()
Definition: joint.cpp:92
Joint::axis
fVec3 axis
joint axis in local frame (for 1DOF joints)
Definition: chain.h:696
Joint::SetJointPosition
int SetJointPosition(const fVec3 &p)
Set relative position.
Definition: joint.cpp:379
Joint::child
Joint * child
pointer to the child joint
Definition: chain.h:685
Chain::SetJointValue
int SetJointValue(const fVec &values)
Set all joint values.
Definition: joint.cpp:415
Joint::n_dof
int n_dof
degrees of freedom (0/1/3/6)
Definition: chain.h:715
fVec3::zero
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
Joint::set_joint_vel
int set_joint_vel(const fVec &vels)
Definition: joint.cpp:467
Joint::t_given
int t_given
torque or motion controlled
Definition: chain.h:709
fMat33::set
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
Joint::total_mass
double total_mass()
Definition: joint.cpp:22
fMat33::mul
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
Joint::n_thrust
int n_thrust
DOF for motion controlled joints.
Definition: chain.h:716
fMat33::tran
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
Joint::init_pos
fVec3 init_pos
origin of the joint value (for prismatic joints)
Definition: chain.h:697
Joint::SetFixedJointType
void SetFixedJointType(const fVec3 &rpos, const fMat33 &ratt)
Set joint type to fixed.
Definition: joint.cpp:56
Chain::SetJointAcc
int SetJointAcc(const fVec &accs)
Definition: joint.cpp:497
fVec3::set
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
Chain::GetJointVel
int GetJointVel(fVec &vels)
Definition: joint.cpp:172
Chain::SetJointVel
int SetJointVel(const fVec &vels)
Set all joint velocities/accelerations.
Definition: joint.cpp:456
b
long b
Definition: jpegint.h:371
Joint::rel_ep
fEulerPara rel_ep
Euler parameter representation of rel_att (for 0/3/6 DOF joints)
Definition: chain.h:702
fEulerPara::set
void set(const fVec3 &v, double s)
Set the elements.
Definition: fEulerPara.h:75
Joint::SetJointAcc
int SetJointAcc(double _qdd)
Definition: joint.cpp:580
Joint::q
double q
joint value (for 1DOF joints)
Definition: chain.h:724
Joint::tau
double tau
joint force/torque (for 1DOF joints)
Definition: chain.h:733
Joint::TotalMass
double TotalMass()
Returns the total mass of the child links.
Definition: joint.cpp:16
Chain::GetJointAcc
int GetJointAcc(fVec &accs)
Definition: joint.cpp:214
JSPHERE
spherical (3DOF)
Definition: chain.h:43
fEulerPara
Euler parameter class.
Definition: fEulerPara.h:28
fMat33::rot2mat
void rot2mat(const fVec3 &, double)
Converts from/to equivalent rotation axis and angle.
Definition: fMatrix3.cpp:362
Joint::rel_pos
fVec3 rel_pos
(initial) position in parent joint&#39;s frame (for 0/3/6 DOF joints)
Definition: chain.h:700
Joint::set_joint_force
int set_joint_force(const fVec &forces)
Definition: joint.cpp:757
Joint::rel_ang_acc
fVec3 rel_ang_acc
Definition: chain.h:731
Joint::GetJointValue
int GetJointValue(double &_q)
Definition: joint.cpp:256
Joint::i_dof
int i_dof
index in all DOF
Definition: chain.h:720
Joint::SetJointForce
int SetJointForce(double _tau)
Definition: joint.cpp:783
Joint::get_joint_acc
int get_joint_acc(fVec &accs)
Definition: joint.cpp:221
Joint::clear_ext_force
int clear_ext_force()
Definition: joint.cpp:934
Joint::qd
double qd
joint velocity (for 1DOF joints)
Definition: chain.h:725
Joint::tau_n
fVec3 tau_n
joint torque for 3 and 6 DOF joints
Definition: chain.h:735
Joint::j_type
JointType j_type
joint type
Definition: chain.h:694
Joint::rel_ang_vel
fVec3 rel_ang_vel
Definition: chain.h:729
Joint::ext_force
fVec3 ext_force
external force
Definition: chain.h:736
Joint::abs_att
fMat33 abs_att
absolute orientation
Definition: chain.h:742
Joint::get_joint_force
int get_joint_force(fVec &forces)
Definition: joint.cpp:877
Joint::SetSlideJointType
void SetSlideJointType(const fVec3 &rpos, const fMat33 &ratt, AxisIndex ai)
Set joint type to prismatic.
Definition: joint.cpp:43
Joint::tau_f
fVec3 tau_f
joint force for 6DOF joints
Definition: chain.h:734
Joint::SetSphereJointType
void SetSphereJointType(const fVec3 &rpos, const fMat33 &ratt)
Set joint type to spherical.
Definition: joint.cpp:68
fVec::resize
void resize(int i)
Change the size.
Definition: fMatrix.h:511
Joint::init_att
fMat33 init_att
origin of the joint value (for rotational joints)
Definition: chain.h:698
fVec
Vector of generic size.
Definition: fMatrix.h:491
Joint::brother
Joint * brother
pointer to the brother joint
Definition: chain.h:684
Joint::set_joint_acc
int set_joint_acc(const fVec &accs)
Definition: joint.cpp:508
JFIXED
fixed (0DOF)
Definition: chain.h:40
chain.h
Classes for defining open/closed kinematic chains.
Chain::GetJointForce
int GetJointForce(fVec &forces)
Obtain the joint forces/torques.
Definition: joint.cpp:866
Joint::get_joint_value
int get_joint_value(fVec &values)
Definition: joint.cpp:131
Joint::SetRotateJointType
void SetRotateJointType(const fVec3 &rpos, const fMat33 &ratt, AxisIndex ai)
Set joint type to rotational.
Definition: joint.cpp:30
JSLIDE
prismatic (1DOF)
Definition: chain.h:42
Joint::rel_att
fMat33 rel_att
(initial) orientation in parent joint&#39;s frame (for 0/3/6 DOF joints)
Definition: chain.h:701
Joint::rel_lin_vel
fVec3 rel_lin_vel
Definition: chain.h:728
Joint::SetJointOrientation
int SetJointOrientation(const fMat33 &r)
Set relative orientation.
Definition: joint.cpp:385
Joint::GetJointAcc
int GetJointAcc(double &_qdd)
Definition: joint.cpp:282
Chain::SetJointForce
int SetJointForce(const fVec &forces)
Set all joint forces/torques.
Definition: joint.cpp:746
fVec3::sub
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
Joint::SetJointValue
int SetJointValue(double _q)
Definition: joint.cpp:538
Joint::GetJointForce
int GetJointForce(double &_tau)
Definition: joint.cpp:796
Chain::GetJointValue
int GetJointValue(fVec &values)
Obtain the joint values/velocities/accelerations.
Definition: joint.cpp:124
Joint::qdd
double qdd
joint acceleration (for 1DOF joints)
Definition: chain.h:726
AxisIndex
AxisIndex
Direction of a 1-DOF joint.
Definition: chain.h:48
fVec3
3-element vector class.
Definition: fMatrix3.h:206
Joint::clear_joint_force
int clear_joint_force()
Definition: joint.cpp:918
Joint::ext_moment
fVec3 ext_moment
external moment around the local frame
Definition: chain.h:737
JROTATE
rotational (1DOF)
Definition: chain.h:41
Joint::set_joint_value
int set_joint_value(const fVec &values)
Definition: joint.cpp:426
Joint::abs_pos
fVec3 abs_pos
absolute position
Definition: chain.h:741
fVec3::mul
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
Chain::ClearExtForce
int ClearExtForce()
Clear the external forces/torques.
Definition: joint.cpp:928
Joint
The class for representing a joint.
Definition: chain.h:538
Joint::get_joint_vel
int get_joint_vel(fVec &vels)
Definition: joint.cpp:179
Chain::set_abs_position_orientation
int set_abs_position_orientation(Joint *jnt, const fVec3 &abs_pos, const fMat33 &abs_att)
Definition: joint.cpp:396
fVec::size
int size() const
Size of the vector (same as row()).
Definition: fMatrix.h:507
Joint::rel_lin_acc
fVec3 rel_lin_acc
Definition: chain.h:730
Joint::GetJointVel
int GetJointVel(double &_qd)
Definition: joint.cpp:269
Joint::SetJointVel
int SetJointVel(double _qd)
Definition: joint.cpp:562
Joint::parent
Joint * parent
pointer to the parent joint
Definition: chain.h:683
JFREE
free (6DOF)
Definition: chain.h:44
Joint::i_value
int i_value
index in all joint values
Definition: chain.h:719
Joint::SetFreeJointType
void SetFreeJointType(const fVec3 &rpos, const fMat33 &ratt)
Set joint type to free.
Definition: joint.cpp:80

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:23