Skip to content

Commit

Permalink
Merge pull request #1964: Use Eigen::Transform::linear() instead of r…
Browse files Browse the repository at this point in the history 
…otation()

... to improve performance (skipping SVD computation).
Only Eigen 3.4 will have a template specialization for Isometry that will skip SVD computation...
  • Loading branch information
@rhaschke
rhaschke authored May 27, 2020
2 parents 3f3ef1f + d15682b commit 272eaa0
Show file tree
Hide file tree
Showing 36 changed files with 265 additions and 141 deletions.
6 changes: 4 additions & 2 deletions ...t_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -239,7 +239,8 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions)
}

pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) *
Eigen::Quaterniond(sqrt(1 - pow(0.258, 2)), 0.0, 0.258, 0.0));
// robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
// robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
Expand All @@ -259,7 +260,8 @@ TYPED_TEST_P(CollisionDetectorTest, ContactPositions)
}

pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0));
pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) *
Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0).normalized());
// robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
// robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
Expand Down
6 changes: 4 additions & 2 deletions moveit_core/collision_detection/include/moveit/collision_detection/world.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,12 +151,14 @@ class World

/** \brief Get the transform to an object or subframe with given name.
* If name does not exist, a std::runtime_error is thrown.
* A subframe name needs to be prefixed with the object's name separated by a slash. */
* A subframe name needs to be prefixed with the object's name separated by a slash.
* The returned transform is guaranteed to be a valid isometry. */
const Eigen::Isometry3d& getTransform(const std::string& name) const;

/** \brief Get the transform to an object or subframe with given name.
* If name does not exist, returns an identity transform and sets frame_found to false.
* A subframe name needs to be prefixed with the object's name separated by a slash. */
* A subframe name needs to be prefixed with the object's name separated by a slash.
* The returned transform is guaranteed to be a valid isometry. */
const Eigen::Isometry3d& getTransform(const std::string& name, bool& frame_found) const;

/** \brief Add shapes to an object in the map.
Expand Down
8 changes: 8 additions & 0 deletions moveit_core/collision_detection/src/world.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
/* Author: Acorn Pooley, Ioan Sucan */

#include <moveit/collision_detection/world.h>
#include <geometric_shapes/check_isometry.h>
#include <boost/algorithm/string/predicate.hpp>
#include <ros/console.h>

Expand All @@ -59,6 +60,7 @@ inline void World::addToObjectInternal(const ObjectPtr& obj, const shapes::Shape
const Eigen::Isometry3d& pose)
{
obj->shapes_.push_back(shape);
ASSERT_ISOMETRY(pose) // unsanitized input, could contain a non-isometry
obj->shape_poses_.push_back(pose);
}

