Define missing protected const getters in DynamicRobot.

laas · Oct 12, 2012 · b97c37d · b97c37d
1 parent f996f60
commit b97c37d
Show file tree

Hide file tree

Showing 2 changed files with 85 additions and 0 deletions.
diff --git a/include/ard/rbdl/model/dynamic-robot.hh b/include/ard/rbdl/model/dynamic-robot.hh
@@ -319,6 +319,34 @@ namespace ard
       /// \brief Set the root joint of the robot.
       virtual void rootJoint (joint_t& joint);
 
+      /// \brief Get a vector containing share pointers to all joints.
+      virtual void jointVector (jointShPtrs_t& jointVector) const;
+
+      /// \brief Get a vector containing weak pointers to all joints.
+      virtual void jointVector (jointWkPtrs_t& jointVector) const;
+
+      /// \brief Get the velocity of the center of mass.
+      virtual const vector3d& velocityCenterOfMass () const;
+
+      /// \brief Get the acceleration of the center of mass.
+      virtual const vector3d& accelerationCenterOfMass () const;
+
+      /// \brief Get the linear momentum of the robot.
+      virtual const vector3d& linearMomentumRobot () const;
+
+      /// \brief Get the time-derivative of the linear momentum.
+      virtual const vector3d& derivativeLinearMomentum () const;
+
+      /// \brief Get the angular momentum of the robot at the center of mass.
+      virtual const vector3d& angularMomentumRobot () const;
+
+      /// \brief Get the time-derivative of the angular momentum at the
+      /// center of mass.
+      virtual const vector3d& derivativeAngularMomentum () const;
+
+      /// \brief Get the vector of actuated joints.
+      void actuatedJoints (jointWkPtrs_t& actuatedJoints) const;
+
     private:
       /// \brief Root joint pointer attribute.
       jointShPtr_t rootJoint_;

diff --git a/src/dynamic-robot.cc b/src/dynamic-robot.cc
@@ -380,5 +380,62 @@ namespace ard
       rootJoint_ = joint.shared_from_this ();
     }
 
+    void DynamicRobot::jointVector (jointShPtrs_t& jointVector) const
+    {
+      jointVector.resize (jointVector_.size ());
+      for (unsigned i = 0; i < jointVector_.size (); ++i)
+	{
+	  jointShPtr_t joint = getSharedPointer (jointVector_[i]);
+	  if (joint)
+	    jointVector[i] = joint;
+	  else
+	    throw std::runtime_error ("Null pointer to joint.");
+	}
+    }
+
+    void DynamicRobot::jointVector (jointWkPtrs_t& jointVector) const
+    {
+      jointVector.resize (jointVector_.size ());
+      for (unsigned i = 0; i < jointVector_.size (); ++i)
+	jointVector[i] = jointVector_[i];
+    }
+
+    const vector3d& DynamicRobot::velocityCenterOfMass () const
+    {
+      return velocityCenterOfMass_;
+    }
+
+    const vector3d& DynamicRobot::accelerationCenterOfMass () const
+    {
+      return accelerationCenterOfMass_;
+    }
+
+    const vector3d& DynamicRobot::linearMomentumRobot () const
+    {
+      return linearMomentumRobot_;
+    }
+
+    const vector3d& DynamicRobot::derivativeLinearMomentum () const
+    {
+      return derivativeLinearMomentum_;
+    }
+
+    const vector3d& DynamicRobot::angularMomentumRobot () const
+    {
+      return angularMomentumRobot_;
+    }
+
+    const vector3d& DynamicRobot::derivativeAngularMomentum () const
+    {
+      return derivativeAngularMomentum_;
+    }
+
+    void DynamicRobot::actuatedJoints (jointWkPtrs_t& actuatedJoints) const
+    {
+      actuatedJoints.resize (actuatedJoints_.size ());
+      for (unsigned i = 0; i < actuatedJoints_.size (); ++i)
+	actuatedJoints[i] = actuatedJoints_[i];
+    }
+
   } // end of namespace rbdl.
 } // end of namespace ard.