Prepare cartesian controller interface for tool actuation (AMOR)

libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp

@@ -74,6 +74,8 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
 
     virtual bool tool(const std::vector<double> &x);
 
+    virtual bool act(int command);
+
     virtual void twist(const std::vector<double> &xdot);
 
     virtual void pose(const std::vector<double> &x, double interval);

libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp

@@ -7,6 +7,8 @@
 
 #include <ColorDebug.h>
 
+#include <yarp/os/Vocab.h>
+
 #include "KinematicRepresentation.hpp"
 
 // ------------------- ICartesianControl Related ------------------------------------
@@ -321,6 +323,37 @@ bool roboticslab::AmorCartesianControl::tool(const std::vector<double> &x)
 
 // -----------------------------------------------------------------------------
 
+bool roboticslab::AmorCartesianControl::act(int command)
+{
+    AMOR_RESULT (*amor_command)(AMOR_HANDLE);
+
+    switch (command)
+    {
+    case VOCAB_CC_ACTUATOR_CLOSE_GRIPPER:
+        amor_command = amor_close_hand;
+        break;
+    case VOCAB_CC_ACTUATOR_OPEN_GRIPPER:
+        amor_command = amor_open_hand;
+        break;
+    case VOCAB_CC_ACTUATOR_STOP_GRIPPER:
+        amor_command = amor_stop_hand;
+        break;
+    default:
+        CD_ERROR("Unrecognized command with code %d (%s).\n", command, yarp::os::Vocab::decode(command).c_str());
+        return false;
+    }
+
+    if (amor_command(handle) != AMOR_SUCCESS)
+    {
+        CD_ERROR("%s\n", amor_error());
+        return false;
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
 void roboticslab::AmorCartesianControl::twist(const std::vector<double> &xdot)
 {
     AMOR_VECTOR7 positions;

libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp

@@ -165,6 +165,8 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
 
     virtual bool tool(const std::vector<double> &x);
 
+    virtual bool act(int command);
+
     virtual void twist(const std::vector<double> &xdot);
 
     virtual void pose(const std::vector<double> &x, double interval);

libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp

@@ -438,6 +438,12 @@ bool roboticslab::BasicCartesianControl::tool(const std::vector<double> &x)
 
 // -----------------------------------------------------------------------------
 
+bool roboticslab::BasicCartesianControl::act(int command)
+{
+    CD_ERROR("Not implemented.\n");
+    return false;
+}
+
 void roboticslab::BasicCartesianControl::twist(const std::vector<double> &xdot)
 {
     if (getCurrentState() != VOCAB_CC_NOT_CONTROLLING || streamingCommand != VOCAB_CC_TWIST)

libraries/YarpPlugins/CartesianControlClient/CartesianControlClient.hpp

@@ -83,6 +83,8 @@ class CartesianControlClient : public yarp::dev::DeviceDriver, public ICartesian
 
     virtual bool tool(const std::vector<double> &x);
 
+    virtual bool act(int command);
+
     virtual void twist(const std::vector<double> &xdot);
 
     virtual void pose(const std::vector<double> &x, double interval);

libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp

@@ -260,6 +260,20 @@ bool roboticslab::CartesianControlClient::tool(const std::vector<double> &x)
 
 // -----------------------------------------------------------------------------
 
+bool roboticslab::CartesianControlClient::act(int command)
+{
+    yarp::os::Bottle cmd, response;
+
+    cmd.addVocab(VOCAB_CC_ACT);
+    cmd.addVocab(command);
+
+    rpcClient.write(cmd,response);
+
+    return checkSuccess(response);
+}
+
+// -----------------------------------------------------------------------------
+
 void roboticslab::CartesianControlClient::twist(const std::vector<double> &xdot)
 {
     handleStreamingConsumerCmd(VOCAB_CC_TWIST, xdot);

libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp

@@ -130,6 +130,7 @@ class RpcResponder : public yarp::dev::DeviceResponder
 
     bool handleStatMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out);
     bool handleWaitMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out);
+    bool handleActMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out);
 
     bool handleRunnableCmdMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out, RunnableFun cmd);
     bool handleConsumerCmdMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out, ConsumerFun cmd);

libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp

@@ -74,6 +74,8 @@ bool roboticslab::RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bo
         return handleWaitMsg(in, out);
     case VOCAB_CC_TOOL:
         return handleConsumerCmdMsg(in, out, &ICartesianControl::tool);
+    case VOCAB_CC_ACT:
+        return handleActMsg(in, out);
     case VOCAB_CC_SET:
         return isGroupParam(in) ? handleParameterSetterGroup(in, out) : handleParameterSetter(in, out);
     case VOCAB_CC_GET:
