From e8d836f3900ac055fcd1cbc3454b3f70d0322484 Mon Sep 17 00:00:00 2001 From: Benjamin Nauck Date: Thu, 27 Mar 2025 19:02:17 +0100 Subject: [PATCH] Robots: Use std::numeric_limits and std::numbers instead of defines --- .../App/kdl_cp/chainiksolverpos_nr_jl.cpp | 15 ++----- src/Mod/Robot/App/kdl_cp/frames.cpp | 8 ++-- .../App/kdl_cp/path_roundedcomposite.hpp | 2 +- src/Mod/Robot/App/kdl_cp/utilities/rall2d.h | 2 +- src/Mod/Robot/Gui/ViewProviderRobotObject.cpp | 40 ++++++++++--------- 5 files changed, 31 insertions(+), 36 deletions(-) diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp index 1ed3c4d72a..3814a77b79 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp @@ -21,17 +21,10 @@ // License along with this library; if not, write to the Free Software // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +#include + #include "chainiksolverpos_nr_jl.hpp" -// FreeCAD change -#ifndef M_PI - #define M_PI 3.14159265358979323846 /* pi */ -#endif - -#ifndef M_PI_2 - #define M_PI_2 1.57079632679489661923 /* pi/2 */ -#endif - namespace KDL { ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, @@ -60,14 +53,14 @@ namespace KDL for(unsigned int j=0; j q_max(j)) //q_out(j) = q_max(j); // FreeCAD change - q_out(j) = q_out(j) - M_PI *2; + q_out(j) = q_out(j) - std::numbers::pi *2; } } diff --git a/src/Mod/Robot/App/kdl_cp/frames.cpp b/src/Mod/Robot/App/kdl_cp/frames.cpp index 67baff97a5..ff24c29f6e 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.cpp +++ b/src/Mod/Robot/App/kdl_cp/frames.cpp @@ -26,9 +26,7 @@ ***************************************************************************/ #include "frames.hpp" - -#define _USE_MATH_DEFINES // For MSVC -#include +#include namespace KDL { @@ -244,7 +242,7 @@ void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const { double epsilon=1E-12; pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); - if ( fabs(pitch) > (M_PI/2.0-epsilon) ) { + if ( fabs(pitch) > (std::numbers::pi/2.0-epsilon) ) { yaw = atan2( -data[1], data[4]); roll = 0.0 ; } else { @@ -358,7 +356,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { return 0; } if (ca < -1+t) { - // The case of angles consisting of multiples of M_PI: + // The case of angles consisting of multiples of std::numbers::pi: // two solutions, choose a positive Z-component of the axis double x = sqrt( (data[0]+1.0)/2); double y = sqrt( (data[4]+1.0)/2); diff --git a/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp b/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp index 79c2657f57..ddb4bf7cc2 100644 --- a/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp +++ b/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp @@ -96,7 +96,7 @@ class Path_RoundedComposite : public Path * - 3101 if the eq. radius <= 0 * - 3102 if the first segment in a rounding has zero length. * - 3103 if the second segment in a rounding has zero length. - * - 3104 if the angle between the first and the second segment is close to M_PI. + * - 3104 if the angle between the first and the second segment is close to std::numbers::pi. * (meaning that the segments are on top of each other) * - 3105 if the distance needed for the rounding is larger then the first segment. * - 3106 if the distance needed for the rounding is larger then the second segment. diff --git a/src/Mod/Robot/App/kdl_cp/utilities/rall2d.h b/src/Mod/Robot/App/kdl_cp/utilities/rall2d.h index ef90674391..b80c27d13a 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/rall2d.h +++ b/src/Mod/Robot/App/kdl_cp/utilities/rall2d.h @@ -26,7 +26,7 @@ #ifndef Rall2D_H #define Rall2D_H -#include +#include #include #include "utility.h" diff --git a/src/Mod/Robot/Gui/ViewProviderRobotObject.cpp b/src/Mod/Robot/Gui/ViewProviderRobotObject.cpp index ec3ecd8feb..5b1852c2ba 100644 --- a/src/Mod/Robot/Gui/ViewProviderRobotObject.cpp +++ b/src/Mod/Robot/Gui/ViewProviderRobotObject.cpp @@ -173,6 +173,8 @@ void ViewProviderRobotObject::onChanged(const App::Property* prop) void ViewProviderRobotObject::updateData(const App::Property* prop) { + using std::numbers::pi; + Robot::RobotObject* robObj = static_cast(pcObject); if (prop == &robObj->RobotVrmlFile) { // read also from file @@ -269,33 +271,33 @@ void ViewProviderRobotObject::updateData(const App::Property* prop) } if (Axis1Node) { Axis1Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis1.getValue() * (M_PI / 180)); + robObj->Axis1.getValue() * (pi / 180)); } if (Axis2Node) { Axis2Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis2.getValue() * (M_PI / 180)); + robObj->Axis2.getValue() * (pi / 180)); } if (Axis3Node) { Axis3Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis3.getValue() * (M_PI / 180)); + robObj->Axis3.getValue() * (pi / 180)); } if (Axis4Node) { Axis4Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis4.getValue() * (M_PI / 180)); + robObj->Axis4.getValue() * (pi / 180)); } if (Axis5Node) { Axis5Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis5.getValue() * (M_PI / 180)); + robObj->Axis5.getValue() * (pi / 180)); } if (Axis6Node) { Axis6Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis6.getValue() * (M_PI / 180)); + robObj->Axis6.getValue() * (pi / 180)); } } else if (prop == &robObj->Axis1) { if (Axis1Node) { Axis1Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis1.getValue() * (M_PI / 180)); + robObj->Axis1.getValue() * (pi / 180)); if (toolShape) { toolShape->setTransformation( (robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()); @@ -305,7 +307,7 @@ void ViewProviderRobotObject::updateData(const App::Property* prop) else if (prop == &robObj->Axis2) { if (Axis2Node) { Axis2Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis2.getValue() * (M_PI / 180)); + robObj->Axis2.getValue() * (pi / 180)); if (toolShape) { toolShape->setTransformation( (robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()); @@ -315,7 +317,7 @@ void ViewProviderRobotObject::updateData(const App::Property* prop) else if (prop == &robObj->Axis3) { if (Axis3Node) { Axis3Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis3.getValue() * (M_PI / 180)); + robObj->Axis3.getValue() * (pi / 180)); if (toolShape) { toolShape->setTransformation( (robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()); @@ -325,7 +327,7 @@ void ViewProviderRobotObject::updateData(const App::Property* prop) else if (prop == &robObj->Axis4) { if (Axis4Node) { Axis4Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis4.getValue() * (M_PI / 180)); + robObj->Axis4.getValue() * (pi / 180)); if (toolShape) { toolShape->setTransformation( (robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()); @@ -335,7 +337,7 @@ void ViewProviderRobotObject::updateData(const App::Property* prop) else if (prop == &robObj->Axis5) { if (Axis5Node) { Axis5Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis5.getValue() * (M_PI / 180)); + robObj->Axis5.getValue() * (pi / 180)); if (toolShape) { toolShape->setTransformation( (robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()); @@ -345,7 +347,7 @@ void ViewProviderRobotObject::updateData(const App::Property* prop) else if (prop == &robObj->Axis6) { if (Axis6Node) { Axis6Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), - robObj->Axis6.getValue() * (M_PI / 180)); + robObj->Axis6.getValue() * (pi / 180)); if (toolShape) { toolShape->setTransformation( (robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()); @@ -395,27 +397,29 @@ void ViewProviderRobotObject::setAxisTo(float A1, float A6, const Base::Placement& Tcp) { + using std::numbers::pi; + Robot::RobotObject* robObj = static_cast(pcObject); if (Axis1Node) { // FIXME Ugly hack for the wrong transformation of the Kuka 500 robot VRML the minus sign on // Axis 1 - Axis1Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A1 * (M_PI / 180)); + Axis1Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A1 * (pi / 180)); } if (Axis2Node) { - Axis2Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A2 * (M_PI / 180)); + Axis2Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A2 * (pi / 180)); } if (Axis3Node) { - Axis3Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A3 * (M_PI / 180)); + Axis3Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A3 * (pi / 180)); } if (Axis4Node) { - Axis4Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A4 * (M_PI / 180)); + Axis4Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A4 * (pi / 180)); } if (Axis5Node) { - Axis5Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A5 * (M_PI / 180)); + Axis5Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A5 * (pi / 180)); } if (Axis6Node) { - Axis6Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A6 * (M_PI / 180)); + Axis6Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), A6 * (pi / 180)); } // update tool position if (toolShape) {