Merge branch 'master' of gitlab.com:ArmarX/RobotAPI

source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp

@@ -359,8 +359,8 @@ armarx::ObstacleAvoidingPlatformUnit::get_velocities()
     ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
     ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
 
-    ARMARX_DEBUG << "Target velocity:  " << target_vel.transpose() << "; " << target_rot_vel;
-    ARMARX_DEBUG << "Modulated velocity:  " << modulated_vel.transpose();
+    ARMARX_DEBUG << "Target velocity:  " << target_vel.transpose() << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+    ARMARX_DEBUG << "Modulated velocity:  " << modulated_vel.transpose() << modulated_vel.norm();
 
     const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
 
@@ -802,14 +802,16 @@ armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
 
     // Settings.
     def->optional(m_control_data.adaptive_max_vel_exp, "adaptive_max_vel_exponent",
-                  "Adaptive max vel exponent.  0 = disable.");
+                  "Adaptive max vel exponent.  This throttles the max_vel adaptively "
+                  "depending on the angle between target velocity and modulated velocity.  "
+                  "0 = disable.");
 
     // Control parameters.
     def->optional(m_control_data.kp, "control.pos.kp");
     def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
     def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
     def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
-    def->optional(m_control_loop.cycle_time, "control.pose.cycle_time");
+    def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
 
     // Obstacle avoidance parameter.
     def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");

source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp

@@ -89,3 +89,10 @@ const
     return target_dist < m_pos_reached_threshold and
            std::fabs(target_angular_dist) < m_ori_reached_threshold;
 }
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setMaxVelocities(float max_vel, float max_angular_vel)
+{
+    m_platform_unit->setMaxVelocities(max_vel, max_angular_vel);
+}

source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h

@@ -61,6 +61,9 @@ namespace armarx
         isFinalTargetReached()
         const;
 
+        void
+        setMaxVelocities(float max_vel, float max_angular_vel);
+
     private:
 
         struct target