Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00014 #include "ForwardDynamics.h"
00015 #include "Body.h"
00016 #include "Link.h"
00017 #include "Sensor.h"
00018
00019 using namespace hrp;
00020
00021
00022 ForwardDynamics::ForwardDynamics(BodyPtr body)
00023 : body(body)
00024 {
00025 g.setZero();
00026 timeStep = 0.005;
00027
00028 integrationMode = RUNGEKUTTA_METHOD;
00029 sensorsEnabled = false;
00030 }
00031
00032
00033 ForwardDynamics::~ForwardDynamics()
00034 {
00035
00036 }
00037
00038
00039 void ForwardDynamics::setTimeStep(double ts)
00040 {
00041 timeStep = ts;
00042 }
00043
00044
00045 void ForwardDynamics::setGravityAcceleration(const Vector3& g)
00046 {
00047 this->g = g;
00048 }
00049
00050
00051 void ForwardDynamics::setEulerMethod()
00052 {
00053 integrationMode = EULER_METHOD;
00054 }
00055
00056
00057 void ForwardDynamics::setRungeKuttaMethod()
00058 {
00059 integrationMode = RUNGEKUTTA_METHOD;
00060 }
00061
00062
00063 void ForwardDynamics::enableSensors(bool on)
00064 {
00065 sensorsEnabled = on;
00066 }
00067
00068
00070 void ForwardDynamics::SE3exp(Vector3& out_p, Matrix33& out_R,
00071 const Vector3& p0, const Matrix33& R0,
00072 const Vector3& w, const Vector3& vo, double dt)
00073 {
00074 using ::std::numeric_limits;
00075
00076 double norm_w = w.norm();
00077
00078 if(norm_w < numeric_limits<double>::epsilon() ) {
00079 out_p = p0 + vo * dt;
00080 out_R = R0;
00081 } else {
00082 double th = norm_w * dt;
00083 Vector3 w_n(w / norm_w);
00084 Vector3 vo_n(vo / norm_w);
00085 Matrix33 rot = rodrigues(w_n, th);
00086
00087 out_p = rot * p0 + (Matrix33::Identity() - rot) * w_n.cross(vo_n) + w_n * w_n.transpose() * vo_n * th;
00088 out_R.noalias() = rot * R0;
00089 }
00090 }
00091
00092
00093 void ForwardDynamics::initializeSensors()
00094 {
00095 body->clearSensorValues();
00096
00097 if(sensorsEnabled){
00098 initializeAccelSensors();
00099 }
00100 }
00101
00102
00103 void ForwardDynamics::updateSensorsFinal()
00104 {
00105 int n;
00106
00107 n = body->numSensors(Sensor::RATE_GYRO);
00108 for(int i=0; i < n; ++i){
00109 RateGyroSensor* sensor = body->sensor<RateGyroSensor>(i);
00110 Link* link = sensor->link;
00111 sensor->w = sensor->localR.transpose() * link->R.transpose() * link->w;
00112 }
00113
00114 n = body->numSensors(Sensor::ACCELERATION);
00115 for(int i=0; i < n; ++i){
00116 updateAccelSensor(body->sensor<AccelSensor>(i));
00117 }
00118
00119 }
00120
00121
00122 void ForwardDynamics::updateAccelSensor(AccelSensor* sensor)
00123 {
00124 Link* link = sensor->link;
00125 vector2* x = sensor->x;
00126
00127 Vector3 o_Vgsens(link->R * (link->R.transpose() * link->w).cross(sensor->localPos) + link->v);
00128
00129 if(sensor->isFirstUpdate){
00130 sensor->isFirstUpdate = false;
00131 for(int i=0; i < 3; ++i){
00132 x[i](0) = o_Vgsens(i);
00133 x[i](1) = 0.0;
00134 }
00135 } else {
00136
00137 for(int i=0; i < 3; ++i){
00138 x[i] = A * x[i] + o_Vgsens(i) * B;
00139 }
00140 }
00141
00142 Vector3 o_Agsens(x[0](1), x[1](1), x[2](1));
00143 o_Agsens += g;
00144
00145 sensor->dv.noalias() = sensor->localR.transpose() * link->R.transpose() * o_Agsens;
00146 }
00147
00148
00149 void ForwardDynamics::initializeAccelSensors()
00150 {
00151 int n = body->numSensors(Sensor::ACCELERATION);
00152 if(n > 0){
00153 for(int i=0; i < n; ++i){
00154 AccelSensor* sensor = body->sensor<AccelSensor>(i);
00155 if(sensor){
00156 sensor->isFirstUpdate = true;
00157 }
00158 }
00159
00160
00161 static const double n_input = 100.0;
00162 static const double n_output = 0.001;
00163
00164
00165
00166
00167 matrix22 Ac;
00168 Ac << -sqrt(2*n_input/n_output), 1.0,
00169 -n_input/n_output, 0.0;
00170
00171 vector2 Bc(sqrt(2*n_input/n_output), n_input/n_output);
00172
00173 A = matrix22::Identity();
00174 matrix22 An(matrix22::Identity());
00175 matrix22 An2;
00176 B = timeStep * Bc;
00177 vector2 Bn(B);
00178 vector2 Bn2;
00179
00180 double factorial[14];
00181 double r = 1.0;
00182 factorial[1] = r;
00183 for(int i=2; i <= 13; ++i){
00184 r += 1.0;
00185 factorial[i] = factorial[i-1] * r;
00186 }
00187
00188 for(int i=1; i <= 12; i++){
00189 An2.noalias() = Ac * An;
00190 An = timeStep * An2;
00191 A += (1.0 / factorial[i]) * An;
00192
00193 Bn2.noalias() = Ac * Bn;
00194 Bn = timeStep * Bn2;
00195 B += (1.0 / factorial[i+1]) * Bn;
00196 }
00197 }
00198 }