From 638ba465eb767c5c21988e6559d9f8bcbf5d9cf0 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 20 Jun 2023 15:09:47 +0200
Subject: [PATCH] Auto format (pure)
---
VirtualRobot/Nodes/HemisphereJoint/Maths.cpp | 75 +++++++++-----------
VirtualRobot/Nodes/HemisphereJoint/Maths.h | 7 +-
2 files changed, 33 insertions(+), 49 deletions(-)
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
index 8ace6dfe8..0d94948af 100644
--- a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
+++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
@@ -5,87 +5,76 @@
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/pose/pose.h>
-
namespace VirtualRobot::hemisphere
{
- Maths::Maths() :
- Maths(1, simox::math::deg_to_rad(25.))
+ Maths::Maths() : Maths(1, simox::math::deg_to_rad(25.))
{
}
-
Maths::Maths(double lever, double theta0)
{
this->setConstants(lever, theta0);
}
-
- void Maths::setConstants(double lever, double theta0)
+ void
+ Maths::setConstants(double lever, double theta0)
{
this->lever = lever;
this->theta0Rad = theta0;
this->radius = 2 * std::sin(theta0) * lever;
- this->limitHi = simox::math::deg_to_rad(45 - 6.0);
- this->limitLo = - simox::math::deg_to_rad(45 - 14.0);
+ this->limitHi = simox::math::deg_to_rad(45 - 6.0);
+ this->limitLo = -simox::math::deg_to_rad(45 - 14.0);
}
-
- void Maths::computeFkOfPosition(double p1, double p2)
+ void
+ Maths::computeFkOfPosition(double p1, double p2)
{
expressions.compute(p1, p2, lever, theta0Rad);
}
-
- void Maths::computeFkOfPosition(const ActuatorPosition& p12)
+ void
+ Maths::computeFkOfPosition(const ActuatorPosition& p12)
{
computeFkOfPosition(p12(0), p12(1));
}
-
- void Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
+ void
+ Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
{
computeFkOfPosition(angleToPosition(alpha12));
}
-
- Eigen::Vector3d Maths::getEndEffectorTranslation() const
+ Eigen::Vector3d
+ Maths::getEndEffectorTranslation() const
{
- return Eigen::Vector3d {
- expressions.ex,
- expressions.ey,
- expressions.ez
- };
+ return Eigen::Vector3d{expressions.ex, expressions.ey, expressions.ez};
}
-
- Eigen::Matrix3d Maths::getEndEffectorRotation() const
+ Eigen::Matrix3d
+ Maths::getEndEffectorRotation() const
{
// r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]])
Eigen::Matrix3d ori;
- ori << expressions.exx, expressions.eyx, expressions.ezx,
- expressions.exy, expressions.eyy, expressions.ezy,
- expressions.exz, expressions.eyz, expressions.ezz;
+ ori << expressions.exx, expressions.eyx, expressions.ezx, expressions.exy, expressions.eyy,
+ expressions.ezy, expressions.exz, expressions.eyz, expressions.ezz;
return ori;
}
-
- Eigen::Matrix4d Maths::getEndEffectorTransform() const
+ Eigen::Matrix4d
+ Maths::getEndEffectorTransform() const
{
return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation());
}
-
- Maths::Jacobian Maths::getJacobian() const
+ Maths::Jacobian
+ Maths::getJacobian() const
{
Maths::Jacobian jacobian;
- jacobian << expressions.jx1, expressions.jx2,
- expressions.jy1, expressions.jy2,
- expressions.jz1, expressions.jz2,
- expressions.jrx1, expressions.jrx2,
- expressions.jry1, expressions.jry2,
- expressions.jrz1, expressions.jrz2;
+ jacobian << expressions.jx1, expressions.jx2, expressions.jy1, expressions.jy2,
+ expressions.jz1, expressions.jz2, expressions.jrx1, expressions.jrx2, expressions.jry1,
+ expressions.jry2, expressions.jrz1, expressions.jrz2;
// Current state of constructing the orientational part.
// ToDo: Do this with symbolic math inside `Expressions`.
@@ -95,7 +84,7 @@ namespace VirtualRobot::hemisphere
for (int column = 0; column < 2; ++column)
{
// Imagine we apply (+1, 0) / (0, +1) actuator velocities.
- const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0
+ const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0
/*
* The rotation axis is orthogonal to the vector from origin to the
@@ -103,20 +92,20 @@ namespace VirtualRobot::hemisphere
*
* For the scaling, ask Cornelius. :)
*/
- const Eigen::Vector3d scaledRotAxis = eefStateTrans.cross(eefVelTrans)
- / eefStateTrans.norm() * 2;
+ const Eigen::Vector3d scaledRotAxis =
+ eefStateTrans.cross(eefVelTrans) / eefStateTrans.norm() * 2;
- jacobian.block<3, 1>(3, column) = scaledRotAxis; // / 1.0;
+ jacobian.block<3, 1>(3, column) = scaledRotAxis; // / 1.0;
}
}
return jacobian;
}
-
- Maths::ActuatorPosition Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
+ Maths::ActuatorPosition
+ Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
{
return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0Rad)).array());
}
-}
+} // namespace VirtualRobot::hemisphere
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.h b/VirtualRobot/Nodes/HemisphereJoint/Maths.h
index 76f0188b7..9928ab054 100644
--- a/VirtualRobot/Nodes/HemisphereJoint/Maths.h
+++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.h
@@ -4,7 +4,6 @@
#include "Expressions.h"
-
namespace VirtualRobot::hemisphere
{
@@ -18,13 +17,11 @@ namespace VirtualRobot::hemisphere
class Maths
{
public:
-
using ActuatorPosition = Eigen::Vector2d;
using ActuatorAngle = Eigen::Vector2d;
using Jacobian = Eigen::Matrix<double, 6, 2>;
public:
-
Maths();
Maths(double lever, double theta0);
@@ -48,7 +45,6 @@ namespace VirtualRobot::hemisphere
public:
-
double lever = 0;
double theta0Rad = 0;
double radius = 0;
@@ -58,7 +54,6 @@ namespace VirtualRobot::hemisphere
Expressions expressions;
-
};
-}
+} // namespace VirtualRobot::hemisphere
--
GitLab