yarp-devices
exampleOnlineTrajectoryRemotePush.cpp

A constant-velocity, single-joint trajectory is generated with configurable final target, motion speed and period between consecutive points. Although it is not mandatory, it is a good practice to force the period to be constant and sent at precise intervals if commanding the real robot (maps to cyclic synchronous position mode, a.k.a. CSP). Although prepared for remote execution, this application could be rewritten to connect to a local instance of CanBusBroker; however, a better alternative exists (see notes). The techniques showcased here are especially suited for online-generated trajectories, e.g. joystick teleoperation or visual servoing.

Usage (showing default option values):

 exampleOnlineTrajectoryRemotePush --remote /teo/leftArm --joint 5 --speed 2.0 --target -20.0 --period 50
See also
exampleOnlineTrajectoryLocalPull.cpp Command a local instance of the real robot controller via callback.
exampleOnlineTrajectoryRemotePull.cpp Command a remote robot via callback, be it real or simulated.
exampleOnlineTrajectoryRemotePush.py
Tutorial: Trajectory Execution (external)
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
33 #include <cmath>
34 
35 #include <functional>
36 #include <iostream>
37 #include <utility>
38 
39 #include <yarp/os/LogStream.h>
40 #include <yarp/os/Network.h>
41 #include <yarp/os/PeriodicThread.h>
42 #include <yarp/os/Property.h>
43 #include <yarp/os/ResourceFinder.h>
44 #include <yarp/os/SystemClock.h>
45 
46 #include <yarp/dev/IControlMode.h>
47 #include <yarp/dev/IEncoders.h>
48 #include <yarp/dev/IPositionDirect.h>
49 #include <yarp/dev/PolyDriver.h>
50 
51 constexpr auto DEFAULT_REMOTE = "/teo/leftArm";
52 constexpr auto DEFAULT_JOINT = 5;
53 constexpr auto DEFAULT_SPEED = 2.0; // deg/s
54 constexpr auto DEFAULT_TARGET = (-20.0);
55 constexpr auto DEFAULT_PERIOD_MS = 50;
56 
57 class Worker : public yarp::os::PeriodicThread
58 {
59 public:
60  Worker(double period, double initial, double increment, double distance, std::function<void(double)> cmd)
61  : yarp::os::PeriodicThread(period, yarp::os::PeriodicThreadClock::Absolute),
62  command(std::move(cmd)),
63  initial(initial),
64  current(initial),
65  increment(increment),
66  distance(distance)
67  {}
68 
69 protected:
70  void run() override
71  {
72  current += increment;
73  yInfo("[%d] New target: %f", getIterations() + 1, current);
74  command(current);
75 
76  if (std::abs(current - initial) >= std::abs(distance))
77  {
78  askToStop();
79  }
80  }
81 
82 private:
83  std::function<void(double)> command;
84  const double initial;
85  double current;
86  const double increment;
87  const double distance;
88 };
89 
90 int main(int argc, char * argv[])
91 {
92  yarp::os::ResourceFinder rf;
93  rf.configure(argc, argv);
94 
95  auto remote = rf.check("remote", yarp::os::Value(DEFAULT_REMOTE), "remote port").asString();
96  auto joint = rf.check("joint", yarp::os::Value(DEFAULT_JOINT), "joint id").asInt32();
97  auto speed = rf.check("speed", yarp::os::Value(DEFAULT_SPEED), "trajectory speed (deg/s)").asFloat64();
98  auto target = rf.check("target", yarp::os::Value(DEFAULT_TARGET), "target position (deg)").asFloat64();
99  auto period = rf.check("period", yarp::os::Value(DEFAULT_PERIOD_MS), "command period (ms)").asInt32() * 0.001;
100 
101  if (speed <= 0)
102  {
103  yError() << "Illegal speed (deg/s):" << speed;
104  return 1;
105  }
106 
107  if (period <= 0)
108  {
109  yError() << "Illegal period (s):" << period;
110  return 1;
111  }
112 
113  yarp::os::Network yarp;
114 
115  if (!yarp::os::Network::checkNetwork())
116  {
117  yError() << "Please start a yarp name server first";
118  return 1;
119  }
120 
121  yarp::os::Property options {{"device", yarp::os::Value("remote_controlboard")},
122  {"local", yarp::os::Value("/exampleOnlineTrajectoryRemotePush")},
123  {"remote", yarp::os::Value(remote)}};
124 
125  yarp::dev::PolyDriver dd(options);
126 
127  if (!dd.isValid())
128  {
129  yError() << "Remote device not available";
130  return 1;
131  }
132 
133  yarp::dev::IControlMode * mode;
134  yarp::dev::IEncoders * enc;
135  yarp::dev::IPositionDirect * posd;
136 
137  if (!dd.view(mode) || !dd.view(enc) || !dd.view(posd))
138  {
139  yError() << "Unable to acquire robot interfaces";
140  return 1;
141  }
142 
143  if (!mode->setControlMode(joint, VOCAB_CM_POSITION_DIRECT))
144  {
145  yError() << "Unable to set position direct mode";
146  return 1;
147  }
148 
149  double initialPos;
150  int retries = 0;
151 
152  while (!enc->getEncoder(joint, &initialPos) && retries++ < 10)
153  {
154  yarp::os::SystemClock::delaySystem(0.05);
155  }
156 
157  if (retries >= 10)
158  {
159  yError() << "getEncoders() failed";
160  return 1;
161  }
162 
163  yInfo() << "Current ENC value:" << initialPos;
164 
165  std::cin.get();
166 
167  yInfo() << "Moving joint" << joint << "to" << target << "degrees...";
168 
169  const double distance = target - initialPos;
170  const double increment = std::copysign(speed * period, distance);
171 
172  Worker worker(period, initialPos, increment, distance, [=](auto pos) { posd->setPosition(joint, pos); });
173 
174  if (!worker.start())
175  {
176  yError() << "Unable to start trajectory thread";
177  return 1;
178  }
179 
180  while (worker.isRunning())
181  {
182  yarp::os::SystemClock::delaySystem(0.1);
183  }
184 
185  return 0;
186 }
Definition: exampleOnlineTrajectoryRemotePush.cpp:58