Merge branch 'realToSim' into develop

- #15

libraries/YarpPlugins/CMakeLists.txt

@@ -21,3 +21,4 @@ install(TARGETS ToolsInterfaces
 
 # YARP devices.
 add_subdirectory(PlaybackThread)
+add_subdirectory(RealToSimControlboard)

libraries/YarpPlugins/RealToSimControlboard/CMakeLists.txt

@@ -0,0 +1,41 @@
+# Copyright: (C) 2019 Universidad Carlos III de Madrid
+# Author: Juan G. Victores
+
+yarp_prepare_plugin(RealToSimControlboard
+                    CATEGORY device
+                    TYPE roboticslab::RealToSimControlboard
+                    INCLUDE RealToSimControlboard.hpp
+                    DEFAULT ON)
+
+if(NOT SKIP_RealToSimControlboard)
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}) # yarp plugin builder needs this
+
+yarp_add_plugin(RealToSimControlboard RealToSimControlboard.hpp
+                                      RealToSimControlboard.cpp
+                                      DeviceDriverImpl.cpp
+                                      IEncodersTimedImpl.cpp
+                                      IPositionControlImpl.cpp
+                                      IVelocityControlImpl.cpp
+                                      Transformation.cpp
+                                      Transformation.hpp)
+
+target_link_libraries(RealToSimControlboard YARP::YARP_OS
+                                            YARP::YARP_dev
+                                            ROBOTICSLAB::ColorDebug)
+
+if(NOT YARP_VERSION_SHORT VERSION_LESS 3.2)
+    yarp_install(TARGETS RealToSimControlboard
+                 LIBRARY DESTINATION ${ROBOTICSLAB-TOOLS_DYNAMIC_PLUGINS_INSTALL_DIR}
+                 ARCHIVE DESTINATION ${ROBOTICSLAB-TOOLS_STATIC_PLUGINS_INSTALL_DIR}
+                 YARP_INI DESTINATION ${ROBOTICSLAB-TOOLS_PLUGIN_MANIFESTS_INSTALL_DIR})
+else()
+    yarp_install(TARGETS RealToSimControlboard
+                 LIBRARY DESTINATION ${ROBOTICSLAB-TOOLS_DYNAMIC_PLUGINS_INSTALL_DIR}
+                 ARCHIVE DESTINATION ${ROBOTICSLAB-TOOLS_STATIC_PLUGINS_INSTALL_DIR})
+
+    yarp_install(FILES RealToSimControlboard.ini
+                 DESTINATION ${ROBOTICSLAB-TOOLS_PLUGIN_MANIFESTS_INSTALL_DIR})
+endif()
+
+endif()

libraries/YarpPlugins/RealToSimControlboard/DeviceDriverImpl.cpp

