diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index 7bd52aec0ffcf6e423cd959f1a8399ba107045ac..16338f1b1b7c294b644f5a9095c645764f9aabee 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -23,12 +23,15 @@
#include "ObjectPoseProviderExample.h"
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <SimoxUtility/math/pose/pose.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+
namespace armarx
{
@@ -118,7 +121,7 @@ namespace armarx
std::map<ObjectID, ObjectInfo> objectInfos;
- while (poseEstimationTask && !poseEstimationTask->isStopped())
+ while (poseEstimationTask and not poseEstimationTask->isStopped())
{
IceUtil::Time now = TimeUtil::GetTime();
float t = float((now - start).toSecondsDouble());
@@ -136,7 +139,7 @@ namespace armarx
}
int i = 0;
- armarx::objpose::data::ProvidedObjectPoseSeq poses;
+ armarx::objpose::ProvidedObjectPoseSeq poses;
for (const ObjectID& id : update.current)
{
if (objectInfos.count(id) == 0)
@@ -152,27 +155,29 @@ namespace armarx
}
const ObjectInfo& info = objectInfos.at(id);
- armarx::objpose::data::ProvidedObjectPose& pose = poses.emplace_back();
+ armarx::objpose::ProvidedObjectPose& pose = poses.emplace_back();
pose.providerName = getName();
pose.objectType = objpose::ObjectType::KnownObject;
- pose.objectID.dataset = info.id().dataset();
- pose.objectID.className = info.id().className();
- pose.objectID.instanceName = info.id().instanceName();
+ pose.objectID = info.id();
Eigen::Vector3f pos = 200 * Eigen::Vector3f(std::sin(t - i), std::cos(t - i), i);
- Eigen::Quaternionf ori = Eigen::Quaternionf::Identity();
- pose.objectPose = new Pose(ori.toRotationMatrix(), pos);
+ Eigen::AngleAxisf ori((t - i) / M_PI_2, Eigen::Vector3f::UnitZ());
+ pose.objectPose = simox::math::pose(pos, ori);
pose.objectPoseFrame = armarx::GlobalFrame;
- pose.confidence = 1.0;
- pose.timestampMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ pose.objectPoseGaussian = objpose::PoseManifoldGaussian();
+ pose.objectPoseGaussian->mean = pose.objectPose;
+ pose.objectPoseGaussian->covariance = Eigen::Matrix6f::Identity();
+
+ pose.confidence = 0.75 + 0.25 * std::sin(t - i);
+ pose.timestamp = TimeUtil::GetTime();
i++;
}
ARMARX_VERBOSE << "Reporting " << poses.size() << " object poses";
- objectPoseTopic->reportObjectPoses(getName(), poses);
+ objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses));
if (singleShot)
{
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index 8de0dac4589f9ab3333becd9a464a0ef236231f5..74cc8bb1bab63c7baf7373d084866597a7f0d000 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -53,7 +53,7 @@ namespace armarx
*/
class ObjectPoseProviderExample :
virtual public armarx::Component
- // Derive from the provider plugin.
+ // Derive from the provider plugin.
, virtual public armarx::ObjectPoseProviderPluginUser
{
public:
@@ -70,6 +70,9 @@ namespace armarx
protected:
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -82,9 +85,6 @@ namespace armarx
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
private:
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp
index fb1dcf83cf6e9ce00401bbcef75d0518f7226612..ee47310699ea73207a266a6db7319dd2fa09a70d 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp
@@ -128,4 +128,5 @@ void armarx::TopicTimingClient::reportSmall(const topic_timing::SmallData& data,
void armarx::TopicTimingClient::reportBig(const topic_timing::BigData& data, const Ice::Current&)
{
IceUtil::Time sentTime = IceUtil::Time::microSeconds(data.sentTimestamp);
+ (void) sentTime;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index f779b851b17ecfdeb4316013234b62b7696e8cea..94c2946f758d1a387f49000c15d38082ab633877 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -952,14 +952,21 @@ namespace armarx
const double newAcceleration = (newState == State::Keep) ? currentAcc : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc);
result.acceleration = newAcceleration;
result.jerk = usedJerk;
+
const double usedDeceleration = hardBrakingNeeded ?
-std::abs(currentV * currentV / 2.f / math::MathUtils::LimitTo(positionError, 0.0001)) :
newAcceleration;
+ (void) usedDeceleration;
+
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
hardBrakingNeeded ||
sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ (void) decelerate;
+
const double deltaVel = ctrlutil::v(useddt, 0, newAcceleration, usedJerk);//signV * newAcceleration * useddt;
+ (void) deltaVel;
+
double newTargetVelRampCtrl = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);//boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
newTargetVelRampCtrl = std::clamp(newTargetVelRampCtrl, -maxV, maxV);
bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl); //||
@@ -1052,6 +1059,7 @@ namespace armarx
const double posWhenBrakingWithCustomJerk = ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk);
// const double brakingDistanceWithCustomJerk = ctrlutil::s(tmp_t, 0, currentV, tmp_acc, curVDir * tmp_jerk);
const double posWhenBrakingWithFixedJerk = ctrlutil::s(brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk);
+ (void) calculatedJerk, (void) posWhenBrakingWithCustomJerk, (void) posWhenBrakingWithFixedJerk;
ARMARX_INFO << VAROUT((int)state) << VAROUT(distance) << VAROUT(brakingDistance);//VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk);
@@ -1100,12 +1108,15 @@ namespace armarx
double accAfterBraking = ctrlutil::a(t_to_0, currentAcc, curVDir * newJerk);
const double posWhenBrakingNow = ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk);
const double simpleBrakingDistance = ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk);
+ (void) tmpV, (void) vAfterBraking, (void) accAfterBraking, (void) posWhenBrakingNow, (void) simpleBrakingDistance;
double acc_at_t = ctrlutil::a(t_to_0, std::abs(currentAcc), -newJerk);
std::tie(t, a0, newJerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
double at = ctrlutil::a(t, a0, newJerk);
double vt = ctrlutil::v(t, currentV, a0, newJerk);
double st = ctrlutil::s(t, targetPosition - currentPosition, currentV, a0, newJerk);
+ (void) at, (void) vt, (void) st;
+
if (this->state <= State::Keep)
{
newState = State::DecAccIncJerk;
@@ -1295,6 +1306,7 @@ namespace armarx
VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAccelerationAndPositionBounds::run() const
{
const double useddt = std::min(std::abs(dt), std::abs(maxDt));
+ (void) useddt;
// if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
// {
@@ -1513,6 +1525,7 @@ namespace armarx
double newAcc = currentAcc + jerk * dt;
double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk); // assumes that fixed jerk would be used, which is not the case here
+ (void) accAtEnd;
std::vector<State> states;
std::vector<State> states2;
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 7ce071730ed3f35b8f2e526c6bf585d000af73f8..4b09fd18ef1c6fe771200b3338ae67073616c42b 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -23,6 +23,9 @@
#pragma once
+//fq
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+
#include <RobotAPI/interface/core/PoseBase.ice>
#include <RobotAPI/interface/core/NameValueMap.ice>
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice>
@@ -61,6 +64,18 @@ module armarx
module data
{
+ /**
+ * @brief A "Gaussian" distribution on the pose manifold.
+ *
+ * @see `armarx::objpose::PoseManifoldGaussian`
+ */
+ class PoseManifoldGaussian // class => optional
+ {
+ PoseBase mean;
+ Ice::FloatSeq covariance6x6;
+ };
+
+
/// An object pose provided by an ObjectPoseProvider.
struct ProvidedObjectPose
{
@@ -77,6 +92,8 @@ module armarx
/// Pose in `objectPoseFrame`.
PoseBase objectPose;
string objectPoseFrame;
+ PoseManifoldGaussian objectPoseGaussian;
+
/// The object's joint values if it is articulated.
NameValueMap objectJointValues;
@@ -108,9 +125,14 @@ module armarx
armarx::data::ObjectID objectID;
PoseBase objectPoseRobot;
+ PoseManifoldGaussian objectPoseRobotGaussian;
+
PoseBase objectPoseGlobal;
+ PoseManifoldGaussian objectPoseGlobalGaussian;
+
PoseBase objectPoseOriginal;
string objectPoseOriginalFrame;
+ PoseManifoldGaussian objectPoseOriginalGaussian;
/// The object's joint values if it is articulated.
NameValueMap objectJointValues;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index b2e71660ae1552d33eeaddb37f93a6f6b5d72491..da1d938b96fc8e7fe3d2a9cb2c0f25df386a06af 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -8,6 +8,7 @@ set(LIBS
RemoteGui
# RobotAPI
RobotAPI::Core
+ RobotAPIInterfaces
aroncommon
)
@@ -17,6 +18,8 @@ set(LIB_FILES
ObjectFinder.cpp
ObjectPose.cpp
ObjectPoseClient.cpp
+ PoseManifoldGaussian.cpp
+ ProvidedObjectPose.cpp
json_conversions.cpp
ice_conversions.cpp
@@ -35,7 +38,10 @@ set(LIB_HEADERS
ObjectFinder.h
ObjectPose.h
ObjectPoseClient.h
+ PoseManifoldGaussian.h
+ ProvidedObjectPose.h
+ forward_declarations.h
json_conversions.h
ice_conversions.h
@@ -61,6 +67,7 @@ armarx_enable_aron_file_generation_for_target(
aron/ObjectNames.xml
aron/ObjectPose.xml
aron/ObjectType.xml
+ aron/PoseManifoldGaussian.xml
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 51fb57e6896277be3538672fce00c1096bc60951..6b5619647ec122f51ea0837f76c2bfe948f26ed2 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -32,15 +32,20 @@ namespace armarx::objpose
isStatic = ice.isStatic;
armarx::fromIce(ice.objectID, objectID);
- objectPoseRobot = ::armarx::fromIce(ice.objectPoseRobot);
- objectPoseGlobal = ::armarx::fromIce(ice.objectPoseGlobal);
- objectPoseOriginal = ::armarx::fromIce(ice.objectPoseOriginal);
+ armarx::fromIce(ice.objectPoseRobot, objectPoseRobot);
+ objpose::fromIce(ice.objectPoseRobotGaussian, objectPoseRobotGaussian);
+
+ armarx::fromIce(ice.objectPoseGlobal, objectPoseGlobal);
+ objpose::fromIce(ice.objectPoseGlobalGaussian, objectPoseGlobalGaussian);
+
+ armarx::fromIce(ice.objectPoseOriginal, objectPoseOriginal);
objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
+ objpose::fromIce(ice.objectPoseOriginalGaussian, objectPoseOriginalGaussian);
objectJointValues = ice.objectJointValues;
robotConfig = ice.robotConfig;
- robotPose = ::armarx::fromIce(ice.robotPose);
+ armarx::fromIce(ice.robotPose, robotPose);
objpose::fromIce(ice.attachment, this->attachment);
@@ -64,10 +69,16 @@ namespace armarx::objpose
ice.isStatic = isStatic;
armarx::toIce(ice.objectID, objectID);
- ice.objectPoseRobot = new Pose(objectPoseRobot);
- ice.objectPoseGlobal = new Pose(objectPoseGlobal);
- ice.objectPoseOriginal = new Pose(objectPoseOriginal);
+ armarx::toIce(ice.objectPoseRobot, objectPoseRobot);
+ objpose::toIce(ice.objectPoseRobotGaussian, objectPoseRobotGaussian);
+
+ armarx::toIce(ice.objectPoseGlobal, objectPoseGlobal);
+ objpose::toIce(ice.objectPoseGlobalGaussian, objectPoseGlobalGaussian);
+
+ armarx::toIce(ice.objectPoseOriginal, objectPoseOriginal);
ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
+ objpose::toIce(ice.objectPoseOriginalGaussian, objectPoseOriginalGaussian);
+
ice.objectJointValues = objectJointValues;
ice.robotConfig = robotConfig;
@@ -88,15 +99,20 @@ namespace armarx::objpose
isStatic = provided.isStatic;
armarx::fromIce(provided.objectID, objectID);
- objectPoseOriginal = ::armarx::fromIce(provided.objectPose);
+ armarx::fromIce(provided.objectPose, objectPoseOriginal);
+ objpose::fromIce(provided.objectPoseGaussian, objectPoseOriginalGaussian);
objectPoseOriginalFrame = provided.objectPoseFrame;
+
objectJointValues = provided.objectJointValues;
armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
framed.changeFrame(robot, robot->getRootNode()->getName());
objectPoseRobot = framed.toEigen();
+ objectPoseRobotGaussian = std::nullopt; // ToDo: Transform covariance matrices
+
framed.changeToGlobal(robot);
objectPoseGlobal = framed.toEigen();
+ objectPoseGlobalGaussian = std::nullopt; // ToDo: Transform covariance matrices
robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
robotPose = robot->getGlobalPose();
@@ -174,6 +190,7 @@ namespace armarx::objpose
robotPose = agent->getGlobalPose();
robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
}
+
}
@@ -211,7 +228,6 @@ namespace armarx
return info;
}
-
void objpose::toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment)
{
ice.agentName = attachment.agentName;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index ca5754aa0dff87780e913f7c255bf2b9067e2258..74b1b3e6dd6632ca7c11dcc060f5934a24725262 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -8,13 +8,17 @@
#include <Eigen/Core>
#include <SimoxUtility/shapes/OrientedBox.h>
+
#include <VirtualRobot/VirtualRobot.h>
-#include "ArmarXCore/core/PackagePath.h"
+
+#include <ArmarXCore/core/PackagePath.h>
#include <IceUtil/Time.h>
#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
namespace armarx::objpose
@@ -33,6 +37,8 @@ namespace armarx::objpose
*/
struct ObjectPose
{
+ public:
+
ObjectPose();
ObjectPose(const data::ObjectPose& ice);
@@ -47,6 +53,8 @@ namespace armarx::objpose
void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);
+ public:
+
/// Name of the providing component.
std::string providerName;
/// Known or unknown object.
@@ -57,16 +65,21 @@ namespace armarx::objpose
/// The object ID, i.e. dataset, class name and instance name.
armarx::ObjectID objectID;
- Eigen::Matrix4f objectPoseRobot;
- Eigen::Matrix4f objectPoseGlobal;
- Eigen::Matrix4f objectPoseOriginal;
+ Eigen::Matrix4f objectPoseRobot = Eigen::Matrix4f::Identity();
+ std::optional<PoseManifoldGaussian> objectPoseRobotGaussian;
+
+ Eigen::Matrix4f objectPoseGlobal = Eigen::Matrix4f::Identity();
+ std::optional<PoseManifoldGaussian> objectPoseGlobalGaussian;
+
+ Eigen::Matrix4f objectPoseOriginal = Eigen::Matrix4f::Identity();
std::string objectPoseOriginalFrame;
+ std::optional<PoseManifoldGaussian> objectPoseOriginalGaussian;
/// The object's joint values if it is articulated.
std::map<std::string, float> objectJointValues;
std::map<std::string, float> robotConfig;
- Eigen::Matrix4f robotPose;
+ Eigen::Matrix4f robotPose = Eigen::Matrix4f::Identity();
/// Attachment information.
std::optional<ObjectAttachmentInfo> attachment;
@@ -102,8 +115,6 @@ namespace armarx::objpose
std::optional<PackagePath> articulatedSimoxXmlPath;
};
- using ObjectPoseSeq = std::vector<ObjectPose>;
- using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
void fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cdc43f287801e16421cac616c81dd95e46b76878
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
@@ -0,0 +1,7 @@
+#include "PoseManifoldGaussian.h"
+
+
+namespace armarx::objpose
+{
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
new file mode 100644
index 0000000000000000000000000000000000000000..d321279354c801e8d769f5467061e447549693c3
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <Eigen/Core>
+
+
+namespace armarx::objpose
+{
+
+ /**
+ * @brief The covariance (matrices) of a pose covariance.
+ */
+ struct PoseManifoldGaussian
+ {
+
+ Eigen::Matrix4f mean = Eigen::Matrix4f::Identity();
+ Eigen::Matrix<float, 6, 6> covariance = Eigen::Matrix<float, 6, 6>::Identity();
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c67af0381b114b1b5e145b60a3c239b464f4ec31
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
@@ -0,0 +1,130 @@
+#include "ProvidedObjectPose.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include "ice_conversions.h"
+
+
+namespace armarx::objpose
+{
+ ProvidedObjectPose::ProvidedObjectPose()
+ {
+ }
+
+ ProvidedObjectPose::ProvidedObjectPose(const data::ProvidedObjectPose& ice)
+ {
+ fromIce(ice);
+ }
+
+ void ProvidedObjectPose::fromIce(const data::ProvidedObjectPose& ice)
+ {
+ providerName = ice.providerName;
+ objectType = ice.objectType;
+ isStatic = ice.isStatic;
+ armarx::fromIce(ice.objectID, objectID);
+
+ armarx::fromIce(ice.objectPose, objectPose);
+ objectPoseFrame = ice.objectPoseFrame;
+ objpose::fromIce(ice.objectPoseGaussian, objectPoseGaussian);
+
+ objectJointValues = ice.objectJointValues;
+
+ confidence = ice.confidence;
+ timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds);
+
+ objpose::fromIce(ice.localOOBB, localOOBB);
+ }
+
+ data::ProvidedObjectPose ProvidedObjectPose::toIce() const
+ {
+ data::ProvidedObjectPose ice;
+ toIce(ice);
+ return ice;
+ }
+
+ void ProvidedObjectPose::toIce(data::ProvidedObjectPose& ice) const
+ {
+ ice.providerName = providerName;
+ ice.objectType = objectType;
+ ice.isStatic = isStatic;
+ armarx::toIce(ice.objectID, objectID);
+
+ armarx::toIce(ice.objectPose, objectPose);
+ ice.objectPoseFrame = objectPoseFrame;
+ objpose::toIce(ice.objectPoseGaussian, objectPoseGaussian);
+
+ ice.objectJointValues = objectJointValues;
+
+ ice.confidence = confidence;
+ ice.timestampMicroSeconds = timestamp.toMicroSeconds();
+
+ objpose::toIce(ice.localOOBB, localOOBB);
+ }
+}
+
+namespace armarx
+{
+
+ void objpose::fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose)
+ {
+ pose.fromIce(ice);
+ }
+ objpose::ProvidedObjectPose objpose::fromIce(const data::ProvidedObjectPose& ice)
+ {
+ return ProvidedObjectPose(ice);
+ }
+
+ void objpose::fromIce(const data::ProvidedObjectPoseSeq& ice, ProvidedObjectPoseSeq& poses)
+ {
+ poses.clear();
+ poses.reserve(ice.size());
+ for (const auto& i : ice)
+ {
+ poses.emplace_back(i);
+ }
+ }
+
+ objpose::ProvidedObjectPoseSeq objpose::fromIce(const data::ProvidedObjectPoseSeq& ice)
+ {
+ ProvidedObjectPoseSeq poses;
+ fromIce(ice, poses);
+ return poses;
+ }
+
+
+ void objpose::toIce(data::ProvidedObjectPose& ice, const ProvidedObjectPose& pose)
+ {
+ pose.toIce(ice);
+ }
+
+ objpose::data::ProvidedObjectPose objpose::toIce(const ProvidedObjectPose& pose)
+ {
+ return pose.toIce();
+ }
+
+
+ void objpose::toIce(data::ProvidedObjectPoseSeq& ice, const ProvidedObjectPoseSeq& poses)
+ {
+ ice.clear();
+ ice.reserve(poses.size());
+ for (const auto& p : poses)
+ {
+ ice.emplace_back(p.toIce());
+ }
+ }
+
+ objpose::data::ProvidedObjectPoseSeq objpose::toIce(const ProvidedObjectPoseSeq& poses)
+ {
+ data::ProvidedObjectPoseSeq ice;
+ toIce(ice, poses);
+ return ice;
+ }
+
+}
+
+
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
new file mode 100644
index 0000000000000000000000000000000000000000..c323a879508a60ad1dfd6b4af58dc4c4298ec534
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
@@ -0,0 +1,83 @@
+#pragma once
+
+#include <optional>
+#include <map>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <IceUtil/Time.h>
+
+#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
+
+
+namespace armarx::objpose
+{
+
+ /**
+ * @brief An object pose provided by an ObjectPoseProvider.
+ */
+ class ProvidedObjectPose
+ {
+ public:
+
+ ProvidedObjectPose();
+ ProvidedObjectPose(const data::ProvidedObjectPose& ice);
+
+ void fromIce(const data::ProvidedObjectPose& ice);
+
+ data::ProvidedObjectPose toIce() const;
+ void toIce(data::ProvidedObjectPose& ice) const;
+
+ public:
+
+ /// Name of the providing component.
+ std::string providerName;
+ /// Known or unknown object.
+ ObjectType objectType = AnyObject;
+ /// Whether object is static. Static objects don't decay.
+ bool isStatic = false;
+
+ /// The object ID, i.e. dataset, class name and instance name.
+ armarx::ObjectID objectID;
+
+ Eigen::Matrix4f objectPose = Eigen::Matrix4f::Identity();
+ std::string objectPoseFrame;
+ std::optional<PoseManifoldGaussian> objectPoseGaussian;
+
+ /// The object's joint values if it is articulated.
+ std::map<std::string, float> objectJointValues;
+
+
+ /// Confidence in [0, 1] (1 = full, 0 = none).
+ float confidence = 0;
+ /// Source timestamp.
+ IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
+
+ /// Object bounding box in object's local coordinate frame.
+ /// @see oobbRobot(), oobbGlobal()
+ std::optional<simox::OrientedBoxf> localOOBB;
+ };
+
+
+
+ void fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose);
+ ProvidedObjectPose fromIce(const data::ProvidedObjectPose& ice);
+
+ void fromIce(const data::ProvidedObjectPoseSeq& ice, ProvidedObjectPoseSeq& poses);
+ ProvidedObjectPoseSeq fromIce(const data::ProvidedObjectPoseSeq& ice);
+
+
+ void toIce(data::ProvidedObjectPose& ice, const ProvidedObjectPose& pose);
+ data::ProvidedObjectPose toIce(const ProvidedObjectPose& pose);
+
+ void toIce(data::ProvidedObjectPoseSeq& ice, const ProvidedObjectPoseSeq& poses);
+ data::ProvidedObjectPoseSeq toIce(const ProvidedObjectPoseSeq& poses);
+
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index ba26c1a618746e41823850dc99a93b670bc2a7e9..d88919e229dc79870eae34729c4dfa1ca1666f08 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
@@ -3,16 +3,11 @@ ARON DTO of armarx::objpose::ObjectPose.
-->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- <Include include="<Eigen/Core>" />
- <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
- <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>" />
- <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>" />
- </CodeIncludes>
<AronIncludes>
- <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
- <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" />
- <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" autoinclude="true" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" autoinclude="true" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" autoinclude="true" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml>" autoinclude="true" />
</AronIncludes>
<GenerateTypes>
@@ -50,18 +45,34 @@ ARON DTO of armarx::objpose::ObjectPose.
<Pose />
</ObjectChild>
+
+ <ObjectChild key='objectPoseRobotGaussian'>
+ <armarx::objpose::arondto::PoseManifoldGaussian optional="true" />
+ </ObjectChild>
+
<ObjectChild key='objectPoseGlobal'>
<Pose />
</ObjectChild>
+
+ <ObjectChild key='objectPoseGlobalGaussian'>
+ <armarx::objpose::arondto::PoseManifoldGaussian optional="true" />
+ </ObjectChild>
+
<ObjectChild key='objectPoseOriginal'>
<Pose />
</ObjectChild>
+
<ObjectChild key='objectPoseOriginalFrame'>
<string />
</ObjectChild>
+ <ObjectChild key='objectPoseOriginalGaussian'>
+ <armarx::objpose::arondto::PoseManifoldGaussian optional="true" />
+ </ObjectChild>
+
+
<!-- The object's joint values if it is articulated. -->
<ObjectChild key='objectJointValues'>
<Dict>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9405350bf7494fcbcc0e4b34a9f89afa90bb8ba2
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.xml
@@ -0,0 +1,22 @@
+<!--
+ARON DTO of armarx::objpose::ObjectPose.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::objpose::arondto::PoseManifoldGaussian">
+
+ <ObjectChild key='mean'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='covariance'>
+ <Matrix rows="6" cols="6" type="float32" />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
index 5fc8ebdea395c6b8bcd149c14fcb2fe567a4ace8..bb9fb98e66584e8e22889d79c57900df0df1d99f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -9,6 +9,7 @@
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.aron.generated.h>
void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
@@ -24,6 +25,19 @@ void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAtt
dto.poseInFrame = bo.poseInFrame;
}
+
+void armarx::objpose::fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo)
+{
+ bo.mean = dto.mean;
+ bo.covariance = dto.covariance;
+}
+void armarx::objpose::toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo)
+{
+ dto.mean = bo.mean;
+ dto.covariance = bo.covariance;
+}
+
+
void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
{
switch (dto.value)
@@ -66,9 +80,13 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
fromAron(dto.objectID, bo.objectID);
bo.objectPoseRobot = dto.objectPoseRobot;
+ aron::fromAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
bo.objectPoseGlobal = dto.objectPoseGlobal;
+ aron::fromAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
bo.objectPoseOriginal = dto.objectPoseOriginal;
bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
+ aron::fromAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
+
bo.objectJointValues = dto.objectJointValues;
bo.robotConfig = dto.robotConfig;
@@ -108,9 +126,15 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
toAron(dto.objectID, bo.objectID);
dto.objectPoseRobot = bo.objectPoseRobot;
+ aron::toAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
+
dto.objectPoseGlobal = bo.objectPoseGlobal;
+ aron::toAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
+
dto.objectPoseOriginal = bo.objectPoseOriginal;
dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
+ aron::toAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
+
dto.objectJointValues = bo.objectJointValues;
dto.robotConfig = bo.robotConfig;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
index 6117009c5ca366ad0fa0082161f3ee67d39c2892..32dfe160ccd41007c1ad7d69c39042e8e795c571 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
@@ -1,26 +1,22 @@
#pragma once
#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-namespace armarx::objpose::arondto
-{
- class ObjectAttachmentInfo;
- class ObjectType;
- class ObjectPose;
-}
namespace armarx::objpose
{
- class ObjectAttachmentInfo;
- class ObjectPose;
-
void fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo);
void toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo);
+ void fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo);
+ void toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo);
+
void fromAron(const arondto::ObjectType& dto, ObjectType& bo);
void toAron(arondto::ObjectType& dto, const ObjectType& bo);
void fromAron(const arondto::ObjectPose& dto, ObjectPose& bo);
void toAron(arondto::ObjectPose& dto, const ObjectPose& bo);
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7175785af42fce4284769d47290b3aae3616614
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <map>
+#include <vector>
+
+
+namespace armarx
+{
+ class ObjectID;
+ class ObjectInfo;
+ class ObjectFinder;
+}
+namespace armarx::objpose
+{
+ struct ObjectAttachmentInfo;
+ struct PoseManifoldGaussian;
+
+ struct ObjectPose;
+
+ using ObjectPoseSeq = std::vector<ObjectPose>;
+ using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
+
+ class ProvidedObjectPose;
+
+ using ProvidedObjectPoseSeq = std::vector<ProvidedObjectPose>;
+ using ProvidedObjectPoseMap = std::map<ObjectID, ProvidedObjectPose>;
+
+ class ObjectPoseClient;
+}
+
+// Ice Types
+namespace armarx::objpose::data
+{
+ class PoseManifoldGaussian;
+}
+// Aron Types
+namespace armarx::objpose::arondto
+{
+ class ObjectAttachmentInfo;
+ class ObjectType;
+ class ObjectPose;
+ struct PoseManifoldGaussian;
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
index ccbae7e21c426f92d946dd6744168a04f2c70e7c..de7c0595614d31765639592e0d5babb7858b8912 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
@@ -3,6 +3,7 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
+#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
@@ -12,6 +13,7 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
namespace armarx
@@ -158,4 +160,75 @@ namespace armarx
toIce(box, oobb);
return box;
}
+
+
+ void objpose::fromIce(const data::PoseManifoldGaussian& ice, PoseManifoldGaussian& cov)
+ {
+ // Only construct error message if necessary.
+ if (static_cast<Eigen::Index>(ice.covariance6x6.size()) != cov.covariance.size())
+ {
+ ARMARX_CHECK_EQUAL(static_cast<Eigen::Index>(ice.covariance6x6.size()), cov.covariance.size())
+ << "Float sequence representing 6x6 covariance matrix must have " << cov.covariance.size()
+ << " values, but has " << ice.covariance6x6.size() << ": \n"
+ << "[" << simox::alg::join(simox::alg::multi_to_string(ice.covariance6x6), ", ") << "]"
+ ;
+ }
+
+ armarx::fromIce(ice.mean, cov.mean);
+ cov.covariance = Eigen::MatrixXf::Map(ice.covariance6x6.data(),
+ cov.covariance.rows(),
+ cov.covariance.cols());
+ }
+
+ void objpose::fromIce(const data::PoseManifoldGaussianPtr& ice, std::optional<PoseManifoldGaussian>& cov)
+ {
+ if (ice)
+ {
+ cov = PoseManifoldGaussian();
+ fromIce(*ice, cov.value());
+ }
+ else
+ {
+ cov = std::nullopt;
+ }
+ }
+
+ std::optional<objpose::PoseManifoldGaussian> objpose::fromIce(const data::PoseManifoldGaussianPtr& ice)
+ {
+ std::optional<objpose::PoseManifoldGaussian> cov;
+ fromIce(ice, cov);
+ return cov;
+ }
+
+
+ void objpose::toIce(data::PoseManifoldGaussian& ice, const PoseManifoldGaussian& cov)
+ {
+ armarx::toIce(ice.mean, cov.mean);
+
+ ice.covariance6x6.resize(cov.covariance.size());
+ Eigen::MatrixXf::Map(ice.covariance6x6.data(),
+ cov.covariance.rows(),
+ cov.covariance.cols()) = cov.covariance;
+ }
+
+ void objpose::toIce(data::PoseManifoldGaussianPtr& ice, const std::optional<PoseManifoldGaussian>& cov)
+ {
+ if (cov.has_value())
+ {
+ ice = new data::PoseManifoldGaussian;
+ toIce(*ice, cov.value());
+ }
+ else
+ {
+ ice = nullptr;
+ }
+ }
+
+ objpose::data::PoseManifoldGaussianPtr objpose::toIce(const std::optional<PoseManifoldGaussian>& cov)
+ {
+ data::PoseManifoldGaussianPtr ice;
+ toIce(ice, cov);
+ return ice;
+ }
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
index 429d6246c18f7a36769d6c3d686c12c175cd8b8f..9ee3dd712a79f8e0f812134b6588778a2b540dea 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
@@ -4,10 +4,11 @@
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.h>
#include <RobotAPI/interface/objectpose/object_pose_types.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include "ObjectPose.h"
-#include "ObjectID.h"
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+
namespace simox
{
@@ -52,4 +53,14 @@ namespace armarx::objpose
void toIce(BoxPtr& box, const std::optional<simox::OrientedBox<float>>& oobb);
void toIce(Box& box, const simox::OrientedBox<float>& oobb);
Box toIce(const simox::OrientedBox<float>& oobb);
+
+
+ void fromIce(const data::PoseManifoldGaussian& ice, PoseManifoldGaussian& cov);
+ void fromIce(const data::PoseManifoldGaussianPtr& ice, std::optional<PoseManifoldGaussian>& cov);
+ std::optional<PoseManifoldGaussian> fromIce(const data::PoseManifoldGaussianPtr& ice);
+
+ void toIce(data::PoseManifoldGaussian& ice, const PoseManifoldGaussian& cov);
+ void toIce(data::PoseManifoldGaussianPtr& ice, const std::optional<PoseManifoldGaussian>& cov);
+ data::PoseManifoldGaussianPtr toIce(const std::optional<PoseManifoldGaussian>& ice);
+
}
diff --git a/source/RobotAPI/libraries/ukfm/CMakeLists.txt b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
index 8a3b22a34c2e87b2dc6f55f533923263b9ba524b..96259ddebcdd577a2e049a2e9dda7546b76f46e4 100644
--- a/source/RobotAPI/libraries/ukfm/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
@@ -22,7 +22,7 @@ set(LIB_FILES
set(LIB_HEADERS
SystemModel.h
- UnscentedKalmanFilter.cpp
+ UnscentedKalmanFilter.h
)
@@ -43,4 +43,4 @@ endif()
if(manif_FOUND)
target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
-endif()
\ No newline at end of file
+endif()