93#include <yarp/os/Network.h>
94#include <yarp/os/RFModule.h>
95#include <yarp/os/Time.h>
96#include <yarp/os/BufferedPort.h>
97#include <yarp/sig/Vector.h>
98#include <yarp/math/Math.h>
100#include <yarp/dev/ControlBoardInterfaces.h>
101#include <yarp/dev/PolyDriver.h>
102#include <yarp/os/PeriodicThread.h>
103#include <yarp/os/Thread.h>
104#include <yarp/os/LogStream.h>
109#include <condition_variable>
118using namespace yarp::os;
119using namespace yarp::sig;
121using namespace yarp::math;
124#define VCTP_TIME yarp::os::createVocab32('t','i','m','e')
125#define VCTP_OFFSET yarp::os::createVocab32('o','f','f')
126#define VCTP_CMD_NOW yarp::os::createVocab32('c','t','p','n')
127#define VCTP_CMD_QUEUE yarp::os::createVocab32('c','t','p','q')
128#define VCTP_CMD_FILE yarp::os::createVocab32('c','t','p','f')
129#define VCTP_POSITION yarp::os::createVocab32('p','o','s')
130#define VCTP_WAIT yarp::os::createVocab32('w','a','i','t')
132#define VEL_FILT_SIZE 10
133#define VEL_FILT_THRES 1.0
166 return actions.size();
171 for(
unsigned int k=0; k<actions.size();k++)
191 actions.push_back(v);
214 cerr<<
"Error: not connected to control board skipping"<<endl;
218 int size=
x->getCmd().size();
220 double time=
x->getTime();
223 enc->getAxes(&nJoints);
224 if ((
offset+size)>nJoints)
226 cerr<<
"Error: detected possible overflow, skipping"<<endl;
227 cerr<<
"For debug --> joints: "<<nJoints<<
" off: "<<
offset<<
" cmd length: "<<size<<endl;
238 for (
size_t i=0; i<disp.length(); i++)
245 cerr<<
"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
249 disp[i]=
x->getCmd()[i]-q;
258 std::vector<int> joints;
259 std::vector<int> modes;
260 std::vector<double> speeds;
261 std::vector<double> positions;
263 for (
size_t i=0; i<disp.length(); i++)
265 joints.push_back(
offset+i);
266 speeds.push_back(disp[i]/
time);
267 modes.push_back(VOCAB_CM_POSITION);
268 positions.push_back(
x->getCmd()[i]);
273 mode->setControlModes(disp.length(), joints.data(), modes.data());
276 yarp::os::Time::delay(0.01);
277 mode->getControlModes(disp.length(), joints.data(), modes.data());
278 for (
size_t i=0; i<disp.length(); i++)
280 if(modes[i] != VOCAB_CM_POSITION)
282 yError() <<
"Joint " << i <<
" not in position mode";
285 pos->setRefSpeeds(disp.length(), joints.data(), speeds.data());
286 pos->positionMove(disp.length(), joints.data(), positions.data());
288 cout <<
"Script port: " <<
x->getCmd().toString() << endl;
302 lock_guard<mutex> lck(
mtx);
309 if (options.check(
"device"))
310 drvOptions.put(
"device", options.find(
"device").asString());
312 drvOptions.put(
"device",
"remote_controlboard");
314 string name=string(options.find(
"name").asString());
316 string remote=string(
"/")+string(options.find(
"robot").asString());
317 remote+=string(
"/")+string(options.find(
"part").asString());
320 local+=string(
"/local/")+string(options.find(
"part").asString());
327 cout <<
"Driver options:\n" <<
drvOptions.toString();
335 lock_guard<mutex> lck(
mtx);
341 lock_guard<mutex> lck(
mtx);
348 lock_guard<mutex> lck(
mtx);
407 double time=tmp->getTime();
418 condition_variable cv;
431 unsigned int filtElemsCnt;
433 bool checkInitConnection(Port *
p)
436 if(
p->getOutputCount() > 0)
438 cout <<
"***** got a connection" << endl;
447 bool initVelCtrl(Port *
p, ResourceFinder *rf)
456 string gainStr =
"gain";
457 sprintf(numberId,
"%d", i);
458 gainStr = gainStr + numberId;
463 if (rf->check(gainStr))
465 c.addFloat64(rf->find(gainStr).asFloat64());
475 cout <<
" velCtrl: missing 'gain' parameters" << endl;
482 string svelStr =
"svel";
483 sprintf(numberId,
"%d", i);
484 svelStr = svelStr + numberId;
489 if (rf->check(svelStr))
491 c.addFloat64(rf->find(svelStr).asFloat64());
501 cout <<
" velCtrl: missing 'svel' parameters" << endl;
507 bool readLine(Vector &v,
double &
time)
509 fin.getline(&line[0],
sizeof(line),
'\n');
519 time=b.get(1).asFloat64();
520 v.resize(b.size()-2);
522 for (
size_t i=0; i<v.length(); i++)
523 v[i]=b.get(i+2).asFloat64();
534 Vector formCommand(
const Vector &
p,
const double t)
538 for (
size_t i=0; i<
p.length(); i++)
552 for (
size_t i=0; i<
p.length(); i++)
554 res.push_back(1000.0+i);
589 bool go(
const string &fileName)
597 fin.open(fileName.c_str());
600 fin.seekg(0,ios_base::beg);
602 fin.getline(&line[0],
sizeof(line),
'\n');
603 fin.getline(&line[0],
sizeof(line),
'\n');
605 cout<<
"File loaded"<<endl;
611 cout<<
"Unable to load file: "<<fileName<<endl;
631 double time1,time2,t;
634 while (!isStopping())
638 if (checkInitConnection(velInitPort))
640 bool b = initVelCtrl(velInitPort, pRF);
643 cout <<
"velocity control initialized. gain, svel parameters loaded" << endl;
647 cout <<
"*** velocity control missing init parameters! (gain, svel)" << endl;
658 unique_lock<mutex> lck(mtx);
666 velInitPort->write(c,r);
667 send=readLine(p1,time1);
676 Vector
cmd=formCommand(p1,t);
681 if (readLine(p2,time2))
683 double dt=time2-time1;
696 velInitPort->write(c,r);
729 reply.addVocab32(
"ack");
742 reply.addVocab32(
"ack");
752 string fileName=
rf->findFile(
cmd.get(1).asString());
756 reply.addVocab32(
"ack");
760 reply.addVocab32(
"nack");
761 reply.addString(
"Unable to load file");
768 cerr<<
"Warning command not implemented yet"<<endl;
769 reply.addVocab32(
"ack");
779 const int expectedMsgSize=7;
781 if (
cmd.size()!=expectedMsgSize)
788 double time=
cmd.get(2).asFloat64();
796 posCmd=
cmd.get(6).asList();
804 cout<<
"Received command:"<<endl;
805 cout<<
"Time: " <<
time <<
"offset: " <<
offset <<
" Cmd: " << posCmd->toString()<<endl;
808 (*action)->
getCmd().resize(posCmd->size());
809 for(
int k=0;k<posCmd->size();k++)
810 (*action)->getCmd()[k]=posCmd->get(k).asFloat64();
812 (*action)->getOffset()=
offset;
813 (*action)->getTime()=
time;
825 if (
rf.check(
"name"))
826 name=string(
"/")+
rf.find(
"name").asString();
830 rpcPort.open(
name+
string(
"/")+
rf.find(
"part").asString()+
"/rpc");
834 portProp.put(
"name",
name);
835 portProp.put(
"robot",
rf.find(
"robot"));
836 portProp.put(
"part",
rf.find(
"part"));
840 cerr<<
"Error configuring position controller, check parameters"<<endl;
846 cerr<<
"Error cannot connect to remote ports"<<endl;
850 cout <<
"***** connect to rpc port and type \"help\" for commands list" << endl;
855 cerr<<
"Thread did not start, queue will not work"<<endl;
858 if (
rf.check(
"autochange_control_mode_enable"))
865 velPort.open(
name+
string(
"/")+
rf.find(
"part").asString()+
"/vc:o");
869 cout <<
"Using parameters:" << endl <<
rf.toString() << endl;
871 cout <<
"***** starting the thread" << endl;
877 virtual bool respond(
const Bottle &command, Bottle &reply)
880 if (command.size()!=0)
882 switch (command.get(0).asVocab32())
884 case yarp::os::createVocab32(
'h',
'e',
'l',
'p'):
886 cout <<
"Available commands:" << endl;
887 cout<<
"Queue command:\n";
888 cout<<
"[ctpq] [time] seconds [off] j [pos] (list)\n";
889 cout<<
"New command, execute now (erase queue):\n";
890 cout<<
"[ctpn] [time] seconds [off] j [pos] (list)\n";
891 cout<<
"Load sequence from file:\n";
892 cout<<
"[ctpf] filename\n";
893 reply.addVocab32(
"ack");
916 return RFModule::respond(command,reply);
921 reply.addVocab32(
"nack");
947int main(
int argc,
char *argv[])
950 rf.setDefaultContext(
"ctpService");
951 rf.configure(argc,argv);
953 if (rf.check(
"help"))
955 cout <<
"Options:" << endl << endl;
956 cout <<
"\t--name moduleName: set new module name" << endl;
957 cout <<
"\t--robot robotname: robot name" << endl;
958 cout <<
"\t--part partname: robot part name" << endl;
959 cout <<
"\t--from fileName: input configuration file" << endl;
960 cout <<
"\t--context dirName: resource finder context" << endl;
961 cout <<
"\t--autochange_control_mode_enable: sets the joints to position mode when trying to execute the command (default off)" << endl;
967 if (!
yarp.checkNetwork())
971 return mod.runModule(rf);
const double & getTime() const
const Vector & getCmd() const
const int & getOffset() const
void push_back(ActionItem *v)
bool go(const string &fileName)
void attachVelPort(Port *p)
void attachVelInitPort(Port *p)
void attachRF(ResourceFinder *attachedRF)
void attachPosPort(scriptPosPort *p)
WorkingThread(int period=100)
Adaptive window linear fitting to estimate the first derivative.
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
void reset()
Reinitialize the internal state.
bool parsePosCmd(const Bottle &cmd, Bottle &reply, ActionItem **action)
virtual bool updateModule()
bool handlectpm(const Bottle &cmd, Bottle &reply)
virtual double getPeriod()
bool handle_ctp_file(const Bottle &cmd, Bottle &reply)
bool handle_wait(const Bottle &cmd, Bottle &reply)
virtual bool respond(const Bottle &command, Bottle &reply)
virtual bool configure(ResourceFinder &rf)
bool handle_ctp_queue(const Bottle &cmd, Bottle &reply)
void _send(const ActionItem *x)
bool autochange_control_mode_enable
void queue(ActionItem *item)
void send(ActionItem *tmp)
bool configure(const Property &options)
void sendNow(ActionItem *item)
deque< ActionItem * > ActionList
Copyright (C) 2008 RobotCub Consortium.