00001 #ifndef JOINT_TORQUE_LIMITER_H 00002 #define JOINT_TORQUE_LIMITER_H 00003 00004 #include <kdl/tree.hpp> 00005 #include <kdl/jntarray.hpp> 00006 #include <kdl/frames.hpp> 00007 #include <sensor_msgs/JointState.h> 00008 #include <r2_msgs/StringArray.h> 00009 00010 #include "robodyn_controllers/KdlTreeParser.h" 00011 #include "robodyn_utilities/RateLimiter.h" 00012 00013 00014 class JointTorqueLimiter : public KdlTreeParser 00015 { 00016 public: 00017 JointTorqueLimiter(const std::string& urdfFile); 00018 ~JointTorqueLimiter(); 00019 00020 void setYankLimit(const double lim); 00021 int populateTorqueMsg(sensor_msgs::JointState& tauLimCmd); 00022 int storeJointTorques(const double& tau); 00023 int storeJointTorques(const sensor_msgs::JointState& desiredTauLim); 00024 void initializeMaps(); 00025 bool getCompletionMessage(r2_msgs::StringArray& completionMsg); 00026 00027 std::map<std::string, double> jointTorqueLimMap; 00028 00029 private: 00030 RateLimiter yl; 00031 00032 }; 00033 00034 #endif