added comments to the controller #7

controller_modules/include/controller_modules/ControllerBase.h

@@ -4,12 +4,17 @@
 #include <Eigen/Core>
 #include <ros/ros.h>
 
+/**
+ * @brief Base class for controllers to be build
+ * 
+ */
 class ControllerBase
 {
    public:
     ControllerBase(){}
     ~ControllerBase(){}
 
+   //overide function to calculate the controller output
     virtual void update(const trajectory_msgs::JointTrajectoryPoint&, const trajectory_msgs::JointTrajectoryPoint&, std::vector<double>&)=0;
     virtual std::vector<double> get_error(){return error;}
     virtual std::vector<double> get_tau(){return tau;}  
@@ -20,6 +25,7 @@ class ControllerBase
       std::vector<double> error;
       std::vector<double> tau;
 
+      //helper function to convert between the std vectors and Eigen vectors
       template<typename T, typename A>
       Eigen::VectorXd VectToEigen(std::vector<T,A> const& msg )   
       {

controller_modules/include/controller_modules/ControllerManager.h

@@ -9,6 +9,10 @@
 #include "controller_modules/ControllerList.h"
 #include <vector>
 
+/**
+ * @brief Manages the attached controllers and wrapps them to be called by ros
+ * 
+ */
 class ControllerManager
 {
     public:

controller_modules/include/controller_modules/PDController.h

@@ -18,7 +18,6 @@ class PDController : public ControllerBase
           void calculate_torque( const Eigen::VectorXd& e, const  Eigen::VectorXd& ed, Eigen::VectorXd& tau);
           void update(const trajectory_msgs::JointTrajectoryPoint&, const  trajectory_msgs::JointTrajectoryPoint&, std::vector<double>&);
 
-
      private:
 
           Eigen::MatrixXd Kp;

controller_modules/src/ControllerManager.cpp

@@ -1,51 +1,92 @@
 #include "controller_modules/ControllerManager.h"
 
 
-
-ControllerManager::ControllerManager(ros::NodeHandle* n):nh(*n)
+/**
+ * @brief Construct a new Controller Manager:: Controller Manager object
+ * 
+ * @param n ros node handle
+ */
+ControllerManager::ControllerManager(ros::NodeHandle *n) : nh(*n)
 {
-    calc_tau_srv =  nh.advertiseService("CalcTau", &ControllerManager::calcTauSrv, this);
+    calc_tau_srv = nh.advertiseService("CalcTau", &ControllerManager::calcTauSrv, this);
     controller_list_srv = nh.advertiseService("ControllerList", &ControllerManager::getControllersListSrv, this);
 }
 
 ControllerManager::ControllerManager() {}
 
-
 ControllerManager::~ControllerManager()
 {
-
 }
 
-bool ControllerManager::getControllersListSrv(controller_modules::ControllerListRequest& req, controller_modules::ControllerListResponse& res)
-{   
+/**
+ * @brief Ros serverice to get a list of the controllers
+ * 
+ * @param req blank message
+ * @param res list of controllers
+ * @return true service was called
+ * @return false service failed to be called
+ */
+bool ControllerManager::getControllersListSrv(controller_modules::ControllerListRequest &req, controller_modules::ControllerListResponse &res)
+{
     res.controllers = getControllerList();
+    return true;
 }
 
+/**
+ * @brief get a list of the attached controllers
+ * 
+ * @return std::vector<std::string> 
+ */
 std::vector<std::string> ControllerManager::getControllerList()
 {
     std::vector<std::string> names;
-    for( std::pair<std::string,  boost::shared_ptr<ControllerBase>>  node: controller_list )
+    for (std::pair<std::string, boost::shared_ptr<ControllerBase>> node : controller_list)
     {
         names.push_back(node.first);
     }
     return names;
 }
 
-void ControllerManager::addController(std::string name, boost::shared_ptr<ControllerBase> controller )
+/**
+ * @brief add a controller to the manager that can be accessed by the server
+ * 
+ * @param name the name of the controller to be added 
+ * @param controller the controller to be added 
+ * 
+ */
+void ControllerManager::addController(std::string name, boost::shared_ptr<ControllerBase> controller)
 {
     //boost::shared_ptr<ControllerBase> cntl(*controller) ;
     controller_list[name] = controller;
 }
 
-
-bool ControllerManager::calcTauSrv(controller_modules::JointControlRequest& req, controller_modules::JointControlResponse& res )
+/**
+ * @brief service to call the control manager, used the name of the controller to calculat the control input
+ * 
+ * @param req Joint control request message 
+ * @param res Joint control responce message 
+ * @return true 
+ * @return false 
+ */
+bool ControllerManager::calcTauSrv(controller_modules::JointControlRequest &req, controller_modules::JointControlResponse &res)
 {
+    std::vector<double> output; // declare a var to hold the output
+    std::string name = req.controller_name; //get the name of the controller to call
+    // ROS_INFO( name.c_str()  ) ;
+    std::unordered_map<std::string, boost::shared_ptr<ControllerBase>>::const_iterator got = controller_list.find (name); //check is controller exists
 
-    std::vector<double> thing;
-    std::string name = req.controller_name; 
-    controller_list[name.c_str()]->update(req.actual, req.desired, thing );
-    //std::vector<double> error = controller_list[req.controller_name]->get_error();
-    res.control_output.effort = thing;
+    //if the controller is not found throw error
+    if (got == controller_list.end() )
+    {
+        ROS_ERROR("Controller not found");
+        return false;
+    }
+    else
+    {
+        //if the controller is found call the controller
+        controller_list[name.c_str()]->update(req.actual, req.desired, output);
+        res.control_output.effort = output;
+        return true;
+    }
 
-    return true;
-}   
+}

controller_modules/src/PDController.cpp

@@ -1,6 +1,11 @@
 #include "controller_modules/PDController.h"
 
-
+/**
+ * @brief Construct a new PDController::PDController object
+ * 
+ * @param _Kp matrix for the Kp gains
+ * @param _Kd matrix forthe Kd gains
+ */
 PDController::PDController(const Eigen::MatrixXd& _Kp, const Eigen::MatrixXd& _Kd):
    Kp(validateMat(_Kp, _Kd)),
    Kd(validateMat(_Kd, _Kp)) //validate the size of the matrix
@@ -17,7 +22,13 @@ PDController::~PDController()
 {
 
 }
-
+/**
+ * @brief initlize the Kp matrix
+ * 
+ * @param mat matrix to set the gains
+ * @return true matrix is valid  
+ * @return false matix is not valid
+ */
 bool PDController::setKp(const Eigen::MatrixXd& mat)
 {
     int r = mat.rows();
@@ -35,6 +46,13 @@ bool PDController::setKp(const Eigen::MatrixXd& mat)
 
 }
 
+/**
+ * @brief initlize the Kd matrix
+ * 
+ * @param mat matrix to set the gains 
+ * @return true matrix is valid  
+ * @return false matix is not valid
+ */
 bool PDController::setKd(const Eigen::MatrixXd& mat)
 {
 
@@ -53,18 +71,33 @@ bool PDController::setKd(const Eigen::MatrixXd& mat)
 
 }
 
+/**
+ * @brief get the Kp matrix
+ * 
+ * @return Eigen::MatrixXd 
+ */
 Eigen::MatrixXd PDController::getKp()
 
 {
     return Kp;
 }
-
+/**
+ * @brief get the Kd matrix
+ * 
+ * @return Eigen::MatrixXd 
+ */
 Eigen::MatrixXd PDController::getKd()
 {
     return Kd;
 }
 
-
+/**
+ * @brief The matrix need to be the same shape
+ * 
+ * @param mat1 matrix 1 to compare
+ * @param mat2 matrix 2 to compare
+ * @return Eigen::MatrixXd 
+ */
 Eigen::MatrixXd PDController::validateMat(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2)
 {
     int r1 = mat1.rows();
@@ -83,17 +116,29 @@ Eigen::MatrixXd PDController::validateMat(const Eigen::MatrixXd& mat1, const Eig
 
 }
 
+
+/**
+ * @brief calculate the torque for the cotnroller
+ *  tau = Kq(q_d - q) + Kd(qd_d - q_d)
+ * @param e vector of the position error
+ * @param ed vector of the velocity error
+ * @param torque output vector of the torque
+ */
 void PDController::calculate_torque(const Eigen::VectorXd &e, const Eigen::VectorXd &ed, Eigen::VectorXd &torque)
 {
-    torque = Kp*e+ Kd*ed;
+    torque = Kp*e + Kd*ed;
 }
 
-
+/**
+ * @brief calls the main controller calculation function and puts the data into the required form
+ * 
+ * @param actual message with the current joint states
+ * @param desired message with the desired jotni states
+ * @param torque output of the torque requried 
+ */
 void PDController::update(const trajectory_msgs::JointTrajectoryPoint& actual, const trajectory_msgs::JointTrajectoryPoint& desired, std::vector<double>& torque)
 {
 
-
-
     int dp_size = desired.positions.size();
     int ap_size = actual.positions.size();
     int pv_size = desired.velocities.size();
@@ -106,15 +151,16 @@ void PDController::update(const trajectory_msgs::JointTrajectoryPoint& actual, c
         Eigen::VectorXd ed = VectToEigen(desired.velocities) - VectToEigen(actual.velocities); 
         Eigen::VectorXd tau_(e.rows());
         calculate_torque(e, ed, tau_);
-
+
+
+        // tau = qdd +  Kq(q_d - q) + Kd(qd_d - q_d)
         if (desired.accelerations.size() == dp_size)
         {
             tau_ = tau_ +  VectToEigen(desired.accelerations); 
         }
 
         std::vector<double> my_tau(&tau_[0], tau_.data()+tau_.cols()*tau_.rows());
         tau = my_tau;
-        //error = std::vector<double>(&e[0], e.data()+e.cols()*e.rows());
         torque = tau;
 
     }