ForwardDynamics.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
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                 // kalman filtering
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                 // Kalman filter design
00161                 static const double n_input = 100.0;  // [N]
00162                 static const double n_output = 0.001; // [m/s]
00163 
00164                 // Analytical solution of Kalman filter (continuous domain)
00165                 // s.kajita  2003 Jan.22
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53