doxy-ocra-wbi-plugins/html/OcraWbiModel_8h_source.html

00001
00009 /*
00010  *  This file is part of ocra-icub.
00011  *  Copyright (C) 2016 Institut des Systèmes Intelligents et de Robotique (ISIR)
00012  *
00013  *  This program is free software: you can redistribute it and/or modify
00014  *  it under the terms of the GNU General Public License as published by
00015  *  the Free Software Foundation, either version 3 of the License, or
00016  *  (at your option) any later version.
00017  *
00018  *  This program is distributed in the hope that it will be useful,
00019  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00020  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00021  *  GNU General Public License for more details.
00022  *
00023  *  You should have received a copy of the GNU General Public License
00024  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00025 */
00026
00027 #ifndef OCRA_WBI_MODEL_H
00028 #define OCRA_WBI_MODEL_H
00029
00030 #include <memory>
00031
00032 #include "ocra/control/Model.h"
00033 #include <wbi/wbi.h>
00034 #include <yarp/os/Log.h>
00035 #include "ocra-icub/OcraWbiConversions.h"
00036 #include "ocra-icub/Utilities.h"
00037
00038 namespace ocra_icub
00039 {
00040
00041 class OcraWbiModel: public ocra::Model
00042 {
00043 // CLASS_POINTER_TYPEDEFS(OcraWbiModel)
00044
00045 public:
00046
00047
00048
00049 //===========================Constructor/Destructor===========================//
00050     OcraWbiModel(const std::string& robotName, const int robotNumDOF, std::shared_ptr<wbi::wholeBodyInterface> wbi, const bool freeRoot);
00051     virtual ~OcraWbiModel();
00052
00053 //=============================General functions==============================//
00054     virtual int                          nbSegments               () const;
00055     virtual const Eigen::VectorXd&       getActuatedDofs          () const;
00056     virtual const Eigen::VectorXd&       getJointLowerLimits      () const;
00057     virtual const Eigen::VectorXd&       getJointUpperLimits      () const;
00058     virtual const Eigen::VectorXd&       getJointPositions        () const;
00059     virtual const Eigen::VectorXd&       getJointVelocities       () const;
00060     virtual const Eigen::VectorXd&       getJointAccelerations    () const;
00061     virtual const Eigen::VectorXd&       getJointTorques          () const;
00062
00063     virtual const Eigen::Displacementd&  getFreeFlyerPosition     () const;
00064     virtual const Eigen::Twistd&         getFreeFlyerVelocity     () const;
00065
00066     virtual const std::string&           getJointName             (int index) const;
00067     virtual const int                    getSegmentIndex          (std::string segmentName) const;
00068
00069 //=============================Dynamic functions==============================//
00070     virtual const Eigen::MatrixXd&       getInertiaMatrix         () const;
00071     virtual const Eigen::MatrixXd&       getInertiaMatrixInverse  () const;
00072     virtual const Eigen::MatrixXd&       getDampingMatrix         () const;
00073     virtual const Eigen::VectorXd&       getNonLinearTerms        () const;
00074     virtual const Eigen::VectorXd&       getLinearTerms           () const;
00075     virtual const Eigen::VectorXd&       getGravityTerms          () const;
00076
00077 //===============================CoM functions================================//
00078     virtual double                                         getMass            () const;
00079     virtual const Eigen::Vector3d&                         getCoMPosition     () const;
00080     void                                                   updateCoMPosition();
00081     void                                                   updateCoMVelocity();
00082     virtual const Eigen::Vector3d&                         getCoMVelocity     () const;
00083     virtual const Eigen::Vector3d&                         getCoMAcceleration () const;
00084     virtual const Eigen::Vector3d&                         getCoMJdotQdot     () const;
00085     virtual const Eigen::Matrix<double,3,Eigen::Dynamic>&  getCoMJacobian     () const;
00086     virtual const Eigen::Matrix<double,3,Eigen::Dynamic>&  getCoMJacobianDot  () const;
00087     virtual const Eigen::Vector3d&                         getCoMAngularVelocity     () const;
00088     virtual const Eigen::Matrix<double,3,Eigen::Dynamic>&  getCoMAngularJacobian     () const;
00089
00090 //=============================Segment functions==============================//
00091     virtual const Eigen::Displacementd&                    getSegmentPosition          (int index) const;
00092     virtual const Eigen::Twistd&                           getSegmentVelocity          (int index) const;
00093     virtual double                                         getSegmentMass              (int index) const;
00094     virtual const Eigen::Vector3d&                         getSegmentCoM               (int index) const;
00095     virtual const Eigen::Matrix<double,6,6>&               getSegmentMassMatrix        (int index) const;
00096     virtual const Eigen::Vector3d&                         getSegmentMomentsOfInertia  (int index) const;
00097     virtual const Eigen::Rotation3d&                       getSegmentInertiaAxes       (int index) const;
00098     virtual const Eigen::Matrix<double,6,Eigen::Dynamic>&  getSegmentJacobian          (int index) const;
00099     virtual const Eigen::Matrix<double,6,Eigen::Dynamic>&  getSegmentJacobian          (int index, wbi::Frame H_world_root) const;
00100     virtual const Eigen::Matrix<double,6,Eigen::Dynamic>&  getSegmentJdot              (int index) const;
00101     virtual const Eigen::Matrix<double,6,Eigen::Dynamic>&  getJointJacobian            (int index) const;
00102     virtual const Eigen::Twistd&                           getSegmentJdotQdot          (int index) const;
00103
00104     void printAllData();
00105
00106     // void getJointTorques(Eigen::VectorXd& wbiTorques);
00107
00108
00109
00110
00111 protected:
00112
00113 //===========================Update state functions===========================//
00114     virtual void  doSetState(const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot);
00115
00116     virtual void  doSetState(const Eigen::Displacementd& H_root, const Eigen::VectorXd& q, const Eigen::Twistd& T_root, const Eigen::VectorXd& q_dot);
00117
00118     virtual void                doSetJointPositions     (const Eigen::VectorXd& q);
00119     virtual void                doSetJointVelocities    (const Eigen::VectorXd& dq);
00120     virtual void                doSetJointAccelerations (const Eigen::VectorXd& ddq);
00121     virtual void                doSetFreeFlyerPosition  (const Eigen::Displacementd& Hroot);
00122     virtual void                doSetFreeFlyerVelocity  (const Eigen::Twistd& Troot);
00123
00124 //============================Index name functions============================//
00125     virtual int                 doGetSegmentIndex       (const std::string& name) const;
00126     virtual const std::string&  doGetSegmentName        (int index) const;
00127     virtual int                 doGetDofIndex           (const std::string& name) const;
00128     virtual const std::string&  doGetDofName            (int index) const;
00129     virtual const std::string   doSegmentName           (const std::string& name) const;
00130     virtual const std::string   doDofName               (const std::string& name) const;
00131
00132 private:
00133     std::shared_ptr<wbi::wholeBodyInterface> robot; // Access to wholeBodyInterface
00134     struct OcraWbiModel_pimpl;
00135     boost::shared_ptr<OcraWbiModel_pimpl> owm_pimpl; // where all internal data are saved
00136     yarp::os::Log yLog;
00137     Eigen::VectorXd dqPrevious;
00138     yarp::os::Mutex mutex;
00139 };
00140 } /* ocra_icub */
00141
00142 #endif // OCRA_WBI_MODEL_H