Merge branch 'streaming-dev-shared-ctrl-act' into amor-cc-actuator

programs/streamingDeviceController/CMakeLists.txt

@@ -21,7 +21,9 @@ if(ENABLE_streamingDeviceController)
                                              WiimoteSensorDevice.hpp
                                              WiimoteSensorDevice.cpp
                                              StreamingDeviceController.hpp
-                                             StreamingDeviceController.cpp)
+                                             StreamingDeviceController.cpp
+                                             CentroidTransform.hpp
+                                             CentroidTransform.cpp)
 
     target_link_libraries(streamingDeviceController ${orocos_kdl_LIBRARIES}
                                                     YARP::YARP_OS

programs/streamingDeviceController/CentroidTransform.cpp

@@ -0,0 +1,99 @@
+#include "CentroidTransform.hpp"
+
+#include <cmath>
+
+#include <yarp/os/Time.h>
+
+#include <kdl/frames.hpp>
+
+#include <KdlVectorConverter.hpp>
+#include <ColorDebug.h>
+
+using namespace roboticslab;
+
+CentroidTransform::CentroidTransform()
+    : streamingDevice(NULL),
+      permanenceTime(0.0)
+{}
+
+bool CentroidTransform::setTcpToCameraRotation(yarp::os::Bottle * b)
+{
+    if (b->size() != 3)
+    {
+        CD_WARNING("Bottle size must equal 3, was: %d.\n", b->size());
+        return false;
+    }
+
+    double roll = b->get(0).asFloat64() * M_PI / 180.0;
+    double pitch = b->get(1).asFloat64() * M_PI / 180.0;
+    double yaw = b->get(2).asFloat64() * M_PI / 180.0;
+
+    CD_INFO("centroidFrameRPY [rad]: %f %f %f\n", roll, pitch, yaw);
+
+    rot_tcp_camera = KDL::Rotation::RPY(roll, pitch, yaw);
+
+    return true;
+}
+
+bool CentroidTransform::acceptBottle(yarp::os::Bottle * b)
+{
+    if (b)
+    {
+        if (b->size() != 2)
+        {
+            CD_WARNING("Malformed input bottle, size %d (expected 2).\n", b->size());
+            return false;
+        }
+
+        lastBottle = *b;
+        lastAcquisition.update();
+        return true;
+    }
+
+    return yarp::os::Time::now() - lastAcquisition.getTime() <= permanenceTime;
+}
+
+bool CentroidTransform::processStoredBottle() const
+{
+    // object centroids scaled to fit into [-1, 1] range
+    double cx = lastBottle.get(0).asFloat64(); // points right
+    double cy = lastBottle.get(1).asFloat64(); // points down
+
+    std::vector<double> x;
+
+    if (!streamingDevice->iCartesianControl->stat(x))
+    {
+        CD_WARNING("stat failed.\n");
+        return false;
+    }
+
+    KDL::Frame H_base_tcp = KdlVectorConverter::vectorToFrame(x);
+
+    // express camera's z axis (points "forward") in base frame
+    KDL::Vector v_base = H_base_tcp.M * rot_tcp_camera * KDL::Vector(0, 0, 1);
+    KDL::Frame H_base_target = KdlVectorConverter::vectorToFrame(streamingDevice->data);
+
+    double norm = KDL::dot(H_base_target.p, v_base);
+
+    if (norm <= 0.0)
+    {
+        // no action if we move away from the target (negative TCP's z axis)
+        return false;
+    }
+
+    // project target vector into TCP's z axis, refer result to base frame
+    H_base_target.p = v_base * norm;
+
+    // find axis along which to rotate (in TCP frame) given pixel coords
+    KDL::Vector coords(cx, cy, 0);
+    KDL::Vector tcp_axis = KDL::Rotation::RotZ(KDL::PI / 2) * coords;
+    KDL::Vector base_axis = H_base_tcp.M * rot_tcp_camera * tcp_axis;
+
+    // rotate towards the target in base frame
+    H_base_target.M = KDL::Rotation::Rot(base_axis, coords.Norm() * ROT_FACTOR);
+
+    // apply changes to input transform
+    streamingDevice->data = KdlVectorConverter::frameToVector(H_base_target);
+
+    return true;
+}

programs/streamingDeviceController/CentroidTransform.hpp

@@ -0,0 +1,58 @@
+#ifndef __CENTROID_TRANSFORM_HPP__
+#define __CENTROID_TRANSFORM_HPP__
+
+#include <yarp/os/Bottle.h>
+#include <yarp/os/Stamp.h>
+
+#include <kdl/frames.hpp>
+
+#include "StreamingDevice.hpp"
+
+#define ROT_FACTOR 0.1
+
+namespace roboticslab
+{
+
+class StreamingDevice;
+
+/**
+ * @ingroup streamingDeviceController
+ *
+ * @brief ...
+ */
+class CentroidTransform
+{
+public:
+
+    //! Constructor
+    CentroidTransform();
+
+    //! Register handle to device
+    void registerStreamingDevice(StreamingDevice * streamingDevice)
+    { this->streamingDevice = streamingDevice; }
+
+    //! Set TCP to camera frame
+    bool setTcpToCameraRotation(yarp::os::Bottle * b);
+
+    //! Set new permanence time
+    void setPermanenceTime(double permanenceTime)
+    { this->permanenceTime = permanenceTime; }
+
+    //! Register or dismiss incoming bottle
+    bool acceptBottle(yarp::os::Bottle * b);
+
+    //! Process last stored bottle
+    bool processStoredBottle() const;
+
+private:
+
+    StreamingDevice * streamingDevice;
+    double permanenceTime;
+    yarp::os::Bottle lastBottle;
+    yarp::os::Stamp lastAcquisition;
+    KDL::Rotation rot_tcp_camera;
+};
+
+}  // namespace roboticslab
+
+#endif  // __CENTROID_TRANSFORM_HPP__

programs/streamingDeviceController/LeapMotionSensorDevice.cpp

@@ -119,7 +119,7 @@ bool roboticslab::LeapMotionSensorDevice::acquireData()
 
     CD_DEBUG("%s\n", data.toString(4, 1).c_str());
 
-    if (data.size() < 6)
+    if (data.size() != 6 && data.size() != 8)
     {
         CD_WARNING("Invalid data size: %zu.\n", data.size());
         return false;

programs/streamingDeviceController/SpnavSensorDevice.cpp

@@ -8,7 +8,9 @@ roboticslab::SpnavSensorDevice::SpnavSensorDevice(yarp::os::Searchable & config,
     : StreamingDevice(config),
       iAnalogSensor(NULL),
       usingMovi(usingMovi),
-      gain(gain)
+      gain(gain),
+      buttonClose(false),
+      buttonOpen(false)
 {}
 
 bool roboticslab::SpnavSensorDevice::acquireInterfaces()
@@ -65,25 +67,31 @@ bool roboticslab::SpnavSensorDevice::acquireData()
 
     CD_DEBUG("%s\n", data.toString(4, 1).c_str());
 
-    if (data.size() != 8)
+    if (data.size() != 6 && data.size() != 8)
     {
         CD_WARNING("Invalid data size: %zu.\n", data.size());
         return false;
     }
 
-    for (int i = 0; i < data.size(); i++)
+    for (int i = 0; i < 6; i++)
     {
         this->data[i] = data[i];
     }
 
+    if (data.size() == 8)
+    {
+        buttonClose = data[6] == 1;
+        buttonOpen = data[7] == 1;
+    }
+
     return true;
 }
 
 bool roboticslab::SpnavSensorDevice::transformData(double scaling)
 {
     if (usingMovi)
     {
-        for (int i = 0; i < data.size(); i++)
+        for (int i = 0; i < 6; i++)
         {
             if (!fixedAxes[i])
             {
@@ -105,14 +113,11 @@ bool roboticslab::SpnavSensorDevice::transformData(double scaling)
 
 int roboticslab::SpnavSensorDevice::getActuatorState()
 {
-    int button1 = data[6];
-    int button2 = data[7];
-
-    if (button1 == 1)
+    if (buttonClose)
     {
         actuatorState = VOCAB_CC_ACTUATOR_CLOSE_GRIPPER;
     }
-    else if (button2 == 1)
+    else if (buttonOpen)
     {
         actuatorState = VOCAB_CC_ACTUATOR_OPEN_GRIPPER;
     }
@@ -139,7 +144,7 @@ bool roboticslab::SpnavSensorDevice::hasValidMovementData() const
 {
     if (usingMovi)
     {
-        for (int i = 0; i < data.size(); i++)
+        for (int i = 0; i < 6; i++)
         {
             if (!fixedAxes[i] && data[i] != currentX[i])
             {
@@ -161,7 +166,7 @@ void roboticslab::SpnavSensorDevice::sendMovementCommand()
     {
         iCartesianControl->movi(data);
 
-        for (int i = 0; i < data.size(); i++)
+        for (int i = 0; i < 6; i++)
         {
             currentX[i] = data[i];
         }

programs/streamingDeviceController/SpnavSensorDevice.hpp

@@ -45,6 +45,8 @@ class SpnavSensorDevice : public StreamingDevice
 
     bool usingMovi;
     double gain;
+    bool buttonClose;
+    bool buttonOpen;
 };
 
 }  // namespace roboticslab

programs/streamingDeviceController/StreamingDevice.cpp

@@ -58,7 +58,7 @@ StreamingDevice::~StreamingDevice()
 
 bool StreamingDevice::transformData(double scaling)
 {
-    for (int i = 0; i < data.size(); i++)
+    for (int i = 0; i < 6; i++)
     {
         if (!fixedAxes[i])
         {
@@ -80,7 +80,7 @@ bool StreamingDevice::hasValidMovementData() const
         return false;
     }
 
-    for (int i = 0; i < data.size(); i++)
+    for (int i = 0; i < 6; i++)
     {
         if (data[i] != 0.0)
         {

programs/streamingDeviceController/StreamingDevice.hpp

@@ -9,11 +9,13 @@
 #include <yarp/dev/PolyDriver.h>
 
 #include "ICartesianControl.h"
+#include "CentroidTransform.hpp"
 
 namespace roboticslab
 {
 
 class StreamingDevice;
+class CentroidTransform;
 
 /**
  * @ingroup streamingDeviceController
@@ -42,6 +44,8 @@ class StreamingDeviceFactory
  */
 class StreamingDevice : protected yarp::dev::PolyDriver
 {
+    friend CentroidTransform;
+
 public:
 
     using PolyDriver::isValid;