Merge branch 'fix-121-controller-conf' into develop

roboticslab-uc3m · Jan 19, 2018 · b0ccbf2 · b0ccbf2
2 parents 922fa9c + 81a5c30
commit b0ccbf2
Show file tree

Hide file tree

Showing 19 changed files with 481 additions and 715 deletions.
diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp
@@ -40,9 +40,9 @@ bool roboticslab::AmorCartesianControl::checkJointVelocities(const std::vector<d
 {
     for (unsigned int i = 0; i < qdot.size(); i++)
     {
-        if (std::abs(qdot[i]) > MAX_ANG_VEL)
+        if (std::abs(qdot[i]) > maxJointVelocity)
         {
-            CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], MAX_ANG_VEL);
+            CD_ERROR("Maximum angular velocity hit at joint %d (qdot[%d] = %f > %f [deg/s]).\n", i + 1, i, qdot[i], maxJointVelocity);
             return false;
         }
     }

diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp
@@ -15,7 +15,9 @@
 #define DEFAULT_CAN_LIBRARY "libeddriver.so"
 #define DEFAULT_CAN_PORT 0
 
-#define MAX_ANG_VEL 10.0
+#define DEFAULT_GAIN 0.05
+#define DEFAULT_QDOT_LIMIT 10.0
+#define DEFAULT_REFERENCE_FRAME "base"
 
 namespace roboticslab
 {
@@ -40,7 +42,10 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
     AmorCartesianControl() : handle(AMOR_INVALID_HANDLE),
                              ownsHandle(false),
                              iCartesianSolver(NULL),
-                             currentState(VOCAB_CC_NOT_CONTROLLING)
+                             currentState(VOCAB_CC_NOT_CONTROLLING),
+                             gain(DEFAULT_GAIN),
+                             maxJointVelocity(DEFAULT_QDOT_LIMIT),
+                             referenceFrame(BASE_FRAME)
     {}
 
     // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp --
@@ -65,19 +70,13 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
 
     virtual bool tool(const std::vector<double> &x);
 
-    virtual void fwd(const std::vector<double> &rot, double step);
+    virtual void twist(const std::vector<double> &xdot);
 
-    virtual void bkwd(const std::vector<double> &rot, double step);
-
-    virtual void rot(const std::vector<double> &rot);
-
-    virtual void pan(const std::vector<double> &transl);
+    virtual void pose(const std::vector<double> &x, double interval);
 
-    virtual void vmos(const std::vector<double> &xdot);
+    virtual bool setParameter(int vocab, double value);
 
-    virtual void eff(const std::vector<double> &xdotee);
-
-    virtual void pose(const std::vector<double> &x, double interval);
+    virtual bool getParameter(int vocab, double * value);
 
     // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
 
@@ -115,6 +114,11 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
     roboticslab::ICartesianSolver *iCartesianSolver;
 
     int currentState;
+
+    double gain;
+    double maxJointVelocity;
+
+    reference_frame referenceFrame;
 };
 
 }  // namespace roboticslab

diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp
@@ -15,6 +15,29 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config)
 {
     CD_DEBUG("AmorCartesianControl config: %s.\n", config.toString().c_str());
 
+    gain = config.check("controllerGain", yarp::os::Value(DEFAULT_GAIN),
+            "controller gain").asDouble();
+
+    maxJointVelocity = config.check("maxJointVelocity", yarp::os::Value(DEFAULT_QDOT_LIMIT),
+            "maximum joint velocity").asDouble();
+
+    std::string referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME),
+            "reference frame").asString();
+
+    if (referenceFrameStr == "base")
+    {
+        referenceFrame = BASE_FRAME;
+    }
+    else if (referenceFrameStr == "tcp")
+    {
+        referenceFrame = TCP_FRAME;
+    }
+    else
+    {
+        CD_ERROR("Unsupported reference frame: %s.\n", referenceFrameStr.c_str());
+        return false;
+    }
+
     std::string kinematicsFile = config.check("kinematics", yarp::os::Value(""),
             "AMOR kinematics description").asString();