@@ -0,0 +1,125 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "RealToSimControlboard.hpp"
+
+#include <sstream>
+
+namespace roboticslab
+{
+
+// ------------------- DeviceDriver Related ------------------------------------
+
+bool RealToSimControlboard::open(yarp::os::Searchable& config)
+{
+    CD_DEBUG("config: %s\n",config.toString().c_str());
+
+    if(!config.check("remotes", "list of remotes to which this device connects"))
+    {
+        CD_ERROR("Required \"remotes\" list\n");
+        return false;
+    }
+    yarp::os::Bottle* controlledDeviceList = config.find("remotes").asList();
+    if (!controlledDeviceList)
+    {
+        CD_ERROR("Error parsing parameters: \"remotes\" should be followed by a list\n");
+        return false;
+    }
+    CD_DEBUG("%s\n", controlledDeviceList->toString().c_str());
+
+    std::map<std::string,int> controlledDeviceNameToIdx;
+    for(size_t controlledDeviceIdx=0; controlledDeviceIdx< controlledDeviceList->size(); controlledDeviceIdx++)
+    {
+        std::string controlledDeviceName = controlledDeviceList->get(controlledDeviceIdx).asString();
+        if(!config.check(controlledDeviceName))
+        {
+            CD_ERROR("\"%s\" group not found!\n", controlledDeviceName.c_str());
+            return false;
+        }
+        CD_SUCCESS("\"%s\" group found!\n", controlledDeviceName.c_str());
+        yarp::os::Bottle controlledDeviceGroup = config.findGroup(controlledDeviceName);
+        CD_INFO("%s\n", controlledDeviceGroup.toString().c_str());
+        yarp::os::Property controlledDeviceOptions;
+        controlledDeviceOptions.fromString(controlledDeviceGroup.toString());
+
+        yarp::dev::PolyDriver* controlledDevice = new yarp::dev::PolyDriver(controlledDeviceOptions);
+        if( ! controlledDevice->isValid() )
+        {
+            CD_ERROR("\"%s\" group did not create a valid yarp device!\n", controlledDeviceName.c_str());
+            return false;
+        }
+        CD_SUCCESS("\"%s\" group created a valid yarp device!\n", controlledDeviceName.c_str());
+
+        controlledDeviceNameToIdx[controlledDeviceName] = controlledDeviceIdx;
+        controlledDevices.push_back(controlledDevice);
+    }
+
+    int idx = 0;
+    while(true)
+    {
+        std::ostringstream exposedJointNameStream("exposed_joint_", std::ios_base::app);
+        exposedJointNameStream << idx;
+        std::string exposedJointName(exposedJointNameStream.str());
+        if(!config.check(exposedJointName))
+        {
+            axes = idx;
+            storedPositions.resize(axes);
+            CD_INFO("Could not find \"%s\" group, setting number of exposed joints to %d.\n",exposedJointName.c_str(), axes);
+            break;
+        }
+        CD_SUCCESS("* %s group found!\n", exposedJointName.c_str());
+
+        ExposedJoint* exposedJoint = new ExposedJoint(exposedJointName);
+        exposedJoints.push_back(exposedJoint);
+
+        yarp::os::Bottle exposedJointGroup = config.findGroup(exposedJointName);
+        CD_DEBUG("* %s\n", exposedJointGroup.toString().c_str());
+        for(size_t exposedJointControlledDeviceIdx=1; exposedJointControlledDeviceIdx<exposedJointGroup.size(); exposedJointControlledDeviceIdx++)
+        {
+            yarp::os::Bottle* exposedJointControlledDeviceGroup = exposedJointGroup.get(exposedJointControlledDeviceIdx).asList();
+            CD_DEBUG("** %s\n", exposedJointControlledDeviceGroup->toString().c_str());
+            std::string exposedJointControlledDeviceName = exposedJointControlledDeviceGroup->get(0).asString();
+            int idx = controlledDeviceNameToIdx[exposedJointControlledDeviceName];
+            CD_DEBUG("** %s [%d]\n", exposedJointControlledDeviceName.c_str(), idx);
+
+            ExposedJointControlledDevice* exposedJointControlledDevice =
+                new ExposedJointControlledDevice(exposedJointControlledDeviceName, controlledDevices[idx]);
+            exposedJoint->addExposedJointControlledDevice(exposedJointControlledDevice);
+
+            for(size_t exposedJointControlledDeviceJointIdx=1; exposedJointControlledDeviceJointIdx<exposedJointControlledDeviceGroup->size(); exposedJointControlledDeviceJointIdx++)
+            {
+                yarp::os::Bottle* exposedJointControlledDeviceJointGroup = exposedJointControlledDeviceGroup->get(exposedJointControlledDeviceJointIdx).asList();
+                CD_DEBUG("*** %s\n", exposedJointControlledDeviceJointGroup->toString().c_str());
+
+                if(!exposedJointControlledDevice->addControlledDeviceJoint(exposedJointControlledDeviceJointGroup))
+                    return false;
+            }
+        }
+
+        idx++;
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool RealToSimControlboard::close()
+{
+    CD_INFO("\n");
+    for(size_t i=0;i<controlledDevices.size();i++)
+    {
+        controlledDevices[i]->close();
+        delete controlledDevices[i];
+        controlledDevices[i] = 0;
+    }
+    for(size_t i=0;i<exposedJoints.size();i++)
+    {
+        delete exposedJoints[i];
+        exposedJoints[i] = 0;
+    }
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+}  // namespace roboticslab

libraries/YarpPlugins/RealToSimControlboard/IEncodersTimedImpl.cpp

@@ -0,0 +1,115 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "RealToSimControlboard.hpp"
+
+// ------------------ IEncodersTimed Related -----------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncodersTimed(double *encs, double *time) {
+    //CD_INFO("\n");  //-- Way too verbose
+    bool ok = true;
+    for(unsigned int i=0; i < axes; i++)
+        ok &= getEncoderTimed(i,&(encs[i]),&(time[i]));
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncoderTimed(int j, double *encs, double *time) {
+    //CD_INFO("(%d)\n",j);  //-- Way too verbose
+
+    getEncoder(j, encs);
+    *time = yarp::os::Time::now();
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::resetEncoder(int j) {
+    CD_INFO("\n");
+    if ((unsigned int)j>axes) return false;
+    return setEncoder(j,0.0);
+  }
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::resetEncoders() {
+    CD_INFO("\n");
+    bool ok = true;
+    for(unsigned int i=0;i<axes;i++)
+        ok &= resetEncoder(i);
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::setEncoder(int j, double val) {  // encExposed = val;
+    CD_INFO("\n");
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::setEncoders(const double *vals) {
+    CD_INFO("\n");
+    bool ok = true;
+    for(unsigned int i=0;i<axes;i++)
+        ok &= setEncoder(i,vals[i]);
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncoder(int j, double *v) {
+    //CD_INFO("\n");  //-- Way too verbose
+    *v = storedPositions[j];
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncoders(double *encs) {
+    //CD_INFO("\n");  //-- Way too verbose
+    bool ok = true;
+    for(unsigned int i=0;i<axes;i++)
+        ok &= getEncoder(i,&encs[i]);
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncoderSpeed(int j, double *sp) {
+    //CD_INFO("\n");  //-- Way too verbose
+    // Make it easy, give the current reference speed.
+    *sp = 0;  // begins to look like we should use semaphores.
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncoderSpeeds(double *spds) {
+    //CD_INFO("\n");  //-- Way too verbose
+    bool ok = true;
+    //for(unsigned int i=0;i<axes;i++)
+    //    ok &= getEncoderSpeed(i,&spds[i]);
+    return ok;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncoderAcceleration(int j, double *spds) {
+    //CD_INFO("\n");  //-- Way too verbose
+    return false;
+}
+
+// -----------------------------------------------------------------------------
+
+bool roboticslab::RealToSimControlboard::getEncoderAccelerations(double *accs) {
+    //CD_INFO("\n");  //-- Way too verbose
+    return false;
+}
+
+// -----------------------------------------------------------------------------
+
+