id.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  * id.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.08.29
12  */
13 
14 #include "chain.h"
15 
16 void Chain::InvDyn(fVec& tau)
17 {
19  root->inv_dyn();
20  root->calc_joint_force(tau);
21 }
22 
24 {
25  inv_dyn_1();
26  child->inv_dyn();
27  brother->inv_dyn();
28  inv_dyn_2();
29  // cerr << name << ": force = " << joint_f << joint_n << endl;
30 }
31 
33 {
34  // compute total force around com
35  static fVec3 v1, v2;
36  total_f.mul(mass, loc_com_acc);
37  v1.mul(inertia, loc_ang_vel);
38  v2.cross(loc_ang_vel, v1);
39  total_n.mul(inertia, loc_ang_acc);
40  total_n += v2;
41 
42  joint_f.zero();
43  joint_n.zero();
44 }
45 
47 {
48  static fVec3 v1;
49  joint_f += total_f;
50  joint_n += total_n;
51  v1.cross(loc_com, joint_f);
52  joint_n += v1;
53  // external force/moment
54  joint_f -= ext_force;
55  joint_n -= ext_moment;
56 // cout << name << ": joint_f = " << joint_f << ", joint_n = " << joint_n << endl;
57  if(parent)
58  {
59  // force
60  v1.mul(rel_att, joint_f);
61  parent->joint_f += v1;
62  // moment
63  v1.mul(rel_att, joint_n);
64  parent->joint_n += v1;
65  static fVec3 p, f;
66  f.mul(rel_att, joint_f);
67  p.sub(parent->loc_com, rel_pos);
68  v1.cross(p, f);
69  parent->joint_n -= v1;
70  }
71 }
72 
74 {
75  double t;
76 // cerr << name << ": force = " << joint_f << joint_n << endl;
77  if(i_dof >= 0)
78  {
79  switch(j_type)
80  {
81  case JROTATE:
82  t = axis * joint_n;
83  tau(i_dof) = t;
84  break;
85  case JSLIDE:
86  t = axis * joint_f;
87  tau(i_dof) = t;
88  break;
89  case JSPHERE:
90  tau(i_dof+0) = joint_n(0);
91  tau(i_dof+1) = joint_n(1);
92  tau(i_dof+2) = joint_n(2);
93  break;
94  case JFREE:
95  tau(i_dof+0) = joint_f(0);
96  tau(i_dof+1) = joint_f(1);
97  tau(i_dof+2) = joint_f(2);
98  tau(i_dof+3) = joint_n(0);
99  tau(i_dof+4) = joint_n(1);
100  tau(i_dof+5) = joint_n(2);
101  break;
102  default:
103  break;
104  }
105  }
106  child->calc_joint_force(tau);
107  brother->calc_joint_force(tau);
108 }
109 
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
Joint * root
Chain information.
Definition: chain.h:466
void calc_joint_force(fVec &tau)
Definition: id.cpp:73
void inv_dyn()
Definition: id.cpp:23
spherical (3DOF)
Definition: chain.h:43
void inv_dyn_2()
Definition: id.cpp:46
t
Vector of generic size.
Definition: fMatrix.h:491
Classes for defining open/closed kinematic chains.
prismatic (1DOF)
Definition: chain.h:42
void CalcAcceleration()
Definition: fk.cpp:75
void inv_dyn_1()
Definition: id.cpp:32
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
3-element vector class.
Definition: fMatrix3.h:206
rotational (1DOF)
Definition: chain.h:41
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
free (6DOF)
Definition: chain.h:44
void InvDyn(fVec &tau)
Inverse dynamics.
Definition: id.cpp:16


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03