diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp index 71594fab1aa5af32feb9f913845b00ba7b19d5db..7b5ce4507fec174e9fd2e129105504a7c6bf3def 100644 --- a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp +++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp @@ -24,7 +24,7 @@ namespace VirtualRobot::hemisphere void Maths::setConstants(double lever, double theta0) { this->lever = lever; - this->theta0 = theta0; + this->theta0Rad = theta0; this->radius = 2 * std::sin(theta0) * lever; this->limitHi = simox::math::deg_to_rad(45 - 6.0); @@ -34,7 +34,7 @@ namespace VirtualRobot::hemisphere void Maths::computeFkOfPosition(double p1, double p2) { - expressions.compute(p1, p2, lever, theta0); + expressions.compute(p1, p2, lever, theta0Rad); } @@ -115,7 +115,7 @@ namespace VirtualRobot::hemisphere Maths::ActuatorPosition Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const { - return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0)).array()); + return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0Rad)).array()); } } diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.h b/VirtualRobot/Nodes/HemisphereJoint/Maths.h index b4c5cfa4613b77f7273afb09732e88889df968fd..76f0188b727a41d6a24c236b29fde0eadaf2ee18 100644 --- a/VirtualRobot/Nodes/HemisphereJoint/Maths.h +++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.h @@ -50,7 +50,7 @@ namespace VirtualRobot::hemisphere public: double lever = 0; - double theta0 = 0; + double theta0Rad = 0; double radius = 0; double limitLo = 0; diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp index 2e6d5231ca8e73c3ef4bbd93a65e7d65cab68693..a2d999867affb1b8adf6f5d79ffc12675ef5607b 100644 --- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp +++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp @@ -10,6 +10,7 @@ #include <SimoxUtility/meta/enum/EnumNames.hpp> #include <SimoxUtility/math/pose/pose.h> +#include <SimoxUtility/math/convert/rad_to_deg.h> namespace VirtualRobot @@ -106,7 +107,7 @@ namespace VirtualRobot { case Role::FIRST: firstData.emplace(FirstData{}); - firstData->maths.maths.setConstants(info.lever, info.theta0); + firstData->maths.maths.setConstants(info.lever, info.theta0Rad); break; case Role::SECOND: @@ -190,7 +191,7 @@ namespace VirtualRobot Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]"); std::cout << __FUNCTION__ << "() of second actuator with " << "\n lever = " << maths.maths.lever - << "\n theta0 = " << maths.maths.theta0 + << "\n theta0 = " << maths.maths.theta0Rad << "\n radius = " << maths.maths.radius << "\n joint value = " << jointValue << "\n actuator (angle) = \n" << actuators.transpose().format(iof) @@ -327,33 +328,34 @@ namespace VirtualRobot VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), std::stringstream() << firstData.has_value() << " / " << secondData.has_value()); + std::stringstream ss; + ss << "\t\t<Joint type='Hemisphere'>" << std::endl; if (firstData) { - // TODO - return ""; + // Constants are defined in first. + + hemisphere::Maths& maths = firstData->maths.maths; + ss << "\t\t\t<hemisphere role='first'" + << " lever='" << maths.lever << "'" + << " theta0='" << simox::math::rad_to_deg(maths.theta0Rad) << "'" + << " />" << std::endl; } else { - hemisphere::CachedMaths& math = secondData->maths(); - - std::stringstream ss; - ss << "\t\t<Joint type='Hemisphere'>" << std::endl; - ss << "\t\t\t<hemisphere lever='" << math.maths.lever << "' theta0='" << math.maths.theta0 << "' />" << std::endl; - ss << "\t\t\t<limits lo='" << jointLimitLo << "' hi='" << jointLimitHi << "' units='radian'/>" << std::endl; - ss << "\t\t\t<MaxAcceleration value='" << maxAcceleration << "'/>" << std::endl; - ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << std::endl; - ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << std::endl; - std::map< std::string, float >::iterator propIt = propagatedJointValues.begin(); - - while (propIt != propagatedJointValues.end()) - { - ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << std::endl; - propIt++; - } + ss << "\t\t\t<hemisphere role='second' />" << std::endl; + } - ss << "\t\t</Joint>" << std::endl; - return ss.str(); + ss << "\t\t\t<MaxAcceleration value='" << maxAcceleration << "'/>" << std::endl; + ss << "\t\t\t<MaxVelocity value='" << maxVelocity << "'/>" << std::endl; + ss << "\t\t\t<MaxTorque value='" << maxTorque << "'/>" << std::endl; + + for (auto propIt = propagatedJointValues.begin(); propIt != propagatedJointValues.end(); ++propIt) + { + ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << std::endl; } + + ss << "\t\t</Joint>" << std::endl; + return ss.str(); } hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths() diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h index e041fad9c52183825dfefb09370b5e2f0e9965be..73c7e8b617fe9fd5be26ae83563fc59df47195ce 100644 --- a/VirtualRobot/Nodes/RobotNodeHemisphere.h +++ b/VirtualRobot/Nodes/RobotNodeHemisphere.h @@ -69,7 +69,7 @@ namespace VirtualRobot Role role; // Only set for first: - double theta0 = -1; + double theta0Rad = -1; double lever = -1; }; diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp index 6b5808efc0744a4f3cb575a71c5901a2cfc3957e..b4f1a4c34c0c067645393b9b4a8efc80ccb0cc76 100644 --- a/VirtualRobot/XML/RobotIO.cpp +++ b/VirtualRobot/XML/RobotIO.cpp @@ -467,7 +467,7 @@ namespace VirtualRobot { case RobotNodeHemisphere::Role::FIRST: hemisphere->lever = getFloatByAttributeName(node, "lever"); - hemisphere->theta0 = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0")); + hemisphere->theta0Rad = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0")); break; case RobotNodeHemisphere::Role::SECOND: break;