Implement status control, wait for movement completion (movj/movl)

See #106
roboticslab-uc3m · Sep 8, 2017 · afd870f · afd870f
1 parent 45b4309
commit afd870f
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 4 deletions.
diff --git a/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.cpp b/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.cpp
@@ -0,0 +1,35 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "AmorCartesianControl.hpp"
+
+#include <yarp/os/Time.h>
+
+#include <ColorDebug.hpp>
+
+bool roboticslab::AmorCartesianControl::waitForCompletion(int vocab)
+{
+    currentState = vocab;
+
+    AMOR_RESULT res;
+    amor_movement_status status;
+
+    do
+    {
+        res = amor_get_movement_status(handle, &status);
+
+        if (res == AMOR_FAILED)
+        {
+            CD_ERROR("%s\n", amor_error());
+            break;
+        }
+
+        yarp::os::Time::delay(0.5);  // seconds
+    }
+    while (status != AMOR_MOVEMENT_STATUS_FINISHED);
+
+    currentState = VOCAB_CC_NOT_CONTROLLING;
+
+    return res == AMOR_SUCCESS;
+}
+
+// -----------------------------------------------------------------------------
diff --git a/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.hpp b/libraries/TeoYarp/AmorCartesianControl/AmorCartesianControl.hpp
@@ -37,7 +37,8 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
 
     AmorCartesianControl() : handle(AMOR_INVALID_HANDLE),
                              ownsHandle(false),
-                             iCartesianSolver(NULL)
+                             iCartesianSolver(NULL),
+                             currentState(VOCAB_CC_NOT_CONTROLLING)
     {}
 
     // -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp --
@@ -101,11 +102,15 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo
 
 private:
 
+    bool waitForCompletion(int vocab);
+
     AMOR_HANDLE handle;
     bool ownsHandle;
 
     yarp::dev::PolyDriver cartesianDevice;
     roboticslab::ICartesianSolver *iCartesianSolver;
+
+    int currentState;
 };
 
 }  // namespace roboticslab

diff --git a/libraries/TeoYarp/AmorCartesianControl/CMakeLists.txt b/libraries/TeoYarp/AmorCartesianControl/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(AMOR_API REQUIRED)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}) # yarp plugin builder needs this
 
 yarp_add_plugin(AmorCartesianControl AmorCartesianControl.hpp
+                                     AmorCartesianControl.cpp
                                      DeviceDriverImpl.cpp
                                      ICartesianControlImpl.cpp)
 

diff --git a/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/TeoYarp/AmorCartesianControl/ICartesianControlImpl.cpp
@@ -30,7 +30,7 @@ bool roboticslab::AmorCartesianControl::stat(int &state, std::vector<double> &x)
 
     KinRepresentation::encodePose(x, x, KinRepresentation::CARTESIAN, KinRepresentation::RPY);
 
-    state = 0;  // dummy value
+    state = currentState;
 
     return true;
 }
@@ -88,7 +88,7 @@ bool roboticslab::AmorCartesianControl::movj(const std::vector<double> &xd)
         return false;
     }
 
-    return true;
+    return waitForCompletion(VOCAB_CC_MOVJ_CONTROLLING);
 }
 
 // -----------------------------------------------------------------------------
@@ -136,7 +136,7 @@ bool roboticslab::AmorCartesianControl::movl(const std::vector<double> &xd)
         return false;
     }
 
-    return true;
+    return waitForCompletion(VOCAB_CC_MOVL_CONTROLLING);
 }
 
 // -----------------------------------------------------------------------------
@@ -173,6 +173,8 @@ bool roboticslab::AmorCartesianControl::movv(const std::vector<double> &xdotd)
         return false;
     }
 
+    currentState = VOCAB_CC_MOVV_CONTROLLING;
+
     return true;
 }
 
@@ -196,6 +198,8 @@ bool roboticslab::AmorCartesianControl::forc(const std::vector<double> &td)
 
 bool roboticslab::AmorCartesianControl::stopControl()
 {
+    currentState = VOCAB_CC_NOT_CONTROLLING;
+
     if (amor_controlled_stop(handle) != AMOR_SUCCESS)
     {
         CD_ERROR("%s\n", amor_error());