From f84e4161e0684ddac5a554a60585d52f5c4a800c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 20 Jun 2023 15:17:14 +0200
Subject: [PATCH] Allow setting joint limits for hemisphere joints
---
VirtualRobot/Nodes/HemisphereJoint/Maths.cpp | 14 +++++++++-----
VirtualRobot/Nodes/HemisphereJoint/Maths.h | 10 +++++++---
VirtualRobot/Nodes/RobotNodeHemisphere.cpp | 11 +++++++++--
VirtualRobot/Nodes/RobotNodeHemisphere.h | 2 ++
VirtualRobot/XML/RobotIO.cpp | 8 ++++++++
5 files changed, 35 insertions(+), 10 deletions(-)
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
index 0d94948af..2dcd49785 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 9928ab054..362041e23 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 2e361f0f1..4f1322a02 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 65a5f8b3e..323c10c6c 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 4972d281a..3af1c6c9e 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;
--
GitLab