diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp index caab69bd730d33da2716ad48361868149f8548c0..17174964573ae8847e22e9a0867d0f11cc6cbd75 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 3e4ac7e056ff4f1cd9a83a7e494c851b3ef62c12..26369304b0161191042237784d8e2ce485fb8c08 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); };