diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
index c3d7dddace3ca77f088d25f96cf03f0b3c876df4..d12070d6bf600951b4ae632dcda593393905de38 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
@@ -14,12 +14,13 @@
namespace VirtualRobot
{
+ static const float limit = 0.6;
+
VirtualRobot::RobotNodeHemisphere::RobotNodeHemisphere()
{
}
-
RobotNodeHemisphere::RobotNodeHemisphere(
RobotWeakPtr rob,
const std::string& name,
@@ -30,20 +31,29 @@ namespace VirtualRobot
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
float jointValueOffset,
- const SceneObject::Physics& p,
+ const SceneObject::Physics& physics,
CollisionCheckerPtr colChecker,
RobotNodeType type,
- bool isSub
+ bool isTail
) :
- RobotNode(rob, name, jointLimitLo, jointLimitHi, visualization, collisionModel,
- jointValueOffset, p, colChecker, type),
- isSub(isSub)
+ RobotNode(rob, name, -limit, limit, visualization, collisionModel,
+ jointValueOffset, physics, colChecker, type)
{
+ (void) axis;
+ (void) jointLimitLo, (void) jointLimitHi;
+
+ if (isTail)
+ {
+ tail.emplace(Tail{});
+ }
+ else
+ {
+ head.emplace(Head{});
+ }
+
initialized = false;
optionalDHParameter.isSet = false;
- this->localTransformation = preJointTransform;
- this->jointRotationAxis = axis;
- this->jointRotationAxis.normalize();
+ localTransformation = preJointTransform;
checkValidRobotNodeType();
}
@@ -62,8 +72,8 @@ namespace VirtualRobot
RobotNodeType type
) :
RobotNode(rob, name, jointLimitLo, jointLimitHi, visualization, collisionModel,
- jointValueOffset, physics, colChecker, type)
-
+ jointValueOffset, physics, colChecker, type),
+ tail(Tail{})
{
initialized = false;
optionalDHParameter.isSet = true;
@@ -88,43 +98,64 @@ namespace VirtualRobot
RotAlpha(2, 1) = sin(alpha);
RotAlpha(2, 2) = cos(alpha);
- this->localTransformation = RotTheta * TransD * TransA * RotAlpha;
- this->jointRotationAxis = Eigen::Vector3f::UnitZ(); // rotation around z axis
+ localTransformation = RotTheta * TransD * TransA * RotAlpha;
checkValidRobotNodeType();
}
+ RobotNodeHemispherePtr RobotNodeHemisphere::MakeHead(
+ RobotWeakPtr robot, const std::string& name, RobotNodeType type)
+ {
+ bool isTail = false;
+ return std::make_shared<RobotNodeHemisphere>(
+ robot, name, -limit, limit,
+ Eigen::Matrix4f::Identity(), Eigen::Vector3f::Zero(),
+ nullptr, nullptr, 0.0, Physics{}, nullptr, type,
+ isTail);
+ }
+
+
RobotNodeHemisphere::~RobotNodeHemisphere()
= default;
bool RobotNodeHemisphere::initialize(
SceneObjectPtr parent,
- const std::vector<SceneObjectPtr>& children)
+ const std::vector<SceneObjectPtr>& _children)
{
- // Create a sub joint as a child.
- if (not isSub)
+ (void) _children;
+ VR_ASSERT_MESSAGE(head xor sub, head.has_value() << " / " sub.has_value());
+
+ if (tail)
{
- const bool isSub = true;
- RobotNodeHemispherePtr sub = std::make_shared<RobotNodeHemisphere>(
- robot,
- name + "_sub",
- jointLimitLo,
- jointLimitHi,
- localTransformation, // const Eigen::Matrix4f& preJointTransform,
- jointRotationAxis, // const Eigen::Vector3f& axis,
- nullptr, // visualizationModel,
- nullptr, // collisionModel
- jointValueOffset,
- Physics{}, //physics,
- nullptr, // collisionChecker,
- nodeType,
- isSub
- );
- sub->initialize(shared_from_this(), children);
-
- bool success = RobotNode::initialize(parent, {sub});
- return success;
+ if (not tail->head)
+ {
+ // Create a head joint as a child of parent.
+ RobotNodeHemispherePtr headNode = RobotNodeHemisphere::MakeHead(
+ robot, name + "_head", nodeType
+ );
+
+ headNode->setLocalTransformation(this->localTransformation);
+ this->localTransformation.setIdentity();
+
+ // Start: parent -> tail
+ // Goal: parent -> head -> tail
+ SceneObjectPtr tailNode = shared_from_this();
+ parent->detachChild(tailNode);
+ headNode->attachChild(tailNode);
+ parent->attachChild(headNode);
+
+ tail->head = headNode;
+
+ headNode->initialize(parent, {tailNode});
+ // Stop here, recurse when the head node initializes this node (its child).
+ return true;
+ }
+ else
+ {
+ // Recurse.
+ return RobotNode::initialize(parent, children);
+ }
}
else
{
@@ -136,23 +167,51 @@ namespace VirtualRobot
void RobotNodeHemisphere::updateTransformationMatrices(
const Eigen::Matrix4f& parentPose)
{
- const Eigen::Matrix4f rot = simox::math::pose(
- Eigen::AngleAxisf(jointValue + jointValueOffset, jointRotationAxis));
- globalPose = parentPose * localTransformation * rot;
-
- std::cout << __FUNCTION__ << "() with "
- << "joint value = " << jointValue
- << ", joint vaue offset = " << jointValueOffset
- << ", joint rotation axis = " << jointRotationAxis.transpose()
- << ", | joint rotation axis | = " << jointRotationAxis.norm()
- << ", rot matrix: \n" << rot
- << std::endl;
+ VR_ASSERT_MESSAGE(head xor sub, head.has_value() << " / " sub.has_value());
+
+ const double maxNorm = 1;
+
+ if (head)
+ {
+ globalPose = parentPose * localTransformation;
+ }
+ else if (tail)
+ {
+ Eigen::Vector2d a(tail->head->getJointValue(), this->getJointValue());
+ double norm = a.norm();
+ if (norm > maxNorm)
+ {
+ a = a / norm * maxNorm;
+ }
+ tail->joint.computeFK(a(0), a(1));
+
+ Eigen::Vector3d translation = tail->joint.getEndEffectorTranslation();
+ translation = translation.normalized() * tail->joint.radius;
+ const Eigen::Matrix3d rotation = tail->joint.getEndEffectorRotation();
+ const Eigen::Matrix4d transform = simox::math::pose(translation, rotation);
+
+ globalPose = parentPose * localTransformation * transform.cast<float>();
+
+ Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]");
+ std::cout << __FUNCTION__ << "() with "
+ << "\n lever = " << tail->joint.lever
+ << "\n theta0 = " << tail->joint.theta0
+ << "\n radius = " << tail->joint.radius
+ << "\n actuator offset = " << tail->joint.actuatorOffset
+ << "\n joint value = " << jointValue
+ << "\n joint vaue offset = " << jointValueOffset
+ << "\n actuator = \n" << a.transpose().format(iof)
+ << "\n local transform = \n" << localTransformation.format(iof)
+ << "\n transform = \n" << transform.format(iof)
+ << std::endl;
+ }
}
void RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const
{
ReadLockPtr lock = getRobot()->getReadLock();
+ VR_ASSERT_MESSAGE(head xor sub, head.has_value() << " / " sub.has_value());
if (printDecoration)
{
@@ -161,7 +220,15 @@ namespace VirtualRobot
RobotNode::print(false, false);
- std::cout << "* JointRotationAxis: " << jointRotationAxis.transpose() << std::endl;
+ if (head)
+ {
+ std::cout << "* Hemisphere joint head node";
+ }
+ else if (tail)
+ {
+ std::cout << "* Hemisphere joint tail node";
+ std::cout << "* Transform: \n" << tail->joint.getEndEffectorTransform() << std::endl;
+ }
if (printDecoration)
{
@@ -196,46 +263,29 @@ namespace VirtualRobot
}
else
{
- Eigen::Matrix4f lt = getLocalTransformation();
- simox::math::position(lt) *= scaling;
- result.reset(new RobotNodeHemisphere(newRobot, name, jointLimitLo, jointLimitHi, lt, jointRotationAxis, visualizationModel, collisionModel, jointValueOffset, p, colChecker, nodeType));
+ Eigen::Matrix4f localTransform = getLocalTransformation();
+ simox::math::position(localTransform) *= scaling;
+ result.reset(new RobotNodeHemisphere(
+ newRobot, name,
+ jointLimitLo, jointLimitHi,
+ localTransform, Eigen::Vector3f::Zero(),
+ visualizationModel, collisionModel,
+ jointValueOffset, p, colChecker, nodeType));
}
return result;
}
- bool RobotNodeHemisphere::isRotationalJoint() const
+ bool RobotNodeHemisphere::isHemisphereJoint() const
{
return true;
}
-
- Eigen::Vector3f RobotNodeHemisphere::getJointRotationAxisInJointCoordSystem() const
+ void RobotNodeHemisphere::setConstants(double lever, double theta0)
{
- return jointRotationAxis;
- }
-
-
- void RobotNodeHemisphere::setJointRotationAxis(const Eigen::Vector3f& newAxis)
- {
- this->jointRotationAxis = newAxis;
- }
-
-
- Eigen::Vector3f RobotNodeHemisphere::getJointRotationAxis(const SceneObjectPtr coordSystem) const
- {
- ReadLockPtr lock = getRobot()->getReadLock();
- Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
- result4f.segment(0, 3) = jointRotationAxis;
- result4f = globalPose * result4f;
-
- if (coordSystem)
- {
- result4f = coordSystem->getGlobalPose().inverse() * result4f;
- }
-
- return result4f.head<3>();
+ VR_ASSERT(tail);
+ tail->joint.setConstants(lever, theta0);
}
void RobotNodeHemisphere::checkValidRobotNodeType()
@@ -247,23 +297,33 @@ namespace VirtualRobot
std::string RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/)
{
- std::stringstream ss;
- ss << "\t\t<Joint type='Hemisphere'>" << std::endl;
- ss << "\t\t\t<axis x='" << jointRotationAxis[0] << "' y='" << jointRotationAxis[1] << "' z='" << jointRotationAxis[2] << "'/>" << 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())
+ VR_ASSERT_MESSAGE(head xor sub, head.has_value() << " / " sub.has_value());
+
+ if (head)
{
- ss << "\t\t\t<PropagateJointValue name='" << propIt->first << "' factor='" << propIt->second << "'/>" << std::endl;
- propIt++;
+ // Hidden, not part of xml.
+ return "";
}
+ else
+ {
+ std::stringstream ss;
+ ss << "\t\t<Joint type='Hemisphere'>" << std::endl;
+ ss << "\t\t\t<hemisphere lever='" << tail->joint.lever << "' theta0='" << tail->joint.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</Joint>" << std::endl;
- return ss.str();
+ ss << "\t\t</Joint>" << std::endl;
+ return ss.str();
+ }
}
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h
index dee902cc6c09d87fbf890006b557c9ee97f09e1b..1c43209d788324738cfbc82319da0ee568a2e15c 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.h
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.h
@@ -24,6 +24,7 @@
#include "../VirtualRobot.h"
#include "RobotNode.h"
+#include "HemisphereJoint/Joint.h"
#include <Eigen/Core>
@@ -58,7 +59,7 @@ namespace VirtualRobot
const SceneObject::Physics& p = {}, ///< physics information
CollisionCheckerPtr colChecker = nullptr, ///< A collision checker instance (if not set, the global col checker is used)
RobotNodeType type = Generic,
- bool isSub = false ///< Whether this node is a sub node of the top-hemisphere node.
+ bool isTail = true
);
RobotNodeHemisphere(
@@ -78,6 +79,8 @@ namespace VirtualRobot
RobotNodeType type = Generic
);
+ public:
+
~RobotNodeHemisphere() override;
@@ -95,25 +98,9 @@ namespace VirtualRobot
) const override;
bool
- isRotationalJoint() const override;
-
-
- /**
- * Standard: In global coordinate system.
- * \param coordSystem
- * When not set, the axis is transformed to global coordinate system.
- * Otherwise any scene object can be used as coordinate system.
- */
- Eigen::Vector3f
- getJointRotationAxis(const SceneObjectPtr coordSystem = nullptr) const;
-
- /// This is the original joint axis, without any transformations applied.
- Eigen::Vector3f
- getJointRotationAxisInJointCoordSystem() const;
-
- void
- setJointRotationAxis(const Eigen::Vector3f& newAxis);
+ isHemisphereJoint() const override;
+ void setConstants(double lever, double theta0);
/**
* \brief getLMTC Calculates the spatial distance between the parent of a Hemisphere joint
@@ -144,6 +131,14 @@ namespace VirtualRobot
RobotNodeHemisphere();
+ // Head node constructor.
+ static RobotNodeHemispherePtr MakeHead(
+ RobotWeakPtr robot, ///< The robot
+ const std::string& name, ///< The name
+ RobotNodeType type = Generic
+ );
+
+
/// Derived classes add custom XML tags here
std::string
@@ -173,8 +168,19 @@ namespace VirtualRobot
protected:
- bool isSub = false;
- Eigen::Vector3f jointRotationAxis; // (given in local joint coord system)
+ struct Head
+ {
+ };
+ std::optional<Head> head;
+ struct Tail
+ {
+ /// The second actuator node.
+ RobotNodeHemispherePtr head = nullptr;
+
+ /// The joint math.
+ hemisphere::Joint joint;
+ };
+ std::optional<Tail> tail;
};
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp
index 76d332f56e769aee1cb581328dd8dfad5d5c4235..0fade1edc79f45ab2d743d9ed1e9fede66b1946d 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.cpp
@@ -21,11 +21,6 @@ namespace VirtualRobot
= default;
- /**
- * This method creates a VirtualRobot::RobotNodeHemisphere.
- *
- * \return instance of VirtualRobot::RobotNodeHemisphere.
- */
RobotNodePtr RobotNodeHemisphereFactory::createRobotNode(
RobotPtr robot,
const std::string& nodeName,
@@ -57,11 +52,6 @@ namespace VirtualRobot
}
- /**
- * This method creates a VirtualRobot::RobotNodeHemisphere from DH parameters.
- *
- * \return instance of VirtualRobot::RobotNodeHemisphere.
- */
RobotNodePtr RobotNodeHemisphereFactory::createRobotNodeDH(
RobotPtr robot,
const std::string& nodeName,
@@ -93,9 +83,6 @@ namespace VirtualRobot
}
- /**
- * register this class in the super class factory
- */
RobotNodeFactory::SubClassRegistry RobotNodeHemisphereFactory::registry(RobotNodeHemisphereFactory::getName(), &RobotNodeHemisphereFactory::createInstance);
@@ -105,9 +92,6 @@ namespace VirtualRobot
}
- /**
- * \return new instance of RobotNodeHemisphereFactory.
- */
std::shared_ptr<RobotNodeFactory> RobotNodeHemisphereFactory::createInstance(void*)
{
return std::make_shared<RobotNodeHemisphereFactory>();
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.h b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.h
index 627e192e876d4a9bf23779a5ce62363f6997fc02..2433a7f19809438e18cbcea01286a13f0e169287 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphereFactory.h
+++ b/VirtualRobot/Nodes/RobotNodeHemisphereFactory.h
@@ -36,8 +36,43 @@ namespace VirtualRobot
RobotNodeHemisphereFactory();
~RobotNodeHemisphereFactory() override;
- RobotNodePtr createRobotNode(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const Eigen::Matrix4f& preJointTransform, const Eigen::Vector3f& axis, const Eigen::Vector3f& translationDirection, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
- RobotNodePtr createRobotNodeDH(RobotPtr robot, const std::string& nodeName, VisualizationNodePtr visualizationModel, CollisionModelPtr collisionModel, float limitLow, float limitHigh, float jointValueOffset, const DHParameter& dhParameters, const SceneObject::Physics& p = SceneObject::Physics(), RobotNode::RobotNodeType rntype = RobotNode::Generic) const override;
+ /**
+ * Create a VirtualRobot::RobotNodeHemisphere.
+ *
+ * \return instance of VirtualRobot::RobotNodeHemisphere.
+ */
+ RobotNodePtr createRobotNode(
+ RobotPtr robot,
+ const std::string& nodeName,
+ VisualizationNodePtr visualizationModel,
+ CollisionModelPtr collisionModel,
+ float limitLow,
+ float limitHigh,
+ float jointValueOffset,
+ const Eigen::Matrix4f& preJointTransform,
+ const Eigen::Vector3f& axis,
+ const Eigen::Vector3f& translationDirection,
+ const SceneObject::Physics& p = SceneObject::Physics(),
+ RobotNode::RobotNodeType rntype = RobotNode::Generic
+ ) const override;
+
+ /**
+ * Create a VirtualRobot::RobotNodeHemisphere from DH parameters.
+ *
+ * \return instance of VirtualRobot::RobotNodeHemisphere.
+ */
+ RobotNodePtr createRobotNodeDH(
+ RobotPtr robot,
+ const std::string& nodeName,
+ VisualizationNodePtr visualizationModel,
+ CollisionModelPtr collisionModel,
+ float limitLow,
+ float limitHigh,
+ float jointValueOffset,
+ const DHParameter& dhParameters,
+ const SceneObject::Physics& p = SceneObject::Physics(),
+ RobotNode::RobotNodeType rntype = RobotNode::Generic
+ ) const override;
// AbstractFactoryMethod
public:
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 18a4865a35f09f0a2c6aac371f2b06789d29545b..5d95620dc11d027df71806d721a6398a7cd4f69f 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -7,6 +7,7 @@
#include "../EndEffector/EndEffector.h"
#include "../EndEffector/EndEffectorActor.h"
#include "../Nodes/RobotNodeFactory.h"
+#include "../Nodes/RobotNodeHemisphere.h"
#include "../Nodes/RobotNodeFixedFactory.h"
#include "../Nodes/RobotNodePrismatic.h"
#include "../Transformation/DHParameter.h"
@@ -17,11 +18,12 @@
#include "../RuntimeEnvironment.h"
#include "VirtualRobot.h"
#include "rapidxml.hpp"
-#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
#include "mujoco/RobotMjcf.h"
#include <VirtualRobot/Import/URDF/SimoxURDFFactory.h>
+#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
#include <SimoxUtility/filesystem/remove_trailing_separator.h>
+#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <vector>
#include <fstream>
@@ -194,44 +196,43 @@ namespace VirtualRobot
rapidxml::xml_attribute<>* attr;
float jointOffset = 0.0f;
float initialvalue = 0.0f;
- std::string jointType;
-
- RobotNodePtr robotNode;
if (!jointXMLNode)
{
// no <Joint> tag -> fixed joint
+ RobotNodePtr robotNode;
RobotNodeFactoryPtr fixedNodeFactory = RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), nullptr);
-
if (fixedNodeFactory)
{
- robotNode = fixedNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, translationDir, physics, rntype);
+ robotNode = fixedNodeFactory->createRobotNode(
+ robot, robotNodeName, visualizationNode, collisionModel,
+ jointLimitLow, jointLimitHigh, jointOffset,
+ preJointTransform, axis, translationDir,
+ physics, rntype);
}
-
return robotNode;
}
+ std::string jointType;
attr = jointXMLNode->first_attribute("type", 0, false);
-
if (attr)
{
jointType = getLowerCase(attr->value());
}
else
{
- VR_WARNING << "No 'type' attribute for <Joint> tag. Assuming fixed joint for RobotNode " << robotNodeName << "!" << std::endl;
+ VR_WARNING << "No 'type' attribute for <Joint> tag. "
+ << "Assuming fixed joint for RobotNode " << robotNodeName << "!" << std::endl;
jointType = RobotNodeFixedFactory::getName();
}
attr = jointXMLNode->first_attribute("offset", 0, false);
-
if (attr)
{
jointOffset = convertToFloat(attr->value());
}
attr = jointXMLNode->first_attribute("initialvalue", 0, false);
-
if (attr)
{
initialvalue = convertToFloat(attr->value());
@@ -248,14 +249,21 @@ namespace VirtualRobot
bool scaleVisu = false;
Eigen::Vector3f scaleVisuFactor = Eigen::Vector3f::Zero();
+ struct Hemisphere
+ {
+ float lever;
+ float theta0;
+ };
+ std::optional<Hemisphere> hemisphere;
+
while (node)
{
- std::string nodeName = getLowerCase(node->name());
+ const std::string nodeName = getLowerCase(node->name());
if (nodeName == "dh")
{
- THROW_VR_EXCEPTION("DH specification in Joint tag is DEPRECATED! Use <RobotNode><Transform><DH>...</DH></Transform><Joint>...</Joint></RobotNode> structure")
-
+ THROW_VR_EXCEPTION("DH specification in Joint tag is DEPRECATED! "
+ "Use <RobotNode><Transform><DH>...</DH></Transform><Joint>...</Joint></RobotNode> structure.")
//THROW_VR_EXCEPTION_IF(dhXMLNode, "Multiple DH definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
//dhXMLNode = node;
}
@@ -267,7 +275,8 @@ namespace VirtualRobot
}
else if (nodeName == "prejointtransform")
{
- THROW_VR_EXCEPTION("PreJointTransform is DEPRECATED! Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure")
+ THROW_VR_EXCEPTION("PreJointTransform is DEPRECATED! "
+ "Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure")
//THROW_VR_EXCEPTION_IF(prejointTransformNode, "Multiple preJoint definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
//prejointTransformNode = node;
}
@@ -283,7 +292,8 @@ namespace VirtualRobot
}
else if (nodeName == "postjointtransform")
{
- THROW_VR_EXCEPTION("postjointtransform is DEPRECATED and not longer allowed! Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure")
+ THROW_VR_EXCEPTION("postjointtransform is DEPRECATED and not longer allowed! "
+ "Use <RobotNode><Transform>...</Transform><Joint>...</Joint></RobotNode> structure")
//THROW_VR_EXCEPTION_IF(postjointTransformNode, "Multiple postjointtransform definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
//postjointTransformNode = node;
}
@@ -442,6 +452,13 @@ namespace VirtualRobot
scaleVisuFactor[1] = getFloatByAttributeName(node, "y");
scaleVisuFactor[2] = getFloatByAttributeName(node, "z");
}
+ else if (nodeName == "hemisphere")
+ {
+ hemisphere = Hemisphere {
+ .lever = getFloatByAttributeName(node, "lever"),
+ .theta0 = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0")),
+ };
+ }
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in <Joint> tag of RobotNode <" << robotNodeName << ">." << endl);
@@ -463,7 +480,7 @@ namespace VirtualRobot
processTransformNode(prejointTransformNode, robotNodeName, preJointTransform);
processTransformNode(postjointTransformNode, robotNodeName, postJointTransform);
*/
- if (jointType == "revolute" || jointType == "hemisphere")
+ if (jointType == "revolute")
{
if (scaleVisu)
{
@@ -501,14 +518,16 @@ namespace VirtualRobot
if (scaleVisu)
{
THROW_VR_EXCEPTION_IF(scaleVisuFactor.norm() == 0.0f, "Zero scale factor");
-
}
}
+ else if (jointType == "hemisphere")
+ {
+ }
//}
+ RobotNodePtr robotNode;
RobotNodeFactoryPtr robotNodeFactory = RobotNodeFactory::fromName(jointType, nullptr);
-
if (robotNodeFactory)
{
/*if (dh.isSet)
@@ -517,7 +536,12 @@ namespace VirtualRobot
} else
{*/
// create nodes that are not defined via DH parameters
- robotNode = robotNodeFactory->createRobotNode(robot, robotNodeName, visualizationNode, collisionModel, jointLimitLow, jointLimitHigh, jointOffset, preJointTransform, axis, translationDir, physics, rntype);
+ robotNode = robotNodeFactory->createRobotNode(
+ robot, robotNodeName, visualizationNode, collisionModel,
+ jointLimitLow, jointLimitHigh, jointOffset,
+ preJointTransform, axis, translationDir,
+ physics, rntype
+ );
//}
}
else
@@ -532,7 +556,7 @@ namespace VirtualRobot
robotNode->jointValue = initialvalue;
- if (robotNode->isRotationalJoint() || robotNode->isTranslationalJoint())
+ if (robotNode->isJoint())
{
if (robotNode->jointValue < robotNode->jointLimitLo)
{
@@ -543,11 +567,15 @@ namespace VirtualRobot
robotNode->jointValue = robotNode->jointLimitHi;
}
}
+ if (robotNode->isHemisphereJoint() and hemisphere.has_value())
+ {
+ RobotNodeHemispherePtr node = std::dynamic_pointer_cast<RobotNodeHemisphere>(robotNode);
+ node->setConstants(hemisphere->lever, hemisphere->theta0);
+ }
if (scaleVisu)
{
- std::shared_ptr<RobotNodePrismatic> rnPM = std::dynamic_pointer_cast<RobotNodePrismatic>(robotNode);
-
+ RobotNodePrismaticPtr rnPM = std::dynamic_pointer_cast<RobotNodePrismatic>(robotNode);
if (rnPM)
{
rnPM->setVisuScaleFactor(scaleVisuFactor);