| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) CNRS |
| 3 |
|
|
// Authors: Joseph Mirabel and Heidy Dallard |
| 4 |
|
|
// |
| 5 |
|
|
|
| 6 |
|
|
#include "hppwidgetsplugin/hppwidgetsplugin.hh" |
| 7 |
|
|
|
| 8 |
|
|
#include <omniORB4/CORBA.h> |
| 9 |
|
|
|
| 10 |
|
|
#include <QDockWidget> |
| 11 |
|
|
#include <QMessageBox> |
| 12 |
|
|
#include <QtGlobal> |
| 13 |
|
|
#include <boost/algorithm/string.hpp> |
| 14 |
|
|
#include <boost/regex.hpp> |
| 15 |
|
|
#include <gepetto/gui/action-search-bar.hh> |
| 16 |
|
|
#include <gepetto/gui/mainwindow.hh> |
| 17 |
|
|
#include <gepetto/gui/safeapplication.hh> |
| 18 |
|
|
#include <gepetto/gui/windows-manager.hh> |
| 19 |
|
|
|
| 20 |
|
|
#include "hppwidgetsplugin/configurationlistwidget.hh" |
| 21 |
|
|
#include "hppwidgetsplugin/constraintwidget.hh" |
| 22 |
|
|
#include "hppwidgetsplugin/conversions.hh" |
| 23 |
|
|
#include "hppwidgetsplugin/joint-action.hh" |
| 24 |
|
|
#include "hppwidgetsplugin/joint-tree-item.hh" |
| 25 |
|
|
#include "hppwidgetsplugin/jointtreewidget.hh" |
| 26 |
|
|
#include "hppwidgetsplugin/listjointconstraint.hh" |
| 27 |
|
|
#include "hppwidgetsplugin/pathplayer.hh" |
| 28 |
|
|
#include "hppwidgetsplugin/roadmap.hh" |
| 29 |
|
|
#include "hppwidgetsplugin/solverwidget.hh" |
| 30 |
|
|
#include "hppwidgetsplugin/twojointsconstraint.hh" |
| 31 |
|
|
|
| 32 |
|
|
using CORBA::ULong; |
| 33 |
|
|
|
| 34 |
|
|
namespace hpp { |
| 35 |
|
|
namespace gui { |
| 36 |
|
|
using gepetto::gui::MainWindow; |
| 37 |
|
|
typedef gepetto::viewer::WindowsManager::Color_t OsgColor_t; |
| 38 |
|
|
typedef gepetto::viewer::Configuration OsgConfiguration_t; |
| 39 |
|
|
typedef gepetto::gui::ActionSearchBar ActionSearchBar; |
| 40 |
|
|
|
| 41 |
|
|
class HppExceptionCatch : public gepetto::gui::SlotExceptionCatch { |
| 42 |
|
|
public: |
| 43 |
|
✗ |
bool safeNotify(QApplication* app, QObject* receiver, QEvent* e) { |
| 44 |
|
|
try { |
| 45 |
|
✗ |
return impl_notify(app, receiver, e); |
| 46 |
|
✗ |
} catch (const hpp::Error& e) { |
| 47 |
|
✗ |
qDebug() << e.msg.in(); |
| 48 |
|
✗ |
MainWindow* main = MainWindow::instance(); |
| 49 |
|
✗ |
if (main != NULL) main->logError(e.msg.in()); |
| 50 |
|
|
} |
| 51 |
|
✗ |
return false; |
| 52 |
|
|
} |
| 53 |
|
|
}; |
| 54 |
|
|
|
| 55 |
|
✗ |
HppWidgetsPlugin::JointElement::JointElement(const std::string& n, |
| 56 |
|
|
const std::string& prefix, |
| 57 |
|
|
const hpp::Names_t& bns, |
| 58 |
|
✗ |
JointTreeItem* i, bool updateV) |
| 59 |
|
✗ |
: name(n), |
| 60 |
|
✗ |
prefix(prefix), |
| 61 |
|
✗ |
bodyNames(bns.length()), |
| 62 |
|
✗ |
item(i), |
| 63 |
|
✗ |
updateViewer(bns.length(), updateV) { |
| 64 |
|
✗ |
for (std::size_t i = 0; i < bns.length(); ++i) |
| 65 |
|
✗ |
bodyNames[i] = std::string(bns[(CORBA::ULong)i]); |
| 66 |
|
|
} |
| 67 |
|
|
|
| 68 |
|
✗ |
HppWidgetsPlugin::HppWidgetsPlugin() |
| 69 |
|
✗ |
: pathPlayer_(NULL), |
| 70 |
|
✗ |
solverWidget_(NULL), |
| 71 |
|
✗ |
configListWidget_(NULL), |
| 72 |
|
✗ |
hpp_(NULL), |
| 73 |
|
✗ |
jointTreeWidget_(NULL) {} |
| 74 |
|
|
|
| 75 |
|
✗ |
HppWidgetsPlugin::~HppWidgetsPlugin() { |
| 76 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 77 |
|
✗ |
foreach (QDockWidget* dock, dockWidgets_) { |
| 78 |
|
✗ |
main->removeDockWidget(dock); |
| 79 |
|
✗ |
delete dock; |
| 80 |
|
|
} |
| 81 |
|
✗ |
closeConnection(); |
| 82 |
|
|
} |
| 83 |
|
|
|
| 84 |
|
✗ |
void HppWidgetsPlugin::init() { |
| 85 |
|
|
gepetto::gui::SafeApplication* app = |
| 86 |
|
✗ |
dynamic_cast<gepetto::gui::SafeApplication*>(QApplication::instance()); |
| 87 |
|
✗ |
if (app) app->addAsLeaf(new HppExceptionCatch); |
| 88 |
|
|
|
| 89 |
|
✗ |
openConnection(); |
| 90 |
|
|
|
| 91 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 92 |
|
|
QDockWidget* dock; |
| 93 |
|
|
|
| 94 |
|
|
// Configuration list widget |
| 95 |
|
✗ |
dock = new QDockWidget("&Configuration List", main); |
| 96 |
|
✗ |
dock->setObjectName("hppwidgetplugin.configurationlist"); |
| 97 |
|
✗ |
configListWidget_ = new ConfigurationListWidget(this, dock); |
| 98 |
|
✗ |
dock->setWidget(configListWidget_); |
| 99 |
|
✗ |
main->insertDockWidget(dock, Qt::RightDockWidgetArea, Qt::Vertical); |
| 100 |
|
✗ |
dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + |
| 101 |
|
|
Qt::Key_C); |
| 102 |
|
✗ |
dockWidgets_.append(dock); |
| 103 |
|
✗ |
main->registerShortcut("Configuration List", "Toggle view", |
| 104 |
|
|
dock->toggleViewAction()); |
| 105 |
|
|
|
| 106 |
|
|
// Solver widget |
| 107 |
|
✗ |
dock = new QDockWidget("Problem &solver", main); |
| 108 |
|
✗ |
dock->setObjectName("hppwidgetplugin.problemsolver"); |
| 109 |
|
✗ |
solverWidget_ = new SolverWidget(this, dock); |
| 110 |
|
✗ |
dock->setWidget(solverWidget_); |
| 111 |
|
✗ |
main->insertDockWidget(dock, Qt::RightDockWidgetArea, Qt::Horizontal); |
| 112 |
|
✗ |
dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + |
| 113 |
|
|
Qt::Key_S); |
| 114 |
|
✗ |
dockWidgets_.append(dock); |
| 115 |
|
✗ |
main->registerShortcut("Problem solver", "Toggle view", |
| 116 |
|
|
dock->toggleViewAction()); |
| 117 |
|
|
|
| 118 |
|
|
// Path player widget |
| 119 |
|
✗ |
dock = new QDockWidget("&Path player", main); |
| 120 |
|
✗ |
dock->setObjectName("hppwidgetplugin.pathplayer"); |
| 121 |
|
✗ |
dock->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Fixed); |
| 122 |
|
✗ |
pathPlayer_ = new PathPlayer(this, dock); |
| 123 |
|
✗ |
dock->setWidget(pathPlayer_); |
| 124 |
|
✗ |
main->insertDockWidget(dock, Qt::BottomDockWidgetArea, Qt::Horizontal); |
| 125 |
|
✗ |
dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + |
| 126 |
|
|
Qt::Key_P); |
| 127 |
|
✗ |
dockWidgets_.append(dock); |
| 128 |
|
✗ |
main->registerShortcut("PathPlayer", "Toggle view", dock->toggleViewAction()); |
| 129 |
|
|
|
| 130 |
|
|
// Joint tree widget |
| 131 |
|
✗ |
dock = new QDockWidget("&Joint Tree", main); |
| 132 |
|
✗ |
dock->setObjectName("hppwidgetplugin.jointtree"); |
| 133 |
|
✗ |
jointTreeWidget_ = new JointTreeWidget(this, dock); |
| 134 |
|
✗ |
dock->setWidget(jointTreeWidget_); |
| 135 |
|
✗ |
jointTreeWidget_->dockWidget(dock); |
| 136 |
|
✗ |
main->insertDockWidget(dock, Qt::RightDockWidgetArea, Qt::Vertical); |
| 137 |
|
✗ |
dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + |
| 138 |
|
|
Qt::Key_J); |
| 139 |
|
✗ |
dockWidgets_.append(dock); |
| 140 |
|
✗ |
main->registerShortcut("JointTree", "Toggle view", dock->toggleViewAction()); |
| 141 |
|
|
|
| 142 |
|
✗ |
loadConstraintWidget(); |
| 143 |
|
|
|
| 144 |
|
|
// Connect widgets |
| 145 |
|
✗ |
connect(solverWidget_, SIGNAL(problemSolved()), pathPlayer_, SLOT(update())); |
| 146 |
|
|
|
| 147 |
|
✗ |
connect(main, SIGNAL(refresh()), SLOT(update())); |
| 148 |
|
|
|
| 149 |
|
✗ |
connect(main, SIGNAL(configurationValidation()), |
| 150 |
|
|
SLOT(configurationValidation())); |
| 151 |
|
✗ |
main->connect(this, SIGNAL(configurationValidationStatus(bool)), |
| 152 |
|
|
SLOT(configurationValidationStatusChanged(bool))); |
| 153 |
|
✗ |
main->connect(this, SIGNAL(configurationValidationStatus(QStringList)), |
| 154 |
|
|
SLOT(configurationValidationStatusChanged(QStringList))); |
| 155 |
|
✗ |
connect(main, SIGNAL(applyCurrentConfiguration()), |
| 156 |
|
|
SLOT(applyCurrentConfiguration())); |
| 157 |
|
✗ |
connect(main, SIGNAL(selectJointFromBodyName(QString)), |
| 158 |
|
|
SLOT(selectJointFromBodyName(QString)), Qt::QueuedConnection); |
| 159 |
|
✗ |
main->connect(this, SIGNAL(logJobFailed(int, QString)), |
| 160 |
|
|
SLOT(logJobFailed(int, QString))); |
| 161 |
|
✗ |
main->connect(this, SIGNAL(logSuccess(QString)), SLOT(log(QString))); |
| 162 |
|
✗ |
main->connect(this, SIGNAL(logFailure(QString)), SLOT(logError(QString))); |
| 163 |
|
|
|
| 164 |
|
✗ |
main->osg()->createGroup("joints"); |
| 165 |
|
✗ |
main->osg()->addToGroup("joints", "hpp-gui"); |
| 166 |
|
✗ |
main->osg()->refresh(); |
| 167 |
|
|
|
| 168 |
|
✗ |
main->registerSlot("requestCreateJointGroup", this); |
| 169 |
|
✗ |
main->registerSlot("requestCreateComGroup", this); |
| 170 |
|
✗ |
main->registerSlot("setRobotVelocity", pathPlayer_); |
| 171 |
|
✗ |
main->registerSlot("lengthBetweenRefresh", pathPlayer_); |
| 172 |
|
✗ |
main->registerSlot("getCurrentPath", pathPlayer_); |
| 173 |
|
✗ |
main->registerSlot("getHppIIOPurl", this); |
| 174 |
|
✗ |
main->registerSlot("getHppContext", this); |
| 175 |
|
✗ |
main->registerSlot("getCurrentConfig", this); |
| 176 |
|
✗ |
main->registerSlot("setCurrentConfig", this); |
| 177 |
|
✗ |
main->registerSlot("getSelectedJoint", jointTreeWidget_); |
| 178 |
|
✗ |
main->registerSignal(SIGNAL(appliedConfigAtParam(int, double)), pathPlayer_); |
| 179 |
|
✗ |
QAction* action = main->findChild<QAction*>("actionFetch_configuration"); |
| 180 |
|
✗ |
if (action != NULL) |
| 181 |
|
✗ |
connect(action, SIGNAL(triggered()), SLOT(fetchConfiguration())); |
| 182 |
|
|
else |
| 183 |
|
✗ |
qDebug() << "Action actionFetch_configuration not found"; |
| 184 |
|
✗ |
action = main->findChild<QAction*>("actionSend_configuration"); |
| 185 |
|
✗ |
if (action != NULL) |
| 186 |
|
✗ |
connect(action, SIGNAL(triggered()), SLOT(sendConfiguration())); |
| 187 |
|
|
else |
| 188 |
|
✗ |
qDebug() << "Action actionSend_configuration not found"; |
| 189 |
|
|
|
| 190 |
|
✗ |
ActionSearchBar* asb = main->actionSearchBar(); |
| 191 |
|
|
JointAction* a; |
| 192 |
|
|
|
| 193 |
|
✗ |
a = new JointAction(tr("Add joint &frame"), jointTreeWidget_, this); |
| 194 |
|
✗ |
connect(a, SIGNAL(triggered(std::string)), SLOT(addJointFrame(std::string))); |
| 195 |
|
✗ |
asb->addAction(a); |
| 196 |
|
|
|
| 197 |
|
✗ |
a = new JointAction(tr("Display &roadmap"), jointTreeWidget_, this); |
| 198 |
|
✗ |
connect(a, SIGNAL(triggered(std::string)), SLOT(displayRoadmap(std::string))); |
| 199 |
|
✗ |
asb->addAction(a); |
| 200 |
|
|
} |
| 201 |
|
|
|
| 202 |
|
✗ |
QString HppWidgetsPlugin::name() const { |
| 203 |
|
✗ |
return QString("Widgets for hpp-corbaserver"); |
| 204 |
|
|
} |
| 205 |
|
|
|
| 206 |
|
✗ |
void HppWidgetsPlugin::loadRobotModel( |
| 207 |
|
|
gepetto::gui::DialogLoadRobot::RobotDefinition rd) { |
| 208 |
|
✗ |
QString urdfFilename("package://" + rd.package_ + "/urdf/" + rd.modelName_ + |
| 209 |
|
✗ |
rd.urdfSuf_ + ".urdf"); |
| 210 |
|
✗ |
QString srdfFilename("package://" + rd.package_ + "/srdf/" + rd.modelName_ + |
| 211 |
|
✗ |
rd.srdfSuf_ + ".srdf"); |
| 212 |
|
✗ |
client()->robot()->loadRobotModel( |
| 213 |
|
✗ |
to_corba(rd.robotName_).in(), to_corba(rd.rootJointType_).in(), |
| 214 |
|
✗ |
to_corba(urdfFilename).in(), to_corba(srdfFilename).in()); |
| 215 |
|
|
// This is already done in requestRefresh |
| 216 |
|
|
// jointTreeWidget_->reload(); |
| 217 |
|
✗ |
gepetto::gui::MainWindow::instance()->requestRefresh(); |
| 218 |
|
✗ |
gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration(); |
| 219 |
|
✗ |
emit logSuccess("Robot " + rd.name_ + " loaded"); |
| 220 |
|
|
} |
| 221 |
|
|
|
| 222 |
|
✗ |
void HppWidgetsPlugin::loadEnvironmentModel( |
| 223 |
|
|
gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed) { |
| 224 |
|
✗ |
QString prefix = ed.envName_ + "/"; |
| 225 |
|
✗ |
QString urdfFilename("package://" + ed.package_ + "/urdf/" + |
| 226 |
|
✗ |
ed.urdfFilename_ + ".urdf"); |
| 227 |
|
✗ |
client()->obstacle()->loadObstacleModel(to_corba(urdfFilename).in(), |
| 228 |
|
✗ |
to_corba(prefix).in()); |
| 229 |
|
✗ |
computeObjectPosition(); |
| 230 |
|
✗ |
gepetto::gui::MainWindow::instance()->requestRefresh(); |
| 231 |
|
✗ |
emit logSuccess("Environment " + ed.name_ + " loaded"); |
| 232 |
|
|
} |
| 233 |
|
|
|
| 234 |
|
✗ |
std::string HppWidgetsPlugin::getBodyFromJoint( |
| 235 |
|
|
const std::string& jointName) const { |
| 236 |
|
✗ |
JointMap::const_iterator itj = jointMap_.find(jointName); |
| 237 |
|
✗ |
if (itj == jointMap_.constEnd()) return std::string(); |
| 238 |
|
✗ |
return itj->prefix + itj->bodyNames[0]; |
| 239 |
|
|
} |
| 240 |
|
|
|
| 241 |
|
✗ |
void HppWidgetsPlugin::fetchConfiguration() { |
| 242 |
|
✗ |
hpp::floatSeq_var c = client()->robot()->getCurrentConfig(); |
| 243 |
|
✗ |
setCurrentConfig(c.in()); |
| 244 |
|
|
} |
| 245 |
|
|
|
| 246 |
|
✗ |
void HppWidgetsPlugin::sendConfiguration() { |
| 247 |
|
✗ |
client()->robot()->setCurrentConfig(config_); |
| 248 |
|
|
} |
| 249 |
|
|
|
| 250 |
|
✗ |
void HppWidgetsPlugin::setCurrentConfig(const hpp::floatSeq& q) { |
| 251 |
|
✗ |
config_ = q; |
| 252 |
|
✗ |
MainWindow::instance()->requestApplyCurrentConfiguration(); |
| 253 |
|
|
} |
| 254 |
|
|
|
| 255 |
|
✗ |
hpp::floatSeq const* HppWidgetsPlugin::getCurrentConfig() const { |
| 256 |
|
✗ |
return &config_; |
| 257 |
|
|
} |
| 258 |
|
|
|
| 259 |
|
✗ |
void HppWidgetsPlugin::setCurrentQtConfig(const QVector<double>& q) { |
| 260 |
|
✗ |
config_.length(q.size()); |
| 261 |
|
✗ |
for (ULong i = 0; i < config_.length(); ++i) config_[i] = q[i]; |
| 262 |
|
✗ |
MainWindow::instance()->requestApplyCurrentConfiguration(); |
| 263 |
|
|
} |
| 264 |
|
|
|
| 265 |
|
✗ |
QVector<double> HppWidgetsPlugin::getCurrentQtConfig() const { |
| 266 |
|
✗ |
QVector<double> c(config_.length()); |
| 267 |
|
✗ |
for (ULong i = 0; i < config_.length(); ++i) c[i] = config_[i]; |
| 268 |
|
✗ |
return c; |
| 269 |
|
|
} |
| 270 |
|
|
|
| 271 |
|
✗ |
QString HppWidgetsPlugin::getHppIIOPurl() const { |
| 272 |
|
✗ |
auto* settings = gepetto::gui::MainWindow::instance()->settings_; |
| 273 |
|
✗ |
QString host("localhost"), port("13331"); |
| 274 |
|
|
|
| 275 |
|
✗ |
QByteArray env = qgetenv("HPP_HOST"); |
| 276 |
|
✗ |
if (!env.isNull()) host = env; |
| 277 |
|
✗ |
env = qgetenv("HPP_PORT"); |
| 278 |
|
✗ |
if (!env.isNull()) port = env; |
| 279 |
|
|
|
| 280 |
|
✗ |
host = settings->getSetting("hpp/host", host).toString(); |
| 281 |
|
✗ |
port = settings->getSetting("hpp/port", port).toString(); |
| 282 |
|
✗ |
return QString("corbaloc:iiop:%1:%2").arg(host).arg(port); |
| 283 |
|
|
} |
| 284 |
|
|
|
| 285 |
|
✗ |
QString HppWidgetsPlugin::getHppContext() const { |
| 286 |
|
|
QString context = |
| 287 |
|
✗ |
gepetto::gui::MainWindow::instance() |
| 288 |
|
✗ |
->settings_->getSetting("hpp/context", QString("corbaserver")) |
| 289 |
|
✗ |
.toString(); |
| 290 |
|
✗ |
return context; |
| 291 |
|
|
} |
| 292 |
|
|
|
| 293 |
|
✗ |
void HppWidgetsPlugin::openConnection() { |
| 294 |
|
✗ |
closeConnection(); |
| 295 |
|
✗ |
hpp_ = new hpp::corbaServer::Client(0, 0); |
| 296 |
|
✗ |
QByteArray iiop = getHppIIOPurl().toLatin1(); |
| 297 |
|
✗ |
QByteArray context = getHppContext().toLatin1(); |
| 298 |
|
|
try { |
| 299 |
|
✗ |
hpp_->connect(iiop.constData(), context.constData()); |
| 300 |
|
✗ |
} catch (const CORBA::Exception&) { |
| 301 |
|
✗ |
const char* msg = "Could not find the HPP server. Is it running ?"; |
| 302 |
|
✗ |
qDebug() << msg; |
| 303 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 304 |
|
✗ |
if (main != NULL) main->logError(msg); |
| 305 |
|
|
} |
| 306 |
|
|
} |
| 307 |
|
|
|
| 308 |
|
✗ |
void HppWidgetsPlugin::closeConnection() { |
| 309 |
|
✗ |
if (hpp_) delete hpp_; |
| 310 |
|
✗ |
hpp_ = NULL; |
| 311 |
|
|
} |
| 312 |
|
|
|
| 313 |
|
✗ |
inline char* c_str(const std::string& in) { |
| 314 |
|
✗ |
char* out = new char[in.length() + 1]; |
| 315 |
|
✗ |
strcpy(out, in.c_str()); |
| 316 |
|
✗ |
return out; |
| 317 |
|
|
} |
| 318 |
|
|
|
| 319 |
|
✗ |
void HppWidgetsPlugin::prepareApplyConfiguration() { |
| 320 |
|
✗ |
bodyNames_.clear(); |
| 321 |
|
✗ |
config_.length(client()->robot()->getConfigSize()); |
| 322 |
|
✗ |
velocity_.length(client()->robot()->getNumberDof()); |
| 323 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 324 |
|
✗ |
CORBA::ULong size = 0; |
| 325 |
|
✗ |
const CORBA::ULong sall = 100; |
| 326 |
|
✗ |
linkNames_.length(sall); |
| 327 |
|
✗ |
for (JointMap::iterator ite = jointMap_.begin(); ite != jointMap_.end(); |
| 328 |
|
✗ |
ite++) { |
| 329 |
|
✗ |
for (std::size_t i = 0; i < ite->bodyNames.size(); ++i) { |
| 330 |
|
✗ |
std::string bodyName = ite->prefix + ite->bodyNames[i]; |
| 331 |
|
✗ |
ite->updateViewer[i] = main->osg()->nodeExists(bodyName); |
| 332 |
|
✗ |
if (ite->updateViewer[i]) { |
| 333 |
|
✗ |
if (size >= linkNames_.length()) { |
| 334 |
|
|
// Allocate |
| 335 |
|
✗ |
linkNames_.length(linkNames_.length() + sall); |
| 336 |
|
|
} |
| 337 |
|
✗ |
linkNames_[size] = c_str(ite->bodyNames[i]); |
| 338 |
|
✗ |
++size; |
| 339 |
|
✗ |
bodyNames_.push_back(bodyName); |
| 340 |
|
|
} |
| 341 |
|
|
} |
| 342 |
|
|
} |
| 343 |
|
✗ |
linkNames_.length(size); |
| 344 |
|
|
} |
| 345 |
|
|
|
| 346 |
|
✗ |
void HppWidgetsPlugin::applyCurrentConfiguration() { |
| 347 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 348 |
|
✗ |
if (jointMap_.isEmpty()) { |
| 349 |
|
✗ |
if (QMessageBox::Ok == |
| 350 |
|
✗ |
QMessageBox::question( |
| 351 |
|
|
NULL, "Refresh required", |
| 352 |
|
|
"The current configuration cannot be applied because the joint map " |
| 353 |
|
|
"is empty. " |
| 354 |
|
|
"This is probably because you are using external commands (python " |
| 355 |
|
|
"interface) and you did not refresh this GUI. " |
| 356 |
|
|
"Do you want to refresh the joint map now ?")) |
| 357 |
|
✗ |
jointTreeWidget_->reload(); |
| 358 |
|
|
else |
| 359 |
|
✗ |
emit logFailure( |
| 360 |
|
|
"The current configuration cannot be applied. " |
| 361 |
|
|
"This is probably because you are using external commands (python " |
| 362 |
|
|
"interface) and you did not refresh this GUI. " |
| 363 |
|
|
"Use the refresh button \"Tools\" menu."); |
| 364 |
|
|
} |
| 365 |
|
|
hpp::TransformSeq_var Ts = |
| 366 |
|
✗ |
client()->robot()->getLinksPosition(config_, linkNames_); |
| 367 |
|
✗ |
fromHPP(Ts, bodyConfs_); |
| 368 |
|
✗ |
main->osg()->applyConfigurations(bodyNames_, bodyConfs_); |
| 369 |
|
|
|
| 370 |
|
✗ |
for (JointMap::iterator ite = jointMap_.begin(); ite != jointMap_.end(); |
| 371 |
|
✗ |
ite++) { |
| 372 |
|
✗ |
if (!ite->item) continue; |
| 373 |
|
✗ |
if (ite->item->config().length() > 0) { |
| 374 |
|
✗ |
ite->item->updateFromRobotConfig(config_); |
| 375 |
|
|
} |
| 376 |
|
|
} |
| 377 |
|
✗ |
Ts = client()->robot()->getJointsPosition(config_, jointFrames_); |
| 378 |
|
✗ |
fromHPP(Ts, bodyConfs_); |
| 379 |
|
✗ |
main->osg()->applyConfigurations(jointGroupNames_, bodyConfs_); |
| 380 |
|
|
|
| 381 |
|
✗ |
if (comFrames_.size() > 0) { |
| 382 |
|
|
static bool firstTime = true; |
| 383 |
|
✗ |
if (firstTime) { |
| 384 |
|
✗ |
main->log("COM frames is not thread safe. Use with care."); |
| 385 |
|
✗ |
firstTime = false; |
| 386 |
|
|
} |
| 387 |
|
✗ |
OsgConfiguration_t T; |
| 388 |
|
✗ |
T.quat.set(0, 0, 0, 1); |
| 389 |
|
✗ |
client()->robot()->setCurrentConfig(config_); |
| 390 |
|
✗ |
for (std::list<std::string>::const_iterator it = comFrames_.begin(); |
| 391 |
|
✗ |
it != comFrames_.end(); ++it) { |
| 392 |
|
✗ |
std::string n = "com_" + escapeJointName(*it); |
| 393 |
|
✗ |
hpp::floatSeq_var t = client()->robot()->getPartialCom(it->c_str()); |
| 394 |
|
✗ |
fromHPP(t, T.position); |
| 395 |
|
✗ |
main->osg()->applyConfiguration(n, T); |
| 396 |
|
|
} |
| 397 |
|
|
} |
| 398 |
|
✗ |
main->osg()->refresh(); |
| 399 |
|
|
} |
| 400 |
|
|
|
| 401 |
|
✗ |
void HppWidgetsPlugin::configurationValidation() { |
| 402 |
|
✗ |
bool valid = false; |
| 403 |
|
✗ |
CORBA::String_var report; |
| 404 |
|
|
try { |
| 405 |
|
✗ |
client()->robot()->isConfigValid(config_, valid, report); |
| 406 |
|
✗ |
} catch (const hpp::Error& e) { |
| 407 |
|
✗ |
emit logFailure(QString(e.msg)); |
| 408 |
|
✗ |
return; |
| 409 |
|
|
} |
| 410 |
|
✗ |
static QRegExp collision("Collision between object (.*) and (.*)"); |
| 411 |
|
✗ |
QStringList col; |
| 412 |
|
✗ |
if (!valid) { |
| 413 |
|
✗ |
if (collision.exactMatch(QString::fromLocal8Bit(report))) { |
| 414 |
|
✗ |
CORBA::String_var robotName = client()->robot()->getRobotName(); |
| 415 |
|
✗ |
size_t pos = strlen(robotName) + 1; |
| 416 |
|
✗ |
for (int i = 1; i < 3; ++i) { |
| 417 |
|
✗ |
std::string c = collision.cap(i).toStdString(); |
| 418 |
|
✗ |
bool found = false; |
| 419 |
|
✗ |
foreach (const JointElement& je, jointMap_) { |
| 420 |
|
✗ |
for (std::size_t j = 0; j < je.bodyNames.size(); ++j) { |
| 421 |
|
✗ |
if (je.bodyNames[j].length() <= pos) continue; |
| 422 |
|
✗ |
size_t len = je.bodyNames[j].length() - pos; |
| 423 |
|
✗ |
if (je.bodyNames[j].compare(pos, len, c, 0, len) == 0) { |
| 424 |
|
✗ |
col.append(QString::fromStdString(je.bodyNames[j])); |
| 425 |
|
✗ |
found = true; |
| 426 |
|
✗ |
break; |
| 427 |
|
|
} |
| 428 |
|
|
} |
| 429 |
|
✗ |
if (found) break; |
| 430 |
|
|
} |
| 431 |
|
✗ |
if (!found) col.append(collision.cap(i)); |
| 432 |
|
|
} |
| 433 |
|
✗ |
} else { |
| 434 |
|
✗ |
qDebug() << report; |
| 435 |
|
✗ |
col.append(QString::fromLocal8Bit(client()->robot()->getRobotName())); |
| 436 |
|
|
} |
| 437 |
|
✗ |
qDebug() << col; |
| 438 |
|
|
} |
| 439 |
|
✗ |
emit configurationValidationStatus(col); |
| 440 |
|
|
} |
| 441 |
|
|
|
| 442 |
|
✗ |
void HppWidgetsPlugin::selectJointFromBodyName(const QString bodyName) { |
| 443 |
|
|
static const boost::regex roadmap( |
| 444 |
|
✗ |
"^(roadmap|path[0-9]+)_(.*)/(node|edge)([0-9]+)$"); |
| 445 |
|
✗ |
boost::cmatch what; |
| 446 |
|
✗ |
const std::string bname = bodyName.toStdString(); |
| 447 |
|
✗ |
if (boost::regex_match(bname.c_str(), what, roadmap)) { |
| 448 |
|
✗ |
std::string group; |
| 449 |
|
✗ |
group.assign(what[1].first, what[1].second); |
| 450 |
|
✗ |
std::string joint; |
| 451 |
|
✗ |
joint.assign(what[2].first, what[2].second); |
| 452 |
|
✗ |
std::string type; |
| 453 |
|
✗ |
type.assign(what[3].first, what[3].second); |
| 454 |
|
✗ |
CORBA::ULong n = (CORBA::ULong)std::atoi(what[4].first); |
| 455 |
|
✗ |
qDebug() << "Detected the" << group.c_str() << type.c_str() << n |
| 456 |
|
✗ |
<< "of joint" << joint.c_str(); |
| 457 |
|
✗ |
if (group == "roadmap") { |
| 458 |
|
✗ |
if (type == "node") { |
| 459 |
|
|
try { |
| 460 |
|
✗ |
hpp::floatSeq_var q = hpp_->problem()->node(n); |
| 461 |
|
✗ |
setCurrentConfig(q.in()); |
| 462 |
|
✗ |
} catch (const hpp::Error& e) { |
| 463 |
|
✗ |
emit logFailure(QString::fromLocal8Bit(e.msg)); |
| 464 |
|
|
} |
| 465 |
|
✗ |
} else if (type == "edge") { |
| 466 |
|
|
// TODO |
| 467 |
|
|
} |
| 468 |
|
✗ |
} else if (group[0] == 'p') { |
| 469 |
|
✗ |
if (type == "node") { |
| 470 |
|
✗ |
int pid = std::atoi(&group[4]); |
| 471 |
|
|
// compute pid |
| 472 |
|
✗ |
hpp::floatSeq_var times; |
| 473 |
|
|
hpp::floatSeqSeq_var waypoints = |
| 474 |
|
✗ |
hpp_->problem()->getWaypoints((CORBA::UShort)pid, times.out()); |
| 475 |
|
✗ |
if (n < waypoints->length()) { |
| 476 |
|
✗ |
setCurrentConfig(waypoints[n]); |
| 477 |
|
|
} |
| 478 |
|
|
} |
| 479 |
|
|
} |
| 480 |
|
✗ |
return; |
| 481 |
|
|
} |
| 482 |
|
✗ |
static const boost::regex bodyname("^(.*)(_[0-9]+)$"); |
| 483 |
|
✗ |
std::string shortBodyName; |
| 484 |
|
✗ |
bool test_short_name = boost::regex_match(bname.c_str(), what, bodyname); |
| 485 |
|
✗ |
if (test_short_name) { |
| 486 |
|
✗ |
shortBodyName.assign(what[0].first, what[1].second); |
| 487 |
|
|
} |
| 488 |
|
|
|
| 489 |
|
✗ |
foreach (const JointElement& je, jointMap_) { |
| 490 |
|
|
// je.bodyNames will be of size 1 most of the time |
| 491 |
|
|
// so it is fine to use a vector + line search, vs map + binary |
| 492 |
|
|
// FIXME A good intermediate is to sort the vector. |
| 493 |
|
✗ |
const std::size_t len = je.prefix.length(); |
| 494 |
|
✗ |
if (bname.compare(0, len, je.prefix) == 0) { |
| 495 |
|
✗ |
for (std::size_t i = 0; i < je.bodyNames.size(); ++i) { |
| 496 |
|
✗ |
if (bname.compare(len, std::string::npos, je.bodyNames[i]) == 0 || |
| 497 |
|
✗ |
(test_short_name && shortBodyName.compare(len, std::string::npos, |
| 498 |
|
✗ |
je.bodyNames[i]) == 0)) { |
| 499 |
|
|
// TODO: use je.item for a faster selection. |
| 500 |
|
✗ |
jointTreeWidget_->selectJoint(je.name); |
| 501 |
|
✗ |
return; |
| 502 |
|
|
} |
| 503 |
|
|
} |
| 504 |
|
|
} |
| 505 |
|
|
} |
| 506 |
|
✗ |
qDebug() << "Joint for body" << bodyName << "not found."; |
| 507 |
|
|
} |
| 508 |
|
|
|
| 509 |
|
✗ |
void HppWidgetsPlugin::update() { |
| 510 |
|
✗ |
jointTreeWidget_->reload(); |
| 511 |
|
✗ |
pathPlayer_->update(); |
| 512 |
|
✗ |
solverWidget_->update(); |
| 513 |
|
✗ |
configListWidget_->fetchInitAndGoalConfigs(); |
| 514 |
|
✗ |
constraintWidget_->reload(); |
| 515 |
|
✗ |
prepareApplyConfiguration(); |
| 516 |
|
|
} |
| 517 |
|
|
|
| 518 |
|
✗ |
QString HppWidgetsPlugin::requestCreateJointGroup(const QString jn) { |
| 519 |
|
✗ |
return createJointGroup(jn.toStdString()).c_str(); |
| 520 |
|
|
} |
| 521 |
|
|
|
| 522 |
|
✗ |
QString HppWidgetsPlugin::requestCreateComGroup(const QString com) { |
| 523 |
|
✗ |
return QString::fromStdString(createComGroup(com.toStdString())); |
| 524 |
|
|
} |
| 525 |
|
|
|
| 526 |
|
✗ |
HppWidgetsPlugin::HppClient* HppWidgetsPlugin::client() const { return hpp_; } |
| 527 |
|
|
|
| 528 |
|
✗ |
HppWidgetsPlugin::JointMap& HppWidgetsPlugin::jointMap() { return jointMap_; } |
| 529 |
|
|
|
| 530 |
|
✗ |
PathPlayer* HppWidgetsPlugin::pathPlayer() const { return pathPlayer_; } |
| 531 |
|
|
|
| 532 |
|
✗ |
JointTreeWidget* HppWidgetsPlugin::jointTreeWidget() const { |
| 533 |
|
✗ |
return jointTreeWidget_; |
| 534 |
|
|
} |
| 535 |
|
|
|
| 536 |
|
✗ |
void HppWidgetsPlugin::updateRobotJoints(const QString robotName) { |
| 537 |
|
✗ |
hpp::Names_t_var joints = client()->robot()->getAllJointNames(); |
| 538 |
|
✗ |
jointMap_.clear(); |
| 539 |
|
✗ |
for (size_t i = 0; i < joints->length(); ++i) { |
| 540 |
|
✗ |
const char* jname = joints[(ULong)i]; |
| 541 |
|
✗ |
hpp::Names_t_var lnames = client()->robot()->getLinkNames(jname); |
| 542 |
|
✗ |
std::string prefix(robotName.toStdString() + "/"); |
| 543 |
|
✗ |
jointMap_[jname] = JointElement(jname, prefix, lnames, 0, true); |
| 544 |
|
|
} |
| 545 |
|
|
} |
| 546 |
|
|
|
| 547 |
|
✗ |
std::string HppWidgetsPlugin::getSelectedJoint() const { |
| 548 |
|
✗ |
return jointTreeWidget_->selectedJoint(); |
| 549 |
|
|
} |
| 550 |
|
|
|
| 551 |
|
✗ |
Roadmap* HppWidgetsPlugin::createRoadmap(const std::string& jointName) { |
| 552 |
|
✗ |
Roadmap* r = new Roadmap(this); |
| 553 |
|
✗ |
r->initRoadmapFromJoint(jointName); |
| 554 |
|
✗ |
return r; |
| 555 |
|
|
} |
| 556 |
|
|
|
| 557 |
|
✗ |
void HppWidgetsPlugin::displayRoadmap(const std::string& jointName) { |
| 558 |
|
✗ |
Roadmap* r = createRoadmap(jointName); |
| 559 |
|
✗ |
r->displayRoadmap(); |
| 560 |
|
✗ |
delete r; |
| 561 |
|
|
} |
| 562 |
|
|
|
| 563 |
|
✗ |
void HppWidgetsPlugin::addJointFrame(const std::string& jointName) { |
| 564 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 565 |
|
✗ |
std::string target = createJointGroup(jointName); |
| 566 |
|
✗ |
const std::string n = target + "/XYZ"; |
| 567 |
|
✗ |
const OsgColor_t color(1, 0, 0, 1); |
| 568 |
|
|
|
| 569 |
|
|
/// This returns false if the frame already exists |
| 570 |
|
✗ |
if (main->osg()->addXYZaxis(n, color, 0.005f, 0.015f)) { |
| 571 |
|
✗ |
main->osg()->setVisibility(n, "ALWAYS_ON_TOP"); |
| 572 |
|
✗ |
return; |
| 573 |
|
|
} else { |
| 574 |
|
✗ |
main->osg()->setVisibility(n, "ALWAYS_ON_TOP"); |
| 575 |
|
|
} |
| 576 |
|
|
} |
| 577 |
|
|
|
| 578 |
|
✗ |
void HppWidgetsPlugin::computeObjectPosition() { |
| 579 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 580 |
|
✗ |
hpp::Names_t_var obs = client()->obstacle()->getObstacleNames(true, false); |
| 581 |
|
✗ |
hpp::Transform__var cfg = hpp::Transform__alloc(); |
| 582 |
|
✗ |
OsgConfiguration_t d; |
| 583 |
|
✗ |
for (ULong i = 0; i < obs->length(); ++i) { |
| 584 |
|
✗ |
client()->obstacle()->getObstaclePosition(obs[i], cfg.out()); |
| 585 |
|
✗ |
fromHPP(cfg, d); |
| 586 |
|
✗ |
main->osg()->applyConfiguration(std::string(obs[i]), d); |
| 587 |
|
|
} |
| 588 |
|
✗ |
main->osg()->refresh(); |
| 589 |
|
|
} |
| 590 |
|
|
|
| 591 |
|
✗ |
void HppWidgetsPlugin::loadConstraintWidget() { |
| 592 |
|
✗ |
MainWindow* main = MainWindow::instance(); |
| 593 |
|
✗ |
QDockWidget* dock = new QDockWidget("&Constraint creator", main); |
| 594 |
|
✗ |
dock->setObjectName("hppwidgetplugin.constraintcreator"); |
| 595 |
|
✗ |
constraintWidget_ = new ConstraintWidget(this, dock); |
| 596 |
|
✗ |
dock->setWidget(constraintWidget_); |
| 597 |
|
✗ |
main->insertDockWidget(dock, Qt::RightDockWidgetArea, Qt::Vertical); |
| 598 |
|
✗ |
dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + |
| 599 |
|
|
Qt::Key_V); |
| 600 |
|
✗ |
dockWidgets_.append(dock); |
| 601 |
|
✗ |
constraintWidget_->addConstraint(new PositionConstraint(this)); |
| 602 |
|
✗ |
constraintWidget_->addConstraint(new OrientationConstraint(this)); |
| 603 |
|
✗ |
constraintWidget_->addConstraint(new TransformConstraint(this)); |
| 604 |
|
✗ |
constraintWidget_->addConstraint(new LockedJointConstraint(this)); |
| 605 |
|
✗ |
main->registerShortcut("Constraint Widget", "Toggle view", |
| 606 |
|
|
dock->toggleViewAction()); |
| 607 |
|
|
} |
| 608 |
|
|
|
| 609 |
|
✗ |
std::string HppWidgetsPlugin::escapeJointName(const std::string jn) { |
| 610 |
|
✗ |
std::string target = jn; |
| 611 |
|
✗ |
boost::replace_all(target, "/", "__"); |
| 612 |
|
✗ |
return target; |
| 613 |
|
|
} |
| 614 |
|
|
|
| 615 |
|
✗ |
std::string HppWidgetsPlugin::createJointGroup(const std::string jn) { |
| 616 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 617 |
|
✗ |
std::string target = escapeJointName(jn); |
| 618 |
|
|
gepetto::viewer::GroupNodePtr_t group = |
| 619 |
|
✗ |
main->osg()->getGroup(target.c_str(), false); |
| 620 |
|
✗ |
if (group) return target; |
| 621 |
|
✗ |
if (!main->osg()->getGroup(target)) { |
| 622 |
|
✗ |
main->osg()->createGroup(target); |
| 623 |
|
✗ |
main->osg()->addToGroup(target, "joints"); |
| 624 |
|
|
|
| 625 |
|
✗ |
hpp::Transform__var t = client()->robot()->getJointPosition(jn.c_str()); |
| 626 |
|
✗ |
OsgConfiguration_t p; |
| 627 |
|
✗ |
fromHPP(t, p); |
| 628 |
|
✗ |
jointFrames_.length(jointFrames_.length() + 1); |
| 629 |
|
✗ |
jointFrames_[jointFrames_.length() - 1] = jn.c_str(); |
| 630 |
|
✗ |
jointGroupNames_.push_back(target); |
| 631 |
|
✗ |
main->osg()->applyConfiguration(target, p); |
| 632 |
|
✗ |
main->osg()->refresh(); |
| 633 |
|
|
} |
| 634 |
|
✗ |
return target; |
| 635 |
|
|
} |
| 636 |
|
|
|
| 637 |
|
✗ |
std::string HppWidgetsPlugin::createComGroup(const std::string com) { |
| 638 |
|
✗ |
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance(); |
| 639 |
|
✗ |
std::string target = "com_" + escapeJointName(com); |
| 640 |
|
|
gepetto::viewer::GroupNodePtr_t group = |
| 641 |
|
✗ |
main->osg()->getGroup(target.c_str(), false); |
| 642 |
|
✗ |
if (group) return target; |
| 643 |
|
✗ |
if (!main->osg()->getGroup(target)) { |
| 644 |
|
✗ |
main->osg()->createGroup(target); |
| 645 |
|
✗ |
main->osg()->addToGroup(target, "joints"); |
| 646 |
|
|
|
| 647 |
|
✗ |
hpp::floatSeq_var p = client()->robot()->getPartialCom(com.c_str()); |
| 648 |
|
✗ |
OsgConfiguration_t t; |
| 649 |
|
✗ |
t.quat.set(0, 0, 0, 1); |
| 650 |
|
✗ |
fromHPP(p, t.position); |
| 651 |
|
✗ |
comFrames_.push_back(com); |
| 652 |
|
✗ |
main->osg()->applyConfiguration(target, t); |
| 653 |
|
✗ |
main->osg()->refresh(); |
| 654 |
|
|
} |
| 655 |
|
✗ |
return target; |
| 656 |
|
|
} |
| 657 |
|
|
|
| 658 |
|
|
#if (QT_VERSION < QT_VERSION_CHECK(5, 0, 0)) |
| 659 |
|
|
Q_EXPORT_PLUGIN2(hppwidgetsplugin, HppWidgetsPlugin) |
| 660 |
|
|
#endif |
| 661 |
|
|
} // namespace gui |
| 662 |
|
|
} // namespace hpp |
| 663 |
|
|
|