diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ac7264404467ea48fd96f2b0f1740b6420863c8..f0b430400d2075468ee3e67b15cba0b7329458c7 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/src/robot_model.cpp b/src/robot_model.cpp index 08257163cc77dcc2184467e418a56c8b70c873d3..e2e8d31768b03f70d464add19a4a79f6ebd1831d 100644 --- a/src/robot_model.cpp +++ b/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. diff --git a/src/robot_model.hh b/src/robot_model.hh index a5a4e65b0933fcad9c2b9a639cfe0286fd4314f1..1bd498d1fca1eb342e0e184db28649d8570c07cd 100644 --- a/src/robot_model.hh +++ b/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.