From 871b9d5d6fc9a397d93d757e066caf49245cfe85 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 29 Sep 2021 22:14:04 +0200
Subject: [PATCH] armem_robot: aron conversions for platform state (=> twist)
---
.../libraries/armem_robot/aron_conversions.cpp | 15 +++++++++++++++
.../libraries/armem_robot/aron_conversions.h | 3 +++
source/RobotAPI/libraries/armem_robot/types.h | 12 ++++++++++++
3 files changed, 30 insertions(+)
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index da72cc16f..426511a9a 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -1,5 +1,6 @@
#include "aron_conversions.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
@@ -107,5 +108,19 @@ namespace armarx::armem
dto.objectPoseGlobal = bo.globalPose.matrix();
dto.objectJointValues = bo.jointMap;
}
+
+ void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+ {
+ bo.twist.linear.setZero();
+ bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+ bo.twist.angular.setZero();
+ bo.twist.angular.z() = dto.velocity.z(); // yaw
+ }
+
+ void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+ {
+ ARMARX_ERROR << "Not implemented yet.";
+ }
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index 9c124d758..e6b462fa8 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -8,6 +8,7 @@
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
@@ -26,6 +27,8 @@ namespace armarx::armem::robot
void fromAron(const arondto::RobotState& dto, RobotState& bo);
void toAron(arondto::RobotState& dto, const RobotState& bo);
+ void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
+ void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 92f16199a..ad7454d6f 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -23,6 +23,18 @@ namespace armarx::armem::robot
PackagePath xml{"", std::filesystem::path("")};
};
+
+ struct Twist
+ {
+ Eigen::Vector3f linear;
+ Eigen::Vector3f angular;
+ };
+
+ struct PlatformState
+ {
+ Twist twist;
+ };
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
--
GitLab