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.
36 #include <yarp/os/LogStream.h>
37 #include <yarp/os/Property.h>
38 #include <yarp/os/ResourceFinder.h>
39 #include <yarp/os/SystemClock.h>
41 #include <yarp/dev/IControlMode.h>
42 #include <yarp/dev/IEncoders.h>
43 #include <yarp/dev/IPositionDirect.h>
44 #include <yarp/dev/PolyDriver.h>
46 #include <StateObserver.hpp>
48 constexpr
auto DEFAULT_BUS =
"socket-leftArm";
49 constexpr
auto DEFAULT_IPOS =
"id26-ipos";
50 constexpr
auto DEFAULT_SPEED = 2.0;
51 constexpr
auto DEFAULT_TARGET = (-20.0);
52 constexpr
auto DEFAULT_PERIOD_MS = 20;
54 int main(
int argc,
char * argv[])
56 yarp::os::ResourceFinder rf;
57 rf.configure(argc, argv);
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;
67 yError() <<
"Illegal speed (deg/s):" << speed;
73 yError() <<
"Illegal period (s):" << period;
77 yarp::os::Property robotConfig;
78 auto * robotConfigPtr = &robotConfig;
79 auto path = rf.findFileByName(
"config.ini");
81 if (path.empty() || !robotConfig.fromConfigFile(path))
83 yError() <<
"Unable to load robot config file";
88 auto * syncObserverPtr = &syncObserver;
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)));
98 yarp::dev::PolyDriver dd(options);
102 yError() <<
"Local device not available";
106 yarp::dev::IControlMode * mode;
107 yarp::dev::IEncoders * enc;
108 yarp::dev::IPositionDirect * posd;
110 if (!dd.view(mode) || !dd.view(enc) || !dd.view(posd))
112 yError() <<
"Unable to acquire robot interfaces";
116 if (!mode->setControlMode(0, VOCAB_CM_POSITION_DIRECT))
118 yError() <<
"Unable to set position direct mode";
125 while (!enc->getEncoder(0, &initialPos) && retries++ < 10)
127 yarp::os::SystemClock::delaySystem(0.05);
132 yError() <<
"getEncoders() failed";
136 yInfo() <<
"Current ENC value:" << initialPos;
140 yInfo() <<
"Moving joint" << 0 <<
"to" << target <<
"degrees...";
142 const double velocity = std::copysign(speed, target - initialPos);
143 const double offset = yarp::os::SystemClock::nowSystem();
146 double lastTimestamp;
148 while (posd->getRefPosition(0, &lastRef) && std::abs(lastRef - initialPos) < std::abs(target - initialPos))
150 if (!syncObserver.
await(&lastTimestamp))
152 yWarning() <<
"Sync observer timeout";
156 double t = lastTimestamp - offset;
157 double e = initialPos + velocity * t;
158 posd->setPosition(0, e);
bool await(T &remote)
Wait with timeout until another thread invokes notify.
Definition: StateObserver.hpp:100