Drop TrajectoryLib in favor of KDL motion API

#134

examples/cpp/CMakeLists.txt

@@ -5,10 +5,7 @@ if(ENABLE_examples)
         add_subdirectory(exampleCartesianControlClient)
     endif()
 
-    if(TARGET ROBOTICSLAB::KdlVectorConverterLib
-            AND TARGET ROBOTICSLAB::KinematicRepresentationLib
-            AND TARGET ROBOTICSLAB::TrajectoryLib
-            AND TARGET ROBOTICSLAB::ScrewTheoryLib)
+    if(TARGET ROBOTICSLAB::ScrewTheoryLib)
         add_subdirectory(exampleScrewTheoryTrajectory)
     endif()
 

examples/cpp/exampleScrewTheoryTrajectory/CMakeLists.txt

@@ -6,10 +6,7 @@ if(NOT YARP_FOUND)
     find_package(YARP 3.3 REQUIRED COMPONENTS os dev)
 endif()
 
-if(NOT TARGET ROBOTICSLAB::KdlVectorConverterLib
-        OR NOT TARGET ROBOTICSLAB::KinematicRepresentationLib
-        OR NOT TARGET ROBOTICSLAB::TrajectoryLib
-        OR NOT TARGET ROBOTICSLAB::ScrewTheoryLib)
+if(NOT TARGET ROBOTICSLAB::ScrewTheoryLib)
     find_package(ROBOTICSLAB_KINEMATICS_DYNAMICS REQUIRED)
 endif()
 
@@ -23,7 +20,6 @@ target_link_libraries(exampleScrewTheoryTrajectory YARP::YARP_os
                                                    YARP::YARP_init
                                                    YARP::YARP_dev
                                                    ${orocos_kdl_LIBRARIES}
-                                                   ROBOTICSLAB::KdlVectorConverterLib
-                                                   ROBOTICSLAB::KinematicRepresentationLib
-                                                   ROBOTICSLAB::TrajectoryLib
                                                    ROBOTICSLAB::ScrewTheoryLib)
+
+target_include_directories(exampleScrewTheoryTrajectory PRIVATE ${orocos_kdl_INCLUDE_DIRS})

examples/cpp/exampleScrewTheoryTrajectory/TrajectoryThread.cpp

@@ -10,9 +10,7 @@
 
 #include <kdl/frames.hpp>
 #include <kdl/jntarray.hpp>
