[WIP]

roboticslab-uc3m · Nov 16, 2024 · 6375b4a · 6375b4a
1 parent 1ac12a9
commit 6375b4a
Show file tree

Hide file tree

Showing 6 changed files with 490 additions and 20 deletions.
diff --git a/libraries/YarpPlugins/KdlSolver/CMakeLists.txt b/libraries/YarpPlugins/KdlSolver/CMakeLists.txt
@@ -4,10 +4,11 @@ endif()
 
 yarp_prepare_plugin(KdlSolver
                     CATEGORY device
-                    TYPE roboticslab::KdlSolver
+                    TYPE KdlSolver
                     INCLUDE KdlSolver.hpp
                     DEFAULT ON
-                    DEPENDS "ENABLE_ScrewTheoryLib;ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND")
+                    DEPENDS "ENABLE_ScrewTheoryLib;ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND"
+                    GENERATE_PARSER)
 
 if(NOT SKIP_KdlSolver)
 
@@ -23,6 +24,11 @@ if(NOT SKIP_KdlSolver)
                               LogComponent.hpp
                               LogComponent.cpp)
 
+    if(YARP_VERSION VERSION_GREATER_EQUAL 3.10)
+        target_sources(KdlSolver PRIVATE KdlSolver_ParamsParser.h
+                                         KdlSolver_ParamsParser.cpp)
+    endif()
+
     target_link_libraries(KdlSolver YARP::YARP_os
                                     YARP::YARP_dev
                                     ${orocos_kdl_LIBRARIES}

diff --git a/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp b/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp
@@ -147,6 +147,12 @@ namespace
 
 bool KdlSolver::open(yarp::os::Searchable & config)
 {
+    if (!parseParams(config))
+    {
+        yCError(KDLS) << "Failed to parse parameters";
+        return false;
+    }
+
     isQuiet = config.check("quiet", "disable logging");
 
     //-- kinematics

diff --git a/libraries/YarpPlugins/KdlSolver/KdlSolver.hpp b/libraries/YarpPlugins/KdlSolver/KdlSolver.hpp
@@ -5,6 +5,7 @@
 
 #include <mutex>
 
+#include <yarp/conf/version.h>
 #include <yarp/dev/DeviceDriver.h>
 
 #include <kdl/chain.hpp>
@@ -14,26 +15,29 @@
 
 #include "ICartesianSolver.h"
 #include "LogComponent.hpp"
-
-namespace roboticslab
-{
+#if YARP_VERSION_COMPARE(>=, 3, 10, 0)
+#include "KdlSolver_ParamsParser.h"
+#endif
 
 /**
  * @ingroup YarpPlugins
  * @defgroup KdlSolver
  *
- * @brief Contains roboticslab::KdlSolver.
+ * @brief Contains KdlSolver.
  */
 
 /**
  * @ingroup KdlSolver
  * @brief The KdlSolver class implements ICartesianSolver.
  */
 class KdlSolver : public yarp::dev::DeviceDriver,
-                  public ICartesianSolver
+#if YARP_VERSION_COMPARE(>=, 3, 10, 0)
+                  public KdlSolver_ParamsParser,
+#endif
+                  public roboticslab::ICartesianSolver
 {
 public:
-    // -- ICartesianSolver declarations. Implementation in ICartesianSolverImpl.cpp--
+    // -- ICartesianSolver declarations. Implementation in ICartesianSolverImpl.cpp --
 
     // Get number of joints for which the solver has been configured.
     int getNumJoints() override;
@@ -48,9 +52,7 @@ class KdlSolver : public yarp::dev::DeviceDriver,
     bool restoreOriginalChain() override;
 
     // Change reference frame.
-    bool changeOrigin(const std::vector<double> &x_old_obj,
-                      const std::vector<double> &x_new_old,
-                      std::vector<double> &x_new_obj) override;
+    bool changeOrigin(const std::vector<double> &x_old_obj, const std::vector<double> &x_new_old, std::vector<double> &x_new_obj) override;
 
     // Perform forward kinematics.
     bool fwdKin(const std::vector<double> &q, std::vector<double> &x) override;
@@ -59,12 +61,10 @@ class KdlSolver : public yarp::dev::DeviceDriver,
     bool poseDiff(const std::vector<double> &xLhs, const std::vector<double> &xRhs, std::vector<double> &xOut) override;
 
     // Perform inverse kinematics.
-    bool invKin(const std::vector<double> &xd, const std::vector<double> &qGuess, std::vector<double> &q,
-                reference_frame frame) override;
+    bool invKin(const std::vector<double> &xd, const std::vector<double> &qGuess, std::vector<double> &q, reference_frame frame) override;
 
     // Perform differential inverse kinematics.
-    bool diffInvKin(const std::vector<double> &q, const std::vector<double> &xdot, std::vector<double> &qdot,
-                    reference_frame frame) override;
+    bool diffInvKin(const std::vector<double> &q, const std::vector<double> &xdot, std::vector<double> &qdot, reference_frame frame) override;
 
     // Perform inverse dynamics.
     bool invDyn(const std::vector<double> &q, std::vector<double> &t) override;
@@ -84,10 +84,7 @@ class KdlSolver : public yarp::dev::DeviceDriver,
 
     mutable std::mutex mtx;
 
-    /** The chain. **/
     KDL::Chain chain;
-
-    /** To store a copy of the original chain. **/
     KDL::Chain originalChain;
 
     KDL::ChainFkSolverPos * fkSolverPos {nullptr};
@@ -98,6 +95,4 @@ class KdlSolver : public yarp::dev::DeviceDriver,
     bool isQuiet;
 };
 
-} // namespace roboticslab
-
 #endif // __KDL_SOLVER_HPP__