16#include <QResizeEvent>
18#include <QXmlStreamWriter>
19#include <QXmlStreamAttribute>
32 m_sequenceWindow(nullptr),
34 m_mixedEnabled(
false),
35 m_positionDirectEnabled(
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),
56 if (partName.at(0) ==
'/') {
59 m_robotPartPort = std::string(
"/") +
robotName +std::string(
"/") + partName;
60 m_partName = partName;
78 yDebug(
"ADDRESS is: %s \n",
adr.toString().c_str());
91 m_interfaceError =
false;
95 m_partOptions.
put(
"device",
"controlBoard_nwc_yarp");
96 m_partOptions.
put(
"remote", m_robotPartPort);
103 if (m_interfaceError ==
true)
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()));
113 yDebug(
"Setting a valid finder \n");
122 if (m_interfaceError ==
false)
132 m_refTrajectorySpeeds[i] = std::nan(
"");
136 m_refTrajectoryPositions[i] = std::nan(
"");
140 m_refTorques[i] = std::nan(
"");
144 m_refVelocitySpeeds[i] = std::nan(
"");
148 m_refVelocityAccelerations[i] = std::nan(
"");
152 m_torques[i] = std::nan(
"");
156 m_positions[i] = std::nan(
"");
160 m_speeds[i] = std::nan(
"");
164 m_currents[i] = std::nan(
"");
168 m_motorPositions[i] = std::nan(
"");
172 m_dutyCycles[i] = std::nan(
"");
186 yError(
"%s iencs->getEncoders() failed, retrying...\n", partName.c_str());
191 yInfo(
"%s iencs->getEncoders() ok!\n", partName.c_str());
210 yError() <<
"Error while getting position available control modes, part " << partName <<
" joint " <<
k;
214 yError() <<
"Error while getting position limits, part " << partName <<
" joint " <<
k;
218 yError() <<
"Error while getting velocity limits, part " << partName <<
" joint " <<
k;
222 yError() <<
"Error while getting current range, part " << partName <<
" joint " <<
k;
225 QSettings settings(
"YARP",
"yarpmotorgui");
226 double max_slider_vel = settings.value(
"velocity_slider_limit", 100.0).toDouble();
237 m_iPid->
getPid(PidControlTypeEnum::VOCAB_PIDTYPE_POSITION,
k, &
myPid);
241 joint->setPWMRange(-100.0, 100.0);
243 m_layout->addWidget(joint);
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);
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();
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();
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)));
306 m_cycleTimer.setSingleShot(
true);
307 m_cycleTimer.setTimerType(Qt::PreciseTimer);
308 connect(&m_cycleTimer,
SIGNAL(timeout()),
this,
SLOT(onCycleTimerTimeout()), Qt::QueuedConnection);
310 m_cycleTimeTimer.setSingleShot(
true);
311 m_cycleTimeTimer.setTimerType(Qt::PreciseTimer);
312 connect(&m_cycleTimeTimer,
SIGNAL(timeout()),
this,
SLOT(onCycleTimeTimerTimeout()), Qt::QueuedConnection);
315 m_runTimeTimer.setSingleShot(
true);
316 m_runTimeTimer.setTimerType(Qt::PreciseTimer);
317 connect(&m_runTimeTimer,
SIGNAL(timeout()),
this,
SLOT(onRunTimerTimeout()), Qt::QueuedConnection);
319 m_runTimer.setSingleShot(
true);
320 m_runTimer.setTimerType(Qt::PreciseTimer);
321 connect(&m_runTimer,
SIGNAL(timeout()),
this,
SLOT(onRunTimeout()), Qt::QueuedConnection);
326 disconnect(&m_runTimer,
SIGNAL(timeout()),
this,
SLOT(onRunTimeout()));
329 disconnect(&m_runTimeTimer,
SIGNAL(timeout()),
this,
SLOT(onRunTimerTimeout()));
330 m_runTimeTimer.stop();
332 disconnect(&m_cycleTimer,
SIGNAL(timeout()),
this,
SLOT(onCycleTimerTimeout()));
335 disconnect(&m_cycleTimeTimer,
SIGNAL(timeout()),
this,
SLOT(onCycleTimeTimerTimeout()));
336 m_cycleTimeTimer.stop();
338 if (m_sequenceWindow){
339 m_sequenceWindow->hide();
340 delete m_sequenceWindow;
343 for (
int i = 0; i<m_layout->
count(); i++){
366 m_partsdd->
open(m_partOptions);
371 #ifdef DEBUG_INTERFACE
376 yError(
"Problems opening the debug client!");
387 yDebug(
"Initializing interfaces...");
403 m_ictrlmode =
nullptr;
404 m_iinteract =
nullptr;
405 m_iremCalib =
nullptr;
406 m_ijointfault =
nullptr;
411 yDebug(
"Opening interfaces...");
415 ok = m_partsdd->
view(m_iPid);
417 yError(
"...iPid was not ok...");
419 ok &= m_partsdd->
view(m_iAmp);
421 yError(
"...iAmp was not ok...");
423 ok &= m_partsdd->
view(m_iPos);
425 yError(
"...iPos was not ok...");
427 ok &= m_partsdd->
view(m_iPosDir);
429 yError(
"...posDirect was not ok...");
431 ok &= m_partsdd->
view(m_iVelDir);
433 yError(
"...velDirect was not ok...");
435 ok &= m_partsdd->
view(m_iVel);
437 yError(
"...iVel was not ok...");
439 ok &= m_partsdd->
view(m_iLim);
441 yError(
"...iLim was not ok...");
443 ok &= m_partsdd->
view(m_iencs);
445 yError(
"...enc was not ok...");
447 ok &= m_partsdd->
view(m_ical);
449 yError(
"...cal was not ok...");
451 ok &= m_partsdd->
view(m_iTrq);
453 yError(
"...trq was not ok...");
455 ok = m_partsdd->
view(m_iPWM);
457 yError(
"...opl was not ok...");
459 ok &= m_partsdd->
view(m_iImp);
461 yError(
"...imp was not ok...");
463 ok &= m_partsdd->
view(m_ictrlmode);
465 yError(
"...ctrlmode2 was not ok...");
467 ok &= m_partsdd->
view(m_iinteract);
469 yError(
"...iinteract was not ok...");
473 if (!m_partsdd->
view(m_ijointfault))
475 yWarning(
"...m_iJointFault was not ok...");
477 if (!m_partsdd->
view(m_ijointbrake))
479 yWarning(
"...m_iJointBrake was not ok...");
481 if (!m_partsdd->
view(m_iVar))
485 if (!m_partsdd->
view(m_iMot))
489 if (!m_partsdd->
view(m_iremCalib))
491 yWarning(
"...remCalib was not ok...");
493 if (!m_partsdd->
view(m_iinfo))
495 yWarning(
"...axisInfo was not ok...");
497 if (!m_partsdd->
view(m_iCur))
503 yError(
"Error while acquiring interfaces!");
504 QMessageBox::critical(
nullptr,
"Problems acquiring interfaces.",
"Check if interface is running");
505 m_interfaceError =
true;
510 yError(
"Device driver was not valid!");
511 m_interfaceError =
true;
514 return !m_interfaceError;
519 return m_interfaceError;
527void PartItem::onSliderPWMCommand(
double pwmVal,
int index)
532void PartItem::onSliderCurrentCommand(
double currentVal,
int index)
537void PartItem::onSliderVelTrajectoryVelocityCommand(
double speedVal,
int index)
542void PartItem::onSliderVelTrajectoryAccelerationCommand(
double accVal,
int index)
547void PartItem::onSliderVelocityDirectCommand(
double speedVal,
int index)
552void PartItem::onSliderTorqueCommand(
double torqueVal,
int index)
557void PartItem::onSliderPosTrajectoryVelocityCommand(
double trajspeedVal,
int index)
563void PartItem::onSliderDirectPositionCommand(
double dirpos,
int index)
573 yWarning(
"Joint not in position direct mode so cannot send references");
577void PartItem::onDumpAllRemoteVariables()
579 if (m_iVar ==
nullptr)
584 QString fileName = QFileDialog::getSaveFileName(
this,
QString(
"Save Dump for Remote Variables as:"), QDir::homePath()+
QString(
"/RemoteVariablesDump.txt"));
615void PartItem::onSliderPosTrajectoryPositionCommand(
double posVal,
int index)
626 yWarning(
"Joint not in position mode so cannot send references");
630void PartItem::onSliderMixedPositionCommand(
double posVal,
int index)
641 LOG_ERROR(
"Joint not in mixed mode so cannot send references");
645void PartItem::onSliderMixedVelocityCommand(
double vel,
int index)
656 LOG_ERROR(
"Joint not in mixed mode so cannot send references");
665 yInfo(
"interaction mode of joint %d set to COMPLIANT", jointIndex);
669 yInfo(
"interaction mode of joint %d set to STIFF", jointIndex);
678void PartItem::onSendPWM(
int jointIndex,
double pwmVal)
690 if (m_currentPidDlg){
747 if (m_currentPidDlg){
757 onRefreshPids(jointIndex);
760void PartItem::onRefreshPids(
int jointIndex)
877void PartItem::onSendSingleRemoteVariable(std::string key,
yarp::os::Bottle val)
883void PartItem::onUpdateAllRemoteVariables()
885 if (m_currentPidDlg){
890void PartItem::onCalibClicked(
JointItem *joint)
894 QMessageBox::critical(
this,
"Operation not supported",
QString(
"The IRemoteCalibrator interface was not found on this application"));
898 if(QMessageBox::question(
this,
"Question",
"Do you really want to recalibrate the joint?") != QMessageBox::Yes){
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"));
909 QMessageBox::critical(
this,
"Calibration failed",
QString(
"The remote calibrator reported that something went wrong during the calibration procedure"));
915void PartItem::onPidClicked(
JointItem *joint)
920 m_currentPidDlg =
new PidDlg(m_partName.c_str(), jointIndex, jointName, state);
923 connect(m_currentPidDlg,
SIGNAL(updateAllRemoteVariables()),
this,
SLOT(onUpdateAllRemoteVariables()));
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()));
931 this->onRefreshPids(jointIndex);
933 m_currentPidDlg->exec();
935 delete m_currentPidDlg;
936 m_currentPidDlg =
nullptr;
939void PartItem::onRunClicked(
JointItem *joint)
950void PartItem::onIdleClicked(
JointItem *joint)
956void PartItem::onHomeClicked(
JointItem *joint)
970 yInfo(
"joint: %d in IDLE mode", jointIndex);
974 yError(
"ERROR: cannot do!");
979 yInfo(
"joint: %d in POSITION mode", jointIndex);
984 yError(
"ERROR: cannot do!");
990 yInfo(
"joint: %d in POSITION DIRECT mode", jointIndex);
995 yError(
"ERROR: cannot do!");
1010 yInfo(
"joint: %d in MIXED mode", jointIndex);
1015 yError(
"ERROR: cannot do!");
1031 yInfo(
"joint: %d in VELOCITY DIRECT mode", jointIndex);
1037 yError(
"ERROR: cannot do!");
1043 yInfo(
"joint: %d in VELOCITY mode", jointIndex);
1047 yInfo() <<
"Changing reference acceleration of joint " << jointIndex <<
" to 100000";
1050 yError(
"ERROR: cannot do!");
1066 yInfo(
"joint: %d in TORQUE mode", jointIndex);
1070 yError(
"ERROR: cannot do!");
1085 yInfo(
"joint: %d in PWM mode", jointIndex);
1089 yError(
"ERROR: cannot do!");
1094 yInfo(
"joint: %d in CURRENT mode", jointIndex);
1099 yError(
"ERROR: cannot do!");
1110 int count = m_layout->
count();
1125 for(
int i=0;i<count;i++){
1137 return m_layout->
count();
1168 if(event->type() == QEvent::WindowStateChange ){
1169 qDebug() <<
"State Changed " << width();
1170 int count = m_layout->
count();
1184 for(
int i=0;i<count;i++){
1199 QMessageBox::critical(
this,
"Operation not supported",
QString(
"The IRemoteCalibrator interface was not found on this application"));
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()));
1211 QMessageBox::critical(
this,
"Calibration failed",
QString(
"The remote calibrator reported that something went wrong during the calibration procedure"));
1220 QMessageBox::critical(
this,
"Operation not supported",
QString(
"The IRemoteCalibrator interface was not found on this application"));
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()));
1236 QMessageBox::critical(
this,
"Operation failed",
QString(
"The remote calibrator reported that something went wrong during the calibration procedure"));
1247 QMessageBox::critical(
this,
"Operation not supported",
QString(
"The IRemoteCalibrator interface was not found on this application"));
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()));
1263 QMessageBox::critical(
this,
"Operation failed",
QString(
"The remote calibrator reported that something went wrong during the calibration procedure"));
1277 QMessageBox::critical(
this,
"Operation failed",
QString(
"No custom position supplied in configuration file for part ") +
QString(m_partName.c_str()));
1293 double velocity =
ytmp.get(jointIndex + 1).asFloat64();
1300 QMessageBox::critical(
this,
"Error",
QString(
"Check the number of entries in the group %1").arg(m_robotPartPort.c_str()));
1319 if (!m_sequenceWindow){
1330 if (!m_sequenceWindow){
1341 if (!m_sequenceWindow){
1350 if (!m_sequenceWindow){
1382 if (m_sequenceWindow)
1384 m_sequenceWindow->close();
1390 if (!m_sequenceWindow){
1392 connect(m_sequenceWindow,
SIGNAL(itemDoubleClicked(
int)),
this,
SLOT(onSequenceWindowDoubleClicked(
int)), Qt::DirectConnection);
1398 connect(m_sequenceWindow,
SIGNAL(openSequence()),
this,
SLOT(onOpenSequence()));
1417 if (!m_sequenceWindow->isVisible()){
1418 m_sequenceWindow->show();
1420 m_sequenceWindow->setFocus();
1421 m_sequenceWindow->raise();
1422 m_sequenceWindow->setWindowState(Qt::WindowActive);
1429 if (!m_sequenceWindow){
1437 m_cycleTimer.stop();
1439 m_runTimeTimer.stop();
1440 m_cycleTimeTimer.stop();
1444void PartItem::onStopSequence()
1449void PartItem::onOpenSequence()
1451 QString fileName = QFileDialog::getOpenFileName(m_sequenceWindow,
QString(
"Load Sequence for part %1 As").arg(m_partName.c_str()), QDir::homePath());
1454 if(!
fInfo.exists()){
1462 QMessageBox::critical(
this,
"Error Loading The Sequence",
1463 QString(
"Wrong format (check extensions) of the file associated with: ").arg(m_partName.c_str()));
1467 QFile file(fileName);
1468 if (!file.open(QFile::ReadOnly | QFile::Text)){
1471 QMessageBox::critical(
this,
"Error Loading The Sequence",
msg);
1482 while(!reader.atEnd()){
1485 if(reader.isStartElement()){
1486 if(reader.name().contains(
"Sequence_")){
1488 referencePart = attributes.value(
"ReferencePart").toString();
1491 if(reader.name() ==
"Position"){
1493 int index = attributes.value(
"Index").toInt();
1494 double timing = attributes.value(
"Timing").toDouble();
1495 item.setTiming(timing);
1496 item.setSequenceNumber(index);
1499 if(reader.name() ==
"JointPositions"){
1501 int count = attributes.value(
"Count").toInt();
1504 for(
int i=0; i<count;i++){
1506 if(reader.name() ==
"PosValue"){
1507 double pos = reader.readElementText().toDouble();
1508 item.addPositionValue(pos);
1515 if(reader.name() ==
"JointVelocities"){
1517 int count = attributes.value(
"Count").toInt();
1520 for(
int i=0; i<count;i++){
1522 if(reader.name() ==
"SpeedValue"){
1523 double speed = reader.readElementText().toDouble();
1524 item.addSpeedValue(speed);
1533 if(reader.isEndElement()){
1534 if(reader.name() ==
"Position"){
1543 if (reader.hasError())
1546 QMessageBox::critical(
this,
"Error Loading The Sequence",
msg);
1548 }
else if (file.error() != QFile::NoError) {
1550 QMessageBox::critical(
this,
"Error Loading The Sequence",
msg);
1554 if (m_sequenceWindow){
1564 fileName = QFileDialog::getSaveFileName(
this,
QString(
"Save Sequence for part %1 As").arg(m_partName.c_str()), QDir::homePath());
1567 if(fileName.isEmpty()){
1579 if(!file.open(QIODevice::WriteOnly)){
1584 writer.setAutoFormatting(
true);
1585 writer.writeStartDocument();
1587 writer.writeStartElement(
QString(
"Sequence_pos%1").arg(m_partName.c_str()));
1589 writer.writeAttribute(
"TotPositions",
QString(
"%1").arg(values.count()));
1590 writer.writeAttribute(
"ReferencePart", m_partName.c_str());
1592 for(
int i=0;i<values.count();i++){
1594 writer.writeStartElement(
"Position");
1598 writer.writeStartElement(
"JointPositions");
1600 for(
int j=0;j<
sequenceItem.getPositions().count(); j++){
1602 writer.writeTextElement(
"PosValue",s);
1604 writer.writeEndElement();
1606 writer.writeStartElement(
"JointVelocities");
1610 writer.writeTextElement(
"SpeedValue",s);
1612 writer.writeEndElement();
1613 writer.writeEndElement();
1615 writer.writeEndElement();
1616 writer.writeEndDocument();
1618 LOG_INFO(
"File saved and closed\n");
1658 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1662 m_cycleTimeValues.clear();
1663 for(
int i=0;i<values.count();i++){
1665 if(
it.getTiming() > 0){
1666 m_cycleTimeValues.append(
it);
1671 if (m_cycleTimeValues.count() > 0)
1673 vals = m_cycleTimeValues.takeFirst();
1674 m_cycleTimeValues.append(vals);
1676 fixedTimeMove(vals);
1677 m_cycleTimeTimer.start(vals.
getTiming() * 1000);
1682void PartItem::onCycleTimeTimerTimeout()
1684 if (m_cycleTimeValues.count() > 0)
1687 m_cycleTimeValues.append(vals);
1689 fixedTimeMove(vals);
1690 m_cycleTimeTimer.start(vals.
getTiming() * 1000);
1697 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1701 m_cycleValues.clear();
1702 for(
int i=0;i<values.count();i++){
1704 if(
it.getTiming() > 0){
1705 m_cycleValues.append(
it);
1710 if (m_cycleValues.count() > 0){
1711 vals = m_cycleValues.takeFirst();
1713 m_cycleValues.append(vals);
1728 m_cycleTimer.start(vals.
getTiming() * 1000);
1734void PartItem::onCycleTimerTimeout()
1736 if (m_cycleValues.count() > 0){
1739 m_cycleValues.append(vals);
1754 m_cycleTimer.start(vals.
getTiming() * 1000);
1761 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1765 m_runValues.clear();
1766 for(
int i=0;i<values.count();i++){
1768 if(
it.getTiming() > 0){
1769 m_runValues.append(
it);
1774 if (m_runValues.count() > 0){
1775 vals = m_runValues.takeFirst();
1777 m_runValues.append(vals);
1792 m_runTimer.start(vals.
getTiming() * 1000);
1797void PartItem::onRunTimeout()
1799 if (m_runValues.count() > 0){
1820 m_runTimer.start(vals.
getTiming() * 1000);
1830 if (m_cycleTimeTimer.isActive() || m_cycleTimer.isActive() || m_runTimeTimer.isActive() || m_runTimer.isActive()){
1834 m_runTimeValues.clear();
1836 for(
int i=0;i<values.count();i++){
1838 if(
it.getTiming() > 0){
1839 m_runTimeValues.append(
it);
1844 if (m_runTimeValues.count() > 0){
1845 vals = m_runTimeValues.takeFirst();
1848 fixedTimeMove(vals);
1849 m_runTimeTimer.start(vals.
getTiming() * 1000);
1856void PartItem::onRunTimerTimeout()
1858 if (m_runTimeValues.count() > 0){
1865 fixedTimeMove(vals);
1866 m_runTimeTimer.start(vals.
getTiming() * 1000);
1900 m_sequence_port_stamp.
update();
1901 m_sequence_port.
setEnvelope(m_sequence_port_stamp);
1903 m_sequence_port.
write(v);
1914 QMessageBox::critical(
this,
"Error",
"Select an entry in the table before performing a movement");
1928void PartItem::onSequenceActivated()
1930 for (
int i = 0; i<m_layout->
count(); i++)
1942void PartItem::onSequenceStopped()
1944 for (
int i = 0; i<m_layout->
count(); i++)
1954void PartItem::onSequenceWindowDoubleClicked(
int sequenceNum)
1958 for (
int i = 0; i<m_layout->
count(); i++)
1961 values.append(this->m_positions[i]);
1970 for (
int i = 0; i<m_layout->
count(); i++)
1979 for (
int i = 0; i<m_layout->
count(); i++)
1988 for (
int i = 0; i<m_layout->
count(); i++)
1997 for (
int i = 0; i<m_layout->
count(); i++)
2006 for (
int i = 0; i<m_layout->
count(); i++)
2015 for (
int i = 0; i<m_layout->
count(); i++)
2024 for (
int i = 0; i<m_layout->
count(); i++)
2033 m_part_speedVisible = view;
2034 for (
int i = 0; i<m_layout->
count(); i++)
2043 m_part_motorPositionVisible = view;
2044 for (
int i = 0; i<m_layout->
count(); i++)
2053 m_part_dutyVisible = view;
2054 for (
int i = 0; i<m_layout->
count(); i++)
2063 m_part_currentVisible = view;
2064 for (
int i = 0; i<m_layout->
count(); i++)
2073 for (
int i = 0; i<m_layout->
count(); i++)
2082 for (
int i = 0; i < m_layout->
count(); i++)
2091 for (
int i = 0; i<m_layout->
count(); i++)
2115 for (
int i = 0; i<m_layout->
count(); i++)
2139 for (
int i = 0; i<m_layout->
count(); i++)
2163 for (
int i = 0; i<m_layout->
count(); i++)
2188 for (
int i = 0; i<m_layout->
count(); i++)
2225 m_modesList.resize(m_layout->
count());
2226 for (
int k = 0;
k < m_layout->
count();
k++){
2227 switch (m_controlModes[
k])
2292 LOG_ERROR(
"Lost connection with the robot. You should save and restart.\n" );
2295 for (
int i = 0; i<m_layout->
count(); i++){
2307 if (!b) {
yWarning(
"Unable to update encoders");
return false; }
2312 if (!b) {
yWarning(
"Unable to update torques"); }
2314 if (this->m_part_currentVisible)
2317 if (!b) {
yWarning(
"Unable to update currents"); }
2319 if (this->m_part_speedVisible)
2322 if (!b) {
yWarning(
"Unable to update speeds"); }
2324 if (this->m_part_motorPositionVisible)
2327 if (!b) {
yWarning(
"Unable to update motorPositions"); }
2329 if (this->m_part_dutyVisible)
2332 if (!b) {
yWarning(
"Unable to update dutyCycles"); }
2383 yErrorThrottle(60.0) <<
"Missing Implementation of getTargetPosition()";
2387 yErrorThrottle(60.0) <<
"Missing Implementation of getTargetVelocity()";
2391 yErrorThrottle(60.0) <<
"Missing Implementation of getTrajSpeed()";
2395 yErrorThrottle(60.0) <<
"Missing Implementation of getRefTorque()";
2399 yErrorThrottle(60.0) <<
"Missing Implementation of checkMotionDone()";
2403 yErrorThrottle(60.0) <<
"Missing Implementation of isJointBraked()";
2431 if (
ret_refPos) {
joint_slow_k->setPosTrajectory_ReferencePosition(m_refTrajectoryPositions[m_slow_k]); }
2435 if (
ret_refAcc) {
joint_slow_k->setVelTrajectory_ReferenceAcceleration(m_refVelocityAccelerations[m_slow_k]); }
2452 LOG_ERROR(
"iint->getInteractionlMode failed\n" );
2463 std::string message;
2471 LOG_ERROR(
"Unsuccessful call to getLastJointFault()\n");
2481 switch (m_controlModes[
k])
2540 switch (m_interactionModes[
k])
#define yErrorThrottle(period,...)
int SIGNAL(int pid, int signum)
Original license follows:
QLayoutItem * itemAt(int index) const override
int count() const override
void setMotorPosition(double meas)
void viewPositionTargetValue(bool)
void setDutyVisible(bool)
void disableAccelerationSliderDouble()
void enableControlPositionDirect(bool control)
void enableVelocitySliderDoubleAuto()
void setCurrentsVisible(bool)
void enableControlMixed(bool control)
JointState getJointState()
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)
void setRefCurrent(double ref)
void setNumberOfCurrentSliderDecimals(size_t num)
double getTrajectoryVelocityValue()
void setSpeed(double val)
void enableCurrentSliderDoubleAuto()
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 enableTorqueSliderDoubleAuto()
void setSpeedVisible(bool)
void setNumberOfPositionSliderDecimals(size_t num)
void enableVelocitySliderDoubleValue(double value)
void enablePositionSliderDoubleAuto()
void disableTorqueSliderDouble()
void enableAccelerationSliderDoubleAuto()
void enableControlTorque(bool control)
void setTorque(double meas)
void setNumberOfVelocitySliderDecimals(size_t num)
void onEnableControlVelocityDirect(bool control)
void onViewDutyCycles(bool)
bool homeToCustomPosition(const yarp::os::Bottle &positionElement)
void setCurrentIndex(int)
JointItem * getJointWidget(int jointIndex)
void onEnableControlCurrent(bool control)
void onSetVelSliderOptionPI(int mode, double step, int numOfDec)
void onViewMotorPositions(bool)
void onEnableControlPWM(bool control)
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)
QString getJointName(int joint)
std::string getPartName()
void onSetCurSliderOptionPI(int mode, double step, int numOfDec)
void onViewPositionTargetValue(bool)
void onViewPositionTargetBox(bool)
bool homeJoint(int joint)
void onViewCurrentValues(bool)
void onSetTrqSliderOptionPI(int mode, double step, int numOfDec)
void sendPartJointsValues(int, QList< double >, QList< double >)
bool checkAndCycleAllSeq()
void closeSequenceWindow()
void onViewSpeedValues(bool)
void openSequenceWindow()
void onEnableControlPositionDirect(bool control)
void saveSequence(QString global_filename)
void resizeEvent(QResizeEvent *event) override
void changeEvent(QEvent *event) override
void onEnableControlTorque(bool control)
const QVector< JointItem::JointState > & getPartModes()
void onEnableControlMixed(bool control)
void onSetPosSliderOptionPI(int mode, double step, int numOfDec)
bool checkAndRunTimeAllSeq()
bool checkAndCycleTimeAllSeq()
void onSetAccSliderOptionPI(int mode, double step, int numOfDec)
void initTorquePID(const std::vector< PidWithExtraInfo > &myPids)
void initVelocityDirectPID(const std::vector< PidWithExtraInfo > &myPids)
void initTorqueOffset(double curForceVal, double minForce, double maxForce)
void initMixedPID(const std::vector< PidWithExtraInfo > &myPids)
void initCurrentPID(const std::vector< PidWithExtraInfo > &myPids)
void initStiffness(double curStiffVal, double minStiff, double maxStiff, double curDampVal, double minDamp, double maxDamp)
void initRemoteVariables(IRemoteVariables *iVar)
void initPositionDirectPID(const std::vector< PidWithExtraInfo > &myPids)
void initMotorParams(MotorTorqueParameters TorqueParam)
void initPWM(double pwmVal, double pwm)
void initVelocityPID(const std::vector< PidWithExtraInfo > &myPids)
void initPositionPID(const std::vector< PidWithExtraInfo > &myPids)
QList< double > getPositions()
QList< double > getSpeeds()
bool checkAndCycleTimeSeq()
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)
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.
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.
size_type size() const
Gets the number of elements in the bottle.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
std::string toString() const override
Gives a human-readable textual representation of the bottle.
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.
bool write(const PortWriter &writer, const PortWriter *callback=nullptr) const override
Write an object to the port.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
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...
static void delaySystem(double seconds)
virtual bool isString() const
Checks if value is a string.
virtual std::string asString() const
Get string value.
bool speedview_param_enabled
@ VOCAB_CM_VELOCITY_DIRECT
@ VOCAB_CM_POSITION_DIRECT
@ VOCAB_JOINTTYPE_REVOLUTE
@ VOCAB_CM_VELOCITY_DIRECT
@ VOCAB_CM_NOT_CONFIGURED
@ VOCAB_CM_POSITION_DIRECT