YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
partitem.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: LGPL-2.1-or-later
5 */
6
7#include "partitem.h"
8#include "log.h"
9
10#include <yarp/os/LogStream.h>
11#include <yarp/dev/Drivers.h>
12#include <yarp/dev/PolyDriver.h>
13
14#include <QDebug>
15#include <QEvent>
16#include <QResizeEvent>
17#include <QFileDialog>
18#include <QXmlStreamWriter>
19#include <QXmlStreamAttribute>
20#include <QMessageBox>
21#include <QSettings>
22#include <cmath>
23#include <cstdio>
24
25PartItem::PartItem(std::string robotName, int id, std::string partName, ResourceFinder& _finder,
29 std::string rcb_protocol,
30 QWidget *parent) :
31 QWidget(parent),
32 m_sequenceWindow(nullptr),
33 m_partId(id),
34 m_mixedEnabled(false),
35 m_positionDirectEnabled(false),
36 m_pwmEnabled(false),
37 m_currentEnabled(false),
38 m_currentPidDlg(nullptr),
39 m_part_speedVisible(false),
40 m_part_motorPositionVisible(false),
41 m_part_dutyVisible(false),
42 m_part_currentVisible(false),
43 m_finder(&_finder),
44 m_iMot(nullptr),
45 m_iinfo(nullptr),
46 m_slow_k(0)
47{
48 m_layout = new FlowLayout();
49 setLayout(m_layout);
50
51 //PolyDriver *cartesiandd[MAX_NUMBER_ACTIVATED];
52
53 if (robotName.at(0) == '/') {
54 robotName.erase(0, 1);
55 }
56 if (partName.at(0) == '/') {
57 partName.erase(0, 1);
58 }
59 m_robotPartPort = std::string("/") + robotName +std::string("/") + partName;
60 m_partName = partName;
61 m_robotName = robotName;
62
63 //checking existence of the port
64 int ind = 0;
65 QString portLocalName = QString("/yarpmotorgui%1/%2").arg(ind).arg(m_robotPartPort.c_str());
66
67
69 nameToCheck += "/rpc:o";
70
71 // NameClient &nic=NameClient::getNameClient();
72 yDebug("Checking the existence of: %s \n", nameToCheck.toLatin1().data());
73 // Address adr=nic.queryName(nameToCheck.c_str());
74
75 Contact adr=Network::queryName(nameToCheck.toLatin1().data());
76
77 //Contact c = yarp::os::Network::queryName(portLocalName.c_str());
78 yDebug("ADDRESS is: %s \n", adr.toString().c_str());
79
80 while(adr.isValid()){
81 ind++;
82
83 portLocalName = QString("/yarpmotorgui%1/%2").arg(ind).arg(m_robotPartPort.c_str());
84
86 nameToCheck += "/rpc:o";
87 // adr=nic.queryName(nameToCheck.c_str());
88 adr=Network::queryName(nameToCheck.toLatin1().data());
89 }
90
91 m_interfaceError = false;
92
93 // Initializing the polydriver options and instantiating the polydrivers
94 m_partOptions.put("local", portLocalName.toLatin1().data());
95 m_partOptions.put("device", "controlBoard_nwc_yarp");
96 m_partOptions.put("remote", m_robotPartPort);
97 m_partOptions.put("carrier", rcb_protocol);
98
99 m_partsdd = new PolyDriver();
100
101 // Opening the drivers
102 m_interfaceError = !openPolyDrivers();
103 if (m_interfaceError == true)
104 {
105 yError("Opening PolyDriver for part %s failed...", m_robotPartPort.c_str());
106 QMessageBox::critical(nullptr, "Error opening a device", QString("Error while opening device for part ").append(m_robotPartPort.c_str()));
107 }
108
109 /*********************************************************************/
110 /**************** PartMover Content **********************************/
111
112 if (!m_finder->isNull()){
113 yDebug("Setting a valid finder \n");
114 }
115
116 QString sequence_portname = QString("/yarpmotorgui/%1/sequence:o").arg(partName.c_str());
117 m_sequence_port.open(sequence_portname.toLatin1().data());
118
121
122 if (m_interfaceError == false)
123 {
124 size_t i = 0;
125 std::string jointname;
126 size_t number_of_joints;
127 m_iPos->getAxes(number_of_joints);
128
129 m_controlModes.resize(number_of_joints); //for (i = 0; i < number_of_joints; i++) m_controlModes = 0;
130 m_refTrajectorySpeeds.resize(number_of_joints);
131 for (i = 0; i < number_of_joints; i++) {
132 m_refTrajectorySpeeds[i] = std::nan("");
133 }
134 m_refTrajectoryPositions.resize(number_of_joints);
135 for (i = 0; i < number_of_joints; i++) {
136 m_refTrajectoryPositions[i] = std::nan("");
137 }
138 m_refTorques.resize(number_of_joints);
139 for (i = 0; i < number_of_joints; i++) {
140 m_refTorques[i] = std::nan("");
141 }
142 m_refVelocitySpeeds.resize(number_of_joints);
143 for (i = 0; i < number_of_joints; i++) {
144 m_refVelocitySpeeds[i] = std::nan("");
145 }
146 m_refVelocityAccelerations.resize(number_of_joints);
147 for (i = 0; i < number_of_joints; i++) {
148 m_refVelocityAccelerations[i] = std::nan("");
149 }
150 m_torques.resize(number_of_joints);
151 for (i = 0; i < number_of_joints; i++) {
152 m_torques[i] = std::nan("");
153 }
154 m_positions.resize(number_of_joints);
155 for (i = 0; i < number_of_joints; i++) {
156 m_positions[i] = std::nan("");
157 }
158 m_speeds.resize(number_of_joints);
159 for (i = 0; i < number_of_joints; i++) {
160 m_speeds[i] = std::nan("");
161 }
162 m_currents.resize(number_of_joints);
163 for (i = 0; i < number_of_joints; i++) {
164 m_currents[i] = std::nan("");
165 }
166 m_motorPositions.resize(number_of_joints);
167 for (i = 0; i < number_of_joints; i++) {
168 m_motorPositions[i] = std::nan("");
169 }
170 m_dutyCycles.resize(number_of_joints);
171 for (i = 0; i < number_of_joints; i++) {
172 m_dutyCycles[i] = std::nan("");
173 }
174 m_done.resize(number_of_joints);
175 m_braked.resize(number_of_joints);
176 for (i = 0; i < number_of_joints; i++) {
177 m_braked[i] = false;
178 }
179 m_interactionModes.resize(number_of_joints);
180
181 bool ret = false;
183 do {
184 ret = m_iencs->getEncoders(m_positions.data());
185 if (!ret) {
186 yError("%s iencs->getEncoders() failed, retrying...\n", partName.c_str());
188 }
189 } while (!ret);
190
191 yInfo("%s iencs->getEncoders() ok!\n", partName.c_str());
192
193 double min_pos = 0;
194 double max_pos = 100;
195 double min_vel = 0;
196 double max_vel = 100;
197 double min_acc = 0;
198 double max_acc = 100;
199 double min_cur = -2.0;
200 double max_cur = +2.0;
201 std::vector<yarp::dev::SelectableControlModeEnum> available_modes;
202 for (size_t k = 0; k<number_of_joints; k++)
203 {
204 bool bpl = m_iLim->getPosLimits(k, &min_pos, &max_pos);
205 bool bvl = m_iLim->getVelLimits(k, &min_vel, &max_vel);
206 bool bcr = m_iCur->getCurrentRange(k, &min_cur, &max_cur);
207 bool bacm = m_ictrlmode->getAvailableControlModes(k, available_modes);
208 if (bacm == false)
209 {
210 yError() << "Error while getting position available control modes, part " << partName << " joint " << k;
211 }
212 if (bpl == false)
213 {
214 yError() << "Error while getting position limits, part " << partName << " joint " << k;
215 }
216 if (bvl == false || (min_vel == 0 && max_vel == 0))
217 {
218 yError() << "Error while getting velocity limits, part " << partName << " joint " << k;
219 }
220 if (bcr == false || (min_cur == 0 && max_cur == 0))
221 {
222 yError() << "Error while getting current range, part " << partName << " joint " << k;
223 }
224
225 QSettings settings("YARP", "yarpmotorgui");
226 double max_slider_vel = settings.value("velocity_slider_limit", 100.0).toDouble();
227 if (max_vel > max_slider_vel) {
229 }
230
231 m_iinfo->getAxisName(k, jointname);
233 m_iinfo->getJointType(k, jtype);
234
235 Pid myPid(0,0,0,0,0,0);
237 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION, k, &myPid);
238
239 auto* joint = new JointItem(k,available_modes);
240 joint->setJointName(jointname.c_str());
241 joint->setPWMRange(-100.0, 100.0);
242 joint->setCurrentRange(min_cur, max_cur);
243 m_layout->addWidget(joint);
244 joint->setPositionRange(min_pos, max_pos);
245 joint->setVelocityRange(min_vel, max_vel);
246 joint->setAccelerationRange(min_acc, max_acc);
247 joint->setTorqueRange(5.0);
248 joint->setUnits(jtype);
249 joint->enableControlPositionDirect(m_positionDirectEnabled);
250 joint->enableControlMixed(m_mixedEnabled);
251 joint->enableControlPWM(m_pwmEnabled);
252 joint->enableControlCurrent(m_currentEnabled);
253
254 int val_pos_choice = settings.value("val_pos_choice", 0).toInt();
255 int val_vel_choice = settings.value("val_vel_choice", 0).toInt();
256 int val_acc_choice = settings.value("val_acc_choice", 0).toInt();
257 int val_trq_choice = settings.value("val_trq_choice", 0).toInt();
258 int val_cur_choice = settings.value("val_cur_choice", 0).toInt();
259
260 int num_of_pos_decimals = settings.value("num_of_pos_decimals", 3).toInt();
261 int num_of_vel_decimals = settings.value("num_of_vel_decimals", 3).toInt();
262 int num_of_acc_decimals = settings.value("num_of_acc_decimals", 3).toInt();
263 int num_of_trq_decimals = settings.value("num_of_trq_decimals", 3).toInt();
264 int num_of_cur_decimals = settings.value("num_of_cur_decimals", 3).toInt();
265
266 double val_pos_custom_step = settings.value("val_pos_custom_step", 1.0).toDouble();
267 double val_vel_custom_step = settings.value("val_vel_custom_step", 1.0).toDouble();
268 double val_acc_custom_step = settings.value("val_acc_custom_step", 1.0).toDouble();
269 double val_trq_custom_step = settings.value("val_trq_custom_step", 1.0).toDouble();
270 double val_cur_custom_step = settings.value("val_cur_custom_step", 1.0).toDouble();
271
272 onSetPosSliderOptionPI(val_pos_choice, val_pos_custom_step, num_of_pos_decimals);
273 onSetVelSliderOptionPI(val_vel_choice, val_vel_custom_step, num_of_vel_decimals);
274 onSetAccSliderOptionPI(val_acc_choice, val_acc_custom_step, num_of_acc_decimals);
275 onSetTrqSliderOptionPI(val_trq_choice, val_trq_custom_step, num_of_trq_decimals);
276 onSetCurSliderOptionPI(val_cur_choice, val_cur_custom_step, num_of_cur_decimals);
277
278 joint->setEnabledOptions(debug_param_enabled,
281
282 connect(joint, SIGNAL(changeMode(int,JointItem*)), this, SLOT(onJointChangeMode(int,JointItem*)));
283 connect(joint, SIGNAL(changeInteraction(int,JointItem*)), this, SLOT(onJointInteraction(int,JointItem*)));
284 connect(joint, SIGNAL(sliderPosTrajectoryPositionCommand(double, int)), this, SLOT(onSliderPosTrajectoryPositionCommand(double, int)));
285 connect(joint, SIGNAL(sliderPosTrajectoryVelocityCommand(double, int)), this, SLOT(onSliderPosTrajectoryVelocityCommand(double, int)));
286 connect(joint, SIGNAL(sliderVelTrajectoryVelocityCommand(double, int)), this, SLOT(onSliderVelTrajectoryVelocityCommand(double, int)));
287 connect(joint, SIGNAL(sliderVelTrajectoryAccelerationCommand(double, int)), this, SLOT(onSliderVelTrajectoryAccelerationCommand(double, int)));
288 connect(joint, SIGNAL(sliderMixedPositionCommand(double, int)), this, SLOT(onSliderMixedPositionCommand(double, int)));
289 connect(joint, SIGNAL(sliderMixedVelocityCommand(double, int)), this, SLOT(onSliderMixedVelocityCommand(double, int)));
290 connect(joint, SIGNAL(sliderTorqueCommand(double, int)), this, SLOT(onSliderTorqueCommand(double, int)));
291 connect(joint, SIGNAL(sliderDirectPositionCommand(double, int)), this, SLOT(onSliderDirectPositionCommand(double, int)));
292 connect(joint, SIGNAL(sliderPWMCommand(double, int)), this, SLOT(onSliderPWMCommand(double, int)));
293 connect(joint, SIGNAL(sliderCurrentCommand(double, int)), this, SLOT(onSliderCurrentCommand(double, int)));
294 connect(joint, SIGNAL(sliderVelocityDirectCommand(double, int)), this, SLOT(onSliderVelocityDirectCommand(double, int)));
295 connect(joint, SIGNAL(homeClicked(JointItem*)),this,SLOT(onHomeClicked(JointItem*)));
296 connect(joint, SIGNAL(idleClicked(JointItem*)),this,SLOT(onIdleClicked(JointItem*)));
297 connect(joint, SIGNAL(runClicked(JointItem*)),this,SLOT(onRunClicked(JointItem*)));
298 connect(joint, SIGNAL(pidClicked(JointItem*)),this,SLOT(onPidClicked(JointItem*)));
299 connect(joint, SIGNAL(calibClicked(JointItem*)),this,SLOT(onCalibClicked(JointItem*)));
300 }
301 }
302
303 /*********************************************************************/
304 /*********************************************************************/
305
306 m_cycleTimer.setSingleShot(true);
307 m_cycleTimer.setTimerType(Qt::PreciseTimer);
308 connect(&m_cycleTimer, SIGNAL(timeout()), this, SLOT(onCycleTimerTimeout()), Qt::QueuedConnection);
309
310 m_cycleTimeTimer.setSingleShot(true);
311 m_cycleTimeTimer.setTimerType(Qt::PreciseTimer);
312 connect(&m_cycleTimeTimer, SIGNAL(timeout()), this, SLOT(onCycleTimeTimerTimeout()), Qt::QueuedConnection);
313
314
315 m_runTimeTimer.setSingleShot(true);
316 m_runTimeTimer.setTimerType(Qt::PreciseTimer);
317 connect(&m_runTimeTimer, SIGNAL(timeout()), this, SLOT(onRunTimerTimeout()), Qt::QueuedConnection);
318
319 m_runTimer.setSingleShot(true);
320 m_runTimer.setTimerType(Qt::PreciseTimer);
321 connect(&m_runTimer, SIGNAL(timeout()), this, SLOT(onRunTimeout()), Qt::QueuedConnection);
322}
323
325{
326 disconnect(&m_runTimer, SIGNAL(timeout()), this, SLOT(onRunTimeout()));
327 m_runTimer.stop();
328
329 disconnect(&m_runTimeTimer, SIGNAL(timeout()), this, SLOT(onRunTimerTimeout()));
330 m_runTimeTimer.stop();
331
332 disconnect(&m_cycleTimer, SIGNAL(timeout()), this, SLOT(onCycleTimerTimeout()));
333 m_cycleTimer.stop();
334
335 disconnect(&m_cycleTimeTimer, SIGNAL(timeout()), this, SLOT(onCycleTimeTimerTimeout()));
336 m_cycleTimeTimer.stop();
337
338 if (m_sequenceWindow){
339 m_sequenceWindow->hide();
340 delete m_sequenceWindow;
341 }
342
343 for (int i = 0; i<m_layout->count(); i++){
344 auto* joint = (JointItem *)m_layout->itemAt(i)->widget();
345 if(joint){
346 disconnect(joint,SIGNAL(changeMode(int,JointItem*)), this, SLOT(onJointChangeMode(int,JointItem*)));
347 disconnect(joint,SIGNAL(changeInteraction(int,JointItem*)), this, SLOT(onJointInteraction(int,JointItem*)));
348 disconnect(joint,SIGNAL(homeClicked(JointItem*)),this,SLOT(onHomeClicked(JointItem*)));
349 disconnect(joint,SIGNAL(idleClicked(JointItem*)),this,SLOT(onIdleClicked(JointItem*)));
350 disconnect(joint,SIGNAL(runClicked(JointItem*)),this,SLOT(onRunClicked(JointItem*)));
351 disconnect(joint,SIGNAL(pidClicked(JointItem*)),this,SLOT(onPidClicked(JointItem*)));
352 disconnect(joint,SIGNAL(calibClicked(JointItem*)),this,SLOT(onCalibClicked(JointItem*)));
353 delete joint;
354 }
355 }
356
357 if (m_partsdd){
358 m_partsdd->close();
359 delete m_partsdd;
360 m_partsdd = nullptr;
361 }
362}
363
365{
366 m_partsdd->open(m_partOptions);
367 if (!m_partsdd->isValid()) {
368 return false;
369 }
370
371 #ifdef DEBUG_INTERFACE
373 {
375 if(!debugdd->isValid()){
376 yError("Problems opening the debug client!");
377 }
378 } else {
379 debugdd = NULL;
380 }
381 #endif
382 return true;
383}
384
386{
387 yDebug("Initializing interfaces...");
388 //default value for unopened interfaces
389 m_iPos = nullptr;
390 m_iVel = nullptr;
391 m_iVar = nullptr;
392 m_iPosDir = nullptr;
393 m_iVelDir = nullptr;
394 m_iencs = nullptr;
395 m_iAmp = nullptr;
396 m_iPid = nullptr;
397 m_iCur = nullptr;
398 m_iPWM = nullptr;
399 m_iTrq = nullptr;
400 m_iImp = nullptr;
401 m_iLim = nullptr;
402 m_ical = nullptr;
403 m_ictrlmode = nullptr;
404 m_iinteract = nullptr;
405 m_iremCalib = nullptr;
406 m_ijointfault = nullptr;
407}
408
410{
411 yDebug("Opening interfaces...");
412 bool ok = false;
413
414 if (m_partsdd->isValid()) {
415 ok = m_partsdd->view(m_iPid);
416 if(!ok){
417 yError("...iPid was not ok...");
418 }
419 ok &= m_partsdd->view(m_iAmp);
420 if(!ok){
421 yError("...iAmp was not ok...");
422 }
423 ok &= m_partsdd->view(m_iPos);
424 if(!ok){
425 yError("...iPos was not ok...");
426 }
427 ok &= m_partsdd->view(m_iPosDir);
428 if(!ok){
429 yError("...posDirect was not ok...");
430 }
431 ok &= m_partsdd->view(m_iVelDir);
432 if(!ok){
433 yError("...velDirect was not ok...");
434 }
435 ok &= m_partsdd->view(m_iVel);
436 if(!ok){
437 yError("...iVel was not ok...");
438 }
439 ok &= m_partsdd->view(m_iLim);
440 if(!ok){
441 yError("...iLim was not ok...");
442 }
443 ok &= m_partsdd->view(m_iencs);
444 if(!ok){
445 yError("...enc was not ok...");
446 }
447 ok &= m_partsdd->view(m_ical);
448 if(!ok){
449 yError("...cal was not ok...");
450 }
451 ok &= m_partsdd->view(m_iTrq);
452 if(!ok){
453 yError("...trq was not ok...");
454 }
455 ok = m_partsdd->view(m_iPWM);
456 if(!ok){
457 yError("...opl was not ok...");
458 }
459 ok &= m_partsdd->view(m_iImp);
460 if(!ok){
461 yError("...imp was not ok...");
462 }
463 ok &= m_partsdd->view(m_ictrlmode);
464 if(!ok){
465 yError("...ctrlmode2 was not ok...");
466 }
467 ok &= m_partsdd->view(m_iinteract);
468 if(!ok){
469 yError("...iinteract was not ok...");
470 }
471
472 //optional interfaces
473 if (!m_partsdd->view(m_ijointfault))
474 {
475 yWarning("...m_iJointFault was not ok...");
476 }
477 if (!m_partsdd->view(m_ijointbrake))
478 {
479 yWarning("...m_iJointBrake was not ok...");
480 }
481 if (!m_partsdd->view(m_iVar))
482 {
483 yWarning("...iVar was not ok...");
484 }
485 if (!m_partsdd->view(m_iMot))
486 {
487 yWarning("...iMot was not ok...");
488 }
489 if (!m_partsdd->view(m_iremCalib))
490 {
491 yWarning("...remCalib was not ok...");
492 }
493 if (!m_partsdd->view(m_iinfo))
494 {
495 yWarning("...axisInfo was not ok...");
496 }
497 if (!m_partsdd->view(m_iCur))
498 {
499 yWarning("...iCur was not ok...");
500 }
501
502 if (!ok) {
503 yError("Error while acquiring interfaces!");
504 QMessageBox::critical(nullptr,"Problems acquiring interfaces.","Check if interface is running");
505 m_interfaceError = true;
506 }
507 }
508 else
509 {
510 yError("Device driver was not valid!");
511 m_interfaceError = true;
512 }
513
514 return !m_interfaceError;
515}
516
518{
519 return m_interfaceError;
520}
521
523{
524 return m_partName;
525}
526
527void PartItem::onSliderPWMCommand(double pwmVal, int index)
528{
529 m_iPWM->setRefDutyCycle(index, pwmVal);
530}
531
532void PartItem::onSliderCurrentCommand(double currentVal, int index)
533{
534 m_iCur->setRefCurrent(index, currentVal);
535}
536
537void PartItem::onSliderVelTrajectoryVelocityCommand(double speedVal, int index)
538{
539 m_iVel->velocityMove(index, speedVal);
540}
541
542void PartItem::onSliderVelTrajectoryAccelerationCommand(double accVal, int index)
543{
544 m_iVel->setTrajAcceleration(index, accVal);
545}
546
547void PartItem::onSliderVelocityDirectCommand(double speedVal, int index)
548{
549 m_iVelDir->setRefVelocity(index, speedVal);
550}
551
552void PartItem::onSliderTorqueCommand(double torqueVal, int index)
553{
554 m_iTrq->setRefTorque(index, torqueVal);
555}
556
557void PartItem::onSliderPosTrajectoryVelocityCommand(double trajspeedVal, int index)
558{
559 m_iPos->setTrajSpeed(index, trajspeedVal);
560}
561
562
563void PartItem::onSliderDirectPositionCommand(double dirpos, int index)
564{
566 m_ictrlmode->getControlMode(index, mode);
568 {
569 m_iPosDir->setPosition(index, dirpos);
570 }
571 else
572 {
573 yWarning("Joint not in position direct mode so cannot send references");
574 }
575}
576
577void PartItem::onDumpAllRemoteVariables()
578{
579 if (m_iVar == nullptr)
580 {
581 return;
582 }
583
584 QString fileName = QFileDialog::getSaveFileName(this, QString("Save Dump for Remote Variables as:"), QDir::homePath()+QString("/RemoteVariablesDump.txt"));
585
586 FILE* dumpfile = fopen(fileName.toStdString().c_str(), "w");
587 if (dumpfile != nullptr)
588 {
589 yarp::os::Bottle keys;
590 if (m_iVar->getRemoteVariablesList(&keys))
591 {
592 std::string s = keys.toString();
593 size_t keys_size = keys.size();
594 for (size_t i = 0; i < keys_size; i++)
595 {
596 std::string key_name;
597 if (keys.get(i).isString())
598 {
600 key_name = keys.get(i).asString();
601 if (m_iVar->getRemoteVariable(key_name, val))
602 {
603 std::string key_value = val.toString();
604 std::string p_value = std::string(key_name).append(" ").append(key_value).append("\n");
605 fputs(p_value.c_str(), dumpfile);
606 }
607 }
608 }
609 }
611 }
612}
613
614
615void PartItem::onSliderPosTrajectoryPositionCommand(double posVal, int index)
616{
618 m_ictrlmode->getControlMode(index, mode);
619
621 {
622 m_iPos->positionMove(index, posVal);
623 }
624 else
625 {
626 yWarning("Joint not in position mode so cannot send references");
627 }
628}
629
630void PartItem::onSliderMixedPositionCommand(double posVal, int index)
631{
633 m_ictrlmode->getControlMode(index, mode);
634
636 {
637 m_iPos->positionMove(index, posVal);
638 }
639 else
640 {
641 LOG_ERROR("Joint not in mixed mode so cannot send references");
642 }
643}
644
645void PartItem::onSliderMixedVelocityCommand( double vel, int index)
646{
648 m_ictrlmode->getControlMode(index, mode);
649
651 {
652 m_iVel->velocityMove(index, vel);
653 }
654 else
655 {
656 LOG_ERROR("Joint not in mixed mode so cannot send references");
657 }
658}
659
660void PartItem::onJointInteraction(int interaction,JointItem *joint)
661{
662 const int jointIndex = joint->getJointIndex();
663 switch (interaction) {
665 yInfo("interaction mode of joint %d set to COMPLIANT", jointIndex);
667 break;
668 case JointItem::Stiff:
669 yInfo("interaction mode of joint %d set to STIFF", jointIndex);
671 break;
672 default:
673 break;
674 }
675}
676
677
678void PartItem::onSendPWM(int jointIndex, double pwmVal)
679{
680 double pwm_reference = 0;
681 double current_pwm = 0;
682
683 m_iPWM->setRefDutyCycle(jointIndex, pwmVal);
684
686 m_iPWM->getRefDutyCycle(jointIndex, &pwm_reference); //This is the reference
688 m_iPWM->getDutyCycle(jointIndex, &current_pwm); //This is the real PWM output
689
690 if (m_currentPidDlg){
691 m_currentPidDlg->initPWM(pwm_reference, current_pwm);
692 }
693}
694
695void PartItem::onSendForceOffset(int jointIdex,double force)
696{
697 double offset_val=0;
698
702
703 double off_max=0.0;
704 double off_min=0.0;
706
707 if (m_currentPidDlg)
708 {
709 m_currentPidDlg->initTorqueOffset(offset_val, off_min, off_max);
710 }
711}
712
713void PartItem::onSendStiffness(int jointIdex,double stiff,double damp)
714{
715 double stiff_val=0;
716 double damp_val=0;
717
718 m_iImp->setImpedance(jointIdex, stiff, damp);
721
722 //update the impedance limits
723 double stiff_max=0.0;
724 double stiff_min=0.0;
725 double damp_max=0.0;
726 double damp_min=0.0;
727
729
730 if (m_currentPidDlg)
731 {
732 m_currentPidDlg->initStiffness(stiff_val, stiff_min, stiff_max,
734 }
735
736
737}
738
739void PartItem::onSendMotorParameters(int jointIndex, MotorTorqueParameters newTrqParam)
740{
741 m_iTrq->setMotorTorqueParams(jointIndex, newTrqParam);
743
745 m_iTrq->getMotorTorqueParams(jointIndex, &TrqParam);
746
747 if (m_currentPidDlg){
748 m_currentPidDlg->initMotorParams(TrqParam);
749 }
750}
751
752void PartItem::onSendPid(PidControlTypeEnum pidtype, int jointIndex,Pid newPid)
753{
754 m_iPid->setPid(pidtype, jointIndex, newPid);
756
757 onRefreshPids(jointIndex);
758}
759
760void PartItem::onRefreshPids(int jointIndex)
761{
762 std::vector<PidWithExtraInfo> myPosPidsWithInfo(3);
763 std::vector<PidWithExtraInfo> myPosDirPidsWithInfo(3);
764 std::vector<PidWithExtraInfo> myTrqPidsWithInfo(3);
765 std::vector<PidWithExtraInfo> myVelPidsWithInfo(3);
766 std::vector<PidWithExtraInfo> myVelDirPidsWithInfo(3);
767 std::vector<PidWithExtraInfo> myMixedPidsWithInfo(3);
768 std::vector<PidWithExtraInfo> myCurPidsWithInfo(3);
770 double stiff_val = 0;
771 double damp_val = 0;
772 double stiff_max = 0;
773 double damp_max = 0;
774 double off_max = 0;
775 double stiff_min = 0;
776 double damp_min = 0;
777 double off_min = 0;
778 double impedance_offset_val = 0;
779 double pwm_reference = 0;
780 double current_pwm = 0;
781
783 m_iTrq->getTorqueRange(jointIndex, &off_min, &off_max);
784
785 // Position
786 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1, jointIndex, &myPosPidsWithInfo[0].pid);
787 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2, jointIndex, &myPosPidsWithInfo[1].pid);
788 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3, jointIndex, &myPosPidsWithInfo[2].pid);
789 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_1, jointIndex, myPosPidsWithInfo[0].pidExtraInfo);
790 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_2, jointIndex, myPosPidsWithInfo[1].pidExtraInfo);
791 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_3, jointIndex, myPosPidsWithInfo[2].pidExtraInfo);
793
794 // PositionDirect
795 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1, jointIndex, &myPosDirPidsWithInfo[0].pid);
796 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2, jointIndex, &myPosDirPidsWithInfo[1].pid);
797 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3, jointIndex, &myPosDirPidsWithInfo[2].pid);
798 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_1, jointIndex, myPosDirPidsWithInfo[0].pidExtraInfo);
799 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_2, jointIndex, myPosDirPidsWithInfo[1].pidExtraInfo);
800 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION_DIRECT_3, jointIndex, myPosDirPidsWithInfo[2].pidExtraInfo);
802
803 // Velocity
804 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1, jointIndex, &myVelPidsWithInfo[0].pid);
805 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2, jointIndex, &myVelPidsWithInfo[1].pid);
806 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3, jointIndex, &myVelPidsWithInfo[2].pid);
807 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_1, jointIndex, myVelPidsWithInfo[0].pidExtraInfo);
808 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_2, jointIndex, myVelPidsWithInfo[1].pidExtraInfo);
809 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_3, jointIndex, myVelPidsWithInfo[2].pidExtraInfo);
811
812 // VelocityDirect
813 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1, jointIndex, &myVelDirPidsWithInfo[0].pid);
814 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2, jointIndex, &myVelDirPidsWithInfo[1].pid);
815 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3, jointIndex, &myVelDirPidsWithInfo[2].pid);
816 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_1, jointIndex, myVelDirPidsWithInfo[0].pidExtraInfo);
817 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_2, jointIndex, myVelDirPidsWithInfo[1].pidExtraInfo);
818 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_VELOCITY_DIRECT_3, jointIndex, myVelDirPidsWithInfo[2].pidExtraInfo);
820
821 // Mixed
822 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1, jointIndex, &myMixedPidsWithInfo[0].pid);
823 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2, jointIndex, &myMixedPidsWithInfo[1].pid);
824 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3, jointIndex, &myMixedPidsWithInfo[2].pid);
825 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_1, jointIndex, myMixedPidsWithInfo[0].pidExtraInfo);
826 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_2, jointIndex, myMixedPidsWithInfo[1].pidExtraInfo);
827 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_MIXED_3, jointIndex, myMixedPidsWithInfo[2].pidExtraInfo);
829
830 // Current
831 if (m_iCur)
832 {
833 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1, jointIndex, &myCurPidsWithInfo[0].pid);
834 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2, jointIndex, &myCurPidsWithInfo[1].pid);
835 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3, jointIndex, &myCurPidsWithInfo[2].pid);
836 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_1, jointIndex, myCurPidsWithInfo[0].pidExtraInfo);
837 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_2, jointIndex, myCurPidsWithInfo[1].pidExtraInfo);
838 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_CURRENT_3, jointIndex, myCurPidsWithInfo[2].pidExtraInfo);
840 }
841
842 // Torque
843 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1, jointIndex, &myTrqPidsWithInfo[0].pid);
844 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2, jointIndex, &myTrqPidsWithInfo[1].pid);
845 m_iPid->getPid(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3, jointIndex, &myTrqPidsWithInfo[2].pid);
846 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_1, jointIndex, myTrqPidsWithInfo[0].pidExtraInfo);
847 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_2, jointIndex, myTrqPidsWithInfo[1].pidExtraInfo);
848 m_iPid->getPidExtraInfo(PidControlTypeEnum::VOCAB_PIDTYPE_TORQUE_3, jointIndex, myTrqPidsWithInfo[2].pidExtraInfo);
849 m_iTrq->getMotorTorqueParams(jointIndex, &motorTorqueParams);
851
852 //Stiff
853 m_iImp->getImpedance(jointIndex, &stiff_val, &damp_val);
854 m_iImp->getImpedanceOffset(jointIndex, &impedance_offset_val);
856
857 // PWM
858 m_iPWM->getRefDutyCycle(jointIndex, &pwm_reference);
859 m_iPWM->getDutyCycle(jointIndex, &current_pwm);
860
861 if (m_currentPidDlg)
862 {
863 m_currentPidDlg->initPositionPID(myPosPidsWithInfo);
865 m_currentPidDlg->initVelocityPID(myVelPidsWithInfo);
867 m_currentPidDlg->initMixedPID(myMixedPidsWithInfo);
868 m_currentPidDlg->initTorquePID(myTrqPidsWithInfo);
869 m_currentPidDlg->initMotorParams(motorTorqueParams);
870 m_currentPidDlg->initCurrentPID(myCurPidsWithInfo);
872 m_currentPidDlg->initPWM(pwm_reference, current_pwm);
873 m_currentPidDlg->initRemoteVariables(m_iVar);
874 }
875}
876
877void PartItem::onSendSingleRemoteVariable(std::string key, yarp::os::Bottle val)
878{
879 m_iVar->setRemoteVariable(key, val);
881}
882
883void PartItem::onUpdateAllRemoteVariables()
884{
885 if (m_currentPidDlg){
886 m_currentPidDlg->initRemoteVariables(m_iVar);
887 }
888}
889
890void PartItem::onCalibClicked(JointItem *joint)
891{
892 if (!m_iremCalib)
893 {
894 QMessageBox::critical(this,"Operation not supported", QString("The IRemoteCalibrator interface was not found on this application"));
895 return;
896 }
897
898 if(QMessageBox::question(this,"Question","Do you really want to recalibrate the joint?") != QMessageBox::Yes){
899 return;
900 }
901 if (!m_iremCalib->calibrateSingleJoint(joint->getJointIndex()))
902 {
903 // provide better feedback to user by verifying if the calibrator device was set or not
904 bool isCalib = false;
905 m_iremCalib->isCalibratorDevicePresent(&isCalib);
906 if (!isCalib) {
907 QMessageBox::critical(this,"Calibration failed", QString("No calibrator device was configured to perform this action, please verify that the wrapper config file has the 'Calibrator' keyword in the attach phase"));
908 } else {
909 QMessageBox::critical(this, "Calibration failed", QString("The remote calibrator reported that something went wrong during the calibration procedure"));
910 }
911 }
912
913}
914
915void PartItem::onPidClicked(JointItem *joint)
916{
917 int state = (int)joint->getJointState();
918 const int jointIndex = joint->getJointIndex();
919 QString jointName = joint->getJointName();
920 m_currentPidDlg = new PidDlg(m_partName.c_str(), jointIndex, jointName, state);
921 connect(m_currentPidDlg, SIGNAL(sendPid(PidControlTypeEnum, int, Pid)), this, SLOT(onSendPid(PidControlTypeEnum, int, Pid)));
922 connect(m_currentPidDlg, SIGNAL(sendSingleRemoteVariable(std::string, yarp::os::Bottle)), this, SLOT(onSendSingleRemoteVariable(std::string, yarp::os::Bottle)));
923 connect(m_currentPidDlg, SIGNAL(updateAllRemoteVariables()), this, SLOT(onUpdateAllRemoteVariables()));
924 connect(m_currentPidDlg, SIGNAL(sendMotorParameters(int, MotorTorqueParameters)), this, SLOT(onSendMotorParameters(int, MotorTorqueParameters)));
925 connect(m_currentPidDlg, SIGNAL(sendStiffness(int, double, double, double)), this, SLOT(onSendStiffness(int, double, double, double)));
926 connect(m_currentPidDlg, SIGNAL(sendForceOffset(int, double)), this, SLOT(onSendForceOffset(int, double)));
927 connect(m_currentPidDlg, SIGNAL(sendPWM(int, double)), this, SLOT(onSendPWM(int, double)));
928 connect(m_currentPidDlg, SIGNAL(refreshPids(int)), this, SLOT(onRefreshPids(int)));
929 connect(m_currentPidDlg, SIGNAL(dumpRemoteVariables()), this, SLOT(onDumpAllRemoteVariables()));
930
931 this->onRefreshPids(jointIndex);
932
933 m_currentPidDlg->exec();
934
935 delete m_currentPidDlg;
936 m_currentPidDlg = nullptr;
937}
938
939void PartItem::onRunClicked(JointItem *joint)
940{
941 const int jointIndex = joint->getJointIndex();
942 double posJoint;
943 while (!m_iencs->getEncoder(jointIndex, &posJoint)){
945 }
946
948}
949
950void PartItem::onIdleClicked(JointItem *joint)
951{
952 const int jointIndex = joint->getJointIndex();
954}
955
956void PartItem::onHomeClicked(JointItem *joint)
957{
958 size_t number_of_joints=0;
959 const int jointIndex = joint->getJointIndex();
960 m_iPos->getAxes(number_of_joints);
961
962 this->homeJoint(jointIndex);
963}
964
965void PartItem::onJointChangeMode(int mode,JointItem *joint)
966{
967 const int jointIndex = joint->getJointIndex();
968 switch (mode) {
969 case JointItem::Idle:{
970 yInfo("joint: %d in IDLE mode", jointIndex);
971 if (m_ictrlmode){
973 } else {
974 yError("ERROR: cannot do!");
975 }
976 break;
977 }
979 yInfo("joint: %d in POSITION mode", jointIndex);
980 if (m_ictrlmode){
982 joint->resetTarget();
983 } else {
984 yError("ERROR: cannot do!");
985 }
986 break;
987 }
989 //if(positionDirectEnabled){
990 yInfo("joint: %d in POSITION DIRECT mode", jointIndex);
991 if (m_ictrlmode){
992 joint->resetTarget();
994 } else {
995 yError("ERROR: cannot do!");
996 }
997 break;
998 /*}else{
999 LOG_ERROR("joint: %d in MIXED mode", jointIndex);
1000 if(ctrlmode2){
1001 ctrlmode2->setControlMode(jointIndex, VOCAB_CM_MIXED);
1002 } else {
1003 yError("ERROR: cannot do!");
1004 }
1005 break;
1006 }*/
1007 }
1008 case JointItem::Mixed:{
1009 //if(positionDirectEnabled){
1010 yInfo("joint: %d in MIXED mode", jointIndex);
1011 if (m_ictrlmode){
1012 joint->resetTarget();
1014 } else {
1015 yError("ERROR: cannot do!");
1016 }
1017 break;
1018 /*}else{
1019 LOG_ERROR("joint: %d in VELOCITY mode", jointIndex);
1020 if(ctrlmode2){
1021 ctrlmode2->setVelocityMode(jointIndex);
1022 } else {
1023 LOG_ERROR("ERROR: cannot do!");
1024 }
1025 break;
1026 }*/
1027
1028 }
1030 //if(positionDirectEnabled){
1031 yInfo("joint: %d in VELOCITY DIRECT mode", jointIndex);
1032 if (m_ictrlmode)
1033 {
1034 joint->resetTarget();
1036 } else {
1037 yError("ERROR: cannot do!");
1038 }
1039 break;
1040 }
1041 case JointItem::Velocity:{
1042 //if(positionDirectEnabled){
1043 yInfo("joint: %d in VELOCITY mode", jointIndex);
1044 if (m_ictrlmode)
1045 {
1047 yInfo() << "Changing reference acceleration of joint " << jointIndex << " to 100000";
1048 m_iVel->setTrajAcceleration(jointIndex, 100000);
1049 } else {
1050 yError("ERROR: cannot do!");
1051 }
1052 break;
1053// } else {
1054// LOG_ERROR("joint: %d in TORQUE mode", jointIndex);
1055// if(ctrlmode2){
1056// ctrlmode2->setTorqueMode(jointIndex);
1057// } else {
1058// LOG_ERROR("ERROR: cannot do!");
1059// }
1060// break;
1061// }
1062 }
1063
1064 case JointItem::Torque:{
1065 //if(positionDirectEnabled){
1066 yInfo("joint: %d in TORQUE mode", jointIndex);
1067 if (m_ictrlmode){
1069 } else {
1070 yError("ERROR: cannot do!");
1071 }
1072 break;
1073// } else {
1074// LOG_ERROR("joint: %d in TORQUE mode", jointIndex);
1075// if(ctrlmode2){
1076// ctrlmode2->setTorqueMode(jointIndex);
1077// } else {
1078// LOG_ERROR("ERROR: cannot do!");
1079// }
1080// break;
1081// }
1082
1083 }
1084 case JointItem::Pwm:{
1085 yInfo("joint: %d in PWM mode", jointIndex);
1086 if (m_ictrlmode){
1088 } else {
1089 yError("ERROR: cannot do!");
1090 }
1091 break;
1092 }
1093 case JointItem::Current:{
1094 yInfo("joint: %d in CURRENT mode", jointIndex);
1095 if (m_ictrlmode){
1097 }
1098 else {
1099 yError("ERROR: cannot do!");
1100 }
1101 break;
1102 }
1103 default:
1104 break;
1105 }
1106}
1107
1109{
1110 int count = m_layout->count();
1111
1112
1113 int jointPerLineCount = (w - 20) / (MAX_WIDTH_JOINT + 10);
1114 if(jointPerLineCount > count){
1115 jointPerLineCount = count;
1116 }
1117 if(jointPerLineCount <= 0){
1118 return;
1119 }
1120
1121 int extraSpace = (w - 20) - jointPerLineCount * (MAX_WIDTH_JOINT + 10);
1122
1123
1124
1125 for(int i=0;i<count;i++){
1126 QWidget *widget = m_layout->itemAt(i)->widget();
1127 if(widget){
1128 widget->setMaximumWidth(MAX_WIDTH_JOINT + (extraSpace/jointPerLineCount));
1129 widget->setMinimumWidth(MAX_WIDTH_JOINT + (extraSpace/jointPerLineCount));
1130 }
1131
1132 }
1133}
1134
1136{
1137 return m_layout->count();
1138}
1139
1141{
1143 {
1144 return "";
1145 }
1146
1147 auto* jointWidget = (JointItem*)m_layout->itemAt(joint)->widget();
1148
1149 return jointWidget->getJointName();
1150}
1151
1153{
1154 return (JointItem*)m_layout->itemAt(jointIndex)->widget();
1155}
1156
1158{
1159 if(!isVisible()){
1160 return;
1161 }
1162
1163 resizeWidget(event->size().width());
1164}
1165
1167{
1168 if(event->type() == QEvent::WindowStateChange ){
1169 qDebug() << "State Changed " << width();
1170 int count = m_layout->count();
1171 int jointPerLineCount = (width() - 20) / (MAX_WIDTH_JOINT + 10);
1172
1173 if(jointPerLineCount > count){
1174 jointPerLineCount = count;
1175 }
1176 if(jointPerLineCount <= 0){
1177 return;
1178 }
1179 int extraSpace = (width() - 20) - jointPerLineCount * (MAX_WIDTH_JOINT + 10);
1180
1181
1183
1184 for(int i=0;i<count;i++){
1185 QWidget *widget = m_layout->itemAt(i)->widget();
1186 if(widget){
1187 widget->setMaximumWidth(MAX_WIDTH_JOINT + (extraSpace/jointPerLineCount));
1188 widget->setMinimumWidth(MAX_WIDTH_JOINT + (extraSpace/jointPerLineCount));
1189 }
1190 }
1191 }
1192
1193}
1194
1196{
1197 if (!m_iremCalib)
1198 {
1199 QMessageBox::critical(this,"Operation not supported", QString("The IRemoteCalibrator interface was not found on this application"));
1200 return;
1201 }
1202
1203 if (!m_iremCalib->calibrateWholePart())
1204 {
1205 // provide better feedback to user by verifying if the calibrator device was set or not
1206 bool isCalib = false;
1207 m_iremCalib->isCalibratorDevicePresent(&isCalib);
1208 if (!isCalib) {
1209 QMessageBox::critical(this, "Calibration failed", QString("No calibrator device was configured to perform this action, please verify that the wrapper config file for part %1 has the 'Calibrator' keyword in the attach phase").arg(m_partName.c_str()));
1210 } else {
1211 QMessageBox::critical(this, "Calibration failed", QString("The remote calibrator reported that something went wrong during the calibration procedure"));
1212 }
1213 }
1214}
1215
1216bool PartItem::homeJoint(int jointIndex)
1217{
1218 if (!m_iremCalib)
1219 {
1220 QMessageBox::critical(this, "Operation not supported", QString("The IRemoteCalibrator interface was not found on this application"));
1221 return false;
1222 }
1223
1224 if (!m_iremCalib->homingSingleJoint(jointIndex))
1225 {
1226 // provide better feedback to user by verifying if the calibrator device was set or not
1227 bool isCalib = false;
1228 m_iremCalib->isCalibratorDevicePresent(&isCalib);
1229 if (!isCalib)
1230 {
1231 QMessageBox::critical(this, "Operation failed", QString("No calibrator device was configured to perform this action, please verify that the wrapper config file for part %1 has the 'Calibrator' keyword in the attach phase").arg(m_partName.c_str()));
1232 return false;
1233 }
1234 else
1235 {
1236 QMessageBox::critical(this, "Operation failed", QString("The remote calibrator reported that something went wrong during the calibration procedure"));
1237 return false;
1238 }
1239 }
1240 return true;
1241}
1242
1244{
1245 if (!m_iremCalib)
1246 {
1247 QMessageBox::critical(this, "Operation not supported", QString("The IRemoteCalibrator interface was not found on this application"));
1248 return false;
1249 }
1250
1251 if (!m_iremCalib->homingWholePart())
1252 {
1253 // provide better feedback to user by verifying if the calibrator device was set or not
1254 bool isCalib = false;
1255 m_iremCalib->isCalibratorDevicePresent(&isCalib);
1256 if (!isCalib)
1257 {
1258 QMessageBox::critical(this, "Operation failed", QString("No calibrator device was configured to perform this action, please verify that the wrapper config file for part %1 has the 'Calibrator' keyword in the attach phase").arg(m_partName.c_str()));
1259 return false;
1260 }
1261 else
1262 {
1263 QMessageBox::critical(this, "Operation failed", QString("The remote calibrator reported that something went wrong during the calibration procedure"));
1264 return false;
1265 }
1266 }
1267 return true;
1268}
1269
1271{
1272 bool ok = true;
1273 size_t number_of_joints=0;
1274 m_iPos->getAxes(number_of_joints);
1275
1276 if (positionElement.isNull()) {
1277 QMessageBox::critical(this, "Operation failed", QString("No custom position supplied in configuration file for part ") + QString(m_partName.c_str()));
1278 return false;
1279 }
1280
1281 //Look for group called m_robotPartPort_Position and m_robotPartPort_Velocity
1282 Bottle xtmp, ytmp;
1283 xtmp = positionElement.findGroup(m_robotPartPort + "_Position");
1284 ok = ok && (xtmp.size() == (size_t) number_of_joints + 1);
1285 ytmp = positionElement.findGroup(m_robotPartPort + "_Velocity");
1286 ok = ok && (ytmp.size() == (size_t) number_of_joints + 1);
1287
1288 if(ok)
1289 {
1290 for (size_t jointIndex = 0; jointIndex < number_of_joints; jointIndex++)
1291 {
1292 double position = xtmp.get(jointIndex+1).asFloat64();
1293 double velocity = ytmp.get(jointIndex + 1).asFloat64();
1294 m_iPos->setTrajSpeed(jointIndex, velocity);
1295 m_iPos->positionMove(jointIndex, position);
1296 }
1297 }
1298 else
1299 {
1300 QMessageBox::critical(this, "Error", QString("Check the number of entries in the group %1").arg(m_robotPartPort.c_str()));
1301 ok = false;
1302 }
1303
1304 return ok;
1305}
1306
1308{
1309 size_t number_of_joints=0;
1310 m_iPos->getAxes(number_of_joints);
1311
1312 for (size_t joint=0; joint < number_of_joints; joint++){
1314 }
1315}
1316
1318{
1319 if (!m_sequenceWindow){
1320 return false;
1321 }
1322
1323 return m_sequenceWindow->checkAndRun();
1324
1325
1326}
1327
1329{
1330 if (!m_sequenceWindow){
1331 return false;
1332 }
1333
1334 return m_sequenceWindow->checkAndRunTime();
1335
1336
1337}
1338
1340{
1341 if (!m_sequenceWindow){
1342 return false;
1343 }
1344
1345 return m_sequenceWindow->checkAndCycleTimeSeq();
1346}
1347
1349{
1350 if (!m_sequenceWindow){
1351 return false;
1352 }
1353
1354 return m_sequenceWindow->checkAndCycleSeq();
1355}
1356
1358{
1359 size_t number_of_joints=0;
1360 m_iPos->getAxes(number_of_joints);
1361
1362 for (size_t joint=0; joint < number_of_joints; joint++){
1363 //iencs->getEncoder(joint, &posJoint);
1365 }
1366}
1367
1369{
1371 onOpenSequence();
1372}
1373
1379
1381{
1382 if (m_sequenceWindow)
1383 {
1384 m_sequenceWindow->close();
1385 }
1386}
1387
1389{
1390 if (!m_sequenceWindow){
1391 m_sequenceWindow = new SequenceWindow(m_partName.c_str(), m_layout->count());
1392 connect(m_sequenceWindow, SIGNAL(itemDoubleClicked(int)), this, SLOT(onSequenceWindowDoubleClicked(int)), Qt::DirectConnection);
1393 connect(this, SIGNAL(sendPartJointsValues(int, QList<double>, QList<double>)), m_sequenceWindow, SLOT(onReceiveValues(int, QList<double>, QList<double>)), Qt::DirectConnection);
1394 connect(m_sequenceWindow, SIGNAL(goToPosition(SequenceItem)), this, SLOT(onGo(SequenceItem)));
1395 connect(m_sequenceWindow, SIGNAL(runTime(QList<SequenceItem>)), this, SLOT(onSequenceRunTime(QList<SequenceItem>)), Qt::QueuedConnection);
1396 connect(m_sequenceWindow, SIGNAL(run(QList<SequenceItem>)), this, SLOT(onSequenceRun(QList<SequenceItem>)), Qt::QueuedConnection);
1397 connect(m_sequenceWindow, SIGNAL(saveSequence(QList<SequenceItem>, QString)), this, SLOT(onSaveSequence(QList<SequenceItem>, QString)), Qt::QueuedConnection);
1398 connect(m_sequenceWindow, SIGNAL(openSequence()), this, SLOT(onOpenSequence()));
1399 connect(m_sequenceWindow, SIGNAL(cycle(QList<SequenceItem>)), this, SLOT(onSequenceCycle(QList<SequenceItem>)), Qt::QueuedConnection);
1400 connect(m_sequenceWindow, SIGNAL(cycleTime(QList<SequenceItem>)), this, SLOT(onSequenceCycleTime(QList<SequenceItem>)), Qt::QueuedConnection);
1401 connect(m_sequenceWindow, SIGNAL(stopSequence()), this, SLOT(onStopSequence()), Qt::QueuedConnection);
1402
1403 connect(this, SIGNAL(runTimeSequence()), m_sequenceWindow, SLOT(onRunTimeSequence()));
1404 connect(this, SIGNAL(cycleTimeSequence()), m_sequenceWindow, SLOT(onCycleTimeSequence()));
1405 connect(this, SIGNAL(cycleSequence()), m_sequenceWindow, SLOT(onCycleSequence()));
1406 connect(this, SIGNAL(stoppedSequence()), m_sequenceWindow, SLOT(onStoppedSequence()));
1407 connect(this, SIGNAL(setCurrentIndex(int)), m_sequenceWindow, SLOT(onSetCurrentSequenceIndex(int)));
1408
1409 connect(this,SIGNAL(runTimeSequence()),this,SLOT(onSequenceActivated()));
1410 connect(this,SIGNAL(cycleTimeSequence()),this,SLOT(onSequenceActivated()));
1411 connect(this,SIGNAL(cycleSequence()),this,SLOT(onSequenceActivated()));
1412 connect(this,SIGNAL(stoppedSequence()),this,SLOT(onSequenceStopped()));
1413
1414
1415 }
1416
1417 if (!m_sequenceWindow->isVisible()){
1418 m_sequenceWindow->show();
1419 }else{
1420 m_sequenceWindow->setFocus();
1421 m_sequenceWindow->raise();
1422 m_sequenceWindow->setWindowState(Qt::WindowActive);
1423 }
1424
1425}
1426
1428{
1429 if (!m_sequenceWindow){
1430 return false;
1431 }
1432
1433 return m_sequenceWindow->checkAndGo();
1434}
1436{
1437 m_cycleTimer.stop();
1438 m_runTimer.stop();
1439 m_runTimeTimer.stop();
1440 m_cycleTimeTimer.stop();
1442}
1443
1444void PartItem::onStopSequence()
1445{
1446 stopSequence();
1447}
1448
1449void PartItem::onOpenSequence()
1450{
1451 QString fileName = QFileDialog::getOpenFileName(m_sequenceWindow, QString("Load Sequence for part %1 As").arg(m_partName.c_str()), QDir::homePath());
1452
1453 QFileInfo fInfo(fileName);
1454 if(!fInfo.exists()){
1455 return;
1456 }
1457
1458 QString desiredExtension = QString("pos%1").arg(m_partName.c_str());
1459 QString extension = fInfo.suffix();
1460
1462 QMessageBox::critical(this,"Error Loading The Sequence",
1463 QString("Wrong format (check extensions) of the file associated with: ").arg(m_partName.c_str()));
1464 return;
1465 }
1466
1467 QFile file(fileName);
1468 if (!file.open(QFile::ReadOnly | QFile::Text)){
1469 QString msg = QString("Error: Cannot read file %1: %2").arg(qPrintable(fileName))
1470 .arg(qPrintable(file.errorString()));
1471 QMessageBox::critical(this,"Error Loading The Sequence",msg);
1472 return;
1473 }
1474
1475 QXmlStreamReader reader(&file);
1476 reader.readNext();
1477
1479
1482 while(!reader.atEnd()){
1483 reader.readNext();
1484
1485 if(reader.isStartElement()){
1486 if(reader.name().contains("Sequence_")){ //Sequence_
1487 QXmlStreamAttributes attributes = reader.attributes();
1488 referencePart = attributes.value("ReferencePart").toString();
1489 }
1490
1491 if(reader.name() == "Position"){ //Position
1492 QXmlStreamAttributes attributes = reader.attributes();
1493 int index = attributes.value("Index").toInt();
1494 double timing = attributes.value("Timing").toDouble();
1495 item.setTiming(timing);
1496 item.setSequenceNumber(index);
1497 }
1498
1499 if(reader.name() == "JointPositions"){
1500 QXmlStreamAttributes attributes = reader.attributes();
1501 int count = attributes.value("Count").toInt();
1502
1503 reader.readNext();
1504 for(int i=0; i<count;i++){
1505 reader.readNext();
1506 if(reader.name() == "PosValue"){ //PosValue
1507 double pos = reader.readElementText().toDouble();
1508 item.addPositionValue(pos);
1509 }
1510 reader.readNext();
1511 }
1512
1513 }
1514
1515 if(reader.name() == "JointVelocities"){
1516 QXmlStreamAttributes attributes = reader.attributes();
1517 int count = attributes.value("Count").toInt();
1518
1519 reader.readNext();
1520 for(int i=0; i<count;i++){
1521 reader.readNext();
1522 if(reader.name() == "SpeedValue"){ //SpeedValue
1523 double speed = reader.readElementText().toDouble();
1524 item.addSpeedValue(speed);
1525 }
1526 reader.readNext();
1527 }
1528
1529 }
1530
1531 }
1532
1533 if(reader.isEndElement()){
1534 if(reader.name() == "Position"){
1535 sequenceItems.append(item);
1536 item = SequenceItem();
1537 }
1538 }
1539 }
1540
1541 file.close();
1542
1543 if (reader.hasError())
1544 {
1545 QString msg = QString("Error: Failed to parse file %1: %2").arg(qPrintable(fileName)).arg(qPrintable(reader.errorString()));
1546 QMessageBox::critical(this,"Error Loading The Sequence",msg);
1547 return;
1548 } else if (file.error() != QFile::NoError) {
1549 QString msg = QString("Error: Cannot read file %1: %2").arg(qPrintable(fileName)).arg(qPrintable(file.errorString()));
1550 QMessageBox::critical(this,"Error Loading The Sequence",msg);
1551 return;
1552 }
1553
1554 if (m_sequenceWindow){
1555 m_sequenceWindow->loadSequence(sequenceItems);
1556 }
1557
1558}
1559
1560void PartItem::onSaveSequence(QList<SequenceItem> values, QString fileName)
1561{
1562 if (fileName=="")
1563 {
1564 fileName = QFileDialog::getSaveFileName(this, QString("Save Sequence for part %1 As").arg(m_partName.c_str()), QDir::homePath());
1565 }
1566
1567 if(fileName.isEmpty()){
1568 return;
1569 }
1570
1571 QFileInfo fInfo(fileName);
1572 QString completeFileName = QString("%1/%2.pos%3").arg(fInfo.absolutePath()).arg(fInfo.baseName()).arg(m_partName.c_str());
1573 std::string completeFileName_s = completeFileName.toStdString();
1574
1575 //QFile file(completeFileName);
1576 yInfo("Saving file %s\n", completeFileName_s.c_str());
1577
1578 QFile file(completeFileName);
1579 if(!file.open(QIODevice::WriteOnly)){
1580 return;
1581 }
1582
1583 QXmlStreamWriter writer(&file);
1584 writer.setAutoFormatting(true);
1585 writer.writeStartDocument();
1586
1587 writer.writeStartElement(QString("Sequence_pos%1").arg(m_partName.c_str()));
1588
1589 writer.writeAttribute("TotPositions", QString("%1").arg(values.count()));
1590 writer.writeAttribute("ReferencePart", m_partName.c_str());
1591
1592 for(int i=0;i<values.count();i++){
1593 SequenceItem sequenceItem = values.at(i);
1594 writer.writeStartElement("Position");
1595 writer.writeAttribute("Index",QString("%1").arg(sequenceItem.getSequenceNumber()));
1596 writer.writeAttribute("Timing",QString("%L1").arg(sequenceItem.getTiming(),0,'f',2));
1597
1598 writer.writeStartElement("JointPositions");
1599 writer.writeAttribute("Count",QString("%1").arg(sequenceItem.getPositions().count()));
1600 for(int j=0;j<sequenceItem.getPositions().count(); j++){
1601 QString s = QString("%L1").arg(sequenceItem.getPositions().at(j),0,'f',2);
1602 writer.writeTextElement("PosValue",s);
1603 }
1604 writer.writeEndElement();
1605
1606 writer.writeStartElement("JointVelocities");
1607 writer.writeAttribute("Count",QString("%1").arg(sequenceItem.getSpeeds().count()));
1608 for(int j=0;j<sequenceItem.getSpeeds().count(); j++){
1609 QString s = QString("%L1").arg(sequenceItem.getSpeeds().at(j),0,'f',2);
1610 writer.writeTextElement("SpeedValue",s);
1611 }
1612 writer.writeEndElement();
1613 writer.writeEndElement();
1614 }
1615 writer.writeEndElement();
1616 writer.writeEndDocument();
1617 file.close();
1618 LOG_INFO("File saved and closed\n");
1619
1620
1621// if(file.open(QIODevice::WriteOnly)){
1622// for(int i=0;i<values.count();i++){
1623// SequenceItem sequenceItem = values.at(i);
1624// QString s = QString("[POSITION%1] \n").arg(sequenceItem.getSequenceNumber());
1625// file.write(s.toLatin1().data(),s.length());
1626
1627// s = QString("jointPositions ");
1628// file.write(s.toLatin1().data(),s.length());
1629
1630// for(int j=0;j<sequenceItem.getPositions().count(); j++){
1631// s = QString("%L1 ").arg(sequenceItem.getPositions().at(j),0,'f',2);
1632// file.write(s.toLatin1().data(),s.length());
1633// }
1634
1635// s = QString("\njointVelocities ");
1636// file.write(s.toLatin1().data(),s.length());
1637// for(int j=0;j<sequenceItem.getSpeeds().count(); j++){
1638// s = QString("%L1 ").arg(sequenceItem.getSpeeds().at(j),0,'f',2);
1639// file.write(s.toLatin1().data(),s.length());
1640// }
1641
1642// s = QString("\ntiming ");
1643// file.write(s.toLatin1().data(),s.length());
1644// s = QString("%L1 \n").arg(sequenceItem.getTiming(),0,'f',2);
1645// file.write(s.toLatin1().data(),s.length());
1646// }
1647// file.flush();
1648// file.close();
1649
1650// LOG_ERROR("File saved and closed\n");
1651// }
1652
1653}
1654
1655void PartItem::onSequenceCycleTime(QList<SequenceItem> values)
1656{
1657 // Remove items after the first timing with value < 0
1658 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1659 return;
1660 }
1661
1662 m_cycleTimeValues.clear();
1663 for(int i=0;i<values.count();i++){
1664 SequenceItem it = values.at(i);
1665 if(it.getTiming() > 0){
1666 m_cycleTimeValues.append(it);
1667 }
1668 }
1669
1670 SequenceItem vals;
1671 if (m_cycleTimeValues.count() > 0)
1672 {
1673 vals = m_cycleTimeValues.takeFirst();
1674 m_cycleTimeValues.append(vals);
1676 fixedTimeMove(vals);
1677 m_cycleTimeTimer.start(vals.getTiming() * 1000);
1679 }
1680}
1681
1682void PartItem::onCycleTimeTimerTimeout()
1683{
1684 if (m_cycleTimeValues.count() > 0)
1685 {
1686 SequenceItem vals = m_cycleTimeValues.takeFirst();
1687 m_cycleTimeValues.append(vals);
1689 fixedTimeMove(vals);
1690 m_cycleTimeTimer.start(vals.getTiming() * 1000);
1691 }
1692}
1693
1694void PartItem::onSequenceCycle(QList<SequenceItem> values)
1695{
1696 // Remove items after the first timing with value < 0
1697 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1698 return;
1699 }
1700
1701 m_cycleValues.clear();
1702 for(int i=0;i<values.count();i++){
1703 SequenceItem it = values.at(i);
1704 if(it.getTiming() > 0){
1705 m_cycleValues.append(it);
1706 }
1707 }
1708
1709 SequenceItem vals;
1710 if (m_cycleValues.count() > 0){
1711 vals = m_cycleValues.takeFirst();
1712
1713 m_cycleValues.append(vals);
1714
1715 double *cmdPositions = new double[vals.getPositions().count()];
1716 double *cmdVelocities = new double[vals.getSpeeds().count()];
1717
1718 for(int i=0;i<vals.getPositions().count();i++){
1719 cmdPositions[i] = vals.getPositions().at(i);
1720 cmdVelocities[i] = vals.getSpeeds().at(i);
1721 //qDebug() << "vals.getSpeeds().at(i)" << vals.getSpeeds().at(i);
1722 }
1723
1725 //fixedTimeMove(vals.getPositions());
1727 m_iPos->positionMove(cmdPositions);
1728 m_cycleTimer.start(vals.getTiming() * 1000);
1729
1731 }
1732}
1733
1734void PartItem::onCycleTimerTimeout()
1735{
1736 if (m_cycleValues.count() > 0){
1737 SequenceItem vals = m_cycleValues.takeFirst();
1738
1739 m_cycleValues.append(vals);
1740
1741 double *cmdPositions = new double[vals.getPositions().count()];
1742 double *cmdVelocities = new double[vals.getSpeeds().count()];
1743
1744 for(int i=0;i<vals.getPositions().count();i++){
1745 cmdPositions[i] = vals.getPositions().at(i);
1746 cmdVelocities[i] = vals.getSpeeds().at(i);
1747 }
1748
1750 //fixedTimeMove(vals.getPositions());
1752 m_iPos->positionMove(cmdPositions);
1753
1754 m_cycleTimer.start(vals.getTiming() * 1000);
1755 }
1756}
1757
1758void PartItem::onSequenceRun(QList<SequenceItem> values)
1759{
1760 // Remove items after the first timing with value < 0
1761 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1762 return;
1763 }
1764
1765 m_runValues.clear();
1766 for(int i=0;i<values.count();i++){
1767 SequenceItem it = values.at(i);
1768 if(it.getTiming() > 0){
1769 m_runValues.append(it);
1770 }
1771 }
1772
1773 SequenceItem vals;
1774 if (m_runValues.count() > 0){
1775 vals = m_runValues.takeFirst();
1776
1777 m_runValues.append(vals);
1778
1779 double *cmdPositions = new double[vals.getPositions().count()];
1780 double *cmdVelocities = new double[vals.getSpeeds().count()];
1781
1782 for(int i=0;i<vals.getPositions().count();i++){
1783 cmdPositions[i] = vals.getPositions().at(i);
1784 cmdVelocities[i] = vals.getSpeeds().at(i);
1785 //qDebug() << "vals.getSpeeds().at(i)" << vals.getSpeeds().at(i);
1786 }
1787
1789 //fixedTimeMove(vals.getPositions());
1791 m_iPos->positionMove(cmdPositions);
1792 m_runTimer.start(vals.getTiming() * 1000);
1793
1794 emit runSequence();
1795 }
1796}
1797void PartItem::onRunTimeout()
1798{
1799 if (m_runValues.count() > 0){
1800 SequenceItem vals = m_runValues.takeFirst();
1801 if(vals.getTiming() < 0){
1803 return;
1804 }
1805 double *cmdPositions = new double[vals.getPositions().count()];
1806 double *cmdVelocities = new double[vals.getSpeeds().count()];
1807
1808 for(int i=0;i<vals.getPositions().count();i++){
1809 cmdPositions[i] = vals.getPositions().at(i);
1810 cmdVelocities[i] = vals.getSpeeds().at(i);
1811 //qDebug() << "vals.getSpeeds().at(i)" << vals.getSpeeds().at(i);
1812 }
1813
1815 //fixedTimeMove(vals.getPositions());
1817 m_iPos->positionMove(cmdPositions);
1818
1819
1820 m_runTimer.start(vals.getTiming() * 1000);
1821 }else{
1823 }
1824}
1825
1826
1827
1828void PartItem::onSequenceRunTime(QList<SequenceItem> values)
1829{
1830 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1831 return;
1832 }
1833
1834 m_runTimeValues.clear();
1835
1836 for(int i=0;i<values.count();i++){
1837 SequenceItem it = values.at(i);
1838 if(it.getTiming() > 0){
1839 m_runTimeValues.append(it);
1840 }
1841 }
1842
1843 SequenceItem vals;
1844 if (m_runTimeValues.count() > 0){
1845 vals = m_runTimeValues.takeFirst();
1846
1848 fixedTimeMove(vals);
1849 m_runTimeTimer.start(vals.getTiming() * 1000);
1850
1852 }
1853
1854}
1855
1856void PartItem::onRunTimerTimeout()
1857{
1858 if (m_runTimeValues.count() > 0){
1859 SequenceItem vals = m_runTimeValues.takeFirst();
1860 if(vals.getTiming() < 0){
1862 return;
1863 }
1865 fixedTimeMove(vals);
1866 m_runTimeTimer.start(vals.getTiming() * 1000);
1867 }else{
1869 }
1870}
1871
1872void PartItem::fixedTimeMove(SequenceItem sequence)
1873{
1874 size_t NUM_JOINTS=0;
1875 m_iPos->getAxes(NUM_JOINTS);
1876 auto* cmdPositions = new double[NUM_JOINTS];
1877 auto* cmdVelocities = new double[NUM_JOINTS];
1878 auto* startPositions = new double[NUM_JOINTS];
1879 double cmdTime = sequence.getTiming();
1880
1881 while (!m_iencs->getEncoders(startPositions)){
1883 }
1884
1885
1886 for(size_t k=0; k<NUM_JOINTS; k++){
1887 cmdVelocities[k] = 0;
1888 cmdPositions[k] = sequence.getPositions().at(k);
1889
1890 if (fabs(startPositions[k] - cmdPositions[k]) > 0.01){
1892 } else {
1893 cmdVelocities[k] = 1.0;
1894 }
1895 }
1896
1898 m_iPos->positionMove(cmdPositions);
1899
1900 m_sequence_port_stamp.update();
1901 m_sequence_port.setEnvelope(m_sequence_port_stamp);
1903 m_sequence_port.write(v);
1904 delete[] cmdVelocities;
1905 delete[] startPositions;
1906 delete[] cmdPositions;
1907 return;
1908}
1909
1910void PartItem::onGo(SequenceItem sequenceItem)
1911{
1912 if(sequenceItem.getPositions().isEmpty() || sequenceItem.getSpeeds().isEmpty())
1913 {
1914 QMessageBox::critical(this,"Error", "Select an entry in the table before performing a movement");
1915 return;
1916 }
1917
1918 size_t number_of_joints=0;
1919 m_iPos->getAxes(number_of_joints);
1920
1921 for(size_t i=0;i<number_of_joints;i++)
1922 {
1923 m_iPos->setTrajSpeed(i, sequenceItem.getSpeeds().at(i));
1924 m_iPos->positionMove(i, sequenceItem.getPositions().at(i));
1925 }
1926}
1927
1928void PartItem::onSequenceActivated()
1929{
1930 for (int i = 0; i<m_layout->count(); i++)
1931 {
1932 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
1933 if(joint){
1934 joint->sequenceActivated();
1935 }
1936 }
1937
1939
1940}
1941
1942void PartItem::onSequenceStopped()
1943{
1944 for (int i = 0; i<m_layout->count(); i++)
1945 {
1946 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
1947 if(joint){
1948 joint->sequenceStopped();
1949 }
1950 }
1952}
1953
1954void PartItem::onSequenceWindowDoubleClicked(int sequenceNum)
1955{
1956 QList<double>values;
1957 QList<double>speeds;
1958 for (int i = 0; i<m_layout->count(); i++)
1959 {
1960 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
1961 values.append(this->m_positions[i]);
1962 speeds.append(joint->getTrajectoryVelocityValue());
1963 }
1964
1965 emit sendPartJointsValues(sequenceNum,values,speeds);
1966}
1967
1969{
1970 for (int i = 0; i<m_layout->count(); i++)
1971 {
1972 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
1974 }
1975}
1976
1978{
1979 for (int i = 0; i<m_layout->count(); i++)
1980 {
1981 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
1983 }
1984}
1985
1987{
1988 for (int i = 0; i<m_layout->count(); i++)
1989 {
1990 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
1992 }
1993}
1994
1996{
1997 for (int i = 0; i<m_layout->count(); i++)
1998 {
1999 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2000 joint->enableControlPWM(control);
2001 }
2002}
2003
2005{
2006 for (int i = 0; i<m_layout->count(); i++)
2007 {
2008 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2010 }
2011}
2012
2014{
2015 for (int i = 0; i<m_layout->count(); i++)
2016 {
2017 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2019 }
2020}
2021
2023{
2024 for (int i = 0; i<m_layout->count(); i++)
2025 {
2026 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2028 }
2029}
2030
2032{
2033 m_part_speedVisible = view;
2034 for (int i = 0; i<m_layout->count(); i++)
2035 {
2036 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2037 joint->setSpeedVisible(view);
2038 }
2039}
2040
2042{
2043 m_part_motorPositionVisible = view;
2044 for (int i = 0; i<m_layout->count(); i++)
2045 {
2046 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2047 joint->setMotorPositionVisible(view);
2048 }
2049}
2050
2052{
2053 m_part_dutyVisible = view;
2054 for (int i = 0; i<m_layout->count(); i++)
2055 {
2056 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2057 joint->setDutyVisible(view);
2058 }
2059}
2060
2062{
2063 m_part_currentVisible = view;
2064 for (int i = 0; i<m_layout->count(); i++)
2065 {
2066 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2067 joint->setCurrentsVisible(view);
2068 }
2069}
2070
2072{
2073 for (int i = 0; i<m_layout->count(); i++)
2074 {
2075 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2076 joint->viewPositionTargetBox(ena);
2077 }
2078}
2079
2081{
2082 for (int i = 0; i < m_layout->count(); i++)
2083 {
2084 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2086 }
2087}
2088
2090{
2091 for (int i = 0; i<m_layout->count(); i++)
2092 {
2093 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2094 if (mode==0)
2095 {
2097 }
2098 else if(mode ==1)
2099 {
2101 }
2102 else if (mode == 2)
2103 {
2105 }
2106 else
2107 {
2109 }
2111 }
2112}
2114{
2115 for (int i = 0; i<m_layout->count(); i++)
2116 {
2117 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2118 if (mode == 0)
2119 {
2121 }
2122 else if (mode == 1)
2123 {
2125 }
2126 else if (mode == 2)
2127 {
2129 }
2130 else
2131 {
2133 }
2135 }
2136}
2138{
2139 for (int i = 0; i<m_layout->count(); i++)
2140 {
2141 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2142 if (mode == 0)
2143 {
2145 }
2146 else if (mode == 1)
2147 {
2149 }
2150 else if (mode == 2)
2151 {
2153 }
2154 else
2155 {
2157 }
2159 }
2160}
2162{
2163 for (int i = 0; i<m_layout->count(); i++)
2164 {
2165 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2166 if (mode == 0)
2167 {
2169 }
2170 else if (mode == 1)
2171 {
2172 joint->enableCurrentSliderDoubleValue(step);
2173 }
2174 else if (mode == 2)
2175 {
2177 }
2178 else
2179 {
2181 }
2183 }
2184}
2185
2187{
2188 for (int i = 0; i<m_layout->count(); i++)
2189 {
2190 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2191 if (mode == 0)
2192 {
2194 }
2195 else if(mode == 1)
2196 {
2197 joint->enableTorqueSliderDoubleValue(step);
2198 }
2199 else if(mode == 2)
2200 {
2202 }
2203 else
2204 {
2206 }
2208 }
2209}
2210
2212{
2213 return m_modesList;
2214}
2215
2217{
2218 bool ret = m_ictrlmode->getControlModes(m_controlModes);
2219
2220
2221 if(ret==false){
2222 LOG_ERROR("ictrl->getControlMode failed on %s\n", getPartName().c_str());
2223 }
2224
2225 m_modesList.resize(m_layout->count());
2226 for (int k = 0; k < m_layout->count(); k++){
2227 switch (m_controlModes[k])
2228 {
2230 m_modesList[k] = JointItem::Idle;
2231 break;
2233 m_modesList[k] = JointItem::Position;
2234 break;
2236 m_modesList[k] = JointItem::PositionDirect;
2237 break;
2239 m_modesList[k] = JointItem::Mixed;
2240 break;
2242 m_modesList[k] = JointItem::Velocity;
2243 break;
2245 m_modesList[k] = JointItem::VelocityDirect;
2246 break;
2248 m_modesList[k] = JointItem::Torque;
2249 break;
2251 m_modesList[k] = JointItem::Pwm;
2252 break;
2254 m_modesList[k] = JointItem::Current;
2255 break;
2257 m_modesList[k] = JointItem::HwFault;
2258 break;
2260 m_modesList[k] = JointItem::Calibrating;
2261 break;
2263 m_modesList[k] = JointItem::CalibDone;
2264 break;
2266 m_modesList[k] = JointItem::NotConfigured;
2267 break;
2269 m_modesList[k] = JointItem::Configured;
2270 break;
2271 default:
2273 m_modesList[k] = JointItem::Unknown;
2274 break;
2275 }
2276 }
2277}
2278
2280{
2281 bool ret = false;
2282 size_t number_of_joints=0;
2283 m_iPos->getAxes(number_of_joints);
2284 if (m_slow_k >= number_of_joints - 1) {
2285 m_slow_k = 0;
2286 } else {
2287 m_slow_k++;
2288 }
2289
2290 if (number_of_joints == 0)
2291 {
2292 LOG_ERROR("Lost connection with the robot. You should save and restart.\n" );
2294
2295 for (int i = 0; i<m_layout->count(); i++){
2296 auto* joint = (JointItem*)m_layout->itemAt(i)->widget();
2298 }
2299 return false;
2300 }
2301
2302 // *** update measured encoders, velocity, torques ***
2303 bool b = true;
2304 if (true)
2305 {
2306 b = m_iencs->getEncoders(m_positions.data());
2307 if (!b) { yWarning("Unable to update encoders"); return false; }
2308 }
2309 if (true)
2310 {
2311 b = m_iTrq->getTorques(m_torques.data());
2312 if (!b) { yWarning("Unable to update torques"); }
2313 }
2314 if (this->m_part_currentVisible)
2315 {
2316 b = m_iCur->getCurrents(m_currents.data());
2317 if (!b) { yWarning("Unable to update currents"); }
2318 }
2319 if (this->m_part_speedVisible)
2320 {
2321 b = m_iencs->getEncoderSpeeds(m_speeds.data());
2322 if (!b) { yWarning("Unable to update speeds"); }
2323 }
2324 if (this->m_part_motorPositionVisible)
2325 {
2326 b = m_iMot->getMotorEncoders(m_motorPositions.data());
2327 if (!b) { yWarning("Unable to update motorPositions"); }
2328 }
2329 if (this->m_part_dutyVisible)
2330 {
2331 b = m_iPWM->getDutyCycles(m_dutyCycles.data());
2332 if (!b) { yWarning("Unable to update dutyCycles"); }
2333 }
2334
2335 // *** update checkMotionDone, refTorque, refTrajectorySpeed, refSpeed ***
2336 // (only one at a time in order to save bandwidth)
2337 bool ret_motdone = true;
2338 bool ret_refTrq = true;
2339 bool ret_refPosSpeed = true;
2340 bool ret_refVel = true;
2341 bool ret_refAcc = true;
2342 bool ret_refPos = true;
2343 bool ret_jntbrk = true;
2344
2345 //if the interface is not available, the ret_xxx is true, and no error is displayed.
2346 //if the interface is available, the ret_xxx is overwritten, and an error is displayed if the method was not successful
2347 if (m_iPos)
2348 {
2349 bool boolval = true;
2350 ret_motdone = m_iPos->checkMotionDone(m_slow_k, boolval); // using k to save bandwidth
2351 m_done[m_slow_k] = boolval;
2352 }
2353 if (m_ijointbrake)
2354 {
2355 bool boolval = true;
2356 ret_jntbrk = m_ijointbrake->isJointBraked(m_slow_k, boolval);
2357 m_braked[m_slow_k] = boolval;
2358 }
2359 if (m_iTrq)
2360 {
2361 ret_refTrq = m_iTrq->getRefTorque(m_slow_k, &m_refTorques[m_slow_k]); // using k to save bandwidth
2362 }
2363 if (m_iPos)
2364 {
2365 ret_refPosSpeed = m_iPos->getTrajSpeed(m_slow_k, &m_refTrajectorySpeeds[m_slow_k]); // using k to save bandwidth
2366 }
2367 if (m_iVel)
2368 {
2369 ret_refVel = m_iVel->getTargetVelocity(m_slow_k, &m_refVelocitySpeeds[m_slow_k]); // this interface is missing!
2370 }
2371 if (m_iVel)
2372 {
2373 ret_refAcc = m_iVel->getTrajAcceleration(m_slow_k, &m_refVelocityAccelerations[m_slow_k]); // this interface is missing!
2374 }
2375 if (m_iPos)
2376 {
2377 ret_refPos = m_iPos->getTargetPosition(m_slow_k, &m_refTrajectoryPositions[m_slow_k]);
2378 }
2379
2380 //Check return values of methods
2381 if (!ret_refPos)
2382 {
2383 yErrorThrottle(60.0) << "Missing Implementation of getTargetPosition()";
2384 }
2385 if (!ret_refVel)
2386 {
2387 yErrorThrottle(60.0) << "Missing Implementation of getTargetVelocity()";
2388 }
2389 if (!ret_refPosSpeed)
2390 {
2391 yErrorThrottle(60.0) << "Missing Implementation of getTrajSpeed()";
2392 }
2393 if (!ret_refTrq)
2394 {
2395 yErrorThrottle(60.0) << "Missing Implementation of getRefTorque()";
2396 }
2397 if (!ret_motdone)
2398 {
2399 yErrorThrottle(60.0) << "Missing Implementation of checkMotionDone()";
2400 }
2401 if (!ret_jntbrk)
2402 {
2403 yErrorThrottle(60.0) << "Missing Implementation of isJointBraked()";
2404 }
2405
2406 // *** update the widget every cycle ***
2407 for (size_t jk = 0; jk < number_of_joints; jk++)
2408 {
2409 auto* joint = (JointItem*)m_layout->itemAt(jk)->widget();
2410 if (true) { joint->setPosition(m_positions[jk]); }
2411 else {}
2412 if (true) { joint->setTorque(m_torques[jk]); }
2413 else {}
2414 if (true) { joint->setSpeed(m_speeds[jk]); }
2415 else {}
2416 if (true) { joint->setCurrent(m_currents[jk]); }
2417 else {}
2418 if (true) { joint->setMotorPosition(m_motorPositions[jk]); }
2419 else {}
2420 if (true) { joint->setDutyCycles(m_dutyCycles[jk]); }
2421 else {}
2422 }
2423
2424 // *** update the widget NOT every cycle ***
2425 {
2426 auto* joint_slow_k = (JointItem*)m_layout->itemAt(m_slow_k)->widget();
2427 if (ret_refTrq) { joint_slow_k->setRefTorque(m_refTorques[m_slow_k]); }
2428 else {}
2429 if (ret_refPosSpeed) { joint_slow_k->setPosTrajectory_ReferenceSpeed(m_refTrajectorySpeeds[m_slow_k]); }
2430 else {}
2431 if (ret_refPos) { joint_slow_k->setPosTrajectory_ReferencePosition(m_refTrajectoryPositions[m_slow_k]); }
2432 else {}
2433 if (ret_refVel) { joint_slow_k->setVelTrajectory_ReferenceSpeed(m_refVelocitySpeeds[m_slow_k]); }
2434 else {}
2435 if (ret_refAcc) { joint_slow_k->setVelTrajectory_ReferenceAcceleration(m_refVelocityAccelerations[m_slow_k]); }
2436 else {}
2437 if (ret_motdone) { joint_slow_k->updateMotionDone(m_done[m_slow_k]); }
2438 else {}
2439 if (ret_jntbrk) { joint_slow_k->updateBraked(m_braked[m_slow_k]); }
2440 else {}
2441 }
2442
2443
2444 // *** update the controlMode, interactionMode ***
2445 // this is already done by updateControlMode() (because it also needs to update the tree, not only the single joint widget)
2446 // ret=ctrlmode2->getControlModes(controlModes);
2447 // if(ret==false){
2448 // LOG_ERROR("ictrl->getControlMode failed\n" );
2449 // }
2450 ret = m_iinteract->getInteractionModes(m_interactionModes);
2451 if(ret==false){
2452 LOG_ERROR("iint->getInteractionlMode failed\n" );
2453 }
2454
2455 //optional
2456 if (m_ijointfault)
2457 {
2458 for (size_t k = 0; k < number_of_joints; k++)
2459 {
2461 {
2462 int fault;
2463 std::string message;
2464 if (m_ijointfault->getLastJointFault(k,fault,message))
2465 {
2466 auto* joint_slow_k = (JointItem*)m_layout->itemAt(m_slow_k)->widget();
2467 joint_slow_k->updateJointFault(fault, message);
2468 }
2469 else
2470 {
2471 LOG_ERROR("Unsuccessful call to getLastJointFault()\n");
2472 }
2473 }
2474 }
2475 }
2476
2477
2478 for (size_t k = 0; k < number_of_joints; k++)
2479 {
2480 auto* joint = (JointItem*)m_layout->itemAt(k)->widget();
2481 switch (m_controlModes[k])
2482 {
2485 break;
2488 break;
2491 break;
2494 break;
2497 break;
2500 break;
2503 break;
2505 {
2507 double ref_current = 0;
2508 m_iCur->getRefCurrent(k, &ref_current);
2509 joint->setRefCurrent(ref_current);
2510 break;
2511 }
2513 {
2515 double ref_duty = 0;
2516 m_iPWM->getRefDutyCycle(k, &ref_duty);
2517 joint->setRefPWM(ref_duty);
2518 break;
2519 }
2522 break;
2525 break;
2528 break;
2531 break;
2534 break;
2535 default:
2538 break;
2539 }
2540 switch (m_interactionModes[k])
2541 {
2544 break;
2547 break;
2548 default:
2550 //joint->setJointInteraction(JointItem::Stiff); TODO
2551 break;
2552 }
2553 }
2554 return true;
2555}
FeatureMode mode
#define yInfo(...)
Definition Log.h:319
#define yError(...)
Definition Log.h:361
#define yDebug(...)
Definition Log.h:275
#define yErrorThrottle(period,...)
Definition Log.h:364
#define yWarning(...)
Definition Log.h:340
int SIGNAL(int pid, int signum)
Original license follows:
Definition flowlayout.h:59
QLayoutItem * itemAt(int index) const override
int count() const override
void setMotorPosition(double meas)
void viewPositionTargetValue(bool)
void setDutyVisible(bool)
void sequenceActivated()
void disableAccelerationSliderDouble()
void enableControlPositionDirect(bool control)
void enableVelocitySliderDoubleAuto()
void setCurrentsVisible(bool)
void enableControlMixed(bool control)
JointState getJointState()
void resetTarget()
Definition jointitem.cpp:23
void enablePositionSliderDoubleValue(double value)
void enableAccelerationSliderDoubleValue(double value)
void enableCurrentSliderDoubleValue(double value)
void viewPositionTargetBox(bool)
void setJointState(JointState)
void enableControlPWM(bool control)
void disablePositionSliderDouble()
void setNumberOfAccelerationSliderDecimals(size_t num)
void disableCurrentSliderDouble()
void enableTorqueSliderDoubleValue(double value)
void setPosition(double val)
void enableControlVelocityDirect(bool control)
QString getJointName()
void setRefCurrent(double ref)
void setNumberOfCurrentSliderDecimals(size_t num)
double getTrajectoryVelocityValue()
void setSpeed(double val)
void enableCurrentSliderDoubleAuto()
int getJointIndex()
void setJointInteraction(JointInteraction interaction)
void setMotorPositionVisible(bool)
void enableControlVelocity(bool control)
void setNumberOfTorqueSliderDecimals(size_t num)
void setDutyCycles(double duty)
void setCurrent(double meas)
void disableVelocitySliderDouble()
void setRefPWM(double ref)
void enableControlCurrent(bool control)
void sequenceStopped()
void enableTorqueSliderDoubleAuto()
void setSpeedVisible(bool)
void setNumberOfPositionSliderDecimals(size_t num)
@ PositionDirect
Definition jointitem.h:33
@ Disconnected
Definition jointitem.h:34
@ VelocityDirect
Definition jointitem.h:33
@ Calibrating
Definition jointitem.h:34
@ NotConfigured
Definition jointitem.h:34
void enableVelocitySliderDoubleValue(double value)
void enablePositionSliderDoubleAuto()
void disableTorqueSliderDouble()
void enableAccelerationSliderDoubleAuto()
void enableControlTorque(bool control)
void setTorque(double meas)
void setNumberOfVelocitySliderDecimals(size_t num)
void initInterfaces()
Definition partitem.cpp:385
bool openPolyDrivers()
Definition partitem.cpp:364
bool checkAndGo()
void cycleTimeSequence()
bool homePart()
void onEnableControlVelocityDirect(bool control)
bool checkAndRunAllSeq()
void onViewDutyCycles(bool)
void resizeWidget(int w)
bool homeToCustomPosition(const yarp::os::Bottle &positionElement)
void setCurrentIndex(int)
void runTimeSequence()
void idlePart()
JointItem * getJointWidget(int jointIndex)
int getNumberOfJoints()
void onEnableControlCurrent(bool control)
bool getInterfaceError()
Definition partitem.cpp:517
void onSetVelSliderOptionPI(int mode, double step, int numOfDec)
void onViewMotorPositions(bool)
void onEnableControlPWM(bool control)
void runPart()
void onEnableControlVelocity(bool control)
PartItem(std::string robotName, int partId, std::string partName, ResourceFinder &_finder, bool debug_param_enabled, bool speedview_param_enabled, bool enable_calib_all, std::string rcb_protocol, QWidget *parent=0)
Definition partitem.cpp:25
QString getJointName(int joint)
std::string getPartName()
Definition partitem.cpp:522
void onSetCurSliderOptionPI(int mode, double step, int numOfDec)
void onViewPositionTargetValue(bool)
void onViewPositionTargetBox(bool)
bool homeJoint(int joint)
void onViewCurrentValues(bool)
void runSequence()
void onSetTrqSliderOptionPI(int mode, double step, int numOfDec)
void sendPartJointsValues(int, QList< double >, QList< double >)
bool checkAndCycleAllSeq()
void closeSequenceWindow()
bool updatePart()
void onViewSpeedValues(bool)
void openSequenceWindow()
void updateControlMode()
void stopSequence()
void stoppedSequence()
void onEnableControlPositionDirect(bool control)
void saveSequence(QString global_filename)
void resizeEvent(QResizeEvent *event) override
void changeEvent(QEvent *event) override
void onEnableControlTorque(bool control)
void loadSequence()
void sequenceStopped()
const QVector< JointItem::JointState > & getPartModes()
void cycleSequence()
void onEnableControlMixed(bool control)
void onSetPosSliderOptionPI(int mode, double step, int numOfDec)
void sequenceActivated()
bool checkAndRunTimeAllSeq()
bool checkAndCycleTimeAllSeq()
void onSetAccSliderOptionPI(int mode, double step, int numOfDec)
void calibratePart()
bool openInterfaces()
Definition partitem.cpp:409
void initTorquePID(const std::vector< PidWithExtraInfo > &myPids)
Definition piddlg.cpp:406
void initVelocityDirectPID(const std::vector< PidWithExtraInfo > &myPids)
Definition piddlg.cpp:332
void initTorqueOffset(double curForceVal, double minForce, double maxForce)
Definition piddlg.cpp:573
void initMixedPID(const std::vector< PidWithExtraInfo > &myPids)
Definition piddlg.cpp:369
void initCurrentPID(const std::vector< PidWithExtraInfo > &myPids)
Definition piddlg.cpp:591
void initStiffness(double curStiffVal, double minStiff, double maxStiff, double curDampVal, double minDamp, double maxDamp)
Definition piddlg.cpp:559
void initRemoteVariables(IRemoteVariables *iVar)
Definition piddlg.cpp:493
void initPositionDirectPID(const std::vector< PidWithExtraInfo > &myPids)
Definition piddlg.cpp:258
void initMotorParams(MotorTorqueParameters TorqueParam)
Definition piddlg.cpp:443
void initPWM(double pwmVal, double pwm)
Definition piddlg.cpp:583
void initVelocityPID(const std::vector< PidWithExtraInfo > &myPids)
Definition piddlg.cpp:295
void initPositionPID(const std::vector< PidWithExtraInfo > &myPids)
Definition piddlg.cpp:221
int getSequenceNumber()
double getTiming()
QList< double > getPositions()
QList< double > getSpeeds()
void save(QString global_filename)
void loadSequence(QList< SequenceItem >)
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getJointType(int axis, yarp::dev::JointTypeEnum &type)
Definition IAxisInfo.h:64
virtual yarp::dev::ReturnValue getAxisName(int axis, std::string &name)=0
virtual yarp::dev::ReturnValue getVelLimits(int axis, double *min, double *max)=0
Get the software speed limits for a particular axis.
virtual yarp::dev::ReturnValue getPosLimits(int axis, double *min, double *max)=0
Get the software limits for a particular axis.
virtual yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum &mode)=0
Get the current control mode.
virtual yarp::dev::ReturnValue getControlModes(std::vector< yarp::dev::ControlModeEnum > &mode)=0
Get the current control mode (multiple joints).
virtual yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode)=0
Set the current control mode.
virtual yarp::dev::ReturnValue getAvailableControlModes(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail)=0
Return a list of available control mode for the given joint.
virtual yarp::dev::ReturnValue setRefCurrent(int m, double curr)=0
Set the reference value of the current for a single motor.
virtual yarp::dev::ReturnValue getCurrentRange(int m, double *min, double *max)=0
Get the full scale of the current measurement for a given motor (e.g.
virtual yarp::dev::ReturnValue getRefCurrent(int m, double *curr)=0
Get the reference value of the current for a single motor.
virtual yarp::dev::ReturnValue getCurrents(double *currs)=0
Get the instantaneous current measurement for all motors.
virtual yarp::dev::ReturnValue getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual yarp::dev::ReturnValue getEncoders(double *encs)=0
Read the position of all axes.
virtual yarp::dev::ReturnValue getEncoder(int j, double *v)=0
Read the value of an encoder.
virtual yarp::dev::ReturnValue getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)=0
Get the current impedance limits for a specific joint.
virtual yarp::dev::ReturnValue getImpedance(int j, double *stiffness, double *damping)=0
Get current impedance gains (stiffness,damping,offset) for a specific joint.
virtual yarp::dev::ReturnValue getImpedanceOffset(int j, double *offset)=0
Get current force Offset for a specific joint.
virtual yarp::dev::ReturnValue setImpedanceOffset(int j, double offset)=0
Set current force Offset for a specific joint.
virtual yarp::dev::ReturnValue setImpedance(int j, double stiffness, double damping)=0
Set current impedance gains (stiffness,damping) for a specific joint.
virtual yarp::dev::ReturnValue setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode)=0
Set the interaction mode of the robot, values can be stiff or compliant.
virtual yarp::dev::ReturnValue getInteractionModes(const std::vector< int > &joints, std::vector< yarp::dev::InteractionModeEnum > &modes)=0
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
virtual yarp::dev::ReturnValue isJointBraked(int j, bool &braked) const =0
Check is the joint brake is currently active.
virtual yarp::dev::ReturnValue getLastJointFault(int j, int &fault, std::string &message)=0
virtual yarp::dev::ReturnValue getMotorEncoders(double *encs)=0
Read the position of all motor encoders.
virtual yarp::dev::ReturnValue getDutyCycle(int m, double *val)=0
Gets the current dutycycle of the output of the amplifier (i.e.
virtual yarp::dev::ReturnValue setRefDutyCycle(int m, double ref)=0
Sets the reference dutycycle to a single motor.
virtual yarp::dev::ReturnValue getDutyCycles(double *vals)=0
Gets the current dutycycle of the output of the amplifier (i.e.
virtual yarp::dev::ReturnValue getRefDutyCycle(int m, double *ref)=0
Gets the last reference sent using the setRefDutyCycle function.
virtual yarp::dev::ReturnValue getPidExtraInfo(const PidControlTypeEnum &pidtype, int j, yarp::dev::PidExtraInfo &info)=0
Get PID extra info and measurement units for a specific joint.
virtual yarp::dev::ReturnValue setPid(const PidControlTypeEnum &pidtype, int j, const Pid &pid)=0
Set new PID value for a joint axis.
virtual yarp::dev::ReturnValue getPid(const PidControlTypeEnum &pidtype, int j, Pid *pid)=0
Get current PID value for a specific joint.
virtual yarp::dev::ReturnValue getTargetPosition(const int joint, double *ref)=0
Get the last position reference for the specified axis.
virtual yarp::dev::ReturnValue setTrajSpeed(int j, double sp)=0
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
virtual yarp::dev::ReturnValue setTrajSpeeds(const double *spds)=0
Set reference speed on all joints.
virtual yarp::dev::ReturnValue positionMove(int j, double ref)=0
Set new reference point for a single axis.
virtual yarp::dev::ReturnValue checkMotionDone(int j, bool &flag)=0
Check if the current trajectory is terminated.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue getTrajSpeed(int j, double *ref)=0
Get reference speed for a joint.
virtual yarp::dev::ReturnValue setPosition(int j, double ref)=0
Set new position for a single axis.
virtual yarp::dev::ReturnValue calibrateWholePart()=0
calibrateWholePart: call the procedure for calibrating the whole device
virtual yarp::dev::ReturnValue homingWholePart()=0
homingWholePart: call the homing procedure for a the whole part/device
virtual yarp::dev::ReturnValue calibrateSingleJoint(int j)=0
calibrateSingleJoint: call the calibration procedure for the single joint
virtual yarp::dev::ReturnValue isCalibratorDevicePresent(bool *isCalib)
isCalibratorDevicePresent: check if a calibrator device has been set
virtual yarp::dev::ReturnValue homingSingleJoint(int j)=0
homingSingleJoint: call the homing procedure for a single joint
virtual yarp::dev::ReturnValue getRemoteVariablesList(yarp::os::Bottle *listOfKeys)=0
virtual yarp::dev::ReturnValue getRemoteVariable(std::string key, yarp::os::Bottle &val)=0
virtual yarp::dev::ReturnValue setRemoteVariable(std::string key, const yarp::os::Bottle &val)=0
virtual yarp::dev::ReturnValue getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params)=0
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
virtual yarp::dev::ReturnValue getTorqueRange(int j, double *min, double *max)=0
Get the full scale of the torque sensor of a given joint.
virtual yarp::dev::ReturnValue setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params)=0
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
virtual yarp::dev::ReturnValue getRefTorque(int j, double *t)=0
Get the reference value of the torque for a given joint.
virtual yarp::dev::ReturnValue setRefTorque(int j, double t)=0
Set the reference value of the torque for a given joint.
virtual yarp::dev::ReturnValue getTorques(double *t)=0
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
virtual yarp::dev::ReturnValue getTargetVelocity(const int joint, double *vel)=0
Get the last reference speed set by velocityMove for single joint.
virtual yarp::dev::ReturnValue setTrajAcceleration(int j, double acc)=0
Set reference acceleration for a joint.
virtual yarp::dev::ReturnValue velocityMove(int j, double sp)=0
Start motion at a given speed, single joint.
virtual yarp::dev::ReturnValue getTrajAcceleration(int j, double *acc)=0
Get reference acceleration for a joint.
virtual yarp::dev::ReturnValue setRefVelocity(int jnt, double vel)=0
Set the velocity of single joint.
Contains the parameters for a PID.
A container for a device driver.
Definition PolyDriver.h:23
bool close() override
Close the DeviceDriver.
bool isValid() const
Check if device is valid.
bool open(const std::string &txt)
Construct and configure a device by its common name.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
size_type size() const
Gets the number of elements in the bottle.
Definition Bottle.cpp:252
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:247
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition Bottle.cpp:212
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Represents how to reach a part of a YARP network.
Definition Contact.h:33
bool write(const PortWriter &writer, const PortWriter *callback=nullptr) const override
Write an object to the port.
Definition Port.cpp:395
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
Definition Port.cpp:506
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
Definition Property.cpp:980
Helper class for finding config files and other external resources.
bool isNull() const override
Checks if the object is invalid.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:49
static void delaySystem(double seconds)
virtual bool isString() const
Checks if value is a string.
Definition Value.cpp:156
virtual std::string asString() const
Get string value.
Definition Value.cpp:240
bool speedview_param_enabled
Definition main.cpp:35
bool debug_param_enabled
Definition main.cpp:34
bool enable_calib_all
Definition main.cpp:36
#define LOG_INFO(...)
Definition log.h:25
#define LOG_ERROR(...)
Definition log.h:19
PidControlTypeEnum
Definition PidEnums.h:15
#define MAX_WIDTH_JOINT
Definition partitem.h:37