[RealToSimControlboard] ini

libraries/YarpPlugins/CMakeLists.txt

@@ -3,6 +3,7 @@
 # CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 
 # YARP devices.
+add_subdirectory(RealToSimControlboard)
 add_subdirectory(YarpOpenraveControlboard)
 add_subdirectory(YarpOpenraveGrabber)
 add_subdirectory(YarpOpenraveRGBDSensor)

libraries/YarpPlugins/RealToSimControlboard/CMakeLists.txt

@@ -0,0 +1,38 @@
+# 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
+                                      DeviceDriverImpl.cpp
+                                      IEncodersTimedImpl.cpp
+                                      IPositionControlImpl.cpp
+                                      IVelocityControlImpl.cpp)
+
+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-OPENRAVE-YARP-PLUGINS_DYNAMIC_PLUGINS_INSTALL_DIR}
+                 ARCHIVE DESTINATION ${ROBOTICSLAB-OPENRAVE-YARP-PLUGINS_STATIC_PLUGINS_INSTALL_DIR}
+                 YARP_INI DESTINATION ${ROBOTICSLAB-OPENRAVE-YARP-PLUGINS_PLUGIN_MANIFESTS_INSTALL_DIR})
+else()
+    yarp_install(TARGETS RealToSimControlboard
+                 LIBRARY DESTINATION ${ROBOTICSLAB-OPENRAVE-YARP-PLUGINS_DYNAMIC_PLUGINS_INSTALL_DIR}
+                 ARCHIVE DESTINATION ${ROBOTICSLAB-OPENRAVE-YARP-PLUGINS_STATIC_PLUGINS_INSTALL_DIR})
+
+    yarp_install(FILES RealToSimControlboard.ini
+                 DESTINATION ${ROBOTICSLAB-OPENRAVE-YARP-PLUGINS_PLUGIN_MANIFESTS_INSTALL_DIR})
+endif()
+
+endif()

libraries/YarpPlugins/RealToSimControlboard/DeviceDriverImpl.cpp

@@ -0,0 +1,27 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "RealToSimControlboard.hpp"
+
+namespace roboticslab
+{
+
+// ------------------- DeviceDriver Related ------------------------------------
+
+bool RealToSimControlboard::open(yarp::os::Searchable& config)
+{
+    CD_DEBUG("config: %s\n",config.toString().c_str());
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool RealToSimControlboard::close()
+{
+    CD_INFO("\n");
+    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 = radToDegIfNotPrismatic(j, vectorOfJointPtr[j]->GetValue(0) );
+
+    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;
+}
+
+// -----------------------------------------------------------------------------
+
+