diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp index 1abe2fe620c86505a3e457a6565d446467fa949f..c21390e09e23ab3afb502eca5a6f48b8b3d45249 100644 --- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp +++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp @@ -588,6 +588,7 @@ bool armarx::ObstacleAvoidingPlatformUnit::is_almost_reaching_target(const Eigen const float sim = cosine_similarity(target_direction, control_direction); + // if almost pointing into same direction if (sim > cos(M_PI_4f32)) { return true;