yarp-devices
exampleOnlineTrajectoryLocalPull.cpp

A constant-velocity, single-joint trajectory is generated with configurable final target, motion speed and period between consecutive points. The period is assumed constant (maps to cyclic synchronous position mode on the real robot, a.k.a. CSP). A callback is registered for listening to a local synchronization loop managed by CanBusBroker, position commands will be prepared and sent in response. The techniques showcased here are especially suited for online-generated trajectories, e.g. joystick teleoperation of visual servoing.

Usage (showing default option values):

 exampleOnlineTrajectoryLocalPull --bus pcan-leftArm --ipos id26-ipos --speed 2.0 --target -20.0 --period 50
See also
exampleOnlineTrajectoryRemotePull.cpp Command a remote instance of the real or simulated robot via callback on a YARP synchronization port.
exampleOnlineTrajectoryRemotePush.cpp Command a remote robot via direct position commands.
Tutorial: Trajectory Execution (external)
Note
This application is not suitable for simulation.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
32 #include <cmath>
33 
34 #include <iostream>
35 
36 #include <yarp/os/LogStream.h>
37 #include <yarp/os/Property.h>
38 #include <yarp/os/ResourceFinder.h>
39 #include <yarp/os/SystemClock.h>
40 
41 #include <yarp/dev/IControlMode.h>
42 #include <yarp/dev/IEncoders.h>
43 #include <yarp/dev/IPositionDirect.h>
44 #include <yarp/dev/PolyDriver.h>
45 
46 #include <StateObserver.hpp>
47 
48 constexpr auto DEFAULT_BUS = "socket-leftArm";
49 constexpr auto DEFAULT_IPOS = "id26-ipos";
50 constexpr auto DEFAULT_SPEED = 2.0; // deg/s
51 constexpr auto DEFAULT_TARGET = (-20.0);
52 constexpr auto DEFAULT_PERIOD_MS = 20;
53 
54 int main(int argc, char * argv[])
55 {
56  yarp::os::ResourceFinder rf;
57  rf.configure(argc, argv);
58 
59  auto bus = rf.check("bus", yarp::os::Value(DEFAULT_BUS), "CAN bus group name").asString();
60  auto ipos = rf.check("ipos", yarp::os::Value(DEFAULT_IPOS), "iPOS group name").asString();
61  auto speed = rf.check("speed", yarp::os::Value(DEFAULT_SPEED), "trajectory speed (deg/s)").asFloat64();
62  auto target = rf.check("target", yarp::os::Value(DEFAULT_TARGET), "target position (deg)").asFloat64();
63  auto period = rf.check("period", yarp::os::Value(DEFAULT_PERIOD_MS), "synchronization period (ms)").asInt32() * 0.001;
64 
65  if (speed <= 0)
66  {
67  yError() << "Illegal speed (deg/s):" << speed;
68  return 1;
69  }
70 
71  if (period <= 0)
72  {
73  yError() << "Illegal period (s):" << period;
74  return 1;
75  }
76 
77  yarp::os::Property robotConfig;
78  auto * robotConfigPtr = &robotConfig;
79  auto path = rf.findFileByName("config.ini");
80 
81  if (path.empty() || !robotConfig.fromConfigFile(path))
82  {
83  yError() << "Unable to load robot config file";
84  return 1;
85  }
86 
88  auto * syncObserverPtr = &syncObserver;
89 
90  yarp::os::Property options;
91  options.put("device", yarp::os::Value("CanBusBroker"));
92  options.put("buses", yarp::os::Value::makeList(bus.c_str()));
93  options.put(bus, yarp::os::Value::makeList(ipos.c_str()));
94  options.put("syncPeriod", yarp::os::Value(period));
95  options.put("robotConfig", yarp::os::Value::makeBlob(&robotConfigPtr, sizeof(robotConfigPtr)));
96  options.put("syncObserver", yarp::os::Value::makeBlob(&syncObserverPtr, sizeof(syncObserverPtr)));
97 
98  yarp::dev::PolyDriver dd(options);
99 
100  if (!dd.isValid())
101  {
102  yError() << "Local device not available";
103  return 1;
104  }
105 
106  yarp::dev::IControlMode * mode;
107  yarp::dev::IEncoders * enc;
108  yarp::dev::IPositionDirect * posd;
109 
110  if (!dd.view(mode) || !dd.view(enc) || !dd.view(posd))
111  {
112  yError() << "Unable to acquire robot interfaces";
113  return 1;
114  }
115 
116  if (!mode->setControlMode(0, VOCAB_CM_POSITION_DIRECT))
117  {
118  yError() << "Unable to set position direct mode";
119  return 1;
120  }
121 
122  double initialPos;
123  int retries = 0;
124 
125  while (!enc->getEncoder(0, &initialPos) && retries++ < 10)
126  {
127  yarp::os::SystemClock::delaySystem(0.05);
128  }
129 
130  if (retries >= 10)
131  {
132  yError() << "getEncoders() failed";
133  return 1;
134  }
135 
136  yInfo() << "Current ENC value:" << initialPos;
137 
138  std::cin.get();
139 
140  yInfo() << "Moving joint" << 0 << "to" << target << "degrees...";
141 
142  const double velocity = std::copysign(speed, target - initialPos);
143  const double offset = yarp::os::SystemClock::nowSystem();
144 
145  double lastRef;
146  double lastTimestamp;
147 
148  while (posd->getRefPosition(0, &lastRef) && std::abs(lastRef - initialPos) < std::abs(target - initialPos))
149  {
150  if (!syncObserver.await(&lastTimestamp))
151  {
152  yWarning() << "Sync observer timeout";
153  break;
154  }
155 
156  double t = lastTimestamp - offset;
157  double e = initialPos + velocity * t;
158  posd->setPosition(0, e);
159  }
160 
161  return 0;
162 }
bool await(T &remote)
Wait with timeout until another thread invokes notify.
Definition: StateObserver.hpp:100