Expand Down Expand Up @@ -208,6 +210,7 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC
if (it->second->shapes_[i] == shape)
{
ensureUnique(it->second);
ASSERT_ISOMETRY(pose) // unsanitized input, could contain a non-isometry
it->second->shape_poses_[i] = pose;

notify(it->second, MOVE_SHAPE);
Expand All @@ -227,6 +230,7 @@ bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& tr
ensureUnique(it->second);
for (size_t i = 0, n = it->second->shapes_.size(); i < n; ++i)
{
ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry
it->second->shape_poses_[i] = transform * it->second->shape_poses_[i];
}
notify(it->second, MOVE_SHAPE);
Expand Down Expand Up @@ -286,6 +290,10 @@ bool World::setSubframesOfObject(const std::string& object_id, const moveit::cor
{
return false;
}
for (const auto& t : subframe_poses)
{
ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry
}
obj_pair->second->subframe_poses_ = subframe_poses;
return true;
}
Expand Down
4 changes: 3 additions & 1 deletion ...it_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
#include <moveit/collision_detection/collision_env.h>
#include <moveit/macros/class_forward.h>
#include <moveit/collision_detection_fcl/fcl_compat.h>
#include <geometric_shapes/check_isometry.h>

#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
#include <fcl/broadphase/broadphase_collision_manager.h>
Expand Down Expand Up @@ -316,10 +317,11 @@ void cleanCollisionGeometryCache();
/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */
inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f)
{
ASSERT_ISOMETRY(b);
#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
f = b.matrix();
#else
Eigen::Quaterniond q(b.rotation());
Eigen::Quaterniond q(b.linear());
f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z()));
f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z()));
#endif
Expand Down
2 changes: 1 addition & 1 deletion ...core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -465,7 +465,7 @@ class IKConstraintSampler : public ConstraintSampler
* Otherwise, a random quaternion is produced.
*
* @param [out] pos The position component of the sample
* @param [out] quat The orientation component of the sample
* @param [out] quat The orientation component of the sample. It will always be a normalized quaternion.
* @param [in] ks The reference state used for performing transforms
* @param [in] max_attempts The maximum attempts to try to sample - applies only to the position constraint
*
Expand Down
34 changes: 20 additions & 14 deletions moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -374,7 +374,7 @@ bool IKConstraintSampler::loadIKSolver()
for (const std::pair<const moveit::core::LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame()))
{
eef_to_ik_tip_transform_ = fixed_link.second;
eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract
need_eef_to_ik_tip_transform_ = true;
wrong_link = false;
break;
Expand All @@ -392,7 +392,7 @@ bool IKConstraintSampler::loadIKSolver()
for (const std::pair<const moveit::core::LinkModel* const, Eigen::Isometry3d>& fixed_link : fixed_links)
if (moveit::core::Transforms::sameFrame(fixed_link.first->getName(), kb_->getTipFrame()))
{
eef_to_ik_tip_transform_ = fixed_link.second;
eef_to_ik_tip_transform_ = fixed_link.second; // valid isometry by contract
need_eef_to_ik_tip_transform_ = true;
wrong_link = false;
break;
Expand Down Expand Up @@ -479,24 +479,29 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q
Eigen::Isometry3d diff(Eigen::AngleAxisd(angle_x, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(angle_y, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(angle_z, Eigen::Vector3d::UnitZ()));
Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.rotation());
quat = Eigen::Quaterniond(reqr.rotation());
// diff is isometry by construction
// getDesiredRotationMatrix() returns a valid rotation matrix by contract
// reqr has thus to be a valid isometry
Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
quat = Eigen::Quaterniond(reqr.linear()); // reqr is isometry, so quat has to be normalized

// if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
// model
if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
{
// getFrameTransform() returns a valid isometry by contract
const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame());
Eigen::Isometry3d rt(t.rotation() * quat);
quat = Eigen::Quaterniond(rt.rotation());
// rt is isometry by construction
Eigen::Isometry3d rt(t.linear() * quat);
quat = Eigen::Quaterniond(rt.linear()); // rt is isometry, so quat has to be normalized
}
}
else
{
// sample a random orientation
double q[4];
random_number_generator_.quaternion(q);
quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
quat = Eigen::Quaterniond(q[3], q[0], q[1], q[2]); // quat is normalized by contract
}

// if there is an offset, we need to undo the induced rotation in the sampled transform origin (point)
Expand Down Expand Up @@ -549,7 +554,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo
{
// sample a point in the constraint region
Eigen::Vector3d point;
Eigen::Quaterniond quat;
Eigen::Quaterniond quat; // quat is normalized by contract
if (!samplePose(point, quat, reference_state, max_attempts))
{
if (verbose_)
Expand All @@ -562,19 +567,20 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo
{
// we need to convert this transform to the frame expected by the IK solver
// both the planning frame and the frame for the IK are assumed to be robot links
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq;
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction
// getFrameTransform() returns a valid isometry by contract
ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; // valid isometry * valid isometry
point = ikq.translation();
quat = Eigen::Quaterniond(ikq.rotation());
quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized
}

if (need_eef_to_ik_tip_transform_)
{
// After sampling the pose needs to be transformed to the ik chain tip
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat);
ikq = ikq * eef_to_ik_tip_transform_;
Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); // valid isometry by construction
ikq = ikq * eef_to_ik_tip_transform_; // eef_to_ik_tip_transform_ is valid isometry (checked in loadIKSolver())
point = ikq.translation();
quat = Eigen::Quaterniond(ikq.rotation());
quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized
}

geometry_msgs::Pose ik_query;
Expand Down
8 changes: 4 additions & 4 deletions moveit_core/distance_field/src/distance_field.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,10 +172,10 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance,
// double angle = -gradient.angle(unitX);
// Eigen::AngleAxisd rotation(angle, axis);

// marker.pose.orientation.x = rotation.rotation().x();
// marker.pose.orientation.y = rotation.rotation().y();
// marker.pose.orientation.z = rotation.rotation().z();
// marker.pose.orientation.w = rotation.rotation().w();
// marker.pose.orientation.x = rotation.linear().x();
// marker.pose.orientation.y = rotation.linear().y();
// marker.pose.orientation.z = rotation.linear().z();
// marker.pose.orientation.w = rotation.linear().w();

marker.scale.x = getResolution();
marker.scale.y = getResolution();
Expand Down
15 changes: 8 additions & 7 deletions moveit_core/dynamics_solver/src/dynamics_solver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,8 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform
{
Eigen::Vector3d p;
p = Eigen::Vector3d(vector.x, vector.y, vector.z);
p = transform.rotation() * p;
// transform has to be a valid isometry; the caller is responsible for the check
p = transform.linear() * p;

geometry_msgs::Vector3 result;
result.x = p.x();
Expand Down Expand Up @@ -243,9 +244,9 @@ bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, doub
}

state_->setJointGroupPositions(joint_model_group_, joint_angles);
const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_);
const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_);
Eigen::Isometry3d transform = tip_frame.inverse() * base_frame;
const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); // valid isometry by contract
const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); // valid isometry by contract
Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; // valid isometry by construction
wrenches.back().force.z = 1.0;
wrenches.back().force = transformVector(transform, wrenches.back().force);
wrenches.back().torque = transformVector(transform, wrenches.back().torque);
Expand Down Expand Up @@ -299,9 +300,9 @@ bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles,
std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
state_->setJointGroupPositions(joint_model_group_, joint_angles);

