ForwardDynamicsABM.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_ABM_H_INCLUDED
00015 #define OPENHRP_FORWARD_DYNAMICS_ABM_H_INCLUDED
00016 
00017 #include "ForwardDynamics.h"
00018 #include <vector>
00019 #include <boost/intrusive_ptr.hpp>
00020 #include <hrpUtil/Eigen3d.h>
00021 #include "Config.h"
00022 
00023 
00024 namespace hrp
00025 {
00026     class LinkTraverse;
00027     class AccelSensor;
00028     class ForceSensor;
00029 
00033     class HRPMODEL_API ForwardDynamicsABM : public ForwardDynamics {
00034 
00035     public:
00036         
00037         ForwardDynamicsABM(BodyPtr body);
00038         ~ForwardDynamicsABM();
00039         
00040         virtual void initialize();
00041         virtual void calcNextState();
00042 
00043     private:
00044         
00045         void calcMotionWithEulerMethod();
00046         void integrateRungeKuttaOneStep(double r, double dt);
00047         void calcMotionWithRungeKuttaMethod();
00048 
00052         void calcABMPhase1();
00053 
00057         void calcABMPhase2();
00058         void calcABMPhase2Part1();
00059         void calcABMPhase2Part2();
00060 
00064         void calcABMPhase3();
00065 
00066         inline void calcABMFirstHalf();
00067         inline void calcABMLastHalf();
00068 
00069         void updateForceSensors();
00070         void updateForceSensor(ForceSensor* sensor);
00071                 
00072         // Buffers for the Runge Kutta Method
00073         Vector3 p0;
00074         Matrix33 R0;
00075         Vector3 vo0;
00076         Vector3 w0;
00077         std::vector<double> q0;
00078         std::vector<double> dq0;
00079                 
00080         Vector3 vo;
00081         Vector3 w;
00082         Vector3 dvo;
00083         Vector3 dw;
00084         std::vector<double> dq;
00085         std::vector<double> ddq;
00086 
00087     };
00088         
00089 };
00090 
00091 #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