diff --git a/GraspPlanning/GraspQuality/GraspQualityMeasure.h b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
index 36461706ad77a6ba43d43e419de64af0c14a9cf4..26e88fd0eb3682064f4eb01ba926cb4eee60b956 100644
--- a/GraspPlanning/GraspQuality/GraspQualityMeasure.h
+++ b/GraspPlanning/GraspQuality/GraspQualityMeasure.h
@@ -44,15 +44,15 @@ namespace GraspStudio
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- GraspQualityMeasure(VirtualRobot::SceneObjectPtr object, float unitForce = 1.0f, float frictionConeCoeff = 0.35f, int frictionConeSamples = 8);
+ /// Constructor.
+ GraspQualityMeasure(VirtualRobot::SceneObjectPtr object, float unitForce = 1.0f,
+ float frictionConeCoeff = 0.35f, int frictionConeSamples = 8);
- // destructor
- ~GraspQualityMeasure() override;
+ //! Destructor
+ virtual ~GraspQualityMeasure() override;
- /*
- Checks if grasp is force closure
- */
+ //! Indicate whether the grasp is in force closure.
virtual bool isGraspForceClosure() = 0;
@@ -66,18 +66,20 @@ namespace GraspStudio
bool isValid() override;
virtual ContactConeGeneratorPtr getConeGenerator();
+
protected:
- //Methods
+ // Methods
bool sampleObjectPoints(int nMaxFaces = 400);
- //Friction cone relevant parameters
+
+ // Friction cone relevant parameters
float unitForce;
float frictionCoeff;
int frictionConeSamples;
ContactConeGeneratorPtr coneGenerator;
- //For Object and Grasp Wrench Space Calculation
+ // For Object and Grasp Wrench Space Calculation
std::vector<VirtualRobot::MathTools::ContactPoint> sampledObjectPoints; // in MM
std::vector<VirtualRobot::MathTools::ContactPoint> sampledObjectPointsM; // converted to M
diff --git a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
index e39d7baa4647ce9b65217e4aeba169cf9b600d79..8abfe98865592242e4a343397ce49746e3ce9c51 100644
--- a/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
+++ b/VirtualRobot/Grasping/BasicGraspQualityMeasure.cpp
@@ -51,7 +51,7 @@ namespace VirtualRobot
bool BasicGraspQualityMeasure::calculateGraspQuality()
{
- graspQuality = (float)contactPoints.size() / (float)maxContacts;
+ graspQuality = static_cast<float>(contactPoints.size()) / static_cast<float>(maxContacts);
if (graspQuality > 1.0f)
{
@@ -116,7 +116,7 @@ namespace VirtualRobot
// store force as projected component of approachDirection
const Eigen::Vector3f nGlob = objPoint.contactPointObstacleGlobal - objPoint.contactPointFingerGlobal;
- if (nGlob.norm() > 1e-10)
+ if (nGlob.norm() > 1e-10f)
{
point.force = nGlob.dot(objPoint.approachDirectionGlobal) / nGlob.norm();
}
@@ -153,8 +153,8 @@ namespace VirtualRobot
}
- p.p /= (float)contactPoints.size();
- p.n /= (float)contactPoints.size();
+ p.p /= static_cast<float>(contactPoints.size());
+ p.n /= static_cast<float>(contactPoints.size());
return p;
}
diff --git a/VirtualRobot/Grasping/BasicGraspQualityMeasure.h b/VirtualRobot/Grasping/BasicGraspQualityMeasure.h
index eb062b59b46864cbea97ea66adf8dfb152d3adfb..d457e86357752a30f3b1aadfeddc5826eb694685 100644
--- a/VirtualRobot/Grasping/BasicGraspQualityMeasure.h
+++ b/VirtualRobot/Grasping/BasicGraspQualityMeasure.h
@@ -43,9 +43,10 @@ namespace VirtualRobot
BasicGraspQualityMeasure(VirtualRobot::SceneObjectPtr object);
- // destructor
+ /// Destructor
virtual ~BasicGraspQualityMeasure();
+
/*!
setup contact information
the contact points are normalized by subtracting the COM
@@ -54,9 +55,7 @@ namespace VirtualRobot
virtual void setContactPoints(const VirtualRobot::EndEffector::ContactInfoVector& contactPoints);
virtual void setContactPoints(const std::vector<VirtualRobot::MathTools::ContactPoint>& contactPoints6d);
- /*!
- Returns calculated grasp quality
- */
+ //! Returns calculated grasp quality
virtual float getGraspQuality();
@@ -79,7 +78,7 @@ namespace VirtualRobot
protected:
- //Object relevant parameters
+ // Object relevant parameters
Eigen::Vector3f centerOfModel;
float objectLength;
float graspQuality;
diff --git a/VirtualRobot/Grasping/Grasp.cpp b/VirtualRobot/Grasping/Grasp.cpp
index e5150396b10ca75a52bb5e64f1586aad73b329ff..1983cb638306e8b11f7caf008e84a4c8235b3758 100644
--- a/VirtualRobot/Grasping/Grasp.cpp
+++ b/VirtualRobot/Grasping/Grasp.cpp
@@ -9,15 +9,17 @@ namespace VirtualRobot
{
- Grasp::Grasp(const std::string& name, const std::string& robotType, const std::string& eef, const Eigen::Matrix4f& poseInTCPCoordSystem, const std::string& creation, float quality, const std::string& eefPreshape)
- : poseTcp(poseInTCPCoordSystem), robotType(robotType), eef(eef), name(name), creation(creation), quality(quality), preshape(eefPreshape)
- {
- }
+ Grasp::Grasp(const std::string& name, const std::string& robotType, const std::string& eef,
+ const Eigen::Matrix4f& poseInTCPCoordSystem, const std::string& creation,
+ float quality, const std::string& eefPreshape)
+ : poseTcp(poseInTCPCoordSystem), robotType(robotType), eef(eef), name(name),
+ creation(creation), quality(quality), preshape(eefPreshape)
+ {}
Grasp::~Grasp()
= default;
- void Grasp::print(bool printDecoration /*= true*/)
+ void Grasp::print(bool printDecoration /*= true*/) const
{
if (printDecoration)
{
@@ -44,17 +46,17 @@ namespace VirtualRobot
}
}
- std::string Grasp::getRobotType()
+ std::string Grasp::getRobotType() const
{
return robotType;
}
- std::string Grasp::getEefName()
+ std::string Grasp::getEefName() const
{
return eef;
}
- Eigen::Matrix4f Grasp::getTargetPoseGlobal(RobotPtr robot)
+ Eigen::Matrix4f Grasp::getTargetPoseGlobal(RobotPtr robot) const
{
THROW_VR_EXCEPTION_IF(!robot, "Null data");
THROW_VR_EXCEPTION_IF(robot->getType() != robotType, "Robot types are not compatible: " << robot->getType() << " != " << robotType);
@@ -77,17 +79,17 @@ namespace VirtualRobot
return tcpNode->toGlobalCoordinateSystem(poseTcp);
}
- std::string Grasp::getName()
+ std::string Grasp::getName() const
{
return name;
}
- std::string Grasp::getPreshapeName()
+ std::string Grasp::getPreshapeName() const
{
return preshape;
}
- Eigen::Matrix4f Grasp::getTransformation()
+ Eigen::Matrix4f Grasp::getTransformation() const
{
return poseTcp;
}
@@ -97,7 +99,7 @@ namespace VirtualRobot
this->name = name;
}
- std::string Grasp::toXML(int tabs)
+ std::string Grasp::toXML(int tabs) const
{
std::stringstream ss;
std::string t;
@@ -142,31 +144,31 @@ namespace VirtualRobot
poseTcp = tcp2Object;
}
- Eigen::Matrix4f Grasp::getTcpPoseGlobal(const Eigen::Matrix4f& objectPose)
+ Eigen::Matrix4f Grasp::getTcpPoseGlobal(const Eigen::Matrix4f& objectPose) const
{
Eigen::Matrix4f result = objectPose * poseTcp.inverse();
return result;
}
- Eigen::Matrix4f Grasp::getObjectTargetPoseGlobal(const Eigen::Matrix4f& graspingPose)
+ Eigen::Matrix4f Grasp::getObjectTargetPoseGlobal(const Eigen::Matrix4f& graspingPose) const
{
Eigen::Matrix4f result = graspingPose * poseTcp;
return result;
}
- VirtualRobot::GraspPtr Grasp::clone()
+ VirtualRobot::GraspPtr Grasp::clone() const
{
GraspPtr result(new Grasp(name, robotType, eef, poseTcp, creation, quality, preshape));
result->setConfiguration(eefConfiguration);
return result;
}
- void Grasp::setConfiguration(std::map< std::string, float >& c)
+ void Grasp::setConfiguration(const std::map< std::string, float >& config)
{
- eefConfiguration = c;
+ eefConfiguration = config;
}
- std::map< std::string, float > Grasp::getConfiguration()
+ std::map< std::string, float > Grasp::getConfiguration() const
{
return eefConfiguration;
}
@@ -176,7 +178,7 @@ namespace VirtualRobot
preshape = preshapeName;
}
- float Grasp::getQuality()
+ float Grasp::getQuality() const
{
return quality;
}
@@ -186,7 +188,7 @@ namespace VirtualRobot
quality = q;
}
- std::string Grasp::getCreationMethod()
+ std::string Grasp::getCreationMethod() const
{
return creation;
}
diff --git a/VirtualRobot/Grasping/Grasp.h b/VirtualRobot/Grasping/Grasp.h
index 596168ef79a51334ffc7f477103c7b4f5ff2f934..c926cba2f3904bcf97b11b4700d22ecdd7584b9e 100644
--- a/VirtualRobot/Grasping/Grasp.h
+++ b/VirtualRobot/Grasping/Grasp.h
@@ -49,13 +49,15 @@ namespace VirtualRobot
\param quality A custom quality index.
\param eefPreshape An optional preshape.
*/
- Grasp(const std::string& name, const std::string& robotType, const std::string& eef, const Eigen::Matrix4f& poseInTCPCoordSystem, const std::string& creation = std::string(""), float quality = 0.0f, const std::string& eefPreshape = std::string(""));
+ Grasp(const std::string& name, const std::string& robotType, const std::string& eef,
+ const Eigen::Matrix4f& poseInTCPCoordSystem, const std::string& creation = "",
+ float quality = 0.0f, const std::string& eefPreshape = "");
/*!
*/
virtual ~Grasp();
- void print(bool printDecoration = true);
+ void print(bool printDecoration = true) const;
void setName(const std::string& name);
void setPreshape(const std::string& preshapeName);
@@ -65,17 +67,17 @@ namespace VirtualRobot
Note, that this pose is only valid for the current configuration of the robot.
When the robot moves, the grasping pose will change, and you have to call this method again.
*/
- Eigen::Matrix4f getTargetPoseGlobal(RobotPtr robot);
+ Eigen::Matrix4f getTargetPoseGlobal(RobotPtr robot) const;
/*!
Get the global pose that has to be achieved by the tcp in order to apply the grasp on the object at position objectPose.
*/
- Eigen::Matrix4f getTcpPoseGlobal(const Eigen::Matrix4f& objectPose);
+ Eigen::Matrix4f getTcpPoseGlobal(const Eigen::Matrix4f& objectPose) const;
/*!
The returned pose is the object pose that have to be set in order to apply a grasp at pose graspingPose.
*/
- Eigen::Matrix4f getObjectTargetPoseGlobal(const Eigen::Matrix4f& graspingPose);
+ Eigen::Matrix4f getObjectTargetPoseGlobal(const Eigen::Matrix4f& graspingPose) const;
/*!
Set the transformation of this grasp.
@@ -84,12 +86,12 @@ namespace VirtualRobot
*/
void setTransformation(const Eigen::Matrix4f& tcp2Object);
- std::string getName();
- std::string getRobotType();
- std::string getEefName();
- std::string getCreationMethod();
- std::string getPreshapeName();
- float getQuality();
+ std::string getName() const;
+ std::string getRobotType() const;
+ std::string getEefName() const;
+ std::string getCreationMethod() const;
+ std::string getPreshapeName() const;
+ float getQuality() const;
void setQuality(float q);
@@ -98,24 +100,21 @@ namespace VirtualRobot
The transformation is given in the coordinate system of the tcp (whereas the tcp belongs to the eef).
This transformation specifies the tcp to object relation.
*/
- Eigen::Matrix4f getTransformation();
+ Eigen::Matrix4f getTransformation() const;
- std::string toXML(int tabs = 2);
+ std::string toXML(int tabs = 2) const;
- GraspPtr clone();
+ GraspPtr clone() const;
- /*!
- Returns the (optionally) stored configuration of the fingers / actors.
- */
- std::map< std::string, float > getConfiguration();
+ //! Returns the (optionally) stored configuration of the fingers / actors.
+ std::map<std::string, float> getConfiguration() const;
- /*!
- Optionally the configuration of the fingers / actors can be stored.
-
- */
- void setConfiguration(std::map< std::string, float >& c);
+ //! Optionally the configuration of the fingers / actors can be stored.
+ void setConfiguration(const std::map<std::string, float>& config);
+
protected:
+
std::map< std::string, float > eefConfiguration; //!< Optional: the configuration of the actors.
Eigen::Matrix4f poseTcp; //!< The pose in TCP's coordinate system
diff --git a/VirtualRobot/Obstacle.cpp b/VirtualRobot/Obstacle.cpp
index 94dbaea9fbf0347313c217de0c5ebf83559ea891..1338b2cae256df5343b6feb1898c065ec7f8b060 100644
--- a/VirtualRobot/Obstacle.cpp
+++ b/VirtualRobot/Obstacle.cpp
@@ -55,11 +55,11 @@ namespace VirtualRobot
if (visualizationType.empty())
{
- visualizationFactory = VisualizationFactory::first(NULL);
+ visualizationFactory = VisualizationFactory::first(nullptr);
}
else
{
- visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
+ visualizationFactory = VisualizationFactory::fromName(visualizationType, nullptr);
}
if (!visualizationFactory)
@@ -108,11 +108,11 @@ namespace VirtualRobot
if (visualizationType.empty())
{
- visualizationFactory = VisualizationFactory::first(NULL);
+ visualizationFactory = VisualizationFactory::first(nullptr);
}
else
{
- visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
+ visualizationFactory = VisualizationFactory::fromName(visualizationType, nullptr);
}
if (!visualizationFactory)
@@ -160,11 +160,11 @@ namespace VirtualRobot
if (visualizationType.empty())
{
- visualizationFactory = VisualizationFactory::first(NULL);
+ visualizationFactory = VisualizationFactory::first(nullptr);
}
else
{
- visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
+ visualizationFactory = VisualizationFactory::fromName(visualizationType, nullptr);
}
if (!visualizationFactory)
@@ -214,11 +214,11 @@ namespace VirtualRobot
if (visualizationType.empty())
{
- visualizationFactory = VisualizationFactory::first(NULL);
+ visualizationFactory = VisualizationFactory::first(nullptr);
}
else
{
- visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
+ visualizationFactory = VisualizationFactory::fromName(visualizationType, nullptr);
}
if (!visualizationFactory)
diff --git a/VirtualRobot/Obstacle.h b/VirtualRobot/Obstacle.h
index d0e2553405c782105b981a998c6e6954c2e768b2..bb50ed0d9e64e09ab8e1941c98c393dd58ee5156 100644
--- a/VirtualRobot/Obstacle.h
+++ b/VirtualRobot/Obstacle.h
@@ -42,7 +42,9 @@ namespace VirtualRobot
/*!
*/
- Obstacle(const std::string& name, VisualizationNodePtr visualization = VisualizationNodePtr(), CollisionModelPtr collisionModel = CollisionModelPtr(), const SceneObject::Physics& p = SceneObject::Physics(), CollisionCheckerPtr colChecker = CollisionCheckerPtr());
+ Obstacle(const std::string& name, VisualizationNodePtr visualization = {},
+ CollisionModelPtr collisionModel = {}, const SceneObject::Physics& p = {},
+ CollisionCheckerPtr colChecker = {});
/*!
*/
@@ -53,7 +55,7 @@ namespace VirtualRobot
/*!
Clones this object. If no col checker is given, the one of the original object is used.
*/
- ObstaclePtr clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr()) const
+ ObstaclePtr clone(const std::string& name, CollisionCheckerPtr colChecker = {}) const
{
return ObstaclePtr(_clone(name, colChecker));
}
@@ -69,7 +71,9 @@ namespace VirtualRobot
\param visualizationType Here the type of visualization can be specified (e.g. "Inventor"). If empty, the first registered visualization type (which is usually the only one) is used.
\param colChecker Only needed if you plan to use the collision checker in parallel. If not given, the object is registered with the global singleton collision checker.
*/
- static ObstaclePtr createBox(float width, float height, float depth, VisualizationFactory::Color color = VisualizationFactory::Color::Red(), std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
+ static ObstaclePtr createBox(float width, float height, float depth,
+ VisualizationFactory::Color color = VisualizationFactory::Color::Red(),
+ std::string visualizationType = "", CollisionCheckerPtr colChecker = {});
/*!
Create a standard obstacle.
\param radius The radius of the sphere.
@@ -77,7 +81,9 @@ namespace VirtualRobot
\param visualizationType Here the type of visualization can be specified (e.g. "Inventor"). If empty, the first registered visualization type (which is usually the only one) is used.
\param colChecker Only needed if you plan to use the collision checker in parallel. If not given, the object is registered with the global singleton collision checker.
*/
- static ObstaclePtr createSphere(float radius, VisualizationFactory::Color color = VisualizationFactory::Color::Red(), std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
+ static ObstaclePtr createSphere(float radius,
+ VisualizationFactory::Color color = VisualizationFactory::Color::Red(),
+ std::string visualizationType = "", CollisionCheckerPtr colChecker = {});
/*!
Create a standard obstacle.
\param radius The radius of the cylinder.
@@ -86,7 +92,9 @@ namespace VirtualRobot
\param visualizationType Here the type of visualization can be specified (e.g. "Inventor"). If empty, the first registered visualization type (which is usually the only one) is used.
\param colChecker Only needed if you plan to use the collision checker in parallel. If not given, the object is registered with the global singleton collision checker.
*/
- static ObstaclePtr createCylinder(float radius, float height, VisualizationFactory::Color color = VisualizationFactory::Color::Red(), std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
+ static ObstaclePtr createCylinder(float radius, float height,
+ VisualizationFactory::Color color = VisualizationFactory::Color::Red(),
+ std::string visualizationType = "", CollisionCheckerPtr colChecker = {});
/*!
Create a standard obstacle from a mesh.
@@ -94,7 +102,8 @@ namespace VirtualRobot
\param visualizationType Here the type of visualization can be specified (e.g. "Inventor"). If empty, the first registered visualization type (which is usually the only one) is used.
\param colChecker Only needed if you plan to use the collision checker in parallel. If not given, the object is registered with the global singleton collision checker.
*/
- static ObstaclePtr createFromMesh(TriMeshModelPtr mesh, std::string visualizationType = "", CollisionCheckerPtr colChecker = CollisionCheckerPtr());
+ static ObstaclePtr createFromMesh(TriMeshModelPtr mesh,
+ std::string visualizationType = "", CollisionCheckerPtr colChecker = {});
virtual std::string toXML(const std::string& basePath, int tabs = 0);
@@ -103,7 +112,7 @@ namespace VirtualRobot
protected:
- Obstacle* _clone(const std::string& name, CollisionCheckerPtr colChecker = CollisionCheckerPtr(), float scaling = 1.0f) const override;
+ Obstacle* _clone(const std::string& name, CollisionCheckerPtr colChecker = {}, float scaling = 1.0f) const override;
std::string filename;
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index 16db594dfe242b6f88184d332ea45ae01d012e46..3e6ac763bafab8b84a41ddd6e5c2c1f8aae2e438 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -214,11 +214,11 @@ namespace VirtualRobot
if (visualizationType == "")
{
- visualizationFactory = VisualizationFactory::first(NULL);
+ visualizationFactory = VisualizationFactory::first(nullptr);
}
else
{
- visualizationFactory = VisualizationFactory::fromName(visualizationType, NULL);
+ visualizationFactory = VisualizationFactory::fromName(visualizationType, nullptr);
}
if (!visualizationFactory)
@@ -258,7 +258,7 @@ namespace VirtualRobot
if (enable)
{
- VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(nullptr);
if (!visualizationFactory)
{
@@ -306,7 +306,7 @@ namespace VirtualRobot
visualizationModel->detachVisualization("__InertiaMatrix");
}
- VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(nullptr);
if (enableCoM && visualizationFactory)
{
@@ -371,21 +371,24 @@ namespace VirtualRobot
cout << "v2:" << v2 << endl;
cout << "v3:" << v3 << endl;*/
- float xl = (float)(eigensolver.eigenvalues().rows() > 0 ? eigensolver.eigenvalues()(0) : 1e-6);
- float yl = (float)(eigensolver.eigenvalues().rows() > 1 ? eigensolver.eigenvalues()(1) : 1e-6);
- float zl = (float)(eigensolver.eigenvalues().rows() > 2 ? eigensolver.eigenvalues()(2) : 1e-6);
+ float xl = static_cast<float>(eigensolver.eigenvalues().rows() > 0 ?
+ eigensolver.eigenvalues()(0) : 1e-6);
+ float yl = static_cast<float>(eigensolver.eigenvalues().rows() > 1 ?
+ eigensolver.eigenvalues()(1) : 1e-6);
+ float zl = static_cast<float>(eigensolver.eigenvalues().rows() > 2 ?
+ eigensolver.eigenvalues()(2) : 1e-6);
- if (fabs(xl) < 1e-6)
+ if (std::abs(xl) < 1e-6f)
{
xl = 1e-6f;
}
- if (fabs(yl) < 1e-6)
+ if (std::abs(yl) < 1e-6f)
{
yl = 1e-6f;
}
- if (fabs(zl) < 1e-6)
+ if (std::abs(zl) < 1e-6f)
{
zl = 1e-6f;
}
@@ -542,7 +545,7 @@ namespace VirtualRobot
Eigen::Matrix4f SceneObject::toLocalCoordinateSystem(const Eigen::Matrix4f& poseGlobal) const
{
- return getGlobalPose().inverse() * poseGlobal;
+ return math::Helpers::InvertedPose(getGlobalPose()) * poseGlobal;
}
@@ -553,49 +556,44 @@ namespace VirtualRobot
Eigen::Vector3f SceneObject::toLocalCoordinateSystemVec(const Eigen::Vector3f& positionGlobal) const
{
- Eigen::Matrix4f t;
- t.setIdentity();
- t.block(0, 3, 3, 1) = positionGlobal;
+ Eigen::Matrix4f t = t.Identity();
+ math::Helpers::Position(t) = positionGlobal;
t = toLocalCoordinateSystem(t);
- Eigen::Vector3f result = t.block(0, 3, 3, 1);
- return result;
+ return math::Helpers::Position(t);
}
Eigen::Vector3f SceneObject::toGlobalCoordinateSystemVec(const Eigen::Vector3f& positionLocal) const
{
- Eigen::Matrix4f t;
- t.setIdentity();
- t.block(0, 3, 3, 1) = positionLocal;
+ Eigen::Matrix4f t = t.Identity();
+ math::Helpers::Position(t) = positionLocal;
t = toGlobalCoordinateSystem(t);
- Eigen::Vector3f result = t.block(0, 3, 3, 1);
- return result;
+ return math::Helpers::Position(t);
}
Eigen::Matrix4f SceneObject::getTransformationTo(const SceneObjectPtr otherObject)
{
- return getGlobalPose().inverse() * otherObject->getGlobalPose();
+ return math::Helpers::InvertedPose(getGlobalPose()) * otherObject->getGlobalPose();
}
Eigen::Matrix4f SceneObject::getTransformationFrom(const SceneObjectPtr otherObject)
{
- return otherObject->getGlobalPose().inverse() * getGlobalPose();
+ return math::Helpers::InvertedPose(otherObject->getGlobalPose()) * getGlobalPose();
}
- Eigen::Matrix4f SceneObject::transformTo(const SceneObjectPtr otherObject, const Eigen::Matrix4f& poseInOtherCoordSystem)
+ Eigen::Matrix4f SceneObject::transformTo(const SceneObjectPtr otherObject,
+ const Eigen::Matrix4f& poseInOtherCoordSystem)
{
- Eigen::Matrix4f m = getTransformationTo(otherObject);
- return m * poseInOtherCoordSystem;
+ Eigen::Matrix4f mat = getTransformationTo(otherObject);
+ return mat * poseInOtherCoordSystem;
}
- Eigen::Vector3f SceneObject::transformTo(const SceneObjectPtr otherObject, const Eigen::Vector3f& positionInOtherCoordSystem)
+ Eigen::Vector3f SceneObject::transformTo(const SceneObjectPtr otherObject,
+ const Eigen::Vector3f& positionInOtherCoordSystem)
{
- Eigen::Matrix4f m = getTransformationTo(otherObject);
- Eigen::Vector4f tmp4 = Eigen::Vector4f::Zero();
- tmp4.segment(0, 3) = positionInOtherCoordSystem;
- Eigen::Vector4f res = m * tmp4;
- Eigen::Vector3f res3(res[0], res[1], res[2]);
- return res3;
+ Eigen::Matrix4f mat = getTransformationTo(otherObject);
+ Eigen::Vector4f res = mat * positionInOtherCoordSystem.homogeneous();
+ return res.head<3>();
}
void SceneObject::setupVisualization(bool showVisualization, bool showAttachedVisualizations)
@@ -899,7 +897,7 @@ namespace VirtualRobot
if (enable)
{
- VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(NULL);
+ VisualizationFactoryPtr visualizationFactory = VisualizationFactory::first(nullptr);
if (!visualizationFactory)
{