const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_);
const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_);
Eigen::Isometry3d transform = tip_frame.inverse() * base_frame;
const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); // valid isometry by contract
const Eigen::Isometry3d& tip_frame = state_->getFrameTransform(tip_name_); // valid isometry by contract
Eigen::Isometry3d transform = tip_frame.inverse() * base_frame; // valid isometry by construction
wrenches.back().force.z = payload * gravity_;
wrenches.back().force = transformVector(transform, wrenches.back().force);
wrenches.back().torque = transformVector(transform, wrenches.back().torque);
Expand Down
20 changes: 12 additions & 8 deletions ...it_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -435,10 +435,13 @@ class OrientationConstraint : public KinematicConstraint
/**
* \brief The rotation target in the reference frame.
*
* @return The target rotation
* @return The target rotation.
*
* The returned matrix is always a valid rotation matrix.
*/
const Eigen::Matrix3d& getDesiredRotationMatrix() const
{
// validity of the rotation matrix is enforced in configure()
return desired_rotation_matrix_;
}

Expand Down Expand Up @@ -477,10 +480,10 @@ class OrientationConstraint : public KinematicConstraint

protected:
const moveit::core::LinkModel* link_model_; /**< \brief The target link model */
Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame */
Eigen::Matrix3d desired_rotation_matrix_; /**< \brief The desired rotation matrix in the tf frame. Guaranteed to
* be valid rotation matrix. */
Eigen::Matrix3d desired_rotation_matrix_inv_; /**< \brief The inverse of the desired rotation matrix, precomputed for
* efficiency
*/
* efficiency. Guaranteed to be valid rotation matrix. */
std::string desired_rotation_frame_id_; /**< \brief The target frame of the transform tree */
bool mobile_frame_; /**< \brief Whether or not the header frame is mobile or fixed */
double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
Expand Down Expand Up @@ -642,10 +645,11 @@ class PositionConstraint : public KinematicConstraint
Eigen::Vector3d offset_; /**< \brief The target offset */
bool has_offset_; /**< \brief Whether the offset is substantially different than 0.0 */
std::vector<bodies::BodyPtr> constraint_region_; /**< \brief The constraint region vector */
EigenSTL::vector_Isometry3d constraint_region_pose_; /**< \brief The constraint region pose vector */
bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/
std::string constraint_frame_id_; /**< \brief The constraint frame id */
const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */
/** \brief The constraint region pose vector. All isometries are guaranteed to be valid. */
EigenSTL::vector_Isometry3d constraint_region_pose_;
bool mobile_frame_; /**< \brief Whether or not a mobile frame is employed*/
std::string constraint_frame_id_; /**< \brief The constraint frame id */
const moveit::core::LinkModel* link_model_; /**< \brief The link model constraint subject */
};

MOVEIT_CLASS_FORWARD(VisibilityConstraint);
Expand Down
Loading

0 comments on commit 272eaa0

Please sign in to comment.