kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Robots: Use std::numeric_limits and std::numbers instead of defines

Browse Source
This commit is contained in:
Benjamin Nauck
2025-03-27 19:02:17 +01:00
parent 7efa4c1830
commit e8d836f390
5 changed files with 31 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp
View File

@@ -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 <numbers>
#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_min.rows(); j++) {
if(q_out(j) < q_min(j))
//q_out(j) = q_min(j); // FreeCAD change
q_out(j) = q_out(j) + M_PI *2;
q_out(j) = q_out(j) + std::numbers::pi *2;
}
for(unsigned int j=0; j<q_max.rows(); j++) {
if(q_out(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;
}
}

8
src/Mod/Robot/App/kdl_cp/frames.cpp
View File

@@ -26,9 +26,7 @@
***************************************************************************/
#include "frames.hpp"
#define _USE_MATH_DEFINES // For MSVC
#include <math.h>
#include <numbers>
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);

2
src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp
View File

@@ -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.

2
src/Mod/Robot/App/kdl_cp/utilities/rall2d.h
View File

@@ -26,7 +26,7 @@
#ifndef Rall2D_H
#define Rall2D_H
#include <math.h>
#include <cmath>
#include <assert.h>
#include "utility.h"

40
src/Mod/Robot/Gui/ViewProviderRobotObject.cpp
View File

@@ -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<Robot::RobotObject*>(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<Robot::RobotObject*>(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) {