diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp index 60672d5da0b446160b6e0a075a52303a44d10c79..950edbf74b1bdc1516614b7dfc8ac5c9a9598845 100644 --- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp +++ b/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"); diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp index 27525c42d19079f8fbd3db620f03ad1710f09897..03ed255120ccf8a5084ea824f7d8e2d790c1c485 100644 --- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp +++ b/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); +} diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h index 96bd461e7f6194de38283839eaaff0d5ec135967..12d7100f997eeb818f6cf0aff989e42a92059df5 100644 --- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h +++ b/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