Skip to content
Snippets Groups Projects
Commit 5bdc99ae authored by Nikolaus Vahrenkamp's avatar Nikolaus Vahrenkamp
Browse files

add navigation demo

parent 681361df
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 1508 additions and 0 deletions
MotionPlanning/examples/CMakeLists.txt
+ 1
− 0
View file @ 5bdc99ae
......@@ -8,3 +8,4 @@ ADD_SUBDIRECTORY(GraspRRT)
ADD_SUBDIRECTORY(MultiThreadedPlanning)
ADD_SUBDIRECTORY(PlatformDemo)
MotionPlanning/examples/PlatformDemo/CMakeLists.txt 0 → 100644
+ 39
− 0
View file @ 5bdc99ae
PROJECT ( PlatformDemo )
INCLUDE(${Simox_DIR}/CMakeModules/SimoxMacros.cmake)
IF(Simox_VISUALIZATION AND Simox_USE_COIN_VISUALIZATION)
# the variable "demo_SRCS" contains all .cpp files of this project
FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/PlatformDemo.cpp ${PROJECT_SOURCE_DIR}/PlatformWindow.cpp)
FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/PlatformWindow.h)
################################## moc'ing ##############################
set(GUI_MOC_HDRS
${PROJECT_SOURCE_DIR}/PlatformWindow.h
)
set(GUI_UIS
${PROJECT_SOURCE_DIR}/PlatformDemo.ui
)
SimoxQtApplication("${PROJECT_NAME}" "${demo_SRCS}" "${demo_INCS}" "${GUI_MOC_HDRS}" "${GUI_UIS}")
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES FOLDER "Examples")
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR})
#######################################################################################
############################ Setup for installation ###################################
#######################################################################################
install(TARGETS ${PROJECT_NAME}
# IMPORTANT: Add the library to the "export-set"
EXPORT SimoxTargets
RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin
COMPONENT dev)
MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be placed into " ${Simox_BIN_DIR})
MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be installed into " ${INSTALL_BIN_DIR})
ENDIF()
MotionPlanning/examples/PlatformDemo/PlatformDemo.cpp 0 → 100644
+ 86
− 0
View file @ 5bdc99ae
#include "PlatformWindow.h"
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/RuntimeEnvironment.h>
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/Qt/SoQt.h>
#include <string>
#include <iostream>
using std::cout;
using std::endl;
using namespace VirtualRobot;
#include <Eigen/Core>
#include <Eigen/Geometry>
int main(int argc, char** argv)
{
VirtualRobot::init(argc, argv, "GraspRrtDemo");
cout << " --- START --- " << endl;
std::string filenameScene("/scenes/examples/PlatformDemo/PlatformDemo.xml");
VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filenameScene);
std::string rnsName("Planning");
std::string colModel1("ColModel Robot Body");
std::string colModel2("ColModel Obstacles");
VirtualRobot::RuntimeEnvironment::considerKey("scene");
VirtualRobot::RuntimeEnvironment::considerKey("robotNodeSet");
VirtualRobot::RuntimeEnvironment::considerKey("colModelRobot");
VirtualRobot::RuntimeEnvironment::considerKey("colModelEnv");
VirtualRobot::RuntimeEnvironment::processCommandLine(argc, argv);
VirtualRobot::RuntimeEnvironment::print();
std::string scFile = VirtualRobot::RuntimeEnvironment::getValue("scene");
if (!scFile.empty() && VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(scFile))
{
filenameScene = scFile;
}
std::string rns = VirtualRobot::RuntimeEnvironment::getValue("robotNodeSet_A");
if (!rns.empty())
{
rnsName = rns;
}
std::string c1 = VirtualRobot::RuntimeEnvironment::getValue("colModelRobot");
if (!c1.empty())
{
colModel1 = c1;
}
std::string c3 = VirtualRobot::RuntimeEnvironment::getValue("colModelEnv");
if (!c3.empty())
{
colModel2 = c3;
}
cout << "Using scene: " << filenameScene << endl;
cout << "Using environment collision model set: <" << colModel2 << ">" << endl;
cout << "\t Using RobotNodeSet for planning: <" << rnsName << ">" << endl;
cout << "\t Using robot collision model set: <" << colModel1 << ">" << endl;
PlatformWindow rw(filenameScene, rnsName, colModel1, colModel2);
rw.main();
return 0;
}
MotionPlanning/examples/PlatformDemo/PlatformDemo.ui 0 → 100644
+ 376
− 0
View file @ 5bdc99ae
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>MainWindowPlatformdemo</class>
<widget class="QMainWindow" name="MainWindowPlatformdemo">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>1055</width>
<height>769</height>
</rect>
</property>
<property name="windowTitle">
<string>GraspRrt Demo</string>
</property>
<widget class="QWidget" name="centralwidget">
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="0">
<widget class="QFrame" name="frameViewer">
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="maximumSize">
<size>
<width>350</width>
<height>16777215</height>
</size>
</property>
<property name="title">
<string/>
</property>
<widget class="QGroupBox" name="groupBox_2">
<property name="geometry">
<rect>
<x>100</x>
<y>430</y>
<width>231</width>
<height>91</height>
</rect>
</property>
<property name="title">
<string>Display options</string>
</property>
<widget class="QCheckBox" name="checkBoxColModel">
<property name="geometry">
<rect>
<x>20</x>
<y>20</y>
<width>121</width>
<height>21</height>
</rect>
</property>
<property name="text">
<string>Collision model</string>
</property>
</widget>
<widget class="QCheckBox" name="checkBoxShowRRT">
<property name="geometry">
<rect>
<x>160</x>
<y>20</y>
<width>51</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>RRT</string>
</property>
</widget>
<widget class="QCheckBox" name="checkBoxShowSolution">
<property name="geometry">
<rect>
<x>20</x>
<y>40</y>
<width>121</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>Solution</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
<widget class="QCheckBox" name="checkBoxShowSolutionOpti">
<property name="geometry">
<rect>
<x>20</x>
<y>60</y>
<width>151</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>Postprocessed Solution</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</widget>
<widget class="QGroupBox" name="groupBox_3">
<property name="geometry">
<rect>
<x>10</x>
<y>540</y>
<width>331</width>
<height>111</height>
</rect>
</property>
<property name="title">
<string>Execute</string>
</property>
<widget class="QLabel" name="label_2">
<property name="geometry">
<rect>
<x>10</x>
<y>50</y>
<width>141</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>Position on Solution</string>
</property>
</widget>
<widget class="QSlider" name="horizontalSliderPos">
<property name="geometry">
<rect>
<x>10</x>
<y>70</y>
<width>301</width>
<height>20</height>
</rect>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
<widget class="QLCDNumber" name="lcdNumber">
<property name="geometry">
<rect>
<x>150</x>
<y>50</y>
<width>64</width>
<height>23</height>
</rect>
</property>
<property name="smallDecimalPoint">
<bool>true</bool>
</property>
<property name="digitCount">
<number>5</number>
</property>
<property name="value" stdset="0">
<double>0.000000000000000</double>
</property>
</widget>
<widget class="QRadioButton" name="radioButtonSolution">
<property name="geometry">
<rect>
<x>20</x>
<y>20</y>
<width>95</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>Solution</string>
</property>
</widget>
<widget class="QRadioButton" name="radioButtonSolutionOpti">
<property name="geometry">
<rect>
<x>100</x>
<y>20</y>
<width>131</width>
<height>20</height>
</rect>
</property>
<property name="text">
<string>Postprocessed Sol.</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</widget>
<widget class="QGroupBox" name="groupBoxStart">
<property name="geometry">
<rect>
<x>10</x>
<y>10</y>
<width>331</width>
<height>401</height>
</rect>
</property>
<property name="title">
<string>Setup</string>
</property>
<widget class="QPushButton" name="pushButtonLoad">
<property name="geometry">
<rect>
<x>10</x>
<y>20</y>
<width>81</width>
<height>28</height>
</rect>
</property>
<property name="text">
<string>Load Scene</string>
</property>
</widget>
<widget class="QDoubleSpinBox" name="doubleSpinBoxColChecking">
<property name="geometry">
<rect>
<x>180</x>
<y>340</y>
<width>62</width>
<height>22</height>
</rect>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>20.000000000000000</double>
</property>
</widget>
<widget class="QDoubleSpinBox" name="doubleSpinBoxCSpaceSampling">
<property name="geometry">
<rect>
<x>180</x>
<y>370</y>
<width>62</width>
<height>22</height>
</rect>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>40.000000000000000</double>
</property>
</widget>
<widget class="QLabel" name="label_7">
<property name="geometry">
<rect>
<x>20</x>
<y>350</y>
<width>131</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>Collision Checking</string>
</property>
</widget>
<widget class="QLabel" name="label_8">
<property name="geometry">
<rect>
<x>20</x>
<y>370</y>
<width>91</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>CSpace Paths</string>
</property>
</widget>
<widget class="QLabel" name="label_9">
<property name="geometry">
<rect>
<x>10</x>
<y>330</y>
<width>91</width>
<height>16</height>
</rect>
</property>
<property name="text">
<string>Sampling</string>
</property>
</widget>
</widget>
<widget class="QGroupBox" name="groupBox_5">
<property name="geometry">
<rect>
<x>10</x>
<y>430</y>
<width>81</width>
<height>91</height>
</rect>
</property>
<property name="title">
<string>Planning</string>
</property>
<widget class="QPushButton" name="pushButtonPlan">
<property name="geometry">
<rect>
<x>20</x>
<y>30</y>
<width>51</width>
<height>41</height>
</rect>
</property>
<property name="text">
<string>Plan</string>
</property>
</widget>
</widget>
</widget>
</item>
</layout>
</widget>
<widget class="QMenuBar" name="menubar">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>1055</width>
<height>20</height>
</rect>
</property>
</widget>
<widget class="QStatusBar" name="statusbar"/>
</widget>
<resources/>
<connections>
<connection>
<sender>horizontalSliderPos</sender>
<signal>sliderMoved(int)</signal>
<receiver>lcdNumber</receiver>
<slot>display(int)</slot>
<hints>
<hint type="sourcelabel">
<x>700</x>
<y>375</y>
</hint>
<hint type="destinationlabel">
<x>751</x>
<y>397</y>
</hint>
</hints>
</connection>
</connections>
</ui>
MotionPlanning/examples/PlatformDemo/PlatformWindow.cpp 0 → 100644
+ 461
− 0
View file @ 5bdc99ae
#include "PlatformWindow.h"
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
#include "VirtualRobot/Grasping/Grasp.h"
#include "VirtualRobot/IK/GenericIKSolver.h"
#include "VirtualRobot/Grasping/GraspSet.h"
#include "VirtualRobot/Obstacle.h"
#include "VirtualRobot/CollisionDetection/CDManager.h"
#include "VirtualRobot/XML/ObjectIO.h"
#include "VirtualRobot/XML/RobotIO.h"
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h"
#include "MotionPlanning/CSpace/CSpaceSampled.h"
#include "MotionPlanning/Planner/Rrt.h"
#include "MotionPlanning/Planner/BiRrt.h"
#include "MotionPlanning/PostProcessing/ShortcutProcessor.h"
#include <MotionPlanning/Visualization/CoinVisualization/CoinRrtWorkspaceVisualization.h>
#include <QFileDialog>
#include <Eigen/Geometry>
#include <time.h>
#include <vector>
#include <iostream>
#include <cmath>
#include "Inventor/actions/SoLineHighlightRenderAction.h"
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoLightModel.h>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoEventCallback.h>
#include <Inventor/nodes/SoMatrixTransform.h>
#include <sstream>
using namespace std;
using namespace VirtualRobot;
float TIMER_MS = 200.0f;
PlatformWindow::PlatformWindow(const std::string& sceneFile,
const std::string& rns,
const std::string& colModelRob,
const std::string& colModelEnv)
: QMainWindow(NULL)
{
VR_INFO << " start " << endl;
this->sceneFile = sceneFile;
allSep = new SoSeparator;
allSep->ref();
sceneFileSep = new SoSeparator;
rrtSep = new SoSeparator;
allSep->addChild(sceneFileSep);
allSep->addChild(rrtSep);
planSetA.rns = rns;
planSetA.colModelRob = colModelRob;
planSetA.colModelEnv = colModelEnv;
setupUI();
loadScene();
viewer->viewAll();
SoSensorManager* sensor_mgr = SoDB::getSensorManager();
SoTimerSensor* timer = new SoTimerSensor(timerCB, this);
timer->setInterval(SbTime(TIMER_MS / 1000.0f));
sensor_mgr->insertTimerSensor(timer);
}
PlatformWindow::~PlatformWindow()
{
allSep->unref();
}
void PlatformWindow::timerCB(void* data, SoSensor* sensor)
{
PlatformWindow* ikWindow = static_cast<PlatformWindow*>(data);
ikWindow->redraw();
}
void PlatformWindow::setupUI()
{
UI.setupUi(this);
viewer = new SoQtExaminerViewer(UI.frameViewer, "", TRUE, SoQtExaminerViewer::BUILD_POPUP);
// setup
viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
viewer->setAccumulationBuffer(true);
viewer->setAntialiasing(true, 4);
viewer->setGLRenderAction(new SoLineHighlightRenderAction);
viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
viewer->setFeedbackVisibility(true);
viewer->setSceneGraph(allSep);
viewer->viewAll();
UI.radioButtonSolution->setChecked(true);
connect(UI.pushButtonLoad, SIGNAL(clicked()), this, SLOT(loadSceneWindow()));
connect(UI.checkBoxShowSolution, SIGNAL(clicked()), this, SLOT(buildVisu()));
connect(UI.checkBoxShowSolutionOpti, SIGNAL(clicked()), this, SLOT(buildVisu()));
connect(UI.checkBoxShowRRT, SIGNAL(clicked()), this, SLOT(buildVisu()));
connect(UI.checkBoxColModel, SIGNAL(clicked()), this, SLOT(colModel()));
connect(UI.pushButtonPlan, SIGNAL(clicked()), this, SLOT(plan()));
connect(UI.horizontalSliderPos, SIGNAL(sliderMoved(int)), this, SLOT(sliderSolution(int)));
connect(UI.radioButtonSolution, SIGNAL(clicked()), this, SLOT(solutionSelected()));
connect(UI.radioButtonSolutionOpti, SIGNAL(clicked()), this, SLOT(solutionSelected()));
}
void PlatformWindow::closeEvent(QCloseEvent* event)
{
quit();
QMainWindow::closeEvent(event);
}
void PlatformWindow::buildVisu()
{
sceneFileSep->removeAllChildren();
SceneObject::VisualizationType colModel = (UI.checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full;
if (scene)
{
visualization = scene->getVisualization<CoinVisualization>(colModel);
SoNode* visualisationNode = NULL;
if (visualization)
{
visualisationNode = visualization->getCoinVisualization();
}
if (visualisationNode)
{
sceneFileSep->addChild(visualisationNode);
}
}
buildRRTVisu();
redraw();
}
int PlatformWindow::main()
{
SoQt::show(this);
SoQt::mainLoop();
return 0;
}
void PlatformWindow::quit()
{
std::cout << "PlatformWindow: Closing" << std::endl;
this->close();
SoQt::exitMainLoop();
}
void PlatformWindow::loadSceneWindow()
{
QString fi = QFileDialog::getOpenFileName(this, tr("Open Scene File"), QString(), tr("XML Files (*.xml)"));
if (fi == "")
{
return;
}
sceneFile = std::string(fi.toLatin1());
loadScene();
}
void PlatformWindow::loadScene()
{
robot.reset();
scene = SceneIO::loadScene(sceneFile);
if (!scene)
{
VR_ERROR << " no scene ..." << endl;
return;
}
std::vector< RobotPtr > robots = scene->getRobots();
if (robots.size() != 1)
{
VR_ERROR << "Need exactly 1 robot" << endl;
return;
}
robot = robots[0];
// setup target object combo box
obstacles = scene->getObstacles();
if (obstacles.size() < 1)
{
VR_ERROR << "Need at least 1 Obstacle (target object)" << endl;
return;
}
// steup scene objects (col models env)
std::vector<SceneObjectSetPtr> soss = scene->getSceneObjectSets();
// Setup robot node sets and col models
std::vector<RobotNodeSetPtr> rnss = robot->getRobotNodeSets();
rns = robot->getRobotNodeSet(planSetA.rns);
colModelRob = robot->getRobotNodeSet(planSetA.colModelRob);
colModelEnv = scene->getSceneObjectSet(planSetA.colModelEnv);
bool startOK = false;
bool goalOK = false;
// setup start Config combo box
configs = scene->getRobotConfigs(robot);
for (auto c:configs)
{
if (c->getName() == "start")
{
c->setJointValues(robot);
Eigen::VectorXf conf(3);
rns->getJointValues(conf);
setStart(conf);
startOK = true;
}
if (c->getName() == "goal")
{
c->setJointValues(robot);
Eigen::VectorXf conf(3);
rns->getJointValues(conf);
setGoal(conf);
goalOK = true;
}
}
if (!startOK)
{
THROW_VR_EXCEPTION("Missing configuartion 'start' in scene definition");
}
if (!goalOK)
{
THROW_VR_EXCEPTION("Missing configuartion 'goal' in scene definition");
}
robot->setThreadsafe(false);
buildVisu();
}
void PlatformWindow::setStart(Eigen::VectorXf &startConf)
{
if (rns)
rns->setJointValues(startConf);
startConfig = startConf;
}
void PlatformWindow::setGoal(Eigen::VectorXf &goalConf)
{
goalConfig = goalConf;
}
void PlatformWindow::selectColModelRob(const std::string &name)
{
colModelRob.reset();
if (!robot)
{
return;
}
this->colModelRob = robot->getRobotNodeSet(name);
}
void PlatformWindow::selectColModelEnv(const std::string &name)
{
colModelEnv.reset();
if (!scene)
{
return;
}
this->colModelEnv = scene->getSceneObjectSet(name);
}
void PlatformWindow::buildRRTVisu()
{
rrtSep->removeAllChildren();
if (!cspace || !robot || !rns)
{
return;
}
boost::shared_ptr<Saba::CoinRrtWorkspaceVisualization> w(new Saba::CoinRrtWorkspaceVisualization(robot, cspace, rns->getTCP()->getName()));
if (UI.checkBoxShowRRT->isChecked())
{
if (tree)
{
w->setCustomColor(0.5f, 0.5f, 0.5f);
w->colorizeTreeNodes(2, Saba::RrtWorkspaceVisualization::eRed);
w->addTree(tree, Saba::RrtWorkspaceVisualization::eCustom);
}
if (tree2)
{
w->setCustomColor(0.5f, 0.5f, 0.5f);
w->colorizeTreeNodes(2, Saba::RrtWorkspaceVisualization::eRed);
w->addTree(tree2, Saba::RrtWorkspaceVisualization::eCustom);
}
}
if (UI.checkBoxShowSolution->isChecked() && solution)
{
w->addCSpacePath(solution);
}
if (UI.checkBoxShowSolutionOpti->isChecked() && solutionOptimized)
{
w->addCSpacePath(solutionOptimized, Saba::CoinRrtWorkspaceVisualization::eGreen);
}
w->addConfiguration(startConfig, Saba::CoinRrtWorkspaceVisualization::eGreen, 3.0f);
SoSeparator* sol = w->getCoinVisualization();
rrtSep->addChild(sol);
}
void PlatformWindow::plan()
{
if (!robot || !rns)
{
return;
}
// setup collision detection
CDManagerPtr cdm(new CDManager());
if (colModelRob)
{
cdm->addCollisionModel(colModelRob);
}
if (colModelEnv)
{
cdm->addCollisionModel(colModelEnv);
}
cspace.reset(new Saba::CSpaceSampled(robot, cdm, rns, 500000));
float sampl = (float)UI.doubleSpinBoxCSpaceSampling->value();
float samplDCD = (float)UI.doubleSpinBoxColChecking->value();
cspace->setSamplingSize(sampl);
cspace->setSamplingSizeDCD(samplDCD);
Eigen::VectorXf w(3);
w << 1,1,0.001f;
cspace->setMetricWeights(w);
Saba::BiRrtPtr rrt(new Saba::BiRrt(cspace));
rrt->setStart(startConfig);
rrt->setGoal(goalConfig);
bool planOk = false;
bool planOK = rrt->plan();
if (planOK)
{
VR_INFO << " Planning succeeded " << endl;
solution = rrt->getSolution();
Saba::ShortcutProcessorPtr postProcessing(new Saba::ShortcutProcessor(solution, cspace, false));
solutionOptimized = postProcessing->optimize(100);
tree = rrt->getTree();
tree2 = rrt->getTree2();
}
else
{
VR_INFO << " Planning failed" << endl;
}
sliderSolution(1000);
buildVisu();
}
void PlatformWindow::colModel()
{
buildVisu();
}
void PlatformWindow::solutionSelected()
{
sliderSolution(UI.horizontalSliderPos->sliderPosition());
}
void PlatformWindow::sliderSolution(int pos)
{
if (!solution)
{
return;
}
Saba::CSpacePathPtr s = solution;
if (UI.radioButtonSolutionOpti->isChecked() && solutionOptimized)
{
s = solutionOptimized;
}
float p = (float)pos / 1000.0f;
Eigen::VectorXf iPos;
s->interpolate(p, iPos);
robot->setJointValues(rns, iPos);
redraw();
}
void PlatformWindow::redraw()
{
viewer->scheduleRedraw();
UI.frameViewer->update();
viewer->scheduleRedraw();
this->update();
viewer->scheduleRedraw();
}
void PlatformWindow::selectPlanSet(int nr)
{
/*if (nr == 0)
{
selectRNS(planSetA.rns);
selectEEF(planSetA.eef);
selectColModelRobA(planSetA.colModelRob1);
selectColModelRobB(planSetA.colModelRob2);
selectColModelEnv(planSetA.colModelEnv);
selectStart(UI.comboBoxStart->currentIndex());
}
else
{
selectRNS(planSetB.rns);
selectEEF(planSetB.eef);
selectColModelRobA(planSetB.colModelRob1);
selectColModelRobB(planSetB.colModelRob2);
selectColModelEnv(planSetB.colModelEnv);
selectStart(UI.comboBoxStart->currentIndex());
}*/
}
MotionPlanning/examples/PlatformDemo/PlatformWindow.h 0 → 100644
+ 131
− 0
View file @ 5bdc99ae
#ifndef __Platform_WINDOW_H__
#define __Platform_WINDOW_H__
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/XML/SceneIO.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/ManipulationObject.h>
#include <GraspPlanning/GraspStudio.h>
#include <GraspPlanning/GraspQuality/GraspQualityMeasureWrenchSpace.h>
#include "MotionPlanning/Saba.h"
#include "MotionPlanning/CSpace/CSpacePath.h"
#include <string.h>
#include <QtCore/QtGlobal>
#include <QtGui/QtGui>
#include <QtCore/QtCore>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoEventCallback.h>
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#include <Inventor/Qt/SoQt.h>
#include <Inventor/nodes/SoSeparator.h>
#include <vector>
#include "ui_PlatformDemo.h"
class PlatformWindow : public QMainWindow
{
Q_OBJECT
public:
PlatformWindow(const std::string& sceneFile,
const std::string& rns,
const std::string& colModelRob,
const std::string& colModelEnv);
~PlatformWindow();
/*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
int main();
void redraw();
public slots:
/*! Closes the window and exits SoQt runloop. */
void quit();
/*!< Overriding the close event, so we know when the window was closed by the user. */
void closeEvent(QCloseEvent* event);
void loadSceneWindow();
void colModel();
void solutionSelected();
void sliderSolution(int pos);
void buildVisu();
void plan();
protected:
struct planSet
{
std::string rns;
std::string colModelRob;
std::string colModelEnv;
};
planSet planSetA;
void loadScene();
void setupUI();
QString formatString(const char* s, float f);
void buildRRTVisu();
void selectRNS(const std::string& rns);
void setStart(Eigen::VectorXf &goalConf);
void setGoal(Eigen::VectorXf &goalConf);
static void timerCB(void* data, SoSensor* sensor);
void buildRrtVisu();
void selectColModelRob(const std::string& colModel);
void selectColModelEnv(const std::string& colModel);
void selectPlanSet(int nr);
Ui::MainWindowPlatformdemo UI;
SoQtExaminerViewer* viewer; /*!< Viewer to display the 3D model of the robot and the environment. */
SoSeparator* allSep;
SoSeparator* sceneFileSep;
SoSeparator* rrtSep;
VirtualRobot::RobotPtr robot;
Saba::CSpaceSampledPtr cspace;
Eigen::VectorXf startConfig;
Eigen::VectorXf goalConfig;
VirtualRobot::RobotNodeSetPtr rns;
VirtualRobot::SceneObjectSetPtr colModelRob;
VirtualRobot::SceneObjectSetPtr colModelEnv;
std::vector< VirtualRobot::RobotConfigPtr > configs;
std::vector< VirtualRobot::ObstaclePtr > obstacles;
std::string sceneFile;
VirtualRobot::ScenePtr scene;
Saba::CSpacePathPtr solution;
Saba::CSpacePathPtr solutionOptimized;
Saba::CSpaceTreePtr tree;
Saba::CSpaceTreePtr tree2;
boost::shared_ptr<VirtualRobot::CoinVisualization> visualization;
Saba::BiRrtPtr rrt;
};
#endif // __Platform_WINDOW_H__
VirtualRobot/data/robots/ArmarIII/ArmarIII_withPlatformJoints.xml 0 → 100644
+ 352
− 0
View file @ 5bdc99ae
<?xml version="1.0" encoding="UTF-8" ?>
<Robot Type="Armar3" RootNode="Armar3_Base">
<RobotNode name="Armar3_Base">
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="20"/>
</Visualization>
<Child name="Dummy_Platform"/>
<!--Child name="Platform"/-->
</RobotNode>
<RobotNode name="Dummy_Platform">
<Transform>
<DH a="0" d="0" theta="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
</Transform>
<Child name="X_Platform"/>
</RobotNode>
<RobotNode name="X_Platform">
<Transform>
<DH a="0" d="0" theta="90" alpha="-90" unitsangle="degree" unitslength="mm"/>
<!--Limits unit="mm" lo="-10000" hi="10000"/-->
</Transform>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
</Visualization>
<Joint type="prismatic">
<TranslationDirection x="0" y="0" z="-1"/>
<Limits unit="mm" lo="-5000" hi="5000"/>
</Joint>
<Child name="Y_Platform"/>
</RobotNode>
<RobotNode name="Y_Platform">
<Transform>
<DH a="0" d="0" theta="-90" alpha="90" unitsangle="degree" unitslength="mm"/>
<!--Limits unit="mm" lo="-10000" hi="10000"/-->
</Transform>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
</Visualization>
<Joint type="prismatic">
<TranslationDirection x="1" y="0" z="0"/>
<Limits unit="mm" lo="-5000" hi="5000"/>
</Joint>
<Child name="Yaw_Platform"/>
</RobotNode>
<RobotNode name="Yaw_Platform">
<Transform>
<DH a="0" d="0" theta="-90" alpha="0" unitsangle="degree" unitslength="mm"/>
<!--Limits unit="mm" lo="-10000" hi="10000"/-->
</Transform>
<Joint type="revolute">
<axis x="0" y="0" z="1"/>
<Limits unit="degree" lo="-360" hi="360"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
</Joint>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
</Visualization>
<Child name="Platform"/>
</RobotNode>
<RobotNode name="Platform"> <!-- visualization via 3D-model at the very end of the kinematic chain-->
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
<File type="Inventor">fullmodel/platform.iv</File>
</Visualization>
<Physics>
<CoM location="Joint" x="0" y="0" z="200"/>
<Mass value="100" units="kg" />
</Physics>
<CollisionModel>
<File type="Inventor">convexModel/platform.iv</File>
</CollisionModel>
<Child name="Hip Pitch"/>
</RobotNode>
<RobotNode name="Hip Pitch">
<Transform>
<Translation x="0" y="158" z="890" units='mm'/>
</Transform>
<Joint type="revolute">
<Axis x="1" y="0" z="0"/>
<Limits unit="degree" lo="-45" hi="45"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
</Joint>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
<File type="Inventor">fullmodel/platform_pitch_link.iv</File>
</Visualization>
<Physics>
<Mass value="10" units="kg" />
</Physics>
<CollisionModel>
<File type="Inventor">convexModel/platform_pitch_link.iv</File>
</CollisionModel>
<Child name="Hip Roll"/>
</RobotNode>
<RobotNode name="Hip Roll">
<Joint type="revolute">
<Axis x="0" y="1" z="0"/>
<Limits unit="degree" lo="-45" hi="45"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
</Joint>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
<File type="Inventor">fullmodel/platform_roll_link.iv</File>
</Visualization>
<Physics>
<Mass value="10" units="kg" />
<IgnoreCollision name="Platform"/>
</Physics>
<CollisionModel>
<File type="Inventor">convexModel/platform_roll_link.iv</File>
</CollisionModel>
<Child name="Hip Yaw"/>
</RobotNode>
<RobotNode name="Hip Yaw">
<Joint type="revolute">
<Axis x="0" y="0" z="1"/>
<Limits unit="degree" lo="-45" hi="45"/>
<MaxVelocity unit="radian" value="1"/>
<MaxAcceleration value="10"/>
</Joint>
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="15.415" units="kg" />
<IgnoreCollision name="Hip Pitch"/>
<IgnoreCollision name="Platform"/>
</Physics>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
<File type="Inventor">fullmodel/torso.iv</File>
</Visualization>
<CollisionModel>
<File type="Inventor">convexModel/torso.iv</File>
</CollisionModel>
<Child name="Center of Arms"/>
</RobotNode>
<RobotNode name="Center of Arms">
<Transform>
<Translation x="0" y="-35" z="485" units='mm'/>
</Transform>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
</Visualization>
<Child name="TrafoToHead"/>
<Child name="TrafoToLeftArm"/>
<Child name="TrafoToRightArm"/>
</RobotNode>
<RobotNode name="TrafoToHead">
<Transform>
<Translation x="0" y="0" z="118" units='mm'/>
</Transform>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
</Visualization>
<ChildFromRobot>
<File importEEF="true">ArmarIII-Head.xml</File>
</ChildFromRobot>
</RobotNode>
<RobotNode name="TrafoToLeftArm">
<Transform>
<Translation x="-232" y="0" z="0" units='mm'/>
</Transform>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
</Visualization>
<ChildFromRobot>
<File importEEF="true">ArmarIII-LeftArm.xml</File>
</ChildFromRobot>
</RobotNode>
<RobotNode name="TrafoToRightArm">
<Transform>
<Translation x="232" y="0" z="0" units='mm'/>
</Transform>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
</Visualization>
<ChildFromRobot>
<File importEEF="true">ArmarIII-RightArm.xml</File>
</ChildFromRobot>
</RobotNode>
<RobotNodeSet name="Torso" kinematicRoot="Platform">
<Node name="Hip Pitch"/>
<Node name="Hip Roll"/>
<Node name="Hip Yaw"/>
</RobotNodeSet>
<RobotNodeSet name="TorsoHeadColModel">
<Node name="Hip Yaw"/>
<Node name="Head_Tilt"/>
</RobotNodeSet>
<RobotNodeSet name="PlatformTorsoColModel">
<Node name="Platform"/>
<Node name="Hip Yaw"/>
</RobotNodeSet>
<RobotNodeSet name="PlatformTorsoHeadColModel">
<Node name="Platform"/>
<Node name="Hip Yaw"/>
<Node name="Head_Tilt"/>
</RobotNodeSet>
<RobotNodeSet name="TorsoLeftArm" kinematicRoot="Platform" tcp="TCP L">
<Node name="Hip Pitch"/>
<Node name="Hip Roll"/>
<Node name="Hip Yaw"/>
<Node name="Shoulder 1 L"/>
<Node name="Shoulder 2 L"/>
<Node name="Upperarm L"/>
<Node name="Elbow L"/>
<Node name="Underarm L"/>
<Node name="Wrist 1 L"/>
<Node name="Wrist 2 L"/>
</RobotNodeSet>
<RobotNodeSet name="TorsoRightArm" kinematicRoot="Platform" tcp="TCP R">
<Node name="Hip Pitch"/>
<Node name="Hip Roll"/>
<Node name="Hip Yaw"/>
<Node name="Shoulder 1 R"/>
<Node name="Shoulder 2 R"/>
<Node name="Upperarm R"/>
<Node name="Elbow R"/>
<Node name="Underarm R"/>
<Node name="Wrist 1 R"/>
<Node name="Wrist 2 R"/>
</RobotNodeSet>
<RobotNodeSet name="TorsoBothArms" kinematicRoot="Platform">
<Node name="Hip Pitch"/>
<Node name="Hip Roll"/>
<Node name="Hip Yaw"/>
<Node name="Shoulder 1 L"/>
<Node name="Shoulder 2 L"/>
<Node name="Upperarm L"/>
<Node name="Elbow L"/>
<Node name="Underarm L"/>
<Node name="Wrist 1 L"/>
<Node name="Wrist 2 L"/>
<Node name="Shoulder 1 R"/>
<Node name="Shoulder 2 R"/>
<Node name="Upperarm R"/>
<Node name="Elbow R"/>
<Node name="Underarm R"/>
<Node name="Wrist 1 R"/>
<Node name="Wrist 2 R"/>
</RobotNodeSet>
<RobotNodeSet name="Robot" kinematicRoot="Platform">
<Node name="Neck_1_Pitch"/>
<Node name="Neck_2_Roll"/>
<Node name="Neck_3_Yaw"/>
<Node name="Head_Tilt"/>
<Node name="Cameras"/>
<Node name="Eye_Left"/>
<Node name="Eye_Right"/>
<Node name="Hip Pitch"/>
<Node name="Hip Roll"/>
<Node name="Hip Yaw"/>
<Node name="Shoulder 1 L"/>
<Node name="Shoulder 2 L"/>
<Node name="Upperarm L"/>
<Node name="Elbow L"/>
<Node name="Underarm L"/>
<Node name="Wrist 1 L"/>
<Node name="Wrist 2 L"/>
<Node name="Shoulder 1 R"/>
<Node name="Shoulder 2 R"/>
<Node name="Upperarm R"/>
<Node name="Elbow R"/>
<Node name="Underarm R"/>
<Node name="Wrist 1 R"/>
<Node name="Wrist 2 R"/>
<Node name="Hand Palm 1 L"/>
<Node name="Hand Palm 2 L"/>
<Node name="Thumb L J0"/>
<Node name="Thumb L J1"/>
<Node name="Index L J0"/>
<Node name="Index L J1"/>
<Node name="Middle L J0"/>
<Node name="Middle L J1"/>
<Node name="Ring L J0"/>
<Node name="Ring L J1"/>
<Node name="Pinky L J0"/>
<Node name="Pinky L J1"/>
<Node name="Hand Palm 1 R"/>
<Node name="Hand Palm 2 R"/>
<Node name="Thumb R J0"/>
<Node name="Thumb R J1"/>
<Node name="Index R J0"/>
<Node name="Index R J1"/>
<Node name="Middle R J0"/>
<Node name="Middle R J1"/>
<Node name="Ring R J0"/>
<Node name="Ring R J1"/>
<Node name="Pinky R J0"/>
<Node name="Pinky R J1"/>
</RobotNodeSet>
<RobotNodeSet name="PlatformYawTorsoLeftArm" kinematicRoot="Armar3_Base" tcp="TCP L">
<Node name="Yaw_Platform"/>
<Node name="Hip Pitch"/>
<Node name="Hip Roll"/>
<Node name="Hip Yaw"/>
<Node name="Shoulder 1 L"/>
<Node name="Shoulder 2 L"/>
<Node name="Upperarm L"/>
<Node name="Elbow L"/>
<Node name="Underarm L"/>
<Node name="Wrist 1 L"/>
<Node name="Wrist 2 L"/>
</RobotNodeSet>
<RobotNodeSet name="PlatformYawTorsoRightArm" kinematicRoot="Armar3_Base" tcp="TCP R">
<Node name="Yaw_Platform"/>
<Node name="Hip Pitch"/>
<Node name="Hip Roll"/>
<Node name="Hip Yaw"/>
<Node name="Shoulder 1 R"/>
<Node name="Shoulder 2 R"/>
<Node name="Upperarm R"/>
<Node name="Elbow R"/>
<Node name="Underarm R"/>
<Node name="Wrist 1 R"/>
<Node name="Wrist 2 R"/>
</RobotNodeSet>
</Robot>
VirtualRobot/data/scenes/examples/PlatformDemo/PlatformDemo.xml 0 → 100644
+ 62
− 0
View file @ 5bdc99ae
<?xml version="1.0" encoding="UTF-8" ?>
<Scene name="PlanningScene">
<Robot name="Armar-III" initConfig="start">
<File>/robots/ArmarIII/ArmarIII_withPlatformJoints.xml</File>
<Configuration name="start">
<Node name="X_Platform" unit="mm" value="-1400"/>
<Node name="Y_Platform" unit="mm" value="1000"/>
<Node name="Yaw_Platform" unit="radian" value="-1.57"/>
</Configuration>
<Configuration name="goal">
<Node name="X_Platform" unit="mm" value="1500"/>
<Node name="Y_Platform" unit="mm" value="900"/>
<Node name="Yaw_Platform" unit="radian" value="1.57"/>
</Configuration>
<!-- These joints are considered for motion planning-->
<RobotNodeSet name="Planning" kinematicRoot="Armar3_Base" tcp="Platform">
<Node name="X_Platform"/>
<Node name="Y_Platform"/>
<Node name="Yaw_Platform"/>
</RobotNodeSet>
<!-- The second collision model (torso, head and platform)-->
<RobotNodeSet name="ColModel Robot Body">
<Node name="Platform"/>
<Node name="Hip Yaw"/>
<Node name="Head_Tilt"/>
</RobotNodeSet>
<GlobalPose>
<Transform>
<Translation x="-6115.0" y="3580.0" z="0"/>
<rollpitchyaw units="degree" roll="0" pitch="0" yaw="0"/>
</Transform>
</GlobalPose>
</Robot>
<Obstacle name="Environment">
<Visualization>
<File type='inventor'>environment/KIT_kitchen.wrl</File>
</Visualization>
<CollisionModel>
<!--File type='inventor'>environment/KIT_kitchen_sideboard.wrl</File-->
<File type='inventor'>environment/KIT_kitchen.wrl</File>
</CollisionModel>
<GlobalPose>
<Transform>
<Translation x="0" y="0" z="-80"/>
<rollpitchyaw units="degree" roll="0" pitch="0" yaw="0"/>
</Transform>
</GlobalPose>
</Obstacle>
<SceneObjectSet name="ColModel Obstacles">
<SceneObject name="Environment"/>
</SceneObjectSet>
</Scene>
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment