Merge branch 'fix-187-current' into develop

doc/yarp-devices-tricks.md

@@ -33,6 +33,11 @@ Note that this is a hack. VOCABs may be updated without warning. The recommended
 [set] [icmd] [cmod] 0 [torq]
 ```
 
+* set current mode (same as torque mode in TechnosoftIpos)
+```
+[set] [icmd] [cmod] 0 [icur]
+```
+
 ### in pos mode
 * set pos (for pos mode)
 ```
@@ -69,7 +74,7 @@ Note that this is a hack. VOCABs may be updated without warning. The recommended
 ```
 
 ### in torq mode
-* get actual torque/current
+* get actual torque
 ```
 [get] [torq] [trq] 0
 ```
@@ -79,6 +84,12 @@ Note that this is a hack. VOCABs may be updated without warning. The recommended
 [set] [torq] [ref] 0 1.0
 ```
 
+### in icur mode
+* get reference current
+```
+[get] [icur] [ref] 0
+```
+
 ### limits
 * get pos limits:
 ```

libraries/YarpPlugins/AmorControlboard/AmorControlboard.hpp

@@ -34,6 +34,7 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
                          public yarp::dev::IAxisInfo,
                          public yarp::dev::IControlLimits,
                          public yarp::dev::IControlMode,
+                         public yarp::dev::ICurrentControl,
                          public yarp::dev::IEncodersTimed,
                          public yarp::dev::IInteractionMode,
                          public yarp::dev::IPositionControl,
@@ -665,6 +666,81 @@ class AmorControlboard : public yarp::dev::DeviceDriver,
      */
     virtual bool setInteractionModes(yarp::dev::InteractionModeEnum* modes);
 
+//  --------- ICurrentControl Declarations. Implementation in ICurrentControlImpl.cpp ---------
+
+    /**
+     * Retrieves the number of controlled axes from the current physical interface.
+     * @param ax returns the number of controlled axes.
+     * @return true/false on success/failure
+     */
+    virtual bool getNumberOfMotors(int *ax);
+
+    /** Get the instantaneous current measurement for a single motor
+    * @param m motor number
+    * @param curr pointer to the result value. Value is expressed in amperes.
+    * @return true/false on success/failure
+    */
+    virtual bool getCurrent(int m, double *curr);
+
+    /** Get the instantaneous current measurement for all motors
+    * @param currs pointer to the array that will store the output. Values are expressed in amperes.
+    * @return true/false on success/failure
+    */
+    virtual bool getCurrents(double *currs);
+
+    /** Get the full scale of the current measurement for a given motor (e.g. -20A +20A)
+    * Reference values set by user with methods such as setRefCurrent() should be in this range.
+    * This method is not related to the current overload protection methods belonging to the iAmplifierControl interface.
+    * @param m motor number
+    * @param min minimum current of the motor m
+    * @param max maximum current of the motor m
+    * @return true/false on success/failure
+    */
+    virtual bool getCurrentRange(int m, double *min, double *max);
+
+    /** Get the full scale of the current measurements for all motors motor (e.g. -20A +20A)
+    * Reference values set by user with methods such as setRefCurrent() should be in this range.
+    * This method is not related to the current overload protection methods belonging to the iAmplifierControl interface.
+    * @param min pointer to the array that will store minimum currents
+    * @param max pointer to the array that will store maximum currents
+    * @return true/false on success/failure
+    */
+    virtual bool getCurrentRanges(double *min, double *max);
+
+    /** Set the reference value of the currents for all motors.
+    * @param currs the array containing the reference current values. Values are expressed in amperes.
+    * @return true/false on success/failure
+    */
+    virtual bool setRefCurrents(const double *currs);
+
+    /** Set the reference value of the current for a single motor.
+    * @param m motor number
+    * @param curr the current reference value for motor m. Value is expressed in amperes.
+    * @return true/false on success/failure
+    */
+    virtual bool setRefCurrent(int m, double curr);
+
+    /**  Set the reference value of the current for a group of motors.
+    * @param n_motor size of motors ans currs arrays
+    * @param motors  pointer to the array containing the list of motor numbers
+    * @param currs   pointer to the array specifying the new current references
+    * @return true/false on success/failure
+    */
+    virtual bool setRefCurrents(const int n_motor, const int *motors, const double *currs);
+
+   /** Get the reference value of the currents for all motors.
+     * @param currs pointer to the array to be filled with reference current values. Values are expressed in amperes.
+     * @return true/false on success/failure
+     */
+    virtual bool getRefCurrents(double *currs);
+
+    /** Get the reference value of the current for a single motor.
+    * @param m motor number
+    * @param curr the current reference value for motor m. Value is expressed in amperes.
+    * @return true/false on success/failure
+    */
+    virtual bool getRefCurrent(int m, double *curr);
+
 // -------- DeviceDriver declarations. Implementation in IDeviceDriverImpl.cpp --------
 
     /**

libraries/YarpPlugins/AmorControlboard/CMakeLists.txt

@@ -23,6 +23,7 @@ if(NOT SKIP_AmorControlboard)
                                      IAxisInfoImpl.cpp
                                      IControlLimitsImpl.cpp
                                      IControlModeImpl.cpp
+                                     ICurrentControlImpl.cpp
                                      IEncodersImpl.cpp
                                      IEncodersTimedImpl.cpp
                                      IInteractionModeImpl.cpp

libraries/YarpPlugins/AmorControlboard/ICurrentControlImpl.cpp

@@ -0,0 +1,219 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "AmorControlboard.hpp"
+
+#include <algorithm>
+
+// ------------------ ICurrentControl Related -----------------------------------------
+
+bool roboticslab::AmorControlboard::getNumberOfMotors(int *ax)
+{
+    CD_DEBUG("\n");
+    return getAxes(ax);
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::getCurrent(int m, double *curr)
+{
+    CD_DEBUG("(%d)\n", m);
+
+    if (!indexWithinRange(m))
+    {
+        return false;
+    }
+
+    AMOR_VECTOR7 currents;
+
+    if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    *curr = currents[m];
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::getCurrents(double *currs)
+{
+    CD_DEBUG("\n");
+
+    AMOR_VECTOR7 currents;
+
+    if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    std::copy(currents, currents + AMOR_NUM_JOINTS, currs);
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::getCurrentRange(int m, double *min, double *max)
+{
+    CD_DEBUG("(%d)\n", m);
+
+    if (!indexWithinRange(m))
+    {
+        return false;
+    }
+
+    AMOR_JOINT_INFO parameters;
+
+    if (amor_get_joint_info(handle, m, &parameters) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    *min = -parameters.maxCurrent;
+    *max = parameters.maxCurrent;
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::getCurrentRanges(double *min, double *max)
+{
+    CD_DEBUG("\n");
+
+    bool ok = true;
+
+    for (int j = 0; j < AMOR_NUM_JOINTS; j++)
+    {
+        ok &= getCurrentRange(j, &min[j], &max[j]);
+    }
+
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::setRefCurrents(const double *currs)
+{
+    CD_DEBUG("\n");
+
+    AMOR_VECTOR7 currents;
+
+    std::copy(currs, currs + AMOR_NUM_JOINTS, currents);
+
+    if (amor_set_currents(handle, currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::setRefCurrent(int m, double curr)
+{
+    CD_DEBUG("(%d)\n", m);
+
+    if (!indexWithinRange(m))
+    {
+        return false;
+    }
+
+    AMOR_VECTOR7 currents;
+
+    if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    currents[m] = curr;
+
+    if (amor_set_currents(handle, currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::setRefCurrents(const int n_motor, const int *motors, const double *currs)
+{
+    CD_DEBUG("(%d)\n", n_motor);
+
+    AMOR_VECTOR7 currents;
+
+    if (amor_get_actual_currents(handle, &currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    for (int i = 0; i < n_motor; i++)
+    {
+        currents[motors[i]] = currs[i];
+    }
+
+    if (amor_set_currents(handle, currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::getRefCurrents(double *currs)
+{
+    CD_DEBUG("\n");
+
+    AMOR_VECTOR7 currents;
+
+    if (amor_get_req_currents(handle, &currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    std::copy(currents, currents + AMOR_NUM_JOINTS, currs);
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::AmorControlboard::getRefCurrent(int m, double *curr)
+{
+    CD_DEBUG("(%d)\n", m);
+
+    if (!indexWithinRange(m))
+    {
+        return false;
+    }
+
+    AMOR_VECTOR7 currents;
+
+    if (amor_get_req_currents(handle, &currents) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    *curr = currents[m];
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------

libraries/YarpPlugins/CanBusControlboard/CMakeLists.txt

@@ -18,6 +18,7 @@ if(NOT SKIP_CanBusControlboard)
                                        DeviceDriverImpl.cpp
                                        IControlLimitsImpl.cpp
                                        IControlModeImpl.cpp
+                                       ICurrentControlImpl.cpp
                                        IEncodersImpl.cpp
                                        IEncodersTimedImpl.cpp
                                        IInteractionModeImpl.cpp