Import AmorCartesianControl from roboticslab-uc3m/kinematics-dynamics

CMakeLists.txt

@@ -32,6 +32,7 @@ find_package(YARP 3.7 REQUIRED COMPONENTS os dev)
 
 # Soft dependencies.
 find_package(AMOR_API QUIET)
+find_package(ROBOTICSLAB_KINEMATICS_DYNAMICS QUIET)
 find_package(Doxygen QUIET)
 find_package(GTestSources 1.8 QUIET)
 

libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp

@@ -0,0 +1,29 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "AmorCartesianControl.hpp"
+
+#include <cmath>
+
+#include <yarp/os/Log.h>
+
+#include "LogComponent.hpp"
+
+using namespace roboticslab;
+
+// -----------------------------------------------------------------------------
+
+bool AmorCartesianControl::checkJointVelocities(const std::vector<double> & qdot)
+{
+    for (unsigned int i = 0; i < qdot.size(); i++)
+    {
+        if (std::abs(qdot[i]) > qdotMax[i])
+        {
+            yCError(ACC, "Maximum angular velocity hit: qdot[%d] = %f > %f [deg/s]", i, qdot[i], qdotMax[i]);
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------

libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp

@@ -0,0 +1,82 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#ifndef __AMOR_CARTESIAN_CONTROL_HPP__
+#define __AMOR_CARTESIAN_CONTROL_HPP__
+
+#include <mutex>
+#include <vector>
+
+#include <amor.h>
+
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/PolyDriver.h>
+
+#include "ICartesianControl.h"
+#include "ICartesianSolver.h"
+
+namespace roboticslab
+{
+
+/**
+ * @ingroup YarpPlugins
+ * @defgroup AmorCartesianControl
+ * @brief Contains roboticslab::AmorCartesianControl.
+ */
+
+/**
+ * @ingroup AmorCartesianControl
+ * @brief The AmorCartesianControl class implements ICartesianControl.
+ *
+ * Uses the roll-pitch-yaw (RPY) angle representation.
+ */
+class AmorCartesianControl : public yarp::dev::DeviceDriver,
+                             public ICartesianControl
+{
+public:
+    // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp --
+    bool stat(std::vector<double> & x, int * state = nullptr, double * timestamp = nullptr) override;
+    bool inv(const std::vector<double> & xd, std::vector<double> & q) override;
+    bool movj(const std::vector<double> & xd) override;
+    bool relj(const std::vector<double> & xd) override;
+    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> & fd) override;
+    bool stopControl() override;
+    bool wait(double timeout) override;
+    bool tool(const std::vector<double> & x) override;
+    bool act(int command) override;
+    void movi(const std::vector<double> & x) override;
+    void twist(const std::vector<double> & xdot) override;
+    void wrench(const std::vector<double> &w) override;
+    bool setParameter(int vocab, double value) override;
+    bool getParameter(int vocab, double * value) override;
+    bool setParameters(const std::map<int, double> & params) override;
+    bool getParameters(std::map<int, double> & params) override;
+
+    // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
+    bool open(yarp::os::Searchable & config) override;
+    bool close() override;
+
+private:
+    bool checkJointVelocities(const std::vector<double> & qdot);
+
+    AMOR_HANDLE handle {AMOR_INVALID_HANDLE};
+    bool ownsHandle {true};
+    mutable std::mutex * handleMutex {nullptr};
+
+    yarp::dev::PolyDriver cartesianDevice;
+    ICartesianSolver * iCartesianSolver;
+
+    int currentState;
+    double gain;
+    int waitPeriodMs;
+
+    std::vector<double> qdotMax;
+
+    ICartesianSolver::reference_frame referenceFrame;
+};
+
+} // namespace roboticslab
+
+#endif // __AMOR_CARTESIAN_CONTROL_HPP__

libraries/YarpPlugins/AmorCartesianControl/CMakeLists.txt

@@ -0,0 +1,43 @@
+if(NOT DEFINED ENABLE_AmorCartesianControl OR ENABLE_AmorCartesianControl)
+    if(NOT TARGET AMOR::amor_api)
+        message(WARNING "amor_api target not found, disabling AmorCartesianControl")
+    elseif(NOT TARGET )
+        message(WARNING "KinematicRepresentationLib target not found, disabling AmorCartesianControl")
+    elseif(NOT TARGET )
+        message(WARNING "KinematicsDynamicsInterfaces target not found, disabling AmorCartesianControl")
+    endif()
+endif()
+
+yarp_prepare_plugin(AmorCartesianControl
+                    CATEGORY device
+                    TYPE roboticslab::AmorCartesianControl
+                    INCLUDE AmorCartesianControl.hpp
+                    DEFAULT ON
+                    DEPENDS "TARGET ROBOTICSLAB::KinematicRepresentationLib;TARGET ROBOTICSLAB::KinematicsDynamicsInterfaces;TARGET AMOR::amor_api"
+                    EXTRA_CONFIG WRAPPER=CartesianControlServer)
+
+if(NOT SKIP_AmorCartesianControl)
+
+    yarp_add_plugin(AmorCartesianControl AmorCartesianControl.hpp
+                                         AmorCartesianControl.cpp
+                                         DeviceDriverImpl.cpp
+                                         ICartesianControlImpl.cpp
+                                         LogComponent.hpp
+                                         LogComponent.cpp)
+
+    target_link_libraries(AmorCartesianControl YARP::YARP_os
+                                               YARP::YARP_dev
+                                               AMOR::amor_api
+                                               ROBOTICSLAB::KinematicRepresentationLib
+                                               ROBOTICSLAB::KinematicsDynamicsInterfaces)
+
+    yarp_install(TARGETS AmorCartesianControl
+                 LIBRARY DESTINATION ${AMOR-KINEMATICS-DYNAMICS_DYNAMIC_PLUGINS_INSTALL_DIR}
+                 ARCHIVE DESTINATION ${AMOR-KINEMATICS-DYNAMICS_STATIC_PLUGINS_INSTALL_DIR}
+                 YARP_INI DESTINATION ${AMOR-KINEMATICS-DYNAMICS_PLUGIN_MANIFESTS_INSTALL_DIR})
+
+else()
+
+    set(ENABLE_AmorCartesianControl OFF CACHE BOOL "Enable/disable AmorCartesianControl device" FORCE)
+
+endif()

libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp

@@ -0,0 +1,158 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "AmorCartesianControl.hpp"
+
+#include <string>
+
+#include <yarp/conf/version.h>
+
+#include <yarp/os/Bottle.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Property.h>
+#include <yarp/os/Value.h>
+
+#include "KinematicRepresentation.hpp"
+#include "LogComponent.hpp"
+
+using namespace roboticslab;
+
+constexpr auto DEFAULT_CAN_LIBRARY = "libeddriver.so";
+constexpr auto DEFAULT_CAN_PORT = 0;
+constexpr auto DEFAULT_GAIN = 0.05;
+constexpr auto DEFAULT_WAIT_PERIOD_MS = 30;
+constexpr auto DEFAULT_REFERENCE_FRAME = "base";
+
+// ------------------- DeviceDriver Related ------------------------------------
+
+bool AmorCartesianControl::open(yarp::os::Searchable& config)
+{
+    gain = config.check("controllerGain", yarp::os::Value(DEFAULT_GAIN),
+            "controller gain").asFloat64();
+
+    waitPeriodMs = config.check("waitPeriodMs", yarp::os::Value(DEFAULT_WAIT_PERIOD_MS),
+            "wait command period (milliseconds)").asInt32();
+
+    auto referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME),
+            "reference frame (base|tcp)").asString();
+
+    if (referenceFrameStr == "base")
+    {
+        referenceFrame = ICartesianSolver::BASE_FRAME;
+    }
+    else if (referenceFrameStr == "tcp")
+    {
+        referenceFrame = ICartesianSolver::TCP_FRAME;
+    }
+    else
+    {
+        yCError(ACC) << "Unsupported reference frame:" << referenceFrameStr;
+        return false;
+    }
+
+    yarp::os::Value vHandle = config.find("handle");
+    yarp::os::Value vHandleMutex = config.find("handleMutex");
+
+    if (vHandle.isNull() || vHandleMutex.isNull())
+    {
+        yCInfo(ACC) << "Creating own AMOR handle";
+
+        auto canLibrary = config.check("canLibrary", yarp::os::Value(DEFAULT_CAN_LIBRARY),
+                "CAN plugin library").asString();
+        int canPort = config.check("canPort", yarp::os::Value(DEFAULT_CAN_PORT),
+                "CAN port number").asInt32();
+
+        ownsHandle = true;
+        handle = amor_connect(const_cast<char *>(canLibrary.c_str()), canPort);
+        handleMutex = new std::mutex;
+    }
+    else
+    {
+        yCInfo(ACC) << "Using external AMOR handle";
+        ownsHandle = false;
+        handle = *reinterpret_cast<AMOR_HANDLE *>(const_cast<char *>(vHandle.asBlob()));
+        handleMutex = reinterpret_cast<std::mutex *>(const_cast<char *>(vHandleMutex.asBlob()));
+    }
+
+    if (std::lock_guard lock(*handleMutex); handle == AMOR_INVALID_HANDLE)
+    {
+        yCError(ACC) << "Could not get AMOR handle:" << amor_error();
+        return false;
+    }
+
+    qdotMax.resize(AMOR_NUM_JOINTS);
+
+    yarp::os::Bottle qMin, qMax;
+
+    for (int i = 0; i < AMOR_NUM_JOINTS; i++)
+    {
+        AMOR_JOINT_INFO jointInfo;
+
+        if (std::lock_guard lock(*handleMutex); amor_get_joint_info(handle, i, &jointInfo) != AMOR_SUCCESS)
+        {
+            yCError(ACC) << "amor_get_joint_info() failed:" << amor_error();
+            return false;
+        }
+
+        qdotMax[i] = KinRepresentation::radToDeg(jointInfo.maxVelocity);
+
+        qMin.addFloat64(KinRepresentation::radToDeg(jointInfo.lowerJointLimit));
+        qMax.addFloat64(KinRepresentation::radToDeg(jointInfo.upperJointLimit));
+    }
+
+    auto kinematicsFile = config.check("kinematics", yarp::os::Value(""),
+            "path to file with description of AMOR kinematics").asString();
+
+    yarp::os::Property cartesianDeviceOptions;
+
+    if (!cartesianDeviceOptions.fromConfigFile(kinematicsFile))
+    {
+        yCError(ACC) << "Cannot read from --kinematics" << kinematicsFile;
+        return false;
+    }
+
+    std::string solverStr = "KdlSolver";
+    cartesianDeviceOptions.put("device", solverStr);
+    cartesianDeviceOptions.put("mins", yarp::os::Value::makeList(qMin.toString().c_str()));
+    cartesianDeviceOptions.put("maxs", yarp::os::Value::makeList(qMax.toString().c_str()));
+    cartesianDeviceOptions.setMonitor(config.getMonitor(), solverStr.c_str());
+
+    if (!cartesianDevice.open(cartesianDeviceOptions))
+    {
+        yCError(ACC) << "Solver device not valid";
+        return false;
+    }
+
+    if (!cartesianDevice.view(iCartesianSolver))
+    {
+        yCError(ACC) << "Could not view iCartesianSolver";
+        return false;
+    }
+
+    currentState = VOCAB_CC_NOT_CONTROLLING;
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool AmorCartesianControl::close()
+{
+    if (handle != AMOR_INVALID_HANDLE)
+    {
+        std::unique_lock lock(*handleMutex);
+        amor_emergency_stop(handle);
+
+        if (ownsHandle)
+        {
+            amor_release(handle);
+            lock.unlock();
+            delete handleMutex;
+        }
+    }
+
+    handle = AMOR_INVALID_HANDLE;
+    handleMutex = nullptr;
+
+    return cartesianDevice.close();
+}
+
+// -----------------------------------------------------------------------------