-
-#include <KdlVectorConverter.hpp>
-#include <KinematicRepresentation.hpp>
+#include <kdl/utilities/utility.h>
 
 namespace
 {
@@ -35,11 +33,8 @@ bool TrajectoryThread::threadInit()
 void TrajectoryThread::run()
 {
     double movementTime = yarp::os::Time::now() - startTime;
+    KDL::Frame H_S_T = trajectory->Pos(movementTime);
 
-    std::vector<double> position;
-    iCartTrajectory->getPosition(movementTime, position);
-
-    KDL::Frame H_S_T = roboticslab::KdlVectorConverter::vectorToFrame(position);
     printCartesianCoordinates(H_S_T, movementTime);
 
     std::vector<KDL::JntArray> solutions;
@@ -65,7 +60,7 @@ void TrajectoryThread::run()
 
     for (int i = 0; i < q.rows(); i++)
     {
-        q(i) = roboticslab::KinRepresentation::degToRad(q(i));
+        q(i) = KDL::deg2rad * q(i);
     }
 
     if (!ikConfig->findOptimalConfiguration(q))
@@ -79,7 +74,7 @@ void TrajectoryThread::run()
     ikConfig->retrievePose(solution);
 
     std::vector<double> refs(solution.data.data(), solution.data.data() + solution.data.size());
-    std::transform(refs.begin(), refs.end(), refs.begin(), roboticslab::KinRepresentation::radToDeg);
+    std::transform(refs.begin(), refs.end(), refs.begin(), [](const auto & v) { return v * KDL::rad2deg; });
     yInfo() << "IK ->" << refs;
 
     iPosDirect->setPositions(refs.data());

examples/cpp/exampleScrewTheoryTrajectory/TrajectoryThread.hpp

@@ -8,24 +8,25 @@
 #include <yarp/dev/IEncoders.h>
 #include <yarp/dev/IPositionDirect.h>
 
+#include <kdl/trajectory.hpp>
+
 #include <ScrewTheoryIkProblem.hpp>
 #include <ConfigurationSelector.hpp>
-#include <ICartesianTrajectory.hpp>
 
 class TrajectoryThread : public yarp::os::PeriodicThread
 {
 public:
     TrajectoryThread(yarp::dev::IEncoders * iEncoders, yarp::dev::IPositionDirect * iPosDirect,
-            roboticslab::ScrewTheoryIkProblem * ikProblem,
-            roboticslab::ConfigurationSelector * ikConfig,
-            roboticslab::ICartesianTrajectory * iCartTrajectory,
-            int period)
+                     roboticslab::ScrewTheoryIkProblem * ikProblem,
+                     roboticslab::ConfigurationSelector * ikConfig,
+                     KDL::Trajectory * trajectory,
+                     int period)
         : yarp::os::PeriodicThread(period * 0.001),
           iEncoders(iEncoders),
           iPosDirect(iPosDirect),
           ikProblem(ikProblem),
           ikConfig(ikConfig),
-          iCartTrajectory(iCartTrajectory),
+          trajectory(trajectory),
           axes(0),
           startTime(0)
     {}
@@ -39,7 +40,7 @@ class TrajectoryThread : public yarp::os::PeriodicThread
     yarp::dev::IPositionDirect * iPosDirect;
     roboticslab::ScrewTheoryIkProblem * ikProblem;
     roboticslab::ConfigurationSelector * ikConfig;
-    roboticslab::ICartesianTrajectory * iCartTrajectory;
+    KDL::Trajectory * trajectory;
     int axes;
     double startTime;
 };

examples/cpp/exampleScrewTheoryTrajectory/exampleScrewTheoryTrajectory.cpp

@@ -47,11 +47,13 @@ make -j$(nproc)
 #include <yarp/dev/PolyDriver.h>
 
 #include <kdl/frames.hpp>
+#include <kdl/utilities/utility.h>
+#include <kdl/path_line.hpp>
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <kdl/trajectory_segment.hpp>
+#include <kdl/velocityprofile_trap.hpp>
 
 #include <ConfigurationSelector.hpp>
-#include <KdlTrajectory.hpp>
-#include <KdlVectorConverter.hpp>
-#include <KinematicRepresentation.hpp>
 #include <MatrixExponential.hpp>
 #include <ProductOfExponentials.hpp>
 #include <ScrewTheoryIkProblem.hpp>
@@ -153,12 +155,12 @@ int main(int argc, char *argv[])
 
     for (int i = 0; i < axes; i++)
     {
-        jntArray(i) = rl::KinRepresentation::degToRad(q[i]);
+        jntArray(i) = KDL::deg2rad * q[i];
     }
 
-    KDL::Frame H;
+    KDL::Frame H_base_start;
 
-    if (!poe.evaluate(jntArray, H))
+    if (!poe.evaluate(jntArray, H_base_start))
     {
         yError() << "FK error";
         return 1;
@@ -188,27 +190,13 @@ int main(int argc, char *argv[])
     rl::ConfigurationSelectorLeastOverallAngularDisplacementFactory confFactory(qMin, qMax);
     std::auto_ptr<rl::ConfigurationSelector> ikConfig(confFactory.create());
 
-    std::vector<double> x = rl::KdlVectorConverter::frameToVector(H);
+    KDL::Frame H_base_end = H_base_start;
+    H_base_end.p += KDL::Vector(0.15, 0.1, 0.1);
 
-    std::vector<double> xd(x);
-    xd[0] += 0.15;
-    xd[1] += 0.1;
-    xd[2] += 0.1;
-
-    rl::KdlTrajectory trajectory;
-
-    trajectory.setDuration(trajDuration);
-    trajectory.setMaxVelocity(trajMaxVel);
-    trajectory.addWaypoint(x);
-    trajectory.addWaypoint(xd);
-    trajectory.configurePath(rl::ICartesianTrajectory::LINE);
-    trajectory.configureVelocityProfile(rl::ICartesianTrajectory::TRAPEZOIDAL);
-
-    if (!trajectory.create())
-    {
-        yError() << "Problem creating cartesian trajectory";
-        return 1;
-    }
+    KDL::RotationalInterpolation_SingleAxis interp;
+    KDL::Path_Line path(H_base_start, H_base_end, &interp, 0.1, false);
+    KDL::VelocityProfile_Trap profile(trajMaxVel, 0.2);
+    KDL::Trajectory_Segment trajectory(&path, &profile, trajDuration, false);
 
     std::vector<int> modes(axes, VOCAB_CM_POSITION_DIRECT);
 

libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp

@@ -13,11 +13,11 @@
 #include <yarp/dev/ControlBoardInterfaces.h>
 #include <yarp/dev/IPreciselyTimed.h>
 
+#include <kdl/trajectory.hpp>
+
 #include "ICartesianSolver.h"
 #include "ICartesianControl.h"
 
-#include "ICartesianTrajectory.hpp"
-
 #define DEFAULT_SOLVER "KdlSolver"
 #define DEFAULT_ROBOT "remote_controlboard"
 #define DEFAULT_GAIN 0.05
@@ -256,7 +256,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver,
     double movementStartTime;
 
     /** MOVL store Cartesian trajectory */
-    std::vector<std::unique_ptr<ICartesianTrajectory>> trajectories;
+    std::vector<std::unique_ptr<KDL::Trajectory>> trajectories;
 
     /** FORC desired Cartesian force */
     std::vector<double> td;

libraries/YarpPlugins/BasicCartesianControl/CMakeLists.txt

@@ -9,7 +9,7 @@ yarp_prepare_plugin(BasicCartesianControl
                     TYPE roboticslab::BasicCartesianControl
                     INCLUDE BasicCartesianControl.hpp
                     DEFAULT ON
-                    DEPENDS ENABLE_TrajectoryLib
+                    DEPENDS "ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND"
                     EXTRA_CONFIG WRAPPER=CartesianControlServer)
 
 if(NOT SKIP_BasicCartesianControl)
@@ -26,9 +26,12 @@ if(NOT SKIP_BasicCartesianControl)
 
     target_link_libraries(BasicCartesianControl YARP::YARP_os
                                                 YARP::YARP_dev
-                                                ROBOTICSLAB::TrajectoryLib
+                                                ${orocos_kdl_LIBRARIES}
+                                                ROBOTICSLAB::KdlVectorConverterLib
                                                 ROBOTICSLAB::KinematicsDynamicsInterfaces)
 
+    target_include_directories(BasicCartesianControl PRIVATE ${orocos_kdl_INCLUDE_DIRS})
+
     target_compile_features(BasicCartesianControl PUBLIC cxx_std_11)
 
     yarp_install(TARGETS BasicCartesianControl

libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp

@@ -11,7 +11,13 @@
 #include <yarp/os/Time.h>
 #include <yarp/os/Vocab.h>
 
-#include "KdlTrajectory.hpp"
+#include <kdl/path_line.hpp>
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <kdl/trajectory_segment.hpp>
+#include <kdl/velocityprofile_rect.hpp>
+#include <kdl/velocityprofile_trap.hpp>
+
+#include "KdlVectorConverter.hpp"
 
 // -----------------------------------------------------------------------------
 
@@ -202,45 +208,14 @@ bool roboticslab::BasicCartesianControl::movl(const std::vector<double> &xd)
         std::vector<double> xd_base_tcp_sub(x_base_tcp.cbegin() + i * 6, x_base_tcp.cbegin() + (i + 1) * 6);
         std::vector<double> xd_obj_sub(xd_obj.cbegin() + i * 6, xd_obj.cbegin() + (i + 1) * 6);
 
-        auto iCartesianTrajectory = std::make_unique<KdlTrajectory>();
-
-        if (!iCartesianTrajectory->setDuration(duration))
-        {
-            yError() << "setDuration() failed";
-            return false;
-        }
-
-        if (!iCartesianTrajectory->addWaypoint(xd_base_tcp_sub))
-        {
-            yError() << "First addWaypoint() failed";
-            return false;
-        }
-
-        if (!iCartesianTrajectory->addWaypoint(xd_obj_sub))
-        {
-            yError() << "Second addWaypoint() failed";
-            return false;
-        }
-
-        if (!iCartesianTrajectory->configurePath(ICartesianTrajectory::LINE))
-        {
-            yError() << "configurePath() failed";
-            return false;
-        }
+        auto H_base_start = KdlVectorConverter::vectorToFrame(xd_base_tcp_sub);
+        auto H_base_end = KdlVectorConverter::vectorToFrame(xd_obj_sub);
 
-        if (!iCartesianTrajectory->configureVelocityProfile(ICartesianTrajectory::TRAPEZOIDAL))
-        {
-            yError() << "configureVelocityProfile() failed";
-            return false;
-        }
+        auto * interpolator = new KDL::RotationalInterpolation_SingleAxis();
+        auto * path = new KDL::Path_Line(H_base_start, H_base_end, interpolator, 1.0);
+        auto * profile = new KDL::VelocityProfile_Trap(10.0, 10.0);
 
-        if (!iCartesianTrajectory->create())
-        {
-            yError() << "create() failed";
-            return false;
-        }
-
-        trajectories.push_back(std::move(iCartesianTrajectory));
+        trajectories.emplace_back(std::make_unique<KDL::Trajectory_Segment>(path, profile, duration));
     }
 
     //-- Set velocity mode and set state which makes periodic thread implement control.
@@ -287,33 +262,15 @@ bool roboticslab::BasicCartesianControl::movv(const std::vector<double> &xdotd)
         std::vector<double> xd_base_tcp_sub(x_base_tcp.cbegin() + i * 6, x_base_tcp.cbegin() + (i + 1) * 6);
         std::vector<double> xdotd_sub(xdotd.cbegin() + i * 6, xdotd.cbegin() + (i + 1) * 6);
 
-        auto iCartesianTrajectory = std::make_unique<KdlTrajectory>();
-
-        if (!iCartesianTrajectory->addWaypoint(xd_base_tcp_sub, xdotd_sub))
-        {
-            yError() << "addWaypoint() failed";
-            return false;
-        }
+        auto H_base_start = KdlVectorConverter::vectorToFrame(xd_base_tcp_sub);
+        auto twist_in_base = KdlVectorConverter::vectorToTwist(xdotd_sub);
 
-        if (!iCartesianTrajectory->configurePath(ICartesianTrajectory::LINE))
-        {
-            yError() << "configurePath() failed";
-            return false;
-        }
-
-        if (!iCartesianTrajectory->configureVelocityProfile(ICartesianTrajectory::RECTANGULAR))
-        {
-            yError() << "configureVelocityProfile() failed";
-            return false;
-        }
-
-        if (!iCartesianTrajectory->create())
-        {
-            yError() << "create() failed";
-            return false;
-        }
+        auto * interpolator = new KDL::RotationalInterpolation_SingleAxis();
+        auto * path = new KDL::Path_Line(H_base_start, twist_in_base, interpolator, 1.0);
+        auto * profile = new KDL::VelocityProfile_Rectangular(10.0);
+        profile->SetProfileDuration(0, 10.0, 10.0 / path->PathLength());
 
-        trajectories.push_back(std::move(iCartesianTrajectory));
+        trajectories.emplace_back(std::make_unique<KDL::Trajectory_Segment>(path, profile));
     }
 
     //-- Set velocity mode and set state which makes periodic thread implement control.

libraries/YarpPlugins/BasicCartesianControl/PeriodicThreadImpl.cpp

@@ -5,6 +5,8 @@
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Time.h>
 
+#include "KdlVectorConverter.hpp"
+
 // ------------------- PeriodicThread Related ------------------------------------
 
 void roboticslab::BasicCartesianControl::run()
@@ -105,19 +107,18 @@ void roboticslab::BasicCartesianControl::handleMovl(const std::vector<double> &q
 
     for (const auto & trajectory : trajectories)
     {
-        double currentTrajectoryDuration;
-        trajectory->getDuration(&currentTrajectoryDuration);
-
-        if (movementTime > currentTrajectoryDuration)
+        if (movementTime > trajectory->Duration())
         {
             stopControl();
             return;
         }
 
         //-- Obtain desired Cartesian position and velocity.
-        std::vector<double> desiredX_sub, desiredXdot_sub;
-        trajectory->getPosition(movementTime, desiredX_sub);
-        trajectory->getVelocity(movementTime, desiredXdot_sub);
+        KDL::Frame H = trajectory->Pos(movementTime);
+        KDL::Twist tw = trajectory->Vel(movementTime);
+
+        std::vector<double> desiredX_sub = KdlVectorConverter::frameToVector(H);
+        std::vector<double> desiredXdot_sub = KdlVectorConverter::twistToVector(tw);
 
         desiredX.insert(desiredX.end(), desiredX_sub.cbegin(), desiredX_sub.cend());
         desiredXdot.insert(desiredXdot.end(), desiredXdot_sub.cbegin(), desiredXdot_sub.cend());
@@ -194,9 +195,11 @@ void roboticslab::BasicCartesianControl::handleMovv(const std::vector<double> &q
     for (const auto & trajectory : trajectories)
     {
         //-- Obtain desired Cartesian position and velocity.
-        std::vector<double> desiredX_sub, desiredXdot_sub;
-        trajectory->getPosition(movementTime, desiredX_sub);
-        trajectory->getVelocity(movementTime, desiredXdot_sub);
+        KDL::Frame H = trajectory->Pos(movementTime);
+        KDL::Twist tw = trajectory->Vel(movementTime);
+
+        std::vector<double> desiredX_sub = KdlVectorConverter::frameToVector(H);
+        std::vector<double> desiredXdot_sub = KdlVectorConverter::twistToVector(tw);
 
         desiredX.insert(desiredX.end(), desiredX_sub.cbegin(), desiredX_sub.cend());
         desiredXdot.insert(desiredXdot.end(), desiredXdot_sub.cbegin(), desiredXdot_sub.cend());