Add joint limits in RosRobotModel.

CMakeLists.txt

@@ -11,7 +11,6 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 set(CMAKE_INSTALL_RPATH "${LIBRARY_OUTPUT_PATH}")
 
-
 rosbuild_genmsg()
 rosbuild_gensrv()
 

src/robot_model.cpp

@@ -80,7 +80,17 @@ namespace dynamicgraph
 	    "RosRobotModel(" + name + ")::output(vector)::com"),
       jcom_ (boost::bind (&RosRobotModel::computeJCom, this, _1, _2),
 	     0,
-	     "RosRobotModel(" + name + ")::output(vector)::Jcom")
+	     "RosRobotModel(" + name + ")::output(vector)::Jcom"),
+      lowerJointLimits_
+      (boost::bind
+       (&RosRobotModel::computeLowerJointLimits, this, _1, _2),
+       0,
+       "RosRobotModel(" + name + ")::output(vector)::lowerJl"),
+      upperJointLimits_
+      (boost::bind
+       (&RosRobotModel::computeUpperJointLimits, this, _1, _2),
+       0,
+       "RosRobotModel(" + name + ")::output(vector)::upperJl")
   {
     signalRegistration(q_);
     signalRegistration(dq_);
@@ -88,6 +98,8 @@ namespace dynamicgraph
     signalRegistration(zmp_);
     signalRegistration(com_);
     signalRegistration(jcom_);
+    signalRegistration(lowerJointLimits_);
+    signalRegistration(upperJointLimits_);
 
     zmp_.addDependency (q_);
     zmp_.addDependency (dq_);
@@ -265,5 +277,33 @@ namespace dynamicgraph
     return jcom;
   }
 
+  ml::Vector&
+  RosRobotModel::computeLowerJointLimits (ml::Vector& lowerJointLimits, int t)
+  {
+    if (!robot_)
+      throw std::runtime_error ("no robot");
+
+    const unsigned int& ndofs = robot_->numberDof ();
+    lowerJointLimits.resize (ndofs);
+
+    for (unsigned int i = 0; i< ndofs; ++i)
+      lowerJointLimits (i) = robot_->lowerBoundDof (i);
+    return lowerJointLimits;
+  }
+
+  ml::Vector&
+  RosRobotModel::computeUpperJointLimits (ml::Vector& upperJointLimits, int t)
+  {
+    if (!robot_)
+      throw std::runtime_error ("no robot");
+
+    const unsigned int& ndofs = robot_->numberDof ();
+    upperJointLimits.resize (ndofs);
+
+    for (unsigned int i = 0; i< ndofs; ++i)
+      upperJointLimits (i) = robot_->upperBoundDof (i);
+    return upperJointLimits;
+  }
+
   DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RosRobotModel, "RosRobotModel");
 } // end of namespace dynamicgraph.

src/robot_model.hh

@@ -95,6 +95,9 @@ namespace dynamicgraph
     ml::Vector& computeCom (ml::Vector& com, int time);
     ml::Matrix& computeJCom (ml::Matrix& jcom, int time);
 
+    ml::Vector& computeLowerJointLimits (ml::Vector&, int time);
+    ml::Vector& computeUpperJointLimits (ml::Vector&, int time);
+
   private:
     CjrlHumanoidDynamicRobot* robot_;
     std::list< ::dynamicgraph::SignalBase<int>* > genericSignalRefs_;
@@ -115,6 +118,11 @@ namespace dynamicgraph
     dynamicgraph::SignalTimeDependent<ml::Vector,int> com_;
     /// \brief Center of mass jacobian
     dynamicgraph::SignalTimeDependent<ml::Matrix,int> jcom_;
+
+    /// \brief Lower joints limits
+    dynamicgraph::SignalTimeDependent<ml::Vector,int> lowerJointLimits_;
+    /// \brief Upper joints limits
+    dynamicgraph::SignalTimeDependent<ml::Vector,int> upperJointLimits_;
   };
 } // end of namespace dynamicgraph.