Merge branch 'develop' into fix-121-controller-conf

roboticslab-uc3m · Jan 19, 2018 · 81a5c30 · 81a5c30
2 parents cbabf77 + 922fa9c
commit 81a5c30
Show file tree

Hide file tree

Showing 8 changed files with 160 additions and 16 deletions.
diff --git a/libraries/TrajectoryLib/ICartesianTrajectory.hpp b/libraries/TrajectoryLib/ICartesianTrajectory.hpp
@@ -12,17 +12,17 @@ namespace roboticslab
 
 /**
  * @ingroup TrajectoryLib
- * @brief Represents a cartesian trajectory.
+ * @brief Base class for a Cartesian trajectory.
  */
 class ICartesianTrajectory : public ITrajectory
 {
 public:
-    //! Lists available cartesian paths.
+    //! Lists available Cartesian paths.
     enum cartesian_path
     {
         LINE        ///< A straight line
     };
-    //! Lists available cartesian velocity profiles.
+    //! Lists available Cartesian velocity profiles.
     enum cartesian_velocity_profile
     {
         TRAPEZOIDAL        ///< A trapezoidal velocity profile
@@ -101,9 +101,34 @@ class ICartesianTrajectory : public ITrajectory
                              const std::vector<double>& waypointVelocity = std::vector<double>(),
                              const std::vector<double>& waypointAcceleration = std::vector<double>()) = 0;
 
+    /**
+     * @brief Configure the type of Cartesian path upon creation
+     *
+     * @param pathType Use a \ref cartesian_path to define the type of Cartesian path
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool configurePath(const int pathType) = 0;
+
+    /**
+     * @brief Configure the type of Cartesian velocity profile upon creation
+     *
+     * @param velocityProfileType Use a \ref cartesian_velocity_profile to define the type of Cartesian velocity profile
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool configureVelocityProfile(const int velocityProfileType) = 0;
+
+    /** @brief Create the trajectory
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool create() = 0;
+
+    /** @brief Destroy the trajectory
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool destroy() = 0;
 
     /** Destructor */

diff --git a/libraries/TrajectoryLib/ITrajectory.hpp b/libraries/TrajectoryLib/ITrajectory.hpp
@@ -17,7 +17,7 @@ namespace roboticslab
 
 /**
  * @ingroup TrajectoryLib
- * @brief Represents a trajectory.
+ * @brief Base class for a generic trajectory.
  */
 class ITrajectory
 {
@@ -84,9 +84,34 @@ class ITrajectory
                              const std::vector<double>& waypointVelocity = std::vector<double>(),
                              const std::vector<double>& waypointAcceleration = std::vector<double>()) = 0;
 
+    /**
+     * @brief Configure the type of path upon creation
+     *
+     * @param pathType See subclasses for types of paths
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool configurePath(const int pathType) = 0;
+
+    /**
+     * @brief Configure the type of velocity profile upon creation
+     *
+     * @param velocityProfileType See subclasses for types of velocity profiles
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool configureVelocityProfile(const int velocityProfileType) = 0;
+
+    /** @brief Create the trajectory
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool create() = 0;
+
+    /** @brief Destroy the trajectory
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool destroy() = 0;
 
     /** Destructor */

diff --git a/libraries/TrajectoryLib/KdlTrajectory.cpp b/libraries/TrajectoryLib/KdlTrajectory.cpp
@@ -84,19 +84,19 @@ bool roboticslab::KdlTrajectory::configurePath(const int pathType)
     {
         if ( frames.size() != 2 )
         {
-            CD_ERROR("Need exactly 2 waypoints for Cartesian line!\n");
+            CD_ERROR("Need exactly 2 waypoints for Cartesian line (have %d)!\n",frames.size());
             return false;
         }
 
         orient = new KDL::RotationalInterpolation_SingleAxis();
         double eqradius = 1.0; //0.000001;
-        KDL::Path * path = new KDL::Path_Line(frames[0], frames[1], orient, eqradius);
+        path = new KDL::Path_Line(frames[0], frames[1], orient, eqradius);
 
         configuredPath = true;
         break;
     }
     default:
-        CD_ERROR("Only LINE cartesian path implemented for now!");
+        CD_ERROR("Only LINE cartesian path implemented for now!\n");
         return false;
     }
 
@@ -117,7 +117,7 @@ bool roboticslab::KdlTrajectory::configureVelocityProfile(const int velocityProf
         break;
     }
     default:
-        CD_ERROR("Only TRAPEZOIDAL cartesian velocity profile implemented for now!");
+        CD_ERROR("Only TRAPEZOIDAL cartesian velocity profile implemented for now!\n");
         return false;
     }
 
