From 907f9442e7258a235180a1d6ecceb02fb4fe6faa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andr=C3=A9=20Meixner?= <andre.meixner@kit.edu>
Date: Tue, 21 Feb 2023 19:19:22 +0100
Subject: [PATCH] Changed scaling factor in SceneObject::clone from float to
optional<float>; Added linear scaling of primitive models; Added method for
linear scaling of node size and weight in RobotFactory, e.g. required for the
MMM reference model;
---
VirtualRobot/Nodes/RobotNode.cpp | 14 +++---
VirtualRobot/Nodes/RobotNode.h | 3 +-
VirtualRobot/Primitive.h | 30 +++++++++++++
VirtualRobot/Robot.cpp | 10 ++---
VirtualRobot/Robot.h | 9 ++--
VirtualRobot/RobotFactory.cpp | 74 ++++++++++++++++++++++++++++++++
VirtualRobot/RobotFactory.h | 15 +++++++
VirtualRobot/SceneObject.cpp | 16 +++++++
VirtualRobot/SceneObject.h | 2 +
9 files changed, 156 insertions(+), 17 deletions(-)
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 8298691bf..077b2d7ab 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -538,11 +538,11 @@ namespace VirtualRobot
RobotNodePtr RobotNode::clone(RobotPtr newRobot, bool cloneChildren,
RobotNodePtr initializeWithParent,
CollisionCheckerPtr colChecker,
- float scaling,
+ std::optional<float> scaling,
bool preventCloningMeshesIfScalingIs1)
{
- // If scaling is <= 0 this->scaling is used instead. This enables different scalings while still able to clone the robot
- auto actualScaling = scaling > 0 ? scaling : this->scaling;
+ const float actualScaling = scaling.value_or(this->scaling);
+ const float scalingFactor = scaling.has_value() ? scaling.value() / this->scaling : 1.0f;
ReadLockPtr lock = getRobot()->getReadLock();
@@ -556,20 +556,20 @@ namespace VirtualRobot
VisualizationNodePtr clonedVisualizationNode;
- const bool deepMeshClone = !preventCloningMeshesIfScalingIs1 || std::abs(scaling - 1) <= 0;
+ const bool deepMeshClone = !preventCloningMeshesIfScalingIs1 || std::abs(scalingFactor - 1) <= 1e-12;
if (visualizationModel)
{
- clonedVisualizationNode = visualizationModel->clone(deepMeshClone, actualScaling);
+ clonedVisualizationNode = visualizationModel->clone(deepMeshClone, scalingFactor);
}
CollisionModelPtr clonedCollisionModel;
if (collisionModel)
{
- clonedCollisionModel = collisionModel->clone(colChecker, actualScaling, deepMeshClone);
+ clonedCollisionModel = collisionModel->clone(colChecker, scalingFactor, deepMeshClone);
}
- RobotNodePtr result = _clone(newRobot, clonedVisualizationNode, clonedCollisionModel, colChecker, scaling > 0 ? scaling : 1.0f);
+ RobotNodePtr result = _clone(newRobot, clonedVisualizationNode, clonedCollisionModel, colChecker, scalingFactor);
if (!result)
{
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 454973471..1e2c57121 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -32,6 +32,7 @@
#include <Eigen/Core>
+#include <optional>
#include <string>
#include <vector>
#include <map>
@@ -271,7 +272,7 @@ namespace VirtualRobot
bool cloneChildren = true,
RobotNodePtr initializeWithParent = RobotNodePtr(),
CollisionCheckerPtr colChecker = CollisionCheckerPtr(),
- float scaling = 1.0f,
+ std::optional<float> scaling = std::nullopt,
bool preventCloningMeshesIfScalingIs1 = false);
inline float getJointValueOffset() const
diff --git a/VirtualRobot/Primitive.h b/VirtualRobot/Primitive.h
index 9d03fef48..0977163be 100644
--- a/VirtualRobot/Primitive.h
+++ b/VirtualRobot/Primitive.h
@@ -22,6 +22,8 @@ namespace VirtualRobot
virtual std::unique_ptr<Primitive> clone() const = 0;
+ virtual void scaleLinear(float scalingFactor) = 0;
+
protected:
Primitive(int type) : type(type), transform(Eigen::Matrix4f::Identity()) {}
std::string getTransformString(int tabs = 0);
@@ -47,6 +49,14 @@ namespace VirtualRobot
clone->transform = transform;
return clone;
}
+
+ void scaleLinear(float scalingFactor) final
+ {
+ transform.block(0, 3, 3, 1) *= scalingFactor;
+ width *= scalingFactor;
+ height *= scalingFactor;
+ depth *= scalingFactor;
+ }
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Sphere : public Primitive
@@ -64,6 +74,12 @@ namespace VirtualRobot
clone->transform = transform;
return clone;
}
+
+ void scaleLinear(float scalingFactor) final
+ {
+ transform.block(0, 3, 3, 1) *= scalingFactor;
+ radius *= scalingFactor;
+ }
};
class VIRTUAL_ROBOT_IMPORT_EXPORT Cylinder : public Primitive
@@ -82,6 +98,13 @@ namespace VirtualRobot
clone->transform = transform;
return clone;
}
+
+ void scaleLinear(float scalingFactor) final
+ {
+ transform.block(0, 3, 3, 1) *= scalingFactor;
+ height *= scalingFactor;
+ radius *= scalingFactor;
+ }
};
/**
@@ -103,6 +126,13 @@ namespace VirtualRobot
clone->transform = transform;
return clone;
}
+
+ void scaleLinear(float scalingFactor) final
+ {
+ transform.block(0, 3, 3, 1) *= scalingFactor;
+ height *= scalingFactor;
+ radius *= scalingFactor;
+ }
};
typedef std::shared_ptr<Primitive> PrimitivePtr;
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index ea1b6d6e6..86643243d 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -766,7 +766,7 @@ namespace VirtualRobot
bool cloneRNS,
bool cloneEEFs,
CollisionCheckerPtr collisionChecker,
- float scaling,
+ std::optional<float> scaling,
bool preventCloningMeshesIfScalingIs1)
{
THROW_VR_EXCEPTION_IF(!startJoint, " StartJoint is nullptr");
@@ -787,7 +787,7 @@ namespace VirtualRobot
RobotNodePtr rootNew = startJoint->clone(result, true, RobotNodePtr(), colChecker, scaling, preventCloningMeshesIfScalingIs1);
THROW_VR_EXCEPTION_IF(!rootNew, "Clone failed...");
result->setRootNode(rootNew);
- result->setScaling(scaling > 0 ? scaling : this->scaling);
+ result->setScaling(scaling.value_or(this->scaling));
std::vector<RobotNodePtr> rn = result->getRobotNodes();
@@ -849,7 +849,7 @@ namespace VirtualRobot
}
RobotPtr Robot::cloneScaling() {
- return clone(getName(), CollisionCheckerPtr(), -1.0f);
+ return clone();
}
Eigen::Matrix4f Robot::getGlobalPoseForRobotNode(
@@ -905,7 +905,7 @@ namespace VirtualRobot
VirtualRobot::RobotPtr Robot::clone(const std::string& name,
CollisionCheckerPtr collisionChecker,
- float scaling,
+ std::optional<float> scaling,
bool preventCloningMeshesIfScalingIs1)
{
VirtualRobot::RobotPtr result = extractSubPart(this->getRootNode(),
@@ -929,7 +929,7 @@ namespace VirtualRobot
}
RobotPtr Robot::clone(CollisionCheckerPtr collisionChecker,
- float scaling,
+ std::optional<float> scaling,
bool preventCloningMeshesIfScalingIs1)
{
return clone(getName(), collisionChecker, scaling, preventCloningMeshesIfScalingIs1);
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 1ab3255d1..868c00deb 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -303,7 +303,7 @@ namespace VirtualRobot
bool cloneRNS = true,
bool cloneEEFs = true,
CollisionCheckerPtr collisionChecker = CollisionCheckerPtr(),
- float scaling = 1.0f,
+ std::optional<float> scaling = std::nullopt,
bool preventCloningMeshesIfScalingIs1 = false);
/*!
@@ -315,13 +315,14 @@ namespace VirtualRobot
*/
virtual RobotPtr clone(const std::string& name,
CollisionCheckerPtr collisionChecker = CollisionCheckerPtr(),
- float scaling = 1.0f,
+ std::optional<float> scaling = std::nullopt,
bool preventCloningMeshesIfScalingIs1 = false);
virtual RobotPtr clone(CollisionCheckerPtr collisionChecker = CollisionCheckerPtr(),
- float scaling = 1.0f,
+ std::optional<float> scaling = std::nullopt,
bool preventCloningMeshesIfScalingIs1 = false);
- /*! @brief Clones this robot and keeps the current scaling for the robot and each robot node */
+ /*! @brief Clones this robot and keeps the current scaling for the robot and each robot node. */
+ [[deprecated("Use clone() instead. Which now contains scaling as an optional.")]]
virtual RobotPtr cloneScaling();
//! Just storing the filename.
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index e49afc9cb..22869b7ff 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -344,6 +344,80 @@ namespace VirtualRobot
return true;
}
+ void RobotFactory::scaleLinear(RobotNode &node, float sizeScaling, float weightScaling)
+ {
+ Eigen::Matrix4f localTransformation = node.getLocalTransformation();
+ localTransformation.block(0, 3, 3, 1) *= sizeScaling;
+ node.setLocalTransformation(localTransformation);
+
+ if (node.optionalDHParameter.isSet)
+ {
+ node.optionalDHParameter.setAInMM(node.optionalDHParameter.aMM() * sizeScaling);
+ node.optionalDHParameter.setDInMM(node.optionalDHParameter.dMM() * sizeScaling);
+ }
+
+ node.setMass(node.getMass() * weightScaling);
+ node.setCoMLocal(node.getCoMLocal() * sizeScaling);
+ node.setInertiaMatrix(node.getInertiaMatrix() * pow(sizeScaling, 2) * weightScaling);
+ node.setScaling(node.getScaling() * sizeScaling);
+ }
+
+ void RobotFactory::scaleLinear(Robot &robot, float sizeScaling, float weightScaling,
+ const std::map<std::string, float> &customSegmentLengths,
+ const std::map<std::string, float>& customSizeScaling)
+ {
+ for (auto node : robot.getRobotNodes())
+ {
+ float model_height_scaling = sizeScaling;
+
+ const auto segLengthIt = customSegmentLengths.find(node->getName());
+ const auto sizeScaleIt = customSizeScaling.find(node->getName());
+
+ if (sizeScaleIt != customSizeScaling.end())
+ {
+ model_height_scaling = sizeScaleIt->second;
+ }
+
+ if (segLengthIt != customSegmentLengths.end())
+ {
+ if (sizeScaleIt != customSizeScaling.end())
+ {
+ VR_WARNING << "Custom segment length ignored for node " << node->getName()
+ << " because custom height scaling was already specified ";
+ }
+ else
+ {
+ const float l = node->getLocalTransformation().block(0, 3, 3, 1).norm();
+ if (l != 0.0f)
+ model_height_scaling = 1.0f / l * segLengthIt->second;
+ }
+ }
+
+ for (auto sensor : node->getSensors())
+ {
+ Eigen::Matrix4f lt = sensor->getParentNodeToSensorTransformation();
+ lt.block(0, 3, 3, 1) *= model_height_scaling;
+ sensor->setRobotNodeToSensorTransformation(lt);
+ }
+
+ scaleLinear(*node.get(), model_height_scaling, weightScaling);
+
+ if (auto vis = node->visualizationModel)
+ {
+ node->setVisualization(node->visualizationModel->clone(true, model_height_scaling));
+ }
+
+ if (auto col = node->getCollisionModel())
+ {
+ node->setCollisionModel(col->clone(node->getCollisionChecker(), model_height_scaling, true));
+ }
+
+ node->getPrimitiveApproximation().scaleLinear(model_height_scaling);
+ }
+ robot.setScaling(robot.getScaling() * sizeScaling);
+ robot.setMass(robot.getMass() * weightScaling);
+ }
+
RobotPtr RobotFactory::cloneChangeStructure(RobotPtr robot, robotStructureDef& newStructure)
{
VR_ASSERT(robot);
diff --git a/VirtualRobot/RobotFactory.h b/VirtualRobot/RobotFactory.h
index 0b5303446..e73cbe19f 100644
--- a/VirtualRobot/RobotFactory.h
+++ b/VirtualRobot/RobotFactory.h
@@ -147,6 +147,21 @@ namespace VirtualRobot
static bool detach(RobotPtr robot, RobotNodePtr rn);
+ static void scaleLinear(RobotNode& node, float sizeScaling, float weightScaling = 1.);
+
+ /**
+ * Method to linearly scale the size (in all axes) and the weight of the robot, e.g. for the MMM reference model.
+ * This includes scaling all kinematic and dynamic properties as well as the visualization, collision and primitive approximation models.
+ * @param robot
+ * @param sizeScaling linear scaling factor in x,y,z-direction
+ * @param weightScaling linear weight factor
+ * @param customSegmentLengths (Optional) RobotNode name -> custom translation length. Takes priority over sizeScaling
+ * @param customSizeScaling (Optional) RobotNode name -> custom size scaling. Takes priority over customSegmentLengths.
+ */
+ static void scaleLinear(Robot& robot, float sizeScaling, float weightScaling = 1.,
+ const std::map<std::string, float>& customSegmentLengths = std::map<std::string, float>(),
+ const std::map<std::string, float>& customSegmentSizeScaling = std::map<std::string, float>());
+
protected:
// some internal stuff
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 734ec4739..96925f44c 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -1662,4 +1662,20 @@ namespace VirtualRobot
return defaultPrimitives.empty() && primitives.empty();
}
+ void SceneObject::PrimitiveApproximation::scaleLinear(float scalingFactor)
+ {
+ for (auto& primitive : this->defaultPrimitives)
+ {
+ primitive->scaleLinear(scalingFactor);
+ }
+
+ for (auto& [id, primitives] : this->primitives)
+ {
+ for (auto& primitive : primitives)
+ {
+ primitive->scaleLinear(scalingFactor);
+ }
+ }
+ }
+
} // namespace
diff --git a/VirtualRobot/SceneObject.h b/VirtualRobot/SceneObject.h
index aefd37fc1..3fb7348e3 100644
--- a/VirtualRobot/SceneObject.h
+++ b/VirtualRobot/SceneObject.h
@@ -105,6 +105,8 @@ namespace VirtualRobot
bool empty() const;
+ void scaleLinear(float scalingFactor);
+
private:
std::vector<Primitive::PrimitivePtr> defaultPrimitives;
std::map<std::string, std::vector<Primitive::PrimitivePtr>> primitives;
--
GitLab