ForwardDynamics.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  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
14 #include "ForwardDynamics.h"
15 #include "Body.h"
16 #include "Link.h"
17 #include "Sensor.h"
18 
19 using namespace hrp;
20 
21 
23  : body(body)
24 {
25  g.setZero();
26  timeStep = 0.005;
27 
29  sensorsEnabled = false;
30 }
31 
32 
34 {
35 
36 }
37 
38 
40 {
41  timeStep = ts;
42 }
43 
44 
46 {
47  this->g = g;
48 }
49 
50 
52 {
54 }
55 
56 
58 {
60 }
61 
62 
64 {
65  sensorsEnabled = on;
66 }
67 
68 
71  const Vector3& p0, const Matrix33& R0,
72  const Vector3& w, const Vector3& vo, double dt)
73 {
74  using ::std::numeric_limits;
75 
76  double norm_w = w.norm();
77 
78  if(norm_w < numeric_limits<double>::epsilon() ) {
79  out_p = p0 + vo * dt;
80  out_R = R0;
81  } else {
82  double th = norm_w * dt;
83  Vector3 w_n(w / norm_w);
84  Vector3 vo_n(vo / norm_w);
85  Matrix33 rot = rodrigues(w_n, th);
86 
87  out_p = rot * p0 + (Matrix33::Identity() - rot) * w_n.cross(vo_n) + w_n * w_n.transpose() * vo_n * th;
88  out_R.noalias() = rot * R0;
89  }
90 }
91 
92 
94 {
95  body->clearSensorValues();
96 
97  if(sensorsEnabled){
99  }
100 }
101 
102 
104 {
105  int n;
106 
107  n = body->numSensors(Sensor::RATE_GYRO);
108  for(int i=0; i < n; ++i){
109  RateGyroSensor* sensor = body->sensor<RateGyroSensor>(i);
110  Link* link = sensor->link;
111  sensor->w = sensor->localR.transpose() * link->R.transpose() * link->w;
112  }
113 
114  n = body->numSensors(Sensor::ACCELERATION);
115  for(int i=0; i < n; ++i){
116  updateAccelSensor(body->sensor<AccelSensor>(i));
117  }
118 
119 }
120 
121 
123 {
124  Link* link = sensor->link;
125  vector2* x = sensor->x;
126 
127  Vector3 o_Vgsens(link->R * (link->R.transpose() * link->w).cross(sensor->localPos) + link->v);
128 
129  if(sensor->isFirstUpdate){
130  sensor->isFirstUpdate = false;
131  for(int i=0; i < 3; ++i){
132  x[i](0) = o_Vgsens(i);
133  x[i](1) = 0.0;
134  }
135  } else {
136  // kalman filtering
137  for(int i=0; i < 3; ++i){
138  x[i] = A * x[i] + o_Vgsens(i) * B;
139  }
140  }
141 
142  Vector3 o_Agsens(x[0](1), x[1](1), x[2](1));
143  o_Agsens += g;
144 
145  sensor->dv.noalias() = sensor->localR.transpose() * link->R.transpose() * o_Agsens;
146 }
147 
148 
150 {
151  int n = body->numSensors(Sensor::ACCELERATION);
152  if(n > 0){
153  for(int i=0; i < n; ++i){
154  AccelSensor* sensor = body->sensor<AccelSensor>(i);
155  if(sensor){
156  sensor->isFirstUpdate = true;
157  }
158  }
159 
160  // Kalman filter design
161  static const double n_input = 100.0; // [N]
162  static const double n_output = 0.001; // [m/s]
163 
164  // Analytical solution of Kalman filter (continuous domain)
165  // s.kajita 2003 Jan.22
166 
167  matrix22 Ac;
168  Ac << -sqrt(2*n_input/n_output), 1.0,
169  -n_input/n_output, 0.0;
170 
171  vector2 Bc(sqrt(2*n_input/n_output), n_input/n_output);
172 
173  A = matrix22::Identity();
174  matrix22 An(matrix22::Identity());
175  matrix22 An2;
176  B = timeStep * Bc;
177  vector2 Bn(B);
178  vector2 Bn2;
179 
180  double factorial[14];
181  double r = 1.0;
182  factorial[1] = r;
183  for(int i=2; i <= 13; ++i){
184  r += 1.0;
185  factorial[i] = factorial[i-1] * r;
186  }
187 
188  for(int i=1; i <= 12; i++){
189  An2.noalias() = Ac * An;
190  An = timeStep * An2;
191  A += (1.0 / factorial[i]) * An;
192 
193  Bn2.noalias() = Ac * Bn;
194  Bn = timeStep * Bn2;
195  B += (1.0 / factorial[i+1]) * Bn;
196  }
197  }
198 }
Eigen::Vector2d vector2
* x
Definition: IceUtils.h:98
void setTimeStep(double timeStep)
enum hrp::ForwardDynamics::@2 integrationMode
png_uint_32 i
Definition: png.h:2735
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
static void SE3exp(Vector3 &out_p, Matrix33 &out_R, const Vector3 &p0, const Matrix33 &R0, const Vector3 &w, const Vector3 &vo, double dt)
update position/orientation using spatial velocity
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
void setGravityAcceleration(const Vector3 &g)
Matrix33 rodrigues(const Vector3 &axis, double q)
Definition: Eigen3d.h:27
Eigen::Matrix2d matrix22
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ForwardDynamics(BodyPtr body)
virtual void updateSensorsFinal()
virtual void initializeSensors()
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
Definition: Tvmet2Eigen.h:15
void enableSensors(bool on)
void updateAccelSensor(AccelSensor *sensor)


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