(FR, RK) Fix orientation PID controller (resetting I-value). Fix visualization

source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp

@@ -171,6 +171,8 @@ armarx::ObstacleAwarePlatformUnit::moveTo(
     invalidate(m_control_data.target_vel);
     invalidate(m_control_data.target_rot_vel);
 
+    m_rot_pid_controller.reset();
+
     schedule_high_level_control_loop(control_mode::position);
 }
 
@@ -460,8 +462,10 @@ const
 
     ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
 
-    return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
-                         uncapped_target_rot_vel);
+    return std::clamp(uncapped_target_rot_vel, -m_control_data.max_rot_vel, m_control_data.max_rot_vel);
+
+    //    return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+    //                         uncapped_target_rot_vel);
 }
 
 
@@ -607,7 +611,7 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
             l_prog.add(Cylinder{"boundingCylinder"}
                        .position(agent_pos)
                        .color(Color::cyan(255, 64))
-                       .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) * Eigen::Isometry3f(Eigen::Translation3f(0, 0, 1000)).matrix())
+                       .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) * Eigen::Isometry3f(Eigen::Translation3f(0, -1000, 0)).matrix())
                        .radius(m_viz.boundingCircle.radius)
                        .height(2000));
         }
@@ -687,6 +691,8 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
                        .color(Color::cyan(255, 128))
                        .width(25));
         }
+
+        // TODO rotation arrow
     }
 
     // Agent.