Skip to content
Snippets Groups Projects
Commit 3f3491a3 authored by Raphael Grimm's avatar Raphael Grimm
Browse files

Improve simulator example 

* allow setting all joints to kinematic
* allow to automatically set the global pose
* format code
parent c33cbf54
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
+ 177
− 57
View file @ 3f3491a3
......@@ -42,6 +42,14 @@
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents_3">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>1075</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Expanding">
<horstretch>0</horstretch>
......@@ -65,64 +73,111 @@
<number>0</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QPushButton" name="pushButtonLoad">
<property name="text">
<string>Load Robot</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="pushButton_reloadRobot">
<property name="text">
<string>Reload Robot</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="pushButtonAddObject">
<property name="text">
<string>Add Object</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QCheckBox" name="checkBox_selfCol">
<property name="text">
<string>Enable self collision (requires robot reload)</string>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Loading</string>
</property>
<property name="checked">
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QWidget" name="widget" native="true">
<layout class="QGridLayout" name="gridLayout_5">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_selfCol">
<property name="text">
<string>Enable self collision</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_kinematicJoints">
<property name="text">
<string>All Joints Kinematic</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_16">
<property name="text">
<string>Changing any of options reqires a robot reload</string>
</property>
</widget>
</item>
<item row="0" column="0">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QPushButton" name="pushButtonLoad">
<property name="text">
<string>Load Robot</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="pushButton_reloadRobot">
<property name="text">
<string>Reload Robot</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="pushButtonAddObject">
<property name="text">
<string>Add Object</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>
<item>
......@@ -267,6 +322,9 @@
<property name="maximum">
<double>9999999999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
......@@ -277,6 +335,9 @@
<property name="maximum">
<double>9999999999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="1" column="2">
......@@ -287,6 +348,9 @@
<property name="maximum">
<double>9999999999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="2">
......@@ -313,6 +377,13 @@
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QCheckBox" name="checkBoxAutoMoveRobot">
<property name="text">
<string>Auto</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
......@@ -770,12 +841,61 @@
<x>0</x>
<y>0</y>
<width>913</width>
<height>25</height>
<height>22</height>
</rect>
</property>
</widget>
<widget class="QStatusBar" name="statusbar"/>
</widget>
<resources/>
<connections/>
<connections>
<connection>
<sender>groupBox</sender>
<signal>clicked(bool)</signal>
<receiver>widget</receiver>
<slot>setVisible(bool)</slot>
<hints>
<hint type="sourcelabel">
<x>524</x>
<y>74</y>
</hint>
<hint type="destinationlabel">
<x>525</x>
<y>94</y>
</hint>
</hints>
</connection>
<connection>
<sender>checkBoxAutoMoveRobot</sender>
<signal>clicked(bool)</signal>
<receiver>button_reset</receiver>
<slot>setDisabled(bool)</slot>
<hints>
<hint type="sourcelabel">
<x>728</x>
<y>459</y>
</hint>
<hint type="destinationlabel">
<x>558</x>
<y>462</y>
</hint>
</hints>
</connection>
<connection>
<sender>checkBoxAutoMoveRobot</sender>
<signal>clicked(bool)</signal>
<receiver>button_set</receiver>
<slot>setDisabled(bool)</slot>
<hints>
<hint type="sourcelabel">
<x>755</x>
<y>472</y>
</hint>
<hint type="destinationlabel">
<x>817</x>
<y>472</y>
</hint>
</hints>
</connection>
</connections>
</ui>
SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+ 27
− 16
View file @ 3f3491a3
......@@ -85,11 +85,11 @@ SimDynamicsWindow::SimDynamicsWindow(std::string& sRobotFilename)
// register callback
float TIMER_MS = 30.0f;
SoSensorManager* sensor_mgr = SoDB::getSensorManager();
timerSensor = new SoTimerSensor(timerCB, static_cast<void *>(this));
timerSensor = new SoTimerSensor(timerCB, static_cast<void*>(this));
timerSensor->setInterval(SbTime(TIMER_MS / 1000.0f));
sensor_mgr->insertTimerSensor(timerSensor);
viewer->addStepCallback(stepCB, static_cast<void *>(this));
viewer->addStepCallback(stepCB, static_cast<void*>(this));
}
......@@ -108,6 +108,10 @@ void SimDynamicsWindow::timerCB(void* data, SoSensor* /*sensor*/)
SimDynamicsWindow* window = static_cast<SimDynamicsWindow*>(data);
VR_ASSERT(window);
if (window->UI.checkBoxAutoMoveRobot->isChecked())
{
window->setPose();
}
// now its safe to update physical information and set the models to the according poses
window->updateJointInfo();
window->updateRobotInfo();
......@@ -118,7 +122,7 @@ void SimDynamicsWindow::timerCB(void* data, SoSensor* /*sensor*/)
window->UI.label_simuStepCount->setText(QString::number(window->simuStepCount));
}
void SimDynamicsWindow::stepCB(void *data, btScalar /*timeStep*/)
void SimDynamicsWindow::stepCB(void* data, btScalar /*timeStep*/)
{
SimDynamicsWindow* window = static_cast<SimDynamicsWindow*>(data);
VR_ASSERT(window);
......@@ -217,7 +221,7 @@ void SimDynamicsWindow::buildVisualization()
viewer->addVisualization(dynamicsRobot, colModel);
viewer->addVisualization(dynamicsObject, colModel);
for (const auto & dynamicsObject : dynamicsObjects)
for (const auto& dynamicsObject : dynamicsObjects)
{
viewer->addVisualization(dynamicsObject, colModel);
}
......@@ -244,7 +248,7 @@ void SimDynamicsWindow::comVisu()
{
std::vector<RobotNodePtr> n = robot->getRobotNodes();
for (const auto & i : n)
for (const auto& i : n)
{
SoSeparator* sep = new SoSeparator;
comSep->addChild(sep);
......@@ -300,7 +304,7 @@ void SimDynamicsWindow::updateJoints()
UI.comboBoxRobotNode->clear();
std::vector<RobotNodePtr> nodes = robot->getRobotNodes();
for (auto & node : nodes)
for (auto& node : nodes)
{
if (node->isRotationalJoint() || node->isTranslationalJoint())
{
......@@ -349,8 +353,15 @@ bool SimDynamicsWindow::loadRobot(std::string robotFilename)
//gp(2,3) = 5.0f;
gp(2, 3) = -bbox.getMin()(2) + 4.0f;
robot->setGlobalPose(gp);
if (UI.checkBox_kinematicJoints->isChecked())
{
for (auto n : robot->getRobotNodes())
{
n->setSimulationType(SceneObject::Physics::eKinematic);
}
}
dynamicsRobot = dynamicsWorld->CreateDynamicsRobot(robot);
if(! UI.checkBox_selfCol->isChecked())
if (! UI.checkBox_selfCol->isChecked())
{
//we don't want to call this function with true (we would enable all collisions)
dynamicsRobot->enableSelfCollisions(false);
......@@ -669,14 +680,14 @@ void SimDynamicsWindow::updateJointInfo()
info += "/";
info += tmp.toStdString();
Eigen::Vector3f pos = rn->getGlobalPose().block<3,1>(0,3);
Eigen::Vector3f pos = rn->getGlobalPose().block<3, 1>(0, 3);
qGP = QString("GlobalPosition (simox): ") + QString::number(pos(0), 'f', 2) + " / " + QString::number(pos(1), 'f', 2) + " / " + QString::number(pos(2), 'f', 2);
auto rpy = VirtualRobot::MathTools::eigen4f2rpy(gp);
qGPRPY = "GlobalRPY (simox): " +
QString::number(rpy(0), 'f', 2) + " / "+
QString::number(rpy(1), 'f', 2) + " / "+
QString::number(rpy(2), 'f', 2);
QString::number(rpy(0), 'f', 2) + " / " +
QString::number(rpy(1), 'f', 2) + " / " +
QString::number(rpy(2), 'f', 2);
gp = rn->getGlobalPose();
qVisu = QString("VISU (simox):");
......@@ -719,7 +730,7 @@ void SimDynamicsWindow::updateJointInfo()
void SimDynamicsWindow::updateRobotInfo()
{
Eigen::Vector3f com = robot->getCoMGlobal();
Eigen::Vector3f pos = robot->getGlobalPose().block<3,1>(0,3);
Eigen::Vector3f pos = robot->getGlobalPose().block<3, 1>(0, 3);
Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(robot->getGlobalPose());
UI.label_RobotPos->setText(QString::number(pos(0), 'f', 2) + " / " + QString::number(pos(1), 'f', 2) + " / " + QString::number(pos(2), 'f', 2));
......@@ -727,12 +738,12 @@ void SimDynamicsWindow::updateRobotInfo()
UI.label_RobotCom->setText(QString::number(com(0), 'f', 2) + " / " + QString::number(com(1), 'f', 2) + " / " + QString::number(com(2), 'f', 2));
Eigen::Vector3f rpos = robot->getRootNode()->getGlobalPose().block<3,1>(0,3);
Eigen::Vector3f rpos = robot->getRootNode()->getGlobalPose().block<3, 1>(0, 3);
Eigen::Vector3f rrpy = VirtualRobot::MathTools::eigen4f2rpy(robot->getRootNode()->getGlobalPose());
UI.label_RootNodePos->setText(QString::number(rpos(0), 'f', 2) + " / " + QString::number(rpos(1), 'f', 2) + " / " + QString::number(rpos(2), 'f', 2));
UI.label_RootNodeRPY->setText(QString::number(rrpy(0), 'f', 2) + " / " + QString::number(rrpy(1), 'f', 2) + " / " + QString::number(rrpy(2), 'f', 2));
Eigen::Vector3f rltpos = robot->getRootNode()->getLocalTransformation().block<3,1>(0,3);
Eigen::Vector3f rltpos = robot->getRootNode()->getLocalTransformation().block<3, 1>(0, 3);
Eigen::Vector3f rltrpy = VirtualRobot::MathTools::eigen4f2rpy(robot->getRootNode()->getLocalTransformation());
UI.label_RootLocalTransfPos->setText(QString::number(rltpos(0), 'f', 2) + " / " + QString::number(rltpos(1), 'f', 2) + " / " + QString::number(rltpos(2), 'f', 2));
UI.label_RootLocalTransfRPY->setText(QString::number(rltrpy(0), 'f', 2) + " / " + QString::number(rltrpy(1), 'f', 2) + " / " + QString::number(rltrpy(2), 'f', 2));
......@@ -802,7 +813,7 @@ void SimDynamicsWindow::updateContactVisu()
std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> c = dynamicsWorld->getEngine()->getContacts();
for (auto & i : c)
for (auto& i : c)
{
std::cout << "Contact: " << i.objectAName << " + " << i.objectBName << std::endl;
SoSeparator* normal = new SoSeparator;
......@@ -999,7 +1010,7 @@ void SimDynamicsWindow::reloadRobot()
void SimDynamicsWindow::resetPose()
{
Eigen::Vector3f rpos = robot->getRootNode()->getGlobalPose().block<3,1>(0,3);
Eigen::Vector3f rpos = robot->getRootNode()->getGlobalPose().block<3, 1>(0, 3);
Eigen::Vector3f rrpy = VirtualRobot::MathTools::eigen4f2rpy(robot->getRootNode()->getGlobalPose());
UI.spinBox_Pos_X->setValue(rpos[0]);
......
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