From 50b401960adcd4aef438882ed5e6331d618ff896 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Sun, 22 Mar 2020 04:14:00 +0100 Subject: [PATCH 1/2] Improve performance by calling Eigen::Transform::linear() instead of Eigen::Transform::rotation(). --- .../collision_common.h | 2 +- .../src/default_constraint_samplers.cpp | 12 +++---- .../distance_field/src/distance_field.cpp | 8 ++--- .../dynamics_solver/src/dynamics_solver.cpp | 2 +- .../src/kinematic_constraint.cpp | 27 ++++++++-------- .../kinematic_constraints/src/utils.cpp | 2 +- .../planning_scene/src/planning_scene.cpp | 2 +- moveit_core/robot_model/src/aabb.cpp | 2 +- .../robot_model/src/floating_joint_model.cpp | 2 +- moveit_core/robot_model/src/link_model.cpp | 4 +-- .../robot_model/src/planar_joint_model.cpp | 2 +- .../robot_model/src/revolute_joint_model.cpp | 2 +- .../src/cartesian_interpolator.cpp | 6 ++-- moveit_core/robot_state/src/robot_state.cpp | 10 +++--- .../robot_state/test/robot_state_test.cpp | 32 +++++++++---------- moveit_core/robot_state/test/test_aabb.cpp | 4 +-- .../include/moveit/transforms/transforms.h | 6 ++-- .../src/approach_and_translate_stage.cpp | 4 +-- ...inematics_speed_and_validity_evaluator.cpp | 2 +- .../src/trajectory_execution_manager.cpp | 2 +- .../src/wrap_python_robot_interface.cpp | 2 +- .../src/motion_planning_frame_objects.cpp | 4 +-- .../src/planning_link_updater.cpp | 2 +- .../src/render_shapes.cpp | 2 +- 24 files changed, 71 insertions(+), 72 deletions(-) diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 66beae404d1..7922a7bbdb5 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -319,7 +319,7 @@ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f) #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 diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index c8dbb1f45e8..4f78d7edd8f 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -479,16 +479,16 @@ 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()); + Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear()); + quat = Eigen::Quaterniond(reqr.linear()); // 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()) { const Eigen::Isometry3d& t = ks.getFrameTransform(sampling_pose_.orientation_constraint_->getReferenceFrame()); - Eigen::Isometry3d rt(t.rotation() * quat); - quat = Eigen::Quaterniond(rt.rotation()); + Eigen::Isometry3d rt(t.linear() * quat); + quat = Eigen::Quaterniond(rt.linear()); } } else @@ -565,7 +565,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); ikq = reference_state.getFrameTransform(ik_frame_).inverse() * ikq; point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.rotation()); + quat = Eigen::Quaterniond(ikq.linear()); } if (need_eef_to_ik_tip_transform_) @@ -574,7 +574,7 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo Eigen::Isometry3d ikq(Eigen::Translation3d(point) * quat); ikq = ikq * eef_to_ik_tip_transform_; point = ikq.translation(); - quat = Eigen::Quaterniond(ikq.rotation()); + quat = Eigen::Quaterniond(ikq.linear()); } geometry_msgs::Pose ik_query; diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 071777706be..0ec6cfecd24 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -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(); diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 20cf844b399..5462fd2ae59 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -53,7 +53,7 @@ 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; + p = transform.linear() * p; geometry_msgs::Vector3 result; result.x = p.x(); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 9ed1c081253..3ca1d5c7191 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -391,7 +391,7 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin) for (std::size_t j = 0; j < o.constraint_region_.size(); ++j) { Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j]; - if (diff.translation().norm() < margin && diff.rotation().isIdentity(margin) && + if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) && constraint_region_[i]->getType() == o.constraint_region_[j]->getType() && fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[j]->computeVolume()) < margin) { @@ -597,16 +597,15 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob Eigen::Vector3d xyz; if (mobile_frame_) { - Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).rotation() * desired_rotation_matrix_; - Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).rotation()); - xyz = diff.rotation().eulerAngles(0, 1, 2); + Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_; + Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); + xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } else { - Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).rotation()); - xyz = - diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints + Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear()); + xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi() - fabs(xyz(0))); @@ -618,7 +617,7 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob if (verbose) { - Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).rotation()); + Eigen::Quaterniond q_act(state.getGlobalLinkTransform(link_model_).linear()); Eigen::Quaterniond q_des(desired_rotation_matrix_); ROS_INFO_NAMED("kinematic_constraints", "Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion " @@ -756,12 +755,12 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; if (diff.translation().norm() > margin) return false; - if (!diff.rotation().isIdentity(margin)) + if (!diff.linear().isIdentity(margin)) return false; diff = target_pose_.inverse() * o.target_pose_; if (diff.translation().norm() > margin) return false; - if (!diff.rotation().isIdentity(margin)) + if (!diff.linear().isIdentity(margin)) return false; return true; } @@ -895,7 +894,7 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, mka.scale.y = .15; mka.scale.z = 0.0; mka.points.resize(2); - Eigen::Vector3d d = tp.translation() + tp.rotation().col(2) * 0.5; + Eigen::Vector3d d = tp.translation() + tp.linear().col(2) * 0.5; mka.points[0].x = tp.translation().x(); mka.points[0].y = tp.translation().y(); mka.points[0].z = tp.translation().z(); @@ -908,7 +907,7 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, mka.color.b = 1.0; mka.color.r = 0.0; - d = sp.translation() + sp.rotation().col(2 - sensor_view_direction_) * 0.5; + d = sp.translation() + sp.linear().col(2 - sensor_view_direction_) * 0.5; mka.points[0].x = sp.translation().x(); mka.points[0].y = sp.translation().y(); mka.points[0].z = sp.translation().z(); @@ -932,11 +931,11 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; // necessary to do subtraction as SENSOR_Z is 0 and SENSOR_X is 2 - const Eigen::Vector3d& normal2 = sp.rotation().col(2 - sensor_view_direction_); + const Eigen::Vector3d& normal2 = sp.linear().col(2 - sensor_view_direction_); if (max_view_angle_ > 0.0) { - const Eigen::Vector3d& normal1 = tp.rotation().col(2) * -1.0; // along Z axis and inverted + const Eigen::Vector3d& normal1 = tp.linear().col(2) * -1.0; // along Z axis and inverted double dp = normal2.dot(normal1); double ang = acos(dp); if (dp < 0.0) diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 9b8813a1f74..c9cb35641e8 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -567,7 +567,7 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta if (c.link_name != robot_link->getName()) { c.link_name = robot_link->getName(); - Eigen::Quaterniond link_name_to_robot_link(transform.linear().inverse() * + Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() * state.getGlobalLinkTransform(robot_link).linear()); Eigen::Quaterniond quat_target; tf::quaternionMsgToEigen(c.orientation, quat_target); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 28f70f32b41..f340a5f1444 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -966,7 +966,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const shapes::saveAsText(obj->shapes_[j].get(), out); out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " " << obj->shape_poses_[j].translation().z() << std::endl; - Eigen::Quaterniond r(obj->shape_poses_[j].rotation()); + Eigen::Quaterniond r(obj->shape_poses_[j].linear()); out << r.x() << " " << r.y() << " " << r.z() << " " << r.w() << std::endl; if (hasObjectColor(id)) { diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index fa7955ed376..51b3523032d 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -44,7 +44,7 @@ void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& trans // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ - const Eigen::Matrix3d& r = transform.rotation(); + const Eigen::Matrix3d& r = transform.linear(); const Eigen::Vector3d& t = transform.translation(); double x_range = 0.5 * (fabs(r(0, 0) * box[0]) + fabs(r(0, 1) * box[1]) + fabs(r(0, 2) * box[2])); diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 47ed6a87d25..b7d25080a22 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -225,7 +225,7 @@ void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& trans joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); joint_values[2] = transf.translation().z(); - Eigen::Quaterniond q(transf.rotation()); + Eigen::Quaterniond q(transf.linear()); joint_values[3] = q.x(); joint_values[4] = q.y(); joint_values[5] = q.z(); diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index a4435e425e8..c75ab24e0ac 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -61,7 +61,7 @@ void LinkModel::setJointOriginTransform(const Eigen::Isometry3d& transform) { joint_origin_transform_ = transform; joint_origin_transform_is_identity_ = - joint_origin_transform_.rotation().isIdentity() && + joint_origin_transform_.linear().isIdentity() && joint_origin_transform_.translation().norm() < std::numeric_limits::epsilon(); } @@ -83,7 +83,7 @@ void LinkModel::setGeometry(const std::vector& shapes, for (std::size_t i = 0; i < shapes_.size(); ++i) { collision_origin_transform_is_identity_[i] = - (collision_origin_transform_[i].rotation().isIdentity() && + (collision_origin_transform_[i].linear().isIdentity() && collision_origin_transform_[i].translation().norm() < std::numeric_limits::epsilon()) ? 1 : 0; diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 9be6a69bfad..f515be4c9f4 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -224,7 +224,7 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); - Eigen::Quaterniond q(transf.rotation()); + Eigen::Quaterniond q(transf.linear()); // taken from Bullet double s_squared = 1.0 - (q.w() * q.w()); if (s_squared < 10.0 * std::numeric_limits::epsilon()) diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index a7afaa9b91d..c3783d48e1b 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -268,7 +268,7 @@ void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Iso void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const { - Eigen::Quaterniond q(transf.rotation()); + Eigen::Quaterniond q(transf.linear()); q.normalize(); size_t max_idx; axis_.array().abs().maxCoeff(&max_idx); diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 46d17753b59..b2b38db361c 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -62,7 +62,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link); // the direction can be in the local reference frame (in which case we rotate it) - const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.rotation() * direction; + const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction; // The target pose is built by applying a translation to the start pose for the desired direction and distance Eigen::Isometry3d target_pose = start_pose; @@ -91,8 +91,8 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons // the target can be in the local reference frame (in which case we rotate it) Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; - Eigen::Quaterniond start_quaternion(start_pose.rotation()); - Eigen::Quaterniond target_quaternion(rotated_target.rotation()); + Eigen::Quaterniond start_quaternion(start_pose.linear()); + Eigen::Quaterniond target_quaternion(rotated_target.linear()); if (max_step.translation <= 0.0 && max_step.rotation <= 0.0) { diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 11e24407598..c8dda3b746d 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -1242,7 +1242,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link if (pjm->getType() == moveit::core::JointModel::REVOLUTE) { joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation()); jacobian.block<3, 1>(3, joint_index) = jacobian.block<3, 1>(3, joint_index) + joint_axis; @@ -1250,7 +1250,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link else if (pjm->getType() == moveit::core::JointModel::PRISMATIC) { joint_transform = reference_transform * getGlobalLinkTransform(link); - joint_axis = joint_transform.rotation() * static_cast(pjm)->getAxis(); + joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; } else if (pjm->getType() == moveit::core::JointModel::PLANAR) @@ -1279,7 +1279,7 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link // [x] [ w -z y ] [ omega_2 ] // [y] [ z w -x ] [ omega_3 ] // [z] [ -y x w ] - Eigen::Quaterniond q(link_transform.rotation()); + Eigen::Quaterniond q(link_transform.linear()); double w = q.w(), x = q.x(), y = q.y(), z = q.z(); Eigen::MatrixXd quaternion_update_matrix(4, 3); quaternion_update_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w; @@ -1836,7 +1836,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: for (std::size_t i = 0; i < transformed_poses.size(); ++i) { - Eigen::Quaterniond quat(transformed_poses[i].rotation()); + Eigen::Quaterniond quat(transformed_poses[i].linear()); Eigen::Vector3d point(transformed_poses[i].translation()); ik_queries[i].position.x = point.x(); ik_queries[i].position.y = point.y(); @@ -2101,7 +2101,7 @@ void RobotState::printStateInfo(std::ostream& out) const void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const { - Eigen::Quaterniond q(transform.rotation()); + Eigen::Quaterniond q(transform.linear()); out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "]" << std::endl; diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 358d0cb64cf..5c1d4f55591 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -450,28 +450,28 @@ TEST_F(OneRobot, FK) state.setVariablePositions(joint_values); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).y(), 1e-5); - EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).z(), 1e-5); - EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).y(), 1e-5); + EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).z(), 1e-5); + EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).z(), 1e-5); - EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).z(), 1e-5); + EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).x(), 1e-5); - EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).z(), 1e-5); - EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).x(), 1e-5); + EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).z(), 1e-5); + EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).w(), 1e-5); EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0)); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).x(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).y(), 1e-5); - EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).z(), 1e-5); - EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").rotation()).w(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).x(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).y(), 1e-5); + EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).z(), 1e-5); + EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).w(), 1e-5); EXPECT_TRUE(state.satisfiesBounds()); diff --git a/moveit_core/robot_state/test/test_aabb.cpp b/moveit_core/robot_state/test/test_aabb.cpp index 1c0cf06f6f7..d41129ef288 100644 --- a/moveit_core/robot_state/test/test_aabb.cpp +++ b/moveit_core/robot_state/test/test_aabb.cpp @@ -211,7 +211,7 @@ TEST_F(TestAABB, TestPR2) msg.scale.z = extents[2]; msg.color.r = 0; msg.color.b = 1; - Eigen::Quaterniond q(transform.rotation()); + Eigen::Quaterniond q(transform.linear()); msg.pose.orientation.x = q.x(); msg.pose.orientation.y = q.y(); msg.pose.orientation.z = q.z(); @@ -257,7 +257,7 @@ TEST_F(TestAABB, TestPR2) msg.scale.z = extents[2]; msg.color.r = 0; msg.color.b = 1; - Eigen::Quaterniond q(transforms[i].rotation()); + Eigen::Quaterniond q(transforms[i].linear()); msg.pose.orientation.x = q.x(); msg.pose.orientation.y = q.y(); msg.pose.orientation.z = q.z(); diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 6833b606aab..8e72213aa68 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -137,7 +137,7 @@ class Transforms : private boost::noncopyable */ void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const { - v_out = getTransform(from_frame).rotation() * v_in; + v_out = getTransform(from_frame).linear() * v_in; } /** @@ -149,7 +149,7 @@ class Transforms : private boost::noncopyable void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in, Eigen::Quaterniond& q_out) const { - q_out = getTransform(from_frame).rotation() * q_in; + q_out = getTransform(from_frame).linear() * q_in; } /** @@ -160,7 +160,7 @@ class Transforms : private boost::noncopyable */ void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const { - m_out = getTransform(from_frame).rotation() * m_in; + m_out = getTransform(from_frame).linear() * m_in; } /** diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index c05e8171492..3fdde918169 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -216,10 +216,10 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // transform the input vectors in accordance to frame specified in the header; if (approach_direction_is_global_frame) approach_direction = - planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction; + planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction; if (retreat_direction_is_global_frame) retreat_direction = - planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).rotation() * retreat_direction; + planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction; // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping moveit::core::GroupStateValidityCallbackFn approach_valid_callback = diff --git a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp index d0267a28501..4dc408396ac 100644 --- a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp +++ b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp @@ -94,7 +94,7 @@ int main(int argc, char** argv) moveit::tools::Profiler::End("IK"); const Eigen::Isometry3d& pose_upd = state.getGlobalLinkTransform(tip); Eigen::Isometry3d diff = pose_upd * pose.inverse(); - double rot_err = (diff.rotation() - Eigen::Matrix3d::Identity()).norm(); + double rot_err = (diff.linear() - Eigen::Matrix3d::Identity()).norm(); double trans_err = diff.translation().norm(); moveit::tools::Profiler::Average("Rotation error", rot_err); moveit::tools::Profiler::Average("Translation error", trans_err); diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index bf83b39e7e7..1db84cf7ac7 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -1009,7 +1009,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont start_transform = tf2::transformToEigen(transforms[i]); Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; - rotation.fromRotationMatrix(cur_transform.rotation().transpose() * start_transform.rotation()); + rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); if ((offset.array() > allowed_start_tolerance_).any() || rotation.angle() > allowed_start_tolerance_) { ROS_ERROR_STREAM_NAMED(name_, "\nInvalid Trajectory: start point deviates from current robot state more than " diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index 893a8c9bb75..35c1e047ad9 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -155,7 +155,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer v[0] = t.translation().x(); v[1] = t.translation().y(); v[2] = t.translation().z(); - Eigen::Quaterniond q(t.rotation()); + Eigen::Quaterniond q(t.linear()); v[3] = q.x(); v[4] = q.y(); v[5] = q.z(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index e22be446129..8573ba7f510 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -238,7 +238,7 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() if (obj->shapes_.size() == 1) { obj_pose = obj->shape_poses_[0]; - Eigen::Vector3d xyz = obj_pose.rotation().eulerAngles(0, 1, 2); + Eigen::Vector3d xyz = obj_pose.linear().eulerAngles(0, 1, 2); update_scene_marker = true; // do the marker update outside locked scope to avoid deadlock bool old_state = ui_->object_x->blockSignals(true); @@ -322,7 +322,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) // Update the interactive marker pose to match the manually introduced one if (update_marker_position && scene_marker_) { - Eigen::Quaterniond eq(p.rotation()); + Eigen::Quaterniond eq(p.linear()); scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()), Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), ""); } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index 0ad0668b06f..2455db97ca6 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -52,7 +52,7 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin } const Eigen::Vector3d& robot_visual_position = kinematic_state_->getGlobalLinkTransform(link_model).translation(); - Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).rotation()); + Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).linear()); visual_position = Ogre::Vector3(robot_visual_position.x(), robot_visual_position.y(), robot_visual_position.z()); visual_orientation = Ogre::Quaternion(robot_visual_orientation.w(), robot_visual_orientation.x(), robot_visual_orientation.y(), robot_visual_orientation.z()); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 55ad73109ea..19e06f150c2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -82,7 +82,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co rviz::Shape* ogre_shape = nullptr; Eigen::Vector3d translation = p.translation(); Ogre::Vector3 position(translation.x(), translation.y(), translation.z()); - Eigen::Quaterniond q(p.rotation()); + Eigen::Quaterniond q(p.linear()); Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z()); // we don't know how to render cones directly, but we can convert them to a mesh From d15682bfd43c0142ff3280fc7992785cf4d1d113 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 21 May 2020 04:29:00 +0200 Subject: [PATCH 2/2] Added isometry checks. --- .../test_collision_common_pr2.h | 6 ++-- .../moveit/collision_detection/world.h | 6 ++-- moveit_core/collision_detection/src/world.cpp | 8 +++++ .../collision_common.h | 2 ++ .../default_constraint_samplers.h | 2 +- .../src/default_constraint_samplers.cpp | 30 +++++++++++-------- .../dynamics_solver/src/dynamics_solver.cpp | 13 ++++---- .../kinematic_constraint.h | 20 ++++++++----- .../src/kinematic_constraint.cpp | 23 ++++++++++++-- .../kinematic_constraints/src/utils.cpp | 1 + .../test/test_constraints.cpp | 4 +-- .../planning_scene/src/planning_scene.cpp | 1 + .../include/moveit/robot_model/joint_model.h | 6 ++-- .../include/moveit/robot_model/link_model.h | 11 +++++-- moveit_core/robot_model/src/aabb.cpp | 2 ++ .../robot_model/src/floating_joint_model.cpp | 7 +++-- moveit_core/robot_model/src/link_model.cpp | 3 ++ .../robot_model/src/planar_joint_model.cpp | 2 ++ .../robot_model/src/revolute_joint_model.cpp | 2 ++ .../moveit/robot_state/attached_body.h | 20 +++++++++---- .../include/moveit/robot_state/robot_state.h | 14 +++++++-- moveit_core/robot_state/src/attached_body.cpp | 15 ++++++++-- .../src/cartesian_interpolator.cpp | 11 +++++-- moveit_core/robot_state/src/robot_state.cpp | 19 +++++++----- .../include/moveit/transforms/transforms.h | 23 ++++++++------ moveit_core/transforms/src/transforms.cpp | 6 ++++ .../src/approach_and_translate_stage.cpp | 2 ++ ...inematics_speed_and_validity_evaluator.cpp | 4 ++- .../src/trajectory_execution_manager.cpp | 3 ++ .../src/wrap_python_robot_interface.cpp | 1 + .../src/motion_planning_frame_objects.cpp | 3 +- .../src/planning_link_updater.cpp | 1 + .../src/render_shapes.cpp | 2 ++ 33 files changed, 199 insertions(+), 74 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h index 8a3eb61e732..b8f3f8e22e3 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h @@ -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); @@ -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); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index ad21d3a0344..0225fd4cb87 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -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. diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index fe49764cb76..383f298471f 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -35,6 +35,7 @@ /* Author: Acorn Pooley, Ioan Sucan */ #include +#include #include #include @@ -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); } @@ -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); @@ -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); @@ -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; } diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h index 7922a7bbdb5..be370daf073 100644 --- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h +++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h @@ -40,6 +40,7 @@ #include #include #include +#include #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0)) #include @@ -316,6 +317,7 @@ 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 diff --git a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h index ffc4b5ea8d6..79ceeab1545 100644 --- a/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h +++ b/moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h @@ -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 * diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 4f78d7edd8f..4cdf1e18554 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -374,7 +374,7 @@ bool IKConstraintSampler::loadIKSolver() for (const std::pair& 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; @@ -392,7 +392,7 @@ bool IKConstraintSampler::loadIKSolver() for (const std::pair& 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; @@ -479,16 +479,21 @@ 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())); + // 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()); + 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()); + // rt is isometry by construction Eigen::Isometry3d rt(t.linear() * quat); - quat = Eigen::Quaterniond(rt.linear()); + quat = Eigen::Quaterniond(rt.linear()); // rt is isometry, so quat has to be normalized } } else @@ -496,7 +501,7 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q // 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) @@ -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_) @@ -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.linear()); + 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.linear()); + quat = Eigen::Quaterniond(ikq.linear()); // ikq is isometry, so quat is normalized } geometry_msgs::Pose ik_query; diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 5462fd2ae59..1126d29cdf5 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -53,6 +53,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform { Eigen::Vector3d p; p = Eigen::Vector3d(vector.x, vector.y, vector.z); + // transform has to be a valid isometry; the caller is responsible for the check p = transform.linear() * p; geometry_msgs::Vector3 result; @@ -243,9 +244,9 @@ bool DynamicsSolver::getMaxPayload(const std::vector& 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); @@ -299,9 +300,9 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, std::vector 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); diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index cf28da88580..70cf8f08710 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h @@ -434,10 +434,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_; } @@ -476,10 +479,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_, @@ -641,10 +644,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 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); diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 3ca1d5c7191..8101a46a4d7 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -318,6 +319,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.primitive_poses[i], t); + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry constraint_region_pose_.push_back(t); if (mobile_frame_) constraint_region_.back()->setPose(constraint_region_pose_.back()); @@ -345,6 +347,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get()))); Eigen::Isometry3d t; tf2::fromMsg(pc.constraint_region.mesh_poses[i], t); + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry constraint_region_pose_.push_back(t); if (mobile_frame_) constraint_region_.back()->setPose(constraint_region_pose_.back()); @@ -390,6 +393,7 @@ bool PositionConstraint::equal(const KinematicConstraint& other, double margin) // need to check against all other regions for (std::size_t j = 0; j < o.constraint_region_.size(); ++j) { + // constraint_region_pose_ contain only valid isometries, so diff is also a valid isometry Eigen::Isometry3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[j]; if (diff.translation().norm() < margin && diff.linear().isIdentity(margin) && constraint_region_[i]->getType() == o.constraint_region_[j]->getType() && @@ -597,13 +601,16 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob Eigen::Vector3d xyz; if (mobile_frame_) { + // getFrameTransform() returns a valid isometry by contract Eigen::Matrix3d tmp = state.getFrameTransform(desired_rotation_frame_id_).linear() * desired_rotation_matrix_; - Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d diff(tmp.transpose() * state.getGlobalLinkTransform(link_model_).linear()); // valid isometry xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } else { + // diff is valid isometry by construction Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear()); xyz = diff.linear().eulerAngles(0, 1, 2); // 0,1,2 corresponds to XYZ, the convention used in sampling constraints } @@ -695,6 +702,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc } tf2::fromMsg(vc.target_pose.pose, target_pose_); + ASSERT_ISOMETRY(target_pose_) // unsanitized input, could contain a non-isometry if (tf.isFixedFrame(vc.target_pose.header.frame_id)) { @@ -712,6 +720,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc } tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_); + ASSERT_ISOMETRY(sensor_pose_) // unsanitized input, could contain a non-isometry if (tf.isFixedFrame(vc.sensor_pose.header.frame_id)) { @@ -752,12 +761,14 @@ bool VisibilityConstraint::equal(const KinematicConstraint& other, double margin { if (fabs(max_view_angle_ - o.max_view_angle_) > margin || fabs(target_radius_ - o.target_radius_) > margin) return false; - Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; + // sensor_pose_ is valid isometry, checked in configure() + Eigen::Isometry3d diff = sensor_pose_.inverse() * o.sensor_pose_; // valid isometry if (diff.translation().norm() > margin) return false; if (!diff.linear().isIdentity(margin)) return false; - diff = target_pose_.inverse() * o.target_pose_; + // target_pose_ is valid isometry, checked in configure() + diff = target_pose_.inverse() * o.target_pose_; // valid isometry if (diff.translation().norm() > margin) return false; if (!diff.linear().isIdentity(margin)) @@ -875,8 +886,11 @@ void VisibilityConstraint::getMarkers(const moveit::core::RobotState& state, markers.markers.push_back(mk); + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& sp = mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& tp = mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; @@ -925,8 +939,11 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo if (max_view_angle_ > 0.0 || max_range_angle_ > 0.0) { + // getFrameTransform() returns a valid isometry by contract + // sensor_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& sp = mobile_sensor_frame_ ? state.getFrameTransform(sensor_frame_id_) * sensor_pose_ : sensor_pose_; + // target_pose_ is valid isometry (checked in configure()) const Eigen::Isometry3d& tp = mobile_target_frame_ ? state.getFrameTransform(target_frame_id_) * target_pose_ : target_pose_; diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index c9cb35641e8..088fff83dbc 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -558,6 +558,7 @@ bool kinematic_constraints::resolveConstraintFrames(const moveit::core::RobotSta { bool frame_found; const moveit::core::LinkModel* robot_link; + // getFrameInfo() returns a valid isometry by contract const Eigen::Isometry3d& transform = state.getFrameInfo(c.link_name, robot_link, frame_found); if (!frame_found) return false; diff --git a/moveit_core/kinematic_constraints/test/test_constraints.cpp b/moveit_core/kinematic_constraints/test/test_constraints.cpp index b16e9505aec..2ffe6df1eee 100644 --- a/moveit_core/kinematic_constraints/test/test_constraints.cpp +++ b/moveit_core/kinematic_constraints/test/test_constraints.cpp @@ -694,13 +694,13 @@ TEST_F(LoadPlanningModelsPr2, VisibilityConstraintsSimple) // very slight angle, so still ok vcm.target_pose.pose.orientation.y = 0.03; - vcm.target_pose.pose.orientation.w = .9995; + vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2)); EXPECT_TRUE(vc.configure(vcm, tf)); EXPECT_TRUE(vc.decide(robot_state, true).satisfied); // a little bit more puts it over vcm.target_pose.pose.orientation.y = 0.06; - vcm.target_pose.pose.orientation.w = .9981; + vcm.target_pose.pose.orientation.w = sqrt(1 - pow(vcm.target_pose.pose.orientation.y, 2)); EXPECT_TRUE(vc.configure(vcm, tf)); EXPECT_FALSE(vc.decide(robot_state, true).satisfied); } diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index f340a5f1444..be510d88f55 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -964,6 +964,7 @@ void PlanningScene::saveGeometryToStream(std::ostream& out) const for (std::size_t j = 0; j < obj->shapes_.size(); ++j) { shapes::saveAsText(obj->shapes_[j].get(), out); + // shape_poses_ is valid isometry by contract out << obj->shape_poses_[j].translation().x() << " " << obj->shape_poses_[j].translation().y() << " " << obj->shape_poses_[j].translation().z() << std::endl; Eigen::Quaterniond r(obj->shape_poses_[j].linear()); diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index aa07321d739..173590129d4 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h @@ -443,10 +443,12 @@ class JointModel /** @name Computing transforms @{ */ - /** \brief Given the joint values for a joint, compute the corresponding transform */ + /** \brief Given the joint values for a joint, compute the corresponding transform. The computed transform is + * guaranteed to be a valid isometry. */ virtual void computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const = 0; - /** \brief Given the transform generated by joint, compute the corresponding joint values */ + /** \brief Given the transform generated by joint, compute the corresponding joint values. Make sure the passed + * transform is a valid isometry. */ virtual void computeVariablePositions(const Eigen::Isometry3d& transform, double* joint_values) const = 0; /** @} */ diff --git a/moveit_core/robot_model/include/moveit/robot_model/link_model.h b/moveit_core/robot_model/include/moveit/robot_model/link_model.h index d586dfded0b..5d81a53fa85 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/link_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/link_model.h @@ -43,6 +43,7 @@ #include #include #include +#include namespace shapes { @@ -137,7 +138,8 @@ class LinkModel /** \brief When transforms are computed for this link, they are usually applied to the link's origin. The joint origin transform acts as an offset -- it is - pre-applied before any other transform */ + pre-applied before any other transform. The + transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getJointOriginTransform() const { return joint_origin_transform_; @@ -157,7 +159,8 @@ class LinkModel /** \brief In addition to the link transform, the geometry of a link that is used for collision checking may have - a different offset itself, with respect to the origin */ + a different offset itself, with respect to the origin. + The transform is guaranteed to be a valid isometry. */ const EigenSTL::vector_Isometry3d& getCollisionOriginTransforms() const { return collision_origin_transform_; @@ -191,7 +194,8 @@ class LinkModel return centered_bounding_box_offset_; } - /** \brief Get the set of links that are attached to this one via fixed transforms */ + /** \brief Get the set of links that are attached to this one via fixed transforms. The returned transforms are + * guaranteed to be valid isometries. */ const LinkTransformMap& getAssociatedFixedTransforms() const { return associated_fixed_transforms_; @@ -200,6 +204,7 @@ class LinkModel /** \brief Remember that \e link_model is attached to this link using a fixed transform */ void addAssociatedFixedTransform(const LinkModel* link_model, const Eigen::Isometry3d& transform) { + ASSERT_ISOMETRY(transform); // unsanitized input, could contain a non-isometry associated_fixed_transforms_[link_model] = transform; } diff --git a/moveit_core/robot_model/src/aabb.cpp b/moveit_core/robot_model/src/aabb.cpp index 51b3523032d..1b115cfefc3 100644 --- a/moveit_core/robot_model/src/aabb.cpp +++ b/moveit_core/robot_model/src/aabb.cpp @@ -35,6 +35,7 @@ /* Author: Martin Pecka */ #include +#include void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& transform, const Eigen::Vector3d& box) { @@ -44,6 +45,7 @@ void moveit::core::AABB::extendWithTransformedBox(const Eigen::Isometry3d& trans // // Here's a nice explanation why it works: https://zeuxcg.org/2010/10/17/aabb-from-obb-with-component-wise-abs/ + ASSERT_ISOMETRY(transform) // unsanitized input, could contain non-isometry const Eigen::Matrix3d& r = transform.linear(); const Eigen::Vector3d& t = transform.translation(); diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index b7d25080a22..15f0191f3b4 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -216,8 +217,9 @@ bool FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bou void FloatingJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const { - transf = Eigen::Isometry3d(Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) * - Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5])); + transf = Eigen::Isometry3d( + Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) * + Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5]).normalized()); } void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const @@ -225,6 +227,7 @@ void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& trans joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); joint_values[2] = transf.translation().z(); + ASSERT_ISOMETRY(transf) // unsanitized input, could contain non-isometry Eigen::Quaterniond q(transf.linear()); joint_values[3] = q.x(); joint_values[4] = q.y(); diff --git a/moveit_core/robot_model/src/link_model.cpp b/moveit_core/robot_model/src/link_model.cpp index c75ab24e0ac..76f45e3bb60 100644 --- a/moveit_core/robot_model/src/link_model.cpp +++ b/moveit_core/robot_model/src/link_model.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -59,6 +60,7 @@ LinkModel::~LinkModel() = default; void LinkModel::setJointOriginTransform(const Eigen::Isometry3d& transform) { + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry joint_origin_transform_ = transform; joint_origin_transform_is_identity_ = joint_origin_transform_.linear().isIdentity() && @@ -82,6 +84,7 @@ void LinkModel::setGeometry(const std::vector& shapes, for (std::size_t i = 0; i < shapes_.size(); ++i) { + ASSERT_ISOMETRY(collision_origin_transform_[i]) // unsanitized input, could contain a non-isometry collision_origin_transform_is_identity_[i] = (collision_origin_transform_[i].linear().isIdentity() && collision_origin_transform_[i].translation().norm() < std::numeric_limits::epsilon()) ? diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index f515be4c9f4..2727ccd48e9 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -224,6 +225,7 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, joint_values[0] = transf.translation().x(); joint_values[1] = transf.translation().y(); + ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(transf.linear()); // taken from Bullet double s_squared = 1.0 - (q.w() * q.w()); diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index c3783d48e1b..4a87291f38c 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -36,6 +36,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -268,6 +269,7 @@ void RevoluteJointModel::computeTransform(const double* joint_values, Eigen::Iso void RevoluteJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const { + ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(transf.linear()); q.normalize(); size_t max_idx; diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h index 7086d00120d..657c147dc7f 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h +++ b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -105,13 +106,15 @@ class AttachedBody return detach_posture_; } - /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link) */ + /** \brief Get the fixed transforms (the transforms to the shapes of this body, relative to the link). The returned + * transforms are guaranteed to be valid isometries. */ const EigenSTL::vector_Isometry3d& getFixedTransforms() const { return attach_trans_; } - /** \brief Get subframes of this object (relative to the link) */ + /** \brief Get subframes of this object (relative to the link). The returned transforms are guaranteed to be valid + * isometries. */ const moveit::core::FixedTransformsMap& getSubframeTransforms() const { return subframe_poses_; @@ -124,18 +127,24 @@ class AttachedBody */ void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses) { + for (const auto& t : subframe_poses) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } subframe_poses_ = subframe_poses; } /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link) * * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). */ + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; /** \brief Get the fixed transform to a named subframe on this body. * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's - * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). */ + * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false). + * The returned transform is guaranteed to be a valid isometry. */ const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const; /** \brief Check whether a subframe of given @frame_name is present in this object. @@ -144,7 +153,8 @@ class AttachedBody * name is "screwdriver"). */ bool hasSubframeTransform(const std::string& frame_name) const; - /** \brief Get the global transforms for the collision bodies */ + /** \brief Get the global transforms for the collision bodies. The returned transforms are guaranteed to be valid + * isometries. */ const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const { return global_collision_body_transforms_; diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index 00004d02472..5731a6a4ffb 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1307,6 +1307,8 @@ class RobotState * Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed. * A related, more comprehensive function is |getFrameTransform|, which additionally to link frames * also searches for attached object frames and their subframes. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) { @@ -1647,18 +1649,24 @@ class RobotState /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr); /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const; /** \brief Get the transformation matrix from the model frame (root of model) to the frame identified by \e frame_id * * If this frame is attached to a robot link, the link pointer is returned in \e robot_link. - * If frame_id was not found, \e frame_found is set to false and an identity transform is returned */ + * If frame_id was not found, \e frame_found is set to false and an identity transform is returned. + * + * The returned transformation is always a valid isometry. */ const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link, bool& frame_found) const; diff --git a/moveit_core/robot_state/src/attached_body.cpp b/moveit_core/robot_state/src/attached_body.cpp index 51a403a7a2d..ff3fbaa3fb7 100644 --- a/moveit_core/robot_state/src/attached_body.cpp +++ b/moveit_core/robot_state/src/attached_body.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include @@ -56,6 +57,14 @@ AttachedBody::AttachedBody(const LinkModel* parent_link_model, const std::string , subframe_poses_(subframe_poses) , global_subframe_poses_(subframe_poses) { + for (const auto& t : attach_trans_) + { + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry + } + for (const auto& t : subframe_poses_) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } global_collision_body_transforms_.resize(attach_trans.size()); for (Eigen::Isometry3d& global_collision_body_transform : global_collision_body_transforms_) global_collision_body_transform.setIdentity(); @@ -82,15 +91,17 @@ void AttachedBody::setScale(double scale) void AttachedBody::computeTransform(const Eigen::Isometry3d& parent_link_global_transform) { + ASSERT_ISOMETRY(parent_link_global_transform) // unsanitized input, could contain a non-isometry + // update collision body transforms for (std::size_t i = 0; i < global_collision_body_transforms_.size(); ++i) - global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; + global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i]; // valid isometry // update subframe transforms for (auto global = global_subframe_poses_.begin(), end = global_subframe_poses_.end(), local = subframe_poses_.begin(); global != end; ++global, ++local) - global->second = parent_link_global_transform * local->second; + global->second = parent_link_global_transform * local->second; // valid isometry } void AttachedBody::setPadding(double padding) diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index b2b38db361c..5431b490af3 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -37,6 +37,7 @@ /* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ #include +#include namespace moveit { @@ -59,13 +60,14 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons const kinematics::KinematicsQueryOptions& options) { // this is the Cartesian pose we start from, and have to move in the direction indicated + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& start_pose = start_state->getGlobalLinkTransform(link); // the direction can be in the local reference frame (in which case we rotate it) const Eigen::Vector3d rotated_direction = global_reference_frame ? direction : start_pose.linear() * direction; // The target pose is built by applying a translation to the start pose for the desired direction and distance - Eigen::Isometry3d target_pose = start_pose; + Eigen::Isometry3d target_pose = start_pose; // valid isometry target_pose.translation() += rotated_direction * distance; // call computeCartesianPath for the computed target pose in the global reference frame @@ -86,10 +88,13 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons start_state->enforceBounds(joint); // this is the Cartesian pose we start from, and we move in the direction indicated - Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d start_pose = start_state->getGlobalLinkTransform(link); // valid isometry + + ASSERT_ISOMETRY(target) // unsanitized input, could contain a non-isometry // the target can be in the local reference frame (in which case we rotate it) - Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; + Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; // valid isometry Eigen::Quaterniond start_quaternion(start_pose.linear()); Eigen::Quaterniond target_quaternion(rotated_target.linear()); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index c8dda3b746d..da3750bfd56 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -1202,13 +1203,15 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link const moveit::core::JointModel* root_joint_model = group->getJointModels()[0]; // group->getJointRoots()[0]; const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel(); + // getGlobalLinkTransform() returns a valid isometry by contract Eigen::Isometry3d reference_transform = root_link_model ? getGlobalLinkTransform(root_link_model).inverse() : Eigen::Isometry3d::Identity(); int rows = use_quaternion_representation ? 7 : 6; int columns = group->getVariableCount(); jacobian = Eigen::MatrixXd::Zero(rows, columns); - Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); + // getGlobalLinkTransform() returns a valid isometry by contract + Eigen::Isometry3d link_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry Eigen::Vector3d point_transform = link_transform * reference_point_position; /* @@ -1239,9 +1242,10 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link continue; } unsigned int joint_index = group->getVariableGroupIndex(pjm->getName()); + // getGlobalLinkTransform() returns a valid isometry by contract + joint_transform = reference_transform * getGlobalLinkTransform(link); // valid isometry if (pjm->getType() == moveit::core::JointModel::REVOLUTE) { - joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis.cross(point_transform - joint_transform.translation()); @@ -1249,13 +1253,11 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link } else if (pjm->getType() == moveit::core::JointModel::PRISMATIC) { - joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform.linear() * static_cast(pjm)->getAxis(); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; } else if (pjm->getType() == moveit::core::JointModel::PLANAR) { - joint_transform = reference_transform * getGlobalLinkTransform(link); joint_axis = joint_transform * Eigen::Vector3d(1.0, 0.0, 0.0); jacobian.block<3, 1>(0, joint_index) = jacobian.block<3, 1>(0, joint_index) + joint_axis; joint_axis = joint_transform * Eigen::Vector3d(0.0, 1.0, 0.0); @@ -1775,6 +1777,7 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: // Each each pose's tip frame naming for (std::size_t i = 0; i < poses_in.size(); ++i) { + ASSERT_ISOMETRY(transformed_poses[i]) // unsanitized input, could contain a non-isometry Eigen::Isometry3d& pose = transformed_poses[i]; std::string& pose_frame = pose_frames[i]; @@ -1795,26 +1798,27 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: if (hasAttachedBody(pose_frame)) { const AttachedBody* body = getAttachedBody(pose_frame); - const EigenSTL::vector_Isometry3d& ab_trans = body->getFixedTransforms(); + const EigenSTL::vector_Isometry3d& ab_trans = body->getFixedTransforms(); // valid isometries by contract if (ab_trans.size() != 1) { ROS_ERROR_NAMED(LOGNAME, "Cannot use an attached body with multiple geometries as a reference frame."); return false; } pose_frame = body->getAttachedLinkName(); - pose = pose * ab_trans[0].inverse(); + pose = pose * ab_trans[0].inverse(); // valid isometry } if (pose_frame != solver_tip_frame) { const moveit::core::LinkModel* link_model = getLinkModel(pose_frame); if (!link_model) return false; + // getAssociatedFixedTransforms() returns valid isometries by contract const moveit::core::LinkTransformMap& fixed_links = link_model->getAssociatedFixedTransforms(); for (const std::pair& fixed_link : fixed_links) if (fixed_link.first->getName() == solver_tip_frame) { pose_frame = solver_tip_frame; - pose = pose * fixed_link.second; + pose = pose * fixed_link.second; // valid isometry break; } } @@ -2101,6 +2105,7 @@ void RobotState::printStateInfo(std::ostream& out) const void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const { + ASSERT_ISOMETRY(transform) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(transform.linear()); out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", " << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 8e72213aa68..5fc6a81d9f4 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -86,13 +86,14 @@ class Transforms : private boost::noncopyable /** * @brief Return all the transforms - * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame) + * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame). The + * transforms are guaranteed to be valid isometries. */ const FixedTransformsMap& getAllTransforms() const; /** * @brief Get a vector of all the transforms as ROS messages - * @param transforms The output transforms + * @param transforms The output transforms. They are guaranteed to be valid isometries. */ void copyTransforms(std::vector& transforms) const; @@ -137,40 +138,44 @@ class Transforms : private boost::noncopyable */ void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const { + // getTransform() returns a valid isometry by contract v_out = getTransform(from_frame).linear() * v_in; } /** * @brief Transform a quaternion in from_frame to the target_frame * @param from_frame The frame in which the input quaternion is specified - * @param v_in The input quaternion (in from_frame) - * @param v_out The resultant (transformed) quaternion + * @param v_in The input quaternion (in from_frame). Make sure the quaternion is normalized. + * @param v_out The resultant (transformed) quaternion. It is guaranteed to be a valid and normalized quaternion. */ void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in, Eigen::Quaterniond& q_out) const { + // getTransform() returns a valid isometry by contract q_out = getTransform(from_frame).linear() * q_in; } /** * @brief Transform a rotation matrix in from_frame to the target_frame * @param from_frame The frame in which the input rotation matrix is specified - * @param m_in The input rotation matrix (in from_frame) - * @param m_out The resultant (transformed) rotation matrix + * @param m_in The input rotation matrix (in from_frame). Make sure the matrix is a valid rotation matrix. + * @param m_out The resultant (transformed) rotation matrix. It is guaranteed to be a valid rotation matrix. */ void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const { + // getTransform() returns a valid isometry by contract m_out = getTransform(from_frame).linear() * m_in; } /** * @brief Transform a pose in from_frame to the target_frame * @param from_frame The frame in which the input pose is specified - * @param t_in The input pose (in from_frame) - * @param t_out The resultant (transformed) pose + * @param t_in The input pose (in from_frame). Make sure the pose is a valid isometry. + * @param t_out The resultant (transformed) pose. It is guaranteed to be a valid isometry. */ void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const { + // getTransform() returns a valid isometry by contract t_out = getTransform(from_frame) * t_in; } /**@}*/ @@ -189,7 +194,7 @@ class Transforms : private boost::noncopyable /** * @brief Get transform for from_frame (w.r.t target frame) * @param from_frame The string id of the frame for which the transform is being computed - * @return The required transform + * @return The required transform. It is guaranteed to be a valid isometry. */ virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const; diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index 190fb10d0d4..005bc2e07ff 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -35,6 +35,7 @@ /* Author: Ioan Sucan */ #include +#include #include #include #include @@ -75,6 +76,10 @@ const FixedTransformsMap& Transforms::getAllTransforms() const void Transforms::setAllTransforms(const FixedTransformsMap& transforms) { + for (const auto& t : transforms) + { + ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry + } transforms_map_ = transforms; } @@ -114,6 +119,7 @@ bool Transforms::canTransform(const std::string& from_frame) const void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame) { + ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry if (from_frame.empty()) ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name"); else diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index 3fdde918169..4a667fd489e 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -215,9 +215,11 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const // transform the input vectors in accordance to frame specified in the header; if (approach_direction_is_global_frame) + // getFrameTransform() returns a valid isometry by contract approach_direction = planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).linear() * approach_direction; if (retreat_direction_is_global_frame) + // getFrameTransform() returns a valid isometry by contract retreat_direction = planning_scene_->getFrameTransform(plan->retreat_.direction.header.frame_id).linear() * retreat_direction; diff --git a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp index 4dc408396ac..7030c918da0 100644 --- a/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp +++ b/moveit_ros/planning/planning_components_tools/src/kinematics_speed_and_validity_evaluator.cpp @@ -87,13 +87,15 @@ int main(int argc, char** argv) for (unsigned int i = 0; i < test_count; ++i) { state.setToRandomPositions(jmg); + // getGlobalLinkTransform() returns a valid isometry by contract Eigen::Isometry3d pose = state.getGlobalLinkTransform(tip); state.setToRandomPositions(jmg); moveit::tools::Profiler::Begin("IK"); state.setFromIK(jmg, pose); moveit::tools::Profiler::End("IK"); + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& pose_upd = state.getGlobalLinkTransform(tip); - Eigen::Isometry3d diff = pose_upd * pose.inverse(); + Eigen::Isometry3d diff = pose_upd * pose.inverse(); // valid isometry double rot_err = (diff.linear() - Eigen::Matrix3d::Identity()).norm(); double trans_err = diff.translation().norm(); moveit::tools::Profiler::Average("Rotation error", rot_err); diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 1db84cf7ac7..c07c592fb84 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -1005,8 +1006,10 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont // compute difference (offset vector and rotation angle) between current transform // and start transform in trajectory Eigen::Isometry3d cur_transform, start_transform; + // computeTransform() computes a valid isometry by contract jm->computeTransform(current_state->getJointPositions(jm), cur_transform); start_transform = tf2::transformToEigen(transforms[i]); + ASSERT_ISOMETRY(start_transform) // unsanitized input, could contain a non-isometry Eigen::Vector3d offset = cur_transform.translation() - start_transform.translation(); Eigen::AngleAxisd rotation; rotation.fromRotationMatrix(cur_transform.linear().transpose() * start_transform.linear()); diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp index 35c1e047ad9..f6e3d400614 100644 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp @@ -150,6 +150,7 @@ class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer const moveit::core::LinkModel* lm = state->getLinkModel(name); if (lm) { + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Isometry3d& t = state->getGlobalLinkTransform(lm); std::vector v(7); v[0] = t.translation().x(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp index 8573ba7f510..85cf1631e1d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp @@ -237,7 +237,7 @@ void MotionPlanningFrame::selectedCollisionObjectChanged() if (obj->shapes_.size() == 1) { - obj_pose = obj->shape_poses_[0]; + obj_pose = obj->shape_poses_[0]; // valid isometry by contract Eigen::Vector3d xyz = obj_pose.linear().eulerAngles(0, 1, 2); update_scene_marker = true; // do the marker update outside locked scope to avoid deadlock @@ -322,6 +322,7 @@ void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position) // Update the interactive marker pose to match the manually introduced one if (update_marker_position && scene_marker_) { + // p is isometry by construction Eigen::Quaterniond eq(p.linear()); scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()), Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), ""); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp index 2455db97ca6..26259978cb2 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_link_updater.cpp @@ -51,6 +51,7 @@ bool moveit_rviz_plugin::PlanningLinkUpdater::getLinkTransforms(const std::strin return false; } + // getGlobalLinkTransform() returns a valid isometry by contract const Eigen::Vector3d& robot_visual_position = kinematic_state_->getGlobalLinkTransform(link_model).translation(); Eigen::Quaterniond robot_visual_orientation(kinematic_state_->getGlobalLinkTransform(link_model).linear()); visual_position = Ogre::Vector3(robot_visual_position.x(), robot_visual_position.y(), robot_visual_position.z()); diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp index 19e06f150c2..f2ddd86aeb1 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/render_shapes.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -82,6 +83,7 @@ void RenderShapes::renderShape(Ogre::SceneNode* node, const shapes::Shape* s, co rviz::Shape* ogre_shape = nullptr; Eigen::Vector3d translation = p.translation(); Ogre::Vector3 position(translation.x(), translation.y(), translation.z()); + ASSERT_ISOMETRY(p) // unsanitized input, could contain a non-isometry Eigen::Quaterniond q(p.linear()); Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z());