diff --git a/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.cpp b/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.cpp
index 097a0cf5c7d5004b35b2baa44f8901050c565917..c5737f9681b2b9883538b1e9a25c93871f43631a 100644
--- a/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.cpp
+++ b/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.cpp
@@ -22,58 +22,98 @@
#include "Armar6GraspProvider.h"
+#include <Armar6Skills/libraries/Armar6Tools/Armar6HandV2Helper.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
-#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+
+#include <pcl/point_types.h>
+#include <pcl/common/transforms.h>
+#include <pcl/filters/passthrough.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
+
+#include <SimoxUtility/algorithm/contains.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/math/pose/transform.h>
+
#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/math/Line.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
+
#include <cmath>
-#include "FitRectangle.h"
-#include "RobotAPI/components/ArViz/Client/elements/Color.h"
-#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+#include "FitRectangle.h"
+#include "grasping_util.h"
-#include <Armar6Skills/libraries/Armar6Tools/Armar6HandV2Helper.h>
using namespace math;
using namespace armarx;
-
-std::string Armar6GraspProvider::GraspTypeToString(Armar6GraspProviderModule::GraspType graspType)
+Armar6GraspProviderPropertyDefinitions::Armar6GraspProviderPropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
{
- switch (graspType)
- {
- case Armar6GraspProviderModule::GraspType::Top:
- return "Top";
- case Armar6GraspProviderModule::GraspType::Side:
- return "Side";
- default:
- throw LocalException("Unknown GraspType");
- }
+ defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the debug observer that should be used");
+ defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Name of the debug drawer topic that should be used");
+ defineOptionalProperty<std::string>("StaticPlotterName", "StaticPlotter", "Name of the static plotter that should be used");
+
+ defineOptionalProperty<std::string>("GraspCandidatesTopicName", "GraspCandidatesTopic", "Name of the Grasp Candidate Topic");
+ defineOptionalProperty<std::string>("ConfigTopicName", "GraspCandidateProviderConfigTopic", "Name of the Grasp Candidate Provider Config Topic");
+
+ defineOptionalProperty<std::string>("PointCloudFormat", "XYZRGBL", "Format of the input and output point cloud (XYZRGBA, XYZL, XYZRGBL)");
+ defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>("providerName", "TabletopSegmentationResult", "name of the point cloud provider");
+ defineOptionalProperty<std::string>("serviceProviderName", "TabletopSegmentation", "name of the service provider");
+ defineOptionalProperty<std::string>("sourceFrameName", "root", "The source frame name");
+
+ defineOptionalProperty<uint32_t>("BackgroundLabelId", 1, "Label in the pointcloud for the plane or background.", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("UseObjectIdMap", false, "Use ID map from config for named objects");
+
+ defineOptionalProperty<float>("DownsamplingLeafSize", 5.f, "If >0, the pointcloud is downsampled with a leaf size of this value. In mm.", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<size_t>("MininumClusterPointCount", 100, "Minimum number of points per cluster/segmented object to be further processed.", PropertyDefinitionBase::eModifiable);
+
+ defineOptionalProperty<float>("BoundingBoxX1", -200, "");
+ defineOptionalProperty<float>("BoundingBoxX2", 200, "");
+ defineOptionalProperty<float>("BoundingBoxY1", 0.f, "");
+ defineOptionalProperty<float>("BoundingBoxY2", 1300.f, "");
+ defineOptionalProperty<float>("BoundingBoxZ1", 975.f, "");
+
+ defineOptionalProperty<bool>("enablePointCloudsVisu", true, "");
+ defineOptionalProperty<bool>("enableVisu", true, "");
+ defineOptionalProperty<bool>("enableCandidatesVisu", true, "");
+ defineOptionalProperty<bool>("enableBoundigBoxesVisu", false, "");
+ defineOptionalProperty<bool>("FlattenInZ", true, "");
+ defineOptionalProperty<bool>("UseObjectPoseObserver", false, "");
+ defineOptionalProperty<bool>("UseWorkingMemory", false, "");
+ defineOptionalProperty<int>("WaitForPointCloudsMS", 200, "");
+
+ defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote gui");
+ defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge");
+ defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory");
+
+
+ defineOptionalProperty<int>("colorNonReachableGraspsR", 128, "The red color channel for non reachable grasps");
+ defineOptionalProperty<int>("colorNonReachableGraspsG", 128, "The green color channel for non reachable grasps");
+ defineOptionalProperty<int>("colorNonReachableGraspsB", 128, "The blue color channel color channel for non reachable grasps");
}
-std::string Armar6GraspProvider::PreshapeTypeToString(Armar6GraspProviderModule::PreshapeType preshapeType)
+armarx::PropertyDefinitionsPtr Armar6GraspProvider::createPropertyDefinitions()
{
- switch (preshapeType)
- {
- case Armar6GraspProviderModule::PreshapeType::Open:
- return "Open";
- case Armar6GraspProviderModule::PreshapeType::Preshaped:
- return "Preshaped";
- default:
- throw LocalException("Unknown GraspType");
- }
+ return armarx::PropertyDefinitionsPtr(new Armar6GraspProviderPropertyDefinitions(getConfigIdentifier()));
}
+
void Armar6GraspProvider::onInitPointCloudProcessor()
{
offeringTopic("ServiceRequests");
@@ -82,11 +122,11 @@ void Armar6GraspProvider::onInitPointCloudProcessor()
sourceFrameName = getProperty<std::string>("sourceFrameName").getValue();
pointCloudFormat = getProperty<std::string>("PointCloudFormat").getValue();
- boundingBox.z1 = getProperty<float>("BoundingBoxZ1").getValue();
- boundingBox.y1 = getProperty<float>("BoundingBoxY1").getValue();
- boundingBox.y2 = getProperty<float>("BoundingBoxY2").getValue();
- boundingBox.x1 = getProperty<float>("BoundingBoxX1").getValue();
- boundingBox.x2 = getProperty<float>("BoundingBoxX2").getValue();
+ boundingBox.min_x() = getProperty<float>("BoundingBoxX1").getValue();
+ boundingBox.max_x() = getProperty<float>("BoundingBoxX2").getValue();
+ boundingBox.min_y() = getProperty<float>("BoundingBoxY1").getValue();
+ boundingBox.max_y() = getProperty<float>("BoundingBoxY2").getValue();
+ boundingBox.min_z() = getProperty<float>("BoundingBoxZ1").getValue();
config.enablePointCloudsVisu = getProperty<bool>("enablePointCloudsVisu").getValue();
config.enableVisu = getProperty<bool>("enableVisu").getValue();
@@ -296,11 +336,11 @@ void Armar6GraspProvider::onConnectPointCloudProcessor()
// ValueProxy<float> x1Slider = makeSlider ...
- makeSlider("x1", -600, 600, boundingBox.x1);
- makeSlider("x2", -600, 600, boundingBox.x2);
- makeSlider("y1", 0, 1500, boundingBox.y1);
- makeSlider("y2", 0, 1500, boundingBox.y2);
- makeSlider("z1", 300, 1500, boundingBox.z1);
+ makeSlider("x1", -600, 600, boundingBox.min_x());
+ makeSlider("x2", -600, 600, boundingBox.max_x());
+ makeSlider("y1", 0, 1500, boundingBox.min_y());
+ makeSlider("y2", 0, 1500, boundingBox.max_y());
+ makeSlider("z1", 300, 1500, boundingBox.min_z());
rootLayoutBuilder.addChild(checkBoxLayoutBuilder);
makeCheckBox("FlattenInZ", true);
makeCheckBox("enableVisu", true);
@@ -314,26 +354,26 @@ void Armar6GraspProvider::onConnectPointCloudProcessor()
guiTask = new SimplePeriodicTask<>([&]()
{
guiTab.receiveUpdates();
- boundingBox.x1 = guiTab.getValue<float>("x1").get();
- boundingBox.x2 = guiTab.getValue<float>("x2").get();
- boundingBox.y1 = guiTab.getValue<float>("y1").get();
- boundingBox.y2 = guiTab.getValue<float>("y2").get();
- boundingBox.z1 = guiTab.getValue<float>("z1").get();
+ boundingBox.min_x() = guiTab.getValue<float>("x1").get();
+ boundingBox.max_x() = guiTab.getValue<float>("x2").get();
+ boundingBox.min_y() = guiTab.getValue<float>("y1").get();
+ boundingBox.max_y() = guiTab.getValue<float>("y2").get();
+ boundingBox.min_z() = guiTab.getValue<float>("z1").get();
flattenInZ = guiTab.getValue<bool>("FlattenInZ").get();
config.enableVisu = guiTab.getValue<bool>("enableVisu").get();
config.enableCandidatesVisu = guiTab.getValue<bool>("enableCandidatesVisu").get();
config.enableBoundigBoxesVisu = guiTab.getValue<bool>("enableBoundigBoxesVisu").get();
config.enablePointCloudsVisu = guiTab.getValue<bool>("enablePointCloudsVisu").get();
- guiTab.getValue<float>("x1_spin").set(boundingBox.x1);
- guiTab.getValue<float>("x2_spin").set(boundingBox.x2);
- guiTab.getValue<float>("y1_spin").set(boundingBox.y1);
- guiTab.getValue<float>("y2_spin").set(boundingBox.y2);
- guiTab.getValue<float>("z1_spin").set(boundingBox.z1);
+ guiTab.getValue<float>("x1_spin").set(boundingBox.min_x());
+ guiTab.getValue<float>("x2_spin").set(boundingBox.max_x());
+ guiTab.getValue<float>("y1_spin").set(boundingBox.min_y());
+ guiTab.getValue<float>("y2_spin").set(boundingBox.max_y());
+ guiTab.getValue<float>("z1_spin").set(boundingBox.min_z());
guiTab.sendUpdates();
- //ARMARX_IMPORTANT << VAROUT(boundingBox.z1);
+ //ARMARX_IMPORTANT << VAROUT(boundingBox.min_z());
}, 10, false, "guiTask");
reportConfigTask = new SimplePeriodicTask<>([&]()
@@ -370,82 +410,26 @@ void Armar6GraspProvider::process()
}
else
{
- ARMARX_ERROR << "Could not process point cloud, because format '" << pointCloudFormat << "' is not suppoted / unknown";
+ ARMARX_ERROR << "Could not process point cloud because format '" << pointCloudFormat << "' is not supported / unknown";
}
}
-namespace maptools
-{
- template<typename TKey, typename TValue>
- bool TryGetValue(const std::map<TKey, TValue>& map, const TKey& key, TValue& outVal)
- {
- typename std::map<TKey, TValue>::const_iterator it = map.find(key);
- if (it == map.end())
- {
- return false;
- }
- outVal = it->second;
- return true;
- }
-
- template<typename TKey, typename TValue, typename TDefaultValue>
- TValue GetValueOrDefault(const std::map<TKey, TValue>& map, const TKey& key, const TDefaultValue& defaultVal)
- {
- typename std::map<TKey, TValue>::const_iterator it = map.find(key);
- if (it == map.end())
- {
- return defaultVal;
- }
- return it->second;
- }
- template<typename TKey, typename TValue>
- bool HasKey(const std::map<TKey, TValue>& map, const TKey& key)
- {
- return map.count(key) > 0;
- }
+static viz::Cylinder drawLine(std::string const & name, Eigen::Vector3f fromRobot, Eigen::Vector3f toRobot, const Eigen::Matrix4f& robotToGlobal)
+{
+ viz::Cylinder cyl = viz::Cylinder(name)
+ .fromTo(simox::math::transform_position(robotToGlobal, fromRobot),
+ simox::math::transform_position(robotToGlobal, toRobot))
+ .radius(3.0f);
+ return cyl;
}
-static void sortBoxExtendsForGraspCandidateGeneration(Eigen::Vector3f& v1,
- Eigen::Vector3f& v2,
- Eigen::Vector3f& v3,
- Eigen::Vector3f& robotExtend1,
- Eigen::Vector3f& robotExtend2,
- Eigen::Vector3f& robotExtend3)
+static viz::Box drawBox(std::string const & name, Eigen::Matrix4f const & poseRobot, const Eigen::Matrix4f& robotToGlobal)
{
- // These have to be sorted in a specific way
- // v3 should point in the direction of Z
- // v1 should point along the longer remaining axis (after aligning v3)
- Eigen::Vector3f z = Eigen::Vector3f::UnitZ();
- if (std::abs(v1.dot(z)) > 0.8f)
- {
- std::swap(v1, v3);
- std::swap(robotExtend1, robotExtend3);
- }
- if (std::abs(v2.dot(z)) > 0.8f)
- {
- std::swap(v2, v3);
- std::swap(robotExtend2, robotExtend3);
- }
-
- if (v3.z() < 0.0f)
- {
- v3 = -v3;
- robotExtend3 = -robotExtend3;
- }
-
- if (robotExtend1.norm() < robotExtend2.norm())
- {
- Eigen::Vector3f temp = v1;
- v1 = v2;
- v2 = -temp;
-
- temp = robotExtend1;
- robotExtend1 = robotExtend2;
- robotExtend2 = -temp;
- }
+ return viz::Box(name).pose(robotToGlobal * poseRobot);
}
+
void Armar6GraspProvider::processPointCloud()
{
pcl::PointCloud<pcl::PointXYZRGBL>::Ptr inputCloudPtr(new pcl::PointCloud<pcl::PointXYZRGBL>());
@@ -473,29 +457,6 @@ void Armar6GraspProvider::processPointCloud()
Eigen::Matrix4f robotToGlobal = Eigen::Matrix4f::Identity();
- auto drawLine = [&](std::string const & name, Eigen::Vector3f fromRobot, Eigen::Vector3f toRobot, viz::Color color,
- viz::Layer & layer)
- {
- Eigen::Vector4f fromGlobal = robotToGlobal * fromRobot.homogeneous();
- Eigen::Vector4f toGlobal = robotToGlobal * toRobot.homogeneous();
- viz::Cylinder cyl = viz::Cylinder(name)
- .fromTo(fromGlobal.head<3>(), toGlobal.head<3>())
- .radius(3.0f)
- .color(color);
- layer.add(cyl);
- };
-
- auto drawBox = [&](std::string const & name, Eigen::Matrix4f const & poseRobot, Eigen::Vector3f size, viz::Color color,
- viz::Layer & layer)
- {
- Eigen::Matrix4f poseGlobal = robotToGlobal * poseRobot;
- viz::Box box = viz::Box(name)
- .pose(poseGlobal)
- .size(size)
- .color(color);
- layer.add(box);
- };
-
if (waitForPointClouds(providerName, waitForPointCloudsMS))
{
getPointClouds<pcl::PointXYZRGBL>(providerName, inputCloudPtr);
@@ -516,22 +477,19 @@ void Armar6GraspProvider::processPointCloud()
visionx::MetaPointCloudFormatPtr format = getPointCloudFormat(providerName);
-
- int inputSize = inputCloudPtr->points.size();
-
- if (inputSize < 100)
+ if (inputCloudPtr->points.size() < 100)
{
ARMARX_INFO << deactivateSpam(60) << "Input size too small! Skipping grasp candidate generation";
return;
}
-
- ARMARX_VERBOSE << "processPointCloud: " << inputCloudPtr->width << "x" << inputCloudPtr->height << " age: " << (TimeUtil::GetTime() - IceUtil::Time::microSeconds(inputCloudPtr->header.stamp)).toMilliSeconds();
+ ARMARX_VERBOSE << "processPointCloud: " << inputCloudPtr->width << "x" << inputCloudPtr->height
+ << " age: " << (TimeUtil::GetTime() - IceUtil::Time::microSeconds(inputCloudPtr->header.stamp)).toMilliSecondsDouble() << " ms";
RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponent, inputCloudPtr->header.stamp);
robotToGlobal = localRobot->getGlobalPose();
Eigen::Matrix4f sourceFrameInRootFrame = Eigen::Matrix4f::Identity();
- if (sourceFrameName == "Global")
+ if (sourceFrameName == armarx::GlobalFrame)
{
sourceFrameInRootFrame = localRobot->getRootNode()->getGlobalPose().inverse();
}
@@ -544,21 +502,19 @@ void Armar6GraspProvider::processPointCloud()
// Transform to global frame
pcl::transformPointCloud(*inputCloudPtr, *tmpCloudPtr, sourceFrameInRootFrame);
tmpCloudPtr.swap(inputCloudPtr);
-
tmpCloudPtr->clear();
- //std::vector<Eigen::Vector3f> segmentedPoints;
-
- uint32_t backgroundLabel = getProperty<uint32_t>("BackgroundLabelId").getValue();
+ const uint32_t backgroundLabel = getProperty<uint32_t>("BackgroundLabelId");
std::map<uint32_t, std::vector<Eigen::Vector3f>> labeledPoints;
std::map<uint32_t, DrawColor> colorMap;
std::map<uint32_t, pcl::PointCloud<pcl::PointXYZRGBL>::Ptr> labeledClouds;
for (const pcl::PointXYZRGBL& point : inputCloudPtr->points)
{
- bool anyNan = ::isnan(point.x) || ::isnan(point.y) || ::isnan(point.z) || ::isnan(point.r) || ::isnan(point.g) || ::isnan(point.b);
- if (anyNan)
+ bool hasNan = std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)
+ || std::isnan(point.r) || std::isnan(point.g) || std::isnan(point.b);
+ if (hasNan)
{
continue;
}
@@ -584,14 +540,14 @@ void Armar6GraspProvider::processPointCloud()
colorMap[point.label] = DrawColor {point.r / 255.f, point.g / 255.f, point.b / 255.f, 1.f};
labeledClouds[point.label]->push_back(p);
-
outputCloudPtr->push_back(p);
}
- for (const std::pair<uint32_t, pcl::PointCloud<pcl::PointXYZRGBL>::Ptr>& pair : labeledClouds)
+
+ for (const auto& [label, cloud] : labeledClouds)
{
pcl::PointCloud<pcl::PointXYZRGBL>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGBL>());
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBL> sor;
- sor.setInputCloud(pair.second);
+ sor.setInputCloud(cloud);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
@@ -601,51 +557,41 @@ void Armar6GraspProvider::processPointCloud()
}
}
-
-
- for (const std::pair<uint32_t, std::vector<Eigen::Vector3f>>& pair : labeledPoints)
+ for (const auto& [label, allPoints] : labeledPoints)
{
- std::vector<Eigen::Vector3f> allPoints = pair.second;
- uint32_t label = pair.first;
-
if (label == backgroundLabel)
{
- ARMARX_VERBOSE << "Skipping background label with " << allPoints.size() << " points";
+ ARMARX_VERBOSE << "Skipping background label with " << allPoints.size() << " points.";
continue;
}
std::string objectName = "";
bool isKnownObject = false;
if (config.UseObjectIdMap)
{
- objectName = maptools::GetValueOrDefault(config.ObjectIdMap, (int)label, "UNKNOWN");
- isKnownObject = maptools::HasKey(config.ObjectIdMap, (int)label);
+ objectName = simox::alg::get_value_or_default(config.ObjectIdMap, int(label), "UNKNOWN");
+ isKnownObject = simox::alg::contains_key(config.ObjectIdMap, int(label));
}
else
{
objectName = "OBJ_" + std::to_string(label);
}
- ARMARX_VERBOSE << VAROUT(objectName) << VAROUT(label) << " " << VAROUT(allPoints.size());
+ ARMARX_VERBOSE << VAROUT(objectName) << " | " << VAROUT(label) << " | " << VAROUT(allPoints.size());
if (allPoints.size() < getProperty<size_t>("MininumClusterPointCount").getValue())
{
- ARMARX_VERBOSE << /*deactivateSpam(1, objectName) <<*/ "too few points for PCA for " << objectName << " (" << label << ")";
+ ARMARX_VERBOSE // << deactivateSpam(1, objectName)
+ << "Too few points for PCA for " << objectName << " (" << label << ").";
}
else
{
- auto determineGraspType = [&](const Box3D & box)
- {
- float height = box.Size3();
- float width = box.Size1();
- return width > height ? Armar6GraspProviderModule::GraspType::Top : Armar6GraspProviderModule::GraspType::Side;
- };
FitRectangle fitBox(90);
Box3D box = fitBox.Fit(allPoints);
Eigen::Vector3f center = box.GetPoint(0, 0, 0);
- Armar6GraspProviderModule::GraspType graspType = determineGraspType(box);
+ Armar6GraspProviderModule::GraspType graspType = grasping::determineGraspType(box);
- std::string graspTypeStr = maptools::GetValueOrDefault(config.ObjectGraspTypeMap, objectName, "calculate");
+ std::string graspTypeStr = simox::alg::get_value_or_default(config.ObjectGraspTypeMap, objectName, "calculate");
if (graspTypeStr == "TopBlob")
{
graspType = Armar6GraspProviderModule::GraspType::Top;
@@ -662,7 +608,7 @@ void Armar6GraspProvider::processPointCloud()
Vector2fSeq boxSizePlot;
for (size_t i = 0; i < fitBox.results.size(); i++)
{
- boxSizePlot.push_back(Vector2f {(float)i, fitBox.results.at(i).Volume()});
+ boxSizePlot.push_back(Vector2f {float(i), fitBox.results.at(i).Volume()});
}
plots[objectName] = boxSizePlot;
@@ -805,10 +751,6 @@ void Armar6GraspProvider::processPointCloud()
result.extent3 = extractExtent(p3);
-
-
-
-
// for (const Eigen::Vector3f& p : allPoints)
// {
// result.points.push_back(Armar6GraspProviderModule::Point {p(0), p(1), p(2)});
@@ -816,14 +758,15 @@ void Armar6GraspProvider::processPointCloud()
std::string labelstr = std::to_string(label);
-
- drawLine("ex1_" + labelstr, l1.Get(result.extent1.p05), l1.Get(result.extent1.p95), viz::Color::red(), layerForPC);
- drawLine("ex2_" + labelstr, l2.Get(result.extent2.p05), l2.Get(result.extent2.p95), viz::Color::green(), layerForPC);
- drawLine("ex3_" + labelstr, l3.Get(result.extent3.p05), l3.Get(result.extent3.p95), viz::Color::blue(), layerForPC);
-
- if (config.enableBoundigBoxesVisu)
{
- drawBox(labelstr + "_bbox", box.GetPose(), box.SizeVec(), viz::Color::blue(128), layerForPC);
+ layerForPC.add(drawLine("ex1_" + labelstr, l1.Get(result.extent1.p05), l1.Get(result.extent1.p95), robotToGlobal).color(viz::Color::red()));
+ layerForPC.add(drawLine("ex2_" + labelstr, l2.Get(result.extent2.p05), l2.Get(result.extent2.p95), robotToGlobal).color(viz::Color::green()));
+ layerForPC.add(drawLine("ex3_" + labelstr, l3.Get(result.extent3.p05), l3.Get(result.extent3.p95), robotToGlobal).color(viz::Color::blue()));
+
+ if (config.enableBoundigBoxesVisu)
+ {
+ layerForPC.add(drawBox(labelstr + "_bbox", box.GetPose(), robotToGlobal).size(box.SizeVec()).color(viz::Color::blue(128)));
+ }
}
Eigen::Vector3f preferredForwardTop(-1, 1, 0);
@@ -953,9 +896,8 @@ void Armar6GraspProvider::processPointCloud()
{
ssDebug << ", not reachable";
}
-
- ssDebug << ") " << candidate->side << GraspTypeToString(graspType) << PreshapeTypeToString(grasp.preshape) << " " << ((int)center(0)) << "," << ((int)center(1)) << "," << ((int)center(2)) << std::endl;
-
+ ssDebug << ") " << candidate->side << grasping::graspTypeToString(graspType) << grasping::preshapeTypeToString(grasp.preshape)
+ << " " << int(center(0)) << "," << int(center(1)) << "," << int(center(2)) << std::endl;
resultList.push_back(result);
@@ -975,27 +917,11 @@ void Armar6GraspProvider::processPointCloud()
if (useObjectPoseObserver)
{
- // TODO: Reuse code
- objpose::ObjectPoseSeq objectPoses = getObjectPoses();
+ const objpose::ObjectPoseSeq objectPoses = getObjectPoses();
for (const objpose::ObjectPose& objectPose : objectPoses)
{
-
-
- std::optional<simox::OrientedBoxf> oobbRobot = objectPose.oobbRobot();
- if (!oobbRobot)
- {
- ARMARX_INFO << "Not generating grasp candidates for " << objectPose.objectID
- << " because no bounding box was found";
- continue;
- }
-
- auto& id = objectPose.objectID;
- calculateGraspCandidatesForBB(objectPose,
- id.dataset() + "/" + id.className(),
- id.str(),
- rns, tcp,
- layerForOPO,
- candidates, resultList);
+ calculateGraspCandidatesForObjectPose(
+ objectPose, rns, tcp, layerForOPO, candidates, resultList);
detectedObjects++;
}
@@ -1004,36 +930,22 @@ void Armar6GraspProvider::processPointCloud()
if (useWorkingMemory)
{
// Get object instance channels from working memory
- std::vector<memoryx::ObjectInstanceWrapper> objects = objectInstanceSegment.queryObjects();
- for (memoryx::ObjectInstanceWrapper& object : objects)
+ const std::vector<memoryx::ObjectInstanceWrapper> objects = objectInstanceSegment.queryObjects();
+ for (const memoryx::ObjectInstanceWrapper& object : objects)
{
- // TODO: Get bounding box for objects
- VirtualRobot::ManipulationObjectPtr manipObj = object.manipulationObject;
- VirtualRobot::BoundingBox localBB = manipObj->getCollisionModel()->getTriMeshModel()->boundingBox;
-
- simox::OrientedBoxf oobb = simox::OrientedBoxf(0.5 * (localBB.getMin() + localBB.getMax()),
- Eigen::Quaternionf::Identity(),
- localBB.getMax() - localBB.getMin())
- .transformed(manipObj->getGlobalPose());
-
- /*
- calculateGraspCandidatesForBB(oobb,
- object.instanceInMemory->getMostProbableClass(),
- object.instanceInMemory->getId(),
- rns, tcp,
- layerForWM,
- candidates, resultList);
- */
+ calculateGraspCandidatesForWorkingMemoryInstance(
+ object, localRobot, rns, tcp, layerForWM, candidates, resultList);
+
detectedObjects++;
}
-
}
- debugObserver->setDebugDatafield(getName(), "objects", new Variant(ssDebug.str()));
- staticPlotter->addPlot(getName() + "::ObjectSizes", plots);
-
+ {
+ debugObserver->setDebugDatafield(getName(), "objects", new Variant(ssDebug.str()));
+ staticPlotter->addPlot(getName() + "::ObjectSizes", plots);
- arviz.commit({layerForPC, layerForOPO, layerForWM});
+ arviz.commit({layerForPC, layerForOPO, layerForWM});
+ }
/*debugObserver->setDebugDatafield(getName(), "center_x", new Variant(center(0)));
debugObserver->setDebugDatafield(getName(), "center_y", new Variant(center(1)));
@@ -1047,8 +959,11 @@ void Armar6GraspProvider::processPointCloud()
std::unique_lock lock(resultMutex);
this->resultList = resultList;
processCount++;
- debugObserver->setDebugDatafield(getName(), "processCount", new Variant(processCount));
- debugObserver->setDebugDatafield(getName(), "detectedObjects", new Variant(detectedObjects));
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "processCount", new Variant(processCount) },
+ { "detectedObjects", new Variant(detectedObjects) }
+ });
}
ARMARX_VERBOSE << "Reporting grasp candidates";
graspCandidatesTopic->reportGraspCandidates(getName(), candidates);
@@ -1066,22 +981,6 @@ void Armar6GraspProvider::processPointCloud()
}
}
-armarx::PropertyDefinitionsPtr Armar6GraspProvider::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new Armar6GraspProviderPropertyDefinitions(
- getConfigIdentifier()));
-}
-
-
-bool Armar6GraspProvider::BoundingBox::isInside(float x, float y, float z)
-{
- return x >= x1 && x <= x2 &&
- y >= y1 && y <= y2 &&
- z >= z1 && z <= z2;
-}
-
-
-
/*
CapturedPointSeq armarx::Armar6GraspProvider::getSegmentedPoints(const Ice::Current&)
@@ -1135,65 +1034,67 @@ Ice::Int Armar6GraspProvider::getProcessCount(const Ice::Current&)
}
-void Armar6GraspProvider::calculateGraspCandidatesForBB(
+void Armar6GraspProvider::calculateGraspCandidatesForObjectPose(
const objpose::ObjectPose& objectPose,
- std::string const& className,
- std::string const& instanceName,
VirtualRobot::RobotNodeSetPtr const& rns,
VirtualRobot::RobotNodePtr const& tcp,
viz::Layer& layer,
grasping::GraspCandidateSeq& candidatesOut,
Armar6GraspProviderModule::ResultSeq& resultListOut)
-/*
-void Armar6GraspProvider::calculateGraspCandidatesForBB(
- const simox::OrientedBoxf& oobbRobot,
- std::string const& className,
- std::string const& instanceName,
- VirtualRobot::RobotNodeSetPtr const& rns,
- VirtualRobot::RobotNodePtr const& tcp,
- viz::Layer& layer,
- grasping::GraspCandidateSeq& candidatesOut,
- Armar6GraspProviderModule::ResultSeq& resultListOut)
-*/
{
- std::optional<simox::OrientedBoxf> oobbRobotOpt = objectPose.oobbRobot();
- if (!oobbRobotOpt)
+ std::optional<simox::OrientedBoxf> oobbRobot = objectPose.oobbRobot();
+ if (!oobbRobot)
{
+ ARMARX_INFO << "Not generating grasp candidates for " << objectPose.objectID
+ << " because no bounding box was found";
return;
}
- const simox::OrientedBoxf oobbRobot = *oobbRobotOpt;
+ const std::string className = objectPose.objectID.getClassID().str();
+ const std::string instanceName = objectPose.objectID.str();
+ calculateGraspCandidatesForBB(*oobbRobot, objectPose.robotPose,
+ className, instanceName, rns, tcp, layer, candidatesOut, resultListOut);
+}
- auto determineGraspType = [&](const Box3D & box)
- {
- float height = box.Size3();
- float width = box.Size1();
- return width > height ? Armar6GraspProviderModule::GraspType::Top : Armar6GraspProviderModule::GraspType::Side;
- };
+void Armar6GraspProvider::calculateGraspCandidatesForWorkingMemoryInstance(
+ const memoryx::ObjectInstanceWrapper& object,
+ const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp,
+ viz::Layer& layer,
+ grasping::GraspCandidateSeq& candidatesOut,
+ Armar6GraspProviderModule::ResultSeq& resultListOut)
+{
+ // TODO: Get bounding box for objects
+ VirtualRobot::ManipulationObjectPtr manipObj = object.manipulationObject;
+ VirtualRobot::BoundingBox localBB = manipObj->getCollisionModel()->getTriMeshModel()->boundingBox;
- Eigen::Matrix4f robotToGlobal = localRobot->getGlobalPose();
+ const Eigen::Matrix4f objectPoseInRobot = object.instanceInMemory->getPose()->toRootEigen(robot);
- auto drawLine = [&](std::string const & name, Eigen::Vector3f fromRobot, Eigen::Vector3f toRobot, viz::Color color,
- viz::Layer & layer)
- {
- Eigen::Vector4f fromGlobal = robotToGlobal * fromRobot.homogeneous();
- Eigen::Vector4f toGlobal = robotToGlobal * toRobot.homogeneous();
- viz::Cylinder cyl = viz::Cylinder(name)
- .fromTo(fromGlobal.head<3>(), toGlobal.head<3>())
- .radius(3.0f)
- .color(color);
- layer.add(cyl);
- };
+ const simox::OrientedBoxf oobbInRobot = simox::OrientedBoxf(
+ 0.5 * (localBB.getMin() + localBB.getMax()),
+ Eigen::Quaternionf::Identity(),
+ localBB.getMax() - localBB.getMin())
+ .transformed(objectPoseInRobot);
- auto drawBox = [&](std::string const & name, Eigen::Matrix4f const & poseRobot, Eigen::Vector3f size, viz::Color color,
- viz::Layer & layer)
- {
- Eigen::Matrix4f poseGlobal = robotToGlobal * poseRobot;
- viz::Box box = viz::Box(name)
- .pose(poseGlobal)
- .size(size)
- .color(color);
- layer.add(box);
- };
+ const std::string className = object.instanceInMemory->getMostProbableClass();
+ const std::string instanceName = object.instanceInMemory->getId();
+
+ calculateGraspCandidatesForBB(oobbInRobot, localRobot->getGlobalPose(),
+ className, instanceName, rns, tcp, layer, candidatesOut, resultListOut);
+}
+
+void Armar6GraspProvider::calculateGraspCandidatesForBB(
+ const simox::OrientedBoxf& oobbRobot,
+ const Eigen::Matrix4f& robotPose,
+ const std::string& className,
+ const std::string& instanceName,
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp,
+ viz::Layer& layer,
+ grasping::GraspCandidateSeq& candidatesOut,
+ Armar6GraspProviderModule::ResultSeq& resultListOut)
+{
+ const Eigen::Matrix4f robotToGlobal = localRobot->getGlobalPose();
Eigen::Vector3f bbCenter = oobbRobot.center();
// We need to take half the extends
@@ -1206,15 +1107,14 @@ void Armar6GraspProvider::calculateGraspCandidatesForBB(
Eigen::Vector3f v3 = bbExtend3.normalized();
// We need to order the extends by size!
- sortBoxExtendsForGraspCandidateGeneration(v1, v2, v3,
- bbExtend1, bbExtend2, bbExtend3);
+ grasping::sortBoxExtendsForGraspCandidateGeneration(v1, v2, v3, bbExtend1, bbExtend2, bbExtend3);
Box3D box(bbCenter, bbExtend1, bbExtend2, bbExtend3);
Eigen::Vector3f center = box.GetPoint(0, 0, 0);
- Armar6GraspProviderModule::GraspType graspType = determineGraspType(box);
+ Armar6GraspProviderModule::GraspType graspType = grasping::determineGraspType(box);
// TODO: Here we can add fixed grasp types in the config for specific object classes!
- std::string graspTypeStr = maptools::GetValueOrDefault(config.ObjectGraspTypeMap, className, "calculate");
+ std::string graspTypeStr = simox::alg::get_value_or_default(config.ObjectGraspTypeMap, className, "calculate");
if (graspTypeStr == "TopBlob")
{
graspType = Armar6GraspProviderModule::GraspType::Top;
@@ -1333,13 +1233,13 @@ void Armar6GraspProvider::calculateGraspCandidatesForBB(
std::string label = instanceName;
std::string labelstr = label;
- drawLine("ex1_" + labelstr, center - v1, center + v1, viz::Color::red(), layer);
- drawLine("ex2_" + labelstr, center - v2, center + v2, viz::Color::green(), layer);
- drawLine("ex3_" + labelstr, center - v3, center + v3, viz::Color::blue(), layer);
+ layer.add(drawLine("ex1_" + labelstr, center - v1, center + v1, robotToGlobal).color(viz::Color::red()));
+ layer.add(drawLine("ex2_" + labelstr, center - v2, center + v2, robotToGlobal).color(viz::Color::green()));
+ layer.add(drawLine("ex3_" + labelstr, center - v3, center + v3, robotToGlobal).color(viz::Color::blue()));
if (config.enableBoundigBoxesVisu)
{
- drawBox(labelstr + "_bbox", box.GetPose(), box.SizeVec(), viz::Color::blue(128), layer);
+ layer.add(drawBox(labelstr + "_bbox", box.GetPose(), robotToGlobal).size(box.SizeVec()).color(viz::Color::blue(128)));
}
Eigen::Vector3f preferredForwardTop(-1, 1, 0);
@@ -1410,7 +1310,7 @@ void Armar6GraspProvider::calculateGraspCandidatesForBB(
result.side = "Right";
result.graspDirection = graspType;
- candidate->robotPose = new Pose(objectPose.robotPose); //localRobot->getGlobalPose());
+ candidate->robotPose = new Pose(robotPose);
candidate->graspPose = new Pose(grasp.pose);
// TODO: What should we put into the group number?
// TODO: Make a member variable for keeping track of groupNrs for object instances
@@ -1456,7 +1356,7 @@ void Armar6GraspProvider::calculateGraspCandidatesForBB(
std::string modelFile = "Armar6RT/robotmodel/Armar6-SH/Armar6-" + graspSide + "Hand-v3.xml";
viz::Robot hand = viz::Robot(labelstr + "_grasp")
.file("Armar6RT", modelFile)
- .pose(objectPose.robotPose * graspPose * tcp2handRoot)
+ .pose(robotPose * graspPose * tcp2handRoot)
.overrideColor(color)
.joints(fingerJointsVisu);
layer.add(hand);
@@ -1466,5 +1366,3 @@ void Armar6GraspProvider::calculateGraspCandidatesForBB(
resultListOut.push_back(result);
}
}
-
-
diff --git a/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.h b/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.h
index 001358df1f9f49849288b311235d4249e3cdbfc0..276d1088625c8b9f6f8ab532fb1303d3d1260813 100644
--- a/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.h
+++ b/source/Armar6Skills/components/Armar6GraspProvider/Armar6GraspProvider.h
@@ -22,38 +22,39 @@
#pragma once
+#include <mutex>
-#include "RobotAPI/components/ArViz/Client/elements/Color.h"
-#include <ArmarXCore/core/Component.h>
-
-#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
-#include <Armar6Skills/interface/Armar6GraspProviderInterface.h>
-#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-#include <RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <pcl/point_types.h>
-
-#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
-#include <pcl/common/transforms.h>
-#include <pcl/filters/passthrough.h>
-
-#include <Armar6Skills/libraries/Armar6Tools/Armar6GraspTrajectory.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/interface/StaticPlotterInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/components/ArViz/Client/elements/Color.h>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
+#include <RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/ObjectInstanceSegmentWrapper.h>
-#include <VirtualRobot/math/Line.h>
+#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
+
+#include <Armar6Skills/interface/Armar6GraspProviderInterface.h>
+#include <Armar6Skills/libraries/Armar6Tools/Armar6GraspTrajectory.h>
-#include <mutex>
+
+namespace armarx
+{
+ class RapidXmlReaderNode;
+}
namespace armarx
{
@@ -63,61 +64,14 @@ namespace armarx
* @class Armar6GraspProviderPropertyDefinitions
* @brief
*/
- class Armar6GraspProviderPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class Armar6GraspProviderPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
{
public:
- Armar6GraspProviderPropertyDefinitions(std::string prefix):
- armarx::ComponentPropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the debug observer that should be used");
- defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Name of the debug drawer topic that should be used");
- defineOptionalProperty<std::string>("StaticPlotterName", "StaticPlotter", "Name of the static plotter that should be used");
-
- defineOptionalProperty<std::string>("GraspCandidatesTopicName", "GraspCandidatesTopic", "Name of the Grasp Candidate Topic");
- defineOptionalProperty<std::string>("ConfigTopicName", "GraspCandidateProviderConfigTopic", "Name of the Grasp Candidate Provider Config Topic");
-
- defineOptionalProperty<std::string>("PointCloudFormat", "XYZRGBL", "Format of the input and output point cloud (XYZRGBA, XYZL, XYZRGBL)");
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
- defineOptionalProperty<std::string>("providerName", "TabletopSegmentationResult", "name of the point cloud provider");
- defineOptionalProperty<std::string>("serviceProviderName", "TabletopSegmentation", "name of the service provider");
- defineOptionalProperty<std::string>("sourceFrameName", "root", "The source frame name");
-
- defineOptionalProperty<uint32_t>("BackgroundLabelId", 1, "Label in the pointcloud for the plane or background.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("UseObjectIdMap", false, "Use ID map from config for named objects");
-
- defineOptionalProperty<float>("DownsamplingLeafSize", 5.f, "If >0, the pointcloud is downsampled with a leaf size of this value. In mm.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<size_t>("MininumClusterPointCount", 100, "Minimum number of points per cluster/segmented object to be further processed.", PropertyDefinitionBase::eModifiable);
-
- defineOptionalProperty<float>("BoundingBoxZ1", 975.f, "");
- defineOptionalProperty<float>("BoundingBoxY1", 0.f, "");
- defineOptionalProperty<float>("BoundingBoxY2", 1300.f, "");
-
- defineOptionalProperty<float>("BoundingBoxX1", -200, "");
- defineOptionalProperty<float>("BoundingBoxX2", 200, "");
-
- defineOptionalProperty<bool>("enablePointCloudsVisu", true, "");
- defineOptionalProperty<bool>("enableVisu", true, "");
- defineOptionalProperty<bool>("enableCandidatesVisu", true, "");
- defineOptionalProperty<bool>("enableBoundigBoxesVisu", false, "");
- defineOptionalProperty<bool>("FlattenInZ", true, "");
- defineOptionalProperty<bool>("UseObjectPoseObserver", false, "");
- defineOptionalProperty<bool>("UseWorkingMemory", false, "");
- defineOptionalProperty<int>("WaitForPointCloudsMS", 200, "");
-
- defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote gui");
- defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge");
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory");
-
-
- defineOptionalProperty<int>("colorNonReachableGraspsR", 128, "The red color channel for non reachable grasps");
- defineOptionalProperty<int>("colorNonReachableGraspsG", 128, "The green color channel for non reachable grasps");
- defineOptionalProperty<int>("colorNonReachableGraspsB", 128, "The blue color channel color channel for non reachable grasps");
- }
+ Armar6GraspProviderPropertyDefinitions(std::string prefix);
};
-
/**
* @defgroup Component-Armar6GraspProvider Armar6GraspProvider
* @ingroup Armar6Skills-Components
@@ -136,27 +90,13 @@ namespace armarx
, virtual public ArVizComponentPluginUser
{
public:
- /**
- * @see armarx::ManagedIceObject::getDefaultName()
- */
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override
{
return "Armar6GraspProvider";
}
- struct BoundingBox
- {
- float x1 = -10000;
- float x2 = 10000;
- float y1 = -10000;
- float y2 = 10000;
- float z1 = -10000;
- float z2 = 10000;
-
- bool isInside(float x, float y, float z);
- };
-
-
struct Config
{
@@ -190,11 +130,8 @@ namespace armarx
{
return enableCandidatesVisu && IceUtil::Time::now() > disableCandidatesVisuUntil;
}
-
};
-
-
struct GraspDescription
{
Eigen::Matrix4f pose;
@@ -202,43 +139,28 @@ namespace armarx
Armar6GraspProviderModule::PreshapeType preshape;
};
- static std::string GraspTypeToString(Armar6GraspProviderModule::GraspType graspType);
- static std::string PreshapeTypeToString(Armar6GraspProviderModule::PreshapeType preshapeType);
-
protected:
- /**
- * @see visionx::PointCloudProcessor::onInitPointCloudProcessor()
- */
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see visionx::PointCloudProcessor::onInitPointCloudProcessor()
void onInitPointCloudProcessor() override;
- /**
- * @see visionx::PointCloudProcessor::onConnectPointCloudProcessor()
- */
+ /// @see visionx::PointCloudProcessor::onConnectPointCloudProcessor()
void onConnectPointCloudProcessor() override;
- /**
- * @see visionx::PointCloudProcessor::onDisconnectPointCloudProcessor()
- */
+ /// @see visionx::PointCloudProcessor::onDisconnectPointCloudProcessor()
void onDisconnectPointCloudProcessor() override;
- /**
- * @see visionx::PointCloudProcessor::onExitPointCloudProcessor()
- */
+ /// @see visionx::PointCloudProcessor::onExitPointCloudProcessor()
void onExitPointCloudProcessor() override;
- /**
- * @see visionx::PointCloudProcessor::process()
- */
+ /// @see visionx::PointCloudProcessor::process()
void process() override;
void processPointCloud();
- /**
- * @see PropertyUser::createPropertyDefinitions()
- */
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
-
// GraspCandidateProviderInterface interface
public:
void requestService(const std::string& providerName, Ice::Int relativeTimeoutMS, const Ice::Current&) override;
@@ -251,23 +173,45 @@ namespace armarx
private:
+
void readConfig();
std::map<std::string, Eigen::Matrix3f> readQuaternionMap(const RapidXmlReaderNode& node);
std::map<int, std::string> readObjectIdMap(const RapidXmlReaderNode& node);
std::map<std::string, std::string> readObjectGraspTypeMap(const RapidXmlReaderNode& node);
std::map<std::string, Eigen::Vector3f> readVectorMap(const RapidXmlReaderNode& node);
std::map<std::string, float> readPreshapeVisu(const RapidXmlReaderNode& node);
+
void calculateReferenceOrientations();
grasping::ProviderInfoPtr getProviderInfo();
- void calculateGraspCandidatesForBB(const objpose::ObjectPose& objectPose, // simox::OrientedBoxf const& bb,
- std::string const& className,
- std::string const& instanceName,
- VirtualRobot::RobotNodeSetPtr const& rns,
- VirtualRobot::RobotNodePtr const& tcp,
- viz::Layer& layer,
- grasping::GraspCandidateSeq& candidatesOut,
- Armar6GraspProviderModule::ResultSeq& resultListOut);
+
+ void calculateGraspCandidatesForObjectPose(
+ const objpose::ObjectPose& objectPose,
+ VirtualRobot::RobotNodeSetPtr const& rns,
+ VirtualRobot::RobotNodePtr const& tcp,
+ viz::Layer& layer,
+ grasping::GraspCandidateSeq& candidatesOut,
+ Armar6GraspProviderModule::ResultSeq& resultListOut);
+
+ void calculateGraspCandidatesForWorkingMemoryInstance(
+ memoryx::ObjectInstanceWrapper const& instance,
+ VirtualRobot::RobotPtr const& robot,
+ VirtualRobot::RobotNodeSetPtr const& rns,
+ VirtualRobot::RobotNodePtr const& tcp,
+ viz::Layer& layer,
+ grasping::GraspCandidateSeq& candidatesOut,
+ Armar6GraspProviderModule::ResultSeq& resultListOut);
+
+ void calculateGraspCandidatesForBB(
+ simox::OrientedBoxf const& bb,
+ Eigen::Matrix4f const& robotPose,
+ std::string const& className,
+ std::string const& instanceName,
+ VirtualRobot::RobotNodeSetPtr const& rns,
+ VirtualRobot::RobotNodePtr const& tcp,
+ viz::Layer& layer,
+ grasping::GraspCandidateSeq& candidatesOut,
+ Armar6GraspProviderModule::ResultSeq& resultListOut);
private:
@@ -287,7 +231,7 @@ namespace armarx
std::string sourceFrameName;
std::string providerName;
- BoundingBox boundingBox;
+ simox::AxisAlignedBoundingBox boundingBox { -1e4, 1e4, -1e4, 1e4, -1e4, 1e4 };
bool flattenInZ = false;
bool useObjectPoseObserver = false;
bool useWorkingMemory = false;
diff --git a/source/Armar6Skills/components/Armar6GraspProvider/CMakeLists.txt b/source/Armar6Skills/components/Armar6GraspProvider/CMakeLists.txt
index 97c36a898941e85420e2ee5b09920bc37906b069..31fe5b94f783da1585cccfa0cec3c56c67716360 100644
--- a/source/Armar6Skills/components/Armar6GraspProvider/CMakeLists.txt
+++ b/source/Armar6Skills/components/Armar6GraspProvider/CMakeLists.txt
@@ -37,10 +37,14 @@ set(COMPONENT_LIBS
set(SOURCES
Armar6GraspProvider.cpp
FitRectangle.cpp
+
+ grasping_util.cpp
)
set(HEADERS
Armar6GraspProvider.h
FitRectangle.h
+
+ grasping_util.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/Armar6Skills/components/Armar6GraspProvider/grasping_util.cpp b/source/Armar6Skills/components/Armar6GraspProvider/grasping_util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b4ad9f7962a310b877b8af755064f6c729c2439
--- /dev/null
+++ b/source/Armar6Skills/components/Armar6GraspProvider/grasping_util.cpp
@@ -0,0 +1,86 @@
+#include "grasping_util.h"
+
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
+
+namespace armarx
+{
+
+ std::string grasping::graspTypeToString(Armar6GraspProviderModule::GraspType graspType)
+ {
+ switch (graspType)
+ {
+ case Armar6GraspProviderModule::GraspType::Top:
+ return "Top";
+ case Armar6GraspProviderModule::GraspType::Side:
+ return "Side";
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(Armar6GraspProviderModule::GraspType, graspType);
+ }
+
+
+ std::string grasping::preshapeTypeToString(Armar6GraspProviderModule::PreshapeType preshapeType)
+ {
+ switch (preshapeType)
+ {
+ case Armar6GraspProviderModule::PreshapeType::Open:
+ return "Open";
+ case Armar6GraspProviderModule::PreshapeType::Preshaped:
+ return "Preshaped";
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(Armar6GraspProviderModule::PreshapeType, preshapeType);
+ }
+
+
+
+
+ void grasping::sortBoxExtendsForGraspCandidateGeneration(
+ Eigen::Vector3f& v1, Eigen::Vector3f& v2, Eigen::Vector3f& v3,
+ Eigen::Vector3f& robotExtend1, Eigen::Vector3f& robotExtend2, Eigen::Vector3f& robotExtend3)
+ {
+ // These have to be sorted in a specific way
+ // v3 should point in the direction of Z
+ // v1 should point along the longer remaining axis (after aligning v3)
+ Eigen::Vector3f z = Eigen::Vector3f::UnitZ();
+ if (std::abs(v1.dot(z)) > 0.8f)
+ {
+ std::swap(v1, v3);
+ std::swap(robotExtend1, robotExtend3);
+ }
+ if (std::abs(v2.dot(z)) > 0.8f)
+ {
+ std::swap(v2, v3);
+ std::swap(robotExtend2, robotExtend3);
+ }
+
+ if (v3.z() < 0.0f)
+ {
+ v3 = -v3;
+ robotExtend3 = -robotExtend3;
+ }
+
+ if (robotExtend1.norm() < robotExtend2.norm())
+ {
+ Eigen::Vector3f temp = v1;
+ v1 = v2;
+ v2 = -temp;
+
+ temp = robotExtend1;
+ robotExtend1 = robotExtend2;
+ robotExtend2 = -temp;
+ }
+ }
+
+ Armar6GraspProviderModule::GraspType grasping::determineGraspType(const math::Box3D & box)
+ {
+ float height = box.Size3();
+ float width = box.Size1();
+ return width > height ? Armar6GraspProviderModule::GraspType::Top
+ : Armar6GraspProviderModule::GraspType::Side;
+ }
+
+
+
+}
diff --git a/source/Armar6Skills/components/Armar6GraspProvider/grasping_util.h b/source/Armar6Skills/components/Armar6GraspProvider/grasping_util.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac4cf8e0b2a63c9b0d7cfd79c30778789228cff8
--- /dev/null
+++ b/source/Armar6Skills/components/Armar6GraspProvider/grasping_util.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <string>
+
+#include <Eigen/Core>
+
+#include <Armar6Skills/interface/Armar6GraspProviderInterface.h>
+
+#include "FitRectangle.h"
+
+namespace armarx::grasping
+{
+
+ std::string graspTypeToString(Armar6GraspProviderModule::GraspType graspType);
+ std::string preshapeTypeToString(Armar6GraspProviderModule::PreshapeType preshapeType);
+
+
+ void sortBoxExtendsForGraspCandidateGeneration(Eigen::Vector3f& v1,
+ Eigen::Vector3f& v2,
+ Eigen::Vector3f& v3,
+ Eigen::Vector3f& robotExtend1,
+ Eigen::Vector3f& robotExtend2,
+ Eigen::Vector3f& robotExtend3);
+
+ Armar6GraspProviderModule::GraspType determineGraspType(const math::Box3D & box);
+
+
+}
+