From 1ed35ba368d3b4b285f4c5bfff06d39171b3b2ec Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 10 Jun 2022 18:10:21 +0200
Subject: [PATCH] Add hemisphere::Joint class, replacing operations
---
VirtualRobot/CMakeLists.txt | 4 +-
VirtualRobot/Nodes/HemisphereJoint/Joint.cpp | 81 +++++++++++++++++++
VirtualRobot/Nodes/HemisphereJoint/Joint.h | 44 ++++++++++
.../Nodes/HemisphereJoint/operations.cpp | 41 ----------
.../Nodes/HemisphereJoint/operations.h | 19 -----
.../examples/HemisphereJoint/main.cpp | 19 +++--
6 files changed, 136 insertions(+), 72 deletions(-)
create mode 100644 VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
create mode 100644 VirtualRobot/Nodes/HemisphereJoint/Joint.h
delete mode 100644 VirtualRobot/Nodes/HemisphereJoint/operations.cpp
delete mode 100644 VirtualRobot/Nodes/HemisphereJoint/operations.h
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index a4cc84f3b..d0e59fd2f 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -336,7 +336,7 @@ SET(SOURCES
Nodes/RobotNodeRevoluteFactory.cpp
Nodes/Sensor.cpp
Nodes/HemisphereJoint/Expressions.cpp
- Nodes/HemisphereJoint/operations.cpp
+ Nodes/HemisphereJoint/Joint.cpp
TimeOptimalTrajectory/Path.cpp
TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
@@ -579,7 +579,7 @@ SET(INCLUDES
Nodes/Sensor.h
Nodes/SensorFactory.h
Nodes/HemisphereJoint/Expressions.h
- Nodes/HemisphereJoint/operations.h
+ Nodes/HemisphereJoint/Joint.h
TimeOptimalTrajectory/Path.h
TimeOptimalTrajectory/TimeOptimalTrajectory.h
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp b/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
new file mode 100644
index 000000000..c6317e042
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/Joint.cpp
@@ -0,0 +1,81 @@
+#include "Joint.h"
+
+#include <cmath>
+
+#include <SimoxUtility/math/convert/deg_to_rad.h>
+#include <SimoxUtility/math/pose/pose.h>
+
+
+namespace VirtualRobot::hemisphere
+{
+
+ Joint::Joint() :
+ Joint(1, simox::math::deg_to_rad(25.))
+ {
+ }
+
+
+ Joint::Joint(double lever, double theta0)
+ {
+ this->setConstants(lever, theta0);
+ }
+
+
+ void Joint::computeFK(double a1, double a2)
+ {
+ a1 += actuatorOffset;
+ a2 += actuatorOffset;
+ fk.compute(a1, a2, lever, theta0);
+ }
+
+
+ Eigen::Vector3d Joint::getEndEffectorTranslation() const
+ {
+ return Eigen::Vector3d {
+ fk.ex,
+ fk.ey,
+ fk.ez
+ };
+ }
+
+
+ Eigen::Matrix3d Joint::getEndEffectorRotation() const
+ {
+ // r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]])
+ Eigen::Matrix3d ori;
+ ori << fk.exx, fk.eyx, fk.ezx,
+ fk.exy, fk.eyy, fk.ezy,
+ fk.exz, fk.eyz, fk.ezz;
+ return ori;
+ }
+
+
+ Eigen::Matrix4d Joint::getEndEffectorTransform() const
+ {
+ return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation());
+ }
+
+
+ Joint::Jacobian Joint::getJacobian() const
+ {
+ Joint::Jacobian jacobian;
+ jacobian << fk.jx1, fk.jx2,
+ fk.jy1, fk.jy2,
+ fk.jz1, fk.jz2,
+ fk.jrx1, fk.jrx2,
+ fk.jry1, fk.jry2,
+ fk.jrz1, fk.jrz2;
+ return jacobian;
+ }
+
+
+ void Joint::setConstants(double lever, double theta0)
+ {
+ this->lever = lever;
+ this->theta0 = theta0;
+ this->radius = 2 * std::sin(theta0) * lever;
+ this->actuatorOffset = std::asin(theta0);
+ this->lever = 1;
+ }
+
+}
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Joint.h b/VirtualRobot/Nodes/HemisphereJoint/Joint.h
new file mode 100644
index 000000000..dc9c36424
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/Joint.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include "Expressions.h"
+
+
+namespace VirtualRobot::hemisphere
+{
+
+ class Joint
+ {
+ public:
+
+ using Jacobian = Eigen::Matrix<double, 6, 2>;
+
+ public:
+
+ Joint();
+ Joint(double lever, double theta0);
+
+ void computeFK(double a1, double a2);
+
+ Eigen::Vector3d getEndEffectorTranslation() const;
+ Eigen::Matrix3d getEndEffectorRotation() const;
+ Eigen::Matrix4d getEndEffectorTransform() const;
+ Jacobian getJacobian() const;
+
+ void setConstants(double lever, double theta0);
+
+
+ public:
+
+ double lever;
+ double theta0;
+ double radius;
+ double actuatorOffset;
+
+
+ Expressions fk;
+
+ };
+
+}
diff --git a/VirtualRobot/Nodes/HemisphereJoint/operations.cpp b/VirtualRobot/Nodes/HemisphereJoint/operations.cpp
deleted file mode 100644
index 6ab710f2c..000000000
--- a/VirtualRobot/Nodes/HemisphereJoint/operations.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-#include "operations.h"
-
-
-namespace VirtualRobot
-{
-
- Eigen::Vector3d hemisphere::getEndEffectorTranslation(const Expressions& expr)
- {
- return Eigen::Vector3d {
- expr.ex,
- expr.ey,
- expr.ez
- };
- }
-
-
- Eigen::Matrix3d hemisphere::getEndEffectorRotation(const Expressions& expr)
- {
- // r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]])
- Eigen::Matrix3d ori;
- ori << expr.exx, expr.eyx, expr.ezx,
- expr.exy, expr.eyy, expr.ezy,
- expr.exz, expr.eyz, expr.ezz;
- return ori;
- }
-
-
- Eigen::Matrix<double, 6, 2> hemisphere::getJacobian(const Expressions& expr)
- {
- Eigen::Matrix<double, 6, 2> jacobian;
- jacobian << expr.jx1, expr.jx2,
- expr.jy1, expr.jy2,
- expr.jz1, expr.jz2,
- expr.jrx1, expr.jrx2,
- expr.jry1, expr.jry2,
- expr.jrz1, expr.jrz2;
- return jacobian;
- }
-
-}
-
diff --git a/VirtualRobot/Nodes/HemisphereJoint/operations.h b/VirtualRobot/Nodes/HemisphereJoint/operations.h
deleted file mode 100644
index 3cde2cad7..000000000
--- a/VirtualRobot/Nodes/HemisphereJoint/operations.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#pragma once
-
-#include <Eigen/Core>
-
-#include "Expressions.h"
-
-
-namespace VirtualRobot::hemisphere
-{
-
- Eigen::Vector3d getEndEffectorTranslation(const Expressions& expr);
-
-
- Eigen::Matrix3d getEndEffectorRotation(const Expressions& expr);
-
-
- Eigen::Matrix<double, 6, 2> getJacobian(const Expressions& expr);
-
-}
diff --git a/VirtualRobot/examples/HemisphereJoint/main.cpp b/VirtualRobot/examples/HemisphereJoint/main.cpp
index e31c783ca..4133412a2 100644
--- a/VirtualRobot/examples/HemisphereJoint/main.cpp
+++ b/VirtualRobot/examples/HemisphereJoint/main.cpp
@@ -6,8 +6,7 @@
#include <SimoxUtility/math/statistics/measures.h>
#include <VirtualRobot/RuntimeEnvironment.h>
-#include <VirtualRobot/Nodes/HemisphereJoint/Expressions.h>
-#include <VirtualRobot/Nodes/HemisphereJoint/operations.h>
+#include <VirtualRobot/Nodes/HemisphereJoint/Joint.h>
using VirtualRobot::RuntimeEnvironment;
@@ -68,12 +67,12 @@ int main(int argc, char* argv[])
{
const time_point start = std::chrono::system_clock::now();
- VirtualRobot::hemisphere::Expressions expr;
- expr.compute(a1, a2, lever, theta0);
+ VirtualRobot::hemisphere::Joint joint;
+ joint.computeFK(a1, a2);
- const Eigen::Vector3d pos = VirtualRobot::hemisphere::getEndEffectorTranslation(expr);
- const Eigen::Matrix3d ori = VirtualRobot::hemisphere::getEndEffectorRotation(expr);
- const Eigen::Matrix<double, 6, 2> jacobian = VirtualRobot::hemisphere::getJacobian(expr);
+ const Eigen::Vector3d pos = joint.getEndEffectorTranslation();
+ const Eigen::Matrix3d ori = joint.getEndEffectorRotation();
+ const Eigen::Matrix<double, 6, 2> jacobian = joint.getJacobian();
const time_point end = std::chrono::system_clock::now();
using duration = std::chrono::nanoseconds;
@@ -110,10 +109,10 @@ int main(int argc, char* argv[])
a1 += offset;
a2 += offset;
- VirtualRobot::hemisphere::Expressions expr;
- expr.compute(a1, a2, lever, theta0);
+ VirtualRobot::hemisphere::Joint joint;
+ joint.computeFK(a1, a2);
- const Eigen::Vector3d pos = VirtualRobot::hemisphere::getEndEffectorTranslation(expr);
+ const Eigen::Vector3d pos = joint.getEndEffectorTranslation();
std::cout << "\n pos = \n" << pos.transpose().format(iof)
<< std::endl;
--
GitLab