Add hemisphere::Joint class, replacing operations

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

VirtualRobot/Nodes/HemisphereJoint/Joint.cpp

+#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;
+    }
+
+}

VirtualRobot/Nodes/HemisphereJoint/Joint.h

+#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;
+
+    };
+
+}

VirtualRobot/Nodes/HemisphereJoint/operations.cpp

-#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;
-    }
-
-}
-

VirtualRobot/Nodes/HemisphereJoint/operations.h

-#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);
-
-}

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;