From c9e9dd714f2b79234fd031b5390b49360bb91f8f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 9 Jul 2021 10:45:30 +0200
Subject: [PATCH] update
---
.../TrajectoryFollowingController.cpp | 24 +++++++++----------
.../TrajectoryFollowingController.h | 1 +
2 files changed, 13 insertions(+), 12 deletions(-)
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index caab69bd..17174964 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
@@ -13,10 +13,9 @@
#include "Navigation/libraries/core/Trajectory.h"
#include "Navigation/libraries/core/types.h"
#include "Navigation/libraries/trajectory_control/TrajectoryController.h"
-#include "Navigation/libraries/trajectory_control/core.h"
-#include "Navigation/libraries/trajectory_control/aron_conversions.h"
-
#include "Navigation/libraries/trajectory_control/aron/TrajectoryFollowingControllerParams.aron.generated.h"
+#include "Navigation/libraries/trajectory_control/aron_conversions.h"
+#include "Navigation/libraries/trajectory_control/core.h"
namespace armarx::nav::traj_ctrl
{
@@ -103,7 +102,8 @@ namespace armarx::nav::traj_ctrl
const core::Pose currentPose(context.robot->getGlobalPose());
- const auto targetPose = trajectory->getProjection(currentPose, core::VelocityInterpolations::LastWaypoint);
+ const auto targetPose =
+ trajectory->getProjection(currentPose, core::VelocityInterpolations::LastWaypoint);
ARMARX_INFO << targetPose.waypoint.pose.translation();
@@ -115,21 +115,21 @@ namespace armarx::nav::traj_ctrl
// return p;
// };
- using simox::math::mat4f_to_rpy;
using simox::math::mat4f_to_pos;
+ using simox::math::mat4f_to_rpy;
- pidPos.update(mat4f_to_pos(currentPose.matrix()), mat4f_to_pos(targetPose.waypoint.pose.matrix()));
- pidOri.update(mat4f_to_rpy(currentPose.matrix()), mat4f_to_rpy(targetPose.waypoint.pose.matrix()));
+ pidPos.update(mat4f_to_pos(currentPose.matrix()),
+ mat4f_to_pos(targetPose.waypoint.pose.matrix()));
+ pidOri.update(mat4f_to_rpy(currentPose.matrix()),
+ mat4f_to_rpy(targetPose.waypoint.pose.matrix()));
ARMARX_INFO << "Feed forward velocity " << targetPose.twist.linear;
ARMARX_INFO << "Control value " << pidPos.getControlValue();
// TODO: better handling if feed forward velocity is not set. need movement direction ...
- core::Twist twist =
- {
- .linear = pidPos.getControlValue() + targetPose.twist.linear,
- .angular = pidOri.getControlValue() + targetPose.twist.angular
- };
+ core::Twist twist = {.linear = pidPos.getControlValue() + targetPose.twist.linear,
+ .angular = pidOri.getControlValue() + targetPose.twist.angular
+ };
return applyTwistLimits(twist);
}
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
index 3e4ac7e0..26369304 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
@@ -42,6 +42,7 @@ namespace armarx::nav::traj_ctrl
Algorithms algorithm() const override;
aron::datanavigator::DictNavigatorPtr toAron() const override;
+
static TrajectoryFollowingControllerParams
FromAron(const aron::datanavigator::DictNavigatorPtr& dict);
};
--
GitLab