From d955bde121ba5b31063ebb0070424eee23a6a48e Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 7 Jun 2022 17:13:12 +0200
Subject: [PATCH] Move code to library
---
VirtualRobot/CMakeLists.txt | 2 ++
.../Nodes/HemisphereJoint/operations.cpp | 35 +++++++++++++++++++
.../Nodes/HemisphereJoint/operations.h | 19 ++++++++++
.../examples/HemisphereJoint/main.cpp | 28 ++++++---------
4 files changed, 67 insertions(+), 17 deletions(-)
create mode 100644 VirtualRobot/Nodes/HemisphereJoint/operations.cpp
create mode 100644 VirtualRobot/Nodes/HemisphereJoint/operations.h
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index b1f94f6eb..4e3246faa 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -336,6 +336,7 @@ SET(SOURCES
Nodes/RobotNodeRevoluteFactory.cpp
Nodes/Sensor.cpp
Nodes/HemisphereJoint/Expressions.cpp
+ Nodes/HemisphereJoint/operations.cpp
TimeOptimalTrajectory/Path.cpp
TimeOptimalTrajectory/TimeOptimalTrajectory.cpp
@@ -578,6 +579,7 @@ SET(INCLUDES
Nodes/Sensor.h
Nodes/SensorFactory.h
Nodes/HemisphereJoint/Expressions.h
+ Nodes/HemisphereJoint/operations.h
TimeOptimalTrajectory/Path.h
TimeOptimalTrajectory/TimeOptimalTrajectory.h
diff --git a/VirtualRobot/Nodes/HemisphereJoint/operations.cpp b/VirtualRobot/Nodes/HemisphereJoint/operations.cpp
new file mode 100644
index 000000000..17e275295
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/operations.cpp
@@ -0,0 +1,35 @@
+#include "operations.h"
+
+
+namespace VirtualRobot
+{
+
+ Eigen::Vector3d hemisphere::getEndEffectorPosition(const Expressions& expr)
+ {
+ return Eigen::Vector3d {expr.ex, expr.ey, expr.ez};
+ }
+
+ Eigen::Matrix3d hemisphere::getEndEffectorOrientation(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
new file mode 100644
index 000000000..091863802
--- /dev/null
+++ b/VirtualRobot/Nodes/HemisphereJoint/operations.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include "Expressions.h"
+
+
+namespace VirtualRobot::hemisphere
+{
+
+ Eigen::Vector3d getEndEffectorPosition(const Expressions& expr);
+
+
+ Eigen::Matrix3d getEndEffectorOrientation(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 fc371c634..db85f4cb3 100644
--- a/VirtualRobot/examples/HemisphereJoint/main.cpp
+++ b/VirtualRobot/examples/HemisphereJoint/main.cpp
@@ -7,6 +7,7 @@
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Nodes/HemisphereJoint/Expressions.h>
+#include <VirtualRobot/Nodes/HemisphereJoint/operations.h>
using VirtualRobot::RuntimeEnvironment;
@@ -63,21 +64,9 @@ int main(int argc, char* argv[])
VirtualRobot::hemisphere::Expressions expr;
expr.compute(a1, a2, lever, theta0);
- Eigen::Vector3d pos {expr.ex, expr.ey, expr.ez};
-
- // 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;
-
- 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;
+ const Eigen::Vector3d pos = VirtualRobot::hemisphere::getEndEffectorPosition(expr);
+ const Eigen::Matrix3d ori = VirtualRobot::hemisphere::getEndEffectorOrientation(expr);
+ const Eigen::Matrix<double, 6, 2> jacobian = VirtualRobot::hemisphere::getJacobian(expr);
const time_point end = std::chrono::system_clock::now();
using duration = std::chrono::nanoseconds;
@@ -85,8 +74,13 @@ int main(int argc, char* argv[])
if (verbose)
{
- std::cout << "(a1, a2) = (" << a1 << ", " << a2 << ") \t -> "
- << "pos = (" << pos.transpose() << ")" << std::endl;
+ Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]", "", "");
+ std::cout << "(a1, a2) = (" << a1 << ", " << a2 << ")"
+ << "\n ->"
+ << "\n pos = \n" << pos.transpose().format(iof)
+ << "\n ori = \n" << ori.format(iof)
+ << "\n jac = \n" << jacobian.format(iof)
+ << std::endl;
}
}
}
--
GitLab