diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
index 0953dd775cb093a9a43e06b25a773ddfadde881f..887fd9b701bfd412ae66de966eed5d7a91daef3e 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
@@ -201,7 +201,7 @@ namespace GraspStudio
}
GraspEvaluationPoseUncertainty::PoseEvalResult GraspEvaluationPoseUncertainty::evaluatePose(
- EndEffectorPtr eef, ObstaclePtr object, const Eigen::Matrix4f& objectPose,
+ EndEffectorPtr eef, GraspableSensorizedObjectPtr object, const Eigen::Matrix4f& objectPose,
GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape,
float closingStepSize, float stepSizeSpeedFactor)
{
@@ -246,7 +246,7 @@ namespace GraspStudio
}
GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluatePoses(
- EndEffectorPtr eef, ObstaclePtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
+ EndEffectorPtr eef, GraspableSensorizedObjectPtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape,
float closingStepSize, float stepSizeSpeedFactor)
{
@@ -320,7 +320,7 @@ namespace GraspStudio
}
GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluateGrasp(
- VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object,
+ VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::GraspableSensorizedObjectPtr object,
GraspQualityMeasurePtr qm, int numPoses,
float closingStepSize, float stepSizeSpeedFactor)
{
diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
index 50dde56ea5301ce0c566126b27a62af7490690b5..422cc0db83e80f1ca5dfa42add0e35c8e2c2f074 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
@@ -156,15 +156,15 @@ namespace GraspStudio
// Pose evaluation
PoseEvalResult evaluatePose(
- VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const Eigen::Matrix4f& objectPose,
+ VirtualRobot::EndEffectorPtr eef, VirtualRobot::GraspableSensorizedObjectPtr object, const Eigen::Matrix4f& objectPose,
GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {}, float closingStepSize = 0.02f, float stepSizeSpeedFactor = 1);
PoseEvalResults evaluatePoses(
- VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
+ VirtualRobot::EndEffectorPtr eef, VirtualRobot::GraspableSensorizedObjectPtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {}, float closingStepSize = 0.02f, float stepSizeSpeedFactor = 1);
PoseEvalResults evaluateGrasp(
- VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object,
+ VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::GraspableSensorizedObjectPtr object,
GraspQualityMeasurePtr qm, int numPoses,
float closingStepSize = 0.02f, float stepSizeSpeedFactor = 1);
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlanner.ui b/GraspPlanning/examples/GraspPlanner/GraspPlanner.ui
index 041df06b681d220a75036907014c45e5c6e7d5c5..ad2b75ce572f23ca436526433fb5ac4b9a1b68d9 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlanner.ui
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlanner.ui
@@ -37,69 +37,14 @@
<string/>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="0" column="0" colspan="2">
- <widget class="QPushButton" name="pushButtonReset">
- <property name="text">
- <string>Reset</string>
- </property>
- </widget>
- </item>
- <item row="9" column="0" colspan="2">
- <widget class="QCheckBox" name="checkBoxGrasps">
- <property name="text">
- <string>Show grasps</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QPushButton" name="pushButtonClose">
- <property name="text">
- <string>Close</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QPushButton" name="pushButtonOpen">
- <property name="text">
- <string>Open</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0" colspan="2">
- <widget class="QLabel" name="label_10">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Visualization</string>
- </property>
- </widget>
- </item>
- <item row="7" column="0" colspan="2">
+ <item row="8" column="0" colspan="2">
<widget class="QCheckBox" name="checkBoxColModel">
<property name="text">
<string>Collision Model</string>
</property>
</widget>
</item>
- <item row="3" column="1">
- <widget class="QPushButton" name="pushButtonPlanBatch">
- <property name="text">
- <string>Plan batch</string>
- </property>
- </widget>
- </item>
- <item row="5" column="0" colspan="2">
- <widget class="QPushButton" name="pushButtonSave">
- <property name="text">
- <string>Save</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0" colspan="2">
+ <item row="3" column="0" colspan="2">
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
<string>Grasp Planning Options</string>
@@ -195,28 +140,82 @@
</layout>
</widget>
</item>
- <item row="3" column="0">
+ <item row="13" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="4" column="1">
+ <widget class="QPushButton" name="pushButtonPlanBatch">
+ <property name="text">
+ <string>Plan batch</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QPushButton" name="pushButtonOpen">
+ <property name="text">
+ <string>Open</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0" colspan="2">
+ <widget class="QLabel" name="label_10">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Visualization</string>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="0" colspan="2">
+ <widget class="QCheckBox" name="checkBoxGrasps">
+ <property name="text">
+ <string>Show grasps</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0" colspan="2">
+ <widget class="QPushButton" name="pushButtonReset">
+ <property name="text">
+ <string>Reset</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
<widget class="QPushButton" name="pushButtonPlan">
<property name="text">
<string>Plan grasp(s)</string>
</property>
</widget>
</item>
- <item row="8" column="0" colspan="2">
- <widget class="QCheckBox" name="checkBoxCones">
+ <item row="1" column="0" colspan="2">
+ <widget class="QPushButton" name="pushButtonLoadObject">
<property name="text">
- <string>Friction Cones</string>
+ <string>Load Object</string>
</property>
</widget>
</item>
- <item row="10" column="0" colspan="2">
- <widget class="QCheckBox" name="checkBoxHighlight">
+ <item row="5" column="0">
+ <widget class="QPushButton" name="pushButtonClose">
<property name="text">
- <string>Highlight</string>
+ <string>Close</string>
</property>
</widget>
</item>
- <item row="11" column="0" colspan="2">
+ <item row="12" column="0" colspan="2">
<widget class="QLabel" name="labelInfo">
<property name="text">
<string>Grasps: 0</string>
@@ -226,26 +225,30 @@
</property>
</widget>
</item>
- <item row="12" column="0">
- <spacer name="verticalSpacer">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
+ <item row="6" column="0" colspan="2">
+ <widget class="QPushButton" name="pushButtonSave">
+ <property name="text">
+ <string>Save</string>
</property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
+ </widget>
+ </item>
+ <item row="11" column="0" colspan="2">
+ <widget class="QCheckBox" name="checkBoxHighlight">
+ <property name="text">
+ <string>Highlight</string>
</property>
- </spacer>
+ </widget>
</item>
- <item row="1" column="0" colspan="2">
- <widget class="QPushButton" name="pushButtonLoadObject">
+ <item row="9" column="0" colspan="2">
+ <widget class="QCheckBox" name="checkBoxCones">
<property name="text">
- <string>Load Object</string>
+ <string>Friction Cones</string>
</property>
</widget>
</item>
+ <item row="2" column="0" colspan="2">
+ <widget class="QComboBox" name="comboBoxObject"/>
+ </item>
</layout>
</widget>
</item>
@@ -257,7 +260,7 @@
<x>0</x>
<y>0</y>
<width>1079</width>
- <height>20</height>
+ <height>22</height>
</rect>
</property>
</widget>
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
index 8670458200708c6803c3edaf871b0cbe7c4e60d7..b283571a62a925f75c097c2717cb9f93dfe8d8ac 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.cpp
@@ -138,6 +138,7 @@ void GraspPlannerWindow::setupUI()
viewer->setSceneGraph(sceneSep);
viewer->viewAll();
+ connect(UI.comboBoxObject, SIGNAL(activated(int)), this, SLOT(selectRobotObject(int)));
connect(UI.pushButtonReset, SIGNAL(clicked()), this, SLOT(resetSceneryAll()));
connect(UI.pushButtonPlan, SIGNAL(clicked()), this, SLOT(plan()));
connect(UI.pushButtonPlanBatch, SIGNAL(clicked()), this, SLOT(planObjectBatch()));
@@ -152,6 +153,13 @@ void GraspPlannerWindow::setupUI()
connect(UI.checkBoxGrasps, SIGNAL(clicked()), this, SLOT(showGrasps()));
}
+void GraspPlannerWindow::selectRobotObject(int n)
+{
+ if (!robotObject) return;
+
+ object = robotObject->getRobotNode(UI.comboBoxObject->itemText(n).toStdString());
+}
+
void GraspPlannerWindow::resetSceneryAll()
{
@@ -305,7 +313,36 @@ void GraspPlannerWindow::loadObject(const string& objFile)
{
if (!objFile.empty())
{
- object = ObjectIO::loadManipulationObject(objFile);
+ try
+ {
+ object = ObjectIO::loadManipulationObject(objFile);
+ }
+ catch (VirtualRobotException& e)
+ {
+ // TODO: not pretty!
+ try {
+ robotObject = RobotIO::loadRobot(objFile, RobotIO::eFullVisAsCol);
+ object = nullptr;
+ }
+ catch (VirtualRobotException& e)
+ {
+ std::cout << " ERROR while creating object" << std::endl;
+ std::cout << e.what();
+ }
+ }
+ }
+
+
+ UI.comboBoxObject->clear();
+
+ if (robotObject) {
+ for (auto robotNode : robotObject->getRobotNodes()) {
+ if (robotNode->getVisualization()) {
+ if (!object)
+ object = robotNode;
+ UI.comboBoxObject->addItem(QString::fromStdString(robotNode->getName()));
+ }
+ }
}
if (!object)
diff --git a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
index e168c4462b1c33a0f5dffd81307c0794fca31cf6..d0d76b5a50ec5ac69f9c47142cc6fbc0a6a5bac9 100644
--- a/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
+++ b/GraspPlanning/examples/GraspPlanner/GraspPlannerWindow.h
@@ -52,7 +52,7 @@ public slots:
void closeEvent(QCloseEvent* event) override;
void resetSceneryAll();
-
+ void selectRobotObject(int n);
void closeEEF();
void openEEF();
@@ -86,7 +86,8 @@ protected:
VirtualRobot::RobotPtr robot;
VirtualRobot::RobotPtr eefCloned;
- VirtualRobot::ObstaclePtr object;
+ VirtualRobot::RobotPtr robotObject;
+ VirtualRobot::GraspableSensorizedObjectPtr object;
VirtualRobot::EndEffectorPtr eef;
VirtualRobot::GraspSetPtr grasps;
diff --git a/SimoxUtility/xml/rapidxml/RapidXMLWrapper.h b/SimoxUtility/xml/rapidxml/RapidXMLWrapper.h
index 7f37047f967dced8956b5b4a654ce60f46c6fc81..f56a579be8af65893247c3a29fa521ef0fcd7086 100644
--- a/SimoxUtility/xml/rapidxml/RapidXMLWrapper.h
+++ b/SimoxUtility/xml/rapidxml/RapidXMLWrapper.h
@@ -10,6 +10,7 @@
#include <iostream>
#include <fstream>
#include <memory>
+#include <map>
#include "SimoxUtility/algorithm/string.h"
@@ -23,8 +24,16 @@ class RapidXMLWrapperNode : public std::enable_shared_from_this<RapidXMLWrapperN
friend class RapidXMLWrapperRootNode;
+public:
+ RapidXMLWrapperNode(rapidxml::xml_node<>* node)
+ : doc(node->document()), node(node), parent(nullptr)
+ { }
+
+ virtual ~RapidXMLWrapperNode()
+ { }
+
protected:
- std::shared_ptr<rapidxml::xml_document<> > doc;
+ rapidxml::xml_document<>* doc;
rapidxml::xml_node<>* node;
RapidXMLWrapperNodePtr parent;
@@ -32,15 +41,15 @@ protected:
return doc->allocate_string(str.c_str());
}
- RapidXMLWrapperNode(std::shared_ptr<rapidxml::xml_document<> > doc, rapidxml::xml_node<>* node, RapidXMLWrapperNodePtr parent)
+ RapidXMLWrapperNode(rapidxml::xml_document<>* doc, rapidxml::xml_node<>* node, RapidXMLWrapperNodePtr parent)
: doc(doc), node(node), parent(parent)
{ }
- RapidXMLWrapperNode(std::shared_ptr<rapidxml::xml_document<> > doc, rapidxml::node_type node_type, RapidXMLWrapperNodePtr parent) : doc(doc), parent(parent) {
+ RapidXMLWrapperNode(rapidxml::xml_document<>* doc, rapidxml::node_type node_type, RapidXMLWrapperNodePtr parent) : doc(doc), parent(parent) {
node = doc->allocate_node(node_type);
}
- RapidXMLWrapperNode(std::shared_ptr<rapidxml::xml_document<> > doc, rapidxml::node_type node_type, RapidXMLWrapperNodePtr parent, const std::string& name) : doc(doc), parent(parent) {
+ RapidXMLWrapperNode(rapidxml::xml_document<>* doc, rapidxml::node_type node_type, RapidXMLWrapperNodePtr parent, const std::string& name) : doc(doc), parent(parent) {
node = doc->allocate_node(node_type, cloneString(name));
}
@@ -69,7 +78,7 @@ protected:
public:
static RapidXMLWrapperNodePtr NullNode() {
- RapidXMLWrapperNodePtr wrapper(new RapidXMLWrapperNode(std::shared_ptr<rapidxml::xml_document<> >(), NULL, nullptr));
+ RapidXMLWrapperNodePtr wrapper(new RapidXMLWrapperNode(new rapidxml::xml_document<>(), NULL, nullptr));
return wrapper;
}
@@ -144,6 +153,24 @@ public:
try { return alg::to_<T>(attribute_value(attribute.attributeName.c_str())); } catch (...) { rethrowXMLFormatError(); };
}
+ std::string attribute_value(const std::vector<std::string> &attrNames, bool throwException = false) const {
+ check();
+
+ for (const auto attrName : attrNames) {
+ rapidxml::xml_attribute<>* attrib = node->first_attribute(attrName.c_str(), 0, false);
+ if (attrib)
+ {
+ return std::string(attrib->value());
+ }
+ }
+
+ if (throwException) {
+ throw error::XMLFormatError(std::string("Attributes do not exist in node " + getPath()));
+ }
+
+ return std::string();
+ }
+
std::vector<std::string> attribute_value_vec_str(const char* attrName, const std::string &delimiter = ";") {
try { return alg::split(attribute_value(attrName), delimiter); } catch (...) { rethrowXMLFormatError(); };
}
@@ -184,10 +211,17 @@ public:
return std::string(attrib->value());
}
- std::vector<std::pair<std::string, std::string> > get_all_attributes()
+ std::map<std::string, std::string> get_all_attributes()
{
check();
- std::vector<std::pair<std::string, std::string> > attributes;
+
+ return get_all_attributes(node);
+ }
+
+
+ static std::map<std::string, std::string> get_all_attributes(rapidxml::xml_node<>* node)
+ {
+ std::map<std::string, std::string> attributes;
rapidxml::xml_attribute<>* attrib = node->first_attribute(0, 0, false);
@@ -195,14 +229,33 @@ public:
{
std::string name = std::string(attrib->name());
std::string value = std::string(attrib->value());
- std::pair<std::string, std::string> attrPair(name, value);
- attributes.push_back(attrPair);
+ attributes.insert( { name, value });
attrib = attrib->next_attribute();
}
return attributes;
}
+ /**
+ * @brief getUnitsScalingToMeter
+ * @return a scaling modifier for meter, if no unit attribute is specified the scaling modifier for millimeter is returned
+ */
+ float getUnitsScalingToMeter() {
+ std::string unit = simox::alg::to_lower(attribute_value({"units", "unit", "unitslength"}, false));
+ if (unit=="mm" || unit=="millimeter" || unit.empty())
+ return 0.001f;
+ else if (unit == "cm" || unit == "centimeter")
+ return 0.01f;
+ else if (unit == "dm" || unit == "decimeter")
+ return 0.1f;
+ else if (unit == "m" || unit == "meter")
+ return 1.0f;
+ else if (unit == "km" || unit == "kilometer")
+ return 1000.0f;
+ else
+ throw error::XMLFormatError(std::string("Unknown meter unit " + unit + " in node " + getPath()));
+ }
+
/*! Returns the content of an xml node.*/
std::string value() const
{
@@ -296,6 +349,7 @@ public:
return std::string(childNode->value());
}
+
RapidXMLWrapperNodePtr next_sibling(const char* name = 0)
{
check();
@@ -308,15 +362,29 @@ public:
return node != NULL;
}
- std::string getPath() const
+ std::string getPath(bool withAttributes = true) const
{
check();
- std::string result = name();
- rapidxml::xml_node<>* p = node->parent();
+ rapidxml::xml_node<>* p = node;
+ std::string result = std::string();
while (p)
{
- result = std::string(p->name()) + "/" + result;
+ std::stringstream att;
+ if (withAttributes)
+ {
+ std::map<std::string, std::string> attributes = get_all_attributes(p);
+ for (const auto &attribute : attributes)
+ {
+ att << " ";
+ att << attribute.first;
+ att << "='";
+ att << attribute.second;
+ att << "'";
+ }
+ }
+
+ result = std::string(p->name()) + att.str().c_str() + "/" + result;
p = p->parent();
}
@@ -352,6 +420,11 @@ public:
return node;
}
+ //! Appends a new node on the current node.
+ void append_node(const RapidXMLWrapperNode& node) {
+ this->node->append_node(doc->clone_node(node.node));
+ }
+
/*! Appends a new node on the current node and deletes the first node with the same name
@return The new Node.*/
RapidXMLWrapperNodePtr append_node_non_duplicate(const std::string& name) {
@@ -421,9 +494,9 @@ public:
/*! Creates an xml std::string representation of this xml nodes' structure
@param indent Usage of tabs in the std::string representation for better readability. */
- std::string print(bool indent = true) {
+ std::string print(bool indenting = true, int indents = 0) {
std::string s;
- rapidxml::print(std::back_inserter(s), *node, indent ? 0 : rapidxml::print_no_indenting);
+ rapidxml::internal::print_node(std::back_inserter(s), node, indenting ? 0 : rapidxml::print_no_indenting, indents);
return s;
}
@@ -445,43 +518,43 @@ public:
//! @brief Helper class for reading information from an xml format via Rapid XML
class RapidXMLWrapperRootNode : public RapidXMLWrapperNode
{
-private:
+protected:
char* cstr; // The string must persist for the lifetime of the document.
std::filesystem::path path;
- RapidXMLWrapperRootNode(const std::string& xml, const std::filesystem::path &path = std::filesystem::path())
- : RapidXMLWrapperNode(getDocument(xml), NULL, nullptr), path(path)
- {
- node = doc->first_node();
- }
-
- RapidXMLWrapperRootNode(std::shared_ptr<rapidxml::xml_document<> > doc, const std::string &name)
+ RapidXMLWrapperRootNode(rapidxml::xml_document<>* doc, const std::string &name)
: RapidXMLWrapperNode(doc, rapidxml::node_element, nullptr, name), path(std::filesystem::path())
{
cstr = new char[0];
}
- std::shared_ptr<rapidxml::xml_document<> > getDocument(const std::string& xml) {
+ rapidxml::xml_document<>* getDocument(const std::string& xml) {
if (xml.empty()) throw error::XMLFormatError(std::string("Empty xml!"));
cstr = new char[xml.size() + 1]; // Create char buffer to store std::string copy
strcpy(cstr, xml.c_str()); // Copy std::string into char buffer
try {
- std::shared_ptr<rapidxml::xml_document<> > doc(new rapidxml::xml_document<>());
+ rapidxml::xml_document<>* doc(new rapidxml::xml_document<>());
doc->parse<0>(cstr); // Pass the non-const char* to parse()
return doc;
}
- catch (rapidxml::parse_error e) {
+ catch (rapidxml::parse_error &e) {
std::string msg = e.what();
throw error::XMLFormatError(std::string("No valid xml format! " + msg));
}
}
public:
- ~RapidXMLWrapperRootNode() {
+ RapidXMLWrapperRootNode(const std::string& xml, const std::filesystem::path &path = std::filesystem::path())
+ : RapidXMLWrapperNode(getDocument(xml), NULL, nullptr), path(path)
+ {
+ node = doc->first_node();
+ }
+
+ virtual ~RapidXMLWrapperRootNode() {
delete[] cstr; // free buffer memory when all is finished
+ delete doc;
}
-public:
static std::string ReadFileContents(const std::string& path) {
try {
std::ifstream in(path.c_str());
@@ -508,8 +581,7 @@ public:
@return The RapidXMLWrapper for the xml string.
*/
static RapidXMLWrapperRootNodePtr FromXmlString(const std::string& xml, const std::string &path = std::string()) {
- RapidXMLWrapperRootNodePtr wrapper(new RapidXMLWrapperRootNode(xml, path));
- return wrapper;
+ return std::make_shared<RapidXMLWrapperRootNode>(xml, path);
}
/*!
@@ -524,7 +596,7 @@ public:
/*! Creates a root node with the given name.
@return The root Node. */
static RapidXMLWrapperRootNodePtr createRootNode(const std::string& name) {
- std::shared_ptr<rapidxml::xml_document<> > doc(new rapidxml::xml_document<>());
+ rapidxml::xml_document<>* doc(new rapidxml::xml_document<>());
RapidXMLWrapperNodePtr declaration(new RapidXMLWrapperNode(doc, rapidxml::node_declaration, nullptr));
declaration->append_attribute("version", "1.0");
declaration->append_attribute("encoding", "utf-8");
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 71ea62709c23258056416f3e1cde4f4ebdcb564e..20715bf16b976331b7a3b4f9e70dcb7a8eee6b90 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -164,6 +164,7 @@ ENDMACRO()
SET(SOURCES
BoundingBox.cpp
+ GraspableSensorizedObject.cpp
KinematicChain.cpp
LinkedCoordinate.cpp
ManipulationObject.cpp
@@ -202,6 +203,7 @@ SET(SOURCES
Grasping/BasicGraspQualityMeasure.cpp
Grasping/Grasp.cpp
Grasping/GraspSet.cpp
+ Grasping/ChainedGrasp.cpp
IK/AdvancedIKSolver.cpp
IK/CoMIK.cpp
@@ -376,6 +378,7 @@ SET(INCLUDES
AbstractFactoryMethod.h
BoundingBox.h
+ GraspableSensorizedObject.h
KinematicChain.h
ManipulationObject.h
MathTools.h
@@ -420,6 +423,7 @@ SET(INCLUDES
Grasping/Grasp.h
Grasping/GraspSet.h
Grasping/BasicGraspQualityMeasure.h
+ Grasping/ChainedGrasp.h
IK/AdvancedIKSolver.h
IK/CoMIK.h
diff --git a/VirtualRobot/GraspableSensorizedObject.cpp b/VirtualRobot/GraspableSensorizedObject.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..083ece5b80ec2ec44b4083b83090afeead69c5fd
--- /dev/null
+++ b/VirtualRobot/GraspableSensorizedObject.cpp
@@ -0,0 +1,205 @@
+#include "GraspableSensorizedObject.h"
+
+#include "VirtualRobotException.h"
+#include "Nodes/Sensor.h"
+#include "Grasping/GraspSet.h"
+
+namespace VirtualRobot
+{
+
+GraspableSensorizedObject::GraspableSensorizedObject(const std::string& name, VisualizationNodePtr visualization, CollisionModelPtr collisionModel, const Physics& p, CollisionCheckerPtr colChecker) :
+ SceneObject(name, visualization, collisionModel, p, colChecker)
+{
+
+}
+
+bool GraspableSensorizedObject::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children) {
+ for (const auto& sensor : sensors)
+ {
+ sensor->initialize(shared_from_this());
+ }
+
+ return SceneObject::initialize(parent, children);
+}
+
+std::string GraspableSensorizedObject::getGraspableSensorizedObjectXML(const std::string& modelPathRelative, bool storeSensors, int tabs) {
+ std::stringstream ss;
+ for (const auto& graspSet : graspSets)
+ {
+ ss << graspSet->getXMLString(tabs) << "\n";
+ }
+
+ if (storeSensors)
+ {
+ for (const auto& sensor : sensors)
+ {
+ ss << sensor->toXML(modelPathRelative, tabs);
+ }
+ }
+ return ss.str();
+}
+
+
+void GraspableSensorizedObject::addGraspSet(GraspSetPtr graspSet)
+{
+ THROW_VR_EXCEPTION_IF(!graspSet, "NULL data");
+ THROW_VR_EXCEPTION_IF(hasGraspSet(graspSet), "Grasp set already added");
+ // don't be too strict
+ //THROW_VR_EXCEPTION_IF(hasGraspSet(graspSet->getRobotType(), graspSet->getEndEffector()), "Only one GraspSet per EEF allowed.");
+ this->graspSets.push_back(graspSet);
+}
+
+void GraspableSensorizedObject::includeGraspSet(GraspSetPtr toBeIncludedGraspSet) //maybe delete
+{
+ THROW_VR_EXCEPTION_IF(!toBeIncludedGraspSet, "NULL data");
+ std::string robotType = toBeIncludedGraspSet->getRobotType();
+ std::string eef = toBeIncludedGraspSet->getEndEffector();
+
+ //include new Grasps
+ //check if grasp is existing
+ int index = -1;
+ for (size_t i = 0 ; i < graspSets.size(); i++)
+ {
+ if (graspSets.at(i)->getRobotType() == robotType && graspSets.at(i)->getEndEffector() == eef)
+ {
+ index = i;
+ }
+ }
+ THROW_VR_EXCEPTION_IF(index == -1, "Index wrong defined");
+ graspSets.at(index)->includeGraspSet(toBeIncludedGraspSet);
+}
+
+bool GraspableSensorizedObject::hasGraspSet(GraspSetPtr graspSet)
+{
+ VR_ASSERT_MESSAGE(graspSet, "NULL data");
+
+ for (const auto& i : graspSets)
+ if (i == graspSet)
+ {
+ return true;
+ }
+
+ return false;
+}
+
+bool GraspableSensorizedObject::hasGraspSet(const std::string& robotType, const std::string& eef)
+{
+ for (auto& graspSet : graspSets)
+ if (graspSet->getRobotType() == robotType && graspSet->getEndEffector() == eef)
+ {
+ return true;
+ }
+
+ return false;
+}
+
+VirtualRobot::GraspSetPtr GraspableSensorizedObject::getGraspSet(EndEffectorPtr eef)
+{
+ THROW_VR_EXCEPTION_IF(!eef, "NULL data");
+
+ return getGraspSet(eef->getRobotType(), eef->getName());
+
+
+}
+
+VirtualRobot::GraspSetPtr GraspableSensorizedObject::getGraspSet(const std::string& robotType, const std::string& eefName)
+{
+ for (auto& graspSet : graspSets)
+ if (graspSet->getRobotType() == robotType && graspSet->getEndEffector() == eefName)
+ {
+ return graspSet;
+ }
+
+ return GraspSetPtr();
+}
+
+VirtualRobot::GraspSetPtr GraspableSensorizedObject::getGraspSet(const std::string& name)
+{
+ for (auto& graspSet : graspSets)
+ if (graspSet->getName() == name)
+ {
+ return graspSet;
+ }
+
+ return GraspSetPtr();
+}
+
+std::vector<GraspSetPtr> GraspableSensorizedObject::getAllGraspSets()
+{
+ return graspSets;
+}
+
+
+SensorPtr GraspableSensorizedObject::getSensor(const std::string& name) const
+{
+ for (const auto& sensor : sensors)
+ {
+ if (sensor->getName() == name)
+ {
+ return sensor;
+ }
+ }
+
+ THROW_VR_EXCEPTION("No sensor with name" << name << " registerd at robot node " << getName());
+ return SensorPtr();
+}
+
+bool GraspableSensorizedObject::hasSensor(const std::string& name) const
+{
+ for (const auto& sensor : sensors)
+ {
+ if (sensor->getName() == name)
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool GraspableSensorizedObject::registerSensor(SensorPtr sensor)
+{
+ if (!this->hasChild(sensor))
+ {
+ sensors.push_back(sensor);
+ this->attachChild(sensor);
+ }
+
+ // if we are already initialized, be sure the sensor is also intialized
+ if (initialized)
+ {
+ sensor->initialize(shared_from_this());
+ }
+
+ return true;
+}
+
+std::vector<SensorPtr> GraspableSensorizedObject::getSensors() const
+{
+ return sensors;
+}
+
+
+void GraspableSensorizedObject::appendGraspSetsTo(GraspableSensorizedObjectPtr other) const {
+ for (auto graspSet : graspSets)
+ {
+ other->addGraspSet(graspSet->clone());
+ }
+}
+
+void GraspableSensorizedObject::appendSensorsTo(GraspableSensorizedObjectPtr other) const {
+ for (auto sensor : sensors)
+ {
+ other->registerSensor(sensor->clone(other));
+ }
+}
+
+void GraspableSensorizedObject::printGrasps() const {
+ for (size_t i = 0; i < graspSets.size(); i++)
+ {
+ std::cout << " * Grasp set " << i << ":" << std::endl;
+ graspSets[i]->print();
+ }
+}
+
+}
diff --git a/VirtualRobot/GraspableSensorizedObject.h b/VirtualRobot/GraspableSensorizedObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..b09ab4ea7cdae8a9a21a54d6a245b42a8a6c6781
--- /dev/null
+++ b/VirtualRobot/GraspableSensorizedObject.h
@@ -0,0 +1,102 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @copyright 2020 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+
+#pragma once
+
+#include "SceneObject.h"
+
+namespace VirtualRobot
+{
+
+class GraspableSensorizedObject;
+
+typedef std::shared_ptr<GraspableSensorizedObject> GraspableSensorizedObjectPtr;
+
+class GraspableSensorizedObject : public SceneObject
+{
+public:
+ GraspableSensorizedObject(const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(), CollisionModelPtr collisionModel = CollisionModelPtr(), const Physics& p = Physics(), CollisionCheckerPtr colChecker = CollisionCheckerPtr());
+
+ bool initialize(SceneObjectPtr parent = SceneObjectPtr(), const std::vector<SceneObjectPtr>& children = std::vector<SceneObjectPtr>()) override;
+
+ bool hasGraspSet(GraspSetPtr graspSet);
+ bool hasGraspSet(const std::string& robotType, const std::string& eef);
+
+ /*!
+ Appends a grasp set. Note, that only one grasp set per EEF is allowed.
+ */
+ void addGraspSet(GraspSetPtr graspSet);
+
+ /*!
+ * \brief includeGraspSet
+ * \param graspSet
+ */
+ void includeGraspSet(GraspSetPtr graspSet);
+
+ /*!
+ Get grasp set for the given end effector. In case multiple grasp sets for the eef are present, the first one is returned.
+ An empty GraspSetPtr is returned when no GraspSet for eef is found.
+ */
+ GraspSetPtr getGraspSet(EndEffectorPtr eef);
+
+ /*!
+ Get grasp set for the given robotType and end effector. In case multiple grasp sets for the robot/eef combination are present, the first one is returned.
+ An empty GraspSetPtr is returned when no GraspSet for robot&eef is found.
+ */
+ GraspSetPtr getGraspSet(const std::string& robotType, const std::string& eefName);
+
+ /*!
+ Get grasp set by name.
+ \param name The name of the grasp set.
+ \return An empty GraspSetPtr is returned when no GraspSet with the given name is found.
+ */
+ GraspSetPtr getGraspSet(const std::string& name);
+
+ /*!
+ Get grasp set vector
+ */
+ std::vector<GraspSetPtr> getAllGraspSets();
+
+ virtual SensorPtr getSensor(const std::string& name) const;
+ virtual bool hasSensor(const std::string& name) const;
+ virtual std::vector<SensorPtr> getSensors() const;
+ virtual bool registerSensor(SensorPtr sensor);
+
+ /*! Clones this grasps sets and appends to other */
+ void appendGraspSetsTo(GraspableSensorizedObjectPtr other) const;
+
+ /*! Clones this sensors and appends to other*/
+ void appendSensorsTo(GraspableSensorizedObjectPtr other) const;
+
+ void printGrasps() const;
+
+protected:
+ GraspableSensorizedObject() {}
+
+ std::string getGraspableSensorizedObjectXML(const std::string& modelPathRelative = "models", bool storeSensors = true, int tabs = 0);
+
+ std::vector< GraspSetPtr > graspSets;
+ std::vector<SensorPtr> sensors;
+};
+
+}
diff --git a/VirtualRobot/Grasping/ChainedGrasp.cpp b/VirtualRobot/Grasping/ChainedGrasp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85f4b229e37830a06e24a0208142e9d8fba1a4e4
--- /dev/null
+++ b/VirtualRobot/Grasping/ChainedGrasp.cpp
@@ -0,0 +1,436 @@
+#include "ChainedGrasp.h"
+
+#include "VirtualRobot/MathTools.h"
+#include "VirtualRobot/EndEffector/EndEffector.h"
+#include "VirtualRobot/Robot.h"
+#include "VirtualRobot/RobotNodeSet.h"
+
+namespace VirtualRobot
+{
+
+ChainedGrasp::ChainedGrasp(const std::string& name, const std::string& robotType, const std::string& eef,
+ const Eigen::Matrix4f& poseInTCPCoordSystem, const std::string& creation,
+ float quality, const std::string& eefPreshape) :
+ Grasp(name, robotType, eef, poseInTCPCoordSystem, creation, quality, eefPreshape),
+ x(VirtualJoint("x", false, 0)),
+ y(VirtualJoint("y", false, 1)),
+ z(VirtualJoint("z", false, 2)),
+ roll(VirtualJoint("roll", true, 0)),
+ pitch(VirtualJoint("pitch", true, 1)),
+ yaw(VirtualJoint("yaw", true, 2)),
+ graspableObjectCoordSysTransformation(Eigen::Matrix4f::Identity())
+{
+}
+
+ChainedGrasp::~ChainedGrasp()
+{
+}
+
+void ChainedGrasp::print(bool printDecoration) const
+{
+ Grasp::print(printDecoration);
+
+ // TODO
+}
+
+RobotNodePtr ChainedGrasp::attachChain(RobotPtr robot, GraspableSensorizedObjectPtr object, bool addObjectVisualization) {
+ if (!robot || !object)
+ return nullptr;
+ auto tcp = getTCP(robot);
+ if (!tcp)
+ return nullptr;
+ auto virtual_object = getObjectNode(robot);
+ if (virtual_object)
+ return virtual_object;
+ return attachChain(tcp, object, addObjectVisualization);
+}
+
+void ChainedGrasp::updateChain(RobotPtr robot) {
+ if (!robot) return;
+
+ update(x, robot);
+ update(y, robot);
+ update(z, robot);
+ update(roll, robot);
+ update(pitch, robot);
+ update(yaw, robot);
+
+ if (robot->hasRobotNode(ROBOT_NODE_NAME("object"))) {
+ auto virtual_object = robot->getRobotNode(ROBOT_NODE_NAME("object"));
+ if (virtual_object->getLocalTransformation() != graspableObjectCoordSysTransformation) {
+ virtual_object->setLocalTransformation(graspableObjectCoordSysTransformation);
+ robot->updatePose();
+ }
+ }
+}
+
+void ChainedGrasp::detachChain(RobotPtr robot) {
+ auto node = getObjectNode(robot);
+ auto tcp = getTCP(robot);
+ if (!node || !tcp) return;
+ while (node && node->getName() != tcp->getName()) {
+ auto parent = node->getParent();
+ parent->detachChild(node);
+ robot->deregisterRobotNode(node);
+ node = std::dynamic_pointer_cast<RobotNode>(parent);
+ }
+}
+
+RobotNodePtr ChainedGrasp::getVirtualNode(RobotPtr robot, const std::string &virtualName) {
+ if (robot) {
+ if (robot->hasEndEffector(eef)) {
+ auto name = ROBOT_NODE_NAME(virtualName);
+ if (robot->hasRobotNode(name))
+ return robot->getRobotNode(name);
+ }
+ }
+ return nullptr;
+}
+
+RobotNodePtr ChainedGrasp::getObjectNode(RobotPtr robot) {
+ return getVirtualNode(robot, "object");
+}
+
+RobotNodePtr ChainedGrasp::attachChain(RobotNodePtr robotNode, GraspableSensorizedObjectPtr object, bool addObjectVisualization) {
+ auto robot = robotNode->getRobot();
+
+ auto virtual_x = attach(x, robotNode);
+ auto virtual_y = attach(y, virtual_x);
+ auto virtual_z = attach(z, virtual_y);
+ auto virtual_roll = attach(roll, virtual_z);
+ auto virtual_pitch = attach(pitch, virtual_roll);
+ auto virtual_yaw = attach(yaw, virtual_pitch);
+
+ RobotNodePtr virtual_object(new RobotNodeFixed(robot, ROBOT_NODE_NAME("object"), graspableObjectCoordSysTransformation, addObjectVisualization ? object->getVisualization() : nullptr,
+ addObjectVisualization ? object->getCollisionModel() : nullptr, object->getPhysics(), object->getCollisionChecker(), RobotNode::Generic));
+ robot->registerRobotNode(virtual_object);
+ virtual_yaw->attachChild(virtual_object);
+ virtual_object->initialize(virtual_yaw);
+
+ return virtual_object;
+}
+
+RobotNodePtr ChainedGrasp::attach(VirtualJoint joint, RobotNodePtr robotNode) {
+ auto robot = robotNode->getRobot();
+ RobotNodePtr virtualNode;
+ if (joint.isRevolute) {
+ virtualNode.reset(new RobotNodeRevolute(robot, ROBOT_NODE_NAME(joint.name), joint.min, joint.max, Eigen::Matrix4f::Identity(), joint.getAxis()));
+ if (joint.isLimitless())
+ virtualNode->setLimitless(true);
+ }
+ else {
+ virtualNode.reset(new RobotNodePrismatic(robot, ROBOT_NODE_NAME(joint.name), joint.min, joint.max, Eigen::Matrix4f::Identity(), joint.getAxis()));
+ }
+ robot->registerRobotNode(virtualNode);
+ robotNode->attachChild(virtualNode);
+ virtualNode->initialize(robotNode);
+ virtualNode->setJointValue(joint.value);
+ return virtualNode;
+}
+
+bool ChainedGrasp::update(VirtualJoint joint, RobotPtr robot) {
+ auto robotNode = robot->getRobotNode(ROBOT_NODE_NAME(joint.name));
+ if (!robotNode) return false;
+ robotNode->setJointValue(joint.value);
+ robotNode->setJointLimits(joint.min, joint.max);
+ if (joint.isRevolute) {
+ robotNode->setLimitless(joint.isLimitless());
+ }
+ return true;
+}
+
+void ChainedGrasp::sampleGraspsUniform(std::vector<Eigen::Matrix4f> &grasps, RobotPtr robot, int grid)
+{
+ if (getObjectNode(robot)) {
+ auto tcp = getTCP(robot);
+ if (tcp)
+ sampleGraspsUniform(grasps, tcp->getName(), tcp, grid);
+ }
+}
+
+void ChainedGrasp::sampleGraspsUniform(std::vector<Eigen::Matrix4f> &grasps, const std::string &rootName, RobotNodePtr robotNode, int grid)
+{
+ float low = robotNode->getJointLimitLo();
+ float high = robotNode->getJointLimitHigh();
+ VR_INFO << robotNode->getName() << " " << low << " " << high << std::endl;
+ auto children = robotNode->getChildren();
+ for (auto value = low; value <= high; value += (high - low) / grid)
+ {
+ if (children.empty())
+ {
+ robotNode->getRobot()->updatePose();
+ Eigen::Matrix4f globalPose = robotNode->getGlobalPose();
+ Eigen::Matrix4f localPose = robotNode->getRobot()->getRobotNode(rootName)->toLocalCoordinateSystem(globalPose);
+ Eigen::Matrix4f inverse = localPose.inverse();
+ grasps.push_back(inverse);
+ }
+ else
+ {
+ robotNode->setJointValue(value);
+ sampleGraspsUniform(grasps, rootName, std::dynamic_pointer_cast<RobotNode>(children[0]), grid);
+ }
+ }
+}
+
+Eigen::Matrix4f ChainedGrasp::getLocalPose() const {
+ return simox::math::pos_rpy_to_mat4f(getPositionXYZ(), getOrientationRPY());
+}
+
+std::vector<RobotPtr> ChainedGrasp::sampleHandsUniform(RobotPtr robot, int grid) {
+ std::vector<RobotPtr> hands;
+ auto virtualObject = getObjectNode(robot);
+ auto endEffector = getEndEffector(robot);
+ if (virtualObject && endEffector) {
+ std::vector<Eigen::Matrix4f> grasps;
+ sampleGraspsUniform(grasps, robot->getRootNode()->getName(), robot->clone("clonedHand")->getEndEffector(endEffector->getName())->getTcp(), grid);
+ auto exampleHand = endEffector->createEefRobot(name, name);
+ detachChain(exampleHand);
+ int i = 0;
+ for (auto grasp : grasps) {
+ auto name = "grasp" + std::to_string(i++);
+ auto hand = exampleHand->clone();
+ auto globalPose = virtualObject->toGlobalCoordinateSystem(grasp);
+ hand->setGlobalPose(globalPose);
+ hands.push_back(hand);
+ }
+ }
+ return hands;
+}
+
+ChainedGrasp::VirtualJoint* ChainedGrasp::getVirtualJoint(const std::string &name) {
+ if (name == x.name) return &x;
+ else if (name == y.name) return &y;
+ else if (name == z.name) return &z;
+ else if (name == roll.name) return &roll;
+ else if (name == pitch.name) return &pitch;
+ else if (name == yaw.name) return &yaw;
+ else
+ {
+ VR_INFO << "No virtual joint called " << name << " in grasp" << std::endl;
+ return nullptr;
+ }
+}
+
+std::vector<float> ChainedGrasp::getUsedVirtualLimits() const {
+ std::vector<float> usedVirtualLimits;
+ x.addLimits(usedVirtualLimits);
+ y.addLimits(usedVirtualLimits);
+ z.addLimits(usedVirtualLimits);
+ roll.addLimits(usedVirtualLimits);
+ pitch.addLimits(usedVirtualLimits);
+ yaw.addLimits(usedVirtualLimits);
+ return usedVirtualLimits;
+}
+
+Eigen::Vector3f ChainedGrasp::getPositionXYZ() const {
+ return Eigen::Vector3f(x.value, y.value, z.value);
+}
+
+Eigen::Vector3f ChainedGrasp::getOrientationRPY() const {
+ return Eigen::Vector3f(roll.value, pitch.value, yaw.value);
+}
+
+Eigen::Matrix4f ChainedGrasp::getTransformation() const {
+ return getLocalPose() * graspableObjectCoordSysTransformation;
+}
+
+Eigen::Matrix4f ChainedGrasp::getLocalPoseGrasp() {
+ return getTransformation().inverse();
+}
+
+void ChainedGrasp::setObjectTransformation(const Eigen::Matrix4f &graspableObjectCoordSysTransformation) {
+ this->graspableObjectCoordSysTransformation = graspableObjectCoordSysTransformation;
+}
+
+std::string ChainedGrasp::getTransformationXML(const std::string &tabs) const {
+ std::stringstream ss;
+ std::string tt = tabs + "\t";
+ std::string ttt = tabs + "\t\t";
+ ss << tabs << "<ChainGrasp>\n";
+ ss << tt << x.toXML();
+ ss << tt << y.toXML();
+ ss << tt << z.toXML();
+ ss << tt << roll.toXML();
+ ss << tt << pitch.toXML();
+ ss << tt << yaw.toXML();
+ if (graspableObjectCoordSysTransformation != Eigen::Matrix4f::Identity()) {
+ ss << tt << "<Transform>\n";
+ ss << MathTools::getTransformXMLString(graspableObjectCoordSysTransformation, ttt);
+ ss << tt << "</Transform>\n";
+ }
+ ss << tabs << "</ChainGrasp>\n";
+ return ss.str();
+}
+
+GraspPtr ChainedGrasp::clone() const {
+ // TODO
+ VR_INFO << "Cloning chained grasp not yet implemented. Calling Grasp::clone()" << std::endl;
+ return Grasp::clone();
+}
+
+bool ChainedGrasp::visualizeRotationCoordSystem(RobotPtr robot, bool visible) {
+ auto node = getVirtualNode(robot, yaw.name);
+ if (node) {
+ node->showCoordinateSystem(visible);
+ return true;
+ }
+ return false;
+}
+
+std::string ChainedGrasp::ROBOT_NODE_NAME(const std::string &virtualJointName)
+{
+ std::stringstream ss;
+ ss << "virtual_";
+ ss << eef;
+ ss << "_";
+ ss << virtualJointName;
+ return ss.str();
+}
+
+EndEffectorPtr ChainedGrasp::getEndEffector(RobotPtr robot) {
+ if (robot->hasEndEffector(eef))
+ return robot->getEndEffector(eef);
+ else return nullptr;
+}
+
+RobotNodePtr ChainedGrasp::getTCP(RobotPtr robot) {
+ auto endEffector = getEndEffector(robot);
+ if (endEffector)
+ return endEffector->getTcp();
+ else
+ return nullptr;
+}
+
+std::vector<std::string> ChainedGrasp::getNames(bool onlyAdaptable) {
+ std::vector<std::string> names;
+ if (x.isUsed() || !onlyAdaptable) names.push_back(ROBOT_NODE_NAME(x.name));
+ if (y.isUsed() || !onlyAdaptable) names.push_back(ROBOT_NODE_NAME(y.name));
+ if (z.isUsed() || !onlyAdaptable) names.push_back(ROBOT_NODE_NAME(z.name));
+ if (roll.isUsed() || !onlyAdaptable) names.push_back(ROBOT_NODE_NAME(roll.name));
+ if (pitch.isUsed() || !onlyAdaptable) names.push_back(ROBOT_NODE_NAME(pitch.name));
+ if (yaw.isUsed() || !onlyAdaptable) names.push_back(ROBOT_NODE_NAME(yaw.name));
+ if (!onlyAdaptable) names.push_back(ROBOT_NODE_NAME("object"));
+ return names;
+}
+
+RobotNodeSetPtr ChainedGrasp::createRobotNodeSet(RobotPtr robot, RobotNodeSetPtr rns) {
+ auto virtualObject = getObjectNode(robot);
+ if (virtualObject) {
+ std::vector<std::string> nodeNames;
+ for (const std::string &name : rns->getNodeNames()) nodeNames.push_back(name);
+ //int index = nodeNames.size();
+ for (const std::string &name : getNames(true)) nodeNames.push_back(name);
+ /*SceneObjectPtr node = robot->getRobotNode(nodeNames.at(index));
+ while (node != robot->getRobotNode(nodeNames.at(index-1))) {
+ node = node->getParent();
+ nodeNames.insert(nodeNames.begin() + index, node->getName());
+ }*/
+ return RobotNodeSet::createRobotNodeSet(robot, this->name, nodeNames, rns->getKinematicRoot()->getName(), virtualObject->getName());
+ }
+ return nullptr;
+}
+
+
+// VirtualJoint
+
+bool ChainedGrasp::VirtualJoint::setValue(float value) {
+ if (isRevolute && (value < -M_PI || value > M_PI)) {
+ return false;
+ }
+ else if (min == max) {
+ min = value;
+ max = value;
+ }
+ else if (value < min || value > max) {
+ return false;
+ }
+ this->value = value;
+ return true;
+}
+
+void ChainedGrasp::VirtualJoint::resetLimits() {
+ min = value;
+ max = value;
+}
+
+bool ChainedGrasp::VirtualJoint::setLimitsValue(float min, float max) {
+ if (setLimits(min, max)) {
+ value = (max + min) / 2.0f;
+ return true;
+ }
+ return false;
+}
+
+bool ChainedGrasp::VirtualJoint::setLimits(float min, float max) {
+ if (min <= max) {
+ if (isRevolute) {
+ this->min = std::fmax(-M_PI, min);
+ this->max = std::fmin(M_PI, max);
+ }
+ else {
+ this->min = min;
+ this->max = max;
+ }
+ return true;
+ }
+ return false;
+}
+
+bool ChainedGrasp::VirtualJoint::setLimitless() {
+ if (isRevolute) {
+ this->min = -M_PI;
+ this->max = M_PI;
+ return true;
+ }
+ return false;
+}
+
+bool ChainedGrasp::VirtualJoint::isLimitless() {
+ return max - min > 2 * M_PI - 0.000001;
+}
+
+Eigen::Vector3f ChainedGrasp::VirtualJoint::getAxis() const {
+ Eigen::Vector3f axis(Eigen::Vector3f::Zero());
+ axis(this->axis) = 1;
+ return axis;
+}
+
+float ChainedGrasp::VirtualJoint::getMin() const {
+ return min;
+}
+
+float ChainedGrasp::VirtualJoint::getMax() const {
+ return max;
+}
+
+float ChainedGrasp::VirtualJoint::isUsed() const {
+ return std::abs(max - min) > 0.000001;
+}
+
+std::string ChainedGrasp::VirtualJoint::toXML() const {
+ std::stringstream ss;
+ ss << "<VirtualJoint name='" << name << "' value='" << value;
+ if (isUsed())
+ ss << "' min='" << min << "' max='" << max;
+ ss << "'/>\n";
+ return ss.str();
+}
+
+ChainedGrasp::VirtualJoint::VirtualJoint(const std::string &name, bool isRevolute, int axis) :
+ name(name),
+ value(0.0f),
+ min(0.0f),
+ max(0.0f),
+ isRevolute(isRevolute),
+ axis(axis)
+{
+}
+
+void ChainedGrasp::VirtualJoint::addLimits(std::vector<float> &usedLimits) const {
+ if (isUsed()) {
+ usedLimits.push_back(min);
+ usedLimits.push_back(max);
+ }
+}
+
+}
diff --git a/VirtualRobot/Grasping/ChainedGrasp.h b/VirtualRobot/Grasping/ChainedGrasp.h
new file mode 100644
index 0000000000000000000000000000000000000000..4667b0e72f6c4fbc73089675f8d5d54c80dd4c29
--- /dev/null
+++ b/VirtualRobot/Grasping/ChainedGrasp.h
@@ -0,0 +1,149 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Andre Meixner
+* @copyright 2020 Andre Meixner
+* GNU Lesser General Public License
+*
+*/
+
+#pragma once
+
+#include "VirtualRobot/VirtualRobot.h"
+
+#include "VirtualRobot/Nodes/RobotNodeRevolute.h"
+#include "VirtualRobot/Nodes/RobotNodePrismatic.h"
+#include "VirtualRobot/Nodes/RobotNodeFixed.h"
+
+#include <SimoxUtility/math/convert.h>
+
+#include "Grasp.h"
+
+namespace VirtualRobot
+{
+
+class VIRTUAL_ROBOT_IMPORT_EXPORT ChainedGrasp : public Grasp
+{
+public:
+ struct VirtualJoint
+ {
+ friend class ChainedGrasp;
+
+ bool setValue(float value);
+
+ void resetLimits();
+
+ bool setLimitsValue(float min, float max);
+
+ bool setLimits(float min, float max);
+
+ bool setLimitless();
+
+ bool isLimitless();
+
+ Eigen::Vector3f getAxis() const;
+
+ float getMin() const;
+
+ float getMax() const;
+
+ float isUsed() const;
+
+ std::string toXML() const;
+
+ private:
+ VirtualJoint(const std::string &name, bool isRevolute, int axis);
+
+ void addLimits(std::vector<float> &usedLimits) const;
+
+ std::string name;
+ float value;
+ float min;
+ float max;
+ bool isRevolute;
+ int axis;
+ };
+
+ ChainedGrasp(const std::string& name, const std::string& robotType, const std::string& eef,
+ const Eigen::Matrix4f& poseInTCPCoordSystem, const std::string& creation = "",
+ float quality = 0.0f, const std::string& eefPreshape = "");
+
+ virtual ~ChainedGrasp();
+
+ virtual void print(bool printDecoration = true) const override;
+
+ RobotNodePtr attachChain(RobotPtr robot, GraspableSensorizedObjectPtr object, bool addObjectVisualization = false);
+
+ void updateChain(RobotPtr robot);
+
+ void detachChain(RobotPtr robot);
+
+ void sampleGraspsUniform(std::vector<Eigen::Matrix4f> &grasps, RobotPtr robot, int grid);
+ std::vector<RobotPtr> sampleHandsUniform(RobotPtr robot, int grid);
+
+ VirtualJoint x;
+ VirtualJoint y;
+ VirtualJoint z;
+ VirtualJoint roll;
+ VirtualJoint pitch;
+ VirtualJoint yaw;
+
+ VirtualJoint* getVirtualJoint(const std::string &name);
+
+ std::vector<float> getUsedVirtualLimits() const;
+
+ Eigen::Vector3f getPositionXYZ() const;
+
+ Eigen::Vector3f getOrientationRPY() const;
+
+ virtual Eigen::Matrix4f getTransformation() const override;
+
+ /*! Returns hand pose in object coordinate system */
+ Eigen::Matrix4f getLocalPoseGrasp();
+
+ void setObjectTransformation(const Eigen::Matrix4f &graspableObjectCoordSysTransformation);
+
+ std::vector<std::string> getNames(bool onlyAdaptable = true);
+ RobotNodeSetPtr createRobotNodeSet(RobotPtr robot, RobotNodeSetPtr rns);
+
+ bool visualizeRotationCoordSystem(RobotPtr robot, bool visible = true);
+
+ virtual GraspPtr clone() const override;
+
+ std::string ROBOT_NODE_NAME(const std::string &virtualJointName);
+
+ RobotNodePtr getObjectNode(RobotPtr robot);
+ RobotNodePtr getVirtualNode(RobotPtr robot, const std::string &virtualName);
+
+ EndEffectorPtr getEndEffector(RobotPtr robot);
+ RobotNodePtr getTCP(RobotPtr robot);
+
+protected:
+ virtual std::string getTransformationXML(const std::string &tabs) const override;
+
+private:
+ Eigen::Matrix4f graspableObjectCoordSysTransformation;
+
+ RobotNodePtr attachChain(RobotNodePtr robotNode, GraspableSensorizedObjectPtr object, bool addObjectVisualization = false);
+ RobotNodePtr attach(VirtualJoint joint, RobotNodePtr robotNode);
+ bool update(VirtualJoint joint, RobotPtr robot);
+ void sampleGraspsUniform(std::vector<Eigen::Matrix4f> &grasps, const std::string &rootName, RobotNodePtr robotNode, int grid);
+
+ Eigen::Matrix4f getLocalPose() const;
+};
+
+}
diff --git a/VirtualRobot/Grasping/Grasp.cpp b/VirtualRobot/Grasping/Grasp.cpp
index 22738b27bc7c482b76107dd05a12e2afd62f4daf..24fe32581b35ecf566fe6863f43f4373ce60e210 100644
--- a/VirtualRobot/Grasping/Grasp.cpp
+++ b/VirtualRobot/Grasping/Grasp.cpp
@@ -24,7 +24,7 @@ namespace VirtualRobot
Grasp::~Grasp()
= default;
- void Grasp::print(bool printDecoration /*= true*/) const
+ void Grasp::print(bool printDecoration) const
{
if (printDecoration)
{
@@ -41,7 +41,7 @@ namespace VirtualRobot
// scope
std::ostringstream sos;
sos << std::setiosflags(std::ios::fixed);
- sos << " * Pose in EEF-TCP coordinate system:" << endl << poseTcp << std::endl;
+ sos << " * Pose in EEF-TCP coordinate system:" << endl << getTransformation() << std::endl;
std::cout << sos.str() << std::endl;
} // scope
@@ -81,7 +81,7 @@ namespace VirtualRobot
return Eigen::Matrix4f::Identity();
}
- return tcpNode->toGlobalCoordinateSystem(poseTcp);
+ return tcpNode->toGlobalCoordinateSystem(getTransformation());
}
std::string Grasp::getName() const
@@ -94,7 +94,7 @@ namespace VirtualRobot
return preshape;
}
- const Eigen::Matrix4f& Grasp::getTransformation() const
+ Eigen::Matrix4f Grasp::getTransformation() const
{
return poseTcp;
}
@@ -129,9 +129,7 @@ namespace VirtualRobot
ss << "' Preshape='" << preshape << "'>\n";
}
- ss << tt << "<Transform>\n";
- ss << MathTools::getTransformXMLString(poseTcp, ttt);
- ss << tt << "</Transform>\n";
+ ss << getTransformationXML(tt);
if (eefConfiguration.size() > 0)
{
@@ -144,6 +142,14 @@ namespace VirtualRobot
return ss.str();
}
+ std::string Grasp::getTransformationXML(const std::string &tabs) const {
+ std::stringstream ss;
+ ss << tabs << "<Transform>\n";
+ ss << MathTools::getTransformXMLString(poseTcp, tabs + "\t");
+ ss << tabs << "</Transform>\n";
+ return ss.str();
+ }
+
void Grasp::setTransformation(const Eigen::Matrix4f& tcp2Object)
{
poseTcp = tcp2Object;
@@ -151,7 +157,7 @@ namespace VirtualRobot
Eigen::Matrix4f Grasp::getTcpPoseGlobal(const Eigen::Matrix4f& objectPose) const
{
- Eigen::Matrix4f result = objectPose * poseTcp.inverse();
+ Eigen::Matrix4f result = objectPose * getTransformation().inverse();
return result;
}
@@ -162,13 +168,13 @@ namespace VirtualRobot
Eigen::Matrix4f Grasp::getObjectTargetPoseGlobal(const Eigen::Matrix4f& graspingPose) const
{
- Eigen::Matrix4f result = graspingPose * poseTcp;
+ Eigen::Matrix4f result = graspingPose * getTransformation();
return result;
}
VirtualRobot::GraspPtr Grasp::clone() const
{
- GraspPtr result(new Grasp(name, robotType, eef, poseTcp, creation, quality, preshape));
+ GraspPtr result(new Grasp(name, robotType, eef, getTransformation(), creation, quality, preshape));
result->setConfiguration(eefConfiguration);
return result;
}
diff --git a/VirtualRobot/Grasping/Grasp.h b/VirtualRobot/Grasping/Grasp.h
index 942612ca9b3bc3c7be949f6dd259cb3375aecb92..7b3810cc1926ee4b29de725c90340bf21bcdd642 100644
--- a/VirtualRobot/Grasping/Grasp.h
+++ b/VirtualRobot/Grasping/Grasp.h
@@ -57,7 +57,7 @@ namespace VirtualRobot
*/
virtual ~Grasp();
- void print(bool printDecoration = true) const;
+ virtual void print(bool printDecoration = true) const;
void setName(const std::string& name);
void setPreshape(const std::string& preshapeName);
@@ -105,11 +105,11 @@ namespace VirtualRobot
The transformation is given in the coordinate system of the tcp (whereas the tcp belongs to the eef).
This transformation specifies the tcp to object relation (object in tcp frame).
*/
- const Eigen::Matrix4f& getTransformation() const;
+ virtual Eigen::Matrix4f getTransformation() const;
std::string toXML(int tabs = 2) const;
- GraspPtr clone() const;
+ virtual GraspPtr clone() const;
//! Returns the (optionally) stored configuration of the fingers / actors.
std::map<std::string, float> getConfiguration() const;
@@ -119,6 +119,7 @@ namespace VirtualRobot
protected:
+ virtual std::string getTransformationXML(const std::string &tabs) const;
std::map< std::string, float > eefConfiguration; //!< Optional: the configuration of the actors.
diff --git a/VirtualRobot/Grasping/GraspEditor/GraspEditor.ui b/VirtualRobot/Grasping/GraspEditor/GraspEditor.ui
index 175fade6c804342fdab4bfe76c5eec598f177ad9..af158f6451af9f38b61c0d57ae83954e637932ff 100644
--- a/VirtualRobot/Grasping/GraspEditor/GraspEditor.ui
+++ b/VirtualRobot/Grasping/GraspEditor/GraspEditor.ui
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1079</width>
- <height>815</height>
+ <height>827</height>
</rect>
</property>
<property name="windowTitle">
@@ -15,20 +15,28 @@
</property>
<widget class="QWidget" name="centralwidget">
<layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QFrame" name="frameViewer">
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
<item>
<widget class="QSplitter" name="splitter">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
- <widget class="QFrame" name="frameViewer">
- <property name="frameShape">
- <enum>QFrame::StyledPanel</enum>
- </property>
- <property name="frameShadow">
- <enum>QFrame::Raised</enum>
- </property>
- </widget>
<widget class="QGroupBox" name="groupBox">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="maximumSize">
<size>
<width>200</width>
@@ -39,98 +47,82 @@
<string/>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="19" column="1">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>Ya</string>
- </property>
- </widget>
- </item>
- <item row="23" column="1" colspan="2">
- <widget class="QCheckBox" name="checkBoxTCP">
- <property name="text">
- <string>Show TCP Coord System</string>
- </property>
- </widget>
- </item>
- <item row="14" column="2">
- <widget class="QSlider" name="horizontalSliderX">
- <property name="minimum">
- <number>-50</number>
- </property>
- <property name="maximum">
- <number>50</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="19" column="2">
- <widget class="QSlider" name="horizontalSliderYa">
- <property name="minimum">
- <number>-50</number>
- </property>
- <property name="maximum">
- <number>50</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="2" column="1" colspan="2">
- <widget class="QPushButton" name="pushButtonLoadObject">
+ <item row="0" column="0">
+ <widget class="QPushButton" name="pushButtonReset">
<property name="text">
- <string>Select ManipulationObject File</string>
+ <string>Reset</string>
</property>
</widget>
</item>
- <item row="10" column="1" colspan="2">
- <widget class="QComboBox" name="comboBoxGrasp"/>
- </item>
- <item row="18" column="1">
- <widget class="QLabel" name="label_8">
- <property name="text">
- <string>Pi</string>
- </property>
+ <item row="7" column="0" colspan="2">
+ <widget class="QGroupBox" name="groupBox_3">
+ <property name="title">
+ <string>Options</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QCheckBox" name="checkBoxColModel">
+ <property name="text">
+ <string>Collision Model</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="checkBoxGraspSet">
+ <property name="text">
+ <string>Show Grasp Set</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="checkBoxTCP">
+ <property name="text">
+ <string>Show TCP Coord System</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <widget class="QSpinBox" name="grid">
+ <property name="minimum">
+ <number>1</number>
+ </property>
+ <property name="maximum">
+ <number>10</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_14">
+ <property name="text">
+ <string>Sample Grasp </string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
</widget>
</item>
- <item row="9" column="1" colspan="2">
- <widget class="QLabel" name="label_2">
+ <item row="9" column="0">
+ <widget class="QLabel" name="labelQuality">
<property name="text">
- <string>Grasp</string>
- </property>
- </widget>
- </item>
- <item row="17" column="2">
- <widget class="QSlider" name="horizontalSliderRo">
- <property name="minimum">
- <number>-50</number>
- </property>
- <property name="maximum">
- <number>50</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <string/>
</property>
</widget>
</item>
- <item row="24" column="1" colspan="2">
+ <item row="8" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Quality</string>
</property>
</widget>
</item>
- <item row="13" column="1" colspan="2">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>Move Grasp</string>
- </property>
- </widget>
- </item>
- <item row="26" column="1">
+ <item row="10" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
@@ -143,164 +135,423 @@
</property>
</spacer>
</item>
- <item row="16" column="1">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>Z</string>
- </property>
- </widget>
- </item>
- <item row="18" column="2">
- <widget class="QSlider" name="horizontalSliderPi">
- <property name="minimum">
- <number>-50</number>
- </property>
- <property name="maximum">
- <number>50</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="1" column="1" colspan="2">
- <widget class="QPushButton" name="pushButtonReset">
- <property name="text">
- <string>Reset</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1" colspan="2">
- <widget class="QLabel" name="label">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="pushButtonSave">
<property name="text">
- <string>End Effector</string>
+ <string>Save</string>
</property>
</widget>
</item>
- <item row="7" column="1" colspan="2">
- <widget class="QPushButton" name="pushButtonClose">
+ <item row="1" column="0" colspan="2">
+ <widget class="QPushButton" name="pushButtonLoadObject">
<property name="text">
- <string>Close EEF</string>
+ <string>Select Object</string>
</property>
</widget>
</item>
- <item row="3" column="1" colspan="2">
+ <item row="3" column="0" colspan="2">
<widget class="QPushButton" name="pushButtonLoadRobot">
<property name="text">
- <string>Select Robot File</string>
- </property>
- </widget>
- </item>
- <item row="6" column="1" colspan="2">
- <widget class="QComboBox" name="comboBoxEEF"/>
- </item>
- <item row="12" column="1" colspan="2">
- <widget class="QPushButton" name="pushButtonAddGrasp">
- <property name="text">
- <string>Add Grasp</string>
- </property>
- </widget>
- </item>
- <item row="14" column="1">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>X</string>
- </property>
- </widget>
- </item>
- <item row="16" column="2">
- <widget class="QSlider" name="horizontalSliderZ">
- <property name="minimum">
- <number>-50</number>
- </property>
- <property name="maximum">
- <number>50</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="17" column="1">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>Ro</string>
- </property>
- </widget>
- </item>
- <item row="20" column="1" colspan="2">
- <widget class="QLabel" name="label_10">
- <property name="text">
- <string>Visualization</string>
- </property>
- </widget>
- </item>
- <item row="11" column="1" colspan="2">
- <widget class="QPushButton" name="pushButtonRenameGrasp">
- <property name="text">
- <string>Rename Grasp</string>
- </property>
- </widget>
- </item>
- <item row="8" column="1" colspan="2">
- <widget class="QPushButton" name="pushButtonOpen">
- <property name="text">
- <string>Open EEF</string>
+ <string>Select Robot</string>
</property>
</widget>
</item>
- <item row="22" column="1" colspan="2">
- <widget class="QCheckBox" name="checkBoxGraspSet">
- <property name="text">
- <string>Show Grasp Set</string>
- </property>
- </widget>
- </item>
- <item row="15" column="2">
- <widget class="QSlider" name="horizontalSliderY">
- <property name="minimum">
- <number>-50</number>
- </property>
- <property name="maximum">
- <number>50</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="15" column="1">
- <widget class="QLabel" name="label_5">
- <property name="text">
- <string>Y</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1" colspan="2">
- <widget class="QPushButton" name="pushButtonSave">
- <property name="text">
- <string>Save</string>
+ <item row="5" column="0" colspan="2">
+ <widget class="QGroupBox" name="groupBox_4">
+ <property name="title">
+ <string>End Effector</string>
</property>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="pushButtonOpen">
+ <property name="text">
+ <string>Open</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButtonClose">
+ <property name="text">
+ <string>Close</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
- <item row="21" column="1" colspan="2">
- <widget class="QCheckBox" name="checkBoxColModel">
- <property name="text">
- <string>Collision Model</string>
- </property>
- </widget>
+ <item row="4" column="0" colspan="2">
+ <widget class="QComboBox" name="comboBoxEEF"/>
</item>
- <item row="25" column="1" colspan="2">
- <widget class="QLabel" name="labelQuality">
- <property name="text">
- <string/>
- </property>
- </widget>
+ <item row="2" column="0" colspan="2">
+ <widget class="QComboBox" name="comboBoxObject"/>
</item>
</layout>
</widget>
</widget>
</item>
+ <item>
+ <widget class="QGroupBox" name="groupBox_5">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="title">
+ <string>Grasp</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="3" column="1" colspan="2">
+ <widget class="QGroupBox" name="groupBox_6">
+ <property name="title">
+ <string>Value</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Y</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Z</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>X</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>Ro</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QDoubleSpinBox" name="minX">
+ <property name="minimum">
+ <double>-300.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>300.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QLabel" name="label_13">
+ <property name="text">
+ <string>Ya</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_12">
+ <property name="text">
+ <string>Pi</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QDoubleSpinBox" name="maxX">
+ <property name="minimum">
+ <double>-300.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>300.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QDoubleSpinBox" name="minY">
+ <property name="minimum">
+ <double>-300.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>300.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QDoubleSpinBox" name="maxY">
+ <property name="minimum">
+ <double>-300.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>300.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="minZ">
+ <property name="minimum">
+ <double>-300.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>300.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QDoubleSpinBox" name="maxZ">
+ <property name="minimum">
+ <double>-300.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>300.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QDoubleSpinBox" name="minRoll">
+ <property name="minimum">
+ <double>-3.140000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>3.140000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QDoubleSpinBox" name="maxRoll">
+ <property name="minimum">
+ <double>-3.140000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>3.140000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QDoubleSpinBox" name="minPitch">
+ <property name="minimum">
+ <double>-3.140000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>3.140000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QDoubleSpinBox" name="maxPitch">
+ <property name="minimum">
+ <double>-3.140000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>3.140000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QDoubleSpinBox" name="minYaw">
+ <property name="minimum">
+ <double>-3.140000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>3.140000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="2">
+ <widget class="QDoubleSpinBox" name="maxYaw">
+ <property name="minimum">
+ <double>-3.140000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>3.140000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QPushButton" name="pushButtonAddGrasp">
+ <property name="text">
+ <string>Add</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1" colspan="2">
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="title">
+ <string>Set transformation</string>
+ </property>
+ <layout class="QFormLayout" name="formLayout">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>X</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSlider" name="horizontalSliderX">
+ <property name="minimum">
+ <number>-50</number>
+ </property>
+ <property name="maximum">
+ <number>50</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Y</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QSlider" name="horizontalSliderY">
+ <property name="minimum">
+ <number>-50</number>
+ </property>
+ <property name="maximum">
+ <number>50</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Z</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QSlider" name="horizontalSliderZ">
+ <property name="minimum">
+ <number>-50</number>
+ </property>
+ <property name="maximum">
+ <number>50</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>Ro</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QSlider" name="horizontalSliderRo">
+ <property name="minimum">
+ <number>-50</number>
+ </property>
+ <property name="maximum">
+ <number>50</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>Pi</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QSlider" name="horizontalSliderPi">
+ <property name="minimum">
+ <number>-50</number>
+ </property>
+ <property name="maximum">
+ <number>50</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Ya</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QSlider" name="horizontalSliderYa">
+ <property name="minimum">
+ <number>-50</number>
+ </property>
+ <property name="maximum">
+ <number>50</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="pushButtonRenameGrasp">
+ <property name="text">
+ <string>Rename</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1" colspan="2">
+ <widget class="QComboBox" name="comboBoxGrasp"/>
+ </item>
+ <item row="4" column="1">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
+ </item>
</layout>
</widget>
<widget class="QMenuBar" name="menubar">
@@ -330,22 +581,6 @@
<tabstop>pushButtonReset</tabstop>
<tabstop>pushButtonLoadObject</tabstop>
<tabstop>pushButtonLoadRobot</tabstop>
- <tabstop>pushButtonSave</tabstop>
- <tabstop>comboBoxEEF</tabstop>
- <tabstop>pushButtonClose</tabstop>
- <tabstop>pushButtonOpen</tabstop>
- <tabstop>comboBoxGrasp</tabstop>
- <tabstop>pushButtonRenameGrasp</tabstop>
- <tabstop>pushButtonAddGrasp</tabstop>
- <tabstop>horizontalSliderX</tabstop>
- <tabstop>horizontalSliderY</tabstop>
- <tabstop>horizontalSliderZ</tabstop>
- <tabstop>horizontalSliderRo</tabstop>
- <tabstop>horizontalSliderPi</tabstop>
- <tabstop>horizontalSliderYa</tabstop>
- <tabstop>checkBoxColModel</tabstop>
- <tabstop>checkBoxGraspSet</tabstop>
- <tabstop>checkBoxTCP</tabstop>
</tabstops>
<resources/>
<connections/>
diff --git a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp
index 6192a0f33191792f1e6f3cc9c41159c338103e32..4433a028709771dff5498e2bf0633271c0e8b2c4 100644
--- a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp
+++ b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.cpp
@@ -3,13 +3,18 @@
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/ManipulationObject.h"
-#include "VirtualRobot/Grasping/Grasp.h"
+#include "VirtualRobot/Grasping/ChainedGrasp.h"
#include "VirtualRobot/Grasping/GraspSet.h"
#include "VirtualRobot/XML/ObjectIO.h"
#include "VirtualRobot/XML/RobotIO.h"
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h"
#include "VirtualRobot/SphereApproximator.h"
#include "VirtualRobot/Visualization/TriMeshModel.h"
+#include "VirtualRobot/Nodes/RobotNodeRevolute.h"
+#include "VirtualRobot/Nodes/RobotNodePrismatic.h"
+#include "VirtualRobot/RobotFactory.h"
+#include "Visualization/CoinVisualization/CoinVisualizationNode.h"
+#include <SimoxUtility/math/convert.h>
#include <QFileDialog>
#include <Eigen/Geometry>
@@ -65,7 +70,11 @@ namespace VirtualRobot
setupUI();
+ if (objectFile.empty())
+ objectFile = settings.value("object/path", "").toString().toStdString();
loadObject();
+ if (robotFile.empty())
+ this->robotFile = settings.value("robot/path", "").toString().toStdString();
loadRobot();
m_pExViewer->viewAll();
@@ -133,6 +142,7 @@ namespace VirtualRobot
connect(UI->pushButtonClose, SIGNAL(clicked()), this, SLOT(closeEEF()));
connect(UI->pushButtonOpen, SIGNAL(clicked()), this, SLOT(openEEF()));
connect(UI->pushButtonLoadRobot, SIGNAL(clicked()), this, SLOT(selectRobot()));
+ connect(UI->comboBoxObject, SIGNAL(activated(int)), this, SLOT(selectRobotObject(int)));
connect(UI->comboBoxEEF, SIGNAL(activated(int)), this, SLOT(selectEEF(int)));
connect(UI->comboBoxGrasp, SIGNAL(activated(int)), this, SLOT(selectGrasp(int)));
connect(UI->pushButtonAddGrasp, SIGNAL(clicked()), this, SLOT(addGrasp()));
@@ -145,8 +155,21 @@ namespace VirtualRobot
connect(UI->horizontalSliderRo, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectA()));
connect(UI->horizontalSliderPi, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectB()));
connect(UI->horizontalSliderYa, SIGNAL(sliderReleased()), this, SLOT(sliderReleased_ObjectG()));
+ connect(UI->minX, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->maxX, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->minY, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->maxY, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->minZ, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->maxZ, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->minRoll, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->maxRoll, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->minPitch, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->maxPitch, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->minYaw, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
+ connect(UI->maxYaw, SIGNAL(valueChanged(double)), this, SLOT(virtualJointValueChanged()));
connect(UI->checkBoxColModel, SIGNAL(clicked()), this, SLOT(buildVisu()));
connect(UI->checkBoxGraspSet, SIGNAL(clicked()), this, SLOT(buildVisu()));
+ connect(UI->grid, SIGNAL(valueChanged(int)), this, SLOT(sampleGrasps()));
// In case of embedded use of this program it should not be possible to load an object after the editor is started
@@ -199,13 +222,11 @@ namespace VirtualRobot
QMainWindow::closeEvent(event);
}
-
-
void GraspEditorWindow::buildVisu()
{
- if (visualizationRobot)
+ if (visualizationAll)
{
- visualizationRobot->highlight(false);
+ visualizationAll->highlight(false);
}
eefVisu->removeAllChildren();
@@ -213,45 +234,32 @@ namespace VirtualRobot
showCoordSystem();
SceneObject::VisualizationType colModel = (UI->checkBoxColModel->isChecked()) ? SceneObject::Collision : SceneObject::Full;
- if (!UI->checkBoxTCP->isChecked())
+ if (robotEEF)
{
- if (robotEEF)
- {
- visualizationRobot = robotEEF->getVisualization<CoinVisualization>(colModel);
- SoNode* visualisationNode = visualizationRobot->getCoinVisualization();
+ visualizationAll = robotEEF->getVisualization<CoinVisualization>(colModel);
+ SoNode* visualisationNode = visualizationAll->getCoinVisualization();
- if (visualisationNode)
- {
- eefVisu->addChild(visualisationNode);
- //visualizationRobot->highlight(true);
- }
- }
- }
- else
- {
- if (robotEEF && robotEEF_EEF)
+ if (visualisationNode)
{
- RobotNodePtr tcp = robotEEF_EEF->getTcp();
+ eefVisu->addChild(visualisationNode);
+ //visualizationRobot->highlight(true);
+ }
- if (tcp)
- {
- SoSeparator* res = new SoSeparator;
- eefVisu->addChild(res);
- Eigen::Matrix4f tcpGP = tcp->getGlobalPose();
- SoMatrixTransform* m = CoinVisualizationFactory::getMatrixTransformScaleMM2M(tcpGP);
- res->addChild(m);
- SoSeparator* co = CoinVisualizationFactory::CreateCoordSystemVisualization();
- res->addChild(co);
+ for (auto hand : hands) {
+ auto visHand = hand->getVisualization<CoinVisualization>(colModel);
+ SoNode* visHandNode = visHand->getCoinVisualization();
+ visHand->setTransparency(0.8);
+ if (visHandNode)
+ {
+ eefVisu->addChild(visHandNode);
}
}
}
objectSep->removeAllChildren();
-
if (object)
{
-
SoNode* visualisationNode = nullptr;
std::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject = object->getVisualization<CoinVisualization>(colModel);
if (visualizationObject)
@@ -262,7 +270,6 @@ namespace VirtualRobot
if (visualisationNode)
{
objectSep->addChild(visualisationNode);
- //visualizationObject->setTransparency(0.5);
}
}
@@ -271,12 +278,17 @@ namespace VirtualRobot
int GraspEditorWindow::main()
{
+ QCoreApplication::setOrganizationName("H2T");
+ QCoreApplication::setOrganizationDomain("h2t.anthropomatik.kit.edu");
+ QCoreApplication::setApplicationName("GraspEditor");
+ QCoreApplication::setAttribute(Qt::AA_UseDesktopOpenGL);
+ QSettings settings;
+
SoQt::show(this);
SoQt::mainLoop();
return 0;
}
-
void GraspEditorWindow::quit()
{
std::cout << "GraspEditorWindow: Closing" << std::endl;
@@ -292,6 +304,8 @@ namespace VirtualRobot
return;
}
robotFile = std::string(fi.toLatin1());
+
+ settings.setValue("robot/path", QString::fromStdString(robotFile));
loadRobot();
}
@@ -311,7 +325,7 @@ namespace VirtualRobot
dialog.setFileMode(QFileDialog::ExistingFile);
dialog.setAcceptMode(QFileDialog::AcceptOpen);
QStringList nameFilters;
- nameFilters << "Manipulation Object XML Files (*.xml *.moxml)"
+ nameFilters << "Manipulation Object / Robot XML Files (*.xml *.moxml)"
// << "XML Files (*.xml)"
<< "All Files (*.*)";
dialog.setNameFilters(nameFilters);
@@ -336,6 +350,7 @@ namespace VirtualRobot
if (s != "")
{
objectFile = s;
+ settings.setValue("object/path", QString::fromStdString(objectFile));
loadObject();
}
}
@@ -354,11 +369,19 @@ namespace VirtualRobot
QFileDialog dialog(this);
dialog.setFileMode(QFileDialog::AnyFile);
dialog.setAcceptMode(QFileDialog::AcceptSave);
- dialog.setDefaultSuffix("moxml");
QStringList nameFilters;
- nameFilters << "Manipulation Object XML Files (*.moxml)"
- << "XML Files (*.xml)"
+ if (!robotObject)
+ {
+ dialog.setDefaultSuffix("moxml");
+ nameFilters << "Manipulation Object XML Files (*.moxml)";
+ }
+ else
+ {
+ dialog.setDefaultSuffix("xml");
+ }
+ nameFilters << "XML Files (*.xml)"
<< "All Files (*.*)";
+
dialog.setNameFilters(nameFilters);
if (dialog.exec())
@@ -382,7 +405,15 @@ namespace VirtualRobot
try
{
- ok = ObjectIO::saveManipulationObject(object, objectFile);
+ if (robotObject)
+ {
+ ok = RobotIO::saveXML(robotObject, objectFile.filename(), objectFile.parent_path(), "", true, true, true, false);
+ }
+ else
+ {
+ ok = ObjectIO::saveManipulationObject(std::dynamic_pointer_cast<ManipulationObject>(object), objectFile);
+ }
+
}
catch (VirtualRobotException& e)
{
@@ -402,7 +433,7 @@ namespace VirtualRobot
{
std::cout << "Changes successful saved to " << objectFile << std::endl;
QMessageBox msgBox;
- msgBox.setText(QString::fromStdString("Changes successful saved to " + objectFile));
+ msgBox.setText(QString::fromStdString("Changes successful saved to " + objectFile.string()));
msgBox.setIcon(QMessageBox::Information);
msgBox.setStandardButtons(QMessageBox::Ok);
msgBox.setDefaultButton(QMessageBox::Ok);
@@ -452,11 +483,17 @@ namespace VirtualRobot
void GraspEditorWindow::selectEEF(int n)
{
+ hands.clear();
currentEEF.reset();
currentGraspSet.reset();
currentGrasp.reset();
eefVisu->removeAllChildren();
+
+ /*if (robotEEF && currentGrasp) {
+ currentGrasp->detachChain(robotEEF);
+ }*/
+
robotEEF.reset();
if (n < 0 || n >= (int)eefs.size() || !robot)
@@ -469,6 +506,7 @@ namespace VirtualRobot
currentEEF->print();
robotEEF = currentEEF->createEefRobot(currentEEF->getName(), currentEEF->getName());
+
//robotEEF->print();
robotEEF_EEF = robotEEF->getEndEffector(currentEEF->getName());
robotEEF_EEF->print();
@@ -480,25 +518,23 @@ namespace VirtualRobot
if (object)
{
currentGraspSet = object->getGraspSet(currentEEF);
-
- if (!currentGraspSet)
- {
- currentGraspSet.reset(new GraspSet(currentEEF->getName(), robot->getType(), currentEEF->getName()));
- currentGrasp.reset(new Grasp("Grasp 0", robot->getType(), currentEEF->getName(), Eigen::Matrix4f::Identity(), "GraspEditor"));
- currentGraspSet->addGrasp(currentGrasp);
- object->addGraspSet(currentGraspSet);
- }
-
- updateGraspBox();
- selectGrasp(0);
}
- else
- {
- updateGraspBox();
+
+ updateGraspBox();
+ selectGrasp(0);
+ if (object && currentGrasp) {
+ currentGrasp->attachChain(robotEEF, object, true);
}
+ sampleGrasps();
+ }
- buildVisu();
+ void GraspEditorWindow::selectRobotObject(int n)
+ {
+ if (!robotObject) return;
+
+ object = robotObject->getRobotNode(UI->comboBoxObject->itemText(n).toStdString());
+ selectEEF(0);
}
void GraspEditorWindow::selectGrasp(int n)
@@ -510,13 +546,12 @@ namespace VirtualRobot
return;
}
- currentGrasp = currentGraspSet->getGrasp(n);
+ currentGrasp = std::dynamic_pointer_cast<ChainedGrasp>(currentGraspSet->getGrasp(n));
- if (currentGrasp && robotEEF && robotEEF_EEF && object)
+ if (currentGrasp && robotEEF_EEF)
{
Eigen::Matrix4f gp;
- gp = currentGrasp->getTransformation().inverse();
- gp = object->toGlobalCoordinateSystem(gp);
+ gp = currentGrasp->getTransformation();
std::string preshape = currentGrasp->getPreshapeName();
if (!preshape.empty() && robotEEF_EEF->hasPreshape(preshape))
@@ -535,30 +570,50 @@ namespace VirtualRobot
void GraspEditorWindow::loadObject()
{
- objectSep->removeAllChildren();
std::cout << "Loading Object from " << objectFile << std::endl;
try
{
object = ObjectIO::loadManipulationObject(objectFile);
+ robotObject = nullptr;
}
catch (VirtualRobotException& e)
{
- std::cout << " ERROR while creating object" << std::endl;
- std::cout << e.what();
-
- if (embeddedGraspEditor)
+ // TODO: not pretty!
+ try {
+ robotObject = RobotIO::loadRobot(objectFile, RobotIO::eFullVisAsCol);
+ object = nullptr;
+ }
+ catch (VirtualRobotException& e)
{
- QMessageBox msgBox;
- msgBox.setText(QString::fromStdString(" ERROR while creating object."));
- msgBox.setInformativeText("Please select a valid manipulation file.");
- msgBox.setIcon(QMessageBox::Information);
- msgBox.setStandardButtons(QMessageBox::Ok);
- msgBox.setDefaultButton(QMessageBox::Ok);
- msgBox.exec();
+ std::cout << " ERROR while creating object" << std::endl;
+ std::cout << e.what();
+
+ if (embeddedGraspEditor)
+ {
+ QMessageBox msgBox;
+ msgBox.setText(QString::fromStdString(" ERROR while creating object."));
+ msgBox.setInformativeText("Please select a valid manipulation file.");
+ msgBox.setIcon(QMessageBox::Information);
+ msgBox.setStandardButtons(QMessageBox::Ok);
+ msgBox.setDefaultButton(QMessageBox::Ok);
+ msgBox.exec();
+ }
+
+ return;
}
+ }
- return;
+ UI->comboBoxObject->clear();
+
+ if (robotObject) {
+ for (auto robotNode : robotObject->getRobotNodes()) {
+ if (robotNode->getVisualization()) {
+ if (!object)
+ object = robotNode;
+ UI->comboBoxObject->addItem(QString::fromStdString(robotNode->getName()));
+ }
+ }
}
if (!object)
@@ -567,6 +622,7 @@ namespace VirtualRobot
return;
}
+
//object->print();
selectEEF(0);
@@ -601,9 +657,13 @@ namespace VirtualRobot
void GraspEditorWindow::closeEEF()
{
- if (robotEEF_EEF)
+ if (currentGrasp && robotEEF)
{
- robotEEF_EEF->closeActors(object);
+ auto virtual_object = currentGrasp->getObjectNode(robotEEF);
+ robotEEF_EEF->closeActors(virtual_object);
+ for (auto hand : hands) {
+ hand->getEndEffector(currentEEF->getName())->closeActors(virtual_object);
+ }
}
m_pExViewer->scheduleRedraw();
@@ -614,6 +674,9 @@ namespace VirtualRobot
if (robotEEF_EEF)
{
robotEEF_EEF->openActors();
+ for (auto hand : hands) {
+ hand->getEndEffector(currentEEF->getName())->openActors();
+ }
}
m_pExViewer->scheduleRedraw();
@@ -621,6 +684,8 @@ namespace VirtualRobot
void GraspEditorWindow::renameGrasp()
{
+ if (!currentGrasp) return;
+
bool ok;
QString text = QInputDialog::getText(this, tr("Rename Grasp"),
tr("New name:"), QLineEdit::Normal,
@@ -638,11 +703,17 @@ namespace VirtualRobot
void GraspEditorWindow::addGrasp()
{
- if (!object || !robot || !currentGraspSet)
+ if (!object || !robot)
{
return;
}
+ if (!currentGraspSet)
+ {
+ currentGraspSet.reset(new GraspSet(currentEEF->getName(), robot->getType(), currentEEF->getName()));
+ object->addGraspSet(currentGraspSet);
+ }
+
std::stringstream ss;
ss << "Grasp " << (currentGraspSet->getSize());
std::string name = ss.str();
@@ -651,13 +722,15 @@ namespace VirtualRobot
if (currentGrasp)
{
pose = currentGrasp->getTransformation();
+ if (robotEEF) currentGrasp->detachChain(robotEEF);
}
else
{
pose = Eigen::Matrix4f::Identity();
}
- GraspPtr g(new Grasp(name, robot->getType(), currentEEF->getName(), pose, std::string("GraspEditor")));
+ ChainedGraspPtr g(new ChainedGrasp(name, robot->getType(), currentEEF->getName(), pose, std::string("GraspEditor")));
+
currentGraspSet->addGrasp(g);
updateGraspBox();
UI->comboBoxGrasp->setCurrentIndex(UI->comboBoxGrasp->count() - 1);
@@ -667,16 +740,19 @@ namespace VirtualRobot
void GraspEditorWindow::updateEEF(float x[6])
{
- if (robotEEF)
+ if (currentGrasp && robotEEF)
{
+ auto virtual_object = currentGrasp->getObjectNode(robotEEF);
//cout << "getGlobalPose robot:" << endl << robotEEF->getGlobalPose() << std::endl;
//cout << "getGlobalPose TCP:" << endl << robotEEF_EEF->getTcp()->getGlobalPose() << std::endl;
- Eigen::Matrix4f m;
- MathTools::posrpy2eigen4f(x, m);
- RobotNodePtr tcp = robotEEF_EEF->getTcp();
- m = tcp->getGlobalPose() * m;
- //cout << "pose:" << endl << m << std::endl;
- setCurrentGrasp(m);
+ if (virtual_object) {
+ Eigen::Matrix4f m;
+ MathTools::posrpy2eigen4f(x, m);
+ Eigen::Matrix4f transformation = virtual_object->getLocalTransformation() * m;
+ virtual_object->setLocalTransformation(transformation);
+ currentGrasp->setObjectTransformation(transformation);
+ virtual_object->updatePose(false);
+ }
}
m_pExViewer->scheduleRedraw();
@@ -715,21 +791,23 @@ namespace VirtualRobot
void GraspEditorWindow::setCurrentGrasp(Eigen::Matrix4f& p)
{
- if (robotEEF && robotEEF_EEF && currentGrasp && object)
+ if (currentGrasp && robotEEF)
{
- RobotNodePtr tcp = robotEEF_EEF->getTcp();
- robotEEF->setGlobalPoseForRobotNode(tcp, p);
- Eigen::Matrix4f objP = object->getGlobalPose();
- Eigen::Matrix4f pLocal = tcp->toLocalCoordinateSystem(objP);
- currentGrasp->setTransformation(pLocal);
+ currentGrasp->attachChain(robotEEF, object, true);
+ auto virtual_object = currentGrasp->getObjectNode(robotEEF);
+ if (virtual_object) {
+ virtual_object->setLocalTransformation(p);
+ virtual_object->updatePose(false);
+ }
}
+ sampleGrasps();
m_pExViewer->scheduleRedraw();
}
void GraspEditorWindow::showCoordSystem()
{
- if (robotEEF && robotEEF_EEF)
+ if (robotEEF)
{
RobotNodePtr tcp = robotEEF_EEF->getTcp();
@@ -739,11 +817,9 @@ namespace VirtualRobot
}
tcp->showCoordinateSystem(UI->checkBoxTCP->isChecked());
- }
- if (object)
- {
- object->showCoordinateSystem(UI->checkBoxTCP->isChecked());
+ if (currentGrasp)
+ currentGrasp->visualizeRotationCoordSystem(robotEEF, UI->checkBoxTCP->isChecked());
}
}
@@ -765,4 +841,28 @@ namespace VirtualRobot
}
}
+ void GraspEditorWindow::virtualJointValueChanged() {
+ if (currentGrasp) {
+ currentGrasp->x.setLimitsValue(UI->minX->value(), UI->maxX->value());
+ currentGrasp->y.setLimitsValue(UI->minY->value(), UI->maxY->value());
+ currentGrasp->z.setLimitsValue(UI->minZ->value(), UI->maxZ->value());
+ currentGrasp->roll.setLimitsValue(UI->minRoll->value(), UI->maxRoll->value());
+ currentGrasp->pitch.setLimitsValue(UI->minPitch->value(), UI->maxPitch->value());
+ currentGrasp->yaw.setLimitsValue(UI->minYaw->value(), UI->maxYaw->value());
+ currentGrasp->updateChain(robotEEF);
+ sampleGrasps();
+ }
+ }
+
+ void GraspEditorWindow::sampleGrasps()
+ {
+ if (currentGrasp && robot)
+ {
+ int grid = UI->grid->value();
+ if (grid > 1) hands = currentGrasp->sampleHandsUniform(robotEEF, grid);
+ else hands.clear();
+ buildVisu();
+ }
+ }
+
}
diff --git a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h
index d6b5461ff6e4fa273b196395459ea96706f0e929..121e9a91d0d2f741b3f73c9d95c02a64f5b4f9dd 100644
--- a/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h
+++ b/VirtualRobot/Grasping/GraspEditor/GraspEditorWindow.h
@@ -2,6 +2,9 @@
#pragma once
#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Nodes/RobotNode.h>
@@ -21,7 +24,7 @@
#include <Inventor/Qt/SoQt.h>
#include <Inventor/nodes/SoSeparator.h>
-
+#include <filesystem>
#include <vector>
// #include "ui_GraspEditor.h"
@@ -59,6 +62,7 @@ namespace VirtualRobot
void selectRobot();
void selectObject(std::string file = "");
void saveObject();
+ void selectRobotObject(int n);
void selectEEF(int n);
void selectGrasp(int n);
@@ -75,6 +79,10 @@ namespace VirtualRobot
void sliderReleased_ObjectB();
void sliderReleased_ObjectG();
+ void sampleGrasps();
+
+ void virtualJointValueChanged();
+
void buildVisu();
void showCoordSystem();
@@ -110,22 +118,26 @@ namespace VirtualRobot
VirtualRobot::RobotPtr robot;
VirtualRobot::RobotPtr robotEEF;
- VirtualRobot::ManipulationObjectPtr object;
+ VirtualRobot::GraspableSensorizedObjectPtr object;
+ VirtualRobot::RobotPtr robotObject;
std::vector<VirtualRobot::EndEffectorPtr> eefs;
VirtualRobot::EndEffectorPtr currentEEF; // the eef of robot
VirtualRobot::EndEffectorPtr robotEEF_EEF; // the eef of robotEEF
VirtualRobot::GraspSetPtr currentGraspSet;
- VirtualRobot::GraspPtr currentGrasp;
+ VirtualRobot::ChainedGraspPtr currentGrasp;
std::string robotFile;
- std::string objectFile;
+ std::filesystem::path objectFile;
SoTimerSensor* timer;
- std::shared_ptr<VirtualRobot::CoinVisualization> visualizationRobot;
- std::shared_ptr<VirtualRobot::CoinVisualization> visualizationObject;
+ std::shared_ptr<VirtualRobot::CoinVisualization> visualizationAll;
+
+ std::vector<RobotPtr> hands;
+
+ QSettings settings;
};
}
diff --git a/VirtualRobot/ManipulationObject.cpp b/VirtualRobot/ManipulationObject.cpp
index 30def0c0d466b45fabedd0385a8965bcf032f2d6..a64158b79367e2d7685b9bff2eef3ea6c03d9e5b 100644
--- a/VirtualRobot/ManipulationObject.cpp
+++ b/VirtualRobot/ManipulationObject.cpp
@@ -2,7 +2,7 @@
#include "ManipulationObject.h"
#include "VirtualRobotException.h"
#include "Visualization/VisualizationNode.h"
-#include "Grasping/GraspSet.h"
+#include "GraspableSensorizedObject.h"
#include "XML/BaseIO.h"
namespace VirtualRobot
@@ -33,108 +33,14 @@ namespace VirtualRobot
Obstacle::print(false);
- for (size_t i = 0; i < graspSets.size(); i++)
- {
- std::cout << "* Grasp set " << i << ":" << std::endl;
- graspSets[i]->print();
- }
-
if (printDecoration)
{
std::cout << std::endl;
}
}
- void ManipulationObject::addGraspSet(GraspSetPtr graspSet)
- {
- THROW_VR_EXCEPTION_IF(!graspSet, "NULL data");
- THROW_VR_EXCEPTION_IF(hasGraspSet(graspSet), "Grasp set already added");
- // don't be too strict
- //THROW_VR_EXCEPTION_IF(hasGraspSet(graspSet->getRobotType(), graspSet->getEndEffector()), "Only one GraspSet per EEF allowed.");
- this->graspSets.push_back(graspSet);
- }
-
- void ManipulationObject::includeGraspSet(GraspSetPtr toBeIncludedGraspSet) //maybe delete
- {
- THROW_VR_EXCEPTION_IF(!toBeIncludedGraspSet, "NULL data");
- std::string robotType = toBeIncludedGraspSet->getRobotType();
- std::string eef = toBeIncludedGraspSet->getEndEffector();
-
- //include new Grasps
- //check if grasp is existing
- int index = -1;
- for (size_t i = 0 ; i < graspSets.size(); i++)
- {
- if (graspSets.at(i)->getRobotType() == robotType && graspSets.at(i)->getEndEffector() == eef)
- {
- index = i;
- }
- }
- THROW_VR_EXCEPTION_IF(index == -1, "Index wrong defined");
- graspSets.at(index)->includeGraspSet(toBeIncludedGraspSet);
- }
-
- bool ManipulationObject::hasGraspSet(GraspSetPtr graspSet)
- {
- VR_ASSERT_MESSAGE(graspSet, "NULL data");
-
- for (const auto& i : graspSets)
- if (i == graspSet)
- {
- return true;
- }
-
- return false;
- }
-
- bool ManipulationObject::hasGraspSet(const std::string& robotType, const std::string& eef)
- {
- for (auto& graspSet : graspSets)
- if (graspSet->getRobotType() == robotType && graspSet->getEndEffector() == eef)
- {
- return true;
- }
-
- return false;
- }
-
- VirtualRobot::GraspSetPtr ManipulationObject::getGraspSet(EndEffectorPtr eef)
- {
- THROW_VR_EXCEPTION_IF(!eef, "NULL data");
-
- return getGraspSet(eef->getRobotType(), eef->getName());
-
-
- }
- VirtualRobot::GraspSetPtr ManipulationObject::getGraspSet(const std::string& robotType, const std::string& eefName)
- {
- for (auto& graspSet : graspSets)
- if (graspSet->getRobotType() == robotType && graspSet->getEndEffector() == eefName)
- {
- return graspSet;
- }
-
- return GraspSetPtr();
- }
-
- VirtualRobot::GraspSetPtr ManipulationObject::getGraspSet(const std::string& name)
- {
- for (auto& graspSet : graspSets)
- if (graspSet->getName() == name)
- {
- return graspSet;
- }
-
- return GraspSetPtr();
- }
-
- std::vector<GraspSetPtr> ManipulationObject::getAllGraspSets()
- {
- return graspSets;
- }
-
- std::string ManipulationObject::toXML(const std::string& basePath, int tabs, bool storeLinkToFile)
+ std::string ManipulationObject::toXML(const std::string& basePath, int tabs, bool storeLinkToFile, const std::string& modelPathRelative, bool storeSensors)
{
std::stringstream ss;
std::string t = "\t";
@@ -170,13 +76,8 @@ namespace VirtualRobot
}
else
{
-
- ss << getSceneObjectXMLString(basePath, tabs + 1);
-
- for (auto& graspSet : graspSets)
- {
- ss << graspSet->getXMLString(tabs + 1) << "\n";
- }
+ ss << getSceneObjectXMLString(basePath, tabs + 1, modelPathRelative);
+ ss << getGraspableSensorizedObjectXML(modelPathRelative, storeSensors, tabs + 1);
}
ss << pre << "</ManipulationObject>\n";
@@ -184,17 +85,12 @@ namespace VirtualRobot
return ss.str();
}
- ManipulationObjectPtr ManipulationObject::clone(const std::string& name, CollisionCheckerPtr colChecker, bool deepVisuCopy) const
- {
- return ManipulationObjectPtr(_clone(name, colChecker, deepVisuCopy));
- }
-
ManipulationObjectPtr ManipulationObject::clone(CollisionCheckerPtr colChecker, bool deepVisuCopy) const
{
return clone(getName(), colChecker, deepVisuCopy);
}
- ManipulationObject* ManipulationObject::_clone(const std::string& name, CollisionCheckerPtr colChecker, bool deepVisuCopy) const
+ ManipulationObjectPtr ManipulationObject::clone(const std::string& name, CollisionCheckerPtr colChecker, bool deepVisuCopy) const
{
VisualizationNodePtr clonedVisualizationNode;
@@ -210,20 +106,12 @@ namespace VirtualRobot
clonedCollisionModel = collisionModel->clone(colChecker, 1.0, deepVisuCopy);
}
- ManipulationObject* result = new ManipulationObject(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker);
-
- if (!result)
- {
- VR_ERROR << "Cloning failed.." << std::endl;
- return result;
- }
+ ManipulationObjectPtr result(new ManipulationObject(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
result->setGlobalPose(getGlobalPose());
- for (const auto& graspSet : graspSets)
- {
- result->addGraspSet(graspSet->clone());
- }
+ appendSensorsTo(result);
+ appendGraspSetsTo(result);
return result;
}
diff --git a/VirtualRobot/ManipulationObject.h b/VirtualRobot/ManipulationObject.h
index f077e8c19f3fe3fbe28e33f0900f46899d9b21af..487e49ca480979ef260b364bce6afec39dd89c3d 100644
--- a/VirtualRobot/ManipulationObject.h
+++ b/VirtualRobot/ManipulationObject.h
@@ -46,44 +46,6 @@ namespace VirtualRobot
void print(bool printDecoration = true) override;
- bool hasGraspSet(GraspSetPtr graspSet);
- bool hasGraspSet(const std::string& robotType, const std::string& eef);
-
- /*!
- Appends a grasp set. Note, that only one grasp set per EEF is allowed.
- */
- void addGraspSet(GraspSetPtr graspSet);
-
- /*!
- * \brief includeGraspSet
- * \param graspSet
- */
- void includeGraspSet(GraspSetPtr graspSet);
-
- /*!
- Get grasp set for the given end effector. In case multiple grasp sets for the eef are present, the first one is returned.
- An empty GraspSetPtr is returned when no GraspSet for eef is found.
- */
- GraspSetPtr getGraspSet(EndEffectorPtr eef);
-
- /*!
- Get grasp set for the given robotType and end effector. In case multiple grasp sets for the robot/eef combination are present, the first one is returned.
- An empty GraspSetPtr is returned when no GraspSet for robot&eef is found.
- */
- GraspSetPtr getGraspSet(const std::string& robotType, const std::string& eefName);
-
- /*!
- Get grasp set by name.
- \param name The name of the grasp set.
- \return An empty GraspSetPtr is returned when no GraspSet with the given name is found.
- */
- GraspSetPtr getGraspSet(const std::string& name);
-
- /*!
- Get grasp set vector
- */
- std::vector<GraspSetPtr> getAllGraspSets();
-
/*!
Creates an XML representation of this object.
\param basePath If set, all visualization and collision model files are made relative to this path.
@@ -91,7 +53,7 @@ namespace VirtualRobot
\param storeLinkToFile If set, the data (e.g. grasps) are not explicitly listed, but an xml tag directing to the XML file,
from which this instance was loaded, is set. If not set a deep description is created.
*/
- virtual std::string toXML(const std::string& basePath = std::string(), int tabs = 0, bool storeLinkToFile = false);
+ virtual std::string toXML(const std::string& basePath = std::string(), int tabs = 0, bool storeLinkToFile = false, const std::string& modelPathRelative = "", bool storeSensors = true);
/*!
Clones this object. If no col checker is given, the one of the original object is used.
@@ -107,14 +69,6 @@ namespace VirtualRobot
*/
static ManipulationObjectPtr createFromMesh(TriMeshModelPtr mesh, std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
- protected:
-
- virtual ManipulationObject* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr(), bool deepVisuCopy = true) const;
-
-
- //std::string filename;
-
- std::vector< GraspSetPtr > graspSets;
};
} // namespace
diff --git a/VirtualRobot/Nodes/CameraSensor.cpp b/VirtualRobot/Nodes/CameraSensor.cpp
index 41138ee69f1d32ee3c1c7d35c13c8c14d7c833c6..283285e6903fc89c5080abc5c3c2818be467b4e8 100644
--- a/VirtualRobot/Nodes/CameraSensor.cpp
+++ b/VirtualRobot/Nodes/CameraSensor.cpp
@@ -7,11 +7,11 @@
namespace VirtualRobot
{
- CameraSensor::CameraSensor(RobotNodeWeakPtr robotNode,
+ CameraSensor::CameraSensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name,
VisualizationNodePtr visualization,
const Eigen::Matrix4f& rnTrafo
- ) : Sensor(robotNode, name, visualization, rnTrafo)
+ ) : Sensor(parentNode, name, visualization, rnTrafo)
{
}
@@ -33,12 +33,12 @@ namespace VirtualRobot
}
- SensorPtr CameraSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling)
+ SensorPtr CameraSensor::_clone(const GraspableSensorizedObjectPtr newParentNode, const VisualizationNodePtr visualizationModel, float scaling)
{
THROW_VR_EXCEPTION_IF(scaling < 0, "Scaling must be >0");
Eigen::Matrix4f rnt = rnTransformation;
rnt.block(0, 3, 3, 1) *= scaling;
- SensorPtr result(new CameraSensor(newRobotNode, name, visualizationModel, rnt));
+ SensorPtr result(new CameraSensor(newParentNode, name, visualizationModel, rnt));
return result;
}
diff --git a/VirtualRobot/Nodes/CameraSensor.h b/VirtualRobot/Nodes/CameraSensor.h
index 6250bd9a2634b83caf75cff1c6599a7e2a738a33..7376c5bff4f4d2ac6ec6383b3962d2b2d8a0dda8 100644
--- a/VirtualRobot/Nodes/CameraSensor.h
+++ b/VirtualRobot/Nodes/CameraSensor.h
@@ -45,7 +45,7 @@ namespace VirtualRobot
/*!
Constructor with settings.
*/
- CameraSensor(RobotNodeWeakPtr robotNode,
+ CameraSensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name,
VisualizationNodePtr visualization = VisualizationNodePtr(),
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()
@@ -73,7 +73,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
+ SensorPtr _clone(const GraspableSensorizedObjectPtr newNode, const VisualizationNodePtr visualizationModel, float scaling) override;
};
diff --git a/VirtualRobot/Nodes/CameraSensorFactory.cpp b/VirtualRobot/Nodes/CameraSensorFactory.cpp
index aa57f8fc188f5e24689c9f2c991b9cf3a38d5d26..af1c6119cf67df557166cb20f988626b95553120 100644
--- a/VirtualRobot/Nodes/CameraSensorFactory.cpp
+++ b/VirtualRobot/Nodes/CameraSensorFactory.cpp
@@ -24,7 +24,7 @@ namespace VirtualRobot
*
* \return instance of VirtualRobot::CameraSensor.
*/
- SensorPtr CameraSensorFactory::createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization,
+ SensorPtr CameraSensorFactory::createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr visualization,
const Eigen::Matrix4f& rnTrafo) const
{
SensorPtr Sensor(new CameraSensor(node, name, visualization, rnTrafo));
@@ -32,7 +32,7 @@ namespace VirtualRobot
return Sensor;
}
- SensorPtr CameraSensorFactory::createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode, const std::string basePath) const
+ SensorPtr CameraSensorFactory::createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription loadMode, const std::string basePath) const
{
THROW_VR_EXCEPTION_IF(!sensorXMLNode, "NULL data");
THROW_VR_EXCEPTION_IF(!node, "NULL data");
diff --git a/VirtualRobot/Nodes/CameraSensorFactory.h b/VirtualRobot/Nodes/CameraSensorFactory.h
index e902f37c83dd68e93d06687e031e20694c021eb0..e7afc729be870cede1b1b73d2e23610b97b0dd3b 100644
--- a/VirtualRobot/Nodes/CameraSensorFactory.h
+++ b/VirtualRobot/Nodes/CameraSensorFactory.h
@@ -40,13 +40,13 @@ namespace VirtualRobot
~CameraSensorFactory() override;
//! Standard init method
- SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/ContactSensor.cpp b/VirtualRobot/Nodes/ContactSensor.cpp
index 391e6792d6d5eb4e306614130159a06431449919..45138c6a789ca8d204ef8f681c01911c7bd985c7 100644
--- a/VirtualRobot/Nodes/ContactSensor.cpp
+++ b/VirtualRobot/Nodes/ContactSensor.cpp
@@ -6,9 +6,9 @@
namespace VirtualRobot
{
- ContactSensor::ContactSensor(RobotNodeWeakPtr robotNode,
+ ContactSensor::ContactSensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name)
- : Sensor(robotNode, name)
+ : Sensor(parentNode, name)
, timestamp(0.0)
{
}
@@ -28,9 +28,9 @@ namespace VirtualRobot
}
- SensorPtr ContactSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr /*visualizationModel*/, float /*scaling*/)
+ SensorPtr ContactSensor::_clone(const GraspableSensorizedObjectPtr newParentNode, const VisualizationNodePtr /*visualizationModel*/, float /*scaling*/)
{
- SensorPtr result(new ContactSensor(newRobotNode, name/*, rnTransformation*/));
+ SensorPtr result(new ContactSensor(newParentNode, name/*, rnTransformation*/));
return result;
}
diff --git a/VirtualRobot/Nodes/ContactSensor.h b/VirtualRobot/Nodes/ContactSensor.h
index 3a0137846027e791ad183db3fd2f06900da459a9..7b594e52006df074ef4154e98f49cccce0a128a1 100644
--- a/VirtualRobot/Nodes/ContactSensor.h
+++ b/VirtualRobot/Nodes/ContactSensor.h
@@ -57,7 +57,7 @@ namespace VirtualRobot
/*!
Constructor with settings.
*/
- ContactSensor(RobotNodeWeakPtr robotNode, const std::string& name);
+ ContactSensor(GraspableSensorizedObjectWeakPtr parentNode, const std::string& name);
/*!
*/
@@ -99,7 +99,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
+ SensorPtr _clone(const GraspableSensorizedObjectPtr newParentNode, const VisualizationNodePtr visualizationModel, float scaling) override;
ContactFrame frame;
double timestamp;
diff --git a/VirtualRobot/Nodes/ContactSensorFactory.cpp b/VirtualRobot/Nodes/ContactSensorFactory.cpp
index 2cc94d5d47d6851d718944055384da4aab0294cd..9b0a6ad39668d7c93ea19afe19c589513335f712 100644
--- a/VirtualRobot/Nodes/ContactSensorFactory.cpp
+++ b/VirtualRobot/Nodes/ContactSensorFactory.cpp
@@ -23,7 +23,7 @@ namespace VirtualRobot
*
* \return instance of VirtualRobot::ContactSensor.
*/
- SensorPtr ContactSensorFactory::createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr /*visualization*/,
+ SensorPtr ContactSensorFactory::createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr /*visualization*/,
const Eigen::Matrix4f& /*rnTrafo*/) const
{
SensorPtr Sensor(new ContactSensor(node, name));
@@ -31,7 +31,7 @@ namespace VirtualRobot
return Sensor;
}
- SensorPtr ContactSensorFactory::createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription /*loadMode*/, const std::string /*basePath*/) const
+ SensorPtr ContactSensorFactory::createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription /*loadMode*/, const std::string /*basePath*/) const
{
THROW_VR_EXCEPTION_IF(!sensorXMLNode, "NULL data");
THROW_VR_EXCEPTION_IF(!node, "NULL data");
diff --git a/VirtualRobot/Nodes/ContactSensorFactory.h b/VirtualRobot/Nodes/ContactSensorFactory.h
index dca2df91218d1714068223af05559c401a632f45..16f94fb7743d195b5c819ca370d27dc91904a30d 100644
--- a/VirtualRobot/Nodes/ContactSensorFactory.h
+++ b/VirtualRobot/Nodes/ContactSensorFactory.h
@@ -39,13 +39,13 @@ namespace VirtualRobot
~ContactSensorFactory() override;
//! Standard init method
- SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.cpp b/VirtualRobot/Nodes/ForceTorqueSensor.cpp
index 5fb80c5a1e3187b6d04b425654167c0c30e8961c..1279446cd493a45b4c27ebde0463ac2b26637747 100644
--- a/VirtualRobot/Nodes/ForceTorqueSensor.cpp
+++ b/VirtualRobot/Nodes/ForceTorqueSensor.cpp
@@ -8,10 +8,10 @@ namespace VirtualRobot
using std::cout;
using std::endl;
- ForceTorqueSensor::ForceTorqueSensor(RobotNodeWeakPtr robotNode,
+ ForceTorqueSensor::ForceTorqueSensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name,
const Eigen::Matrix4f& rnTrafo) :
- Sensor(robotNode, name, VisualizationNodePtr(), rnTrafo),
+ Sensor(parentNode, name, VisualizationNodePtr(), rnTrafo),
forceTorqueValues(6)
{
forceTorqueValues.setZero();
@@ -41,7 +41,6 @@ namespace VirtualRobot
Eigen::Vector3f torqueVector = forceTorqueValues.tail(3);
// project onto joint axis
- //RobotNodePtr rn(robotNode);
Eigen::Vector3f zAxis = this->globalPose.block(0, 2, 3, 1);
Eigen::Vector3f axisTorque = (torqueVector.dot(zAxis)) * zAxis;
@@ -61,12 +60,12 @@ namespace VirtualRobot
}
- SensorPtr ForceTorqueSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr /*visualizationModel*/, float scaling)
+ SensorPtr ForceTorqueSensor::_clone(const GraspableSensorizedObjectPtr newParentNode, const VisualizationNodePtr /*visualizationModel*/, float scaling)
{
THROW_VR_EXCEPTION_IF(scaling < 0, "Scaling must be >0");
Eigen::Matrix4f rnt = rnTransformation;
rnt.block(0, 3, 3, 1) *= scaling;
- ForceTorqueSensorPtr result(new ForceTorqueSensor(newRobotNode, name, rnt));
+ ForceTorqueSensorPtr result(new ForceTorqueSensor(newParentNode, name, rnt));
result->updateSensors(forceTorqueValues);
return result;
}
diff --git a/VirtualRobot/Nodes/ForceTorqueSensor.h b/VirtualRobot/Nodes/ForceTorqueSensor.h
index 7e86af9e76a38939dccb20fa3309e9d98e2abae8..772cf77f5e4a4dea7c7b3f9f8b4a97949f971137 100644
--- a/VirtualRobot/Nodes/ForceTorqueSensor.h
+++ b/VirtualRobot/Nodes/ForceTorqueSensor.h
@@ -47,7 +47,7 @@ namespace VirtualRobot
/*!
Constructor with settings.
*/
- ForceTorqueSensor(RobotNodeWeakPtr robotNode,
+ ForceTorqueSensor(GraspableSensorizedObjectWeakPtr node,
const std::string& name,
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()
);
@@ -84,7 +84,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
+ SensorPtr _clone(const GraspableSensorizedObjectPtr newParentNode, const VisualizationNodePtr visualizationModel, float scaling) override;
Eigen::VectorXf forceTorqueValues;
};
diff --git a/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp
index e11d57aa16ae3c890fe1dc3e984c6bf35f5313f1..92acc3f98b9a38ea4321c23dbf51ba92d28ab45d 100644
--- a/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp
+++ b/VirtualRobot/Nodes/ForceTorqueSensorFactory.cpp
@@ -24,7 +24,7 @@ namespace VirtualRobot
*
* \return instance of VirtualRobot::ForceTorqueSensor.
*/
- SensorPtr ForceTorqueSensorFactory::createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr /*visualization*/,
+ SensorPtr ForceTorqueSensorFactory::createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr /*visualization*/,
const Eigen::Matrix4f& rnTrafo) const
{
SensorPtr Sensor(new ForceTorqueSensor(node, name, rnTrafo));
@@ -32,7 +32,7 @@ namespace VirtualRobot
return Sensor;
}
- SensorPtr ForceTorqueSensorFactory::createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription /*loadMode*/, const std::string /*basePath*/) const
+ SensorPtr ForceTorqueSensorFactory::createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription /*loadMode*/, const std::string /*basePath*/) const
{
THROW_VR_EXCEPTION_IF(!sensorXMLNode, "NULL data");
THROW_VR_EXCEPTION_IF(!node, "NULL data");
diff --git a/VirtualRobot/Nodes/ForceTorqueSensorFactory.h b/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
index 129c29595b729b408b5be75110165f39054cf3cd..8c02ed2574a180c82d4cb946a32fc264ca1778c9 100644
--- a/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
+++ b/VirtualRobot/Nodes/ForceTorqueSensorFactory.h
@@ -38,13 +38,13 @@ namespace VirtualRobot
~ForceTorqueSensorFactory() override;
//! Standard init method
- SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/PositionSensor.cpp b/VirtualRobot/Nodes/PositionSensor.cpp
index 1773e94f601f1d21b8942816decb6b48c02cb48d..1c002acfcfa80426b286e49d512e71a72d6fb258 100644
--- a/VirtualRobot/Nodes/PositionSensor.cpp
+++ b/VirtualRobot/Nodes/PositionSensor.cpp
@@ -8,11 +8,11 @@
namespace VirtualRobot
{
- PositionSensor::PositionSensor(RobotNodeWeakPtr robotNode,
+ PositionSensor::PositionSensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name,
VisualizationNodePtr visualization,
const Eigen::Matrix4f& rnTrafo
- ) : Sensor(robotNode, name, visualization, rnTrafo)
+ ) : Sensor(parentNode, name, visualization, rnTrafo)
{
}
@@ -34,12 +34,12 @@ namespace VirtualRobot
}
- SensorPtr PositionSensor::_clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling)
+ SensorPtr PositionSensor::_clone(const GraspableSensorizedObjectPtr parentNode, const VisualizationNodePtr visualizationModel, float scaling)
{
THROW_VR_EXCEPTION_IF(scaling < 0, "Scaling must be >0");
Eigen::Matrix4f rnt = rnTransformation;
rnt.block(0, 3, 3, 1) *= scaling;
- SensorPtr result(new PositionSensor(newRobotNode, name, visualizationModel, rnt));
+ SensorPtr result(new PositionSensor(parentNode, name, visualizationModel, rnt));
return result;
}
diff --git a/VirtualRobot/Nodes/PositionSensor.h b/VirtualRobot/Nodes/PositionSensor.h
index 9c7b80032c63e02731b52e12f0c04ef9c755e0ee..356c7179b10db7646ff538c8b26b8594e410e5f5 100644
--- a/VirtualRobot/Nodes/PositionSensor.h
+++ b/VirtualRobot/Nodes/PositionSensor.h
@@ -46,7 +46,7 @@ namespace VirtualRobot
/*!
Constructor with settings.
*/
- PositionSensor(RobotNodeWeakPtr robotNode,
+ PositionSensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name,
VisualizationNodePtr visualization = VisualizationNodePtr(),
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()
@@ -73,7 +73,7 @@ namespace VirtualRobot
/*!
Derived classes must implement their clone method here.
*/
- SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
+ SensorPtr _clone(const GraspableSensorizedObjectPtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) override;
};
diff --git a/VirtualRobot/Nodes/PositionSensorFactory.cpp b/VirtualRobot/Nodes/PositionSensorFactory.cpp
index f97a9bc10c3a983910cba73559adb44e77f22b90..df8d20175094fe9bd1564974acb0d61548c85f8f 100644
--- a/VirtualRobot/Nodes/PositionSensorFactory.cpp
+++ b/VirtualRobot/Nodes/PositionSensorFactory.cpp
@@ -23,7 +23,7 @@ namespace VirtualRobot
*
* \return instance of VirtualRobot::PositionSensor.
*/
- SensorPtr PositionSensorFactory::createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization,
+ SensorPtr PositionSensorFactory::createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr visualization,
const Eigen::Matrix4f& rnTrafo) const
{
SensorPtr Sensor(new PositionSensor(node, name, visualization, rnTrafo));
@@ -31,7 +31,7 @@ namespace VirtualRobot
return Sensor;
}
- SensorPtr PositionSensorFactory::createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode, const std::string basePath) const
+ SensorPtr PositionSensorFactory::createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription loadMode, const std::string basePath) const
{
THROW_VR_EXCEPTION_IF(!sensorXMLNode, "NULL data");
THROW_VR_EXCEPTION_IF(!node, "NULL data");
diff --git a/VirtualRobot/Nodes/PositionSensorFactory.h b/VirtualRobot/Nodes/PositionSensorFactory.h
index ce6c71fa2ab8827739bb5bce64422aa4f0b442c9..72889c7273450d2c7dc89277a68c21a62dbea575 100644
--- a/VirtualRobot/Nodes/PositionSensorFactory.h
+++ b/VirtualRobot/Nodes/PositionSensorFactory.h
@@ -40,13 +40,13 @@ namespace VirtualRobot
~PositionSensorFactory() override;
//! Standard init method
- SensorPtr createSensor(RobotNodePtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(),
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()) const override;
/*!
Create sensor from XML tag.
*/
- SensorPtr createSensor(RobotNodePtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
+ SensorPtr createSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, BaseIO::RobotDescription loadMode = RobotIO::eFull, const std::string basePath = std::string()) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index f560e69d3204bf8cce6faecd26df27c22cf451b7..faa49eb77f3bec30356ad893915c5107739ec419 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -1,4 +1,3 @@
-
#include "RobotNode.h"
#include <VirtualRobot/VirtualRobotException.h>
@@ -12,7 +11,6 @@
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/XML/BaseIO.h>
-
#include <Eigen/Core>
#include <filesystem>
@@ -33,7 +31,7 @@ namespace VirtualRobot
const SceneObject::Physics& p,
CollisionCheckerPtr colChecker,
RobotNodeType type)
- : SceneObject(name, visualization, collisionModel, p, colChecker)
+ : GraspableSensorizedObject(name, visualization, collisionModel, p, colChecker)
{
@@ -88,12 +86,7 @@ namespace VirtualRobot
checkValidRobotNodeType();
- for (size_t i = 0; i < sensors.size(); i++)
- {
- sensors[i]->initialize(shared_from_this());
- }
-
- return SceneObject::initialize(parent, children);
+ return GraspableSensorizedObject::initialize(parent, children);
}
void RobotNode::checkValidRobotNodeType()
@@ -530,6 +523,9 @@ namespace VirtualRobot
float scaling,
bool preventCloningMeshesIfScalingIs1)
{
+ // If scaling is <= 0 this->scaling is used instead. This enables different scalings while still able to clone the robot
+ auto actualScaling = scaling > 0 ? scaling : this->scaling;
+
ReadLockPtr lock = getRobot()->getReadLock();
if (!newRobot)
@@ -545,17 +541,17 @@ namespace VirtualRobot
const bool deepMeshClone = !preventCloningMeshesIfScalingIs1 || std::abs(scaling - 1) <= 0;
if (visualizationModel)
{
- clonedVisualizationNode = visualizationModel->clone(deepMeshClone, scaling);
+ clonedVisualizationNode = visualizationModel->clone(deepMeshClone, actualScaling);
}
CollisionModelPtr clonedCollisionModel;
if (collisionModel)
{
- clonedCollisionModel = collisionModel->clone(colChecker, scaling, deepMeshClone);
+ clonedCollisionModel = collisionModel->clone(colChecker, actualScaling, deepMeshClone);
}
- RobotNodePtr result = _clone(newRobot, clonedVisualizationNode, clonedCollisionModel, colChecker, scaling);
+ RobotNodePtr result = _clone(newRobot, clonedVisualizationNode, clonedCollisionModel, colChecker, scaling > 0 ? scaling : 1.0f);
if (!result)
{
@@ -563,6 +559,16 @@ namespace VirtualRobot
return result;
}
+ if (!visualizationModelXML.empty())
+ {
+ result->visualizationModelXML = visualizationModelXML;
+ }
+
+ if (!collisionModelXML.empty())
+ {
+ result->collisionModelXML = collisionModelXML;
+ }
+
if (cloneChildren)
{
std::vector< SceneObjectPtr > children = this->getChildren();
@@ -580,26 +586,9 @@ namespace VirtualRobot
result->attachChild(c);
}
}
- else
- {
- SensorPtr s = std::dynamic_pointer_cast<Sensor>(children[i]);
-
- if (s)
- {
- // performs registering and initialization
- SensorPtr c = s->clone(result, scaling);
- }
- else
- {
- SceneObjectPtr so = children[i]->clone(children[i]->getName(), colChecker, scaling);
-
- if (so)
- {
- result->attachChild(so);
- }
- }
- }
}
+ appendSensorsTo(result);
+ appendGraspSetsTo(result);
}
result->setMaxVelocity(maxVelocity);
@@ -622,7 +611,8 @@ namespace VirtualRobot
{
result->initialize(initializeWithParent);
}
-
+ result->basePath = basePath;
+ result->setScaling(actualScaling);
return result;
}
@@ -1009,56 +999,7 @@ namespace VirtualRobot
}
}
- SensorPtr RobotNode::getSensor(const std::string& name) const
- {
- for (size_t i = 0; i < sensors.size(); i++)
- {
- if (sensors[i]->getName() == name)
- {
- return sensors[i];
- }
- }
-
- THROW_VR_EXCEPTION("No sensor with name" << name << " registerd at robot node " << getName());
- return SensorPtr();
- }
-
- bool RobotNode::hasSensor(const std::string& name) const
- {
- for (size_t i = 0; i < sensors.size(); i++)
- {
- if (sensors[i]->getName() == name)
- {
- return true;
- }
- }
-
- return false;
- }
-
- bool RobotNode::registerSensor(SensorPtr sensor)
- {
- if (!this->hasChild(sensor))
- {
- sensors.push_back(sensor);
- this->attachChild(sensor);
- }
-
- // if we are already initialized, be sure the sensor is also intialized
- if (initialized)
- {
- sensor->initialize(shared_from_this());
- }
-
- return true;
- }
-
- std::vector<SensorPtr> RobotNode::getSensors() const
- {
- return sensors;
- }
-
- std::string RobotNode::toXML(const std::string& basePath, const std::string& modelPathRelative /*= "models"*/, bool storeSensors)
+ std::string RobotNode::toXML(const std::string& basePath, const std::string& modelPathRelative, bool storeSensors, bool storeModelFiles)
{
std::stringstream ss;
ss << "\t<RobotNode name='" << name << "'>" << std::endl;
@@ -1081,32 +1022,30 @@ namespace VirtualRobot
if (visualizationModel && visualizationModel->getTriMeshModel() && visualizationModel->getTriMeshModel()->faces.size() > 0)
{
- std::string visuFile = getFilenameReplacementVisuModel();
+ if (storeModelFiles) {
+ std::string visuFile = getFilenameReplacementVisuModel();
- std::filesystem::path pModel(modelPathRelative);
- std::filesystem::path modelDirComplete = pBase / pModel;
- std::filesystem::path fn(visuFile);
- std::filesystem::path modelFileComplete = modelDirComplete / fn;
+ std::filesystem::path pModel(modelPathRelative);
+ std::filesystem::path modelDirComplete = pBase / pModel;
+ std::filesystem::path fn(visuFile);
+ std::filesystem::path modelFileComplete = modelDirComplete / fn;
- ss << visualizationModel->toXML(pBase.string(), modelFileComplete.string(), 2);
+ ss << visualizationModel->toXML(pBase.string(), modelFileComplete.string(), 2);
+ }
+ else ss << visualizationModel->toXML(basePath, 2);
}
if (collisionModel && collisionModel->getTriMeshModel() && collisionModel->getTriMeshModel()->faces.size() > 0)
{
- std::string colFile = getFilenameReplacementColModel();
- std::filesystem::path pModel(modelPathRelative);
- std::filesystem::path modelDirComplete = pBase / pModel;
- std::filesystem::path fn(colFile);
- std::filesystem::path modelFileComplete = modelDirComplete / fn;
- ss << collisionModel->toXML(pBase.string(), modelFileComplete.string(), 2);
- }
-
- if (storeSensors)
- {
- for (size_t i = 0; i < sensors.size(); i++)
- {
- ss << sensors[i]->toXML(modelPathRelative, 2);
+ if (storeModelFiles) {
+ std::string colFile = getFilenameReplacementColModel();
+ std::filesystem::path pModel(modelPathRelative);
+ std::filesystem::path modelDirComplete = pBase / pModel;
+ std::filesystem::path fn(colFile);
+ std::filesystem::path modelFileComplete = modelDirComplete / fn;
+ ss << collisionModel->toXML(pBase.string(), modelFileComplete.string(), 2);
}
+ else ss << collisionModel->toXML(basePath, 2);
}
std::vector<SceneObjectPtr> children = this->getChildren();
@@ -1122,6 +1061,8 @@ namespace VirtualRobot
}
}
+ ss << getGraspableSensorizedObjectXML(modelPathRelative, storeSensors, 2);
+
ss << "\t</RobotNode>\n\n";
return ss.str();
}
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 89ddb7531369cbdcbcfc301e6d18ce92e273c322..113b0d91515c6bb6211e07bd7f0d6a1f982a495d 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -26,7 +26,7 @@
#include "../VirtualRobot.h"
#include "../VirtualRobotException.h"
-#include "../SceneObject.h"
+#include "../GraspableSensorizedObject.h"
#include "../Transformation/DHParameter.h"
#include "Sensor.h"
@@ -48,7 +48,7 @@ namespace VirtualRobot
The visualization (of limb and/or coordinateAxis) is linked to the local coordinate sysytem of this joint: localTransformation*jointTransformation
The global pose of this joint is the same: localTransformation*jointTransformation
*/
- class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNode : public SceneObject
+ class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNode : public GraspableSensorizedObject
{
public:
friend class Robot;
@@ -191,7 +191,7 @@ namespace VirtualRobot
\p enable Show or hide coordinate system
\p scaling Size of coordinate system
\p text Text to display at coordinate system. If not given, the name of this robot node will be displayed.
- \p visualizationType This option is only needed when the current robot node does not yet own a visualization.
+ \p izationType This option is only needed when the current robot node does not yet own a visualization.
Then a visualziationNode has to be built and the \p visualizationType specifies which type of visualization should be used.
If not given, the first registered visaulizationfactory is used.
*/
@@ -378,17 +378,11 @@ namespace VirtualRobot
*/
virtual void propagateJointValue(const std::string& jointName, float factor = 1.0f);
-
- virtual SensorPtr getSensor(const std::string& name) const;
- virtual bool hasSensor(const std::string& name) const;
- virtual std::vector<SensorPtr> getSensors() const;
- virtual bool registerSensor(SensorPtr sensor);
-
/*!
Creates an XML string that defines the robotnode. Filenames of all visualization models are set to modelPath/RobotNodeName_visu and/or modelPath/RobotNodeName_colmodel.
@see RobotIO::saveXML.
*/
- virtual std::string toXML(const std::string& basePath, const std::string& modelPathRelative = "models", bool storeSensors = true);
+ virtual std::string toXML(const std::string& basePath, const std::string& modelPathRelative = "models", bool storeSensors = true, bool storeModelFiles = true);
/*!
Set the local transformation matrix that is used in this node.
@@ -454,8 +448,6 @@ namespace VirtualRobot
RobotNodeType nodeType;
- std::vector<SensorPtr> sensors;
-
float jointValue; //< The joint value
/*!
diff --git a/VirtualRobot/Nodes/Sensor.cpp b/VirtualRobot/Nodes/Sensor.cpp
index 74464323125c7f4c3ca622ec3ffd729fc6ab0d3c..4479b2cb9ec67d59268c95cbb508705373009588 100644
--- a/VirtualRobot/Nodes/Sensor.cpp
+++ b/VirtualRobot/Nodes/Sensor.cpp
@@ -4,8 +4,8 @@
#include "../VirtualRobotException.h"
#include "../XML/BaseIO.h"
#include "../Visualization/TriMeshModel.h"
-#include "RobotNode.h"
#include <VirtualRobot/Visualization/VisualizationNode.h>
+#include "Grasping/GraspSet.h"
#include <Eigen/Core>
@@ -16,13 +16,13 @@ namespace VirtualRobot
using std::cout;
using std::endl;
- Sensor::Sensor(RobotNodeWeakPtr robotNode,
+ Sensor::Sensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name,
VisualizationNodePtr visualization,
const Eigen::Matrix4f& rnTrafo
) : SceneObject(name, visualization)
{
- this->robotNode = robotNode;
+ this->parentNode = parentNode;
rnTransformation = rnTrafo;
}
@@ -33,27 +33,30 @@ namespace VirtualRobot
bool Sensor::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children)
{
- RobotNodePtr rn = robotNode.lock();
- THROW_VR_EXCEPTION_IF(!rn, "Could not init Sensor without robotnode");
+ GraspableSensorizedObjectPtr node = parentNode.lock();
+ THROW_VR_EXCEPTION_IF(!node, "Could not init Sensor without parent node");
- // robotnode
- if (!rn->hasSensor(name))
+ if (!node->hasSensor(name))
{
- rn->registerSensor(std::static_pointer_cast<Sensor>(shared_from_this()));
+ node->registerSensor(std::static_pointer_cast<Sensor>(shared_from_this()));
}
return SceneObject::initialize(parent, children);
}
- RobotNodePtr Sensor::getRobotNode() const
+ RobotNodePtr Sensor::getRobotNode() const {
+ return std::dynamic_pointer_cast<RobotNode>(getParentNode());
+ }
+
+ GraspableSensorizedObjectPtr Sensor::getParentNode() const
{
- RobotNodePtr result(robotNode);
+ GraspableSensorizedObjectPtr result(parentNode);
return result;
}
void Sensor::setGlobalPose(const Eigen::Matrix4f& /*pose*/)
{
- THROW_VR_EXCEPTION("Use setJointValues of parent RobotNode to control the position of a Sensor");
+ THROW_VR_EXCEPTION("Use parent node to control the position of a Sensor");
}
void Sensor::print(bool printChildren, bool printDecoration) const
@@ -100,7 +103,7 @@ namespace VirtualRobot
// scope1
std::ostringstream sos;
sos << std::setiosflags(std::ios::fixed);
- sos << "* RobotNode to sensor transformation:" << endl << rnTransformation << std::endl;
+ sos << "* Parent node to sensor transformation:" << endl << rnTransformation << std::endl;
sos << "* globalPose:" << endl << getGlobalPose() << std::endl;
std::cout << sos.str();
} // scope1
@@ -121,11 +124,11 @@ namespace VirtualRobot
}
}
- SensorPtr Sensor::clone(RobotNodePtr newRobotNode, float scaling)
+ SensorPtr Sensor::clone(GraspableSensorizedObjectPtr newNode, float scaling)
{
- if (!newRobotNode)
+ if (!newNode)
{
- VR_ERROR << "Attempting to clone Sensor for invalid robot node";
+ VR_ERROR << "Attempting to clone Sensor for invalid parent node";
return SensorPtr();
}
@@ -138,7 +141,7 @@ namespace VirtualRobot
}
- SensorPtr result = _clone(newRobotNode, clonedVisualizationNode, scaling);
+ SensorPtr result = _clone(newNode, clonedVisualizationNode, scaling);
if (!result)
{
@@ -146,9 +149,9 @@ namespace VirtualRobot
return result;
}
- newRobotNode->registerSensor(result);
+ newNode->registerSensor(result);
- result->initialize(newRobotNode);
+ result->initialize(newNode);
return result;
}
diff --git a/VirtualRobot/Nodes/Sensor.h b/VirtualRobot/Nodes/Sensor.h
index 8e65e25c1815844decf7bdcaa93fc746da77c8fc..29220830c6a042143caf3fa729d0b3d34dc5fdb0 100644
--- a/VirtualRobot/Nodes/Sensor.h
+++ b/VirtualRobot/Nodes/Sensor.h
@@ -41,7 +41,7 @@ namespace VirtualRobot
/*!
- A sensor can be attached to a RobotNode.
+ A sensor can be attached to a GraspableSensorizedObject.
*/
class VIRTUAL_ROBOT_IMPORT_EXPORT Sensor : public SceneObject
{
@@ -53,7 +53,7 @@ namespace VirtualRobot
/*!
Constructor with settings.
*/
- Sensor(RobotNodeWeakPtr robotNode,
+ Sensor(GraspableSensorizedObjectWeakPtr parentNode,
const std::string& name,
VisualizationNodePtr visualization = VisualizationNodePtr(),
const Eigen::Matrix4f& rnTrafo = Eigen::Matrix4f::Identity()
@@ -63,14 +63,15 @@ namespace VirtualRobot
*/
~Sensor() override;
-
+ /*! returns the parent as robot node if it is a robot node otherwise a nullptr */
RobotNodePtr getRobotNode() const;
+ GraspableSensorizedObjectPtr getParentNode() const;
/*!
- The transformation that specifies the pose of the sensor relatively to the pose of the parent RobotNode.
+ The transformation that specifies the pose of the sensor relatively to the pose of the parent node.
*/
- virtual Eigen::Matrix4f getRobotNodeToSensorTransformation()
+ virtual Eigen::Matrix4f getParentNodeToSensorTransformation()
{
return rnTransformation;
}
@@ -81,7 +82,7 @@ namespace VirtualRobot
virtual void setRobotNodeToSensorTransformation(const Eigen::Matrix4f& t);
/*!
- Calling this SceneObject method will cause an exception, since Sensors are controlled via their RobotNode parent.
+ Calling this SceneObject method will cause an exception, since Sensors are controlled via their node parent.
*/
void setGlobalPose(const Eigen::Matrix4f& pose) override;
@@ -93,13 +94,13 @@ namespace VirtualRobot
/*!
Clone this Sensor.
- \param newRobotNode The newly created Sensor belongs to newRobotNode.
+ \param newNode The newly created Sensor belongs to newNode.
\param scaling Scales the visualization and transformation data.
*/
- virtual SensorPtr clone(RobotNodePtr newRobotNode, float scaling = 1.0f);
+ virtual SensorPtr clone(GraspableSensorizedObjectPtr newNode, float scaling = 1.0f);
- //! Forbid cloning method from SceneObject. We need to know the new robotnode for cloning
+ //! Forbid cloning method from SceneObject. We need to know the new parent node for cloning
SceneObjectPtr clone(const std::string& /*name*/, CollisionCheckerPtr /*colChecker*/ = CollisionCheckerPtr(), float /*scaling*/ = 1.0f) const
{
THROW_VR_EXCEPTION("Cloning not allowed this way...");
@@ -127,13 +128,13 @@ namespace VirtualRobot
Eigen::Matrix4f rnTransformation; //<! Transformation from parent's coordinate system to this sensor
- RobotNodeWeakPtr robotNode;
+ GraspableSensorizedObjectWeakPtr parentNode;
/*!
Derived classes must implement their clone method here.
The visualization is already scaled, the kinematic information (i.e. transformations) have to be scaled by derived implementations.
*/
- virtual SensorPtr _clone(const RobotNodePtr newRobotNode, const VisualizationNodePtr visualizationModel, float scaling) = 0;
+ virtual SensorPtr _clone(const GraspableSensorizedObjectPtr newNode, const VisualizationNodePtr visualizationModel, float scaling) = 0;
SceneObject* _clone(const std::string& /*name*/, CollisionCheckerPtr /*colChecker*/ = CollisionCheckerPtr(), float /*scaling*/ = 1.0f) const override
{
diff --git a/VirtualRobot/Nodes/SensorFactory.h b/VirtualRobot/Nodes/SensorFactory.h
index bb6c792816ab5b5b69d50620e501643295e38e64..1c25bdb53efa27bb65d5c9c01042247b9b4d9bfe 100644
--- a/VirtualRobot/Nodes/SensorFactory.h
+++ b/VirtualRobot/Nodes/SensorFactory.h
@@ -26,7 +26,7 @@
#include "../VirtualRobot.h"
#include "../AbstractFactoryMethod.h"
#include "Sensor.h"
-#include "RobotNode.h"
+#include "GraspableSensorizedObject.h"
#include "../XML/RobotIO.h"
#include <Eigen/Core>
@@ -56,7 +56,7 @@ namespace VirtualRobot
}
//! Standard init method
- virtual SensorPtr createSensor(RobotNodePtr /*node*/, const std::string& /*name*/, VisualizationNodePtr /*visualization*/ = VisualizationNodePtr(),
+ virtual SensorPtr createSensor(GraspableSensorizedObjectPtr /*node*/, const std::string& /*name*/, VisualizationNodePtr /*visualization*/ = VisualizationNodePtr(),
const Eigen::Matrix4f& /*rnTrafo*/ = Eigen::Matrix4f::Identity()) const
{
return SensorPtr();
@@ -65,7 +65,7 @@ namespace VirtualRobot
/*!
Create sensor from XML tag. Factories of custom sensors can initialize with this method.
*/
- virtual SensorPtr createSensor(RobotNodePtr /*node*/, rapidxml::xml_node<char>* /*sensorXMLNode*/, RobotIO::RobotDescription /*loadMode*/ = RobotIO::eFull, const std::string /*basePath*/ = std::string()) const
+ virtual SensorPtr createSensor(GraspableSensorizedObjectPtr /*node*/, rapidxml::xml_node<char>* /*sensorXMLNode*/, BaseIO::RobotDescription /*loadMode*/ = RobotIO::eFull, const std::string /*basePath*/ = std::string()) const
{
return SensorPtr();
}
diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index 34d8518cbe3049e1f5b8dbf7dd215fad3335ba88..3881df846ded2e30bb40be1cf1958af5f443d8ef 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -16,7 +16,7 @@ namespace VirtualRobot
int Obstacle::idCounter = 20000;
Obstacle::Obstacle(const std::string& name, VisualizationNodePtr visualization, CollisionModelPtr collisionModel, const SceneObject::Physics& p, CollisionCheckerPtr colChecker)
- : SceneObject(name, visualization, collisionModel, p, colChecker)
+ : GraspableSensorizedObject(name, visualization, collisionModel, p, colChecker)
{
if (name == "")
{
@@ -274,13 +274,15 @@ namespace VirtualRobot
SceneObject::print(false);
std::cout << " * id: " << id << std::endl;
+ printGrasps();
+
if (printDecoration)
{
std::cout << std::endl;
}
}
- Obstacle* Obstacle::_clone(const std::string& name, CollisionCheckerPtr colChecker, float scaling) const
+ ObstaclePtr Obstacle::clone(const std::string& name, CollisionCheckerPtr colChecker, float scaling) const
{
VisualizationNodePtr clonedVisualizationNode;
@@ -296,20 +298,17 @@ namespace VirtualRobot
clonedCollisionModel = collisionModel->clone(colChecker, scaling);
}
- Obstacle* result = new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker);
-
- if (!result)
- {
- VR_ERROR << "Cloning failed.." << std::endl;
- return result;
- }
+ ObstaclePtr result(new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
result->setGlobalPose(getGlobalPose());
+ appendSensorsTo(result);
+ appendGraspSetsTo(result);
+
return result;
}
- std::string Obstacle::toXML(const std::string& basePath, int tabs)
+ std::string Obstacle::toXML(const std::string& basePath, int tabs, const std::string& modelPathRelative, bool storeSensors)
{
std::stringstream ss;
std::string t = "\t";
@@ -322,7 +321,9 @@ namespace VirtualRobot
ss << pre << "<Obstacle name='" << name << "'>\n";
- ss << getSceneObjectXMLString(basePath, tabs);
+
+ ss << getSceneObjectXMLString(basePath, tabs, modelPathRelative);
+ ss << getGraspableSensorizedObjectXML(modelPathRelative, storeSensors, tabs + 1);
ss << pre << "</Obstacle>\n";
diff --git a/VirtualRobot/Obstacle.h b/VirtualRobot/Obstacle.h
index 62c6e881638882f3fa3214a7e07ba4adc9f7f88d..540aca9148c0b5e7c23604349712d301c181f739 100644
--- a/VirtualRobot/Obstacle.h
+++ b/VirtualRobot/Obstacle.h
@@ -25,7 +25,7 @@
#include "VirtualRobot.h"
#include "CollisionDetection/CollisionModel.h"
#include "Visualization/VisualizationFactory.h"
-#include "SceneObject.h"
+#include "GraspableSensorizedObject.h"
#include <string>
#include <vector>
@@ -37,7 +37,7 @@ namespace VirtualRobot
An obstacle is an object that owns a visualization and a collision model.
It can be moved around and used for collision detection.
*/
- class VIRTUAL_ROBOT_IMPORT_EXPORT Obstacle : public SceneObject
+ class VIRTUAL_ROBOT_IMPORT_EXPORT Obstacle : public GraspableSensorizedObject
{
public:
@@ -61,10 +61,7 @@ namespace VirtualRobot
/*!
Clones this object. If no col checker is given, the one of the original object is used.
*/
- ObstaclePtr clone(const std::string& name, CollisionCheckerPtr colChecker = {}) const
- {
- return ObstaclePtr(_clone(name, colChecker));
- }
+ ObstaclePtr clone(const std::string& name, CollisionCheckerPtr colChecker = {}, float scaling = 1.0) const;
int getID();
@@ -111,15 +108,13 @@ namespace VirtualRobot
static ObstaclePtr createFromMesh(TriMeshModelPtr mesh,
std::string visualizationType = "", CollisionCheckerPtr colChecker = {});
- virtual std::string toXML(const std::string& basePath, int tabs = 0);
+ virtual std::string toXML(const std::string& basePath, int tabs = 0, const std::string& modelPathRelative = "", bool storeSensors = true);
void setFilename(const std::string& filename);
std::string getFilename() const;
protected:
- Obstacle* _clone(const std::string& name, CollisionCheckerPtr colChecker = {}, float scaling = 1.0f) const override;
-
std::string filename;
// a counter for internal ids
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index df5def540bf145ec31ad0bcc2f0a000878b9ab25..c043284344f53dbaeee0ffccbaaf5e75a97c0f94 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -9,6 +9,7 @@
#include "CollisionDetection/CollisionChecker.h"
#include "CollisionDetection/CollisionModel.h"
#include "EndEffector/EndEffector.h"
+#include "Grasping/GraspSet.h"
#include "math/Helpers.h"
@@ -20,7 +21,6 @@ namespace VirtualRobot
const RobotPtr Robot::NullPtr{nullptr};
Robot::Robot(const std::string& name, const std::string& type)
: SceneObject(name)
- , scaling(1.0f)
{
this->type = type;
updateVisualization = true;
@@ -771,7 +771,8 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION_IF(!startJoint, " StartJoint is nullptr");
THROW_VR_EXCEPTION_IF(!hasRobotNode(startJoint), " StartJoint '" + startJoint->getName() + "' is not part of this robot '" + getName() + "'");
- THROW_VR_EXCEPTION_IF(scaling <= 0, " Scaling must be >0.");
+ //THROW_VR_EXCEPTION_IF(scaling <= 0, " Scaling must be >0.");
+ // If scaling is <= 0 this->scaling is used instead. This enables different scalings while still able to clone the robot
CollisionCheckerPtr colChecker = collisionChecker;
@@ -786,7 +787,7 @@ namespace VirtualRobot
RobotNodePtr rootNew = startJoint->clone(result, true, RobotNodePtr(), colChecker, scaling, preventCloningMeshesIfScalingIs1);
THROW_VR_EXCEPTION_IF(!rootNew, "Clone failed...");
result->setRootNode(rootNew);
- result->setScaling(scaling);
+ result->setScaling(scaling > 0 ? scaling : this->scaling);
std::vector<RobotNodePtr> rn = result->getRobotNodes();
@@ -843,9 +844,14 @@ namespace VirtualRobot
result->propagatingJointValuesEnabled = propagatingJointValuesEnabled;
result->applyJointValues();
+ result->basePath = basePath;
return result;
}
+ RobotPtr Robot::cloneScaling() {
+ return clone(getName(), CollisionCheckerPtr(), -1.0f);
+ }
+
Eigen::Matrix4f Robot::getGlobalPoseForRobotNode(
const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode) const
{
@@ -1170,7 +1176,7 @@ namespace VirtualRobot
return result;
}
- std::string Robot::toXML(const std::string& basePath, const std::string& modelPath /*= "models"*/, bool storeEEF, bool storeRNS, bool storeSensors) const
+ std::string Robot::toXML(const std::string& basePath, const std::string& modelPath, bool storeEEF, bool storeRNS, bool storeSensors, bool storeModelFiles) const
{
std::stringstream ss;
ss << "<?xml version='1.0' encoding='UTF-8'?>" << endl << std::endl;
@@ -1180,7 +1186,7 @@ namespace VirtualRobot
for (auto& node : nodes)
{
- ss << node->toXML(basePath, modelPath, storeSensors) << std::endl;
+ ss << node->toXML(basePath, modelPath, storeSensors, storeModelFiles) << std::endl;
}
ss << std::endl;
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 6612aad306ce7ed89339532608445dfbe407799b..dab2baf82213aefe6962fe00bc7cb5f35d0feeb1 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -275,6 +275,9 @@ namespace VirtualRobot
float scaling = 1.0f,
bool preventCloningMeshesIfScalingIs1 = false);
+ /*! @brief Clones this robot and keeps the current scaling for the robot and each robot node */
+ virtual RobotPtr cloneScaling();
+
//! Just storing the filename.
virtual void setFilename(const std::string& filename);
@@ -422,7 +425,8 @@ namespace VirtualRobot
Creates an XML string that defines the complete robot. Filenames of all visualization models are set to modelPath/RobotNodeName_visu and/or modelPath/RobotNodeName_colmodel.
@see RobotIO::saveXML.
*/
- virtual std::string toXML(const std::string& basePath = ".", const std::string& modelPath = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true) const;
+
+ virtual std::string toXML(const std::string& basePath = ".", const std::string& modelPath = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true, bool storeModelFiles = true) const;
float getScaling() const;
void setScaling(float scaling);
@@ -467,7 +471,6 @@ namespace VirtualRobot
//! It is assumed that the mutex is already set
virtual void applyJointValuesNoLock();
- float scaling;
std::string filename; // RobotIO stores the filename here
std::string type;
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 418fdf8225b8b7f5f539d41c4836e728b460022e..4fa1bbe8fa36eef44ba19dbfcad8ef4cb3c217b1 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -6,9 +6,11 @@
#include "Visualization/VisualizationFactory.h"
#include "Visualization/VisualizationNode.h"
#include "Visualization/Visualization.h"
-#include "VirtualRobotException.h"
+#include "XML/BaseIO.h"
+#include <SimoxUtility/xml/rapidxml/rapidxml.hpp>
#include "Robot.h"
#include "math/Helpers.h"
+#include <SimoxUtility/filesystem/make_relative.h>
#include <Eigen/Dense>
@@ -51,6 +53,7 @@ namespace VirtualRobot
}
setGlobalPose(Eigen::Matrix4f::Identity());
+ this->scaling = 1.0f;
}
@@ -739,7 +742,7 @@ namespace VirtualRobot
}
// check for inertia matrix determination
- if (physics.inertiaMatrix.isZero())
+ if (physics.inertiaMatrix.isZero(1e-8))
{
if (physics.massKg <= 0)
{
@@ -980,7 +983,8 @@ namespace VirtualRobot
}
result->setGlobalPose(getGlobalPose());
-
+ result->scaling = scaling;
+ result->basePath = basePath;
return result;
}
@@ -1013,7 +1017,7 @@ namespace VirtualRobot
return getInertiaMatrix(transform.block<3, 1>(0, 3), transform.block<3, 3>(0, 0));
}
- std::string SceneObject::getSceneObjectXMLString(const std::string& basePath, int tabs)
+ std::string SceneObject::getSceneObjectXMLString(const std::string& basePath, int tabs, const std::string &modelPathRelative)
{
std::stringstream ss;
std::string pre = "";
@@ -1028,7 +1032,7 @@ namespace VirtualRobot
ss << visualizationModel->toXML(basePath, tabs);
}
- if (collisionModel && collisionModel->getVisualization())
+ if (collisionModel)
{
ss << collisionModel->toXML(basePath, tabs);
}
@@ -1446,4 +1450,47 @@ namespace VirtualRobot
return res;
}
+ void SceneObject::setScaling(float scaling)
+ {
+ THROW_VR_EXCEPTION_IF(scaling <= 0, "Scaling must be >0.")
+ this->scaling = scaling;
+ }
+
+ float SceneObject::getScaling()
+ {
+ return scaling;
+ }
+
+ bool SceneObject::reloadVisualizationFromXML(bool useVisAsColModelIfMissing) {
+ bool reloaded = false;
+ if (!collisionModelXML.empty())
+ {
+ rapidxml::xml_document<> doc;
+ char* cstr = new char[collisionModelXML.size() + 1]; // Create char buffer to store string copy
+ strcpy(cstr, collisionModelXML.c_str()); // Copy string into char buffer
+ doc.parse<0>(cstr);
+ collisionModel = BaseIO::processCollisionTag(doc.first_node(), name, basePath);
+ if (collisionModel && scaling != 1.0f) collisionModel = collisionModel->clone(collisionChecker, scaling);
+ delete[] cstr;
+ reloaded = true;
+ }
+ if (!visualizationModelXML.empty())
+ {
+ rapidxml::xml_document<> doc;
+ char* cstr = new char[visualizationModelXML.size() + 1]; // Create char buffer to store string copy
+ strcpy(cstr, visualizationModelXML.c_str()); // Copy string into char buffer
+ doc.parse<0>(cstr);
+ bool useAsColModel;
+ visualizationModel = BaseIO::processVisualizationTag(doc.first_node(), name, basePath, useAsColModel);
+ if (visualizationModel && scaling != 1.0f) visualizationModel = visualizationModel->clone(true, scaling);
+ if (visualizationModel && collisionModel == nullptr && (useVisAsColModelIfMissing || useAsColModel))
+ collisionModel.reset(new CollisionModel(visualizationModel->clone(true), getName() + "_VISU_ColModel", CollisionCheckerPtr()));
+ delete[] cstr;
+ reloaded = true;
+ }
+ for (auto child : this->getChildren()) {
+ reloaded |= child->reloadVisualizationFromXML(useVisAsColModelIfMissing);
+ }
+ return reloaded;
+ }
} // namespace
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index 1d25b987a1e3a34b8763fd45b812cb380c99a091..126f9402f73301910c8fcba788a254bd2f0bddc0 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -29,6 +29,7 @@
#include <type_traits>
#include <string>
#include <vector>
+#include <filesystem>
namespace VirtualRobot
@@ -390,6 +391,11 @@ namespace VirtualRobot
*/
virtual bool saveModelFiles(const std::string& modelPath, bool replaceFilenames);
+ void setScaling(float scaling);
+ float getScaling();
+
+ bool reloadVisualizationFromXML(bool useVisAsColModelIfMissing = true);
+
protected:
virtual SceneObject* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr(), float scaling = 1.0f) const;
@@ -408,7 +414,7 @@ namespace VirtualRobot
SceneObject() {}
//! basic data, used by Obstacle and ManipulationObject
- std::string getSceneObjectXMLString(const std::string& basePath, int tabs);
+ std::string getSceneObjectXMLString(const std::string& basePath, int tabs, const std::string &modelPathRelative = "");
///////////////////////// SETUP ////////////////////////////////////
std::string name;
@@ -423,8 +429,12 @@ namespace VirtualRobot
SceneObjectWeakPtr parent;
CollisionModelPtr collisionModel;
+ std::string collisionModelXML;
//< This is the main visualization
VisualizationNodePtr visualizationModel;
+ std::string visualizationModelXML;
+
+ std::filesystem::path basePath;
bool updateVisualization;
bool updateCollisionModel;
@@ -433,6 +443,8 @@ namespace VirtualRobot
Physics physics;
CollisionCheckerPtr collisionChecker;
+
+ float scaling = 1.0f;
};
diff --git a/VirtualRobot/VirtualRobot.h b/VirtualRobot/VirtualRobot.h
index 1f104c1e092c0f14455fb063b88567dfd2104afc..363cccbe3968bf3ad463cb024818928b6ad1fde2 100644
--- a/VirtualRobot/VirtualRobot.h
+++ b/VirtualRobot/VirtualRobot.h
@@ -161,7 +161,9 @@ namespace VirtualRobot
class Scene;
class RobotConfig;
class Grasp;
+ class ChainedGrasp;
class GraspSet;
+ class GraspableSensorizedObject;
class ManipulationObject;
class CDManager;
class Reachability;
@@ -216,7 +218,10 @@ namespace VirtualRobot
typedef std::shared_ptr<Scene> ScenePtr;
typedef std::shared_ptr<RobotConfig> RobotConfigPtr;
typedef std::shared_ptr<Grasp> GraspPtr;
+ typedef std::shared_ptr<ChainedGrasp> ChainedGraspPtr;
typedef std::shared_ptr<GraspSet> GraspSetPtr;
+ typedef std::shared_ptr<GraspableSensorizedObject> GraspableSensorizedObjectPtr;
+ typedef std::weak_ptr<GraspableSensorizedObject> GraspableSensorizedObjectWeakPtr;
typedef std::shared_ptr<ManipulationObject> ManipulationObjectPtr;
typedef std::shared_ptr<CDManager> CDManagerPtr;
typedef std::shared_ptr<PoseQualityMeasurement> PoseQualityMeasurementPtr;
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index e06a73befe138941a94cac61e5734f02e9d59ce1..e3f1f991b20c056b47465f36fb45ca3ff044a45c 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -14,6 +14,12 @@
#include "../Transformation/DHParameter.h"
#include "../Visualization/VisualizationFactory.h"
#include "../Visualization/VisualizationNode.h"
+#include "../Grasping/Grasp.h"
+#include "../Grasping/ChainedGrasp.h"
+#include "../Grasping/GraspSet.h"
+#include "../Nodes/SensorFactory.h"
+#include <SimoxUtility/filesystem/make_relative.h>
+#include <SimoxUtility/algorithm/string/string_conversion.h>
#include "rapidxml.hpp"
#include <filesystem>
@@ -821,79 +827,7 @@ namespace VirtualRobot
return;
}
-#if (BOOST_VERSION>=104800)
- // canonical needs boost version >=1.48
- namespace fs = std::filesystem;
-
- fs::path filepath;
-
- if (fs::path(filename).is_absolute() && std::filesystem::exists(fs::path(basePath) / fs::path(filename)))
- {
- return;
- }
- else if (fs::path(filename).is_absolute())
- {
- if (std::filesystem::exists(fs::path(filename)))
- {
- filepath = fs::canonical(fs::path(filename));
- }
- else
- {
- filepath = fs::path(filename);
- }
- }
- else
- {
- // combine paths
- // fs::path res = fs::path(basePath) / fs::path(filename).filename();
- // filename = res.generic_string();
- return;
- //THROW_VR_EXCEPTION("Could not make path " + filename + " relative to " + basePath);
- }
-
- fs::path basePathDir = fs::canonical(fs::path(basePath));
- fs::path::iterator itBasePath = basePathDir.begin();
- fs::path::iterator itFile = filepath.begin();
- fs::path newPath;
-
- while (itBasePath != basePathDir.end() && itFile != filepath.end() && *itBasePath == *itFile)
- {
- itFile++;
- itBasePath++;
- }
-
- for (; itBasePath != basePathDir.end(); itBasePath++)
- {
- newPath /= "..";
- }
-
- for (; itFile != filepath.end(); itFile++)
- {
- newPath /= *itFile;
- }
-
- filename = newPath.generic_string();
-#else
- // version compatible with boost below version 1.48,
- // may be buggy in some cases...
- std::filesystem::path diffpath;
- std::filesystem::path tmppath = filename;
-
- while (tmppath != basePath)
- {
- diffpath = tmppath.filename() / diffpath;
- tmppath = tmppath.parent_path();
-
- if (tmppath.empty())
- {
- // no relative path found, take complete path
- diffpath = filename;
- break;
- }
- }
-
- filename = diffpath.generic_string();
-#endif
+ filename = simox::fs::make_relative(std::filesystem::path(basePath), std::filesystem::path(filename));
}
@@ -1901,5 +1835,183 @@ namespace VirtualRobot
return ss.str();
}
+ GraspPtr BaseIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& /*objName*/)
+ {
+ THROW_VR_EXCEPTION_IF(!graspXMLNode, "No <Grasp> tag ?!");
+ // get name
+ std::string name = processNameAttribute(graspXMLNode, true);
+ std::string method = processStringAttribute("creation", graspXMLNode, true);
+ float quality = processFloatAttribute(std::string("quality"), graspXMLNode, true);
+ std::string preshapeName = processStringAttribute("preshape", graspXMLNode, true);
+ Eigen::Matrix4f pose;
+ pose.setIdentity();
+ std::vector< RobotConfig::Configuration > configDefinitions;
+ std::string configName;
+
+ rapidxml::xml_node<>* node = graspXMLNode->first_node();
+
+ rapidxml::xml_node<char>* chainedGraspNode = NULL;
+
+ while (node)
+ {
+ std::string nodeName = getLowerCase(node->name());
+
+ if (nodeName == "transform")
+ {
+ processTransformNode(node, name, pose);
+
+ }
+ else if (nodeName == "chaingrasp")
+ {
+ chainedGraspNode = node;
+ }
+ else if (nodeName == "configuration")
+ {
+ THROW_VR_EXCEPTION_IF(configDefinitions.size() > 0, "Only one configuration per grasp allowed");
+ bool cOK = processConfigurationNode(node, configDefinitions, configName);
+ THROW_VR_EXCEPTION_IF(!cOK, "Invalid configuration defined in grasp tag '" << name << "'." << endl);
+
+ }
+ else
+ {
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl);
+ }
+
+ node = node->next_sibling();
+ }
+
+ GraspPtr grasp = nullptr;
+
+ if (!chainedGraspNode)
+ grasp.reset(new Grasp(name, robotType, eef, pose, method, quality, preshapeName));
+ else {
+ ChainedGraspPtr chainedGrasp(new ChainedGrasp(name, robotType, eef, Eigen::Matrix4f::Zero(), method, quality, preshapeName));
+ node = chainedGraspNode->first_node();
+ while (node)
+ {
+ std::string nodeName = getLowerCase(node->name());
+ if (nodeName == "transform")
+ {
+ processTransformNode(node, name, pose);
+ chainedGrasp->setObjectTransformation(pose);
+ }
+ else if (nodeName == "virtualjoint")
+ {
+ rapidxml::xml_attribute<>* attrName = node->first_attribute("name", 0, false);
+ THROW_VR_EXCEPTION_IF(!attrName, "XML tag <VirtualJoint> does not contain an attribute 'name'!" << std::endl);
+ auto joint = chainedGrasp->getVirtualJoint(attrName->value());
+ rapidxml::xml_attribute<>* attrValue = node->first_attribute("value", 0, false);
+ THROW_VR_EXCEPTION_IF(!attrName, "XML tag <VirtualJoint> does not contain an attribute 'value'!" << std::endl);
+ joint->setValue(simox::alg::to_<float>(attrValue->value()));
+ rapidxml::xml_attribute<>* attrMin = node->first_attribute("min", 0, false);
+ rapidxml::xml_attribute<>* attrMax = node->first_attribute("max", 0, false);
+ if (attrMin && attrMax)
+ joint->setLimits(simox::alg::to_<float>(attrMin->value()), simox::alg::to_<float>(attrMax->value()));
+ }
+ else
+ {
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl);
+ }
+
+ node = node->next_sibling();
+ }
+ grasp = chainedGrasp;
+ }
+
+ if (!configDefinitions.empty())
+ {
+ // create & register configs
+ std::map< std::string, float > rc;
+
+ for (auto& configDefinition : configDefinitions)
+ {
+ rc[ configDefinition.name ] = configDefinition.value;
+ }
+
+ grasp->setConfiguration(rc);
+ }
+
+ return grasp;
+ }
+
+ GraspSetPtr BaseIO::processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName)
+ {
+ THROW_VR_EXCEPTION_IF(!graspSetXMLNode, "No <GraspSet> tag ?!");
+
+ // get name
+ std::string gsName = processNameAttribute(graspSetXMLNode, true);
+ std::string gsRobotType = processStringAttribute(std::string("robottype"), graspSetXMLNode, true);
+ std::string gsEEF = processStringAttribute(std::string("endeffector"), graspSetXMLNode, true);
+
+ if (gsName.empty() || gsRobotType.empty() || gsEEF.empty())
+ {
+ THROW_VR_EXCEPTION("GraspSet tags must have valid attributes 'Name', 'RobotType' and 'EndEffector'");
+ }
+
+ GraspSetPtr result(new GraspSet(gsName, gsRobotType, gsEEF));
+
+ rapidxml::xml_node<>* node = graspSetXMLNode->first_node();
+
+ while (node)
+ {
+ std::string nodeName = getLowerCase(node->name());
+
+ if (nodeName == "grasp")
+ {
+ GraspPtr grasp = processGrasp(node, gsRobotType, gsEEF, objName);
+ THROW_VR_EXCEPTION_IF(!grasp, "Invalid 'Grasp' tag in '" << objName << "'." << endl);
+ result->addGrasp(grasp);
+ }
+ else
+ {
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in GraspSet <" << gsName << ">." << endl);
+ }
+
+ node = node->next_sibling();
+ }
+
+ return result;
+ }
+
+ bool BaseIO::processSensor(GraspableSensorizedObjectPtr rn, rapidxml::xml_node<char>* sensorXMLNode, RobotDescription loadMode, const std::string& basePath)
+ {
+ if (!rn || !sensorXMLNode)
+ {
+ VR_ERROR << "NULL DATA ?!" << std::endl;
+ return false;
+ }
+
+ rapidxml::xml_attribute<>* attr;
+ std::string sensorType;
+
+ attr = sensorXMLNode->first_attribute("type", 0, false);
+
+ if (attr)
+ {
+ sensorType = getLowerCase(attr->value());
+ }
+ else
+ {
+ VR_WARNING << "No 'type' attribute for <Sensor> tag. Skipping Sensor definition of RobotNode " << rn->getName() << "!" << std::endl;
+ return false;
+ }
+
+ SensorPtr s;
+
+
+ SensorFactoryPtr sensorFactory = SensorFactory::fromName(sensorType, nullptr);
+
+ if (sensorFactory)
+ {
+ s = sensorFactory->createSensor(rn, sensorXMLNode, loadMode, basePath);
+ }
+ else
+ {
+ VR_WARNING << "No Factory found for sensor of type " << sensorType << ". Skipping Sensor definition of RobotNode " << rn->getName() << "!" << std::endl;
+ return false;
+ }
+
+ return rn->registerSensor(s);
+ }
} // namespace VirtualRobot
diff --git a/VirtualRobot/XML/BaseIO.h b/VirtualRobot/XML/BaseIO.h
index be962a3d0899ac08f3559dca5bf91fa5ea686694..9f46c63738cd449883a74d2b13629f054dea4a4e 100644
--- a/VirtualRobot/XML/BaseIO.h
+++ b/VirtualRobot/XML/BaseIO.h
@@ -51,6 +51,16 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT BaseIO
{
public:
+ enum RobotDescription
+ {
+ eFull, // load complete robot definition
+ eCollisionModel, // skip visualization tags and load only collision model
+ eStructure, // load only the structure of the robot, ignore visualization and collision tags -> faster access when robot is only used for coordinate transformations
+ eStructureStore, // load only the structure of the robot, ignore visualization and collision tags -> faster access when robot is only used for coordinate transformations. Both tags are stored in string and can be retrieved by reloadVisualizationFromXML()
+ eFullVisAsCol // load complete robot definition - use visualization as collision model if col model not available
+ };
+
+
static void makeAbsolutePath(const std::string& basePath, std::string& filename);
static void makeRelativePath(const std::string& basePath, std::string& filename);
@@ -103,6 +113,11 @@ namespace VirtualRobot
static std::string toXML(const Eigen::Matrix4f& m, std::string ident = "\t");
static std::vector<VisualizationNodePtr> processVisuFiles(rapidxml::xml_node<char>* visualizationXMLNode, const std::string& basePath, std::string& fileType);
+
+ static GraspSetPtr processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName);
+ static GraspPtr processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& objName);
+
+ static bool processSensor(GraspableSensorizedObjectPtr node, rapidxml::xml_node<char>* sensorXMLNode, RobotDescription loadMode, const std::string& basePath);
protected:
// instantiation not allowed
BaseIO();
diff --git a/VirtualRobot/XML/ObjectIO.cpp b/VirtualRobot/XML/ObjectIO.cpp
index b0537a604250994991736b94a4f3314477cc019e..2434027ba0e76af4a0dd6d32e1165238e2d6ace8 100644
--- a/VirtualRobot/XML/ObjectIO.cpp
+++ b/VirtualRobot/XML/ObjectIO.cpp
@@ -6,8 +6,6 @@
#include "RobotIO.h"
-#include "../Grasping/Grasp.h"
-#include "../Grasping/GraspSet.h"
#include "../ManipulationObject.h"
#include "../Visualization/TriMeshModel.h"
@@ -88,63 +86,6 @@ namespace VirtualRobot
return res;
}
- GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& /*objName*/)
- {
- THROW_VR_EXCEPTION_IF(!graspXMLNode, "No <Grasp> tag ?!");
- // get name
- std::string name = processNameAttribute(graspXMLNode, true);
- std::string method = processStringAttribute("creation", graspXMLNode, true);
- float quality = processFloatAttribute(std::string("quality"), graspXMLNode, true);
- std::string preshapeName = processStringAttribute("preshape", graspXMLNode, true);
- Eigen::Matrix4f pose;
- pose.setIdentity();
- std::vector< RobotConfig::Configuration > configDefinitions;
- std::string configName;
-
- rapidxml::xml_node<>* node = graspXMLNode->first_node();
-
- while (node)
- {
- std::string nodeName = getLowerCase(node->name());
-
- if (nodeName == "transform")
- {
- processTransformNode(graspXMLNode, name, pose);
-
- }
- else if (nodeName == "configuration")
- {
- THROW_VR_EXCEPTION_IF(configDefinitions.size() > 0, "Only one configuration per grasp allowed");
- bool cOK = processConfigurationNode(node, configDefinitions, configName);
- THROW_VR_EXCEPTION_IF(!cOK, "Invalid configuration defined in grasp tag '" << name << "'." << endl);
-
- }
- else
- {
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl);
- }
-
- node = node->next_sibling();
- }
-
- GraspPtr grasp(new Grasp(name, robotType, eef, pose, method, quality, preshapeName));
-
- if (configDefinitions.size() > 0)
- {
- // create & register configs
- std::map< std::string, float > rc;
-
- for (auto& configDefinition : configDefinitions)
- {
- rc[ configDefinition.name ] = configDefinition.value;
- }
-
- grasp->setConfiguration(rc);
- }
-
- return grasp;
- }
-
bool ObjectIO::writeSTL(TriMeshModelPtr t, const std::string& filename, const std::string& objectName, float scaling)
{
if (!t || t->faces.size() == 0)
@@ -197,48 +138,11 @@ namespace VirtualRobot
}
- GraspSetPtr ObjectIO::processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName)
- {
- THROW_VR_EXCEPTION_IF(!graspSetXMLNode, "No <GraspSet> tag ?!");
-
- // get name
- std::string gsName = processNameAttribute(graspSetXMLNode, true);
- std::string gsRobotType = processStringAttribute(std::string("robottype"), graspSetXMLNode, true);
- std::string gsEEF = processStringAttribute(std::string("endeffector"), graspSetXMLNode, true);
-
- if (gsName.empty() || gsRobotType.empty() || gsEEF.empty())
- {
- THROW_VR_EXCEPTION("GraspSet tags must have valid attributes 'Name', 'RobotType' and 'EndEffector'");
- }
-
- GraspSetPtr result(new GraspSet(gsName, gsRobotType, gsEEF));
-
- rapidxml::xml_node<>* node = graspSetXMLNode->first_node();
-
- while (node)
- {
- std::string nodeName = getLowerCase(node->name());
-
- if (nodeName == "grasp")
- {
- GraspPtr grasp = processGrasp(node, gsRobotType, gsEEF, objName);
- THROW_VR_EXCEPTION_IF(!grasp, "Invalid 'Grasp' tag in '" << objName << "'." << endl);
- result->addGrasp(grasp);
- }
- else
- {
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in GraspSet <" << gsName << ">." << endl);
- }
-
- node = node->next_sibling();
- }
- return result;
- }
- ManipulationObjectPtr ObjectIO::processManipulationObject(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
+ ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
{
- THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <ManipulationObject> tag in XML definition");
+ THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition");
bool visuProcessed = false;
bool colProcessed = false;
@@ -247,7 +151,6 @@ namespace VirtualRobot
bool useAsColModel = false;
SceneObject::Physics physics;
bool physicsDefined = false;
- std::vector<GraspSetPtr> graspSets;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
// get name
@@ -267,7 +170,7 @@ namespace VirtualRobot
if (xmlFileNode)
{
std::string xmlFile = processFileNode(xmlFileNode, basePath);
- ManipulationObjectPtr result = loadManipulationObject(xmlFile);
+ ObstaclePtr result = loadObstacle(xmlFile);
if (!result)
{
@@ -285,8 +188,7 @@ namespace VirtualRobot
return result;
}
-
- THROW_VR_EXCEPTION_IF(objName.empty(), "ManipulationObject definition expects attribute 'name'");
+ THROW_VR_EXCEPTION_IF(objName.empty(), "Obstacle definition expects attribute 'name'");
rapidxml::xml_node<>* node = objectXMLNode->first_node();
@@ -296,13 +198,13 @@ namespace VirtualRobot
if (nodeName == "visualization")
{
- THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in ManipulationObject '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Obstacle '" << objName << "'." << endl);
visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
visuProcessed = true;
if (useAsColModel)
{
- THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
std::string colModelName = objName;
colModelName += "_VISU_ColModel";
// todo: ID?
@@ -312,53 +214,88 @@ namespace VirtualRobot
}
else if (nodeName == "collisionmodel")
{
- THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
collisionModel = processCollisionTag(node, objName, basePath);
colProcessed = true;
}
else if (nodeName == "physics")
{
- THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in ManipulationObject '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
processPhysicsTag(node, objName, physics);
physicsDefined = true;
}
- else if (nodeName == "graspset")
- {
- GraspSetPtr gs = processGraspSet(node, objName);
- THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << objName << "'." << endl);
- graspSets.push_back(gs);
-
- }
else if (nodeName == "globalpose")
{
processTransformNode(node, objName, globalPose);
}
else
{
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in ManipulationObject <" << objName << ">." << endl);
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl);
}
node = node->next_sibling();
}
+ // build object
+ ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics));
+ object->setGlobalPose(globalPose);
+ return object;
+ }
+ VirtualRobot::ManipulationObjectPtr ObjectIO::createManipulationObjectFromString(const std::string& xmlString, const std::string& basePath /*= ""*/)
+ {
+ // copy string content to char array
+ char* y = new char[xmlString.size() + 1];
+ strncpy(y, xmlString.c_str(), xmlString.size() + 1);
- // build object
- ManipulationObjectPtr object(new ManipulationObject(objName, visualizationNode, collisionModel, physics));
+ VirtualRobot::ManipulationObjectPtr obj;
- for (const auto& graspSet : graspSets)
+ try
{
- object->addGraspSet(graspSet);
- }
+ rapidxml::xml_document<char> doc; // character type defaults to char
+ doc.parse<0>(y); // 0 means default parse flags
+ rapidxml::xml_node<char>* objectXMLNode = doc.first_node("ManipulationObject");
- object->setGlobalPose(globalPose);
+ obj = processManipulationObject(objectXMLNode, basePath);
- return object;
+
+
+ }
+ catch (rapidxml::parse_error& e)
+ {
+ delete[] y;
+ THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl
+ << "Error message:" << e.what() << endl
+ << "Position: " << endl << e.where<char>() << endl);
+ return ManipulationObjectPtr();
+ }
+ catch (VirtualRobot::VirtualRobotException&)
+ {
+ // rethrow the current exception
+ delete[] y;
+ throw;
+ }
+ catch (std::exception& e)
+ {
+ delete[] y;
+ THROW_VR_EXCEPTION("Error while parsing xml definition" << endl
+ << "Error code:" << e.what() << endl);
+ return ManipulationObjectPtr();
+ }
+ catch (...)
+ {
+ delete[] y;
+ THROW_VR_EXCEPTION("Error while parsing xml definition" << endl);
+ return ManipulationObjectPtr();
+ }
+
+ delete[] y;
+ return obj;
}
- ObstaclePtr ObjectIO::processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
+ ManipulationObjectPtr ObjectIO::processManipulationObject(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath)
{
- THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <Obstacle> tag in XML definition");
+ THROW_VR_EXCEPTION_IF(!objectXMLNode, "No <ManipulationObject> tag in XML definition");
bool visuProcessed = false;
bool colProcessed = false;
@@ -367,7 +304,9 @@ namespace VirtualRobot
bool useAsColModel = false;
SceneObject::Physics physics;
bool physicsDefined = false;
+ std::vector<GraspSetPtr> graspSets;
Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
+ std::vector< rapidxml::xml_node<>* > sensorTags;
// get name
std::string objName = processNameAttribute(objectXMLNode);
@@ -378,7 +317,7 @@ namespace VirtualRobot
if (xmlFileNode)
{
std::string xmlFile = processFileNode(xmlFileNode, basePath);
- ObstaclePtr result = loadObstacle(xmlFile);
+ ManipulationObjectPtr result = loadManipulationObject(xmlFile);
if (!result)
{
@@ -402,9 +341,10 @@ namespace VirtualRobot
return result;
}
- THROW_VR_EXCEPTION_IF(objName.empty(), "Obstacle definition expects attribute 'name'");
+ THROW_VR_EXCEPTION_IF(objName.empty(), "ManipulationObject definition expects attribute 'name'");
+
rapidxml::xml_node<>* node = objectXMLNode->first_node();
while (node)
@@ -413,13 +353,13 @@ namespace VirtualRobot
if (nodeName == "visualization")
{
- THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in Obstacle '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in ManipulationObject '" << objName << "'." << endl);
visualizationNode = processVisualizationTag(node, objName, basePath, useAsColModel);
visuProcessed = true;
if (useAsColModel)
{
- THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << objName << "'." << endl);
std::string colModelName = objName;
colModelName += "_VISU_ColModel";
// todo: ID?
@@ -429,87 +369,59 @@ namespace VirtualRobot
}
else if (nodeName == "collisionmodel")
{
- THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in Obstacle '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in ManipulationObject '" << objName << "'." << endl);
collisionModel = processCollisionTag(node, objName, basePath);
colProcessed = true;
}
else if (nodeName == "physics")
{
- THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in Obstacle '" << objName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in ManipulationObject '" << objName << "'." << endl);
processPhysicsTag(node, objName, physics);
physicsDefined = true;
}
+ else if (nodeName == "graspset")
+ {
+ GraspSetPtr gs = processGraspSet(node, objName);
+ THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << objName << "'." << endl);
+ graspSets.push_back(gs);
+
+ }
+ else if (nodeName == "sensor")
+ {
+ sensorTags.push_back(node);
+ }
else if (nodeName == "globalpose")
{
processTransformNode(node, objName, globalPose);
}
else
{
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Obstacle <" << objName << ">." << endl);
+ THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in ManipulationObject <" << objName << ">." << endl);
}
node = node->next_sibling();
}
- // build object
- ObstaclePtr object(new Obstacle(objName, visualizationNode, collisionModel, physics));
- object->setGlobalPose(globalPose);
- return object;
- }
-
-
- VirtualRobot::ManipulationObjectPtr ObjectIO::createManipulationObjectFromString(const std::string& xmlString, const std::string& basePath /*= ""*/)
- {
- // copy string content to char array
- char* y = new char[xmlString.size() + 1];
- strncpy(y, xmlString.c_str(), xmlString.size() + 1);
- VirtualRobot::ManipulationObjectPtr obj;
+ // build object
+ ManipulationObjectPtr object(new ManipulationObject(objName, visualizationNode, collisionModel, physics));
- try
+ // process sensors
+ for (auto& sensorTag : sensorTags)
{
- rapidxml::xml_document<char> doc; // character type defaults to char
- doc.parse<0>(y); // 0 means default parse flags
- rapidxml::xml_node<char>* objectXMLNode = doc.first_node("ManipulationObject");
-
- obj = processManipulationObject(objectXMLNode, basePath);
-
-
-
+ processSensor(object, sensorTag, RobotDescription::eFull, basePath);
}
- catch (rapidxml::parse_error& e)
- {
- delete[] y;
- THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl
- << "Error message:" << e.what() << endl
- << "Position: " << endl << e.where<char>() << endl);
- return ManipulationObjectPtr();
- }
- catch (VirtualRobot::VirtualRobotException&)
- {
- // rethrow the current exception
- delete[] y;
- throw;
- }
- catch (std::exception& e)
- {
- delete[] y;
- THROW_VR_EXCEPTION("Error while parsing xml definition" << endl
- << "Error code:" << e.what() << endl);
- return ManipulationObjectPtr();
- }
- catch (...)
+
+ for (const auto& graspSet : graspSets)
{
- delete[] y;
- THROW_VR_EXCEPTION("Error while parsing xml definition" << endl);
- return ManipulationObjectPtr();
+ object->addGraspSet(graspSet);
}
- delete[] y;
- return obj;
- }
+ object->setGlobalPose(globalPose);
+ return object;
+ }
VirtualRobot::ObstaclePtr ObjectIO::createObstacleFromString(const std::string& xmlString, const std::string& basePath /*= ""*/)
{
diff --git a/VirtualRobot/XML/ObjectIO.h b/VirtualRobot/XML/ObjectIO.h
index a888f7a5d66db8444363aef1e83f62d261cf1c2c..ae18f00a3368215d536d940910d6ff92e85672ad 100644
--- a/VirtualRobot/XML/ObjectIO.h
+++ b/VirtualRobot/XML/ObjectIO.h
@@ -106,8 +106,6 @@ namespace VirtualRobot
static ObstaclePtr processObstacle(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath);
static ManipulationObjectPtr processManipulationObject(rapidxml::xml_node<char>* objectXMLNode, const std::string& basePath);
- static GraspSetPtr processGraspSet(rapidxml::xml_node<char>* graspSetXMLNode, const std::string& objName);
- static GraspPtr processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& objName);
/*!
* \brief writeSTL Write ascii stl file.
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index d7a2d7dcede9e49c4c48e53bb01f716df0560352..d184ab243ee19f71fedbc4444a0e72e62a5dc95d 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -7,7 +7,6 @@
#include "../EndEffector/EndEffector.h"
#include "../EndEffector/EndEffectorActor.h"
#include "../Nodes/RobotNodeFactory.h"
-#include "../Nodes/SensorFactory.h"
#include "../Nodes/RobotNodeFixedFactory.h"
#include "../Nodes/RobotNodePrismatic.h"
#include "../Transformation/DHParameter.h"
@@ -18,6 +17,7 @@
#include "../RuntimeEnvironment.h"
#include "VirtualRobot.h"
#include "rapidxml.hpp"
+#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
#include "mujoco/RobotMjcf.h"
#include <VirtualRobot/Import/URDF/SimoxURDFFactory.h>
@@ -171,47 +171,6 @@ namespace VirtualRobot
}
- bool RobotIO::processSensor(RobotNodePtr rn, rapidxml::xml_node<char>* sensorXMLNode, RobotDescription loadMode, const std::string& basePath)
- {
- if (!rn || !sensorXMLNode)
- {
- VR_ERROR << "NULL DATA ?!" << std::endl;
- return false;
- }
-
- rapidxml::xml_attribute<>* attr;
- std::string sensorType;
-
- attr = sensorXMLNode->first_attribute("type", 0, false);
-
- if (attr)
- {
- sensorType = getLowerCase(attr->value());
- }
- else
- {
- VR_WARNING << "No 'type' attribute for <Sensor> tag. Skipping Sensor definition of RobotNode " << rn->getName() << "!" << std::endl;
- return false;
- }
-
- SensorPtr s;
-
-
- SensorFactoryPtr sensorFactory = SensorFactory::fromName(sensorType, nullptr);
-
- if (sensorFactory)
- {
- s = sensorFactory->createSensor(rn, sensorXMLNode, loadMode, basePath);
- }
- else
- {
- VR_WARNING << "No Factory found for sensor of type " << sensorType << ". Skipping Sensor definition of RobotNode " << rn->getName() << "!" << std::endl;
- return false;
- }
-
- return rn->registerSensor(s);
- }
-
RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char>* jointXMLNode, const std::string& robotNodeName,
RobotPtr robot,
VisualizationNodePtr visualizationNode,
@@ -643,6 +602,8 @@ namespace VirtualRobot
VisualizationNodePtr visualizationNode;
CollisionModelPtr collisionModel;
RobotNodePtr robotNode;
+ std::string visualizationModelXML;
+ std::string collisionModelXML;
SceneObject::Physics physics;
bool physicsDefined = false;
Eigen::Matrix4f transformMatrix = Eigen::Matrix4f::Identity();
@@ -651,6 +612,7 @@ namespace VirtualRobot
rapidxml::xml_node<>* jointNodeXML = nullptr;
std::vector< rapidxml::xml_node<>* > sensorTags;
+ std::vector<GraspSetPtr> graspSets;
while (node)
{
@@ -658,7 +620,7 @@ namespace VirtualRobot
if (nodeName == "visualization")
{
- if (loadMode == eFull)
+ if (loadMode == eFull || loadMode == eFullVisAsCol)
{
THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in RobotNode '" << robotNodeName << "'." << endl);
visualizationNode = processVisualizationTag(node, robotNodeName, basePath, useAsColModel);
@@ -690,16 +652,29 @@ namespace VirtualRobot
collisionModel.reset(new CollisionModel(visualizationNodeCM, colModelName, CollisionCheckerPtr()));
colProcessed = true;
}
- }// else silently ignore tag
+ }
+ else if (loadMode == eStructureStore)
+ {
+ std::stringstream ss;
+ ss << *node;
+ visualizationModelXML = ss.str();
+ //rapidxml::print(std::back_inserter(robotNode->visualizationModelXML), node, rapidxml::print_no_indenting);
+ }
}
else if (nodeName == "collisionmodel")
{
- if (loadMode == eFull || loadMode == eCollisionModel)
+ if (loadMode == eFull || loadMode == eCollisionModel || loadMode == eFullVisAsCol)
{
THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in RobotNode '" << robotNodeName << "'." << endl);
collisionModel = processCollisionTag(node, robotNodeName, basePath);
colProcessed = true;
- } // else silently ignore tag
+ }
+ else if (loadMode == eStructureStore)
+ {
+ std::stringstream ss;
+ ss << *node;
+ collisionModelXML = ss.str();
+ }
}
else if (nodeName == "child")
{
@@ -728,6 +703,13 @@ namespace VirtualRobot
{
sensorTags.push_back(node);
}
+ else if (nodeName == "graspset")
+ {
+ GraspSetPtr gs = processGraspSet(node, robotNodeName);
+ THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << robotNodeName << "'." << endl);
+ graspSets.push_back(gs);
+
+ }
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in RobotNode <" << robotNodeName << ">." << endl);
@@ -736,9 +718,22 @@ namespace VirtualRobot
node = node->next_sibling();
}
+ if (!colProcessed && visualizationNode && loadMode == eFullVisAsCol)
+ {
+ // Use collision model as visu model if not found
+ std::string colModelName = robotNodeName;
+ colModelName += "_VISU_ColModel";
+ // clone model
+ VisualizationNodePtr visualizationNodeClone = visualizationNode->clone();
+ // todo: ID?
+ collisionModel.reset(new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
+ }
//create joint from xml data
robotNode = processJointNode(jointNodeXML, robotNodeName, robo, visualizationNode, collisionModel, physics, rntype, transformMatrix);
+ robotNode->basePath = basePath;
+ robotNode->visualizationModelXML = visualizationModelXML;
+ robotNode->collisionModelXML = collisionModelXML;
// process sensors
for (auto& sensorTag : sensorTags)
@@ -746,6 +741,12 @@ namespace VirtualRobot
processSensor(robotNode, sensorTag, loadMode, basePath);
}
+ for (const auto& graspSet : graspSets)
+ {
+ robotNode->addGraspSet(graspSet);
+ }
+
+
return robotNode;
}
@@ -1575,7 +1576,7 @@ namespace VirtualRobot
return false;
}
- std::string xmlRob = robot->toXML(basePath, modelDir, storeEEF, storeRNS, storeSensors);
+ std::string xmlRob = robot->toXML(basePath, modelDir, storeEEF, storeRNS, storeSensors, storeModelFiles);
f << xmlRob;
f.close();
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index b1e9d11a1be94af79f3a62b2c6019111d296176c..b09b9321155fc74fea69ae382bd6a3b2859bc75e 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -48,13 +48,6 @@ namespace VirtualRobot
{
public:
- enum RobotDescription
- {
- eFull, // load complete robot definition
- eCollisionModel, // skip visualization tags and load only collision model
- eStructure // load only the structure of the robot, ignore visualization and collision tags -> faster access when robot is only used for coordinate transformations
- };
-
/*!
Loads robot from file.
@param xmlFile The file
@@ -138,7 +131,6 @@ namespace VirtualRobot
SceneObject::Physics& physics, RobotNode::RobotNodeType rntype, Eigen::Matrix4f& transformationMatrix);
static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode, const std::string& nodeName, std::vector< ChildFromRobotDef >& childrenFromRobot);
static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool& limitless);
- static bool processSensor(RobotNodePtr rn, rapidxml::xml_node<char>* sensorXMLNode, RobotDescription loadMode, const std::string& basePath);
static std::map<std::string, int> robot_name_counter;
static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode, const std::string& robotNodeName, const std::string& basePath);
};