ForwardDynamics.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * National Institute of Advanced Industrial Science and Technology (AIST)
 */
#include "ForwardDynamics.h"
#include "Body.h"
#include "Link.h"
#include "Sensor.h"

using namespace hrp;


ForwardDynamics::ForwardDynamics(BodyPtr body)
        : body(body)
{
    g.setZero();
    timeStep = 0.005;

    integrationMode = RUNGEKUTTA_METHOD;
    sensorsEnabled = false;
}


ForwardDynamics::~ForwardDynamics()
{

}


void ForwardDynamics::setTimeStep(double ts)
{
    timeStep = ts;
}


void ForwardDynamics::setGravityAcceleration(const Vector3& g)
{
    this->g = g;
}


void ForwardDynamics::setEulerMethod()
{
    integrationMode = EULER_METHOD;
}


void ForwardDynamics::setRungeKuttaMethod()
{
    integrationMode = RUNGEKUTTA_METHOD;
}


void ForwardDynamics::enableSensors(bool on)
{
    sensorsEnabled = on;
}


void ForwardDynamics::SE3exp(Vector3& out_p, Matrix33& out_R,
                                                         const Vector3& p0, const Matrix33& R0,
                                                         const Vector3& w, const Vector3& vo, double dt)
{
    using ::std::numeric_limits;
        
    double norm_w = w.norm();
        
    if(norm_w < numeric_limits<double>::epsilon() ) {
                out_p = p0 + vo * dt;
                out_R = R0;
    } else {
                double th = norm_w * dt;
                Vector3 w_n(w / norm_w);
                Vector3 vo_n(vo / norm_w);
                Matrix33 rot = rodrigues(w_n, th);
                
                out_p = rot * p0 + (Matrix33::Identity() - rot) * w_n.cross(vo_n) + w_n * w_n.transpose() * vo_n * th;
                out_R.noalias() = rot * R0;
    }
}


void ForwardDynamics::initializeSensors()
{
        body->clearSensorValues();

        if(sensorsEnabled){
                initializeAccelSensors();
        }
}
        

void ForwardDynamics::updateSensorsFinal()
{
    int n;

        n = body->numSensors(Sensor::RATE_GYRO);
    for(int i=0; i < n; ++i){
        RateGyroSensor* sensor = body->sensor<RateGyroSensor>(i);
        Link* link = sensor->link;
        sensor->w = sensor->localR.transpose() * link->R.transpose() * link->w;
        }

        n = body->numSensors(Sensor::ACCELERATION);
    for(int i=0; i < n; ++i){
                updateAccelSensor(body->sensor<AccelSensor>(i));
        }

}


void ForwardDynamics::updateAccelSensor(AccelSensor* sensor)
{
        Link* link = sensor->link;
        vector2* x = sensor->x;

        Vector3 o_Vgsens(link->R * (link->R.transpose() * link->w).cross(sensor->localPos) + link->v);

    if(sensor->isFirstUpdate){
                sensor->isFirstUpdate = false;
                for(int i=0; i < 3; ++i){
                        x[i](0) = o_Vgsens(i);
                        x[i](1) = 0.0;
                }
    } else {
                // kalman filtering
                for(int i=0; i < 3; ++i){
                        x[i] = A * x[i] + o_Vgsens(i) * B;
                }
    }

    Vector3 o_Agsens(x[0](1), x[1](1), x[2](1));
    o_Agsens += g;

    sensor->dv.noalias() = sensor->localR.transpose() * link->R.transpose() * o_Agsens;
}


void ForwardDynamics::initializeAccelSensors()
{
        int n = body->numSensors(Sensor::ACCELERATION);
        if(n > 0){
                for(int i=0; i < n; ++i){
                        AccelSensor* sensor = body->sensor<AccelSensor>(i);
                        if(sensor){
                                sensor->isFirstUpdate = true;
                        }
                }

                // Kalman filter design
                static const double n_input = 100.0;  // [N]
                static const double n_output = 0.001; // [m/s]

                // Analytical solution of Kalman filter (continuous domain)
                // s.kajita  2003 Jan.22

                matrix22 Ac;
                Ac << -sqrt(2*n_input/n_output), 1.0,
                        -n_input/n_output, 0.0;

                vector2 Bc(sqrt(2*n_input/n_output), n_input/n_output);

                A = matrix22::Identity();
                matrix22 An(matrix22::Identity());
                matrix22 An2;
                B = timeStep * Bc;
                vector2 Bn(B);
                vector2 Bn2;

                double factorial[14];
                double r = 1.0;
                factorial[1] = r;
                for(int i=2; i <= 13; ++i){
                        r += 1.0;
                        factorial[i] = factorial[i-1] * r;
                }

                for(int i=1; i <= 12; i++){
                        An2.noalias() = Ac * An;
                        An = timeStep * An2;
                        A += (1.0 / factorial[i]) * An;

                        Bn2.noalias() = Ac * Bn;
                        Bn = timeStep * Bn2;
                        B += (1.0 / factorial[i+1]) * Bn;
                }
        }
}