Skip to content
Snippets Groups Projects
Commit d3c924e7 authored by Christoph Pohl's avatar Christoph Pohl
Browse files

Merge branch 'merge/manipulation-pipeline' into 'master' 

Changes from manipulation pipeline paper experiments

See merge request sw/armarx/robot-api!386
parents f85ff722 ab711bf0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+ 2
− 1
View file @ d3c924e7
......@@ -62,7 +62,8 @@ namespace armarx::objpose
std::optional<ObjectPose>
ObjectPoseClient::fetchObjectPose(const ObjectID& objectID) const
{
const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
const auto objectPoses = fetchObjectPoses();
const auto *object = findObjectPoseByID(objectPoses, objectID);
if(object != nullptr)
{
......
source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+ 16
− 17
View file @ d3c924e7
#include "SimpleReaderBase.h"
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem::client::util
{
......@@ -12,9 +11,8 @@ namespace armarx::armem::client::util
{
}
void SimpleReaderBase::registerPropertyDefinitions(
armarx::PropertyDefinitionsPtr& def)
void
SimpleReaderBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
......@@ -26,16 +24,17 @@ namespace armarx::armem::client::util
def->optional(props.coreSegmentName, prefix + "CoreSegment");
}
void SimpleReaderBase::connect()
void
SimpleReaderBase::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName
<< "' ...";
ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName << "' ...";
try
{
memoryReaderClient = memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName << "'";
memoryReaderClient =
memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName
<< "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
......@@ -44,20 +43,20 @@ namespace armarx::armem::client::util
}
}
std::mutex& SimpleReaderBase::memoryReaderMutex()
std::mutex&
SimpleReaderBase::memoryReaderMutex()
{
return memoryMutex;
}
const armem::client::Reader& SimpleReaderBase::memoryReader() const
const armem::client::Reader&
SimpleReaderBase::memoryReader() const
{
return memoryReaderClient;
}
const SimpleReaderBase::Properties& SimpleReaderBase::properties() const
const SimpleReaderBase::Properties&
SimpleReaderBase::properties() const
{
return props;
}
......
source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+ 43
− 67
View file @ d3c924e7
#include "KnownGraspProviderSegment.h"
#include <VirtualRobot/Grasping/GraspSet.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
......@@ -26,83 +28,57 @@ namespace armarx::armem::grasping::segment
if (std::filesystem::is_regular_file(graspFilePath))
{
auto reader = RapidXmlReader::FromFile(graspFilePath);
RapidXmlReaderNode root = reader->getRoot();
std::string objectClassNameXML = root.attribute_value("name");
ARMARX_CHECK_EQUAL(objectClassName, objectClassNameXML);
ARMARX_INFO << "loading " << graspFilePath;
try {
auto manipulationObject = VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
if(manipulationObject == nullptr)
{
ARMARX_WARNING << "Invalid file content: " << graspFilePath;
return std::nullopt;
}
arondto::KnownGraspInfo ret;
ret.correspondingObject.memoryName = "Object";
ret.correspondingObject.coreSegmentName = "Class";
ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
ret.correspondingObject.entityName = info.idStr();
ret.xml.package = fileLocInfo.package;
ret.xml.path = fileLocInfo.relativePath;
arondto::KnownGraspInfo ret;
ret.correspondingObject.memoryName = "Object";
ret.correspondingObject.coreSegmentName = "Class";
ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
ret.correspondingObject.entityName = info.idStr();
ret.xml.package = fileLocInfo.package;
ret.xml.path = fileLocInfo.relativePath;
for (const auto& graspSetNode : root.nodes())
{
if (graspSetNode.name() != "GraspSet")
for (const VirtualRobot::GraspSetPtr& graspSet : manipulationObject->getAllGraspSets())
{
continue;
}
ARMARX_CHECK_NOT_NULL(graspSet);
arondto::KnownGraspSet retGraspSet;
arondto::KnownGraspSet retGraspSet;
retGraspSet.name = graspSetNode.attribute_value("name");
retGraspSet.robot = graspSetNode.attribute_value("RobotType");
retGraspSet.endeffector = graspSetNode.attribute_value("EndEffector");
retGraspSet.name = graspSet->getName();
retGraspSet.robot = graspSet->getRobotType();
retGraspSet.endeffector = graspSet->getEndEffector();
for (const auto& graspNode : graspSetNode.nodes())
{
if (graspNode.name() != "Grasp")
for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
{
continue;
}
ARMARX_CHECK_NOT_NULL(grasp);
arondto::KnownGrasp retGrasp;
retGrasp.name = grasp->getName();
retGrasp.quality = grasp->getQuality();
retGrasp.creator = grasp->getCreationMethod();
retGrasp.pose = grasp->getTransformation();
arondto::KnownGrasp retGrasp;
retGrasp.name = graspNode.attribute_value("name");
retGrasp.quality = std::stof(graspNode.attribute_value("quality"));
retGrasp.creator = graspNode.attribute_value("Creation");
ARMARX_CHECK(graspNode.nodes().size() == 2 or graspNode.nodes().size() == 1);
RapidXmlReaderNode transformNode = graspNode.nodes()[0];
ARMARX_CHECK_EQUAL(transformNode.name(), "Transform");
ARMARX_CHECK_EQUAL(transformNode.nodes().size(), 1);
RapidXmlReaderNode matrixNode = transformNode.nodes()[0];
ARMARX_CHECK_EQUAL(matrixNode.nodes().size(), 4);
RapidXmlReaderNode row0 = matrixNode.nodes()[0];
RapidXmlReaderNode row1 = matrixNode.nodes()[1];
RapidXmlReaderNode row2 = matrixNode.nodes()[2];
RapidXmlReaderNode row3 = matrixNode.nodes()[3];
retGrasp.pose(0, 0) = std::stof(row0.attribute_value("c1"));
retGrasp.pose(0, 1) = std::stof(row0.attribute_value("c2"));
retGrasp.pose(0, 2) = std::stof(row0.attribute_value("c3"));
retGrasp.pose(0, 3) = std::stof(row0.attribute_value("c4"));
retGrasp.pose(1, 0) = std::stof(row1.attribute_value("c1"));
retGrasp.pose(1, 1) = std::stof(row1.attribute_value("c2"));
retGrasp.pose(1, 2) = std::stof(row1.attribute_value("c3"));
retGrasp.pose(1, 3) = std::stof(row1.attribute_value("c4"));
retGrasp.pose(2, 0) = std::stof(row2.attribute_value("c1"));
retGrasp.pose(2, 1) = std::stof(row2.attribute_value("c2"));
retGrasp.pose(2, 2) = std::stof(row2.attribute_value("c3"));
retGrasp.pose(2, 3) = std::stof(row2.attribute_value("c4"));
retGrasp.pose(3, 0) = std::stof(row3.attribute_value("c1"));
retGrasp.pose(3, 1) = std::stof(row3.attribute_value("c2"));
retGrasp.pose(3, 2) = std::stof(row3.attribute_value("c3"));
retGrasp.pose(3, 3) = std::stof(row3.attribute_value("c4"));
ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
retGraspSet.grasps.push_back(retGrasp);
ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
retGraspSet.grasps.push_back(retGrasp);
}
ret.graspSets[retGraspSet.name] = retGraspSet;
}
ret.graspSets[retGraspSet.name] = retGraspSet;
return ret;
} catch (...) {
ARMARX_WARNING << graspFilePath << " is not a manipulation object!";
return std::nullopt;
}
return ret;
}
return std::nullopt;
}
......
source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+ 1
− 1
View file @ d3c924e7
......@@ -98,7 +98,7 @@ namespace armarx::plugins
return;
}
ARMARX_INFO << "Adding skill and set owning provider name" << skillName;
ARMARX_INFO << "Adding skill and set owning provider name `" << skillName << "`.";
auto s = skillImplementations.emplace(skillName, std::move(skill));
s.first->second.statusUpdate.skillId = skills::SkillID(parent().getName(), skillName);
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment