diff --git a/source/Navigation/libraries/core/CMakeLists.txt b/source/Navigation/libraries/core/CMakeLists.txt
index c301c053f566688980482a68798898219cf2c5c9..2491642595950325c0be186450a58b0faa235a82 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 6e4b3dfdbf848e2baa979e96cd99a41061002e42..ac64be684b1a22cd504e2900422c8b2db2e1cfe5 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 0000000000000000000000000000000000000000..6cbc5a1cbec2d8c153964fe58754a2ffdf5814e2
--- /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 3e0460af135e93e9651bde2d4dd774588efaf2be..a6d4617c181b4a57ad11a427063d2b745d6f758e 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 7e2159b92bf9d79dbac0cf6c43e85c6f017f9adf..eb36a46e81ec75087e62265254b9d2a4d7b134c6 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 21e9743a9b283423bd3859f262feae4e42ef2f38..0ff85fdc9e39e246d6800dce94d3a354bcce4c3d 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 fd38f8d3f4a4ed96481afb071af5150611d36199..4054ad3f15d823eebb1a312fe507a8b984989496 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 c23d9186efb3aac8f4ede0642107b37fe00b02ac..71894e875e4e5d957b28db417f0d5816414ff503 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 ac00c7c0a37ae28e16b3bf88e52b24fffd602247..2ccc88a7ed2c9694d9324e92dcb8bea527efebac 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 9c0efc0bd87f8c72e855c212f6410a0277fdc3c1..4cb81ebbb6ef0eb5bd9c5dff8c4090c9aa8fd421 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 8c9d381eac4ac78a57cc66d7da99f151eeb31ac8..306cb92d614ae6d0c5da8d3ef79f29249b236b28 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 b8660edf1b3055423b02a1dbe2c070ac870ab5b0..af9d00c14d9e15194e53613f5009e128494c70ef 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 9435aa5365ccc8bfc149dd7eb263ffc8bb2adc2d..93483adc55af99ef3e8376b7c3811946e62055b4 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 12178527bffe21e04875a54c0ca01ed4e5cf819b..423552bdf942493ee18a52706f5ea6f17446ef58 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 0809845c7eedd8afc00d9a0aeb973b3ca162c047..02f589d080dbf0c374cb1be5e750a1ee1750fbde 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 dadd2d944829b6880c110f399fd0467171194cc6..5690bc1ddbe44039ea4dbd3a2ae79466f39e8890 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 d7cea76828970be5309b415bc34d91859a17591a..cfff609f9887bf1d1b3401d0fc7bc44065ce08ac 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 b2d43512b2387b0748d546dd5308fc4cb0b058bd..545dc4d5b5cd588f8c7e346fa1d1260e22e5f9fc 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 df8af9f0ef0d1919b8e8e6f2ca3061ad7cc4be9d..72350a3c6dbc7442d12eddbeeef4c86cba4e30f6 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 f84a76dcb584e698b75e7ed53e1c99c7550cd21c..6b5a7f0294752dc281d38e8f44fb79270fc80b3c 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;
}