diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 3ce4720491a26f54baf895e14ea1feb5fe3aa5da..3f6bfc013b4e7043e83de1a94a92ef9b85ae7484 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -64,7 +64,7 @@ stages:
- cd "$PROJECT_PATH_IN_WORKSPACE/build"
- ctest . || true
- ctest --rerun-failed --output-on-failure . || true
- - cat Temporary/LastTest.log || true
+ - cat Testing/Temporary/LastTest.log || true
# Once again to make the job fail if an error occurs.
- ctest .
diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index dabe7e348b3bcbefd3e4f382c1ef1252cacaa65a..f124608da17e0d11f1e8b18cc4b1fe82d11975a3 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -43,6 +43,9 @@ namespace simox::geometric_planning
const auto node = articulatedObject->getRobotNode(nodeName);
REQUIRE(node != nullptr);
+ const auto global_T_object_root = articulatedObject->getGlobalPose();
+ articulatedObject->setGlobalPose(Eigen::Matrix4f::Identity());
+
simox::geometric_planning::ArticulatedObjectGeometricPlanningHelper helper(
articulatedObject);
@@ -84,6 +87,10 @@ namespace simox::geometric_planning
const auto parametricPath = helper.getPathForNode(node->getName(), joint->getName());
+ // reset global pose
+ articulatedObject->setGlobalPose(global_T_object_root);
+
+
return parametricPath;
}
diff --git a/GeometricPlanning/ParametricPath.cpp b/GeometricPlanning/ParametricPath.cpp
index bd5e81e9e4cf9616508bb4c478e9a1de35b68744..9c9de69fdb92049c238af7594ed571f5fe241eac 100644
--- a/GeometricPlanning/ParametricPath.cpp
+++ b/GeometricPlanning/ParametricPath.cpp
@@ -19,6 +19,11 @@ namespace simox::geometric_planning
{
return path->progress(toLocalPathFrame(global_T_pose));
}
+
+ float ParametricPath::progress(const float param) const
+ {
+ return path->progress(param);
+ }
Pose
ParametricPath::toLocalPathFrame(const Pose& global_T_pose) const
diff --git a/GeometricPlanning/ParametricPath.h b/GeometricPlanning/ParametricPath.h
index ec58679d9b4d4e33f3a5eb5ce083641b9cd874be..dd2400f85fce57eef08bee5d698c98b6115f76ca 100644
--- a/GeometricPlanning/ParametricPath.h
+++ b/GeometricPlanning/ParametricPath.h
@@ -32,6 +32,7 @@ namespace simox::geometric_planning
// helper functions to obtain the PathPrimitive's parameter and progress directly from a global pose
float progress(const Pose& global_T_pose) const;
+ float progress(float param) const;
Pose toLocalPathFrame(const Pose& global_T_pose) const;
diff --git a/GeometricPlanning/path_primitives/Circle.cpp b/GeometricPlanning/path_primitives/Circle.cpp
index 0f074f8c6bb5df4b267584e938a1706398cf20fd..55137eef3b4f4d3eca7c206b72c7a722ac08852f 100644
--- a/GeometricPlanning/path_primitives/Circle.cpp
+++ b/GeometricPlanning/path_primitives/Circle.cpp
@@ -23,7 +23,9 @@ namespace simox::geometric_planning
Eigen::Vector3f
Circle::getPosition(float t) const
{
- REQUIRE(parameterRange().isInRange(t));
+ // REQUIRE(parameterRange().isInRange(t));
+
+ t = clampParameter(t);
return radius * Eigen::Vector3f(std::cos(t), std::sin(t), 0.0F);
}
@@ -31,7 +33,8 @@ namespace simox::geometric_planning
Eigen::Vector3f
Circle::getPositionDerivative([[maybe_unused]] float t) const
{
- REQUIRE(parameterRange().isInRange(t));
+ // REQUIRE(parameterRange().isInRange(t));
+
// return radius * Eigen::Vector3f(-std::sin(t + M_PI_2f32), std::cos(t + M_PI_2f32), 0.0F);
@@ -80,7 +83,7 @@ namespace simox::geometric_planning
const float param = phi;
// ARMARX_DEBUG << "Param is " << param;
- return std::clamp(param, parameterRange().min, parameterRange().max);
+ return clampParameter(param);
}
} // namespace simox::geometric_planning
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.cpp b/GeometricPlanning/path_primitives/PathPrimitive.cpp
index d78fb0e528b454c1547fa548a596c3f4403a3c76..a00ada676a913c29d879a09eac87ac0371054b3c 100644
--- a/GeometricPlanning/path_primitives/PathPrimitive.cpp
+++ b/GeometricPlanning/path_primitives/PathPrimitive.cpp
@@ -16,9 +16,23 @@ namespace simox::geometric_planning
PathPrimitive::progress(const Pose& pose) const
{
const float param = parameter(pose);
+ return progress(param);
+ }
+
+ float PathPrimitive::progress(const float param) const
+ {
const auto range = parameterRange();
- return (param - range.min) / (range.max - range.min);
+ const float paramSanitized = clampParameter(param);
+
+ return (paramSanitized - range.min) / (range.max - range.min);
+ }
+
+
+ float PathPrimitive::clampParameter(float t) const
+ {
+ const auto range = parameterRange();
+ return std::clamp(t, range.min, range.max);
}
Pose
@@ -26,6 +40,7 @@ namespace simox::geometric_planning
{
return Pose(::math::Helpers::CreatePose(getPosition(t), getOrientation(t)));
}
+
Eigen::Vector3f
PathPrimitive::GetPosition(float t)
diff --git a/GeometricPlanning/path_primitives/PathPrimitive.h b/GeometricPlanning/path_primitives/PathPrimitive.h
index e5e81ae1120548cc4387cb777719f198598ecb68..48aa40203189c1085bb4d3b16256c5111a0c1199 100644
--- a/GeometricPlanning/path_primitives/PathPrimitive.h
+++ b/GeometricPlanning/path_primitives/PathPrimitive.h
@@ -36,6 +36,7 @@ namespace simox::geometric_planning
* @return the progress in range [0,1]
*/
float progress(const Pose& pose) const;
+ float progress(float param) const;
// math::AbstractFunctionR1R6 interface
Eigen::Vector3f GetPosition(float t) override;
@@ -51,6 +52,8 @@ namespace simox::geometric_planning
Pose getPose(float t) const;
+ float clampParameter(float t) const;
+
~PathPrimitive() override = default;
};
diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index b70bba190863d7a20eb606f6fe7ec7daef499774..23f6b730846254277032f61eafcb1307ce698aee 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -390,11 +390,12 @@ namespace VirtualRobot
// Calculus for rotational joints is different as for prismatic joints.
if (dof->isRotationalJoint())
{
- // get axis
+ // todo: find a better way of handling different joint types
std::shared_ptr<RobotNodeRevolute> revolute
= std::dynamic_pointer_cast<RobotNodeRevolute>(dof);
THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
- // todo: find a better way of handling different joint types
+
+ // get axis
axis = revolute->getJointRotationAxis(coordSystem);
// if necessary calculate the position part of the Jacobian
@@ -452,18 +453,31 @@ namespace VirtualRobot
RobotNodeHemispherePtr hemisphere
= std::dynamic_pointer_cast<RobotNodeHemisphere>(dof);
- VR_ASSERT(hemisphere->first.has_value() xor hemisphere->second.has_value());
-
if (hemisphere->isSecondHemisphereJointNode())
{
// Set Jacobian for both DoFs.
RobotNodeHemisphere::SecondData& second = hemisphere->getSecondData();
const hemisphere::Maths::Jacobian jacobian = second.getJacobian();
- tmpUpdateJacobianPosition.block<3, 2>(0, i - 1)
- = jacobian.block<3, 2>(0, 0).cast<float>();
- tmpUpdateJacobianOrientation.block<3, 2>(0, i - 1)
- = jacobian.block<3, 2>(3, 0).cast<float>();
+ using Matrix3x2f = Eigen::Matrix<float, 3, 2>;
+
+ const Matrix3x2f velocities = jacobian.block<3, 2>(0, 0).cast<float>();
+ const Matrix3x2f rotAxes = jacobian.block<3, 2>(3, 0).cast<float>();
+
+ const Eigen::Matrix3f globalOri = second.firstNode->getGlobalOrientation();
+
+ Matrix3x2f velocitiesCoord = globalOri * velocities;
+ Matrix3x2f rotAxesCoord = globalOri * rotAxes;
+ if (coordSystem)
+ {
+ auto coordSystemInverseGlobalRot = coordSystem->getGlobalOrientation().transpose();
+
+ velocitiesCoord = coordSystemInverseGlobalRot * velocitiesCoord;
+ rotAxesCoord = coordSystemInverseGlobalRot * rotAxesCoord;
+ }
+
+ tmpUpdateJacobianPosition.block<3, 2>(0, i - 1) = velocitiesCoord;
+ tmpUpdateJacobianOrientation.block<3, 2>(0, i - 1) = rotAxesCoord;
}
else
{
diff --git a/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.cpp b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.cpp
index 329648250c4683cdc13bb76f2b9411451112c64a..6723df86c5c7bfc53842691dd4bf289e4d8dd4c9 100755
--- a/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.cpp
+++ b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.cpp
@@ -17,6 +17,7 @@
*/
#include "OmniWheelPlatformKinematics.h"
+#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
@@ -44,15 +45,11 @@ namespace VirtualRobot
Eigen::Matrix3f
OmniWheelPlatformKinematicsParams::C() const
{
- Eigen::Matrix3f r = Eigen::Vector3f{-1, 1, 1}.asDiagonal();
-
// Rotation around platform center to define which direction is front
- // For ARMAR-7, this is 60 degrees (between the two "front" wheels).
- float relativeAngle = M_PI / 3.0f;
Eigen::Matrix3f relativeRotation = Eigen::Matrix3f::Identity();
relativeRotation.block<2, 2>(0, 0) = Eigen::Rotation2Df(relativeAngle).toRotationMatrix();
- return relativeRotation * r * B().transpose().inverse() * R / n;
+ return relativeRotation * wheelFactor.asDiagonal() * B().transpose().inverse() * 2 * M_PI * R / n;
}
// OmniWheelPlatformKinematics
diff --git a/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h
index 2262d39703b5ffc70371f6983e135c0310363991..7df2cd739763c8ff71097307213674e4a358df08 100755
--- a/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h
+++ b/VirtualRobot/IK/platform/OmniWheelPlatformKinematics.h
@@ -49,6 +49,13 @@ namespace VirtualRobot
//! gear ratio
float n;
+ //! rotation to reference
+ // Rotation around platform center to define which direction is front
+ // For ARMAR-7, this is 60 degrees (between the two "front" wheels).
+ float relativeAngle;
+
+ Eigen::Vector3f wheelFactor;
+
// see Liu et al., formula 2.2.2
Eigen::Matrix3f B() const;
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 8298691bfc22221a3daf83ba7a676d53841392a0..077b2d7ab09ebf176ca677d959e82ffc2a9abc17 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 454973471658e6eecc0698d000484b2e83b69dc8..1e2c571211c3c0b099b9970f686ae0ffa414be21 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/Nodes/RobotNodeFourBar.cpp b/VirtualRobot/Nodes/RobotNodeFourBar.cpp
index a70581dd9fec393485ec191d9d01234d0a67c417..84c486ef0b22430e8ddfab4aeeba9f07f0c5d5fc 100644
--- a/VirtualRobot/Nodes/RobotNodeFourBar.cpp
+++ b/VirtualRobot/Nodes/RobotNodeFourBar.cpp
@@ -125,7 +125,7 @@ namespace VirtualRobot
void
RobotNodeFourBar::setJointValueNoUpdate(float q)
{
- std::cout << "RobotNodeFourBar: setting joint value no update " << q << std::endl;
+ // std::cout << "RobotNodeFourBar: setting joint value no update " << q << std::endl;
if (active)
{
@@ -146,9 +146,9 @@ namespace VirtualRobot
{
// We must update the preceeding node (the passive node).
// This usually causes issues as the order to update the kinematic chain is strict.
- std::cout << "RobotNodeFourBar: setting joint value " << getName() << " " << q << std::endl;
+ // std::cout << "RobotNodeFourBar: setting joint value " << getName() << " " << q << std::endl;
- std::cout << "RobotNodeFourBar: active? " << active.has_value() << std::endl;
+ // std::cout << "RobotNodeFourBar: active? " << active.has_value() << std::endl;
// update this node (without the global / internal pose!)
{
@@ -160,7 +160,7 @@ namespace VirtualRobot
if (active)
{
- std::cout << "RobotNodeFourBar: triggering update of passive joint " << std::endl;
+ // std::cout << "RobotNodeFourBar: triggering update of passive joint " << std::endl;
// update all nodes including this one
active->passive->updatePose(true);
@@ -176,7 +176,7 @@ namespace VirtualRobot
{
this->xmlInfo = info;
- VR_ASSERT(second.has_value());
+ VR_ASSERT(active.has_value());
switch (info.role)
{
case Role::PASSIVE:
@@ -199,16 +199,16 @@ namespace VirtualRobot
bool
RobotNodeFourBar::initialize(SceneObjectPtr parent, const std::vector<SceneObjectPtr>& children)
{
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(),
- std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
- // The second node needs to store a reference to the first node.
+ // The active node needs to store a reference to the first node.
// Whenever the joint value has changed, the passive joint will be updated.
if (active)
{
- std::cout << "Initializing active four bar joint" << std::endl;
+ // std::cout << "Initializing active four bar joint" << std::endl;
- VR_ASSERT_MESSAGE(not second->first, "Second must not be initialized yet.");
+ VR_ASSERT_MESSAGE(not active->passive, "Second must not be initialized yet.");
VirtualRobot::SceneObjectPtr currentParent = parent;
@@ -281,10 +281,10 @@ namespace VirtualRobot
void
RobotNodeFourBar::updateTransformationMatrices(const Eigen::Matrix4f& parentPose)
{
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(),
- std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
- std::cout << "Updating RobotNodeFourBar::updateTransformationMatrices" << std::endl;
+ // std::cout << "Updating RobotNodeFourBar::updateTransformationMatrices" << std::endl;
Eigen::Isometry3f tmp = Eigen::Isometry3f::Identity();
@@ -292,21 +292,21 @@ namespace VirtualRobot
if (active)
{
- std::cout << "active: joint value " << jV << std::endl;
+ // std::cout << "active: joint value " << jV << std::endl;
active->math.update(jV);
tmp = active->math.joint.computeFk(jV).matrix().cast<float>();
}
else // passive
{
- std::cout << "passive: joint value " << jV << std::endl;
+ // std::cout << "passive: joint value " << jV << std::endl;
tmp.linear() = Eigen::AngleAxisf(jV + jointValueOffset, Eigen::Vector3f::UnitZ())
.toRotationMatrix();
}
- std::cout << "local transformation: " << getName() << tmp.matrix() << std::endl;
+ // std::cout << "local transformation: " << getName() << tmp.matrix() << std::endl;
globalPose = parentPose * localTransformation * tmp.matrix();
@@ -316,7 +316,7 @@ namespace VirtualRobot
// }
// else if (active)
// {
- // VR_ASSERT_MESSAGE(second->first, "First node must be known to second node.");
+ // VR_ASSERT_MESSAGE(active->first, "First node must be known to active node.");
// JointMath& math = active->math();
// Eigen::Vector2f actuators(active->passive->getJointValue(), this->getJointValue());
@@ -333,7 +333,7 @@ namespace VirtualRobot
// Eigen::IOFormat iof(5, 0, " ", "\n", " [", "]");
// std::cout
- // << __FUNCTION__ << "() of second actuator with "
+ // << __FUNCTION__ << "() of active actuator with "
// << "\n lever = " << math.joint.lever << "\n theta0 = " << math.joint.theta0
// << "\n radius = " << math.joint.radius << "\n joint value = " << jointValue
// << "\n actuator (angle) = \n"
@@ -351,8 +351,8 @@ namespace VirtualRobot
RobotNodeFourBar::print(bool printChildren, bool printDecoration) const
{
ReadLockPtr lock = getRobot()->getReadLock();
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(),
- std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
if (printDecoration)
{
@@ -367,7 +367,7 @@ namespace VirtualRobot
}
else if (active)
{
- std::cout << "* four_bar joint second node";
+ std::cout << "* four_bar joint active node";
// std::cout << "* Transform: \n"
// << active->math.joint.getEndEffectorTransform() << std::endl;
}
@@ -519,8 +519,8 @@ namespace VirtualRobot
std::string
RobotNodeFourBar::_toXML(const std::string& /*modelPath*/)
{
- VR_ASSERT_MESSAGE(first.has_value() xor second.has_value(),
- std::stringstream() << first.has_value() << " / " << second.has_value());
+ VR_ASSERT_MESSAGE(first.has_value() xor active.has_value(),
+ std::stringstream() << first.has_value() << " / " << active.has_value());
if (first)
{
diff --git a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
index a2d999867affb1b8adf6f5d79ffc12675ef5607b..d472654467060a64d9de011afd00050be2575c48 100644
--- a/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
+++ b/VirtualRobot/Nodes/RobotNodeHemisphere.cpp
@@ -127,7 +127,7 @@ namespace VirtualRobot
// The second node needs to store a reference to the first node.
if (secondData)
{
- VR_ASSERT_MESSAGE(not secondData->first, "Second must not be initialized yet.");
+ VR_ASSERT_MESSAGE(not secondData->firstNode, "Second must not be initialized yet.");
RobotNodeHemisphere* firstNode = dynamic_cast<RobotNodeHemisphere*>(parent.get());
RobotNodeHemisphere* secondNode = this;
@@ -171,7 +171,7 @@ namespace VirtualRobot
}
else if (secondData)
{
- VR_ASSERT_MESSAGE(secondData->first, "First node must be known to second node.");
+ 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());
diff --git a/VirtualRobot/Primitive.h b/VirtualRobot/Primitive.h
index 9d03fef485396abd4c647cdfce38adde4fa8a806..0977163be8d85009654d7c94df33440cf04017a2 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 ea1b6d6e698d919d9b0d8c1de7941b063238a20c..86643243dbec2439c1c739517da975ce66d0dd23 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 1ab3255d1128438ff71ff27369eefe55df24a82d..868c00deb126dcac31e13277eb3f37cfef1e549c 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 e49afc9cb4e22e1fccceaf54eb3987e565d74434..025904d0df8f18d24a4cd0f13f03d3670547fbf1 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);
@@ -1110,32 +1184,47 @@ namespace VirtualRobot
const Eigen::Matrix4f globalPoseInv = simox::math::inverted_pose(n.node_cloned->getGlobalPose());
if (!visus.empty())
{
- if (const auto visu = n.node->getVisualization())
+ if (visus.size() == 1)
{
- visus.insert(visus.begin(), visu->clone());
+ n.node_cloned->setVisualization(visus.front()->clone());
}
- auto v = vf->createUnitedVisualization(visus);
- if (n.parentNode_cloned)
+ else
{
- vf->applyDisplacement(v, globalPoseInv);
+ if (const auto visu = n.node->getVisualization())
+ {
+ visus.insert(visus.begin(), visu->clone());
+ }
+ auto v = vf->createUnitedVisualization(visus);
+ if (n.parentNode_cloned)
+ {
+ v->setLocalPose(globalPoseInv);
+ }
+ n.node_cloned->setVisualization(v);
}
- n.node_cloned->setVisualization(v);
}
if (!colVisus.empty())
{
- if (const auto colModel = n.node->getCollisionModel())
+ if (colVisus.size() == 1)
{
- if (const auto vis = colModel->getVisualization())
- {
- colVisus.insert(colVisus.begin(), vis->clone());
- }
+ n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisus.front(), n.node->getName()));
}
- auto colVisu = vf->createUnitedVisualization(colVisus);
- if (n.parentNode_cloned)
+ else
{
- vf->applyDisplacement(colVisu, globalPoseInv);
+ if (const auto colModel = n.node->getCollisionModel())
+ {
+ if (const auto vis = colModel->getVisualization())
+ {
+ colVisus.insert(colVisus.begin(), vis->clone());
+ }
+ }
+ auto colVisu = vf->createUnitedVisualization(colVisus);
+ if (n.parentNode_cloned)
+ {
+ colVisu->setLocalPose(globalPoseInv);
+ }
+ n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisu, n.node->getName()));
}
- n.node_cloned->setCollisionModel(std::make_shared<CollisionModel>(colVisu, n.node->getName()));
+
}
for (const auto& childNode : n.childNodes)
diff --git a/VirtualRobot/RobotFactory.h b/VirtualRobot/RobotFactory.h
index 0b5303446453bcd3bf55baee5bffc92b7308374a..e73cbe19fe49bbe0680d8ff24d5f820fb391fd69 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 734ec473976bfd677d1607a1853d0a6e8c878e24..1466ed3e9d7bc6c6538f2783ddd5352d1f483840 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -1464,7 +1464,7 @@ namespace VirtualRobot
return scaling;
}
- bool SceneObject::reloadVisualizationFromXML(bool useVisAsColModelIfMissing) {
+ bool SceneObject::reloadVisualizationFromXML(bool useVisAsColModelIfMissing, bool loadColOnly) {
bool reloaded = false;
if (!collisionModelXML.empty())
{
@@ -1484,7 +1484,8 @@ namespace VirtualRobot
}
reloaded = true;
}
- if (!visualizationModelXML.empty())
+ if ((!loadColOnly || (!collisionModel && useVisAsColModelIfMissing))
+ && !visualizationModelXML.empty())
{
rapidxml::xml_document<> doc;
std::vector<char> cstr(visualizationModelXML.size() + 1); // Create char buffer to store string copy
@@ -1492,13 +1493,16 @@ namespace VirtualRobot
doc.parse<0>(cstr.data());
bool useAsColModel;
auto visualizationModel = BaseIO::processVisualizationTag(doc.first_node(), name, basePath, useAsColModel);
- if (visualizationModel && scaling != 1.0f)
+ if (!loadColOnly)
{
- setVisualization(visualizationModel->clone(true, scaling));
- }
- else
- {
- setVisualization(visualizationModel);
+ if (visualizationModel && scaling != 1.0f)
+ {
+ setVisualization(visualizationModel->clone(true, scaling));
+ }
+ else
+ {
+ setVisualization(visualizationModel);
+ }
}
if (visualizationModel && collisionModel == nullptr && (useVisAsColModelIfMissing || useAsColModel))
{
@@ -1507,7 +1511,7 @@ namespace VirtualRobot
reloaded = true;
}
for (auto child : this->getChildren()) {
- reloaded |= child->reloadVisualizationFromXML(useVisAsColModelIfMissing);
+ reloaded |= child->reloadVisualizationFromXML(useVisAsColModelIfMissing, loadColOnly);
}
return reloaded;
}
@@ -1662,4 +1666,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 aefd37fc17badf91da02a82b8691284a7be07e23..5572e1cae677c92020227a9095f44536de205ced 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;
@@ -418,7 +420,7 @@ namespace VirtualRobot
void setScaling(float scaling);
float getScaling();
- bool reloadVisualizationFromXML(bool useVisAsColModelIfMissing = true);
+ bool reloadVisualizationFromXML(bool useVisAsColModelIfMissing = true, bool loadColOnly = false);
const std::string& getVisualizationModelXML() const;
diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
index 7a4e74191658a561faadbe05d921508a8340f759..b41899e0af4952759437b26221807efda5acc983 100644
--- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
+++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp
@@ -4076,8 +4076,6 @@ namespace VirtualRobot
{
SoSeparator* ss = new SoSeparator();
- SoUnits* u = new SoUnits();
- u->units = SoUnits::MILLIMETERS;
const Eigen::Isometry3f localPose(visualizations[i]->getLocalPose());
const Eigen::Vector3f& translation = localPose.translation();
@@ -4087,7 +4085,6 @@ namespace VirtualRobot
const Eigen::Quaternionf q = simox::math::mat3f_to_quat(localPose.linear());
t->rotation.setValue(q.x(), q.y(), q.z(), q.w());
- ss->addChild(u);
ss->addChild(t);
ss->addChild(n->copy(FALSE));
diff --git a/VirtualRobot/tests/OmniPlatformKinematicsTest.cpp b/VirtualRobot/tests/OmniPlatformKinematicsTest.cpp
index 5cceb677c02a138a6abf83834031a7e4854271f5..a9d23c70145b60c9f05dfeb79a04b68248733153 100755
--- a/VirtualRobot/tests/OmniPlatformKinematicsTest.cpp
+++ b/VirtualRobot/tests/OmniPlatformKinematicsTest.cpp
@@ -19,6 +19,7 @@
* GNU General Public License
*/
+#include <cmath>
#include <boost/test/tools/old/interface.hpp>
#define BOOST_TEST_MODULE VirtualRobot_OmniPlatformKinematics
@@ -40,7 +41,9 @@ const inline VirtualRobot::OmniWheelPlatformKinematics::Params ARMAR7_OMNI_PLATF
.L = 326, // [mm]
.R = 125 / 2, // [mm]
.delta = VirtualRobot::MathTools::deg2rad(30), // [rad]
- .n = 1};
+ .n = 1,
+ .relativeAngle = M_PI / 3,
+ .wheelFactor = Eigen::Vector3f{-1, 1, 1}};
BOOST_AUTO_TEST_CASE(testExample)
{
diff --git a/VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp b/VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp
index 8cfed15f1940a14c125d5f415267c238f5043ca4..9abb5be5e167a87edceaabcbdbc284ea41a54ad2 100644
--- a/VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotManipulabilityTest.cpp
@@ -12,19 +12,20 @@
#include <SimoxUtility/algorithm/string/string_conversion_eigen.h>
#define CHECK_SMALL_VELOCITY(a, b, tolerance) { \
- BOOST_REQUIRE_EQUAL(a.rows(), b.rows()); \
- for (unsigned int i = 0; i < a.rows(); i++) { \
- BOOST_CHECK_SMALL(std::abs(a(i) - b(i)), tolerance); \
+ BOOST_REQUIRE_EQUAL((a).rows(), (b).rows()); \
+ for (unsigned int i = 0; i < (a).rows(); i++) { \
+ BOOST_CHECK_SMALL(std::abs((a)(i) - (b)(i)), (tolerance)); \
} \
}
-#define FLOAT_CLOSE_TO_DIFF 1e-4f
-
-#define CHECK_SMALL_DIFF_VELOCITY(a, b) { \
- CHECK_SMALL_VELOCITY(a, b, FLOAT_CLOSE_TO_DIFF) \
+#define CHECK_SMALL_DIFF_VELOCITY(a, b, tolerance) { \
+ CHECK_SMALL_VELOCITY(a, b, tolerance) \
}
+static const float DefaultTolerance = 1e-4f;
+
+
BOOST_AUTO_TEST_SUITE(ManipulabilityTest)
BOOST_AUTO_TEST_CASE(testPositionalVelocityManipulabilityTracking)
@@ -55,7 +56,7 @@ BOOST_AUTO_TEST_CASE(testPositionalVelocityManipulabilityTracking)
VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
- CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity, DefaultTolerance)
}
//BOOST_AUTO_TEST_CASE(testOrientationalVelocityManipulabilityTracking)
@@ -120,7 +121,7 @@ BOOST_AUTO_TEST_CASE(testWholeVelocityManipulabilityTracking)
VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
- CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity, DefaultTolerance)
}
BOOST_AUTO_TEST_CASE(testPositionalForceManipulabilityTracking)
@@ -151,7 +152,7 @@ BOOST_AUTO_TEST_CASE(testPositionalForceManipulabilityTracking)
VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
- CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity, DefaultTolerance)
}
//BOOST_AUTO_TEST_CASE(testOrientationalForceManipulabilityTracking)
@@ -216,7 +217,7 @@ BOOST_AUTO_TEST_CASE(testWholeForceManipulabilityTracking)
VirtualRobot::SingleChainManipulabilityTrackingPtr tracking(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
Eigen::VectorXf velocity = tracking->calculateVelocity(desiredManipulability);
- CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity)
+ CHECK_SMALL_DIFF_VELOCITY(velocity, expectedVelocity, 4e-4f)
}