iCub-main
ServerCartesianController.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #ifndef __SERVERCARTESIANCONTROLLER_H__
20 #define __SERVERCARTESIANCONTROLLER_H__
21 
22 #include <mutex>
23 #include <condition_variable>
24 #include <string>
25 #include <vector>
26 #include <set>
27 #include <deque>
28 #include <map>
29 
30 #include <yarp/os/all.h>
31 #include <yarp/dev/all.h>
32 #include <yarp/sig/all.h>
33 
34 #include <iCub/ctrl/pids.h>
35 #include <iCub/iKin/iKinHlp.h>
36 #include <iCub/iKin/iKinFwd.h>
37 #include <iCub/iKin/iKinInv.h>
38 
39 #include "SmithPredictor.h"
40 
41 
43 
44 
46 {
49 
50  yarp::sig::Vector minAbsVels;
52 };
53 
54 
55 class CartesianCtrlRpcProcessor : public yarp::os::PortReader
56 {
57 protected:
59  bool read(yarp::os::ConnectionReader &connection);
60 
61 public:
63 };
64 
65 
66 class CartesianCtrlCommandPort : public yarp::os::BufferedPort<yarp::os::Bottle>
67 {
68 protected:
70  void onRead(yarp::os::Bottle &command);
71 
72 public:
74 };
75 
76 
78 {
79 protected:
81  yarp::sig::Matrix R;
82 
83 public:
84  TaskRefVelTargetGenerator(const double Ts, const yarp::sig::Vector &x0);
85  void reset(const yarp::sig::Vector &x0);
86  yarp::sig::Vector integrate(const yarp::sig::Vector &vel);
88 };
89 
104 class ServerCartesianController : public yarp::dev::DeviceDriver,
105  public yarp::dev::IMultipleWrapper,
106  public yarp::dev::ICartesianControl,
107  public yarp::os::PeriodicThread,
109 {
110 protected:
111  yarp::dev::PolyDriverList drivers;
112 
113  bool attached;
114  bool connected;
115  bool closed;
129 
134  int numDrv;
135 
140 
141  yarp::os::Property plantModelProperties;
143 
144  std::deque<DriverDescriptor> lDsc;
145  std::deque<yarp::dev::IControlMode*> lMod;
146  std::deque<yarp::dev::IEncoders*> lEnc;
147  std::deque<yarp::dev::IEncodersTimed*> lEnt;
148  std::deque<yarp::dev::IPidControl*> lPid;
149  std::deque<yarp::dev::IControlLimits*> lLim;
150  std::deque<yarp::dev::IVelocityControl*> lVel;
151  std::deque<yarp::dev::IPositionDirect*> lPos;
152  std::deque<yarp::dev::IPositionControl*> lStp;
153  std::deque<int> lJnt;
154  std::deque<int*> lRmp;
155 
156  unsigned int connectCnt;
157  unsigned int ctrlPose;
159  double targetTol;
160  double trajTime;
161  double pathPerc;
164 
165  double txToken;
166  double rxToken;
171 
172  std::mutex mtx;
173  std::mutex mtx_syncEvent;
174  std::condition_variable cv_syncEvent;
175  yarp::os::Stamp txInfo;
176  yarp::os::Stamp poseInfo;
177  yarp::os::Stamp eventInfo;
178  yarp::os::Stamp debugInfo;
179 
180  yarp::sig::Vector xdes;
181  yarp::sig::Vector qdes;
182  yarp::sig::Vector xdot_set;
183  yarp::sig::Vector velCmd;
184  yarp::sig::Vector fb;
185  yarp::sig::Vector q0;
186 
187  yarp::os::BufferedPort<yarp::os::Bottle> portSlvIn;
188  yarp::os::BufferedPort<yarp::os::Bottle> portSlvOut;
189  yarp::os::RpcClient portSlvRpc;
190 
191  yarp::os::BufferedPort<yarp::sig::Vector> portState;
192  yarp::os::BufferedPort<yarp::os::Bottle> portEvent;
193  yarp::os::BufferedPort<yarp::os::Bottle> portDebugInfo;
194  yarp::os::RpcServer portRpc;
195 
198 
199  struct Context
200  {
201  yarp::sig::Vector dof;
202  yarp::sig::Vector restPos;
203  yarp::sig::Vector restWeights;
204  yarp::sig::Vector tip_x;
205  yarp::sig::Vector tip_o;
206  yarp::sig::Matrix limits;
207  double trajTime;
208  double tol;
209  bool mode;
211  double straightness;
213  yarp::os::Value task_2;
214  yarp::os::Bottle solverConvergence;
215  };
216 
218  std::map<int,Context> contextMap;
219  std::map<std::string,yarp::dev::CartesianEvent*> eventsMap;
220 
221  std::multiset<double> motionOngoingEvents;
222  std::multiset<double> motionOngoingEventsCurrent;
223 
224  yarp::os::Bottle sendCtrlCmdMultipleJointsPosition();
225  yarp::os::Bottle sendCtrlCmdMultipleJointsVelocity();
226  yarp::os::Bottle sendCtrlCmdSingleJointPosition();
227  yarp::os::Bottle sendCtrlCmdSingleJointVelocity();
228  yarp::os::Bottle (ServerCartesianController::*sendCtrlCmd)();
229 
230  void init();
231  void openPorts();
232  void closePorts();
233  bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply);
234  bool alignJointsBounds();
235  double getFeedback(yarp::sig::Vector &_fb);
236  void createController();
237  bool getNewTarget();
238  bool areJointsHealthyAndSet(std::vector<int> &jointsToSet);
239  void setJointsCtrlMode(const std::vector<int> &jointsToSet);
240  void stopLimb(const bool execStopPosition=true);
241  bool goTo(unsigned int _ctrlPose, const yarp::sig::Vector &xd, const double t, const bool latchToken=false);
242  bool deleteContexts(yarp::os::Bottle *contextIdList);
243  void notifyEvent(const std::string &event, const double checkPoint=-1.0);
246  bool registerMotionOngoingEvent(const double checkPoint);
247  bool unregisterMotionOngoingEvent(const double checkPoint);
248  yarp::os::Bottle listMotionOngoingEvents();
249 
250  bool threadInit();
251  void afterStart(bool s);
252  void run();
253  void threadRelease();
254 
257 
258  bool stopControlHelper();
259  bool setTrackingModeHelper(const bool f);
260  bool setTrajTimeHelper(const double t);
261  bool setInTargetTolHelper(const double tol);
262  bool isInTargetHelper();
263 
264  bool getTask2ndOptions(yarp::os::Value &v);
265  bool setTask2ndOptions(const yarp::os::Value &v);
266  bool getSolverConvergenceOptions(yarp::os::Bottle &options);
267  bool setSolverConvergenceOptions(const yarp::os::Bottle &options);
268 
269 public:
271  ServerCartesianController(yarp::os::Searchable &config);
272 
273  bool open(yarp::os::Searchable &config);
274  bool close();
275 
276  bool pingSolver();
277  bool connectToSolver();
278 
279  bool attachAll(const yarp::dev::PolyDriverList &p);
280  bool detachAll();
281 
282  bool setTrackingMode(const bool f);
283  bool getTrackingMode(bool *f);
284  bool setReferenceMode(const bool f);
285  bool getReferenceMode(bool *f);
286  bool setPosePriority(const std::string &p);
288  bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
289  bool getPose(const int axis, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
290  bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0);
291  bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0);
292  bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0);
293  bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0);
294  bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
295  bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat,
296  yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
297  bool askForPose(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, const yarp::sig::Vector &od,
298  yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
299  bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat,
300  yarp::sig::Vector &qdhat);
301  bool askForPosition(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat,
302  yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
303  bool getDOF(yarp::sig::Vector &curDof);
304  bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof);
305  bool getRestPos(yarp::sig::Vector &curRestPos);
306  bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos);
307  bool getRestWeights(yarp::sig::Vector &curRestWeights);
308  bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights);
309  bool getLimits(const int axis, double *min, double *max);
310  bool setLimits(const int axis, const double min, const double max);
311  bool getTrajTime(double *t);
312  bool setTrajTime(const double t);
313  bool getInTargetTol(double *tol);
314  bool setInTargetTol(const double tol);
315  bool getJointsVelocities(yarp::sig::Vector &qdot);
316  bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot);
317  bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot);
318  bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o);
319  bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o);
320  bool removeTipFrame();
321  bool checkMotionDone(bool *f);
322  bool waitMotionDone(const double period=0.1, const double timeout=0.0);
323  bool stopControl();
324  bool storeContext(int *id);
325  bool restoreContext(const int id);
326  bool deleteContext(const int id);
327  bool getInfo(yarp::os::Bottle &info);
328  bool registerEvent(yarp::dev::CartesianEvent &event);
329  bool unregisterEvent(yarp::dev::CartesianEvent &event);
330  bool tweakSet(const yarp::os::Bottle &options);
331  bool tweakGet(yarp::os::Bottle &options);
332 
333  virtual ~ServerCartesianController();
334 };
335 
336 
337 #endif
338 
339 
ServerCartesianController::getDesired
bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
Definition: ServerCartesianController.cpp:2606
ServerCartesianController::encTimedEnabled
bool encTimedEnabled
Definition: ServerCartesianController.h:121
ServerCartesianController::portCmd
CartesianCtrlCommandPort * portCmd
Definition: ServerCartesianController.h:196
ServerCartesianController::attachTipFrame
bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
Definition: ServerCartesianController.cpp:3203
ServerCartesianController::limbState
iCub::iKin::iKinLimb * limbState
Definition: ServerCartesianController.h:136
ServerCartesianController::pathPerc
double pathPerc
Definition: ServerCartesianController.h:161
ServerCartesianController::threadRelease
void threadRelease()
Definition: ServerCartesianController.cpp:1755
ServerCartesianController::sendCtrlCmdSingleJointVelocity
yarp::os::Bottle sendCtrlCmdSingleJointVelocity()
Definition: ServerCartesianController.cpp:1493
ServerCartesianController::rpcProcessor
CartesianCtrlRpcProcessor * rpcProcessor
Definition: ServerCartesianController.h:197
iKinFwd.h
ServerCartesianController::Context::solverConvergence
yarp::os::Bottle solverConvergence
Definition: ServerCartesianController.h:214
ServerCartesianController::jointsHealthy
bool jointsHealthy
Definition: ServerCartesianController.h:127
iCub::ctrl::Integrator
Definition: pids.h:47
ServerCartesianController::multipleJointsControlEnabled
bool multipleJointsControlEnabled
Definition: ServerCartesianController.h:124
CartesianCtrlRpcProcessor::server
ServerCartesianController * server
Definition: ServerCartesianController.h:58
ServerCartesianController::getNewTarget
bool getNewTarget()
Definition: ServerCartesianController.cpp:1192
ServerCartesianController::run
void run()
Definition: ServerCartesianController.cpp:1610
ServerCartesianController::debugInfo
yarp::os::Stamp debugInfo
Definition: ServerCartesianController.h:178
TaskRefVelTargetGenerator::R
yarp::sig::Matrix R
Definition: ServerCartesianController.h:81
ServerCartesianController::drivers
yarp::dev::PolyDriverList drivers
Definition: ServerCartesianController.h:111
ServerCartesianController::removeTipFrame
bool removeTipFrame()
Definition: ServerCartesianController.cpp:3264
ServerCartesianController::setJointsCtrlMode
void setJointsCtrlMode(const std::vector< int > &jointsToSet)
Definition: ServerCartesianController.cpp:1318
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
iKinInv.h
ServerCartesianController::xdot_set
yarp::sig::Vector xdot_set
Definition: ServerCartesianController.h:182
ServerCartesianController::goTo
bool goTo(unsigned int _ctrlPose, const yarp::sig::Vector &xd, const double t, const bool latchToken=false)
Definition: ServerCartesianController.cpp:2327
ServerCartesianController::sendCtrlCmdSingleJointPosition
yarp::os::Bottle sendCtrlCmdSingleJointPosition()
Definition: ServerCartesianController.cpp:1453
ServerCartesianController::getReferenceMode
bool getReferenceMode(bool *f)
Definition: ServerCartesianController.cpp:2431
ServerCartesianController::poseInfo
yarp::os::Stamp poseInfo
Definition: ServerCartesianController.h:176
ServerCartesianController::registerMotionOngoingEvent
bool registerMotionOngoingEvent(const double checkPoint)
Definition: ServerCartesianController.cpp:3662
ServerCartesianController::goToPose
bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ServerCartesianController.cpp:2553
ServerCartesianController::Context::trajTime
double trajTime
Definition: ServerCartesianController.h:207
ServerCartesianController::unregisterEvent
bool unregisterEvent(yarp::dev::CartesianEvent &event)
Definition: ServerCartesianController.cpp:3613
ServerCartesianController::motionOngoingEventsCurrent
std::multiset< double > motionOngoingEventsCurrent
Definition: ServerCartesianController.h:222
ServerCartesianController::attached
bool attached
Definition: ServerCartesianController.h:113
ServerCartesianController::goToPosition
bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ServerCartesianController.cpp:2576
ServerCartesianController::taskRefVelPeriodFactor
int taskRefVelPeriodFactor
Definition: ServerCartesianController.h:162
ServerCartesianController::setTaskVelocities
bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
Definition: ServerCartesianController.cpp:3169
ServerCartesianController::setReferenceMode
bool setReferenceMode(const bool f)
Definition: ServerCartesianController.cpp:2418
ServerCartesianController::trajTime
double trajTime
Definition: ServerCartesianController.h:160
ServerCartesianController::sendCtrlCmd
yarp::os::Bottle(ServerCartesianController::* sendCtrlCmd)()
Definition: ServerCartesianController.h:228
iCub::iKin::CartesianHelper
Definition: iKinHlp.h:47
ServerCartesianController::lJnt
std::deque< int > lJnt
Definition: ServerCartesianController.h:153
ServerCartesianController::lStp
std::deque< yarp::dev::IPositionControl * > lStp
Definition: ServerCartesianController.h:152
ServerCartesianController::getInTargetTol
bool getInTargetTol(double *tol)
Definition: ServerCartesianController.cpp:3053
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
ServerCartesianController::plantModelProperties
yarp::os::Property plantModelProperties
Definition: ServerCartesianController.h:141
ServerCartesianController::stopLimb
void stopLimb(const bool execStopPosition=true)
Definition: ServerCartesianController.cpp:1539
SmithPredictor
Definition: SmithPredictor.h:36
ServerCartesianController::eventInfo
yarp::os::Stamp eventInfo
Definition: ServerCartesianController.h:177
ServerCartesianController::Context::restPos
yarp::sig::Vector restPos
Definition: ServerCartesianController.h:202
ServerCartesianController::kinPart
std::string kinPart
Definition: ServerCartesianController.h:132
ServerCartesianController::useReferences
bool useReferences
Definition: ServerCartesianController.h:126
ServerCartesianController::askForPosition
bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
ServerCartesianController::open
bool open(yarp::os::Searchable &config)
Definition: ServerCartesianController.cpp:1767
ServerCartesianController::lDsc
std::deque< DriverDescriptor > lDsc
Definition: ServerCartesianController.h:144
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
ServerCartesianController::pingSolver
bool pingSolver()
Definition: ServerCartesianController.cpp:2248
ServerCartesianController::slvName
std::string slvName
Definition: ServerCartesianController.h:131
CartesianCtrlCommandPort::onRead
void onRead(yarp::os::Bottle &command)
Definition: ServerCartesianController.cpp:82
ServerCartesianController::init
void init()
Definition: ServerCartesianController.cpp:189
ServerCartesianController::Context::restWeights
yarp::sig::Vector restWeights
Definition: ServerCartesianController.h:203
ServerCartesianController::taskVelModeOn
bool taskVelModeOn
Definition: ServerCartesianController.h:118
TaskRefVelTargetGenerator
Definition: ServerCartesianController.h:77
ServerCartesianController::connected
bool connected
Definition: ServerCartesianController.h:114
ServerCartesianController::connectToSolver
bool connectToSolver()
Definition: ServerCartesianController.cpp:2262
ServerCartesianController::tweakSet
bool tweakSet(const yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3804
ServerCartesianController::fb
yarp::sig::Vector fb
Definition: ServerCartesianController.h:184
ServerCartesianController::stopControlHelper
bool stopControlHelper()
Definition: ServerCartesianController.cpp:3303
ServerCartesianController::getJointsVelocities
bool getJointsVelocities(yarp::sig::Vector &qdot)
Definition: ServerCartesianController.cpp:3114
ServerCartesianController::pidAvailable
bool pidAvailable
Definition: ServerCartesianController.h:125
ServerCartesianController::setDOF
bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
Definition: ServerCartesianController.cpp:2778
ServerCartesianController::checkMotionDone
bool checkMotionDone(bool *f)
Definition: ServerCartesianController.cpp:3271
ServerCartesianController::Context::posePriority
std::string posePriority
Definition: ServerCartesianController.h:212
ServerCartesianController::syncEventEnabled
bool syncEventEnabled
Definition: ServerCartesianController.h:170
p
p
Definition: show_eyes_axes.m:23
ServerCartesianController::contextMap
std::map< int, Context > contextMap
Definition: ServerCartesianController.h:218
ServerCartesianController::stopControl
bool stopControl()
Definition: ServerCartesianController.cpp:3334
ServerCartesianController::txInfo
yarp::os::Stamp txInfo
Definition: ServerCartesianController.h:175
ServerCartesianController::ctrlName
std::string ctrlName
Definition: ServerCartesianController.h:130
DriverDescriptor
Definition: ServerCartesianController.h:45
ServerCartesianController::motionOngoingEvents
std::multiset< double > motionOngoingEvents
Definition: ServerCartesianController.h:221
ServerCartesianController::numDrv
int numDrv
Definition: ServerCartesianController.h:134
ServerCartesianController::Context::useReferences
bool useReferences
Definition: ServerCartesianController.h:210
ServerCartesianController::areJointsHealthyAndSet
bool areJointsHealthyAndSet(std::vector< int > &jointsToSet)
Definition: ServerCartesianController.cpp:1284
ServerCartesianController::sendCtrlCmdMultipleJointsPosition
yarp::os::Bottle sendCtrlCmdMultipleJointsPosition()
Definition: ServerCartesianController.cpp:1350
ServerCartesianController::setTrajTimeHelper
bool setTrajTimeHelper(const double t)
Definition: ServerCartesianController.cpp:3032
ServerCartesianController::getDOF
bool getDOF(yarp::sig::Vector &curDof)
Definition: ServerCartesianController.cpp:2760
ServerCartesianController::Context::mode
bool mode
Definition: ServerCartesianController.h:209
ServerCartesianController::restoreContext
bool restoreContext(const int id)
Definition: ServerCartesianController.cpp:3402
ServerCartesianController::taskRefVelPeriodCnt
int taskRefVelPeriodCnt
Definition: ServerCartesianController.h:163
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
ServerCartesianController::Context::tip_x
yarp::sig::Vector tip_x
Definition: ServerCartesianController.h:204
ServerCartesianController::targetTol
double targetTol
Definition: ServerCartesianController.h:159
ServerCartesianController::chainPlan
iCub::iKin::iKinChain * chainPlan
Definition: ServerCartesianController.h:137
ServerCartesianController::lPos
std::deque< yarp::dev::IPositionDirect * > lPos
Definition: ServerCartesianController.h:151
ServerCartesianController::motionOngoingEventsFlush
void motionOngoingEventsFlush()
Definition: ServerCartesianController.cpp:3650
ServerCartesianController::lVel
std::deque< yarp::dev::IVelocityControl * > lVel
Definition: ServerCartesianController.h:150
ServerCartesianController::mtx
std::mutex mtx
Definition: ServerCartesianController.h:172
ServerCartesianController::lEnt
std::deque< yarp::dev::IEncodersTimed * > lEnt
Definition: ServerCartesianController.h:147
TaskRefVelTargetGenerator::reset
void reset(const yarp::sig::Vector &x0)
Definition: ServerCartesianController.cpp:145
ServerCartesianController::lLim
std::deque< yarp::dev::IControlLimits * > lLim
Definition: ServerCartesianController.h:149
ServerCartesianController::Context::dof
yarp::sig::Vector dof
Definition: ServerCartesianController.h:201
ServerCartesianController::attachAll
bool attachAll(const yarp::dev::PolyDriverList &p)
Definition: ServerCartesianController.cpp:2032
ServerCartesianController::kinType
std::string kinType
Definition: ServerCartesianController.h:133
iCub::iKin::MultiRefMinJerkCtrl
Definition: iKinInv.h:759
ServerCartesianController::closed
bool closed
Definition: ServerCartesianController.h:115
ServerCartesianController::portRpc
yarp::os::RpcServer portRpc
Definition: ServerCartesianController.h:194
ServerCartesianController::velCmd
yarp::sig::Vector velCmd
Definition: ServerCartesianController.h:183
ServerCartesianController::afterStart
void afterStart(bool s)
Definition: ServerCartesianController.cpp:1600
ServerCartesianController
servercartesiancontroller : implements the server part of the Cartesian Interface.
Definition: ServerCartesianController.h:104
ServerCartesianController::xdes
yarp::sig::Vector xdes
Definition: ServerCartesianController.h:180
ServerCartesianController::getLimits
bool getLimits(const int axis, double *min, double *max)
Definition: ServerCartesianController.cpp:2961
ServerCartesianController::ServerCartesianController
ServerCartesianController()
Definition: ServerCartesianController.cpp:172
ServerCartesianController::txToken
double txToken
Definition: ServerCartesianController.h:165
ServerCartesianController::ctrlModeAvailable
bool ctrlModeAvailable
Definition: ServerCartesianController.h:120
ServerCartesianController::portEvent
yarp::os::BufferedPort< yarp::os::Bottle > portEvent
Definition: ServerCartesianController.h:192
ServerCartesianController::getTrajTime
bool getTrajTime(double *t)
Definition: ServerCartesianController.cpp:3019
iKinHlp.h
ServerCartesianController::smithPredictor
SmithPredictor smithPredictor
Definition: ServerCartesianController.h:142
ServerCartesianController::respond
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: ServerCartesianController.cpp:289
ServerCartesianController::setTrajTime
bool setTrajTime(const double t)
Definition: ServerCartesianController.cpp:3045
ServerCartesianController::lRmp
std::deque< int * > lRmp
Definition: ServerCartesianController.h:154
ServerCartesianController::setRestPos
bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
Definition: ServerCartesianController.cpp:2861
ServerCartesianController::executingTraj
bool executingTraj
Definition: ServerCartesianController.h:117
ServerCartesianController::q0
yarp::sig::Vector q0
Definition: ServerCartesianController.h:185
ServerCartesianController::Context
Definition: ServerCartesianController.h:199
ServerCartesianController::~ServerCartesianController
virtual ~ServerCartesianController()
Definition: ServerCartesianController.cpp:3864
CartesianCtrlRpcProcessor
Definition: ServerCartesianController.h:55
ServerCartesianController::lEnc
std::deque< yarp::dev::IEncoders * > lEnc
Definition: ServerCartesianController.h:146
CartesianCtrlCommandPort::server
ServerCartesianController * server
Definition: ServerCartesianController.h:69
CartesianCtrlRpcProcessor::CartesianCtrlRpcProcessor
CartesianCtrlRpcProcessor(ServerCartesianController *server)
Definition: ServerCartesianController.cpp:52
DriverDescriptor::minAbsVels
yarp::sig::Vector minAbsVels
Definition: ServerCartesianController.h:50
ServerCartesianController::deleteContext
bool deleteContext(const int id)
Definition: ServerCartesianController.cpp:3446
ServerCartesianController::setTrackingMode
bool setTrackingMode(const bool f)
Definition: ServerCartesianController.cpp:2397
ServerCartesianController::skipSlvRes
bool skipSlvRes
Definition: ServerCartesianController.h:169
ServerCartesianController::Context::tol
double tol
Definition: ServerCartesianController.h:208
x
x
Definition: compute_ekf_sym.m:21
ServerCartesianController::portState
yarp::os::BufferedPort< yarp::sig::Vector > portState
Definition: ServerCartesianController.h:191
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
ServerCartesianController::trackingMode
bool trackingMode
Definition: ServerCartesianController.h:116
ServerCartesianController::setInTargetTolHelper
bool setInTargetTolHelper(const double tol)
Definition: ServerCartesianController.cpp:3066
ServerCartesianController::lPid
std::deque< yarp::dev::IPidControl * > lPid
Definition: ServerCartesianController.h:148
ServerCartesianController::getRestPos
bool getRestPos(yarp::sig::Vector &curRestPos)
Definition: ServerCartesianController.cpp:2829
ServerCartesianController::unregisterMotionOngoingEvent
bool unregisterMotionOngoingEvent(const double checkPoint)
Definition: ServerCartesianController.cpp:3676
ServerCartesianController::getRestWeights
bool getRestWeights(yarp::sig::Vector &curRestWeights)
Definition: ServerCartesianController.cpp:2895
TaskRefVelTargetGenerator::~TaskRefVelTargetGenerator
virtual ~TaskRefVelTargetGenerator()
Definition: ServerCartesianController.cpp:165
ServerCartesianController::portSlvIn
yarp::os::BufferedPort< yarp::os::Bottle > portSlvIn
Definition: ServerCartesianController.h:187
ServerCartesianController::getPose
bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
ServerCartesianController::ctrl
iCub::iKin::MultiRefMinJerkCtrl * ctrl
Definition: ServerCartesianController.h:138
ServerCartesianController::Context::tip_o
yarp::sig::Vector tip_o
Definition: ServerCartesianController.h:205
ServerCartesianController::connectCnt
unsigned int connectCnt
Definition: ServerCartesianController.h:156
ServerCartesianController::contextIdCnt
int contextIdCnt
Definition: ServerCartesianController.h:217
ServerCartesianController::setRestWeights
bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
Definition: ServerCartesianController.cpp:2927
ServerCartesianController::askForPose
bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
CartesianCtrlCommandPort::CartesianCtrlCommandPort
CartesianCtrlCommandPort(ServerCartesianController *server)
Definition: ServerCartesianController.cpp:74
ServerCartesianController::registerEvent
bool registerEvent(yarp::dev::CartesianEvent &event)
Definition: ServerCartesianController.cpp:3591
f
f
Definition: compute_ekf_sym.m:22
ServerCartesianController::listMotionOngoingEvents
yarp::os::Bottle listMotionOngoingEvents()
Definition: ServerCartesianController.cpp:3695
ServerCartesianController::setSolverConvergenceOptions
bool setSolverConvergenceOptions(const yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3780
ServerCartesianController::portDebugInfo
yarp::os::BufferedPort< yarp::os::Bottle > portDebugInfo
Definition: ServerCartesianController.h:193
DriverDescriptor::jointsDirectOrder
bool jointsDirectOrder
Definition: ServerCartesianController.h:48
ServerCartesianController::threadInit
bool threadInit()
Definition: ServerCartesianController.cpp:1592
ServerCartesianController::getTask2ndOptions
bool getTask2ndOptions(yarp::os::Value &v)
Definition: ServerCartesianController.cpp:3708
ServerCartesianController::getFeedback
double getFeedback(yarp::sig::Vector &_fb)
Definition: ServerCartesianController.cpp:1096
ServerCartesianController::setLimits
bool setLimits(const int axis, const double min, const double max)
Definition: ServerCartesianController.cpp:2992
ServerCartesianController::setPosePriority
bool setPosePriority(const std::string &p)
Definition: ServerCartesianController.cpp:2444
ServerCartesianController::goToPoseSync
bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ServerCartesianController.cpp:2591
ServerCartesianController::portSlvOut
yarp::os::BufferedPort< yarp::os::Bottle > portSlvOut
Definition: ServerCartesianController.h:188
ServerCartesianController::motionOngoingEventsHandling
void motionOngoingEventsHandling()
Definition: ServerCartesianController.cpp:3635
ServerCartesianController::ctrlPose
unsigned int ctrlPose
Definition: ServerCartesianController.h:157
ServerCartesianController::getSolverConvergenceOptions
bool getSolverConvergenceOptions(yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3756
ServerCartesianController::Context::task_2
yarp::os::Value task_2
Definition: ServerCartesianController.h:213
CartesianCtrlRpcProcessor::read
bool read(yarp::os::ConnectionReader &connection)
Definition: ServerCartesianController.cpp:59
ServerCartesianController::eventsMap
std::map< std::string, yarp::dev::CartesianEvent * > eventsMap
Definition: ServerCartesianController.h:219
v
static int v
Definition: iCub_Sim.cpp:42
ServerCartesianController::waitMotionDone
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
Definition: ServerCartesianController.cpp:3284
ServerCartesianController::Context::straightness
double straightness
Definition: ServerCartesianController.h:211
ServerCartesianController::maxPartJoints
int maxPartJoints
Definition: ServerCartesianController.h:158
SmithPredictor.h
ServerCartesianController::close
bool close()
Definition: ServerCartesianController.cpp:2010
ServerCartesianController::Context::limits
yarp::sig::Matrix limits
Definition: ServerCartesianController.h:206
ServerCartesianController::isInTargetHelper
bool isInTargetHelper()
Definition: ServerCartesianController.cpp:3080
ServerCartesianController::motionDone
bool motionDone
Definition: ServerCartesianController.h:119
ServerCartesianController::createController
void createController()
Definition: ServerCartesianController.cpp:1147
ServerCartesianController::posDirectEnabled
bool posDirectEnabled
Definition: ServerCartesianController.h:122
ServerCartesianController::txTokenLatchedGoToRpc
double txTokenLatchedGoToRpc
Definition: ServerCartesianController.h:168
ServerCartesianController::portSlvRpc
yarp::os::RpcClient portSlvRpc
Definition: ServerCartesianController.h:189
ServerCartesianController::getTrackingMode
bool getTrackingMode(bool *f)
Definition: ServerCartesianController.cpp:2405
ServerCartesianController::posDirectAvailable
bool posDirectAvailable
Definition: ServerCartesianController.h:123
ServerCartesianController::qdes
yarp::sig::Vector qdes
Definition: ServerCartesianController.h:181
ServerCartesianController::setInTargetTol
bool setInTargetTol(const double tol)
Definition: ServerCartesianController.cpp:3106
ServerCartesianController::detachAll
bool detachAll()
Definition: ServerCartesianController.cpp:2213
TaskRefVelTargetGenerator::integrate
yarp::sig::Vector integrate(const yarp::sig::Vector &vel)
Definition: ServerCartesianController.cpp:154
DriverDescriptor::useDefaultMinAbsVel
bool useDefaultMinAbsVel
Definition: ServerCartesianController.h:51
ServerCartesianController::getTaskVelocities
bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
Definition: ServerCartesianController.cpp:3128
TaskRefVelTargetGenerator::I
iCub::ctrl::Integrator * I
Definition: ServerCartesianController.h:80
ServerCartesianController::getTipFrame
bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
Definition: ServerCartesianController.cpp:3245
ServerCartesianController::sendCtrlCmdMultipleJointsVelocity
yarp::os::Bottle sendCtrlCmdMultipleJointsVelocity()
Definition: ServerCartesianController.cpp:1399
TaskRefVelTargetGenerator::TaskRefVelTargetGenerator
TaskRefVelTargetGenerator(const double Ts, const yarp::sig::Vector &x0)
Definition: ServerCartesianController.cpp:135
ServerCartesianController::alignJointsBounds
bool alignJointsBounds()
Definition: ServerCartesianController.cpp:1065
ServerCartesianController::cv_syncEvent
std::condition_variable cv_syncEvent
Definition: ServerCartesianController.h:174
CartesianCtrlCommandPort
Definition: ServerCartesianController.h:66
ServerCartesianController::notifyEvent
void notifyEvent(const std::string &event, const double checkPoint=-1.0)
Definition: ServerCartesianController.cpp:3525
ServerCartesianController::limbPlan
iCub::iKin::iKinLimb * limbPlan
Definition: ServerCartesianController.h:136
ServerCartesianController::debugInfoEnabled
bool debugInfoEnabled
Definition: ServerCartesianController.h:128
ServerCartesianController::getPosePriority
bool getPosePriority(std::string &p)
Definition: ServerCartesianController.cpp:2468
ServerCartesianController::goToPositionSync
bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ServerCartesianController.cpp:2599
ServerCartesianController::txTokenLatchedStopControl
double txTokenLatchedStopControl
Definition: ServerCartesianController.h:167
pids.h
ServerCartesianController::storeContext
bool storeContext(int *id)
Definition: ServerCartesianController.cpp:3342
ServerCartesianController::setTrackingModeHelper
bool setTrackingModeHelper(const bool f)
Definition: ServerCartesianController.cpp:2370
ServerCartesianController::tweakGet
bool tweakGet(yarp::os::Bottle &options)
Definition: ServerCartesianController.cpp:3826
DriverDescriptor::key
std::string key
Definition: ServerCartesianController.h:47
ServerCartesianController::setTask2ndOptions
bool setTask2ndOptions(const yarp::os::Value &v)
Definition: ServerCartesianController.cpp:3732
ServerCartesianController::openPorts
void openPorts()
Definition: ServerCartesianController.cpp:228
ServerCartesianController::rxToken
double rxToken
Definition: ServerCartesianController.h:166
ServerCartesianController::deleteContexts
bool deleteContexts(yarp::os::Bottle *contextIdList)
Definition: ServerCartesianController.cpp:3460
ServerCartesianController::lMod
std::deque< yarp::dev::IControlMode * > lMod
Definition: ServerCartesianController.h:145
ServerCartesianController::taskRefVelTargetGen
TaskRefVelTargetGenerator * taskRefVelTargetGen
Definition: ServerCartesianController.h:139
ServerCartesianController::chainState
iCub::iKin::iKinChain * chainState
Definition: ServerCartesianController.h:137
ServerCartesianController::getInfo
bool getInfo(yarp::os::Bottle &info)
Definition: ServerCartesianController.cpp:3482
ServerCartesianController::mtx_syncEvent
std::mutex mtx_syncEvent
Definition: ServerCartesianController.h:173
ServerCartesianController::closePorts
void closePorts()
Definition: ServerCartesianController.cpp:251