ForwardDynamics.h
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 #ifndef OPENHRP_FORWARD_DYNAMICS_H_INCLUDED
00015 #define OPENHRP_FORWARD_DYNAMICS_H_INCLUDED
00016 
00017 #include "Body.h"
00018 
00019 #include <boost/shared_ptr.hpp>
00020 #include <boost/intrusive_ptr.hpp>
00021 #include <hrpUtil/Eigen3d.h>
00022 
00023 #include "Config.h"
00024 
00025 
00026 namespace hrp
00027 {
00028         class AccelSensor;
00029 
00035     class HRPMODEL_API ForwardDynamics {
00036 
00037     public:
00038         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00039         
00040         ForwardDynamics(BodyPtr body);
00041                 virtual ~ForwardDynamics();
00042         
00043         void setGravityAcceleration(const Vector3& g);
00044         void setEulerMethod();
00045         void setRungeKuttaMethod();
00046         void setTimeStep(double timeStep);
00047         void enableSensors(bool on);
00048 
00049         virtual void initialize() = 0;
00050         virtual void calcNextState() = 0;
00051 
00052     protected:
00053 
00054                 virtual void initializeSensors();
00055                 virtual void updateSensorsFinal();
00056 
00067                 static void SE3exp(Vector3& out_p, Matrix33& out_R,
00068                                                    const Vector3& p0, const Matrix33& R0,
00069                                                    const Vector3& w, const Vector3& vo, double dt);
00070                 
00071         BodyPtr body;
00072         Vector3 g;
00073         double timeStep;
00074         bool sensorsEnabled;
00075 
00076         enum { EULER_METHOD, RUNGEKUTTA_METHOD } integrationMode;
00077 
00078         private:
00079 
00080                 void updateAccelSensor(AccelSensor* sensor);
00081                 void initializeAccelSensors();
00082 
00083                 // varialbes for calculating sensor values
00084                 // preview control gain matrices for force sensors
00085                 typedef Eigen::Matrix2d matrix22;
00086                 typedef Eigen::Vector2d vector2;
00087                 matrix22 A;
00088                 vector2 B;
00089                 
00090     };
00091 
00092         typedef boost::shared_ptr<ForwardDynamics> ForwardDynamicsPtr;
00093         
00094 };
00095 
00096 #endif


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