From 42e21a7a39fcadd6c73891dc2e3dc7f5a3f5148f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 28 Jul 2021 10:37:19 +0200
Subject: [PATCH] aron stuff: extends; moving files; renaming structs, ...
---
.../Navigation/libraries/core/CMakeLists.txt | 2 +
.../aron/PIDParams.xml | 2 +-
.../libraries/core/aron/TwistLimits.xml | 20 +++++
.../libraries/core/aron_conversions.cpp | 32 +++++++
.../libraries/core/aron_conversions.h | 12 +++
source/Navigation/libraries/core/types.h | 14 ++++
.../libraries/global_planning/AStar.cpp | 5 ++
.../libraries/global_planning/AStar.h | 2 +-
.../global_planning/GlobalPlanner.cpp | 5 ++
.../libraries/global_planning/Point2Point.cpp | 1 +
.../libraries/global_planning/Point2Point.h | 1 +
.../trajectory_control/CMakeLists.txt | 1 -
.../trajectory_control/TrajectoryController.h | 13 +--
.../TrajectoryFollowingController.cpp | 16 ++--
.../TrajectoryFollowingController.h | 10 +--
.../aron/TrajectoryControllerParams.xml | 13 +--
.../TrajectoryFollowingControllerParams.xml | 27 ++----
.../trajectory_control/aron_conversions.cpp | 22 +++++
.../trajectory_control/aron_conversions.h | 6 ++
.../test/trajectory_controlTest.cpp | 84 +++++++++----------
20 files changed, 189 insertions(+), 99 deletions(-)
rename source/Navigation/libraries/{trajectory_control => core}/aron/PIDParams.xml (85%)
create mode 100644 source/Navigation/libraries/core/aron/TwistLimits.xml
diff --git a/source/Navigation/libraries/core/CMakeLists.txt b/source/Navigation/libraries/core/CMakeLists.txt
index c301c053..24916425 100644
--- a/source/Navigation/libraries/core/CMakeLists.txt
+++ b/source/Navigation/libraries/core/CMakeLists.txt
@@ -45,6 +45,8 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME "${LIB_NAME}"
ARON_FILES
aron/Twist.xml
+ aron/TwistLimits.xml
+ aron/PIDParams.xml
aron/Trajectory.xml
)
diff --git a/source/Navigation/libraries/trajectory_control/aron/PIDParams.xml b/source/Navigation/libraries/core/aron/PIDParams.xml
similarity index 85%
rename from source/Navigation/libraries/trajectory_control/aron/PIDParams.xml
rename to source/Navigation/libraries/core/aron/PIDParams.xml
index 6e4b3dfd..ac64be68 100644
--- a/source/Navigation/libraries/trajectory_control/aron/PIDParams.xml
+++ b/source/Navigation/libraries/core/aron/PIDParams.xml
@@ -2,7 +2,7 @@
<AronTypeDefinition>
<GenerateTypes>
- <Object name='armarx::nav::traj_ctrl::arondto::PIDParams'>
+ <Object name='armarx::nav::core::arondto::PIDParams'>
<ObjectChild key='Kp'>
<float />
</ObjectChild>
diff --git a/source/Navigation/libraries/core/aron/TwistLimits.xml b/source/Navigation/libraries/core/aron/TwistLimits.xml
new file mode 100644
index 00000000..6cbc5a1c
--- /dev/null
+++ b/source/Navigation/libraries/core/aron/TwistLimits.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::nav::core::arondto::TwistLimits'>
+ <ObjectChild key='linear'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='angular'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/Navigation/libraries/core/aron_conversions.cpp b/source/Navigation/libraries/core/aron_conversions.cpp
index 3e0460af..a6d4617c 100644
--- a/source/Navigation/libraries/core/aron_conversions.cpp
+++ b/source/Navigation/libraries/core/aron_conversions.cpp
@@ -67,4 +67,36 @@ namespace armarx::nav::core
bo.linear = dto.linear;
bo.angular = dto.angular;
}
+
+ void
+ toAron(armarx::nav::core::arondto::PIDParams& dto, const armarx::nav::core::PIDParams& bo)
+ {
+ aron::toAron(dto.Kp, bo.Kp);
+ aron::toAron(dto.Ki, bo.Ki);
+ aron::toAron(dto.Kd, bo.Kd);
+ }
+
+
+ void
+ fromAron(const armarx::nav::core::arondto::PIDParams& dto, armarx::nav::core::PIDParams& bo)
+ {
+ aron::fromAron(dto.Kp, bo.Kp);
+ aron::fromAron(dto.Ki, bo.Ki);
+ aron::fromAron(dto.Kd, bo.Kd);
+ }
+
+ void
+ toAron(armarx::nav::core::arondto::TwistLimits& dto, const armarx::nav::core::TwistLimits& bo)
+ {
+ aron::toAron(dto.linear, bo.linear);
+ aron::toAron(dto.angular, bo.angular);
+ }
+
+ void
+ fromAron(const armarx::nav::core::arondto::TwistLimits& dto, armarx::nav::core::TwistLimits& bo)
+ {
+ aron::fromAron(dto.linear, bo.linear);
+ aron::fromAron(dto.angular, bo.angular);
+ }
+
} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/core/aron_conversions.h b/source/Navigation/libraries/core/aron_conversions.h
index 7e2159b9..eb36a46e 100644
--- a/source/Navigation/libraries/core/aron_conversions.h
+++ b/source/Navigation/libraries/core/aron_conversions.h
@@ -21,6 +21,8 @@
#pragma once
+#include "Navigation/libraries/core/aron/PIDParams.aron.generated.h"
+#include "Navigation/libraries/core/aron/TwistLimits.aron.generated.h"
#include <Navigation/libraries/core/Trajectory.h>
#include <Navigation/libraries/core/aron/Trajectory.aron.generated.h>
#include <Navigation/libraries/core/aron/Twist.aron.generated.h>
@@ -35,4 +37,14 @@ namespace armarx::nav::core
void toAron(arondto::Twist& dto, const Twist& bo);
void fromAron(const arondto::Twist& dto, Twist& bo);
+
+ void toAron(armarx::nav::core::arondto::PIDParams& dto, const armarx::nav::core::PIDParams& bo);
+ void fromAron(const armarx::nav::core::arondto::PIDParams& dto,
+ armarx::nav::core::PIDParams& bo);
+
+ void toAron(armarx::nav::core::arondto::TwistLimits& dto,
+ const armarx::nav::core::TwistLimits& bo);
+ void fromAron(const armarx::nav::core::arondto::TwistLimits& dto,
+ armarx::nav::core::TwistLimits& bo);
+
} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/core/types.h b/source/Navigation/libraries/core/types.h
index 21e9743a..0ff85fdc 100644
--- a/source/Navigation/libraries/core/types.h
+++ b/source/Navigation/libraries/core/types.h
@@ -92,4 +92,18 @@ namespace armarx::nav::core
using TimestampUs = std::chrono::microseconds;
+
+ struct PIDParams
+ {
+ float Kp{1.0};
+ float Ki{0.F};
+ float Kd{0.F};
+ };
+
+ struct TwistLimits
+ {
+ float linear{std::numeric_limits<float>::max()};
+ float angular{std::numeric_limits<float>::max()};
+ };
+
} // namespace armarx::nav::core
diff --git a/source/Navigation/libraries/global_planning/AStar.cpp b/source/Navigation/libraries/global_planning/AStar.cpp
index fd38f8d3..4054ad3f 100644
--- a/source/Navigation/libraries/global_planning/AStar.cpp
+++ b/source/Navigation/libraries/global_planning/AStar.cpp
@@ -6,7 +6,12 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "RobotAPI/libraries/aron/core/navigator/data/container/Dict.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/String.h"
+
+#include "Navigation/libraries/core/constants.h"
#include "Navigation/libraries/global_planning/aron_conversions.h"
+#include "Navigation/libraries/global_planning/core.h"
#include <Navigation/libraries/algorithms/astar/AStarPlanner.h>
#include <Navigation/libraries/algorithms/smoothing/ChainApproximation.h>
#include <Navigation/libraries/algorithms/smoothing/CircularPathSmoothing.h>
diff --git a/source/Navigation/libraries/global_planning/AStar.h b/source/Navigation/libraries/global_planning/AStar.h
index c23d9186..71894e87 100644
--- a/source/Navigation/libraries/global_planning/AStar.h
+++ b/source/Navigation/libraries/global_planning/AStar.h
@@ -38,8 +38,8 @@ namespace armarx::nav::glob_plan
float resampleDistance{500};
Algorithms algorithm() const override;
-
aron::datanavigator::DictNavigatorPtr toAron() const override;
+
static AStarParams FromAron(const aron::datanavigator::DictNavigatorPtr& dict);
};
diff --git a/source/Navigation/libraries/global_planning/GlobalPlanner.cpp b/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
index ac00c7c0..2ccc88a7 100644
--- a/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
+++ b/source/Navigation/libraries/global_planning/GlobalPlanner.cpp
@@ -1,9 +1,14 @@
#include "GlobalPlanner.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/String.h"
+
+#include "Navigation/libraries/core/constants.h"
#include "Navigation/libraries/core/types.h"
namespace armarx::nav::glob_plan
{
+
+
GlobalPlanner::GlobalPlanner(const core::Scene& context) : scene(context)
{
}
diff --git a/source/Navigation/libraries/global_planning/Point2Point.cpp b/source/Navigation/libraries/global_planning/Point2Point.cpp
index 9c0efc0b..4cb81ebb 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.cpp
+++ b/source/Navigation/libraries/global_planning/Point2Point.cpp
@@ -7,6 +7,7 @@
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
#include "Navigation/libraries/core/Trajectory.h"
+#include "Navigation/libraries/core/constants.h"
#include "Navigation/libraries/core/types.h"
#include "Navigation/libraries/global_planning/GlobalPlanner.h"
#include "Navigation/libraries/global_planning/core.h"
diff --git a/source/Navigation/libraries/global_planning/Point2Point.h b/source/Navigation/libraries/global_planning/Point2Point.h
index 8c9d381e..306cb92d 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.h
+++ b/source/Navigation/libraries/global_planning/Point2Point.h
@@ -40,6 +40,7 @@ namespace armarx::nav::glob_plan
Algorithms algorithm() const override;
aron::datanavigator::DictNavigatorPtr toAron() const override;
+
static Point2PointParams FromAron(const aron::datanavigator::DictNavigatorPtr& dict);
};
diff --git a/source/Navigation/libraries/trajectory_control/CMakeLists.txt b/source/Navigation/libraries/trajectory_control/CMakeLists.txt
index b8660edf..af9d00c1 100644
--- a/source/Navigation/libraries/trajectory_control/CMakeLists.txt
+++ b/source/Navigation/libraries/trajectory_control/CMakeLists.txt
@@ -30,7 +30,6 @@ add_library(
armarx_enable_aron_file_generation_for_target(
TARGET_NAME "${LIB_NAME}"
ARON_FILES
- aron/PIDParams.xml
aron/TrajectoryControllerParams.xml
aron/TrajectoryFollowingControllerParams.xml
aron/WaypointControllerParams.xml
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryController.h b/source/Navigation/libraries/trajectory_control/TrajectoryController.h
index 9435aa53..93483adc 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryController.h
@@ -39,17 +39,12 @@
namespace armarx::nav::traj_ctrl
{
+
struct TrajectoryControllerParams
{
- struct Limits
- {
- float linearVelocity{std::numeric_limits<float>::max()};
- float angularVelocity{std::numeric_limits<float>::max()};
- };
-
- Limits limits{
- .linearVelocity = 500.F, // [mm/s]
- .angularVelocity = 2.F * M_PIf32 / 10.F // [rad/s]
+ core::TwistLimits limits{
+ .linear = 500.F, // [mm/s]
+ .angular = 2.F * M_PIf32 / 10.F // [rad/s]
};
virtual Algorithms algorithm() const = 0;
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index 12178527..423552bd 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
@@ -91,8 +91,8 @@ namespace armarx::nav::traj_ctrl
TrajectoryFollowingController::applyTwistLimits(core::Twist twist)
{
core::Twist limits;
- limits.linear = Eigen::Vector3f::Ones() * params.limits.linearVelocity;
- limits.angular = Eigen::Vector3f::Ones() * params.limits.angularVelocity;
+ limits.linear = Eigen::Vector3f::Ones() * params.limits.linear;
+ limits.angular = Eigen::Vector3f::Ones() * params.limits.angular;
// for all entries, scale should be less than 1
const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs());
@@ -110,12 +110,12 @@ namespace armarx::nav::traj_ctrl
twist.angular /= scaleMax;
// pedantic checks
- ARMARX_CHECK(std::abs(twist.linear.x()) <= params.limits.linearVelocity);
- ARMARX_CHECK(std::abs(twist.linear.y()) <= params.limits.linearVelocity);
- ARMARX_CHECK(std::abs(twist.linear.z()) <= params.limits.linearVelocity);
- ARMARX_CHECK(std::abs(twist.angular.x()) <= params.limits.angularVelocity);
- ARMARX_CHECK(std::abs(twist.angular.y()) <= params.limits.angularVelocity);
- ARMARX_CHECK(std::abs(twist.angular.z()) <= params.limits.angularVelocity);
+ ARMARX_CHECK(std::abs(twist.linear.x()) <= params.limits.linear);
+ ARMARX_CHECK(std::abs(twist.linear.y()) <= params.limits.linear);
+ ARMARX_CHECK(std::abs(twist.linear.z()) <= params.limits.linear);
+ ARMARX_CHECK(std::abs(twist.angular.x()) <= params.limits.angular);
+ ARMARX_CHECK(std::abs(twist.angular.y()) <= params.limits.angular);
+ ARMARX_CHECK(std::abs(twist.angular.z()) <= params.limits.angular);
return twist;
}
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
index 0809845c..02f589d0 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
@@ -33,15 +33,9 @@ namespace armarx::nav::traj_ctrl
struct TrajectoryFollowingControllerParams : public TrajectoryControllerParams
{
- struct PIDParams
- {
- float Kp{1.0};
- float Ki{0.F};
- float Kd{0.F};
- };
- PIDParams pidPos;
- PIDParams pidOri;
+ core::PIDParams pidPos;
+ core::PIDParams pidOri;
Algorithms algorithm() const override;
aron::datanavigator::DictNavigatorPtr toAron() const override;
diff --git a/source/Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml b/source/Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml
index dadd2d94..5690bc1d 100644
--- a/source/Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml
+++ b/source/Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml
@@ -1,24 +1,19 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <!-- <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>" /> -->
+ <Include include="<Navigation/libraries/core/aron/TwistLimits.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
- <!-- <Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml>" /> -->
+ <Include include="<Navigation/libraries/core/aron/TwistLimits.xml>" />
</AronIncludes>
<GenerateTypes>
- <!-- <Object name='armarx::nav::glob_plan::arondto::Point2PointParams' extends="armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams"> -->
<Object name='armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams'>
- <!-- <ObjectChild key='includeStartPose'>
- <bool />
+ <ObjectChild key='limits'>
+ <armarx::nav::core::arondto::TwistLimits />
</ObjectChild>
- <ObjectChild key='velocity'>
- <float />
- </ObjectChild> -->
</Object>
-
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.xml b/source/Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
index d7cea768..cfff609f 100644
--- a/source/Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
+++ b/source/Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.xml
@@ -2,38 +2,25 @@
<AronTypeDefinition>
<CodeIncludes>
<Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.aron.generated.h>" />
- <Include include="<Navigation/libraries/trajectory_control/aron/PIDParams.aron.generated.h>" />
+ <Include include="<Navigation/libraries/core/aron/PIDParams.aron.generated.h>" />
+ <Include include="<Navigation/libraries/core/aron/TwistLimits.aron.generated.h>" />
</CodeIncludes>
<AronIncludes>
<Include include="<Navigation/libraries/trajectory_control/aron/TrajectoryControllerParams.xml>" />
- <Include include="<Navigation/libraries/trajectory_control/aron/PIDParams.xml>" />
+ <Include include="<Navigation/libraries/core/aron/PIDParams.xml>" />
+ <Include include="<Navigation/libraries/core/aron/TwistLimits.xml>" />
</AronIncludes>
<GenerateTypes>
- <Object name='armarx::nav::arondto::TwistLimits'>
- <ObjectChild key='linear'>
- <float/>
- </ObjectChild>
-
- <ObjectChild key='angular'>
- <float/>
- </ObjectChild>
- </Object>
-
-
- <!-- <Object name='armarx::nav::traj_ctrl::arondto::TrajectoryFollowingControllerParams' extends="armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams"> -->
- <Object name='armarx::nav::traj_ctrl::arondto::TrajectoryFollowingControllerParams'>
+ <Object name='armarx::nav::traj_ctrl::arondto::TrajectoryFollowingControllerParams' extends="armarx::nav::traj_ctrl::arondto::TrajectoryControllerParams">
<ObjectChild key='pidPos'>
- <armarx::nav::traj_ctrl::arondto::PIDParams />
+ <armarx::nav::core::arondto::PIDParams />
</ObjectChild>
<ObjectChild key='pidOri'>
- <armarx::nav::traj_ctrl::arondto::PIDParams />
+ <armarx::nav::core::arondto::PIDParams />
</ObjectChild>
- <ObjectChild key="limits">
- <armarx::nav::arondto::TwistLimits />
- </ObjectChild>
</Object>
</GenerateTypes>
diff --git a/source/Navigation/libraries/trajectory_control/aron_conversions.cpp b/source/Navigation/libraries/trajectory_control/aron_conversions.cpp
index b2d43512..545dc4d5 100644
--- a/source/Navigation/libraries/trajectory_control/aron_conversions.cpp
+++ b/source/Navigation/libraries/trajectory_control/aron_conversions.cpp
@@ -1,7 +1,10 @@
#include "aron_conversions.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions/core.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions/stl.h"
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include "Navigation/libraries/core/aron_conversions.h"
#include "Navigation/libraries/trajectory_control/TrajectoryController.h"
#include "Navigation/libraries/trajectory_control/TrajectoryFollowingController.h"
#include "Navigation/libraries/trajectory_control/WaypointController.h"
@@ -9,38 +12,57 @@
#include "Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h"
#include "Navigation/libraries/trajectory_control/aron/WaypointControllerParams.aron.generated.h"
+
namespace armarx::nav::traj_ctrl::aron_conv
{
+
void
toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo)
{
+ core::toAron(dto.limits, bo.limits);
}
void
fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo)
{
+ core::fromAron(dto.limits, bo.limits);
}
void
toAron(arondto::TrajectoryFollowingControllerParams& dto,
const TrajectoryFollowingControllerParams& bo)
{
+ toAron(static_cast<arondto::TrajectoryControllerParams&>(dto),
+ static_cast<const TrajectoryControllerParams&>(bo));
+
+ core::toAron(dto.pidOri, bo.pidOri);
+ core::toAron(dto.pidPos, bo.pidPos);
}
void
fromAron(const arondto::TrajectoryFollowingControllerParams& dto,
TrajectoryFollowingControllerParams& bo)
{
+ fromAron(static_cast<const arondto::TrajectoryControllerParams&>(dto),
+ static_cast<TrajectoryControllerParams&>(bo));
+
+ core::fromAron(dto.pidOri, bo.pidOri);
+ core::fromAron(dto.pidPos, bo.pidPos);
}
void
toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo)
{
+ toAron(static_cast<arondto::WaypointControllerParams&>(dto),
+ static_cast<const WaypointControllerParams&>(bo));
}
void
fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo)
{
+ fromAron(static_cast<const arondto::WaypointControllerParams&>(dto),
+ static_cast<WaypointControllerParams&>(bo));
}
+
} // namespace armarx::nav::traj_ctrl::aron_conv
diff --git a/source/Navigation/libraries/trajectory_control/aron_conversions.h b/source/Navigation/libraries/trajectory_control/aron_conversions.h
index df8af9f0..72350a3c 100644
--- a/source/Navigation/libraries/trajectory_control/aron_conversions.h
+++ b/source/Navigation/libraries/trajectory_control/aron_conversions.h
@@ -24,20 +24,26 @@
namespace armarx::nav::traj_ctrl
{
+ struct PIDParams;
+
struct TrajectoryControllerParams;
struct TrajectoryFollowingControllerParams;
struct WaypointControllerParams;
namespace arondto
{
+ class PIDParams;
+
class TrajectoryControllerParams;
class TrajectoryFollowingControllerParams;
class WaypointControllerParams;
} // namespace arondto
+
} // namespace armarx::nav::traj_ctrl
namespace armarx::nav::traj_ctrl::aron_conv
{
+
void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo);
void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo);
diff --git a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
index f84a76dc..6b5a7f02 100644
--- a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
+++ b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
@@ -61,8 +61,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
params.pidOri.Ki = 0;
params.pidOri.Kd = 0;
- params.limits.linearVelocity = std::numeric_limits<float>::max();
- params.limits.angularVelocity = std::numeric_limits<float>::max();
+ params.limits.linear = std::numeric_limits<float>::max();
+ params.limits.angular = std::numeric_limits<float>::max();
traj_ctrl::TrajectoryFollowingController controller(params, scene);
@@ -103,8 +103,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
traj_ctrl::TrajectoryFollowingControllerParams params;
params.pidPos = {1, 0, 0};
params.pidOri = {1, 0, 0};
- params.limits.linearVelocity = std::numeric_limits<float>::max();
- params.limits.angularVelocity = std::numeric_limits<float>::max();
+ params.limits.linear = std::numeric_limits<float>::max();
+ params.limits.angular = std::numeric_limits<float>::max();
traj_ctrl::TrajectoryFollowingController controller(params, scene);
@@ -185,8 +185,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
traj_ctrl::TrajectoryFollowingControllerParams params;
params.pidPos = {1, 0, 0};
params.pidOri = {1, 0, 0};
- params.limits.linearVelocity = std::numeric_limits<float>::max();
- params.limits.angularVelocity = std::numeric_limits<float>::max();
+ params.limits.linear = std::numeric_limits<float>::max();
+ params.limits.angular = std::numeric_limits<float>::max();
traj_ctrl::TrajectoryFollowingController controller(params, scene);
@@ -266,8 +266,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedLinear)
params.pidOri.Ki = 0;
params.pidOri.Kd = 0;
- params.limits.linearVelocity = 500.F; // [mm/s]
- params.limits.angularVelocity = 2.F * M_PIf32 / 10.F; // [rad/s]
+ params.limits.linear = 500.F; // [mm/s]
+ params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
traj_ctrl::TrajectoryFollowingController controller(params, scene);
@@ -282,16 +282,16 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedLinear)
ARMARX_DEBUG << "Clipped twist angular " << clippedTwist.angular;
// check that velocity is clipped correctly
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linearVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linear);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angularVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angular);
ARMARX_DEBUG << std::abs(clippedTwist.angular.z());
- ARMARX_DEBUG << "GT " << params.limits.angularVelocity;
+ ARMARX_DEBUG << "GT " << params.limits.angular;
}
@@ -312,8 +312,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedLinear2)
params.pidOri.Ki = 0;
params.pidOri.Kd = 0;
- params.limits.linearVelocity = 500.F; // [mm/s]
- params.limits.angularVelocity = 2.F * M_PIf32 / 10.F; // [rad/s]
+ params.limits.linear = 500.F; // [mm/s]
+ params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
traj_ctrl::TrajectoryFollowingController controller(params, scene);
@@ -328,16 +328,16 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedLinear2)
ARMARX_DEBUG << "Clipped twist angular " << clippedTwist.angular;
// check that velocity is clipped correctly
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linearVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linear);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angularVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angular);
ARMARX_DEBUG << std::abs(clippedTwist.angular.z());
- ARMARX_DEBUG << "GT" << params.limits.angularVelocity;
+ ARMARX_DEBUG << "GT" << params.limits.angular;
}
// TODO this should be part of the TrajectoryController base class
@@ -357,8 +357,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedAngular)
params.pidOri.Ki = 0;
params.pidOri.Kd = 0;
- params.limits.linearVelocity = 500.F; // [mm/s]
- params.limits.angularVelocity = 2.F * M_PIf32 / 10.F; // [rad/s]
+ params.limits.linear = 500.F; // [mm/s]
+ params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
traj_ctrl::TrajectoryFollowingController controller(params, scene);
@@ -373,16 +373,16 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedAngular)
ARMARX_DEBUG << "Clipped twist angular " << clippedTwist.angular;
// check that velocity is clipped correctly
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linearVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linear);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angularVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angular);
ARMARX_DEBUG << std::abs(clippedTwist.angular.z());
- ARMARX_DEBUG << "GT" << params.limits.angularVelocity;
+ ARMARX_DEBUG << "GT" << params.limits.angular;
}
@@ -403,8 +403,8 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedBoth)
params.pidOri.Ki = 0;
params.pidOri.Kd = 0;
- params.limits.linearVelocity = 500.F; // [mm/s]
- params.limits.angularVelocity = 2.F * M_PIf32 / 10.F; // [rad/s]
+ params.limits.linear = 500.F; // [mm/s]
+ params.limits.angular = 2.F * M_PIf32 / 10.F; // [rad/s]
traj_ctrl::TrajectoryFollowingController controller(params, scene);
@@ -419,14 +419,14 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerApplyLimitsExceedBoth)
ARMARX_DEBUG << "Clipped twist angular " << clippedTwist.angular;
// check that velocity is clipped correctly
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linearVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linearVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.x()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.y()), params.limits.linear);
+ BOOST_CHECK_LE(std::abs(clippedTwist.linear.z()), params.limits.linear);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angularVelocity);
- BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angularVelocity);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.x()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.y()), params.limits.angular);
+ BOOST_CHECK_LE(std::abs(clippedTwist.angular.z()), params.limits.angular);
ARMARX_DEBUG << std::abs(clippedTwist.angular.z());
- ARMARX_DEBUG << "GT " << params.limits.angularVelocity;
+ ARMARX_DEBUG << "GT " << params.limits.angular;
}
--
GitLab