yarp-devices
exampleOnlineTrajectoryRemotePull.cpp

A constant-velocity, single-joint trajectory is generated with configurable final target and motion speed. The period between consecutive points is fixed (maps to cyclic synchronous position mode on the real robot, a.k.a. CSP). A callback is registered for listening to a remote synchronization port, position commands will be prepared and sent in response. 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 of visual servoing.

Usage (showing default option values):

 exampleOnlineTrajectoryRemotePull --robot /teo --part /leftArm --joint 5 --speed 2.0 --target -20.0
See also
exampleOnlineTrajectoryLocalPull.cpp Command a local instance of the real robot controller via callback.
exampleOnlineTrajectoryRemotePush.cpp Command a remote robot via direct position commands.
exampleOnlineTrajectoryRemotePull.py
Tutorial: Trajectory Execution (external)
Note
If you need to command a simulated robot, you must emulate the periodic synchronization port via yarp clock --name /teoSim/sync:o --period XXX with the desired period between points (in milliseconds) and assuming --robot /teoSim was passed in this example.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
37 #include <cmath>
38 
39 #include <functional>
40 #include <iostream>
41 #include <utility>
42 
43 #include <yarp/os/Bottle.h>
44 #include <yarp/os/BufferedPort.h>
45 #include <yarp/os/LogStream.h>
46 #include <yarp/os/Network.h>
47 #include <yarp/os/Property.h>
48 #include <yarp/os/ResourceFinder.h>
49 #include <yarp/os/SystemClock.h>
50 #include <yarp/os/TypedReaderCallback.h>
51 
52 #include <yarp/dev/IControlMode.h>
53 #include <yarp/dev/IEncoders.h>
54 #include <yarp/dev/IPositionDirect.h>
55 #include <yarp/dev/PolyDriver.h>
56 
57 constexpr auto DEFAULT_ROBOT = "/teo";
58 constexpr auto DEFAULT_PART = "/leftArm";
59 constexpr auto DEFAULT_JOINT = 5;
60 constexpr auto DEFAULT_SPEED = 2.0; // deg/s
61 constexpr auto DEFAULT_TARGET = (-20.0);
62 
63 class SyncCallback : public yarp::os::TypedReaderCallback<yarp::os::Bottle>
64 {
65 public:
66  SyncCallback(double initialPos, double speed, std::function<void(double)> cmd)
67  : count(0), e0(initialPos), v(speed), offset(0.0), command(std::move(cmd))
68  {}
69 
70  void onRead(yarp::os::Bottle & b) override
71  {
72  if (b.size() == 2)
73  {
74  double currentTime = b.get(0).asInt32() + b.get(1).asInt32() * 1e-9;
75 
76  if (count == 0)
77  {
78  offset = currentTime;
79  }
80 
81  double t = currentTime - offset;
82  double e = e0 + v * t;
83 
84  yInfo("[%d] New target: %f", ++count, e);
85  command(e);
86  }
87  }
88 
89 private:
90  int count;
91  const double e0;
92  const double v;
93  double offset;
94  std::function<void(double)> command;
95 };
96 
97 int main(int argc, char * argv[])
98 {
99  yarp::os::ResourceFinder rf;
100  rf.configure(argc, argv);
101 
102  auto robot = rf.check("robot", yarp::os::Value(DEFAULT_ROBOT), "robot port").asString();
103  auto part = rf.check("part", yarp::os::Value(DEFAULT_PART), "part port").asString();
104  auto joint = rf.check("joint", yarp::os::Value(DEFAULT_JOINT), "joint id").asInt32();
105  auto speed = rf.check("speed", yarp::os::Value(DEFAULT_SPEED), "trajectory speed (deg/s)").asFloat64();
106  auto target = rf.check("target", yarp::os::Value(DEFAULT_TARGET), "target position (deg)").asFloat64();
107 
108  if (speed <= 0)
109  {
110  yError() << "Illegal speed (deg/s):" << speed;
111  return 1;
112  }
113 
114  yarp::os::Network yarp;
115 
116  if (!yarp::os::Network::checkNetwork())
117  {
118  yError() << "Please start a yarp name server first";
119  return 1;
120  }
121 
122  yarp::os::Property options {{"device", yarp::os::Value("remote_controlboard")},
123  {"local", yarp::os::Value("/exampleOnlineTrajectoryRemotePull")},
124  {"remote", yarp::os::Value(robot + part)}};
125 
126  yarp::dev::PolyDriver dd(options);
127 
128  if (!dd.isValid())
129  {
130  yError() << "Remote device not available";
131  return 1;
132  }
133 
134  yarp::dev::IControlMode * mode;
135  yarp::dev::IEncoders * enc;
136  yarp::dev::IPositionDirect * posd;
137 
138  if (!dd.view(mode) || !dd.view(enc) || !dd.view(posd))
139  {
140  yError() << "Unable to acquire robot interfaces";
141  return 1;
142  }
143 
144  yarp::os::BufferedPort<yarp::os::Bottle> syncPort;
145  syncPort.setReadOnly();
146 
147  if (!syncPort.open("/exampleOnlineTrajectoryRemotePull/sync:i"))
148  {
149  yError() << "Unable to open local sync port";
150  return 1;
151  }
152 
153  if (!yarp::os::Network::connect(robot + "/sync:o", syncPort.getName(), "fast_tcp"))
154  {
155  yError() << "Unable to connect to remote sync port";
156  return 1;
157  }
158 
159  if (!mode->setControlMode(joint, VOCAB_CM_POSITION_DIRECT))
160  {
161  yError() << "Unable to set position direct mode";
162  return 1;
163  }
164 
165  double initialPos;
166  int retries = 0;
167 
168  while (!enc->getEncoder(joint, &initialPos) && retries++ < 10)
169  {
170  yarp::os::SystemClock::delaySystem(0.05);
171  }
172 
173  if (retries >= 10)
174  {
175  yError() << "getEncoders() failed";
176  return 1;
177  }
178 
179  yInfo() << "Current ENC value:" << initialPos;
180 
181  std::cin.get();
182 
183  yInfo() << "Moving joint" << joint << "to" << target << "degrees...";
184 
185  const double distance = target - initialPos;
186  double velocity = std::copysign(speed, distance);
187 
188  SyncCallback callback(initialPos, velocity, [=](auto pos) { posd->setPosition(joint, pos); });
189  syncPort.useCallback(callback);
190 
191  double lastRef;
192 
193  while (posd->getRefPosition(joint, &lastRef) && std::abs(lastRef - initialPos) < std::abs(distance))
194  {
195  yarp::os::SystemClock::delaySystem(0.01);
196  }
197 
198  syncPort.interrupt();
199  syncPort.close();
200 
201  return 0;
202 }
Definition: exampleOnlineTrajectoryRemotePull.cpp:64