update

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);
     }

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);
     };