diff --git a/include/ard/rbdl/model/dynamic-robot.hh b/include/ard/rbdl/model/dynamic-robot.hh index 1b4b603..2151697 100644 --- a/include/ard/rbdl/model/dynamic-robot.hh +++ b/include/ard/rbdl/model/dynamic-robot.hh @@ -75,6 +75,13 @@ namespace ard /// \brief Get underlying rbdl model. virtual rbdlModel_t rbdlModel () const; + /// \brief Get gravity vector. It is set to a default value on + /// initialization. + virtual vector3d gravity () const; + + /// \brief Set gravity vector. + virtual void gravity (const vector3d& gravity); + /// \brief Get the chain of joints between two joints /// \param inStartJoint First joint. /// \param inEndJoint Second joint. diff --git a/include/ard/rbdl/tools/types.hh b/include/ard/rbdl/tools/types.hh index f702294..a096bb8 100644 --- a/include/ard/rbdl/tools/types.hh +++ b/include/ard/rbdl/tools/types.hh @@ -48,7 +48,7 @@ namespace ard namespace rbdl { // Define gravity vector. - static const vector3d gravity (0, 0, -9.81); + static const vector3d defaultGravity (0, 0, -9.81); // RBDL typedefs. ARD_RBDL_DEFINE_TYPES (RigidBodyDynamics::Joint, rbdlJoint); diff --git a/src/dynamic-robot.cc b/src/dynamic-robot.cc index c17af3a..9489e85 100644 --- a/src/dynamic-robot.cc +++ b/src/dynamic-robot.cc @@ -113,7 +113,7 @@ namespace ard // Initialize rbdl model. This method is called before building // the model. rbdlModel_.Init (); - toRbdlFromMal (rbdlModel_.gravity, gravity); + toRbdlFromMal (rbdlModel_.gravity, defaultGravity); // Build rbdl model. if (!buildRbdlModel ()) @@ -150,6 +150,16 @@ namespace ard return rbdlModel_; } + vector3d DynamicRobot::gravity () const + { + return toMalFromRbdl (rbdlModel_.gravity); + } + + void DynamicRobot::gravity (const vector3d& gravity) + { + rbdlModel_.gravity = toRbdlFromMal (gravity); + } + std::vector::type > DynamicRobot::jointsBetween (const CjrlJoint& startJoint, const CjrlJoint& endJoint) const