diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp index 0d94948af7cc2ddeaaacc8a48afef55d9a3f45e5..2dcd497855156bd54608b87498aa523b85013501 100644 --- a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp +++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp @@ -14,18 +14,22 @@ namespace VirtualRobot::hemisphere Maths::Maths(double lever, double theta0) { - this->setConstants(lever, theta0); + this->setConstants(lever, theta0, std::nullopt, std::nullopt); } void - Maths::setConstants(double lever, double theta0) + Maths::setConstants(double lever, + double theta0, + std::optional<double> limitLo, + std::optional<double> limitHi) { this->lever = lever; this->theta0Rad = theta0; - this->radius = 2 * std::sin(theta0) * lever; + this->radiusOfRollingSpheres = 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(limitHi.has_value() ? limitHi.value() : (45 - 6.0)); + this->limitLo = + simox::math::deg_to_rad(limitLo.has_value() ? limitLo.value() : -(45 - 14.0)); } void diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.h b/VirtualRobot/Nodes/HemisphereJoint/Maths.h index 9928ab0547b4b2b60ab01e6d5098620c37c3d140..362041e23b778503babd5ac1368162625c6a2dfd 100644 --- a/VirtualRobot/Nodes/HemisphereJoint/Maths.h +++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.h @@ -1,5 +1,7 @@ #pragma once +#include <optional> + #include <Eigen/Core> #include "Expressions.h" @@ -26,8 +28,10 @@ namespace VirtualRobot::hemisphere Maths(double lever, double theta0); - void setConstants(double lever, double theta0); - + void setConstants(double lever, + double theta0, + std::optional<double> limitLo, + std::optional<double> limitHi); void computeFkOfAngle(const ActuatorAngle& alpha12); @@ -47,7 +51,7 @@ namespace VirtualRobot::hemisphere public: double lever = 0; double theta0Rad = 0; - double radius = 0; + double radiusOfRollingSpheres = 0; double limitLo = 0; double limitHi = 0; diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp index 2e361f0f1e123feb5c9cb3076d687bcb19db7071..4f1322a02c60d6cb3b27961df4d9ebf161b2cfd4 100644 --- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp +++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp @@ -119,7 +119,8 @@ namespace VirtualRobot { case Role::FIRST: firstData.emplace(FirstData{}); - firstData->maths.maths.setConstants(info.lever, info.theta0Rad); + firstData->maths.maths.setConstants( + info.lever, info.theta0Rad, info.limitLo, info.limitHi); break; case Role::SECOND: @@ -209,7 +210,13 @@ namespace VirtualRobot << __FUNCTION__ << "() of second actuator with " << "\n lever = " << maths.maths.lever << "\n theta0 = " << maths.maths.theta0Rad - << "\n radius = " << maths.maths.radius << "\n joint value = " << jointValue + << "\n theta0 = " << maths.maths.theta0Rad << " rad (" + << simox::math::rad_to_deg(maths.maths.theta0Rad) << " deg)" + << "\n radius of rolling spheres = " << maths.maths.radiusOfRollingSpheres + << "\n joint limit lo = " << getJointLimitLo() << " rad (" + << simox::math::rad_to_deg(getJointLimitLo()) << " deg)" + << "\n joint limit hi = " << getJointLimitHi() << " rad (" + << simox::math::rad_to_deg(getJointLimitHi()) << " deg)" << "\n actuator (angle) = \n" << actuators.transpose().format(iof) << "\n actuator (pos) = \n" << maths.maths.angleToPosition(actuators.cast<double>()).transpose().format(iof) diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h index 65a5f8b3e936cb329c01910a4219710d4f563dd3..323c10c6cbc5d4742e6ec50120b34cf120328e0f 100644 --- a/VirtualRobot/Nodes/RobotNodeHemisphere.h +++ b/VirtualRobot/Nodes/RobotNodeHemisphere.h @@ -68,6 +68,8 @@ namespace VirtualRobot // Only set for first: double theta0Rad = -1; double lever = -1; + std::optional<double> limitLo = std::nullopt; + std::optional<double> limitHi = std::nullopt; }; /// Data held by the first joint. diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp index 4972d281a7dbda03d4be817bf2548e2a889c72bd..3af1c6c9e44d71f68c7b1e51819c693ccfe566ff 100644 --- a/VirtualRobot/XML/RobotIO.cpp +++ b/VirtualRobot/XML/RobotIO.cpp @@ -507,6 +507,14 @@ namespace VirtualRobot hemisphere->lever = getFloatByAttributeName(node, "lever"); hemisphere->theta0Rad = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0")); + if (node->first_attribute("limitLo")) + { + hemisphere->limitLo = getFloatByAttributeName(node, "limitLo"); + } + if (node->first_attribute("limitHi")) + { + hemisphere->limitHi = getFloatByAttributeName(node, "limitHi"); + } break; case RobotNodeHemisphere::Role::SECOND: break;