Merge pull request #3178 from robotology/yarpActionsPlayer

YarpActionsPlayer

cmake/YarpFindDependencies.cmake

@@ -524,6 +524,10 @@ yarp_dependent_option(
   YARP_COMPILE_yarpdatadumper "Do you want to compile yarpdatadumper?" ON
   "YARP_COMPILE_EXECUTABLES" OFF
 )
+yarp_dependent_option(
+  YARP_COMPILE_yarpActionsPlayer "Do you want to compile yarpActionsPlayer?" ON
+  "YARP_COMPILE_EXECUTABLES" OFF
+)
 yarp_dependent_option(
   YARP_COMPILE_yarpview "Do you want to compile yarpview?" ON
   "YARP_COMPILE_EXECUTABLES;YARP_COMPILE_GUIS;YARP_HAS_Qt5" OFF
@@ -682,6 +686,7 @@ yarp_print_feature(YARP_COMPILE_EXECUTABLES 0 "Compile executables")
 yarp_print_feature(YARP_COMPILE_yarprobotinterface 1 "Compile yarprobotinterface${YARP_COMPILE_yarprobotinterface_disable_reason}")
 yarp_print_feature(YARP_COMPILE_yarpmanager-console 1 "Compile YARP Module Manager (console)${YARP_COMPILE_yarpmanager-console_disable_reason}")
 yarp_print_feature(YARP_COMPILE_yarpdatadumper 1 "Compile yarpdatadumper${YARP_COMPILE_yarpdatadumper_disable_reason}")
+yarp_print_feature(YARP_COMPILE_yarpActionsPlayer 1 "Compile yarpActionsPlayer${YARP_COMPILE_yarpActionsPlayer_disable_reason}")
 yarp_print_feature("YARP_COMPILE_yarpdatadumper AND YARP_HAS_OpenCV" 2 "yarpdatadumper video support")
 yarp_print_feature(YARP_COMPILE_GUIS 1 "Compile GUIs${YARP_COMPILE_GUIS_disable_reason}")
 yarp_print_feature(YARP_COMPILE_yarpview 2 "Compile yarpview${YARP_COMPILE_yarpview_disable_reason}")

doc/release/master.md

@@ -89,3 +89,8 @@ New Features
   Navigation2D_nwc_yarp
   Navigation2D_nws_yarp
   Odometry2D_nws_yarp
+
+### Other
+
+* Added new CLI executable yarpActionPlayer to playback trajectories on robot.
+  See the related documentation included in the README.md file

src/CMakeLists.txt

@@ -55,6 +55,7 @@ if(YARP_COMPILE_EXECUTABLES)
     add_subdirectory(yarplogger-console)
     add_subdirectory(yarpdataplayer-console)
     add_subdirectory(yarpdatadumper)
+    add_subdirectory(yarpActionsPlayer)
 
     # GUIs
     add_subdirectory(guis)

src/yarpActionsPlayer/CMakeLists.txt

@@ -0,0 +1,25 @@
+# SPDX-FileCopyrightText: 2024-2024 Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+if(YARP_COMPILE_yarpActionsPlayer)
+  project(yarpActionsPlayer)
+
+  file(GLOB folder_source *.cpp)
+  file(GLOB folder_header *.h)
+  source_group("Source Files" FILES ${folder_source})
+  source_group("Header Files" FILES ${folder_header})
+
+  add_executable(yarpActionsPlayer ${folder_source} ${folder_header})
+
+  target_link_libraries(yarpActionsPlayer
+    PRIVATE
+      YARP::YARP_init
+      YARP::YARP_os
+      YARP::YARP_sig
+      YARP::YARP_dev
+  )
+
+  install(TARGETS yarpActionsPlayer COMPONENT utilities DESTINATION ${CMAKE_INSTALL_BINDIR})
+
+  set_property(TARGET yarpActionsPlayer PROPERTY FOLDER "Command Line Tools")
+endif()

src/yarpActionsPlayer/README.md

@@ -0,0 +1,93 @@
+# yarpActionsPlayer
+
+`yarpActionsPlayer` is an executable designed to control a robot (real or simulated) by sending timestamped position commands to its joints, enabling it to replicate specified trajectories.
+
+## Startup
+At the startup, the executable receives in input a set of files, each of them describing a trajectory for one or more joints. Each trajectory is called `action` and is identified by a name.
+The trajectory files have the following format:
+- The first column is a progressive counter (not used)
+- The second column is a timestamp (in seconds)
+- All subsequent columns are position values, one joint for each column.
+
+Here is an example of a trajectory file for two joints, with positions sampled at a period of 10ms:
+```
+0    0.010   0.000   0.000
+1    0.020   1.253   1.253
+2    0.030   2.487   2.487
+3    0.040   3.681   3.681
+4    0.050   4.818   4.818
+5    0.060   5.878   5.878
+6    0.070   6.845   6.845
+7    0.080   7.705   7.705
+8    0.090   8.443   8.443
+9    0.090   9.048   9.048
+10   0.100   9.511   9.511
+11   0.110   9.823   9.823
+12   0.120   9.980   9.980
+```
+This format is consistent with the output of the state port of a `controlBoard_nws_yarp`.
+The following command can be used to record the trajectory of a moving robot and obtain a trajectory file that can reproduced with `yarpActionsPlayer`.
+```
+yarp read ... /robot/part/state:o envelope > file.txt
+```
+The joints commands are assigned to the robot by using a `remoteControlBoardremapper` device. In this way, the user can select to work with joints also belonging to different parts of the robot and synchronize the movements between them.
+The following file, configuration.ini (see the `example` folder) creates two different controllers, `controller1` and `controller2`. The first one will control the joints called `hjoint1` and `hjoint2` of the robot part `/robot/head`.
+The second one will attach to two different parts of the robot, i.e. `/robot/head` `/robot/arm` and control the joints`hjoint1`,`ajoint1`,`ajoint3` which belong to these parts.
+Please note that the number of joints described in the controller (and their order) must match the number of joints indicated in the trajectory file.
+As shown in the following example, user has to associate a controller for each action file to correctly map the trajectories described in the file with the joints to actuate.
+```
+[CONTROLLERS]
+controller1 (/robot/head) (hjoint1 hjoint2)
+controller2 (/robot/head /robot/arm) (hjoint1 ajoint1 ajoint3)
+
+[ACTIONS]
+wave_hand        controller1   trajectory_file1.txt
+rise_hand        controller1   trajectory_file2.txt
+turn_head_left   controller2   trajectory_file3.txt
+turn_head_right  controller2   trajectory_file4.txt
+```
+## Basic usage
+
+After starting `yarpActionsPlayer` by providing the above mentioned configuration files, the module will wait to received user commands via rpc port, e.g. `/yarpActionsPlayer/rpc`
+Use the `help` command to list all possible commands. The command `show_actions` will display the names of the actions loaded at startup.
+The command `play <action_name>` will play an action once. The command `choose <action_name>` will allow to select an action to perform more advanced commands, such as changing its playback speed, pausing it during execution or setting it to loop continuously.
+
+## Advanced considerations / Parameters tuning
+
+`yarpActionsPlayer` uses a periodic thread to schedule the execution of the commanded position at the correct time. The option `--period` allows to set the period of this thread: small values allows to schedule the command with more accuracy. The suggested value is 0.005 seconds.
+
+Another important parameter to consider is the trajectory sampling period. By default, the application does not internally resample loaded trajectories, allowing files with variable sampling rates to be used. However, users may choose to resample a trajectory to a specific rate using the `--resample` option.
+When this option is enabled, all trajectories are resampled at the specified frequency using linear interpolation. This feature can help compensate for nonlinearities in the joint position controller’s response and reduce vibrations caused by overshoots (*)
+It is generally recommended to keep this sampling period small, in the 0.005 - 0.0010 seconds range to avoid overshoots of the controller when performing a direct position control of the joint. Small values will also guarantee that the difference in position between two subsequent frames is small.
+---
+(*) The response of the controller for the following two set of trajectories is generally different, event if the final position is reached in the same amount of time:
+```
+q2(t2=0.1)  = 1
+q1(t1=0.0)  = 0
+```
+```
+q5(t5=0.1)   = 1
+q4(t4=0.75)  = 0.75
+q3(t3=0.5)   = 0.5
+q2(t2=0.025) = 0.25
+q1(t1=0.0)   = 0
+```
+In the first case, a large movement (1 degree) might cause overshoot, while in the second case, the phenomena is mitigated by keeping limited the reference delta to a small value.
+
+## Joints initial position and robot safety
+
+yarpActionsPlayer controls the robot using `positionDirect` control mode (see https://www.yarp.it/latest/classyarp_1_1dev_1_1IPositionDirect.html). In this mode, the target is provided as a step reference and the joints are commanded to reach the final position with no limitations in the the movement speed. This control mode is thus intrinsically unsafe, especially if the difference between the current and the commanded position is large. For this reason a number of safety checks are included in the control module.
+- When the action is tarted, the robot will start to move in position mode (and not in position direct mode) to the first reference using a limited speed.
+- Before starting the action playback, the module will check that the first reference is reached within a specified tolerance, which can be defined by the option `pos_tolerance` (default value: 2 degrees).
+- If the option `pos_strict_check` is enabled (default value: false) the system will halt if the tolerance threshold is not reached, otherwise it will continue with normal operation afterwards, switching to position direct mode. The timeout is defined by the parameter `pos_timeout` (default value: 2seconds)
+- By the default, launching the module will NOT control the robot, it will just simulate it. The user can enable the real control, after checking the correctness of the trajectories on a simulator, using the option `execute`.
+
+## Examples
+
+The `example` folder contains the `configuration.ini` and some test trajectories for a fake robot.
+To try the the example, you must instantiate a fakerobot using the following commands:
+```
+yarpdev --device deviceBundler --wrapper_device controlBoard_nws_yarp --attached_device fakeMotionControl --name /robot/head   --GENERAL::Joints 3  --GENERAL::AxisName "(hjoint1 hjoint2 hjoint3)"
+yarpdev --device deviceBundler --wrapper_device controlBoard_nws_yarp --attached_device fakeMotionControl --name /robot/arm    --GENERAL::Joints 6  --GENERAL::AxisName "(ajoint1 ajoint2 ajoint3 ajoint4 ajoint5 ajoint6)"
+```
+You can then observe the movement of the joints using `yarpmotorgui` or `yarpscope` executables.

src/yarpActionsPlayer/broadcastingThread.cpp

@@ -0,0 +1,139 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <yarp/os/Time.h>
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/BufferedPort.h>
+#include <yarp/dev/IPidControl.h>
+
+#include "robotDriver.h"
+#include "robotAction.h"
+#include "broadcastingThread.h"
+
+
+BroadcastingThread::BroadcastingThread(std::string name, robotDriver *p, action_class *a, double period): PeriodicThread(period)
+{
+    yAssert(p != nullptr);
+    yAssert(a != nullptr);
+
+    module_name = name;
+    driver = p;
+    actions = a;
+}
+
+BroadcastingThread::~BroadcastingThread()
+{
+    port_data_out.interrupt();
+    port_data_out.close();
+}
+
+bool BroadcastingThread::threadInit()
+{
+    if (!port_data_out.open(std::string("/") + module_name + "/all_joints_data_out:o"));
+    {
+        return false;
+    }
+
+    if (!driver)
+    {
+        return false;
+    }
+
+    if (!actions)
+    {
+        return false;
+    }
+
+    njoints = driver->getNJoints();
+    encs.resize(njoints);
+    outs.resize(njoints);
+    errs.resize(njoints);
+    mots.resize(njoints);
+
+    return true;
+}
+
+void BroadcastingThread::run()
+{
+    //reads the current position
+    if (driver && driver->ienc_ll)
+    {
+        driver->ienc_ll->getEncoders(encs.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    //reads the pid output
+    if (driver && driver->ipid_ll)
+    {
+        driver->ipid_ll->getPidOutputs(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION,outs.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    //reads the pid error
+    if (driver && driver->ipid_ll)
+    {
+        driver->ipid_ll->getPidErrors(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION,errs.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    //reads the motor encoders
+    if (driver && driver->imotenc_ll)
+    {
+        driver->imotenc_ll->getMotorEncoders(mots.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    size_t j = actions->current_frame;
+
+    yarp::os::Bottle& bot2 = this->port_data_out.prepare();
+    bot2.clear();
+    bot2.addInt32((int)actions->action_frames_vector[j].counter);
+    bot2.addFloat64(actions->action_frames_vector[j].time);
+
+    size_t size = this->actions->action_frames_vector[j].q_joints.size();
+    double *ll = actions->action_frames_vector[j].q_joints.data();
+
+    bot2.addString("commands:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(ll[ix]);
+    }
+    bot2.addString("joint encoders:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(encs[ix]);
+    }
+    bot2.addString("outputs:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(outs[ix]);
+    }
+    bot2.addString("motor encoders:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(mots[ix]);
+    }
+    bot2.addString("errors:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(errs[ix]);
+    }
+    bot2.addString("timestamp:");
+    bot2.addFloat64(yarp::os::Time::now());
+    this->port_data_out.write();
+}

src/yarpActionsPlayer/broadcastingThread.h

@@ -0,0 +1,49 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/BufferedPort.h>
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/sig/Vector.h>
+
+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <vector>
+#include <cmath>
+#include <mutex>
+
+#include "robotDriver.h"
+#include "robotAction.h"
+
+#ifndef BROADCASTING_THREAD
+#define BROADCASTING_THREAD
+
+// ******************** THE THREAD
+class BroadcastingThread: public yarp::os::PeriodicThread
+{
+    size_t njoints=0;
+    std::vector<double> encs;
+    std::vector<double> outs;
+    std::vector<double> errs;
+    std::vector<double> mots;
+
+private:
+    std::string           module_name;
+    action_class          *actions=nullptr;
+    robotDriver           *driver=nullptr;
+    yarp::os::BufferedPort<yarp::os::Bottle>  port_data_out;
+
+public:
+    BroadcastingThread(std::string module_name, robotDriver *p, action_class *a, double period = 0.001);
+    ~BroadcastingThread();
+    void attachRobotDriver(robotDriver *p);
+    void attachActions(action_class *a);
+    bool threadInit() override;
+    void run() override;
+};
+
+#endif

src/yarpActionsPlayer/controlClock.cpp

@@ -0,0 +1,40 @@
+/*
+ * SPDX-FileCopyrightText: 2025 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "controlClock.h"
+#include <yarp/os/Time.h>
+
+double ControlClock::getElapsedTime()
+{
+    if (running)
+    {
+        return elapsedTime + (yarp::os::Time::now() - startTime);
+    }
+    return elapsedTime;
+}
+
+void ControlClock::startTimer()
+{
+    if (!running)
+    {
+        startTime = yarp::os::Time::now();
+        running = true;
+    }
+}
+
+void ControlClock::pauseTimer()
+{
+    if (running)
+    {
+        elapsedTime += (yarp::os::Time::now() - startTime);
+        running = false;
+    }
+}
+void ControlClock::resetTimer()
+{
+    running = false;
+    startTime = 0.0;
+    elapsedTime = 0.0;
+}