Use angles as actuators instead of positions

VirtualRobot/Nodes/HemisphereJoint/Joint.cpp

@@ -26,14 +26,27 @@ namespace VirtualRobot::hemisphere
         this->lever = lever;
         this->theta0 = theta0;
         this->radius = 2 * std::sin(theta0) * lever;
-        this->actuatorOffset = std::asin(theta0);
-        this->lever = 1;
+
+        this->limitHi =   simox::math::deg_to_rad(45 - 6.0);
+        this->limitLo = - simox::math::deg_to_rad(45 - 14.0);
+    }
+
+
+    void Joint::computeFkOfPosition(double p1, double p2)
+    {
+        fk.compute(p1, p2, lever, theta0);
     }
 
 
-    void Joint::computeFK(double a1, double a2)
+    void Joint::computeFkOfPosition(const Eigen::Vector2d& p12)
     {
-        fk.compute(a1, a2, lever, theta0);
+        computeFkOfPosition(p12(0), p12(1));
+    }
+
+
+    void Joint::computeFkOfAngle(const Eigen::Vector2d& alpha12)
+    {
+        computeFkOfPosition(angleToPosition(alpha12));
     }
 
 
@@ -46,13 +59,6 @@ namespace VirtualRobot::hemisphere
         };
     }
 
-    Eigen::Vector3d Joint::getCorrectedEndEffectorTranslation() const
-    {
-        Eigen::Vector3d translation = getEndEffectorTranslation();
-        translation = translation.normalized() * radius;
-        return translation;
-    }
-
 
     Eigen::Matrix3d Joint::getEndEffectorRotation() const
     {
@@ -83,4 +89,9 @@ namespace VirtualRobot::hemisphere
         return jacobian;
     }
 
+    Eigen::Vector2d Joint::angleToPosition(const Eigen::Vector2d& alpha) const
+    {
+        return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0)).array());
+    }
+
 }

VirtualRobot/Nodes/HemisphereJoint/Joint.h

@@ -23,22 +23,28 @@ namespace VirtualRobot::hemisphere
         void setConstants(double lever, double theta0);
 
 
-        void computeFK(double a1, double a2);
+        void computeFkOfAngle(const Eigen::Vector2d& alpha12);
+
+        void computeFkOfPosition(const Eigen::Vector2d& p12);
+        void computeFkOfPosition(double p1, double p2);
 
 
         Eigen::Vector3d getEndEffectorTranslation() const;
-        Eigen::Vector3d getCorrectedEndEffectorTranslation() const;
         Eigen::Matrix3d getEndEffectorRotation() const;
         Eigen::Matrix4d getEndEffectorTransform() const;
         Jacobian getJacobian() const;
 
+        Eigen::Vector2d angleToPosition(const Eigen::Vector2d& alpha) const;
+
 
     public:
 
-        double lever;
-        double theta0;
-        double radius;
-        double actuatorOffset;
+        double lever = 0;
+        double theta0 = 0;
+        double radius = 0;
+
+        double limitLo = 0;
+        double limitHi = 0;
 
 
         Expressions fk;

VirtualRobot/Nodes/RobotNodeHemisphere.cpp

@@ -8,8 +8,8 @@
 
 #include <Eigen/Geometry>
 
-#include <SimoxUtility/math/pose/pose.h>
 #include <SimoxUtility/meta/enum/EnumNames.hpp>
+#include <SimoxUtility/math/pose/pose.h>
 
 
 namespace VirtualRobot
@@ -142,7 +142,7 @@ namespace VirtualRobot
             {
                 std::stringstream ss;
                 ss << "The parent of a hemisphere joint with role '" << RoleNames.to_name(Role::SECOND) << "' "
-                   << "must have be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST) << "' ";
+                   << "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST) << "' ";
                 THROW_VR_EXCEPTION(ss.str());
             }
 
@@ -153,17 +153,11 @@ namespace VirtualRobot
             {
                 const hemisphere::Joint& joint = second->math().joint;
 
-                // (actuator + offset) must be in [-1, 1]
-                // => low: actuator = -1 - offset
-                // => hi: actuator = 1 - offset
-                double lo = -1 - joint.actuatorOffset;
-                double hi = +1 - joint.actuatorOffset;
+                firstNode->jointLimitLo = joint.limitLo;
+                secondNode->jointLimitLo = joint.limitLo;
 
-                firstNode->jointLimitLo = lo;
-                secondNode->jointLimitLo = lo;
-
-                firstNode->jointLimitHi = hi;
-                secondNode->jointLimitHi = hi;
+                firstNode->jointLimitHi = joint.limitHi;
+                secondNode->jointLimitHi = joint.limitHi;
             }
         }
 
@@ -173,21 +167,9 @@ namespace VirtualRobot
 
     void RobotNodeHemisphere::JointMath::update(const Eigen::Vector2f& actuators)
     {
-        const double maxNorm = 1;
-
         if (actuators != this->actuators)
         {
-            Eigen::Vector2f a = actuators;
-            a = a.array() + joint.actuatorOffset;
-
-            double norm = a.norm();
-            if (norm > maxNorm)
-            {
-                a *= (maxNorm / norm);
-            }
-
-            this->actuators = actuators;
-            joint.computeFK(a(0), a(1));
+            joint.computeFkOfAngle(actuators.cast<double>());
         }
     }
 
@@ -210,7 +192,7 @@ namespace VirtualRobot
 
             math.update(actuators);
 
-            Eigen::Vector3d translation = math.joint.getCorrectedEndEffectorTranslation();
+            Eigen::Vector3d translation = math.joint.getEndEffectorTranslation();
             const Eigen::Matrix3d rotation = math.joint.getEndEffectorRotation();
             const Eigen::Matrix4d transform = simox::math::pose(translation, rotation);
 
@@ -223,11 +205,9 @@ namespace VirtualRobot
                           << "\n  lever = " << math.joint.lever
                           << "\n  theta0 = " << math.joint.theta0
                           << "\n  radius = " << math.joint.radius
-                          << "\n  actuator offset = " << math.joint.actuatorOffset
                           << "\n  joint value = " << jointValue
-                          << "\n  joint value offset = " << jointValueOffset
-                          << "\n  actuator = \n" << actuators.transpose().format(iof)
-                          << "\n  actuator + offset = \n" << (actuators.array() + math.joint.actuatorOffset).transpose().format(iof)
+                          << "\n  actuator (angle) = \n" << actuators.transpose().format(iof)
+                          << "\n  actuator (pos) =  \n" << math.joint.angleToPosition(actuators.cast<double>()).transpose().format(iof)
                           << "\n  local transform = \n" << localTransformation.format(iof)
                           << "\n  joint transform = \n" << transform.format(iof)
                           << std::endl;

VirtualRobot/examples/HemisphereJoint/main.cpp

@@ -68,7 +68,7 @@ int main(int argc, char* argv[])
                 const time_point start = std::chrono::system_clock::now();
 
                 VirtualRobot::hemisphere::Joint joint;
-                joint.computeFK(a1, a2);
+                joint.computeFkOfPosition(a1, a2);
 
                 const Eigen::Vector3d pos = joint.getEndEffectorTranslation();
                 const Eigen::Matrix3d ori = joint.getEndEffectorRotation();
@@ -110,7 +110,7 @@ int main(int argc, char* argv[])
         a2 += offset;
 
         VirtualRobot::hemisphere::Joint joint;
-        joint.computeFK(a1, a2);
+        joint.computeFkOfPosition(a1, a2);
 
         const Eigen::Vector3d pos = joint.getEndEffectorTranslation();
         std::cout << "\n  pos = \n" << pos.transpose().format(iof)