fixing control params

source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp

@@ -63,21 +63,21 @@ namespace armarx::nav::traj_ctrl
                std::numeric_limits<double>::max(),
                false,
                std::vector<bool>{false, false, false}),
-        pidPosTarget(params.pidPos.Kp / 5,
+        pidPosTarget(params.pidPos.Kp,
                      params.pidPos.Ki,
                      params.pidPos.Kd,
                      std::numeric_limits<double>::max(),
                      std::numeric_limits<double>::max(),
                      false,
                      std::vector<bool>{false, false, false}),
-        pidOri(params.pidOri.Kp / 2, // FIXME
+        pidOri(params.pidOri.Kp,
                params.pidOri.Ki,
                params.pidOri.Kd,
                std::numeric_limits<double>::max(),
                std::numeric_limits<double>::max(),
                false,
                std::vector<bool>{true, true, true}),
-        pidOriTarget(params.pidOri.Kp / 5, // FIXME
+        pidOriTarget(params.pidOri.Kp,
                      params.pidOri.Ki,
                      params.pidOri.Kd,
                      std::numeric_limits<double>::max(),