diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
index 8ace6dfe80971894e197b8133b4e5cd5e12cecb2..2dcd497855156bd54608b87498aa523b85013501 100644
--- a/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
+++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.cpp
@@ -5,87 +5,80 @@
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/pose/pose.h>
-
namespace VirtualRobot::hemisphere
{
- Maths::Maths() :
- Maths(1, simox::math::deg_to_rad(25.))
+ Maths::Maths() : Maths(1, simox::math::deg_to_rad(25.))
{
}
-
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)
+ void
+ 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 Maths::computeFkOfPosition(double p1, double p2)
+ void
+ Maths::computeFkOfPosition(double p1, double p2)
{
expressions.compute(p1, p2, lever, theta0Rad);
}
-
- void Maths::computeFkOfPosition(const ActuatorPosition& p12)
+ void
+ Maths::computeFkOfPosition(const ActuatorPosition& p12)
{
computeFkOfPosition(p12(0), p12(1));
}
-
- void Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
+ void
+ Maths::computeFkOfAngle(const ActuatorAngle& alpha12)
{
computeFkOfPosition(angleToPosition(alpha12));
}
-
- Eigen::Vector3d Maths::getEndEffectorTranslation() const
+ Eigen::Vector3d
+ Maths::getEndEffectorTranslation() const
{
- return Eigen::Vector3d {
- expressions.ex,
- expressions.ey,
- expressions.ez
- };
+ return Eigen::Vector3d{expressions.ex, expressions.ey, expressions.ez};
}
-
- Eigen::Matrix3d Maths::getEndEffectorRotation() const
+ Eigen::Matrix3d
+ Maths::getEndEffectorRotation() const
{
// r_wrist_to_base = np.array([[exx, eyx, ezx], [exy, eyy, ezy], [exz, eyz, ezz]])
Eigen::Matrix3d ori;
- ori << expressions.exx, expressions.eyx, expressions.ezx,
- expressions.exy, expressions.eyy, expressions.ezy,
- expressions.exz, expressions.eyz, expressions.ezz;
+ ori << expressions.exx, expressions.eyx, expressions.ezx, expressions.exy, expressions.eyy,
+ expressions.ezy, expressions.exz, expressions.eyz, expressions.ezz;
return ori;
}
-
- Eigen::Matrix4d Maths::getEndEffectorTransform() const
+ Eigen::Matrix4d
+ Maths::getEndEffectorTransform() const
{
return simox::math::pose(getEndEffectorTranslation(), getEndEffectorRotation());
}
-
- Maths::Jacobian Maths::getJacobian() const
+ Maths::Jacobian
+ Maths::getJacobian() const
{
Maths::Jacobian jacobian;
- jacobian << expressions.jx1, expressions.jx2,
- expressions.jy1, expressions.jy2,
- expressions.jz1, expressions.jz2,
- expressions.jrx1, expressions.jrx2,
- expressions.jry1, expressions.jry2,
- expressions.jrz1, expressions.jrz2;
+ jacobian << expressions.jx1, expressions.jx2, expressions.jy1, expressions.jy2,
+ expressions.jz1, expressions.jz2, expressions.jrx1, expressions.jrx2, expressions.jry1,
+ expressions.jry2, expressions.jrz1, expressions.jrz2;
// Current state of constructing the orientational part.
// ToDo: Do this with symbolic math inside `Expressions`.
@@ -95,7 +88,7 @@ namespace VirtualRobot::hemisphere
for (int column = 0; column < 2; ++column)
{
// Imagine we apply (+1, 0) / (0, +1) actuator velocities.
- const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0
+ const Eigen::Vector3d eefVelTrans = jacobian.block<3, 1>(0, column); // * 1.0
/*
* The rotation axis is orthogonal to the vector from origin to the
@@ -103,20 +96,20 @@ namespace VirtualRobot::hemisphere
*
* For the scaling, ask Cornelius. :)
*/
- const Eigen::Vector3d scaledRotAxis = eefStateTrans.cross(eefVelTrans)
- / eefStateTrans.norm() * 2;
+ const Eigen::Vector3d scaledRotAxis =
+ eefStateTrans.cross(eefVelTrans) / eefStateTrans.norm() * 2;
- jacobian.block<3, 1>(3, column) = scaledRotAxis; // / 1.0;
+ jacobian.block<3, 1>(3, column) = scaledRotAxis; // / 1.0;
}
}
return jacobian;
}
-
- Maths::ActuatorPosition Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
+ Maths::ActuatorPosition
+ Maths::angleToPosition(const Maths::ActuatorAngle& alpha) const
{
return lever * Eigen::sin((alpha + Eigen::Vector2d::Constant(theta0Rad)).array());
}
-}
+} // namespace VirtualRobot::hemisphere
diff --git a/VirtualRobot/Nodes/HemisphereJoint/Maths.h b/VirtualRobot/Nodes/HemisphereJoint/Maths.h
index 76f0188b727a41d6a24c236b29fde0eadaf2ee18..362041e23b778503babd5ac1368162625c6a2dfd 100644
--- a/VirtualRobot/Nodes/HemisphereJoint/Maths.h
+++ b/VirtualRobot/Nodes/HemisphereJoint/Maths.h
@@ -1,10 +1,11 @@
#pragma once
+#include <optional>
+
#include <Eigen/Core>
#include "Expressions.h"
-
namespace VirtualRobot::hemisphere
{
@@ -18,19 +19,19 @@ namespace VirtualRobot::hemisphere
class Maths
{
public:
-
using ActuatorPosition = Eigen::Vector2d;
using ActuatorAngle = Eigen::Vector2d;
using Jacobian = Eigen::Matrix<double, 6, 2>;
public:
-
Maths();
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);
@@ -48,17 +49,15 @@ namespace VirtualRobot::hemisphere
public:
-
double lever = 0;
double theta0Rad = 0;
- double radius = 0;
+ double radiusOfRollingSpheres = 0;
double limitLo = 0;
double limitHi = 0;
Expressions expressions;
-
};
-}
+} // namespace VirtualRobot::hemisphere
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
index d472654467060a64d9de011afd00050be2575c48..4f1322a02c60d6cb3b27961df4d9ebf161b2cfd4 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
@@ -1,51 +1,57 @@
#include "RobotNodeHemisphere.h"
-#include "Nodes/Sensor.h"
-#include "Robot.h"
-#include "VirtualRobotException.h"
#include <algorithm>
#include <cmath>
#include <Eigen/Geometry>
-#include <SimoxUtility/meta/enum/EnumNames.hpp>
-#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/convert/rad_to_deg.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+#include "Nodes/Sensor.h"
+#include "Robot.h"
+#include "VirtualRobotException.h"
namespace VirtualRobot
{
static const float initialLimit = 1.0;
- extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames =
- {
- { RobotNodeHemisphere::Role::FIRST, "first" },
- { RobotNodeHemisphere::Role::SECOND, "second" },
+ extern const simox::meta::EnumNames<RobotNodeHemisphere::Role> RoleNames = {
+ {RobotNodeHemisphere::Role::FIRST, "first"},
+ {RobotNodeHemisphere::Role::SECOND, "second"},
};
VirtualRobot::RobotNodeHemisphere::RobotNodeHemisphere()
{
}
- RobotNodeHemisphere::Role RobotNodeHemisphere::RoleFromString(const std::string& string)
+ RobotNodeHemisphere::Role
+ RobotNodeHemisphere::RoleFromString(const std::string& string)
{
return RoleNames.from_name(string);
}
- RobotNodeHemisphere::RobotNodeHemisphere(
- RobotWeakPtr rob,
- const std::string& name,
- const Eigen::Matrix4f& preJointTransform,
- VisualizationNodePtr visualization,
- CollisionModelPtr collisionModel,
- float jointValueOffset,
- const SceneObject::Physics& physics,
- CollisionCheckerPtr colChecker,
- RobotNodeType type
- ) :
- RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel,
- jointValueOffset, physics, colChecker, type)
+ RobotNodeHemisphere::RobotNodeHemisphere(RobotWeakPtr rob,
+ const std::string& name,
+ const Eigen::Matrix4f& preJointTransform,
+ VisualizationNodePtr visualization,
+ CollisionModelPtr collisionModel,
+ float jointValueOffset,
+ const SceneObject::Physics& physics,
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type) :
+ RobotNode(rob,
+ name,
+ -initialLimit,
+ initialLimit,
+ visualization,
+ collisionModel,
+ jointValueOffset,
+ physics,
+ colChecker,
+ type)
{
initialized = false;
optionalDHParameter.isSet = false;
@@ -53,20 +59,28 @@ namespace VirtualRobot
checkValidRobotNodeType();
}
-
- RobotNodeHemisphere::RobotNodeHemisphere(
- RobotWeakPtr rob,
- const std::string& name,
- float a, float d, float alpha, float theta,
- VisualizationNodePtr visualization,
- CollisionModelPtr collisionModel,
- float jointValueOffset,
- const SceneObject::Physics& physics,
- CollisionCheckerPtr colChecker,
- RobotNodeType type
- ) :
- RobotNode(rob, name, -initialLimit, initialLimit, visualization, collisionModel,
- jointValueOffset, physics, colChecker, type)
+ RobotNodeHemisphere::RobotNodeHemisphere(RobotWeakPtr rob,
+ const std::string& name,
+ float a,
+ float d,
+ float alpha,
+ float theta,
+ VisualizationNodePtr visualization,
+ CollisionModelPtr collisionModel,
+ float jointValueOffset,
+ const SceneObject::Physics& physics,
+ CollisionCheckerPtr colChecker,
+ RobotNodeType type) :
+ RobotNode(rob,
+ name,
+ -initialLimit,
+ initialLimit,
+ visualization,
+ collisionModel,
+ jointValueOffset,
+ physics,
+ colChecker,
+ type)
{
initialized = false;
optionalDHParameter.isSet = true;
@@ -95,34 +109,33 @@ namespace VirtualRobot
checkValidRobotNodeType();
}
+ RobotNodeHemisphere::~RobotNodeHemisphere() = default;
- RobotNodeHemisphere::~RobotNodeHemisphere()
- = default;
-
-
- void RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
+ void
+ RobotNodeHemisphere::setXmlInfo(const XmlInfo& info)
{
VR_ASSERT(secondData.has_value());
switch (info.role)
{
- case Role::FIRST:
- firstData.emplace(FirstData{});
- firstData->maths.maths.setConstants(info.lever, info.theta0Rad);
- break;
-
- case Role::SECOND:
- secondData.emplace(SecondData{});
- break;
+ case Role::FIRST:
+ firstData.emplace(FirstData{});
+ firstData->maths.maths.setConstants(
+ info.lever, info.theta0Rad, info.limitLo, info.limitHi);
+ break;
+
+ case Role::SECOND:
+ secondData.emplace(SecondData{});
+ break;
}
}
-
- bool RobotNodeHemisphere::initialize(
- SceneObjectPtr parent,
- const std::vector<SceneObjectPtr>& children)
+ bool
+ RobotNodeHemisphere::initialize(SceneObjectPtr parent,
+ const std::vector<SceneObjectPtr>& children)
{
VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
- std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
+ std::stringstream()
+ << firstData.has_value() << " / " << secondData.has_value());
// The second node needs to store a reference to the first node.
if (secondData)
@@ -132,11 +145,13 @@ namespace VirtualRobot
RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get());
RobotNodeHemisphere* secondNode = this;
- if (not (firstNode and firstNode->firstData))
+ if (not(firstNode and firstNode->firstData))
{
std::stringstream ss;
- ss << "The parent of a hemisphere joint with role '" << RoleNames.to_name(Role::SECOND) << "' "
- << "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST) << "' ";
+ ss << "The parent of a hemisphere joint with role '"
+ << RoleNames.to_name(Role::SECOND) << "' "
+ << "must be a hemisphere joint with role '" << RoleNames.to_name(Role::FIRST)
+ << "' ";
THROW_VR_EXCEPTION(ss.str());
}
@@ -159,11 +174,12 @@ namespace VirtualRobot
return RobotNode::initialize(parent, children);
}
-
- void RobotNodeHemisphere::updateTransformationMatrices(
- const Eigen::Matrix4f& parentPose)
+ void
+ RobotNodeHemisphere::updateTransformationMatrices(const Eigen::Matrix4f& parentPose)
{
- VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
+ VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
+ std::stringstream()
+ << firstData.has_value() << " / " << secondData.has_value());
if (firstData)
{
@@ -174,7 +190,8 @@ namespace VirtualRobot
VR_ASSERT_MESSAGE(secondData->firstNode, "First node must be known to second node.");
hemisphere::CachedMaths& maths = secondData->maths();
- Eigen::Vector2f actuators(secondData->firstNode->getJointValue(), this->getJointValue());
+ Eigen::Vector2f actuators(secondData->firstNode->getJointValue(),
+ this->getJointValue());
maths.update(actuators);
@@ -189,25 +206,34 @@ namespace VirtualRobot
if (verbose)
{
Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]");
- std::cout << __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 actuator (angle) = \n" << actuators.transpose().format(iof)
- << "\n actuator (pos) = \n" << maths.maths.angleToPosition(actuators.cast<double>()).transpose().format(iof)
- << "\n local transform = \n" << localTransformation.format(iof)
- << "\n joint transform = \n" << transform.format(iof)
- << std::endl;
+ std::cout
+ << __FUNCTION__ << "() of second actuator with "
+ << "\n lever = " << maths.maths.lever
+ << "\n theta0 = " << maths.maths.theta0Rad
+ << "\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)
+ << "\n local transform = \n"
+ << localTransformation.format(iof) << "\n joint transform = \n"
+ << transform.format(iof) << std::endl;
}
}
}
-
- void RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const
+ void
+ RobotNodeHemisphere::print(bool printChildren, bool printDecoration) const
{
ReadLockPtr lock = getRobot()->getReadLock();
- VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(), std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
+ VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
+ std::stringstream()
+ << firstData.has_value() << " / " << secondData.has_value());
if (printDecoration)
{
@@ -223,7 +249,8 @@ namespace VirtualRobot
else if (secondData)
{
std::cout << "* Hemisphere joint second node";
- std::cout << "* Transform: \n" << secondData->maths().maths.getEndEffectorTransform() << std::endl;
+ std::cout << "* Transform: \n"
+ << secondData->maths().maths.getEndEffectorTransform() << std::endl;
}
if (printDecoration)
@@ -240,13 +267,12 @@ namespace VirtualRobot
}
}
-
- RobotNodePtr RobotNodeHemisphere::_clone(
- const RobotPtr newRobot,
- const VisualizationNodePtr visualizationModel,
- const CollisionModelPtr collisionModel,
- CollisionCheckerPtr colChecker,
- float scaling)
+ RobotNodePtr
+ RobotNodeHemisphere::_clone(const RobotPtr newRobot,
+ const VisualizationNodePtr visualizationModel,
+ const CollisionModelPtr collisionModel,
+ CollisionCheckerPtr colChecker,
+ float scaling)
{
ReadLockPtr lock = getRobot()->getReadLock();
Physics physics = this->physics.scale(scaling);
@@ -254,24 +280,32 @@ namespace VirtualRobot
RobotNodeHemispherePtr cloned;
if (optionalDHParameter.isSet)
{
- cloned.reset(new RobotNodeHemisphere(
- newRobot, name,
- optionalDHParameter.aMM() * scaling,
- optionalDHParameter.dMM() * scaling,
- optionalDHParameter.alphaRadian(),
- optionalDHParameter.thetaRadian(),
- visualizationModel, collisionModel,
- jointValueOffset,
- physics, colChecker, nodeType));
+ cloned.reset(new RobotNodeHemisphere(newRobot,
+ name,
+ optionalDHParameter.aMM() * scaling,
+ optionalDHParameter.dMM() * scaling,
+ optionalDHParameter.alphaRadian(),
+ optionalDHParameter.thetaRadian(),
+ visualizationModel,
+ collisionModel,
+ jointValueOffset,
+ physics,
+ colChecker,
+ nodeType));
}
else
{
Eigen::Matrix4f localTransform = getLocalTransformation();
simox::math::position(localTransform) *= scaling;
- cloned.reset(new RobotNodeHemisphere(
- newRobot, name, localTransform,
- visualizationModel, collisionModel,
- jointValueOffset, physics, colChecker, nodeType));
+ cloned.reset(new RobotNodeHemisphere(newRobot,
+ name,
+ localTransform,
+ visualizationModel,
+ collisionModel,
+ jointValueOffset,
+ physics,
+ colChecker,
+ nodeType));
}
if (this->firstData)
@@ -288,45 +322,52 @@ namespace VirtualRobot
return cloned;
}
-
- bool RobotNodeHemisphere::isHemisphereJoint() const
+ bool
+ RobotNodeHemisphere::isHemisphereJoint() const
{
return true;
}
- bool RobotNodeHemisphere::isFirstHemisphereJointNode() const
+ bool
+ RobotNodeHemisphere::isFirstHemisphereJointNode() const
{
return firstData.has_value();
}
- bool RobotNodeHemisphere::isSecondHemisphereJointNode() const
+ bool
+ RobotNodeHemisphere::isSecondHemisphereJointNode() const
{
return secondData.has_value();
}
- const RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData() const
+ const RobotNodeHemisphere::SecondData&
+ RobotNodeHemisphere::getSecondData() const
{
VR_ASSERT(secondData.has_value());
return secondData.value();
}
- RobotNodeHemisphere::SecondData& RobotNodeHemisphere::getSecondData()
+ RobotNodeHemisphere::SecondData&
+ RobotNodeHemisphere::getSecondData()
{
VR_ASSERT(secondData.has_value());
return secondData.value();
}
- void RobotNodeHemisphere::checkValidRobotNodeType()
+ void
+ RobotNodeHemisphere::checkValidRobotNodeType()
{
RobotNode::checkValidRobotNodeType();
- THROW_VR_EXCEPTION_IF(nodeType == Body || nodeType == Transform, "RobotNodeHemisphere must be a JointNode or a GenericNode");
+ THROW_VR_EXCEPTION_IF(nodeType == Body || nodeType == Transform,
+ "RobotNodeHemisphere must be a JointNode or a GenericNode");
}
-
- std::string RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/)
+ std::string
+ RobotNodeHemisphere::_toXML(const std::string& /*modelPath*/)
{
VR_ASSERT_MESSAGE(firstData.has_value() xor secondData.has_value(),
- std::stringstream() << firstData.has_value() << " / " << secondData.has_value());
+ std::stringstream()
+ << firstData.has_value() << " / " << secondData.has_value());
std::stringstream ss;
ss << "\t\t<Joint type='Hemisphere'>" << std::endl;
@@ -349,26 +390,31 @@ namespace VirtualRobot
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)
+ 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\t<PropagateJointValue name='" << propIt->first << "' factor='"
+ << propIt->second << "'/>" << std::endl;
}
ss << "\t\t</Joint>" << std::endl;
return ss.str();
}
- hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths()
+ hemisphere::CachedMaths&
+ RobotNodeHemisphere::SecondData::maths()
{
return firstNode->firstData->maths;
}
- const hemisphere::CachedMaths& RobotNodeHemisphere::SecondData::maths() const
+ const hemisphere::CachedMaths&
+ RobotNodeHemisphere::SecondData::maths() const
{
return firstNode->firstData->maths;
}
- hemisphere::Maths::Jacobian RobotNodeHemisphere::SecondData::getJacobian()
+ hemisphere::Maths::Jacobian
+ RobotNodeHemisphere::SecondData::getJacobian()
{
VR_ASSERT(firstNode);
VR_ASSERT(secondNode);
@@ -382,4 +428,4 @@ namespace VirtualRobot
return jacobian;
}
-}
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h
index 35e05fd4488d6b3d7bc8c1f08bd408bfc9e50340..323c10c6cbc5d4742e6ec50120b34cf120328e0f 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.h
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.h
@@ -21,18 +21,16 @@
*/
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h>
-#include <VirtualRobot/Nodes/HemisphereJoint/Maths.h>
-
-#include <Eigen/Core>
-
+#include <optional>
#include <string>
#include <vector>
-#include <optional>
+#include <Eigen/Core>
+
+#include <VirtualRobot/Nodes/HemisphereJoint/CachedMaths.h>
+#include <VirtualRobot/Nodes/HemisphereJoint/Maths.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/VirtualRobot.h>
namespace VirtualRobot
{
@@ -53,7 +51,6 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotNodeHemisphere : public RobotNode
{
public:
-
enum class Role
{
/// The first DoF in the kinematic chain.
@@ -71,9 +68,10 @@ 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.
struct FirstData
{
@@ -97,60 +95,53 @@ namespace VirtualRobot
public:
-
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
RobotNodeHemisphere(
- RobotWeakPtr rob, ///< The robot
- const std::string& name, ///< The name
- const Eigen::Matrix4f& preJointTransform, ///< This transformation is applied before the translation of the joint is done
- VisualizationNodePtr visualization = nullptr, ///< A visualization model
- CollisionModelPtr collisionModel = nullptr, ///< A collision model
- float jointValueOffset = 0.0f, ///< An offset that is internally added to the joint value
- 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
- );
+ RobotWeakPtr rob, ///< The robot
+ const std::string& name, ///< The name
+ const Eigen::Matrix4f&
+ preJointTransform, ///< This transformation is applied before the translation of the joint is done
+ VisualizationNodePtr visualization = nullptr, ///< A visualization model
+ CollisionModelPtr collisionModel = nullptr, ///< A collision model
+ float jointValueOffset =
+ 0.0f, ///< An offset that is internally added to the joint value
+ 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);
// The DH-based constructor is not tested so far for Hemisphere joints.
RobotNodeHemisphere(
- RobotWeakPtr rob, ///< The robot
- const std::string& name, ///< The name
- float a, ///< dh paramters
- float d, ///< dh paramters
- float alpha, ///< dh paramters
- float theta, ///< dh paramters
- VisualizationNodePtr visualization = nullptr, ///< A visualization model
- CollisionModelPtr collisionModel = nullptr, ///< A collision model
- float jointValueOffset = 0.0f, ///< An offset that is internally added to the joint value
- const SceneObject::Physics& p = {}, ///< physics information
- CollisionCheckerPtr colChecker = {}, ///< A collision checker instance (if not set, the global col checker is used)
- RobotNodeType type = Generic
- );
+ RobotWeakPtr rob, ///< The robot
+ const std::string& name, ///< The name
+ float a, ///< dh paramters
+ float d, ///< dh paramters
+ float alpha, ///< dh paramters
+ float theta, ///< dh paramters
+ VisualizationNodePtr visualization = nullptr, ///< A visualization model
+ CollisionModelPtr collisionModel = nullptr, ///< A collision model
+ float jointValueOffset =
+ 0.0f, ///< An offset that is internally added to the joint value
+ const SceneObject::Physics& p = {}, ///< physics information
+ CollisionCheckerPtr colChecker =
+ {}, ///< A collision checker instance (if not set, the global col checker is used)
+ RobotNodeType type = Generic);
public:
-
~RobotNodeHemisphere() override;
void setXmlInfo(const XmlInfo& info);
- bool
- initialize(
- SceneObjectPtr parent = nullptr,
- const std::vector<SceneObjectPtr>& children = {}
- ) override;
+ bool initialize(SceneObjectPtr parent = nullptr,
+ const std::vector<SceneObjectPtr>& children = {}) override;
/// Print status information.
- void
- print(
- bool printChildren = false,
- bool printDecoration = true
- ) const override;
+ void print(bool printChildren = false, bool printDecoration = true) const override;
- bool
- isHemisphereJoint() const override;
+ bool isHemisphereJoint() const override;
bool isFirstHemisphereJointNode() const;
bool isSecondHemisphereJointNode() const;
@@ -163,39 +154,27 @@ namespace VirtualRobot
const SecondData& getSecondData() const;
protected:
-
RobotNodeHemisphere();
/// Derived classes add custom XML tags here
- std::string
- _toXML(
- const std::string& modelPath
- ) override;
+ std::string _toXML(const std::string& modelPath) override;
/// Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown.
/// Called on initialization.
- void
- checkValidRobotNodeType() override;
+ void checkValidRobotNodeType() override;
- void
- updateTransformationMatrices(
- const Eigen::Matrix4f& parentPose
- ) override;
+ void updateTransformationMatrices(const Eigen::Matrix4f& parentPose) override;
- RobotNodePtr
- _clone(const RobotPtr newRobot,
- const VisualizationNodePtr visualizationModel,
- const CollisionModelPtr collisionModel,
- CollisionCheckerPtr colChecker,
- float scaling
- ) override;
+ RobotNodePtr _clone(const RobotPtr newRobot,
+ const VisualizationNodePtr visualizationModel,
+ const CollisionModelPtr collisionModel,
+ CollisionCheckerPtr colChecker,
+ float scaling) override;
private:
-
std::optional<FirstData> firstData;
std::optional<SecondData> secondData;
-
};
} // namespace VirtualRobot
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index f7b339b6a9c71a7855ddfc7c0e2d51a6648bff55..9b075c451f946e7f0ee6f65b9fdbbb1455762f97 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -754,7 +754,7 @@ namespace VirtualRobot
return nullptr;
};
- const auto processSegmentNode = [&segmentsNode, &findSegmentNode](const std::string& type)
+ const auto processSegmentNode = [&findSegmentNode](const std::string& type)
{
// find the segment node with the correct type
const auto* node = findSegmentNode(type);
@@ -766,7 +766,7 @@ namespace VirtualRobot
const std::string name = processStringAttribute("name", node, true);
Eigen::Matrix4f transform;
- processTransformNode(segmentsNode->first_node("transform", 0, false), "transform", transform);
+ processTransformNode(node->first_node("transform", 0, false), "transform", transform);
return HumanMapping::ArmDescription::Segment
{
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 7cdf397b9462917fe76f43b1df4f5759bb15850b..3af1c6c9e44d71f68c7b1e51819c693ccfe566ff 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -1,35 +1,37 @@
#include "RobotIO.h"
-#include "../RobotFactory.h"
-#include "../RobotNodeSet.h"
-#include "../VirtualRobotException.h"
+
+#include <fstream>
+#include <iostream>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <SimoxUtility/filesystem/remove_trailing_separator.h>
+#include <SimoxUtility/math/convert/deg_to_rad.h>
+#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
+#include <VirtualRobot/Import/URDF/SimoxURDFFactory.h>
+#include <VirtualRobot/Nodes/FourBar/Joint.h>
+
#include "../CollisionDetection/CollisionModel.h"
#include "../EndEffector/EndEffector.h"
#include "../EndEffector/EndEffectorActor.h"
#include "../Nodes/RobotNodeFactory.h"
-#include "../Nodes/RobotNodeHemisphere.h"
#include "../Nodes/RobotNodeFixedFactory.h"
+#include "../Nodes/RobotNodeHemisphere.h"
#include "../Nodes/RobotNodePrismatic.h"
+#include "../RobotConfig.h"
+#include "../RobotFactory.h"
+#include "../RobotNodeSet.h"
+#include "../RuntimeEnvironment.h"
#include "../Transformation/DHParameter.h"
+#include "../VirtualRobotException.h"
+#include "../Visualization/TriMeshModel.h"
#include "../Visualization/VisualizationFactory.h"
#include "../Visualization/VisualizationNode.h"
-#include "../Visualization/TriMeshModel.h"
-#include "../RobotConfig.h"
-#include "../RuntimeEnvironment.h"
#include "Nodes/RobotNodeFourBar.h"
#include "VirtualRobot.h"
-#include "rapidxml.hpp"
#include "mujoco/RobotMjcf.h"
-#include <VirtualRobot/Import/URDF/SimoxURDFFactory.h>
-#include <VirtualRobot/Nodes/FourBar/Joint.h>
-#include <SimoxUtility/xml/rapidxml/rapidxml_print.hpp>
-#include <SimoxUtility/filesystem/remove_trailing_separator.h>
-#include <SimoxUtility/math/convert/deg_to_rad.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <vector>
-#include <fstream>
-#include <iostream>
+#include "rapidxml.hpp"
namespace VirtualRobot
{
@@ -37,15 +39,13 @@ namespace VirtualRobot
std::map<std::string, int> RobotIO::robot_name_counter;
- RobotIO::RobotIO()
- = default;
-
- RobotIO::~RobotIO()
- = default;
-
+ RobotIO::RobotIO() = default;
+ RobotIO::~RobotIO() = default;
- void RobotIO::processChildNode(rapidxml::xml_node<char>* childXMLNode, std::vector<std::string>& childrenNames)
+ void
+ RobotIO::processChildNode(rapidxml::xml_node<char>* childXMLNode,
+ std::vector<std::string>& childrenNames)
{
THROW_VR_EXCEPTION_IF(!childXMLNode, "NULL data for childXMLNode in processChildNode()")
@@ -57,11 +57,15 @@ namespace VirtualRobot
childrenNames.push_back(s);
}
- void RobotIO::processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode, const std::string& nodeName, std::vector< ChildFromRobotDef >& childrenFromRobot)
+ void
+ RobotIO::processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode,
+ const std::string& nodeName,
+ std::vector<ChildFromRobotDef>& childrenFromRobot)
{
rapidxml::xml_attribute<>* attr;
- THROW_VR_EXCEPTION_IF(!childXMLNode, "NULL data for childXMLNode in processChildFromRobotNode()")
+ THROW_VR_EXCEPTION_IF(!childXMLNode,
+ "NULL data for childXMLNode in processChildFromRobotNode()")
ChildFromRobotDef d;
@@ -87,10 +91,11 @@ namespace VirtualRobot
fileXMLNode = fileXMLNode->next_sibling("file", 0, false);
}
- THROW_VR_EXCEPTION_IF(((!counter) == 0), "Missing file for <ChildFromRobot> tag (in node '" << nodeName << "')." << endl);
+ THROW_VR_EXCEPTION_IF(((!counter) == 0),
+ "Missing file for <ChildFromRobot> tag (in node '" << nodeName
+ << "')." << endl);
}
-
/**
* This method processes Limits tags.
* The values for the attributes "lo" and "hi" are extracted based on the
@@ -98,7 +103,11 @@ namespace VirtualRobot
*
* The values are stored in \p jointLimitLo and \p jointLimitHi
*/
- void RobotIO::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool& limitless)
+ void
+ RobotIO::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode,
+ float& jointLimitLo,
+ float& jointLimitHi,
+ bool& limitless)
{
THROW_VR_EXCEPTION_IF(!limitsXMLNode, "NULL data for limitsXMLNode in processLimitsNode()");
@@ -112,13 +121,15 @@ namespace VirtualRobot
{
if (unit.isLength())
{
- VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -1000 [mm]." << std::endl;
+ VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -1000 [mm]."
+ << std::endl;
jointLimitLo = -1000.0f;
unit = Units("mm");
}
else
{
- VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -180 [deg]." << std::endl;
+ VR_WARNING << "No 'lo' attribute in <Limits> tag. Assuming -180 [deg]."
+ << std::endl;
jointLimitLo = float(-M_PI);
unit = Units("rad");
}
@@ -164,36 +175,39 @@ namespace VirtualRobot
if (llAttr != nullptr)
{
limitless = isTrue(llAttr->value());
- if (limitless && unit.isAngle() && (unit.toDegree(jointLimitHi) - unit.toDegree(jointLimitLo) != 360))
+ if (limitless && unit.isAngle() &&
+ (unit.toDegree(jointLimitHi) - unit.toDegree(jointLimitLo) != 360))
{
- VR_WARNING << "Limitless Joint: Angular distance between 'lo' and 'hi' attributes should equal 2*pi [rad] or 360 [deg]." << endl
+ VR_WARNING << "Limitless Joint: Angular distance between 'lo' and 'hi' attributes "
+ "should equal 2*pi [rad] or 360 [deg]."
+ << endl
<< "Setting 'lo' to -pi and 'hi' to pi [rad]..." << std::endl;
jointLimitLo = float(-M_PI);
jointLimitHi = float(M_PI);
}
}
-
}
- RobotNodePtr RobotIO::processJointNode(rapidxml::xml_node<char>* jointXMLNode, const std::string& robotNodeName,
- RobotPtr robot,
- VisualizationNodePtr visualizationNode,
- CollisionModelPtr collisionModel,
- SceneObject::Physics& physics,
- RobotNode::RobotNodeType rntype,
- Eigen::Matrix4f& transformationMatrix
- )
+ RobotNodePtr
+ RobotIO::processJointNode(rapidxml::xml_node<char>* jointXMLNode,
+ const std::string& robotNodeName,
+ RobotPtr robot,
+ VisualizationNodePtr visualizationNode,
+ CollisionModelPtr collisionModel,
+ SceneObject::Physics& physics,
+ RobotNode::RobotNodeType rntype,
+ Eigen::Matrix4f& transformationMatrix)
{
- float jointLimitLow = float(- M_PI);
+ float jointLimitLow = float(-M_PI);
float jointLimitHigh = float(M_PI);
bool limitless = false;
- Eigen::Matrix4f preJointTransform = transformationMatrix;//Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f preJointTransform = transformationMatrix; //Eigen::Matrix4f::Identity();
Eigen::Vector3f axis = Eigen::Vector3f::Zero();
Eigen::Vector3f translationDir = Eigen::Vector3f::Zero();
- std::vector< std::string > propagateJVName;
- std::vector< float > propagateJVFactor;
+ std::vector<std::string> propagateJVName;
+ std::vector<float> propagateJVFactor;
rapidxml::xml_attribute<>* attr;
float jointOffset = 0.0f;
@@ -203,14 +217,22 @@ namespace VirtualRobot
{
// no <Joint> tag -> fixed joint
RobotNodePtr robotNode;
- RobotNodeFactoryPtr fixedNodeFactory = RobotNodeFactory::fromName(RobotNodeFixedFactory::getName(), nullptr);
+ 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;
}
@@ -224,7 +246,8 @@ namespace VirtualRobot
else
{
VR_WARNING << "No 'type' attribute for <Joint> tag. "
- << "Assuming fixed joint for RobotNode " << robotNodeName << "!" << std::endl;
+ << "Assuming fixed joint for RobotNode " << robotNodeName << "!"
+ << std::endl;
jointType = RobotNodeFixedFactory::getName();
}
@@ -261,37 +284,50 @@ namespace VirtualRobot
if (nodeName == "dh")
{
THROW_VR_EXCEPTION("DH specification in Joint tag is DEPRECATED! "
- "Use <RobotNode><Transform><DH>...</DH></Transform><Joint>...</Joint></RobotNode> structure.")
+ "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;
}
else if (nodeName == "limits")
{
- THROW_VR_EXCEPTION_IF(limitsNode, "Multiple limits definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
+ THROW_VR_EXCEPTION_IF(limitsNode,
+ "Multiple limits definitions in <Joint> tag of robot node <"
+ << robotNodeName << ">." << endl);
limitsNode = node;
processLimitsNode(limitsNode, jointLimitLow, jointLimitHigh, limitless);
}
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;
}
else if (nodeName == "axis")
{
- THROW_VR_EXCEPTION_IF(tmpXMLNodeAxis, "Multiple axis definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
+ THROW_VR_EXCEPTION_IF(tmpXMLNodeAxis,
+ "Multiple axis definitions in <Joint> tag of robot node <"
+ << robotNodeName << ">." << endl);
tmpXMLNodeAxis = node;
}
else if (nodeName == "translationdirection")
{
- THROW_VR_EXCEPTION_IF(tmpXMLNodeTranslation, "Multiple translation definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
+ THROW_VR_EXCEPTION_IF(
+ tmpXMLNodeTranslation,
+ "Multiple translation definitions in <Joint> tag of robot node <"
+ << robotNodeName << ">." << endl);
tmpXMLNodeTranslation = node;
}
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;
}
@@ -300,7 +336,7 @@ namespace VirtualRobot
maxVelocity = getFloatByAttributeName(node, "value");
// convert to m/s
- std::vector< Units > unitsAttr = getUnitsAttributes(node);
+ std::vector<Units> unitsAttr = getUnitsAttributes(node);
Units uTime("sec");
Units uLength("m");
Units uAngle("rad");
@@ -346,14 +382,13 @@ namespace VirtualRobot
}
maxVelocity *= factor;
-
}
else if (nodeName == "maxacceleration")
{
maxAcceleration = getFloatByAttributeName(node, "value");
// convert to m/s^2
- std::vector< Units > unitsAttr = getUnitsAttributes(node);
+ std::vector<Units> unitsAttr = getUnitsAttributes(node);
Units uTime("sec");
Units uLength("m");
Units uAngle("rad");
@@ -399,13 +434,12 @@ namespace VirtualRobot
}
maxAcceleration *= factor;
-
}
else if (nodeName == "maxtorque")
{
maxTorque = getFloatByAttributeName(node, "value");
// convert to Nm
- std::vector< Units > unitsAttr = getUnitsAttributes(node);
+ std::vector<Units> unitsAttr = getUnitsAttributes(node);
Units uLength("m");
for (auto& i : unitsAttr)
@@ -429,7 +463,9 @@ namespace VirtualRobot
{
rapidxml::xml_attribute<>* attrPropa;
attrPropa = node->first_attribute("name", 0, false);
- THROW_VR_EXCEPTION_IF(!attrPropa, "Expecting 'name' attribute in <PropagateJointValue> tag..." << endl);
+ THROW_VR_EXCEPTION_IF(!attrPropa,
+ "Expecting 'name' attribute in <PropagateJointValue> tag..."
+ << endl);
std::string s(attrPropa->value());
propagateJVName.push_back(s);
@@ -467,12 +503,21 @@ namespace VirtualRobot
switch (hemisphere->role)
{
- case RobotNodeHemisphere::Role::FIRST:
- hemisphere->lever = getFloatByAttributeName(node, "lever");
- hemisphere->theta0Rad = simox::math::deg_to_rad(getFloatByAttributeName(node, "theta0"));
- break;
- case RobotNodeHemisphere::Role::SECOND:
- break;
+ case RobotNodeHemisphere::Role::FIRST:
+ 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;
}
}
else if (nodeName == "four_bar")
@@ -490,28 +535,28 @@ namespace VirtualRobot
THROW_VR_EXCEPTION("Invalid role in four_bar joint: " << e.what())
}
- const rapidxml::xml_node<>* dimensionsNode = node->first_node("dimensions", 0, false);
+ const rapidxml::xml_node<>* dimensionsNode =
+ node->first_node("dimensions", 0, false);
- if(fourBarXmlInfo->role == RobotNodeFourBar::Role::ACTIVE)
+ if (fourBarXmlInfo->role == RobotNodeFourBar::Role::ACTIVE)
{
- if(dimensionsNode == nullptr)
+ if (dimensionsNode == nullptr)
{
THROW_VR_EXCEPTION("Missing <dimensions> node for four_bar joint.");
}
-
- fourBarXmlInfo->dimensions = four_bar::Joint::Dimensions
- {
+
+ fourBarXmlInfo->dimensions = four_bar::Joint::Dimensions{
.shank = getFloatByAttributeName(dimensionsNode, "shank"),
.p1 = getFloatByAttributeName(dimensionsNode, "p1"),
.p2 = getFloatByAttributeName(dimensionsNode, "p2"),
- .p3 = getFloatByAttributeName(dimensionsNode, "p3")
- };
+ .p3 = getFloatByAttributeName(dimensionsNode, "p3")};
}
-
}
else
{
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in <Joint> tag of RobotNode <" << robotNodeName << ">." << endl);
+ THROW_VR_EXCEPTION("XML definition <"
+ << nodeName << "> not supported in <Joint> tag of RobotNode <"
+ << robotNodeName << ">." << endl);
}
node = node->next_sibling();
@@ -562,7 +607,10 @@ namespace VirtualRobot
}
else
{
- THROW_VR_EXCEPTION("Prismatic joint '" << robotNodeName << "' wrongly defined, expecting 'TranslationDirection' tag." << endl);
+ THROW_VR_EXCEPTION("Prismatic joint '"
+ << robotNodeName
+ << "' wrongly defined, expecting 'TranslationDirection' tag."
+ << endl);
}
if (scaleVisu)
@@ -586,12 +634,18 @@ 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
@@ -623,7 +677,7 @@ namespace VirtualRobot
node->setXmlInfo(hemisphere.value());
}
- if(robotNode->isFourBarJoint() and fourBarXmlInfo.has_value())
+ if (robotNode->isFourBarJoint() and fourBarXmlInfo.has_value())
{
auto node = std::dynamic_pointer_cast<RobotNodeFourBar>(robotNode);
node->setXmlInfo(fourBarXmlInfo.value());
@@ -648,16 +702,15 @@ namespace VirtualRobot
return robotNode;
}
-
-
- RobotNodePtr RobotIO::processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
- RobotPtr robo,
- const std::string& basePath,
- int& robotNodeCounter,
- std::vector< std::string >& childrenNames,
- std::vector< ChildFromRobotDef >& childrenFromRobot,
- RobotDescription loadMode,
- RobotNode::RobotNodeType rntype)
+ RobotNodePtr
+ RobotIO::processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
+ RobotPtr robo,
+ const std::string& basePath,
+ int& robotNodeCounter,
+ std::vector<std::string>& childrenNames,
+ std::vector<ChildFromRobotDef>& childrenFromRobot,
+ RobotDescription loadMode,
+ RobotNode::RobotNodeType rntype)
{
childrenFromRobot.clear();
THROW_VR_EXCEPTION_IF(!robotNodeXMLNode, "NULL data in processRobotNode");
@@ -671,7 +724,8 @@ namespace VirtualRobot
ss << robo->getType() << "_Node_" << robotNodeCounter;
robotNodeName = ss.str();
robotNodeCounter++;
- VR_WARNING << "RobotNode definition expects attribute 'name'. Setting name to " << robotNodeName << std::endl;
+ VR_WARNING << "RobotNode definition expects attribute 'name'. Setting name to "
+ << robotNodeName << std::endl;
}
@@ -696,7 +750,7 @@ namespace VirtualRobot
rapidxml::xml_node<>* node = robotNodeXMLNode->first_node();
rapidxml::xml_node<>* jointNodeXML = nullptr;
- std::vector< rapidxml::xml_node<>* > sensorTags;
+ std::vector<rapidxml::xml_node<>*> sensorTags;
std::vector<GraspSetPtr> graspSets;
while (node)
@@ -707,34 +761,44 @@ namespace VirtualRobot
{
if (loadMode == eFull || loadMode == eFullVisAsCol)
{
- THROW_VR_EXCEPTION_IF(visuProcessed, "Two visualization tags defined in RobotNode '" << robotNodeName << "'." << endl);
- visualizationNode = processVisualizationTag(node, robotNodeName, basePath, useAsColModel);
+ THROW_VR_EXCEPTION_IF(visuProcessed,
+ "Two visualization tags defined in RobotNode '"
+ << robotNodeName << "'." << endl);
+ visualizationNode =
+ processVisualizationTag(node, robotNodeName, basePath, useAsColModel);
visuProcessed = true;
if (useAsColModel && visualizationNode)
{
- THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in RobotNode '" << robotNodeName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(colProcessed,
+ "Two collision tags defined in RobotNode '"
+ << robotNodeName << "'." << endl);
std::string colModelName = robotNodeName;
colModelName += "_VISU_ColModel";
// clone model
VisualizationNodePtr visualizationNodeClone = visualizationNode->clone();
// todo: ID?
- collisionModel.reset(new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
+ collisionModel.reset(new CollisionModel(
+ visualizationNodeClone, colModelName, CollisionCheckerPtr()));
colProcessed = true;
}
}
else if (loadMode == eCollisionModel)
{
- VisualizationNodePtr visualizationNodeCM = checkUseAsColModel(node, robotNodeName, basePath);
+ VisualizationNodePtr visualizationNodeCM =
+ checkUseAsColModel(node, robotNodeName, basePath);
if (visualizationNodeCM)
{
useAsColModel = true;
- THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in RobotNode '" << robotNodeName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(colProcessed,
+ "Two collision tags defined in RobotNode '"
+ << robotNodeName << "'." << endl);
std::string colModelName = robotNodeName;
colModelName += "_VISU_ColModel";
// todo: ID?
- collisionModel.reset(new CollisionModel(visualizationNodeCM, colModelName, CollisionCheckerPtr()));
+ collisionModel.reset(new CollisionModel(
+ visualizationNodeCM, colModelName, CollisionCheckerPtr()));
colProcessed = true;
}
}
@@ -750,7 +814,9 @@ namespace VirtualRobot
{
if (loadMode == eFull || loadMode == eCollisionModel || loadMode == eFullVisAsCol)
{
- THROW_VR_EXCEPTION_IF(colProcessed, "Two collision tags defined in RobotNode '" << robotNodeName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(colProcessed,
+ "Two collision tags defined in RobotNode '"
+ << robotNodeName << "'." << endl);
collisionModel = processCollisionTag(node, robotNodeName, basePath);
colProcessed = true;
}
@@ -775,12 +841,16 @@ namespace VirtualRobot
}
else if (nodeName == "joint")
{
- THROW_VR_EXCEPTION_IF(jointNodeXML, "Two joint tags defined in RobotNode '" << robotNodeName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(jointNodeXML,
+ "Two joint tags defined in RobotNode '" << robotNodeName
+ << "'." << endl);
jointNodeXML = node;
}
else if (nodeName == "physics")
{
- THROW_VR_EXCEPTION_IF(physicsDefined, "Two physics tags defined in RobotNode '" << robotNodeName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(physicsDefined,
+ "Two physics tags defined in RobotNode '" << robotNodeName
+ << "'." << endl);
processPhysicsTag(node, robotNodeName, physics);
physicsDefined = true;
}
@@ -795,13 +865,15 @@ namespace VirtualRobot
else if (nodeName == "graspset")
{
GraspSetPtr gs = processGraspSet(node, robotNodeName);
- THROW_VR_EXCEPTION_IF(!gs, "Invalid grasp set in '" << robotNodeName << "'." << endl);
+ THROW_VR_EXCEPTION_IF(!gs,
+ "Invalid grasp set in '" << robotNodeName << "'." << endl);
graspSets.push_back(gs);
-
}
else
{
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in RobotNode <" << robotNodeName << ">." << endl);
+ THROW_VR_EXCEPTION("XML definition <"
+ << nodeName << "> not supported in RobotNode <" << robotNodeName
+ << ">." << endl);
}
node = node->next_sibling();
@@ -815,11 +887,19 @@ namespace VirtualRobot
// clone model
VisualizationNodePtr visualizationNodeClone = visualizationNode->clone();
// todo: ID?
- collisionModel.reset(new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
+ collisionModel.reset(
+ new CollisionModel(visualizationNodeClone, colModelName, CollisionCheckerPtr()));
}
//create joint from xml data
- robotNode = processJointNode(jointNodeXML, robotNodeName, robo, visualizationNode, collisionModel, physics, rntype, transformMatrix);
+ robotNode = processJointNode(jointNodeXML,
+ robotNodeName,
+ robo,
+ visualizationNode,
+ collisionModel,
+ physics,
+ rntype,
+ transformMatrix);
robotNode->basePath = basePath;
robotNode->visualizationModelXML = visualizationModelXML;
robotNode->collisionModelXML = collisionModelXML;
@@ -840,8 +920,10 @@ namespace VirtualRobot
return robotNode;
}
-
- VisualizationNodePtr RobotIO::checkUseAsColModel(rapidxml::xml_node<>* visuXMLNode, const std::string& /*robotNodeName*/, const std::string& basePath)
+ VisualizationNodePtr
+ RobotIO::checkUseAsColModel(rapidxml::xml_node<>* visuXMLNode,
+ const std::string& /*robotNodeName*/,
+ const std::string& basePath)
{
bool enableVisu = true;
//bool coordAxis = false;
@@ -872,17 +954,20 @@ namespace VirtualRobot
if (visuFileXMLNode)
{
attr = visuFileXMLNode->first_attribute("type", 0, false);
- THROW_VR_EXCEPTION_IF(!attr, "Missing 'type' attribute in <Visualization> tag." << endl);
+ THROW_VR_EXCEPTION_IF(!attr,
+ "Missing 'type' attribute in <Visualization> tag." << endl);
visuFileType = attr->value();
getLowerCase(visuFileType);
visuFile = processFileNode(visuFileXMLNode, basePath);
}
- rapidxml::xml_node<>* useColModel = visuXMLNode->first_node("useascollisionmodel", 0, false);
+ rapidxml::xml_node<>* useColModel =
+ visuXMLNode->first_node("useascollisionmodel", 0, false);
if (useColModel && visuFile != "")
{
- VisualizationFactoryPtr visualizationFactory = VisualizationFactory::fromName(visuFileType, nullptr);
+ VisualizationFactoryPtr visualizationFactory =
+ VisualizationFactory::fromName(visuFileType, nullptr);
if (visualizationFactory)
{
@@ -890,7 +975,8 @@ namespace VirtualRobot
}
else
{
- VR_WARNING << "VisualizationFactory of type '" << visuFileType << "' not present. Ignoring Visualization data." << std::endl;
+ VR_WARNING << "VisualizationFactory of type '" << visuFileType
+ << "' not present. Ignoring Visualization data." << std::endl;
}
}
}
@@ -898,9 +984,13 @@ namespace VirtualRobot
return visualizationNode;
}
- RobotPtr RobotIO::processRobot(rapidxml::xml_node<char>* robotXMLNode, const std::string& basePath, RobotDescription loadMode)
+ RobotPtr
+ RobotIO::processRobot(rapidxml::xml_node<char>* robotXMLNode,
+ const std::string& basePath,
+ RobotDescription loadMode)
{
- THROW_VR_EXCEPTION_IF(!robotXMLNode, "No <Robot> tag in XML definition! base path = " << basePath);
+ THROW_VR_EXCEPTION_IF(!robotXMLNode,
+ "No <Robot> tag in XML definition! base path = " << basePath);
// process Attributes
std::string robotRoot;
@@ -908,23 +998,34 @@ namespace VirtualRobot
robo = processRobotAttributes(robotXMLNode, robotRoot);
// process xml nodes
- std::map< RobotNodePtr, std::vector< ChildFromRobotDef > > childrenFromRobotFilesMap;
- std::vector<rapidxml::xml_node<char>* > robotNodeSetNodes;
- std::vector<rapidxml::xml_node<char>* > endeffectorNodes;
+ std::map<RobotNodePtr, std::vector<ChildFromRobotDef>> childrenFromRobotFilesMap;
+ std::vector<rapidxml::xml_node<char>*> robotNodeSetNodes;
+ std::vector<rapidxml::xml_node<char>*> endeffectorNodes;
NodeMapping nodeMapping;
std::optional<HumanMapping> humanMapping;
std::map<std::string, std::vector<std::string>> attachments;
- processRobotChildNodes(robotXMLNode, robo, robotRoot, basePath, childrenFromRobotFilesMap, robotNodeSetNodes, endeffectorNodes, nodeMapping, humanMapping, attachments, loadMode);
+ processRobotChildNodes(robotXMLNode,
+ robo,
+ robotRoot,
+ basePath,
+ childrenFromRobotFilesMap,
+ robotNodeSetNodes,
+ endeffectorNodes,
+ nodeMapping,
+ humanMapping,
+ attachments,
+ loadMode);
// process childfromrobot tags
- std::map< RobotNodePtr, std::vector< ChildFromRobotDef > >::iterator iter = childrenFromRobotFilesMap.begin();
+ std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>::iterator iter =
+ childrenFromRobotFilesMap.begin();
while (iter != childrenFromRobotFilesMap.end())
{
- std::vector< ChildFromRobotDef > childrenFromRobot = iter->second;
+ std::vector<ChildFromRobotDef> childrenFromRobot = iter->second;
RobotNodePtr node = iter->first;
for (auto& i : childrenFromRobot)
@@ -936,21 +1037,29 @@ namespace VirtualRobot
try
{
- THROW_VR_EXCEPTION_IF(!std::filesystem::exists(filenameNewComplete), "File <" << filenameNewComplete.string() << "> does not exist." << endl);
+ THROW_VR_EXCEPTION_IF(!std::filesystem::exists(filenameNewComplete),
+ "File <" << filenameNewComplete.string()
+ << "> does not exist." << endl);
}
catch (...)
{
- THROW_VR_EXCEPTION("Error while processing file <" << filenameNewComplete.string() << ">." << endl);
-
+ THROW_VR_EXCEPTION("Error while processing file <"
+ << filenameNewComplete.string() << ">." << endl);
}
RobotPtr r = loadRobot(filenameNewComplete.string(), loadMode);
- THROW_VR_EXCEPTION_IF(!r, "Could not add child-from-robot due to failed loading of robot from file" << i.filename);
+ THROW_VR_EXCEPTION_IF(
+ !r,
+ "Could not add child-from-robot due to failed loading of robot from file"
+ << i.filename);
RobotNodePtr root = r->getRootNode();
- THROW_VR_EXCEPTION_IF(!root, "Could not add child-from-robot. No root node in file" << i.filename);
+ THROW_VR_EXCEPTION_IF(
+ !root, "Could not add child-from-robot. No root node in file" << i.filename);
RobotNodePtr rootNew = root->clone(robo, true, node);
- THROW_VR_EXCEPTION_IF(!rootNew, "Clone failed. Could not add child-from-robot from file " << i.filename);
+ THROW_VR_EXCEPTION_IF(!rootNew,
+ "Clone failed. Could not add child-from-robot from file "
+ << i.filename);
std::vector<EndEffectorPtr> eefs;
r->getEndEffectors(eefs);
@@ -980,22 +1089,26 @@ namespace VirtualRobot
const auto robotNodes = robo->getRobotNodes();
// extend children map with attachments
- for(const auto& [parentNodeName, childrenNodeNames]: attachments)
+ for (const auto& [parentNodeName, childrenNodeNames] : attachments)
{
// find parent node
- const auto parentNodeIt = std::find_if(robotNodes.begin(), robotNodes.end(), [&](const auto& robotNode){
- return robotNode->getName() == parentNodeName;
- });
+ const auto parentNodeIt = std::find_if(
+ robotNodes.begin(),
+ robotNodes.end(),
+ [&](const auto& robotNode) { return robotNode->getName() == parentNodeName; });
- if(parentNodeIt == robotNodes.end())
+ if (parentNodeIt == robotNodes.end())
{
- THROW_VR_EXCEPTION("Robot node `" << parentNodeName << "` was defined as as parent node but is is not known!" << std::endl);
+ THROW_VR_EXCEPTION("Robot node `"
+ << parentNodeName
+ << "` was defined as as parent node but is is not known!"
+ << std::endl);
}
// add all children to the mapping
const auto& parentNode = *parentNodeIt;
- for(const auto& childName : childrenNodeNames)
+ for (const auto& childName : childrenNodeNames)
{
robo->getRobotNode(childName)->initialize(parentNode);
}
@@ -1013,7 +1126,8 @@ namespace VirtualRobot
for (auto& robotNodeSetNode : robotNodeSetNodes)
{
- RobotNodeSetPtr rns = processRobotNodeSet(robotNodeSetNode, robo, robotRoot, rnsCounter);
+ RobotNodeSetPtr rns =
+ processRobotNodeSet(robotNodeSetNode, robo, robotRoot, rnsCounter);
//nodeSets.push_back(rns);
}
@@ -1031,30 +1145,33 @@ namespace VirtualRobot
robo->registerNodeMapping(nodeMapping);
- if(humanMapping.has_value())
+ if (humanMapping.has_value())
{
robo->registerHumanMapping(humanMapping.value());
}
-
+
return robo;
}
- inline bool toBool(const std::string& strRepr) noexcept
+ inline bool
+ toBool(const std::string& strRepr) noexcept
{
- try {
+ try
+ {
const int passiveIntRepr = std::stoi(strRepr);
return static_cast<bool>(passiveIntRepr);
-
- } catch (std::invalid_argument&) {
+ }
+ catch (std::invalid_argument&)
+ {
// nothing to do here
}
return strRepr == "true";
}
-
- RobotPtr RobotIO::processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot)
+ RobotPtr
+ RobotIO::processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot)
{
std::string robotName;
std::string robotType;
@@ -1099,7 +1216,7 @@ namespace VirtualRobot
if (robot_name_counter[robotType] == 1)
{
- ss << robotType; // first one
+ ss << robotType; // first one
}
else
{
@@ -1121,11 +1238,11 @@ namespace VirtualRobot
attr = robotXMLNode->first_attribute("passive", 0, false);
- if(attr != nullptr)
+ if (attr != nullptr)
{
const std::string passiveStrRep = attr->value();
passive = toBool(passiveStrRep);
-
+
VR_INFO << "Robot is 'passive' according to config" << std::endl;
}
@@ -1137,7 +1254,8 @@ namespace VirtualRobot
robo->setRadianToMMfactor(atof(attr->value()));
}*/
- if(passive){
+ if (passive)
+ {
robo->setPassive();
}
@@ -1145,22 +1263,22 @@ namespace VirtualRobot
return robo;
}
-
- void RobotIO::processRobotChildNodes(rapidxml::xml_node<char>* robotXMLNode,
- RobotPtr robo,
- const std::string& robotRoot,
- const std::string& basePath,
- std::map < RobotNodePtr,
- std::vector<ChildFromRobotDef> >& childrenFromRobotFilesMap,
- std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes,
- std::vector<rapidxml::xml_node<char>* >& endeffectorNodes,
- NodeMapping& nodeMapping,
- std::optional<HumanMapping>& humanMapping,
- std::map<std::string, std::vector<std::string>>& attachments,
- RobotDescription loadMode)
+ void
+ RobotIO::processRobotChildNodes(
+ rapidxml::xml_node<char>* robotXMLNode,
+ RobotPtr robo,
+ const std::string& robotRoot,
+ const std::string& basePath,
+ std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>& childrenFromRobotFilesMap,
+ std::vector<rapidxml::xml_node<char>*>& robotNodeSetNodes,
+ std::vector<rapidxml::xml_node<char>*>& endeffectorNodes,
+ NodeMapping& nodeMapping,
+ std::optional<HumanMapping>& humanMapping,
+ std::map<std::string, std::vector<std::string>>& attachments,
+ RobotDescription loadMode)
{
std::vector<RobotNodePtr> robotNodes;
- std::map< RobotNodePtr, std::vector< std::string > > childrenMap;
+ std::map<RobotNodePtr, std::vector<std::string>> childrenMap;
RobotNodePtr rootNode;
int robotNodeCounter = 0; // used for robotnodes without names
@@ -1175,10 +1293,12 @@ namespace VirtualRobot
std::string nodeName_ = XMLNode->name();
std::string nodeName = getLowerCase(XMLNode->name());
- if (nodeName == "robotnode" || nodeName == "jointnode" || nodeName == "transformationnode" || nodeName == "bodynode" || nodeName == "modelnode")
+ if (nodeName == "robotnode" || nodeName == "jointnode" ||
+ nodeName == "transformationnode" || nodeName == "bodynode" ||
+ nodeName == "modelnode")
{
- std::vector< ChildFromRobotDef > childrenFromRobot;
- std::vector< std::string > childrenNames;
+ std::vector<ChildFromRobotDef> childrenFromRobot;
+ std::vector<std::string> childrenNames;
// check for type
RobotNode::RobotNodeType rntype = RobotNode::Generic;
@@ -1197,7 +1317,14 @@ namespace VirtualRobot
rntype = RobotNode::Transform;
}
- RobotNodePtr n = processRobotNode(XMLNode, robo, basePath, robotNodeCounter, childrenNames, childrenFromRobot, loadMode, rntype);
+ RobotNodePtr n = processRobotNode(XMLNode,
+ robo,
+ basePath,
+ robotNodeCounter,
+ childrenNames,
+ childrenFromRobot,
+ loadMode,
+ rntype);
if (!n)
{
@@ -1209,7 +1336,10 @@ namespace VirtualRobot
// double name check
for (auto& robotNode : robotNodes)
{
- THROW_VR_EXCEPTION_IF((robotNode->getName() == n->getName()), "At least two RobotNodes with name <" << n->getName() << "> defined in robot definition");
+ THROW_VR_EXCEPTION_IF((robotNode->getName() == n->getName()),
+ "At least two RobotNodes with name <"
+ << n->getName()
+ << "> defined in robot definition");
}
childrenMap[n] = childrenNames;
@@ -1264,7 +1394,9 @@ namespace VirtualRobot
}
else
{
- THROW_VR_EXCEPTION("XML node of type <" << nodeName_ << "> is not supported. Ignoring contents..." << endl);
+ THROW_VR_EXCEPTION("XML node of type <"
+ << nodeName_ << "> is not supported. Ignoring contents..."
+ << endl);
}
XMLNode = XMLNode->next_sibling(nullptr, 0, false);
@@ -1277,10 +1409,8 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION("Error while initializing Robot" << endl);
}
-
}
-
/**
* This method parses the EndEffector which are child tags of the Robot tag.
* Each EndEffector has a name, a base node, a list of static robot nodes and a list of actors.
@@ -1291,7 +1421,8 @@ namespace VirtualRobot
*
* \return instance of VirtualRobot::EndEffector
*/
- EndEffectorPtr RobotIO::processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo)
+ EndEffectorPtr
+ RobotIO::processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo)
{
std::string endeffectorName("");
std::string baseNodeName;
@@ -1310,37 +1441,60 @@ namespace VirtualRobot
if ("name" == attributeName)
{
- THROW_VR_EXCEPTION_IF(!endeffectorName.empty(), "Endeffector tag has more than one <name> tag. Value of the first one is: " + endeffectorName);
+ THROW_VR_EXCEPTION_IF(
+ !endeffectorName.empty(),
+ "Endeffector tag has more than one <name> tag. Value of the first one is: " +
+ endeffectorName);
endeffectorName = attr->value();
- THROW_VR_EXCEPTION_IF(endeffectorName.empty(), "Endeffector tag does not specify a <name> tag.");
+ THROW_VR_EXCEPTION_IF(endeffectorName.empty(),
+ "Endeffector tag does not specify a <name> tag.");
}
else if ("base" == attributeName)
{
- THROW_VR_EXCEPTION_IF(!baseNodeName.empty(), "Endeffector tag has more than one <base> tag. Value of the first one is: " + baseNodeName);
+ THROW_VR_EXCEPTION_IF(
+ !baseNodeName.empty(),
+ "Endeffector tag has more than one <base> tag. Value of the first one is: " +
+ baseNodeName);
baseNodeName = attr->value();
- THROW_VR_EXCEPTION_IF(baseNodeName.empty(), "Endeffector tag does not specify a <base> tag.");
+ THROW_VR_EXCEPTION_IF(baseNodeName.empty(),
+ "Endeffector tag does not specify a <base> tag.");
baseNode = robo->getRobotNode(baseNodeName);
- THROW_VR_EXCEPTION_IF(!baseNode, "base associated with <Endeffector> not available in the robot model.");
+ THROW_VR_EXCEPTION_IF(
+ !baseNode,
+ "base associated with <Endeffector> not available in the robot model.");
}
else if ("tcp" == attributeName)
{
- THROW_VR_EXCEPTION_IF(!tcpNodeName.empty(), "Endeffector tag has more than one <tcp> tag. Value of the first one is: " + tcpNodeName);
+ THROW_VR_EXCEPTION_IF(
+ !tcpNodeName.empty(),
+ "Endeffector tag has more than one <tcp> tag. Value of the first one is: " +
+ tcpNodeName);
tcpNodeName = attr->value();
- THROW_VR_EXCEPTION_IF(tcpNodeName.empty(), "Endeffector tag does not specify a <tcp> tag.");
+ THROW_VR_EXCEPTION_IF(tcpNodeName.empty(),
+ "Endeffector tag does not specify a <tcp> tag.");
tcpNode = robo->getRobotNode(tcpNodeName);
- THROW_VR_EXCEPTION_IF(!tcpNode, "tcp associated with <Endeffector> not available in the robot model.");
+ THROW_VR_EXCEPTION_IF(
+ !tcpNode,
+ "tcp associated with <Endeffector> not available in the robot model.");
}
else if ("gcp" == attributeName)
{
- THROW_VR_EXCEPTION_IF(!gcpNodeName.empty(), "Endeffector tag has more than one <gcp> tag. Value of the first one is: " + gcpNodeName);
+ THROW_VR_EXCEPTION_IF(
+ !gcpNodeName.empty(),
+ "Endeffector tag has more than one <gcp> tag. Value of the first one is: " +
+ gcpNodeName);
gcpNodeName = attr->value();
- THROW_VR_EXCEPTION_IF(gcpNodeName.empty(), "Endeffector tag does not specify a <gcp> tag.");
+ THROW_VR_EXCEPTION_IF(gcpNodeName.empty(),
+ "Endeffector tag does not specify a <gcp> tag.");
gcpNode = robo->getRobotNode(gcpNodeName);
- THROW_VR_EXCEPTION_IF(!gcpNode, "gcp associated with <Endeffector> not available in the robot model.");
+ THROW_VR_EXCEPTION_IF(
+ !gcpNode,
+ "gcp associated with <Endeffector> not available in the robot model.");
}
else
{
- VR_WARNING << "Ignoring unknown attribute in EEF <" << endeffectorName << "> definition:" << attributeName << std::endl;
+ VR_WARNING << "Ignoring unknown attribute in EEF <" << endeffectorName
+ << "> definition:" << attributeName << std::endl;
}
attr = attr->next_attribute();
@@ -1348,9 +1502,9 @@ namespace VirtualRobot
std::vector<RobotNodePtr> staticParts;
std::vector<EndEffectorActorPtr> actors;
- std::vector< std::vector< RobotConfig::Configuration > > configDefinitions;
- std::vector< std::string > configNames;
- std::vector< std::string > tcpNames;
+ std::vector<std::vector<RobotConfig::Configuration>> configDefinitions;
+ std::vector<std::string> configNames;
+ std::vector<std::string> tcpNames;
rapidxml::xml_node<>* node = endeffectorXMLNode->first_node();
while (node)
@@ -1369,17 +1523,22 @@ namespace VirtualRobot
}
else
{
- VR_ERROR << "There should only be one <static> tag inside <endeffector> tags" << std::endl;
+ VR_ERROR << "There should only be one <static> tag inside <endeffector> tags"
+ << std::endl;
}
}
else if ("preshape" == nodeName)
{
- bool cOK = processConfigurationNodeList(node, configDefinitions, configNames, tcpNames);
- THROW_VR_EXCEPTION_IF(!cOK, "Invalid Preshape defined in robot's eef tag '" << nodeName << "'." << endl);
+ bool cOK =
+ processConfigurationNodeList(node, configDefinitions, configNames, tcpNames);
+ THROW_VR_EXCEPTION_IF(!cOK,
+ "Invalid Preshape defined in robot's eef tag '"
+ << nodeName << "'." << endl);
}
else
{
- THROW_VR_EXCEPTION("XML tag <" << nodeName << "> not supported in endeffector <" << nodeName << ">");
+ THROW_VR_EXCEPTION("XML tag <" << nodeName << "> not supported in endeffector <"
+ << nodeName << ">");
}
node = node->next_sibling();
@@ -1395,10 +1554,12 @@ namespace VirtualRobot
gcpNode = tcpNode;
}
- EndEffectorPtr endEffector(new EndEffector(endeffectorName, actors, staticParts, baseNode, tcpNode, gcpNode));
+ EndEffectorPtr endEffector(
+ new EndEffector(endeffectorName, actors, staticParts, baseNode, tcpNode, gcpNode));
// create & register configs
- THROW_VR_EXCEPTION_IF(configDefinitions.size() != configNames.size(), "Invalid Preshape definitions " << endl);
+ THROW_VR_EXCEPTION_IF(configDefinitions.size() != configNames.size(),
+ "Invalid Preshape definitions " << endl);
for (size_t i = 0; i < configDefinitions.size(); i++)
{
@@ -1413,17 +1574,20 @@ namespace VirtualRobot
return endEffector;
}
-
/**
* This method processes the attributes and the children of an actor tag which
* itself is a child of the endeffector tag.
* First the name attribute is retrieved and afterwards the child nodes are
* processed which make up the actor.
*/
- EndEffectorActorPtr RobotIO::processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode, RobotPtr robo)
+ EndEffectorActorPtr
+ RobotIO::processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode,
+ RobotPtr robo)
{
std::string actorName = processNameAttribute(endeffectorActorXMLNode);
- THROW_VR_EXCEPTION_IF(actorName.empty(), "<Actor> tag inside <Endeffector> does not specify a <name> attribute.");
+ THROW_VR_EXCEPTION_IF(
+ actorName.empty(),
+ "<Actor> tag inside <Endeffector> does not specify a <name> attribute.");
std::vector<EndEffectorActor::ActorDefinition> actors;
processActorNodeList(endeffectorActorXMLNode, robo, actors);
@@ -1431,17 +1595,18 @@ namespace VirtualRobot
return actor;
}
-
/**
* This method processes the children of the static tag which
* itself is a child of the endeffector tag.
*/
- void RobotIO::processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode, RobotPtr robo, std::vector<RobotNodePtr>& staticNodesList)
+ void
+ RobotIO::processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode,
+ RobotPtr robo,
+ std::vector<RobotNodePtr>& staticNodesList)
{
processNodeList(endeffectorStaticXMLNode, robo, staticNodesList, false);
}
-
/**
* This method processes the \p parentNode Tag and extracts a list of \<Node name="xyz" speed="0123" /\> tags.
* All other child tags raise a VirtualRobot::VirtualRobotException.
@@ -1449,7 +1614,11 @@ namespace VirtualRobot
*
* If the parameter \p clearList is true all elements from \p nodeList are removed.
*/
- void RobotIO::processActorNodeList(rapidxml::xml_node<char>* parentNode, RobotPtr robot, std::vector<EndEffectorActor::ActorDefinition>& actorList, bool clearList /*= true*/)
+ void
+ RobotIO::processActorNodeList(rapidxml::xml_node<char>* parentNode,
+ RobotPtr robot,
+ std::vector<EndEffectorActor::ActorDefinition>& actorList,
+ bool clearList /*= true*/)
{
if (clearList)
{
@@ -1468,9 +1637,13 @@ namespace VirtualRobot
{
EndEffectorActor::ActorDefinition actor;
std::string nodeNameAttr = processNameAttribute(node, true);
- THROW_VR_EXCEPTION_IF(nodeNameAttr.empty(), "Missing name attribute for <Node> belonging to Robot node set " << parentName);
+ THROW_VR_EXCEPTION_IF(
+ nodeNameAttr.empty(),
+ "Missing name attribute for <Node> belonging to Robot node set " << parentName);
actor.robotNode = robot->getRobotNode(nodeNameAttr);
- THROW_VR_EXCEPTION_IF(!actor.robotNode, "<node> tag with name '" << nodeNameAttr << "' not present in the current robot");
+ THROW_VR_EXCEPTION_IF(!actor.robotNode,
+ "<node> tag with name '"
+ << nodeNameAttr << "' not present in the current robot");
actor.directionAndSpeed = processFloatAttribute(speedname, node, true);
if (actor.directionAndSpeed == 0.0f)
@@ -1483,15 +1656,18 @@ namespace VirtualRobot
}
else
{
- THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in <Actor> with name " << parentName);
+ THROW_VR_EXCEPTION("XML definition <" << nodeName
+ << "> not supported in <Actor> with name "
+ << parentName);
}
node = node->next_sibling();
}
}
-
- EndEffectorActor::CollisionMode RobotIO::processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes /* = false */)
+ EndEffectorActor::CollisionMode
+ RobotIO::processEEFColAttributes(rapidxml::xml_node<char>* node,
+ bool allowOtherAttributes /* = false */)
{
THROW_VR_EXCEPTION_IF(!node, "Can not process name attribute of NULL node" << endl);
@@ -1527,15 +1703,17 @@ namespace VirtualRobot
}
else
{
- THROW_VR_EXCEPTION("<" << node->name() << "> considerCollisions attribute is unknowne: " << name);
-
+ THROW_VR_EXCEPTION("<"
+ << node->name()
+ << "> considerCollisions attribute is unknowne: " << name);
}
}
else
{
if (!allowOtherAttributes)
{
- THROW_VR_EXCEPTION("<" << node->name() << "> tag contains unknown attribute: " << attr->name());
+ THROW_VR_EXCEPTION("<" << node->name()
+ << "> tag contains unknown attribute: " << attr->name());
}
}
@@ -1551,9 +1729,10 @@ namespace VirtualRobot
return result;
}
-
-
- VirtualRobot::RobotPtr RobotIO::createRobotFromString(const std::string& xmlString, const std::string& basePath, RobotDescription loadMode)
+ VirtualRobot::RobotPtr
+ RobotIO::createRobotFromString(const std::string& xmlString,
+ const std::string& basePath,
+ RobotDescription loadMode)
{
// copy string content to char array
char* y = new char[xmlString.size() + 1];
@@ -1563,8 +1742,8 @@ namespace VirtualRobot
try
{
- rapidxml::xml_document<char> doc; // character type defaults to char
- doc.parse<0>(y); // 0 means default parse flags
+ rapidxml::xml_document<char> doc; // character type defaults to char
+ doc.parse<0>(y); // 0 means default parse flags
rapidxml::xml_node<char>* robotXMLNode = doc.first_node("robot", 0, false);
if (!robotXMLNode)
@@ -1577,9 +1756,11 @@ namespace VirtualRobot
catch (rapidxml::parse_error& e)
{
delete[] y;
- THROW_VR_EXCEPTION("Could not parse data in xml definition" << endl
+ THROW_VR_EXCEPTION("Could not parse data in xml definition"
+ << endl
<< "Error message:" << e.what() << endl
- << "Position: " << endl << e.where<char>() << endl);
+ << "Position: " << endl
+ << e.where<char>() << endl);
return RobotPtr();
}
catch (VirtualRobot::VirtualRobotException&)
@@ -1592,7 +1773,8 @@ namespace VirtualRobot
{
delete[] y;
THROW_VR_EXCEPTION("Error while parsing xml definition" << endl
- << "Error code:" << e.what() << endl);
+ << "Error code:" << e.what()
+ << endl);
return RobotPtr();
}
catch (...)
@@ -1613,8 +1795,8 @@ namespace VirtualRobot
return robot;
}
-
- VirtualRobot::RobotPtr RobotIO::loadRobot(const std::string& modelFile, RobotDescription loadMode)
+ VirtualRobot::RobotPtr
+ RobotIO::loadRobot(const std::string& modelFile, RobotDescription loadMode)
{
std::string fullFile = modelFile;
@@ -1625,10 +1807,9 @@ namespace VirtualRobot
{
VR_ERROR << "Could not open " + fileType + " file:" << modelFile << std::endl;
return RobotPtr();
-
}
- if(fileType == "xml")
+ if (fileType == "xml")
{
std::ifstream in(fullFile.c_str());
@@ -1658,9 +1839,8 @@ namespace VirtualRobot
res->applyJointValues();
res->setFilename(fullFile);
return res;
-
}
- else if(fileType == "urdf")
+ else if (fileType == "urdf")
{
SimoxURDFFactory f;
@@ -1677,17 +1857,23 @@ namespace VirtualRobot
VirtualRobot::RobotPtr r = f.loadFromFile(modelFile);
return r;
-
}
else
{
- std::cout << "File does not have correct file Type!" << std::endl;
+ std::cout << "File does not have correct file Type!" << std::endl;
return RobotPtr();
}
}
-
- bool RobotIO::saveXML(RobotPtr robot, const std::string& filename, const std::string& basePath, const std::string& modelDir, bool storeEEF, bool storeRNS, bool storeSensors, bool storeModelFiles)
+ bool
+ RobotIO::saveXML(RobotPtr robot,
+ const std::string& filename,
+ const std::string& basePath,
+ const std::string& modelDir,
+ bool storeEEF,
+ bool storeRNS,
+ bool storeSensors,
+ bool storeModelFiles)
{
THROW_VR_EXCEPTION_IF(!robot, "NULL data");
@@ -1698,9 +1884,11 @@ namespace VirtualRobot
std::filesystem::path fnComplete = p / fn;
std::filesystem::path modelDirComplete = p / pModelDir;
- if (std::filesystem::exists(modelDirComplete) && !std::filesystem::is_directory(modelDirComplete))
+ if (std::filesystem::exists(modelDirComplete) &&
+ !std::filesystem::is_directory(modelDirComplete))
{
- VR_ERROR << "Could not create model directory (existing & !dir) " << pModelDir.string() << std::endl;
+ VR_ERROR << "Could not create model directory (existing & !dir) " << pModelDir.string()
+ << std::endl;
return false;
}
@@ -1708,7 +1896,8 @@ namespace VirtualRobot
{
if (!std::filesystem::create_directories(modelDirComplete))
{
- VR_ERROR << "Could not create model dir " << modelDirComplete.string() << std::endl;
+ VR_ERROR << "Could not create model dir " << modelDirComplete.string()
+ << std::endl;
return false;
}
}
@@ -1721,7 +1910,8 @@ namespace VirtualRobot
return false;
}
- std::string xmlRob = robot->toXML(basePath, modelDir, storeEEF, storeRNS, storeSensors, storeModelFiles);
+ std::string xmlRob =
+ robot->toXML(basePath, modelDir, storeEEF, storeRNS, storeSensors, storeModelFiles);
f << xmlRob;
f.close();
@@ -1738,8 +1928,11 @@ namespace VirtualRobot
return true;
}
- void RobotIO::saveMJCF(RobotPtr robot, const std::string& filename,
- const std::string& basePath, const std::string& meshDir)
+ void
+ RobotIO::saveMJCF(RobotPtr robot,
+ const std::string& filename,
+ const std::string& basePath,
+ const std::string& meshDir)
{
mujoco::RobotMjcf mjcf(robot);
mjcf.setOutputPaths(std::filesystem::path(basePath) / filename, meshDir);
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index ba839184166a5ab0a2690d4c2f574de8d60fb066..ba0f3f7898d47c57e3808cd87a886aa156e98422 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -22,22 +22,20 @@
*/
#pragma once
-#include "../VirtualRobot.h"
-#include "../Robot.h"
-#include "../Nodes/RobotNode.h"
-#include "../EndEffector/EndEffectorActor.h"
-#include "BaseIO.h"
-
+#include <map>
#include <string>
#include <vector>
-#include <map>
-
+#include "../EndEffector/EndEffectorActor.h"
+#include "../Nodes/RobotNode.h"
+#include "../Robot.h"
+#include "../VirtualRobot.h"
+#include "BaseIO.h"
// using forward declarations here, so that the rapidXML header does not have to be parsed when this file is included
namespace rapidxml
{
- template<class Ch>
+ template <class Ch>
class xml_node;
}
@@ -47,7 +45,6 @@ namespace VirtualRobot
class VIRTUAL_ROBOT_IMPORT_EXPORT RobotIO : public BaseIO
{
public:
-
/*!
Loads robot from file.
@param xmlFile The file
@@ -62,7 +59,9 @@ namespace VirtualRobot
@param basePath If any \<childFromRobot\> tags are given, the path for searching the robot files can be specified.
@param loadMode Standard: eFull, When eStructure is used no visualization and collision models are loaded for faster access.
*/
- static RobotPtr createRobotFromString(const std::string& xmlString, const std::string& basePath = "", RobotDescription loadMode = eFull);
+ static RobotPtr createRobotFromString(const std::string& xmlString,
+ const std::string& basePath = "",
+ RobotDescription loadMode = eFull);
/*!
@@ -72,7 +71,14 @@ namespace VirtualRobot
@param basePath The directory to store the robot to
@param modelDir The local directory where all visualization files should be stored to.
*/
- static bool saveXML(RobotPtr robot, const std::string& filename, const std::string& basePath, const std::string& modelDir = "models", bool storeEEF = true, bool storeRNS = true, bool storeSensors = true, bool storeModelFiles = true);
+ static bool saveXML(RobotPtr robot,
+ const std::string& filename,
+ const std::string& basePath,
+ const std::string& modelDir = "models",
+ bool storeEEF = true,
+ bool storeRNS = true,
+ bool storeSensors = true,
+ bool storeModelFiles = true);
/*!
@@ -83,12 +89,12 @@ namespace VirtualRobot
@param meshDir The local directory where all mesh files should be stored to.
*/
static void saveMJCF(RobotPtr robot,
- const std::string& filename, const std::string& basePath,
+ const std::string& filename,
+ const std::string& basePath,
const std::string& meshDir = "mesh");
protected:
-
struct ChildFromRobotDef
{
std::string filename;
@@ -99,42 +105,67 @@ namespace VirtualRobot
RobotIO();
~RobotIO() override;
- static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode, const std::string& basePath, RobotDescription loadMode = eFull);
- static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode, std::string& robotRoot);
- static void processRobotChildNodes(rapidxml::xml_node<char>* robotXMLNode,
- RobotPtr robo,
- const std::string& robotRoot,
- const std::string& basePath,
- std::map < RobotNodePtr,
- std::vector<ChildFromRobotDef> > & childrenFromRobotFilesMap,
- std::vector<rapidxml::xml_node<char>* >& robotNodeSetNodes,
- std::vector<rapidxml::xml_node<char>* >& endeffectorNodes,
- NodeMapping& nodeMapping,
- std::optional<HumanMapping>& humanMapping,
- std::map<std::string, std::vector<std::string>>& attachments,
- RobotDescription loadMode = eFull);
+ static RobotPtr processRobot(rapidxml::xml_node<char>* robotXMLNode,
+ const std::string& basePath,
+ RobotDescription loadMode = eFull);
+ static RobotPtr processRobotAttributes(rapidxml::xml_node<char>* robotXMLNode,
+ std::string& robotRoot);
+ static void processRobotChildNodes(
+ rapidxml::xml_node<char>* robotXMLNode,
+ RobotPtr robo,
+ const std::string& robotRoot,
+ const std::string& basePath,
+ std::map<RobotNodePtr, std::vector<ChildFromRobotDef>>& childrenFromRobotFilesMap,
+ std::vector<rapidxml::xml_node<char>*>& robotNodeSetNodes,
+ std::vector<rapidxml::xml_node<char>*>& endeffectorNodes,
+ NodeMapping& nodeMapping,
+ std::optional<HumanMapping>& humanMapping,
+ std::map<std::string, std::vector<std::string>>& attachments,
+ RobotDescription loadMode = eFull);
static RobotNodePtr processRobotNode(rapidxml::xml_node<char>* robotNodeXMLNode,
RobotPtr robo,
const std::string& basePath,
int& robotNodeCounter,
- std::vector< std::string >& childrenNames,
- std::vector< ChildFromRobotDef >& childrenFromRobot,
+ std::vector<std::string>& childrenNames,
+ std::vector<ChildFromRobotDef>& childrenFromRobot,
RobotDescription loadMode = eFull,
RobotNode::RobotNodeType rntype = RobotNode::Generic);
- static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode, RobotPtr robo);
- static EndEffectorActorPtr processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode, RobotPtr robo);
- static void processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode, RobotPtr robo, std::vector<RobotNodePtr>& staticNodesList);
- static EndEffectorActor::CollisionMode processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes = false);
- static void processActorNodeList(rapidxml::xml_node<char>* parentNode, RobotPtr robot, std::vector<EndEffectorActor::ActorDefinition>& actorList, bool clearList = true);
+ static EndEffectorPtr processEndeffectorNode(rapidxml::xml_node<char>* endeffectorXMLNode,
+ RobotPtr robo);
+ static EndEffectorActorPtr
+ processEndeffectorActorNode(rapidxml::xml_node<char>* endeffectorActorXMLNode,
+ RobotPtr robo);
+ static void processEndeffectorStaticNode(rapidxml::xml_node<char>* endeffectorStaticXMLNode,
+ RobotPtr robo,
+ std::vector<RobotNodePtr>& staticNodesList);
+ static EndEffectorActor::CollisionMode
+ processEEFColAttributes(rapidxml::xml_node<char>* node, bool allowOtherAttributes = false);
+ static void processActorNodeList(rapidxml::xml_node<char>* parentNode,
+ RobotPtr robot,
+ std::vector<EndEffectorActor::ActorDefinition>& actorList,
+ bool clearList = true);
//static RobotNodeSetPtr processRobotNodeSet(rapidxml::xml_node<char> *setXMLNode, RobotPtr robo, const std::string &rootName, int &robotNodeSetCounter);
- static void processChildNode(rapidxml::xml_node<char>* childXMLNode, std::vector<std::string>& childrenNames);
- static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode, const std::string& robotNodeName,
- RobotPtr robot, VisualizationNodePtr visualizationNode, CollisionModelPtr collisionModel,
- SceneObject::Physics& physics, RobotNode::RobotNodeType rntype, Eigen::Matrix4f& transformationMatrix);
- static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode, const std::string& nodeName, std::vector< ChildFromRobotDef >& childrenFromRobot);
- static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool& limitless);
+ static void processChildNode(rapidxml::xml_node<char>* childXMLNode,
+ std::vector<std::string>& childrenNames);
+ static RobotNodePtr processJointNode(rapidxml::xml_node<char>* jointXMLNode,
+ const std::string& robotNodeName,
+ RobotPtr robot,
+ VisualizationNodePtr visualizationNode,
+ CollisionModelPtr collisionModel,
+ SceneObject::Physics& physics,
+ RobotNode::RobotNodeType rntype,
+ Eigen::Matrix4f& transformationMatrix);
+ static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode,
+ const std::string& nodeName,
+ std::vector<ChildFromRobotDef>& childrenFromRobot);
+ static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode,
+ float& jointLimitLo,
+ float& jointLimitHi,
+ bool& limitless);
static std::map<std::string, int> robot_name_counter;
- static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode, const std::string& robotNodeName, const std::string& basePath);
+ static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode,
+ const std::string& robotNodeName,
+ const std::string& basePath);
};
-}
+} // namespace VirtualRobot