@@ -130,17 +130,17 @@ bool roboticslab::KdlTrajectory::create()
 {
     if( DURATION_NOT_SET == duration )
     {
-        CD_ERROR("Duration not set!");
+        CD_ERROR("Duration not set!\n");
         return false;
     }
     if( ! configuredPath )
     {
-        CD_ERROR("Path not configured!");
+        CD_ERROR("Path not configured!\n");
         return false;
     }
     if( ! configuredVelocityProfile )
     {
-        CD_ERROR("Velocity profile not configured!");
+        CD_ERROR("Velocity profile not configured!\n");
         return false;
     }
 

diff --git a/libraries/TrajectoryLib/KdlTrajectory.hpp b/libraries/TrajectoryLib/KdlTrajectory.hpp
@@ -12,15 +12,15 @@
 #include "ICartesianTrajectory.hpp"
 
 #define DURATION_NOT_SET -1
-#define DEFAULT_CARTESIAN_MAX_VEL 7.5      // unit/s
+#define DEFAULT_CARTESIAN_MAX_VEL 7.5      // unit/s, enforces a min duration of KDL::Trajectory_Segment
 #define DEFAULT_CARTESIAN_MAX_ACC 0.2      // unit/s^2
 
 namespace roboticslab
 {
 
 /**
  * @ingroup TrajectoryLib
- * @brief Implements a line trajectory.
+ * @brief Implements Cartesian trajectory functionalities using KDL.
  */
 class KdlTrajectory : public ICartesianTrajectory
 {
@@ -101,9 +101,34 @@ class KdlTrajectory : public ICartesianTrajectory
                              const std::vector<double>& waypointVelocity = std::vector<double>(),
                              const std::vector<double>& waypointAcceleration = std::vector<double>());
 
+    /**
+     * @brief Configure the type of Cartesian path upon creation
+     *
+     * @param pathType Use a \ref cartesian_path to define the type of Cartesian path
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool configurePath(const int pathType);
+
+    /**
+     * @brief Configure the type of Cartesian velocity profile upon creation
+     *
+     * @param velocityProfileType Use a \ref cartesian_velocity_profile to define the type of Cartesian velocity profile
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool configureVelocityProfile(const int velocityProfileType);
+
+    /** @brief Create the trajectory
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool create();
+
+    /** @brief Destroy the trajectory
+     *
+     * @return true on success, false otherwise
+     */
     virtual bool destroy();
 
 private:

diff --git a/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp b/libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
@@ -201,9 +201,9 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
         /** State encoded as a VOCAB which can be stored as an int */
         int currentState;
 
-        int getCurrentState();
+        int getCurrentState() const;
         void setCurrentState(int value);
-        yarp::os::Semaphore currentStateReady;
+        mutable yarp::os::Semaphore currentStateReady;
 
         /** MOVL keep track of movement start time to know at what time of trajectory movement we are */
         double movementStartTime;

diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
@@ -125,7 +125,7 @@ bool roboticslab::BasicCartesianControl::close()
 
 // -----------------------------------------------------------------------------
 
-int roboticslab::BasicCartesianControl::getCurrentState()
+int roboticslab::BasicCartesianControl::getCurrentState() const
 {
     int tmp;
     currentStateReady.wait();

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -30,6 +30,10 @@ add_executable(testAsibotSolverFromFile testAsibotSolverFromFile.cpp)
 add_dependencies(testAsibotSolverFromFile COLOR_DEBUG)
 target_link_libraries(testAsibotSolverFromFile YARP::YARP_OS YARP::YARP_dev gtest_main KinematicRepresentationLib)
 
+add_executable(testKdlTrajectory testKdlTrajectory.cpp)
+add_dependencies(testKdlTrajectory COLOR_DEBUG)
+target_link_libraries(testKdlTrajectory YARP::YARP_OS YARP::YARP_dev gtest_main TrajectoryLib)
+
 add_executable(testBasicCartesianControl testBasicCartesianControl.cpp)
 add_dependencies(testBasicCartesianControl COLOR_DEBUG)
 target_link_libraries(testBasicCartesianControl YARP::YARP_OS YARP::YARP_dev gtest_main)

diff --git a/test/testKdlTrajectory.cpp b/test/testKdlTrajectory.cpp
@@ -0,0 +1,65 @@
+#include "gtest/gtest.h"
+
+#include <cmath>
+#include <vector>
+
+#include "KdlTrajectory.hpp"
+
+namespace roboticslab
+{
+
+/**
+ * @brief Tests \ref KdlTrajectory.
+ */
+class KdlTrajectoryTest : public testing::Test
+{
+public:
+    virtual void SetUp()
+    {
+        iCartesianTrajectory = new KdlTrajectory;
+    }
+
+    virtual void TearDown()
+    {
+        delete iCartesianTrajectory;
+        iCartesianTrajectory = 0;
+    }
+
+protected:
+    ICartesianTrajectory* iCartesianTrajectory;
+};
+
+TEST_F(KdlTrajectoryTest, KdlTrajectoryLine)
+{
+    //-- Create line trajectory
+    ASSERT_TRUE( iCartesianTrajectory->setDuration(20.0) );  // Must be high, or may be expanded due to defaults
+    std::vector<double> x(6);
+    x[0] = 0;  // x
+    x[1] = 0;  // y
+    x[2] = 0;  // z
+    x[3] = 0;  // o(x)
+    x[4] = 0;  // o(y)
+    x[5] = 0;  // o(z)
+    std::vector<double> xd(6);
+    xd[0] = 1;  // x
+    xd[1] = 0;  // y
+    xd[2] = 0;  // z
+    xd[3] = 0;  // o(x)
+    xd[4] = 0;  // o(y)
+    xd[5] = 0;  // o(z)
+    ASSERT_TRUE( iCartesianTrajectory->addWaypoint(x) );
+    ASSERT_TRUE( iCartesianTrajectory->addWaypoint(xd) );
+    ASSERT_TRUE( iCartesianTrajectory->configurePath( ICartesianTrajectory::LINE ) );
+    ASSERT_TRUE( iCartesianTrajectory->configureVelocityProfile( ICartesianTrajectory::TRAPEZOIDAL ) );
+    ASSERT_TRUE( iCartesianTrajectory->create() );
+
+    //-- Use line
+    double duration;
+    ASSERT_TRUE( iCartesianTrajectory->getDuration(&duration) );
+    ASSERT_EQ(duration, 20.0);
+
+    //-- Destroy line
+    ASSERT_TRUE( iCartesianTrajectory->destroy() );
+}
+
+}  // namespace roboticslab