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.
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>
52 #include <yarp/dev/IControlMode.h>
53 #include <yarp/dev/IEncoders.h>
54 #include <yarp/dev/IPositionDirect.h>
55 #include <yarp/dev/PolyDriver.h>
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;
61 constexpr
auto DEFAULT_TARGET = (-20.0);
63 class SyncCallback :
public yarp::os::TypedReaderCallback<yarp::os::Bottle>
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))
70 void onRead(yarp::os::Bottle & b)
override
74 double currentTime = b.get(0).asInt32() + b.get(1).asInt32() * 1e-9;
81 double t = currentTime - offset;
82 double e = e0 + v * t;
84 yInfo(
"[%d] New target: %f", ++count, e);
94 std::function<void(
double)> command;
97 int main(
int argc,
char * argv[])
99 yarp::os::ResourceFinder rf;
100 rf.configure(argc, argv);
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();
110 yError() <<
"Illegal speed (deg/s):" << speed;
114 yarp::os::Network yarp;
116 if (!yarp::os::Network::checkNetwork())
118 yError() <<
"Please start a yarp name server first";
122 yarp::os::Property options {{
"device", yarp::os::Value(
"remote_controlboard")},
123 {
"local", yarp::os::Value(
"/exampleOnlineTrajectoryRemotePull")},
124 {
"remote", yarp::os::Value(robot + part)}};
126 yarp::dev::PolyDriver dd(options);
130 yError() <<
"Remote device not available";
134 yarp::dev::IControlMode * mode;
135 yarp::dev::IEncoders * enc;
136 yarp::dev::IPositionDirect * posd;
138 if (!dd.view(mode) || !dd.view(enc) || !dd.view(posd))
140 yError() <<
"Unable to acquire robot interfaces";
144 yarp::os::BufferedPort<yarp::os::Bottle> syncPort;
145 syncPort.setReadOnly();
147 if (!syncPort.open(
"/exampleOnlineTrajectoryRemotePull/sync:i"))
149 yError() <<
"Unable to open local sync port";
153 if (!yarp::os::Network::connect(robot +
"/sync:o", syncPort.getName(),
"fast_tcp"))
155 yError() <<
"Unable to connect to remote sync port";
159 if (!mode->setControlMode(joint, VOCAB_CM_POSITION_DIRECT))
161 yError() <<
"Unable to set position direct mode";
168 while (!enc->getEncoder(joint, &initialPos) && retries++ < 10)
170 yarp::os::SystemClock::delaySystem(0.05);
175 yError() <<
"getEncoders() failed";
179 yInfo() <<
"Current ENC value:" << initialPos;
183 yInfo() <<
"Moving joint" << joint <<
"to" << target <<
"degrees...";
185 const double distance = target - initialPos;
186 double velocity = std::copysign(speed, distance);
188 SyncCallback callback(initialPos, velocity, [=](
auto pos) { posd->setPosition(joint, pos); });
189 syncPort.useCallback(callback);
193 while (posd->getRefPosition(joint, &lastRef) && std::abs(lastRef - initialPos) < std::abs(distance))
195 yarp::os::SystemClock::delaySystem(0.01);
198 syncPort.interrupt();
Definition: exampleOnlineTrajectoryRemotePull.cpp:64