From c57051feed43cc8551ebfe0f750935602119427f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 20 Jun 2023 15:09:43 +0200
Subject: [PATCH] Auto format (pure)
---
VirtualRobot/Nodes/RobotNodeHemisphere.cpp | 267 ++++++++++++---------
VirtualRobot/Nodes/RobotNodeHemisphere.h | 113 ++++-----
2 files changed, 198 insertions(+), 182 deletions(-)
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
index d47265446..2e361f0f1 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,32 @@ 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);
+ 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 +144,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 +173,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 +189,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 +205,28 @@ 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 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;
}
}
}
-
- 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 +242,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 +260,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 +273,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 +315,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 +383,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 +421,4 @@ namespace VirtualRobot
return jacobian;
}
-}
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.h b/VirtualRobot/Nodes/RobotNodeHemisphere.h
index 35e05fd44..65a5f8b3e 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.
@@ -73,7 +70,6 @@ namespace VirtualRobot
double lever = -1;
};
-
/// Data held by the first joint.
struct FirstData
{
@@ -97,60 +93,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 +152,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
--
GitLab