diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
index 8252cedb302096ffe3bb8fa4fe42e3f4c5eafd63..be79e84e239d3269a3fe7d69c66ed9cb6364069d 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
@@ -2,8 +2,8 @@
namespace armarx::navigation::traj_ctrl
{
- TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
- {
- }
+ // TrajectoryController::TrajectoryController(const core::Scene& context) : context(context)
+ // {
+ // }
} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.h b/source/armarx/navigation/trajectory_control/TrajectoryController.h
index f13e168ade4b91fb653e123c9cf4b2b556ecc87e..72d5927d31f6c8b0885997a25c1aee3ce0ccb853 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryController.h
@@ -61,15 +61,11 @@ namespace armarx::navigation::traj_ctrl
class TrajectoryController
{
public:
- TrajectoryController(const core::Scene& context);
+ TrajectoryController() = default;
virtual ~TrajectoryController() = default;
virtual TrajectoryControllerResult control(const core::Trajectory& goal) = 0;
- protected:
- const core::Scene& context;
-
- private:
};
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
index 1997cb8fc631a6d360c04ab70e336541c0bf168f..2a68520c2133c6076355e5545943e99debd675e1 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp
@@ -34,7 +34,7 @@ namespace armarx::navigation::traj_ctrl
arondto::TrajectoryFollowingControllerParams dto;
TrajectoryFollowingControllerParams bo;
- aron_conv::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::toAron(dto, bo);
return dto.toAron();
}
@@ -46,16 +46,17 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
TrajectoryFollowingControllerParams bo;
- aron_conv::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::fromAron(dto, bo);
return bo;
}
// TrajectoryFollowingController
- TrajectoryFollowingController::TrajectoryFollowingController(const Params& params,
- const core::Scene& context) :
- TrajectoryController(context),
+ TrajectoryFollowingController::TrajectoryFollowingController(
+ const VirtualRobot::RobotPtr& robot,
+ const Params& params) :
+ robot(robot),
params(params),
pidPos(params.pidPos.Kp,
params.pidPos.Ki,
@@ -93,11 +94,8 @@ namespace armarx::navigation::traj_ctrl
core::Twist
TrajectoryFollowingController::applyTwistLimits(core::Twist twist)
{
- const core::Twist limits
- {
- .linear = Eigen::Vector3f::Ones() * params.limits.linear,
- .angular = Eigen::Vector3f::Ones() * params.limits.angular
- };
+ const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
+ .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());
@@ -128,9 +126,9 @@ namespace armarx::navigation::traj_ctrl
TrajectoryControllerResult
TrajectoryFollowingController::control(const core::Trajectory& trajectory)
{
- ARMARX_CHECK_NOT_NULL(context.robot) << "Robot not available";
+ ARMARX_CHECK_NOT_NULL(robot) << "Robot not available";
- const core::Pose currentPose(context.robot->getGlobalPose());
+ const core::Pose currentPose(robot->getGlobalPose());
const auto projectedPose = trajectory.getProjection(
currentPose.translation(), core::VelocityInterpolation::LinearInterpolation);
@@ -182,7 +180,7 @@ namespace armarx::navigation::traj_ctrl
const auto feedforwardVelocity = desiredMovementDirection * ffVel;
ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward direction "
- << feedforwardVelocity.normalized();
+ << feedforwardVelocity.normalized();
ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward velocity " << feedforwardVelocity;
ARMARX_VERBOSE << deactivateSpam(1) << "Control value " << pidPos.getControlValue();
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
index a9a8a94c46ed3b149d7a48f65e04660a6b5758c6..ffb81732fa4f599c341a3e1528bb791ab4a4395b 100644
--- a/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
+++ b/source/armarx/navigation/trajectory_control/TrajectoryFollowingController.h
@@ -48,7 +48,7 @@ namespace armarx::navigation::traj_ctrl
public:
using Params = TrajectoryFollowingControllerParams;
- TrajectoryFollowingController(const Params& params, const core::Scene& context);
+ TrajectoryFollowingController(const VirtualRobot::RobotPtr& robot, const Params& params);
~TrajectoryFollowingController() override = default;
TrajectoryControllerResult control(const core::Trajectory& trajectory) override;
@@ -57,8 +57,10 @@ namespace armarx::navigation::traj_ctrl
core::Twist applyTwistLimits(core::Twist twist);
private:
+ const VirtualRobot::RobotPtr robot;
const Params params;
+
MultiDimPIDControllerTemplate<3> pidPos;
MultiDimPIDControllerTemplate<3> pidPosTarget;
MultiDimPIDControllerTemplate<3> pidOri;
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.cpp b/source/armarx/navigation/trajectory_control/WaypointController.cpp
index c08ff5b97d8e25f5fb5e02934662c0b9436de1b1..527bd4f3a485f233f77db40ff9f55e5d54319117 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.cpp
+++ b/source/armarx/navigation/trajectory_control/WaypointController.cpp
@@ -21,7 +21,7 @@ namespace armarx::navigation::traj_ctrl
arondto::WaypointControllerParams dto;
WaypointControllerParams bo;
- aron_conv::toAron(dto, bo);
+ armarx::navigation::traj_ctrl::toAron(dto, bo);
return dto.toAron();
}
@@ -33,15 +33,15 @@ namespace armarx::navigation::traj_ctrl
dto.fromAron(dict);
WaypointControllerParams bo;
- aron_conv::fromAron(dto, bo);
+ armarx::navigation::traj_ctrl::fromAron(dto, bo);
return bo;
}
// WaypointController
- WaypointController::WaypointController(const Params& params, const core::Scene& context) :
- TrajectoryController(context), params(params)
+ WaypointController::WaypointController(const Params& params) :
+ params(params)
{
}
diff --git a/source/armarx/navigation/trajectory_control/WaypointController.h b/source/armarx/navigation/trajectory_control/WaypointController.h
index 13133e67fbd0a5001fae2451f3fdad0463d34cc4..ec5d4372840eadb710bdd8708fcd829cedc4038e 100644
--- a/source/armarx/navigation/trajectory_control/WaypointController.h
+++ b/source/armarx/navigation/trajectory_control/WaypointController.h
@@ -39,7 +39,7 @@ namespace armarx::navigation::traj_ctrl
public:
using Params = WaypointControllerParams;
- WaypointController(const Params& params, const core::Scene& context);
+ WaypointController(const Params& params);
~WaypointController() override = default;
TrajectoryControllerResult control(const core::Trajectory& goal) override;
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.cpp b/source/armarx/navigation/trajectory_control/aron_conversions.cpp
index d8d6d89847b9fcc580874080c457118384e517c5..2cc2d0b42889e5b9323c600f6b0e734b2874f44f 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.cpp
+++ b/source/armarx/navigation/trajectory_control/aron_conversions.cpp
@@ -13,7 +13,7 @@
#include <armarx/navigation/trajectory_control/aron/WaypointControllerParams.aron.generated.h>
-namespace armarx::navigation::traj_ctrl::aron_conv
+namespace armarx::navigation::traj_ctrl
{
void
@@ -67,4 +67,4 @@ namespace armarx::navigation::traj_ctrl::aron_conv
}
-} // namespace armarx::navigation::traj_ctrl::aron_conv
+} // namespace armarx::navigation::traj_ctrl
diff --git a/source/armarx/navigation/trajectory_control/aron_conversions.h b/source/armarx/navigation/trajectory_control/aron_conversions.h
index 14954575365d06862418f2862054df81dc8eeccc..f8496c98a49cf147968a3e02f19a03cd271f6e37 100644
--- a/source/armarx/navigation/trajectory_control/aron_conversions.h
+++ b/source/armarx/navigation/trajectory_control/aron_conversions.h
@@ -39,10 +39,6 @@ namespace armarx::navigation::traj_ctrl
class WaypointControllerParams;
} // namespace arondto
-} // namespace armarx::navigation::traj_ctrl
-
-namespace armarx::navigation::traj_ctrl::aron_conv
-{
void toAron(arondto::TrajectoryControllerParams& dto, const TrajectoryControllerParams& bo);
void fromAron(const arondto::TrajectoryControllerParams& dto, TrajectoryControllerParams& bo);
@@ -55,4 +51,4 @@ namespace armarx::navigation::traj_ctrl::aron_conv
void toAron(arondto::WaypointControllerParams& dto, const WaypointControllerParams& bo);
void fromAron(const arondto::WaypointControllerParams& dto, WaypointControllerParams& bo);
-} // namespace armarx::navigation::traj_ctrl::aron_conv
+} // namespace armarx::navigation::traj_ctrl