From c0b67517c520a964c6f685003dc66197b701ee0f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Thu, 21 Feb 2019 15:53:41 +0100
Subject: [PATCH] Refactored calculation of global pose/position for robot node
into own get_() method
---
VirtualRobot/Robot.cpp | 36 ++++++++++++++++++++++++------------
VirtualRobot/Robot.h | 39 +++++++++++++++------------------------
2 files changed, 39 insertions(+), 36 deletions(-)
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 080bf27a5..297b39387 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -647,7 +647,7 @@ namespace VirtualRobot
{
setGlobalPose(globalPose, true);
}
-
+
VirtualRobot::CollisionCheckerPtr Robot::getCollisionChecker()
{
std::vector<RobotNodePtr> robotNodes = this->getRobotNodes();
@@ -826,16 +826,15 @@ namespace VirtualRobot
return result;
}
- void Robot::setGlobalPoseForRobotNode(
- const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode)
+ Eigen::Matrix4f Robot::getGlobalPoseForRobotNode(
+ const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode) const
{
THROW_VR_EXCEPTION_IF(!node, "NULL node");
-
+
if (math::Helpers::Orientation(globalPoseNode).isIdentity())
{
// set position to avoid accumulating rounding errors when using rotation
- setGlobalPositionForRobotNode(node, math::Helpers::Position(globalPoseNode));
- return;
+ return getGlobalPositionForRobotNode(node, math::Helpers::Position(globalPoseNode));
}
// get transformation from current to wanted tcp pose
@@ -843,27 +842,40 @@ namespace VirtualRobot
// apply transformation to current global pose of robot
tf = tf * getGlobalPose();
-
+
// re-orthogonolize to keep it a valid transformation matrix
math::Helpers::Orientation(tf) = math::Helpers::Orthogonalize(math::Helpers::Orientation(tf));
- // set t
- setGlobalPose(tf);
+ // return tf
+ return tf;
}
- void Robot::setGlobalPositionForRobotNode(
- const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode)
+ void Robot::setGlobalPoseForRobotNode(
+ const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode)
{
THROW_VR_EXCEPTION_IF(!node, "NULL node");
+ setGlobalPose(getGlobalPoseForRobotNode(node, globalPoseNode));
+ }
+
+ Eigen::Matrix4f Robot::getGlobalPositionForRobotNode(
+ const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode) const
+ {
// get translation from current to wanted tcp pose
const Eigen::Vector3f translation = globalPositionNode - node->getGlobalPosition();
// apply translation to current global position of robot
const Eigen::Vector3f robotPosition = getGlobalPosition() + translation;
- setGlobalPose(math::Helpers::Pose(robotPosition));
+ return math::Helpers::Pose(robotPosition);
}
+
+ void Robot::setGlobalPositionForRobotNode(
+ const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode)
+ {
+ setGlobalPose(getGlobalPositionForRobotNode(node, globalPositionNode));
+ }
+
VirtualRobot::RobotPtr Robot::clone(const std::string& name, CollisionCheckerPtr collisionChecker, float scaling)
{
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 91a3a07a9..d20618969 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -197,38 +197,33 @@ namespace VirtualRobot
*/
int getNumFaces(bool collisionModel = false) override;
- /*!
- Set the global pose of this robot
- */
+ //! Set the global pose of this robot
virtual void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues) = 0;
void setGlobalPose(const Eigen::Matrix4f& globalPose) override;
- /*!
- Set the global pose of this robot so that the RobotNode node is at pose globalPoseNode.
- */
+ //! Get the global pose of this robot so that the RobotNode node is at pose globalPoseNode.
+ virtual Eigen::Matrix4f getGlobalPoseForRobotNode(const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode) const;
+ //! Set the global pose of this robot so that the RobotNode node is at pose globalPoseNode.
virtual void setGlobalPoseForRobotNode(const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode);
- /*!
- Set the global position of this robot so that the RobotNode node is at position globalPoseNode
- */
+ //! Get the global position of this robot so that the RobotNode node is at position globalPoseNode
+ virtual Eigen::Matrix4f getGlobalPositionForRobotNode(const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode) const;
+ //! Set the global position of this robot so that the RobotNode node is at position globalPoseNode
virtual void setGlobalPositionForRobotNode(const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode);
//virtual Eigen::Matrix4f getGlobalPose() = 0;
- /*!
- Return center of mass of this robot in local coordinate frame. All RobotNodes of this robot are considered according to their mass.
- */
+
+ //! Return center of mass of this robot in local coordinate frame. All RobotNodes of this robot are considered according to their mass.
Eigen::Vector3f getCoMLocal() override;
- /*!
- Return Center of Mass of this robot in global coordinates. All RobotNodes of this robot are considered according to their mass.
- */
+
+ //! Return Center of Mass of this robot in global coordinates. All RobotNodes of this robot are considered according to their mass.
Eigen::Vector3f getCoMGlobal() override;
- /*!
- Return accumulated mass of this robot.
- */
+ //! Return accumulated mass of this robot.
virtual float getMass();
+
/*!
Extract a sub kinematic from this robot and create a new robot instance.
\param startJoint The kinematic starts with this RobotNode
@@ -251,14 +246,10 @@ namespace VirtualRobot
virtual RobotPtr clone(const std::string& name, CollisionCheckerPtr collisionChecker = CollisionCheckerPtr(), float scaling = 1.0f);
virtual RobotPtr clone();
- /*!
- Just storing the filename.
- */
+ //! Just storing the filename.
virtual void setFilename(const std::string& filename);
- /*!
- Retrieve the stored filename.
- */
+ //! Retrieve the stored filename.
virtual std::string getFilename();
/*!
--
GitLab