From 0887eb4559da436bbf488fa65da61a8985228d9b Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 4 May 2021 08:43:13 +0200
Subject: [PATCH] new aron types
---
.../libraries/aron/common/CMakeLists.txt | 9 ++
.../libraries/aron/common/aron/Attachment.xml | 30 ++++++
.../libraries/aron/common/aron/Constraint.xml | 93 +++++++++++++++++++
.../libraries/aron/common/aron/Robot.xml | 32 +++++++
.../aron/common/aron/RobotDescription.xml | 40 ++++++++
.../libraries/aron/common/aron/RobotState.xml | 30 ++++++
.../aron/common/aron/ice_conversions.cpp | 32 +++++++
.../aron/common/aron/ice_conversions.h | 14 +++
.../libraries/aron/common/aron/new_types.h | 48 ++++++++++
.../aron/common/aron_conversions/armarx.cpp | 87 +++++++++++++++--
.../aron/common/aron_conversions/armarx.h | 28 +++++-
11 files changed, 433 insertions(+), 10 deletions(-)
create mode 100644 source/RobotAPI/libraries/aron/common/aron/Attachment.xml
create mode 100644 source/RobotAPI/libraries/aron/common/aron/Constraint.xml
create mode 100644 source/RobotAPI/libraries/aron/common/aron/Robot.xml
create mode 100644 source/RobotAPI/libraries/aron/common/aron/RobotDescription.xml
create mode 100644 source/RobotAPI/libraries/aron/common/aron/RobotState.xml
create mode 100644 source/RobotAPI/libraries/aron/common/aron/ice_conversions.cpp
create mode 100644 source/RobotAPI/libraries/aron/common/aron/ice_conversions.h
create mode 100644 source/RobotAPI/libraries/aron/common/aron/new_types.h
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 7a4f383eb..a64ff5e71 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -23,6 +23,9 @@ armarx_add_library(
aron_conversions/armarx.cpp
aron_conversions/simox.cpp
aron_conversions/stl.cpp
+
+ # TODO move
+ aron/ice_conversions.cpp
)
@@ -33,6 +36,12 @@ armarx_enable_aron_file_generation_for_target(
aron/AxisAlignedBoundingBox.xml
aron/OrientedBox.xml
aron/PackagePath.xml
+
+ # aron/Attachment.xml
+ aron/RobotDescription.xml
+ aron/RobotState.xml
+ aron/Robot.xml
+ aron/Constraint.xml
)
diff --git a/source/RobotAPI/libraries/aron/common/aron/Attachment.xml b/source/RobotAPI/libraries/aron/common/aron/Attachment.xml
new file mode 100644
index 000000000..de6386369
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/Attachment.xml
@@ -0,0 +1,30 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::Attachment">
+
+ <ObjectChild key="constraintType">
+
+ </ObjectChild>
+
+ <ObjectChild key="transformation">
+ <Pose/>
+ </ObjectChild>
+
+ <ObjectChild key="objectName">
+ <string/>
+ </ObjectChild>
+
+ <ObjectChild key="objectNode">
+ <string/>
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/Constraint.xml b/source/RobotAPI/libraries/aron/common/aron/Constraint.xml
new file mode 100644
index 000000000..b1360fc5b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/Constraint.xml
@@ -0,0 +1,93 @@
+<!-- This class defines a link between two scene entities, e.g. robots, articulated objects or objects -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </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/PackagePath.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::RobotID">
+ <ObjectChild key="name">
+ <string/>
+ </ObjectChild>
+ <ObjectChild key="instance">
+ <string/>
+ </ObjectChild>
+ </Object>
+
+ <IntEnum name="armarx::arondto::SceneEntityType">
+ <EnumValue key="Robot" value="0" />
+ <EnumValue key="ArticulatedObject" value="1" />
+ <EnumValue key="Object" value="2" />
+ </IntEnum>
+
+ <Object name="armarx::arondto::SceneEntity">
+ <ObjectChild key="type">
+ <armarx::arondto::SceneEntityType />
+ </ObjectChild>
+
+ <!-- could be a variant -->
+ <!-- <ObjectChild key="objectId">
+ <armarx::arondto::ObjectID />
+ </ObjectChild>
+ <ObjectChild key="robotId">
+ <armarx::arondto::RobotID />
+ </ObjectChild> -->
+ </Object>
+
+ <Object name="armarx::arondto::BodyLinkDescription">
+ <ObjectChild key="entity">
+ <armarx::arondto::SceneEntity />
+ </ObjectChild>
+
+ <ObjectChild key="node">
+ <string/>
+ </ObjectChild>
+
+ <ObjectChild key="offset">
+ <Pose/>
+ </ObjectChild>
+ </Object>
+
+ <!-- http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual -->
+ <IntEnum name="armarx::arondto::ConstraintType">
+ <EnumValue key="Fixed" value="0" />
+ <!-- <EnumValue key="Prismatic" value="1" /> -->
+ <!-- <EnumValue key="Revolute" value="2" /> -->
+ <!-- <EnumValue key="PointToPoint" value="3" /> -->
+ <!-- <EnumValue key="ConeTwist" value="4" /> -->
+ <!-- <EnumValue key="Contact" value="6" /> -->
+ <!-- <EnumValue key="Gear" value="7" /> -->
+ <!-- <EnumValue key="Spring" value="7" /> -->
+ </IntEnum>
+
+ <Object name="armarx::arondto::Constraint">
+
+ <ObjectChild key="type">
+ <armarx::arondto::ConstraintType />
+ </ObjectChild>
+
+ <!-- <ObjectChild key="transformation">
+ <Pose/>
+ </ObjectChild> -->
+
+
+ <ObjectChild key="bodyA">
+ <armarx::arondto::BodyLinkDescription/>
+ </ObjectChild>
+
+ <ObjectChild key="bodyB">
+ <armarx::arondto::BodyLinkDescription/>
+ </ObjectChild>
+
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/Robot.xml b/source/RobotAPI/libraries/aron/common/aron/Robot.xml
new file mode 100644
index 000000000..dc92382a7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/Robot.xml
@@ -0,0 +1,32 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/RobotDescription.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/RobotState.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/RobotDescription.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/RobotState.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::Robot">
+
+ <ObjectChild key='description'>
+ <armarx::arondto::RobotDescription />
+ </ObjectChild>
+
+ <ObjectChild key='state'>
+ <armarx::arondto::RobotState />
+ </ObjectChild>
+
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/RobotDescription.xml b/source/RobotAPI/libraries/aron/common/aron/RobotDescription.xml
new file mode 100644
index 000000000..bc6f8f2a0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/RobotDescription.xml
@@ -0,0 +1,40 @@
+<!--This class cotains information on how to create a VirtualRobot::Robot -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::arondto::RobotDescription'>
+
+ <ObjectChild key='name'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='xml'>
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <!-- <ObjectChild key='scaling'>
+ <float />
+ </ObjectChild> -->
+
+ <!--
+ <ObjectChild key='attachments'>
+ <List>
+ <armarx::arondto::Attachment />
+ </List>
+ <ObjectChild/>
+ -->
+
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/RobotState.xml b/source/RobotAPI/libraries/aron/common/aron/RobotState.xml
new file mode 100644
index 000000000..1579bf790
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/RobotState.xml
@@ -0,0 +1,30 @@
+<!--This class contains the data structure for ObjectPose -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::RobotState">
+
+ <ObjectChild key='timestamp'>
+ <Time />
+ </ObjectChild>
+
+ <ObjectChild key='globalPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='jointMap'>
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/ice_conversions.cpp b/source/RobotAPI/libraries/aron/common/aron/ice_conversions.cpp
new file mode 100644
index 000000000..335686861
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/ice_conversions.cpp
@@ -0,0 +1,32 @@
+#include "ice_conversions.cpp"
+
+#include <RobotAPI/libraries/aron/common/aron/new_types.h>
+#include <RobotAPI/interface/core/articulated_object/types.h>
+
+namespace armarx
+{
+ void fromIce(const ::armarx::articulated_object::data::RobotState& ice, ::armarx::RobotState& bo)
+ {
+ fromIce(ice.config, bo.config);
+ fromIce(ice.id, bo.description);
+ }
+
+ void toIce(::armarx::articulated_object::data::ArticulatedObject& ice, const ::armarx::articulated_object::ArticulatedObject& bo)
+ {
+
+ }
+
+
+ void fromIce(const ::armarx::articulated_object::data::ArticulatedObject& ice, ::armarx::articulated_object::ArticulatedObject& bo)
+ {
+ fromIce(ice.config, bo.config);
+ fromIce(ice.id, bo.description);
+ }
+
+ void toIce(::armarx::articulated_object::data::ArticulatedObject& ice, const ::armarx::articulated_object::ArticulatedObject& bo)
+ {
+
+ }
+
+
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron/common/aron/ice_conversions.h b/source/RobotAPI/libraries/aron/common/aron/ice_conversions.h
new file mode 100644
index 000000000..00aa04afd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/ice_conversions.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <RobotAPI/libraries/aron/common/aron/new_types.h>
+#include <RobotAPI/interface/core/articulated_object/types.h>
+
+namespace armarx
+{
+ void fromIce(const ::armarx::articulated_object::data::ArticulatedObject& ice, ::armarx::articulated_object::ArticulatedObject& bo);
+ void toIce(::armarx::articulated_object::data::ArticulatedObject& ice, const ::armarx::articulated_object::ArticulatedObject& bo);
+
+} // namespace armarx
+
+
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/new_types.h b/source/RobotAPI/libraries/aron/common/aron/new_types.h
new file mode 100644
index 000000000..60fa683ae
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/new_types.h
@@ -0,0 +1,48 @@
+#pragma once
+
+// these types should be core types
+
+#include <map>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+
+namespace armarx
+{
+
+ struct RobotDescription
+ {
+ // IceUtil::Time timestamp;
+
+ std::string name;
+ PackagePath xml;
+ };
+
+
+ struct RobotState
+ {
+ IceUtil::Time timestamp;
+
+ Eigen::Affine3f globalPose;
+ std::map<std::string, float> jointMap;
+ };
+
+ struct Robot
+ {
+ RobotDescription description;
+ RobotState config;
+
+ // IceUtil::Time timestamp;
+ };
+
+ namespace articulated_object
+ {
+ using ArticulatedObject = Robot;
+ using ArticulatedObjects = std::vector<ArticulatedObject>;
+ }
+
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index da205a523..0aa43a2b2 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -1,14 +1,83 @@
#include "armarx.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
-void armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackagePath& bo)
+namespace armarx
{
- bo = { dto.package, dto.path };
-}
-void armarx::toAron(armarx::arondto::PackagePath& dto, const armarx::PackagePath& bo)
-{
- const armarx::data::PackagePath icedto = bo.serialize();
- dto.package = icedto.package;
- dto.path = icedto.path;
-}
+ /* to be moved */
+ void fromAron(const long& dto, IceUtil::Time& time)
+ {
+ time = IceUtil::Time::microSeconds(dto);
+ }
+
+ void toAron(long& dto, const IceUtil::Time& time)
+ {
+ dto = time.toMicroSeconds();
+ }
+
+ /* PackagePath */
+
+ void fromAron(const arondto::PackagePath& dto, PackagePath& bo)
+ {
+ bo = {dto.package, dto.path};
+ }
+
+ void toAron(arondto::PackagePath& dto, const PackagePath& bo)
+ {
+ const data::PackagePath icedto = bo.serialize();
+ dto.package = icedto.package;
+ dto.path = icedto.path;
+ }
+
+
+ /* Robot */
+
+ void fromAron(const arondto::Robot& dto, Robot& bo)
+ {
+ fromAron(dto.description, bo.description);
+ fromAron(dto.state, bo.config);
+ }
+
+ void toAron(arondto::Robot& dto, const Robot& bo)
+ {
+ toAron(dto.description, bo.description);
+ toAron(dto.state, bo.config);
+ }
+
+
+ /* RobotDescription */
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo)
+ {
+ aron::fromAron(dto.name, bo.name);
+ fromAron(dto.xml, bo.xml);
+ }
+
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo)
+ {
+ aron::toAron(dto.name, bo.name);
+ toAron(dto.xml, bo.xml);
+ }
+
+
+ /* RobotState */
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo)
+ {
+ fromAron(dto.timestamp, bo.timestamp);
+ bo.globalPose.matrix() = dto.globalPose;
+ bo.jointMap = dto.jointMap;
+ }
+
+ void toAron(arondto::RobotState& dto, const RobotState& bo)
+ {
+ toAron(dto.timestamp, bo.timestamp);
+ dto.globalPose = bo.globalPose.matrix();
+ dto.jointMap = bo.jointMap;
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
index 241067b7b..fafb4c33a 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -1,11 +1,37 @@
#pragma once
#include <ArmarXCore/core/PackagePath.h>
+
+#include <IceUtil/Time.h>
+
#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/new_types.h>
+#include <RobotAPI/libraries/aron/common/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/Robot.aron.generated.h>
namespace armarx
{
+
+ // TODO move the following
+ void fromAron(const long& dto, IceUtil::Time& time);
+ void toAron(long& dto, const IceUtil::Time& time);
+
+ // end TODO
+
+
void fromAron(const arondto::PackagePath& dto, PackagePath& bo);
void toAron(arondto::PackagePath& dto, const PackagePath& bo);
-}
+
+
+ void fromAron(const arondto::Robot& dto, Robot& bo);
+ void toAron(arondto::Robot& dto, const Robot& bo);
+
+ void fromAron(const arondto::RobotDescription& dto, RobotDescription& bo);
+ void toAron(arondto::RobotDescription& dto, const RobotDescription& bo);
+
+ void fromAron(const arondto::RobotState& dto, RobotState& bo);
+ void toAron(arondto::RobotState& dto, const RobotState& bo);
+
+} // namespace armarx
--
GitLab