...
 
Commits (7)
......@@ -97,4 +97,14 @@ ELSE ()
DESTINATION ${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui)
ENDIF ()
# If basic.conf not yet installed, install it
IF (EXISTS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui/basic.conf")
MESSAGE (STATUS
"File ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui/basic.conf detected.")
ELSE ()
MESSAGE (STATUS "File ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui/basic.conf does not exist. Will install it.")
INSTALL (FILES etc/gepetto-gui/basic.conf
DESTINATION ${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui)
ENDIF ()
SETUP_HPP_PROJECT_FINALIZE()
[plugins]
# Plugin for hpp-manipulation-server
hppmanipulationwidgetsplugin.so=false
hppmonitoringplugin.so=false
# Plugin for hppcorbaserver
hppwidgetsplugin.so=true
......@@ -31,8 +31,6 @@ GEPETTO_GUI_PLUGIN(hppmanipulationwidgetsplugin
HEADERS
hppmanipulationwidgetsplugin.hh
linkwidget.hh
manipulationconstraintwidget.hh
manipulationncpicker.hh
HEADERS_NO_MOC
roadmap.hh
......@@ -44,8 +42,6 @@ GEPETTO_GUI_PLUGIN(hppmanipulationwidgetsplugin
hppmanipulationwidgetsplugin.cc
roadmap.cc
linkwidget.cc
manipulationconstraintwidget.cc
manipulationncpicker.cc
LINK_DEPENDENCIES
hppwidgetsplugin
......
......@@ -21,9 +21,6 @@
#include "hppwidgetsplugin/conversions.hh"
#include "hppwidgetsplugin/jointtreewidget.hh"
#include "hppmanipulationwidgetsplugin/linkwidget.hh"
#include "hppmanipulationwidgetsplugin/manipulationconstraintwidget.hh"
#include "hppwidgetsplugin/twojointsconstraint.hh"
#include "hppwidgetsplugin/listjointconstraint.hh"
using CORBA::ULong;
......@@ -83,6 +80,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd)
{
if (hpp_) return;
try {
hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
(void)q;
......@@ -104,6 +102,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::loadEnvironmentModel(gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed)
{
if (hpp_) return;
try {
hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
(void)q;
......@@ -132,7 +131,24 @@ namespace hpp {
hpp_ = new HppManipClient (0,0);
QByteArray iiop = getHppIIOPurl ().toLatin1();
QByteArray context = getHppContext ().toLatin1();
hpp_->connect (iiop.constData (), context.constData ());
try {
hpp_->connect (iiop.constData (), context.constData ());
hpp::Names_t_var for_memory_deletion = hpp_->problem()->getAvailable("type");
} catch (const CORBA::Exception& e) {
QString error ("Could not find the manipulation server. Is it running ?");
error += "\n";
error += e._name();
error += " : ";
error += e._rep_id();
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
if (main != NULL)
main->logError(error);
else
qDebug () << error;
if (hpp_) delete hpp_;
hpp_ = NULL;
}
}
void HppManipulationWidgetsPlugin::closeConnection()
......@@ -147,17 +163,6 @@ namespace hpp {
return hpp_;
}
void HppManipulationWidgetsPlugin::updateRobotJoints(const QString robotName)
{
Q_UNUSED(robotName)
hpp::Names_t_var joints = client()->robot()->getAllJointNames ();
for (size_t i = 0; i < joints->length (); ++i) {
const char* jname = joints[(ULong) i];
hpp::Names_t_var lnames = client()->robot()->getLinkNames (jname);
jointMap_[jname] = JointElement(jname, "", lnames, 0, true);
}
}
Roadmap *HppManipulationWidgetsPlugin::createRoadmap(const std::string &jointName)
{
ManipulationRoadmap* r = new ManipulationRoadmap(this);
......@@ -197,6 +202,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawRobotContacts()
{
if (hpp_) return;
hpp::Names_t_var rcs = hpp_->problem()->getRobotContactNames();
hpp::floatSeqSeq_var points;
hpp::intSeq_var indexes;
......@@ -220,6 +226,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawEnvironmentContacts()
{
if (hpp_) return;
hpp::Names_t_var rcs = hpp_->problem()->getEnvironmentContactNames();
hpp::floatSeqSeq_var points;
hpp::intSeq_var indexes;
......@@ -240,6 +247,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawHandlesFrame()
{
if (hpp_) return;
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
hpp::Names_t_var rcs = hpp_->problem()->getAvailable("handle");
hpp::Transform__var t (new Transform_);
......@@ -261,6 +269,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawGrippersFrame()
{
if (hpp_) return;
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
hpp::Names_t_var rcs = hpp_->problem()->getAvailable("gripper");
hpp::Transform__var t (new Transform_);
......@@ -335,6 +344,7 @@ namespace hpp {
HppManipulationWidgetsPlugin::MapNames HppManipulationWidgetsPlugin::getObjects()
{
assert (hpp_ != NULL);
HppManipulationWidgetsPlugin::MapNames map;
hpp::Names_t_var handles = manipClient()->problem()->getAvailable("handle");
hpp::Names_t_var surfaces = manipClient()->problem()->getAvailable("robotcontact");
......@@ -406,6 +416,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::buildGraph()
{
if (hpp_) return;
QListWidget* l = dynamic_cast<QListWidget*>(tw_->widget(0));
HppManipulationWidgetsPlugin::MapNames handlesMap = getObjects();
HppManipulationWidgetsPlugin::MapNames shapesMap = getObjects();
......@@ -444,6 +455,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::autoBuildGraph()
{
if (hpp_) return;
if (graphBuilder_ == NULL) {
graphBuilder_ = new QDialog(NULL, Qt::Dialog);
tw_ = new QTabWidget(graphBuilder_);
......@@ -511,22 +523,6 @@ namespace hpp {
graphBuilder_->show();
}
void HppManipulationWidgetsPlugin::loadConstraintWidget()
{
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
QDockWidget* dock = new QDockWidget ("&Constraint creator", main);
dock->setObjectName ("hppmanipulationwidgetsplugin.constraintcreator");
constraintWidget_ = new ManipulationConstraintWidget (this, dock);
dock->setWidget(constraintWidget_);
main->insertDockWidget (dock, Qt::RightDockWidgetArea, Qt::Vertical);
dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + Qt::Key_V);
dockWidgets_.append(dock);
constraintWidget_->addConstraint(new PositionConstraint(this));
constraintWidget_->addConstraint(new OrientationConstraint(this));
constraintWidget_->addConstraint(new TransformConstraint(this));
constraintWidget_->addConstraint(new LockedJointConstraint(this));
}
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
Q_EXPORT_PLUGIN2 (hppmanipulationwidgetsplugin, HppManipulationWidgetsPlugin)
#endif
......
......@@ -72,10 +72,6 @@ namespace hpp {
/// Get the instance of corba manipulation client.
HppManipClient* manipClient () const;
/// Get the list of joints from corbaserver and update internal joint map.
/// \param robotName name of the robot (unused)
void updateRobotJoints (const QString robotName);
/// Create the roadmap of a given joint.
/// \param jointName name of the joint
virtual Roadmap* createRoadmap (const std::string& jointName);
......@@ -100,9 +96,6 @@ namespace hpp {
/// Construct all the corba vars and create the graph.
void buildGraph();
protected:
virtual void loadConstraintWidget();
private:
// Type used to make one function to build datas needed for autoBuild
typedef std::pair<hpp::Names_t, hpp::corbaserver::manipulation::Namess_t> NamesPair;
......
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#include "hppmanipulationwidgetsplugin/hppmanipulationwidgetsplugin.hh"
#include "hppwidgetsplugin/ui_constraintwidget.h"
#include "hppmanipulationwidgetsplugin/manipulationncpicker.hh"
#include "manipulationconstraintwidget.hh"
namespace hpp {
namespace gui {
ManipulationConstraintWidget::ManipulationConstraintWidget(HppWidgetsPlugin* plugin, QWidget* parent)
: ConstraintWidget(plugin, parent)
{
}
ManipulationConstraintWidget::~ManipulationConstraintWidget()
{
}
void ManipulationConstraintWidget::applyConstraints()
{
}
void ManipulationConstraintWidget::refresh()
{
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
ConstraintWidget::refresh();
hpp::Names_t_var names = plugin->client()->problem()->getAvailable("lockedjoint");
for (unsigned i = 0; i < names->length(); i++) {
ui->nameList->addItem(names[i].in());
}
}
void ManipulationConstraintWidget::reset()
{
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
hpp::GraphComp_var graphComp;
hpp::GraphElements_var graphElems;
plugin->manipClient()->graph()->getGraph(graphComp.out(), graphElems.out());
for (unsigned i = 0; i < graphElems->edges.length(); ++i) {
plugin->manipClient()->graph()->resetConstraints(graphElems->edges[i].id);
}
for (unsigned i = 0; i < graphElems->nodes.length(); ++i) {
plugin->manipClient()->graph()->resetConstraints(graphElems->nodes[i].id);
}
}
void ManipulationConstraintWidget::confirmNumerical()
{
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
ManipulationNCPicker* ncp = new ManipulationNCPicker(plugin);
ncp->show();
}
}
}
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#ifndef HPP_GUI_MANIPULATIONCONSTRAINTWIDGET_HH
#define HPP_GUI_MANIPULATIONCONSTRAINTWIDGET_HH
#include "hppwidgetsplugin/constraintwidget.hh"
#include "hppwidgetsplugin/hppwidgetsplugin.hh"
namespace hpp {
namespace gui {
class ManipulationConstraintWidget : public ConstraintWidget
{
Q_OBJECT
public:
ManipulationConstraintWidget(HppWidgetsPlugin* plugin, QWidget* parent = 0);
virtual ~ManipulationConstraintWidget();
public slots:
virtual void refresh();
private slots:
virtual void applyConstraints();
virtual void reset();
virtual void confirmNumerical();
protected:
};
}
}
#endif // HPP_GUI_MANIPULATIONCONSTRAINTWIDGET_HH
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#include <hppmanipulationwidgetsplugin/hppmanipulationwidgetsplugin.hh>
#include "hppwidgetsplugin/ui_numericalconstraintpicker.h"
#include "manipulationncpicker.hh"
namespace hpp {
namespace gui {
ManipulationNCPicker::ManipulationNCPicker(HppManipulationWidgetsPlugin* plugin,
QWidget* parent)
: NumericalConstraintPicker(plugin, parent)
{
listComp_ = new QListWidget(this);
hpp::GraphComp_var graphComp;
hpp::GraphElements_var graphElems;
plugin->manipClient()->graph()->getGraph(graphComp.out(), graphElems.out());
components_[graphComp->name.in()] = graphComp;
listComp_->addItem(graphComp->name.in());
for (unsigned i = 0; i < graphElems->nodes.length(); ++i) {
if (graphElems->nodes[i].id > graphComp->id) {
components_[graphElems->nodes[i].name.in()] = graphElems->nodes[i];
listComp_->addItem(graphElems->nodes[i].name.in());
}
}
for (unsigned i = 0; i < graphElems->edges.length(); ++i) {
if (graphElems->edges[i].id > graphComp->id) {
components_[graphElems->edges[i].name.in()] = graphElems->edges[i];
listComp_->addItem(graphElems->edges[i].name.in());
}
}
listComp_->setSelectionMode(QAbstractItemView::ExtendedSelection);
QBoxLayout* l = dynamic_cast<QBoxLayout *>(layout());
l->insertWidget(2, new QLabel("Select on which graph components you want to apply constraints.\n"
"The constraints will be automatically applied to all the components under the one you choose."));
l->insertWidget(3, listComp_);
}
ManipulationNCPicker::~ManipulationNCPicker()
{
}
void ManipulationNCPicker::onConfirmClicked()
{
QList<QListWidgetItem*> lj = ui->lockedJointList->selectedItems();
QList<QListWidgetItem*> nc = ui->numericalList->selectedItems();
hpp::Names_t_var constraints = new hpp::Names_t;
hpp::Names_t_var locked = new hpp::Names_t;
hpp::Names_t_var dofs = new hpp::Names_t;
QList<QListWidgetItem*> selectedComp = listComp_->selectedItems();
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
constraints->length(nc.count());
locked->length(lj.count());
int i = 0;
foreach (QListWidgetItem* item, lj) {
locked[i] = item->text().toStdString().c_str();
++i;
}
i = 0;
foreach (QListWidgetItem* item, nc) {
constraints[i] = item->text().toStdString().c_str();
++i;
}
foreach (QListWidgetItem* item, selectedComp) {
if (constraints->length())
plugin->manipClient()->graph()->setNumericalConstraints(components_[item->text().toStdString()].id,
constraints.in(), dofs.in());
if (locked->length())
plugin->manipClient()->graph()->setLockedDofConstraints(components_[item->text().toStdString()].id,
locked.in());
}
onCancelClicked();
}
}
}
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#ifndef MANIPULATIONNCPICKER_HH
#define MANIPULATIONNCPICKER_HH
#include <hpp/corbaserver/manipulation/graph-idl.hh>
#include "hppwidgetsplugin/numericalconstraintpicker.hh"
namespace hpp {
namespace gui {
class HppManipulationWidgetsPlugin;
class ManipulationNCPicker : public NumericalConstraintPicker
{
Q_OBJECT
private slots:
virtual void onConfirmClicked();
public:
ManipulationNCPicker(HppManipulationWidgetsPlugin* plugin,
QWidget* parent = 0);
virtual ~ManipulationNCPicker();
protected:
std::map<std::string, GraphComp> components_;
QListWidget* listComp_;
};
}
}
#endif // MANIPULATIONNCPICKER_HH
......@@ -100,6 +100,10 @@ namespace hpp {
for (unsigned i = 0; i < nc->length(); i++) {
ui->nameList->addItem(nc[i].in());
}
hpp::Names_t_var lj = plugin_->client()->problem()->getAvailable("LockedJoint");
for (unsigned i = 0; i < lj->length(); i++) {
ui->nameList->addItem(lj[i].in());
}
}
void ConstraintWidget::createConstraint()
......
......@@ -271,7 +271,15 @@ namespace hpp {
hpp_ = new hpp::corbaServer::Client (0,0);
QByteArray iiop = getHppIIOPurl ().toLatin1();
QByteArray context = getHppContext ().toLatin1();
hpp_->connect (iiop.constData (), context.constData ());
try {
hpp_->connect (iiop.constData (), context.constData ());
} catch (const CosNaming::NamingContext::NotFound&) {
const char* msg = "Could not find the HPP server. Is it running ?";
qDebug () << msg;
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
if (main != NULL)
main->logError(msg);
}
}
void HppWidgetsPlugin::closeConnection ()
......