twist limits

source/armarx/navigation/trajectory_control/TrajectoryController.h

@@ -49,7 +49,7 @@ namespace armarx::navigation::traj_ctrl
     {
         core::TwistLimits limits{
             .linear = 500.F,                // [mm/s]
-            .angular = 2.F * M_PIf32 / 30.F // [rad/s]
+            .angular = 2.F * M_PIf32 / 10.F // [rad/s]
         };
 
         virtual ~TrajectoryControllerParams() = default;

source/armarx/navigation/trajectory_control/TrajectoryFollowingController.cpp

@@ -179,18 +179,18 @@ namespace armarx::navigation::traj_ctrl
 
             const auto feedforwardVelocity = desiredMovementDirection * ffVel;
 
-            ARMARX_DEBUG << deactivateSpam(100) << "Feed forward direction "
+            ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward direction "
                          << feedforwardVelocity.normalized();
-            ARMARX_DEBUG << deactivateSpam(100) << "Feed forward velocity " << feedforwardVelocity;
-            ARMARX_DEBUG << deactivateSpam(100) << "Control value " << pidPos.getControlValue();
+            ARMARX_VERBOSE << deactivateSpam(1) << "Feed forward velocity " << feedforwardVelocity;
+            ARMARX_VERBOSE << deactivateSpam(1) << "Control value " << pidPos.getControlValue();
 
             return core::Twist{.linear = pidPos.getControlValue() + feedforwardVelocity,
                                .angular = pidOri.getControlValue()};
         }();
 
-        const auto twistLimited = twist;
-        //const auto twistLimited = applyTwistLimits(twist);
-        ARMARX_DEBUG << deactivateSpam(100) << "Twist limited " << twistLimited.linear;
+        const auto twistLimited = applyTwistLimits(twist);
+        ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();
+        ARMARX_VERBOSE << deactivateSpam(1) << "Twist angular " << twistLimited.angular.transpose();
 
         return TrajectoryControllerResult{.twist = twistLimited,
                                           .dropPoint = projectedPose.projection};