Accessors for af joints

ambf_framework/afFramework.cpp

@@ -3232,6 +3232,19 @@ bool afJointController::createFromAttribs(afJointControllerAttributes *a_attribs
     return true;
 }
 
+////
+/// \brief afCartesianController::setLinearGains
+/// \param a_P
+/// \param a_I
+/// \param a_D
+///
+void afJointController::setLinearGains(double a_P, double a_I, double a_D){
+    m_P = a_P;
+    m_I = a_I;
+    m_D = a_D;
+}
+
+
 double afJointController::computeOutput(double process_val, double set_point, double current_time){
     uint n = queue_length - 1;
     for (uint i = 0 ; i < n ; i++){
@@ -3782,6 +3795,18 @@ double afJoint::getEffort(){
 }
 
 
+void afJoint::setLinearGains(double a_P, double a_I, double a_D) {
+    this->m_controller.setLinearGains(a_P, a_I, a_D);
+}
+
+vector<double> afJoint::getLinearGains() {
+    vector<double> v;
+    v.push_back(this->m_controller.getP_lin());
+    v.push_back(this->m_controller.getI_lin());
+    v.push_back(this->m_controller.getD_lin());
+    return v;
+}
+
 ///
 /// \brief afSensor::afSensor
 /// \param a_afWorld

ambf_framework/afFramework.h

@@ -404,6 +404,13 @@ class afJointController: public afJointControllerAttributes{
     // Store the last effort command to compute and bound max impulse
     double m_last_cmd = 0;
 
+    // Get Controller Gains
+    inline double getP_lin(){return m_P;}
+    inline double getI_lin(){return m_I;}
+    inline double getD_lin(){return m_D;}
+
+    void setLinearGains(double a_P, double a_I, double a_D);
+
     double computeOutput(double process_val, double set_point, double current_time);
 
     void boundImpulse(double& effort_cmd);
@@ -1350,6 +1357,12 @@ class afJoint: public afBaseObject{
 
     bool isFeedBackEnabled(){return m_enableFeedback;}
 
+    // Set the gain of this joint
+    void setLinearGains(double a_P, double a_I, double a_D);
+
+    // Get the gain of this joint
+    vector<double> getLinearGains();
+
 protected:
 
     string m_childName;