Fix force sign and clarify method contract

Author: PeterBowman

libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp

@@ -41,7 +41,7 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver,
     bool movl(const std::vector<double> & xd) override;
     bool movv(const std::vector<double> & xdotd) override;
     bool gcmp() override;
-    bool forc(const std::vector<double> & td) override;
+    bool forc(const std::vector<double> & fd) override;
     bool stopControl() override;
     bool wait(double timeout) override;
     bool tool(const std::vector<double> & x) override;

libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp

@@ -252,7 +252,7 @@ bool AmorCartesianControl::gcmp()
 
 // -----------------------------------------------------------------------------
 
-bool AmorCartesianControl::forc(const std::vector<double> &td)
+bool AmorCartesianControl::forc(const std::vector<double> &fd)
 {
     yCWarning(AMOR) << "forc() not implemented";
     return false;

libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp

@@ -120,7 +120,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     bool movl(const std::vector<double> & xd) override;
     bool movv(const std::vector<double> & xdotd) override;
     bool gcmp() override;
-    bool forc(const std::vector<double> & td) override;
+    bool forc(const std::vector<double> & fd) override;
     bool stopControl() override;
     bool wait(double timeout) override;
     bool tool(const std::vector<double> & x) override;
@@ -215,7 +215,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     std::vector<std::unique_ptr<KDL::Trajectory>> trajectories;
 
     /** FORC desired Cartesian force */
-    std::vector<double> td;
+    std::vector<double> fd;
 
     int encoderErrors {0};
     std::atomic_bool cmcSuccess;

libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp

@@ -5,6 +5,8 @@
 #include <cmath> //-- std::abs
 
 #include <algorithm> // std::transform
+#include <functional> // std::negate
+#include <iterator> // std::back_inserter
 #include <vector>
 
 #include <yarp/os/LogStream.h>
@@ -301,7 +303,6 @@ bool BasicCartesianControl::movv(const std::vector<double> &xdotd)
 
 bool BasicCartesianControl::gcmp()
 {
-    //-- Set torque mode and set state which makes periodic thread implement control.
     if (!setControlModes(VOCAB_CM_TORQUE))
     {
         yCError(BCC) << "Unable to set torque mode";
@@ -314,7 +315,7 @@ bool BasicCartesianControl::gcmp()
 
 // -----------------------------------------------------------------------------
 
-bool BasicCartesianControl::forc(const std::vector<double> &td)
+bool BasicCartesianControl::forc(const std::vector<double> &fd)
 {
     yCWarning(BCC) << "FORC mode still experimental";
 
@@ -324,8 +325,11 @@ bool BasicCartesianControl::forc(const std::vector<double> &td)
         return false;
     }
 
-    //-- Set torque mode and set state which makes periodic thread implement control.
-    this->td = td;
+    this->fd.clear();
+
+    // negate since the solver contract interprets the wrench as an external force applied on the
+    // end-effector, whereas the controller's contract interprets it as a force exerted by us
+    std::transform(fd.cbegin(), fd.cend(), std::back_inserter(this->fd), std::negate<>());
 
     if (!setControlModes(VOCAB_CM_TORQUE))
     {

libraries/YarpPlugins/BasicCartesianControl/PeriodicThreadImpl.cpp

@@ -346,7 +346,7 @@ void BasicCartesianControl::handleForc(const std::vector<double> &q, const std::
 
     std::vector<double> t(numJoints);
 
-    if (!iCartesianSolver->invDyn(q, qdot, qdotdot, td, t, referenceFrame))
+    if (!iCartesianSolver->invDyn(q, qdot, qdotdot, fd, t, referenceFrame))
     {
         yCWarning(BCC) << "invDyn() failed";
         return;

libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp

@@ -61,7 +61,7 @@ class CartesianControlClient : public yarp::dev::DeviceDriver,
     bool movl(const std::vector<double> &xd) override;
     bool movv(const std::vector<double> &xdotd) override;
     bool gcmp() override;
-    bool forc(const std::vector<double> &td) override;
+    bool forc(const std::vector<double> &fd) override;
     bool stopControl() override;
     bool wait(double timeout) override;
     bool tool(const std::vector<double> &x) override;

libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp

@@ -225,9 +225,9 @@ bool roboticslab::CartesianControlClient::gcmp()
 
 // -----------------------------------------------------------------------------
 
-bool roboticslab::CartesianControlClient::forc(const std::vector<double> &td)
+bool roboticslab::CartesianControlClient::forc(const std::vector<double> &fd)
 {
-    return handleRpcConsumerCmd(VOCAB_CC_FORC, td);
+    return handleRpcConsumerCmd(VOCAB_CC_FORC, fd);
 }
 
 // -----------------------------------------------------------------------------

libraries/YarpPlugins/ICartesianControl.h

@@ -254,13 +254,13 @@ class ICartesianControl
      *
      * Apply desired forces in task space.
      *
-     * @param td 6-element vector describing desired forces in cartesian space; first
-     * three elements denote translational acceleration (meters/second²), last three
-     * denote angular acceleration (radians/second²).
+     * @param fd 6-element vector describing desired force exerted by the TCP in
+     * cartesian space; first three elements denote linear force (Newton), last
+     * three denote torque (Newton*meters).
      *
      * @return true on success, false otherwise
      */
-    virtual bool forc(const std::vector<double> &td) = 0;
+    virtual bool forc(const std::vector<double> &fd) = 0;
 
     /**
      * @brief Stop control