From ad92494538094e047b737c060920650578625ce4 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 13:36:43 +0200
Subject: [PATCH] trajectory_control: only robot is necessary (instead of full
scene)
---
.../TrajectoryController.cpp | 6 ++---
.../trajectory_control/TrajectoryController.h | 6 +----
.../TrajectoryFollowingController.cpp | 24 +++++++++----------
.../TrajectoryFollowingController.h | 4 +++-
.../trajectory_control/WaypointController.cpp | 8 +++----
.../trajectory_control/WaypointController.h | 2 +-
.../trajectory_control/aron_conversions.cpp | 4 ++--
.../trajectory_control/aron_conversions.h | 6 +----
8 files changed, 26 insertions(+), 34 deletions(-)
diff --git a/source/armarx/navigation/trajectory_control/TrajectoryController.cpp b/source/armarx/navigation/trajectory_control/TrajectoryController.cpp
index 8252cedb..be79e84e 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 f13e168a..72d5927d 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 1997cb8f..2a68520c 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 a9a8a94c..ffb81732 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 c08ff5b9..527bd4f3 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 13133e67..ec5d4372 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 d8d6d898..2cc2d0b4 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 14954575..f8496c98 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
--
GitLab