@@ -133,6 +135,10 @@ void roboticslab::RpcResponder::makeUsage()
     addUsage(ss.str().c_str(), "append fixed link to end effector");
     ss.str("");
 
+    ss << "[" << yarp::os::Vocab::decode(VOCAB_CC_ACT) << "] vocab";
+    addUsage(ss.str().c_str(), "actuate tool using selected command vocab");
+    ss.str("");
+
     ss << "[" << yarp::os::Vocab::decode(VOCAB_CC_SET) << "] vocab value";
     addUsage(ss.str().c_str(), "set configuration parameter");
     ss.str("");
@@ -254,6 +260,33 @@ bool roboticslab::RpcResponder::handleWaitMsg(const yarp::os::Bottle& in, yarp::
 
 // -----------------------------------------------------------------------------
 
+bool roboticslab::RpcResponder::handleActMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out)
+{
+    if (in.size() > 1)
+    {
+        int commandCode = in.get(1).asVocab();
+
+        if (iCartesianControl->act(commandCode))
+        {
+            out.addVocab(VOCAB_CC_OK);
+            return true;
+        }
+        else
+        {
+            out.addVocab(VOCAB_CC_FAILED);
+            return false;
+        }
+    }
+    else
+    {
+        CD_ERROR("size error\n");
+        out.addVocab(VOCAB_CC_FAILED);
+        return false;
+    }
+}
+
+// -----------------------------------------------------------------------------
+
 bool roboticslab::RpcResponder::handleRunnableCmdMsg(const yarp::os::Bottle& in, yarp::os::Bottle& out, RunnableFun cmd)
 {
     if ((iCartesianControl->*cmd)())

libraries/YarpPlugins/ICartesianControl.h

@@ -63,6 +63,7 @@
 #define VOCAB_CC_STOP ROBOTICSLAB_VOCAB('s','t','o','p') ///< Stop control
 #define VOCAB_CC_WAIT ROBOTICSLAB_VOCAB('w','a','i','t') ///< Wait motion done
 #define VOCAB_CC_TOOL ROBOTICSLAB_VOCAB('t','o','o','l') ///< Change tool
+#define VOCAB_CC_ACT ROBOTICSLAB_VOCAB('a','c','t',0)    ///< Actuate tool
 
 /** @} */
 
@@ -99,6 +100,22 @@
 
 /** @} */
 
+/**
+ * @anchor ICartesianControl_actuator_vocabs
+ * @name Actuator control vocabs
+ *
+ * Used by roboticslab::ICartesianControl::act to control the actuator.
+ *
+ * @{
+ */
+
+// Actuator control
+#define VOCAB_CC_ACTUATOR_NONE ROBOTICSLAB_VOCAB('a','c','n',0)            ///< No actuator or no action
+#define VOCAB_CC_ACTUATOR_CLOSE_GRIPPER ROBOTICSLAB_VOCAB('a','c','c','g') ///< Close gripper
+#define VOCAB_CC_ACTUATOR_OPEN_GRIPPER ROBOTICSLAB_VOCAB('a','c','o','g')  ///< Open gripper
+#define VOCAB_CC_ACTUATOR_STOP_GRIPPER ROBOTICSLAB_VOCAB('a','c','s','g')  ///< Stop gripper
+#define VOCAB_CC_ACTUATOR_GENERIC ROBOTICSLAB_VOCAB('a','c','g',0)         ///< Generic actuator
+
 /**
  * @name Controller configuration vocabs
  *
@@ -296,6 +313,17 @@ class ICartesianControl
          */
         virtual bool tool(const std::vector<double> &x) = 0;
 
+        /**
+         * @brief Actuate tool
+         *
+         * Send control command to actuate the robot's tool, if available.
+         *
+         * @param command One of available @ref ICartesianControl_actuator_vocabs "actuator vocabs".
+         *
+         * @return true on success, false otherwise
+         */
+        virtual bool act(int command) = 0;
+
         /** @} */
 
         //--------------------- Streaming commands ---------------------

programs/CMakeLists.txt

@@ -1,5 +1,6 @@
 # programs
 
+add_subdirectory(haarDetectionController)
 add_subdirectory(keyboardController)
 add_subdirectory(streamingDeviceController)
 add_subdirectory(transCoords)

programs/haarDetectionController/CMakeLists.txt

@@ -0,0 +1,30 @@
+if(NOT ROBOTICSLAB_YARP_DEVICES_FOUND AND (NOT DEFINED ENABLE_haarDetectionController OR ENABLE_haarDetectionController))
+    message(WARNING "ROBOTICSLAB_YARP_DEVICES package not found, disabling haarDetectionController program")
+endif()
+
+cmake_dependent_option(ENABLE_haarDetectionController "Enable/disable haarDetectionController program" ON
+                       ROBOTICSLAB_YARP_DEVICES_FOUND OFF)
+
+if(ENABLE_haarDetectionController)
+
+    add_executable(haarDetectionController main.cpp
+                                           HaarDetectionController.hpp
+                                           HaarDetectionController.cpp
+                                           GrabberResponder.hpp
+                                           GrabberResponder.cpp)
+
+    target_link_libraries(haarDetectionController YARP::YARP_OS
+                                                  YARP::YARP_init
+                                                  YARP::YARP_dev
+                                                  ROBOTICSLAB::ColorDebug
+                                                  KinematicsDynamicsInterfaces
+                                                  ROBOTICSLAB::YarpDevicesInterfaces)
+
+    install(TARGETS haarDetectionController
+            DESTINATION ${CMAKE_INSTALL_BINDIR})
+
+else()
+
+    set(ENABLE_haarDetectionController OFF CACHE BOOL "Enable/disable haarDetectionController program" FORCE)
+
+endif()

programs/haarDetectionController/GrabberResponder.cpp

@@ -0,0 +1,71 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "GrabberResponder.hpp"
+
+#include <cmath>
+#include <vector>
+
+#include <ColorDebug.h>
+
+namespace
+{
+    const double STEP_X = 0.0001;
+    const double STEP_Y = 0.0001;
+    const double STEP_Z = 0.01;
+
+    // just in case, not used by now
+    const double DEADBAND_X_PX = 0.0;
+    const double DEADBAND_Y_PX = 0.0;
+}
+
+void roboticslab::GrabberResponder::onRead(yarp::os::Bottle &b)
+{
+    CD_DEBUG("Got: %s\n", b.toString().c_str());
+
+    if (b.size() != 2)
+    {
+        CD_WARNING("Wrong data size: %d (expected: 2).\n", b.size());
+        return;
+    }
+
+    const double input_x = b.get(0).asFloat64();
+    const double input_y = b.get(1).asFloat64();
+
+    double target_x = 0.0;
+    double target_y = 0.0;
+
+    bool move = false;
+
+    if (std::abs(input_x) > DEADBAND_X_PX)
+    {
+        target_x = input_x * STEP_X;
+        move = true;
+    }
+
+    if (std::abs(input_y) > DEADBAND_Y_PX)
+    {
+        target_y = input_y * STEP_Y;
+        move = true;
+    }
+
+    if (move)
+    {
+        std::vector<double> xdot(6);
+        xdot[0] = target_x;
+        xdot[1] = -target_y;  // inverted for AMOR
+        xdot[2] = noApproach ? 0.0 : STEP_Z;
+
+        iCartesianControl->twist(xdot);
+
+        isStopped = false;
+    }
+    else if (!isStopped)
+    {
+        iCartesianControl->stopControl();
+        isStopped = true;
+    }
+    else
+    {
+        isStopped = true;
+    }
+}

programs/haarDetectionController/GrabberResponder.hpp

@@ -0,0 +1,50 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#ifndef __GRABBER_RESPONDER_HPP__
+#define __GRABBER_RESPONDER_HPP__
+
+#include <yarp/os/Bottle.h>
+#include <yarp/os/PortReaderBuffer.h>
+
+#include "ICartesianControl.h"
+
+namespace roboticslab
+{
+
+/**
+ * @ingroup haarDetectionController
+ *
+ * @brief Callback class for dealing with incoming grabber
+ * data streams.
+ */
+class GrabberResponder : public yarp::os::TypedReaderCallback<yarp::os::Bottle>
+{
+public:
+
+    GrabberResponder() : iCartesianControl(NULL),
+                         isStopped(true),
+                         noApproach(false)
+    {}
+
+    virtual void onRead(yarp::os::Bottle &b);
+
+    void setICartesianControlDriver(roboticslab::ICartesianControl *iCartesianControl)
+    {
+        this->iCartesianControl = iCartesianControl;
+    }
+
+    void setNoApproachSetting(bool noApproach)
+    {
+        this->noApproach = noApproach;
+    }
+
+private:
+
+    roboticslab::ICartesianControl *iCartesianControl;
+
+    bool isStopped, noApproach;
+};
+
+}  // namespace roboticslab
+
+#endif  // __GRABBER_RESPONDER_HPP__