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.
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>
46 #include <yarp/dev/IControlMode.h>
47 #include <yarp/dev/IEncoders.h>
48 #include <yarp/dev/IPositionDirect.h>
49 #include <yarp/dev/PolyDriver.h>
51 constexpr
auto DEFAULT_REMOTE =
"/teo/leftArm";
52 constexpr
auto DEFAULT_JOINT = 5;
53 constexpr
auto DEFAULT_SPEED = 2.0;
54 constexpr
auto DEFAULT_TARGET = (-20.0);
55 constexpr
auto DEFAULT_PERIOD_MS = 50;
57 class Worker :
public yarp::os::PeriodicThread
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)),
73 yInfo(
"[%d] New target: %f", getIterations() + 1, current);
76 if (std::abs(current - initial) >= std::abs(distance))
83 std::function<void(
double)> command;
86 const double increment;
87 const double distance;
90 int main(
int argc,
char * argv[])
92 yarp::os::ResourceFinder rf;
93 rf.configure(argc, argv);
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;
103 yError() <<
"Illegal speed (deg/s):" << speed;
109 yError() <<
"Illegal period (s):" << period;
113 yarp::os::Network yarp;
115 if (!yarp::os::Network::checkNetwork())
117 yError() <<
"Please start a yarp name server first";
121 yarp::os::Property options {{
"device", yarp::os::Value(
"remote_controlboard")},
122 {
"local", yarp::os::Value(
"/exampleOnlineTrajectoryRemotePush")},
123 {
"remote", yarp::os::Value(remote)}};
125 yarp::dev::PolyDriver dd(options);
129 yError() <<
"Remote device not available";
133 yarp::dev::IControlMode * mode;
134 yarp::dev::IEncoders * enc;
135 yarp::dev::IPositionDirect * posd;
137 if (!dd.view(mode) || !dd.view(enc) || !dd.view(posd))
139 yError() <<
"Unable to acquire robot interfaces";
143 if (!mode->setControlMode(joint, VOCAB_CM_POSITION_DIRECT))
145 yError() <<
"Unable to set position direct mode";
152 while (!enc->getEncoder(joint, &initialPos) && retries++ < 10)
154 yarp::os::SystemClock::delaySystem(0.05);
159 yError() <<
"getEncoders() failed";
163 yInfo() <<
"Current ENC value:" << initialPos;
167 yInfo() <<
"Moving joint" << joint <<
"to" << target <<
"degrees...";
169 const double distance = target - initialPos;
170 const double increment = std::copysign(speed * period, distance);
172 Worker worker(period, initialPos, increment, distance, [=](
auto pos) { posd->setPosition(joint, pos); });
176 yError() <<
"Unable to start trajectory thread";
180 while (worker.isRunning())
182 yarp::os::SystemClock::delaySystem(0.1);
Definition: exampleOnlineTrajectoryRemotePush.cpp:58