Skip to content

Commit

Permalink
Deprecate RigidTransform/Isometry3 automatic conversion (RobotLocomot…
Browse files Browse the repository at this point in the history
…ion#13595)

* Deprecate RigidTransform/Isometry3 automatic conversion
  • Loading branch information
jwnimmer-tri authored Jun 26, 2020
1 parent cacac0a commit 24113fc
Show file tree
Hide file tree
Showing 21 changed files with 126 additions and 71 deletions.
1 change: 1 addition & 0 deletions bindings/pydrake/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,7 @@ drake_pybind_library(
"//bindings/pydrake:symbolic_types_pybind",
"//bindings/pydrake/common:cpp_template_pybind",
"//bindings/pydrake/common:default_scalars_pybind",
"//bindings/pydrake/common:deprecation_pybind",
"//bindings/pydrake/common:type_pack",
"//bindings/pydrake/common:value_pybind",
],
Expand Down
58 changes: 43 additions & 15 deletions bindings/pydrake/math_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include "drake/bindings/pydrake/autodiff_types_pybind.h"
#include "drake/bindings/pydrake/common/cpp_template_pybind.h"
#include "drake/bindings/pydrake/common/default_scalars_pybind.h"
#include "drake/bindings/pydrake/common/deprecation_pybind.h"
#include "drake/bindings/pydrake/common/type_pack.h"
#include "drake/bindings/pydrake/common/value_pybind.h"
#include "drake/bindings/pydrake/documentation_pybind.h"
Expand Down Expand Up @@ -42,13 +43,6 @@ void DoScalarDependentDefinitions(py::module m, T) {
constexpr auto& doc = pydrake_doc.drake.math;

{
// N.B. Keep the deprecation date here in sync with the deprecation comment
// inside drake/math/rigid_transform.h.
const char* doc_rigid_transform_linear_matrix_deprecation =
"DO NOT USE! We offer this API for backwards compatibility with "
"Isometry3, but it will be removed on or around 2020-07-01. "
"See drake issue #9865 for details.";

using Class = RigidTransform<T>;
constexpr auto& cls_doc = doc.RigidTransform;
auto cls = DefineTemplateClassWithDefault<Class>(
Expand Down Expand Up @@ -122,11 +116,36 @@ void DoScalarDependentDefinitions(py::module m, T) {
return *self * p_BoQ_B;
},
py::arg("p_BoQ_B"),
cls_doc.operator_mul.doc_1args_constEigenMatrixBase)
.def("matrix", &RigidTransform<T>::matrix,
doc_rigid_transform_linear_matrix_deprecation)
.def("linear", &RigidTransform<T>::linear, py_reference_internal,
doc_rigid_transform_linear_matrix_deprecation)
cls_doc.operator_mul.doc_1args_constEigenMatrixBase);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
cls // BR
.def(
"matrix",
[](RigidTransform<T>* self) {
WarnDeprecated(
"The RigidTransform::matrix() method was added for "
"compatibility with Eigen::Isometry3, and is now deprecated. "
"Use RigidTransform::GetAsMatrix4() instead.");
return self->GetAsIsometry3();
},
"The RigidTransform::matrix() method was added for "
"compatibility with Eigen::Isometry3, and is now deprecated. "
"Use RigidTransform::GetAsMatrix4() instead.")
.def(
"linear",
[](RigidTransform<T>* self) {
WarnDeprecated(
"The RigidTransform::linear() method was added for "
"compatibility with Eigen::Isometry3, and is now deprecated. "
"Use RigidTransform::rotation().matrix() instead.");
return self->GetAsMatrix4();
},
"The RigidTransform::linear() method was added for "
"compatibility with Eigen::Isometry3, and is now deprecated. "
"Use RigidTransform::rotation().matrix() instead.");
#pragma GCC diagnostic pop
cls // BR
.def(py::pickle([](const Class& self) { return self.GetAsMatrix34(); },
[](const Eigen::Matrix<T, 3, 4>& matrix) {
return Class(matrix);
Expand Down Expand Up @@ -270,12 +289,21 @@ void DoScalarDependentDefinitions(py::module m, T) {
auto eigen_geometry_py = py::module::import("pydrake.common.eigen_geometry");
// Install constructor to Isometry3 to permit `implicitly_convertible` to
// work.
// TODO(eric.cousineau): Add deprecation message.
// TODO(eric.cousineau): Consider more elegant implicit conversion process.
// See pybind/pybind11#1735
py::class_<Isometry3<T>>(eigen_geometry_py.attr("Isometry3"))
.def(py::init(
[](const RigidTransform<T>& X) { return X.GetAsIsometry3(); }));
.def(py::init([](const RigidTransform<T>& X) {
WarnDeprecated(
"Implicit conversion from RigidTransform to Eigen::Isometry3 is "
"deprecated. You may convert explicitly by calling "
"GetAsIsometry3(), or try to avoid using Eigen::Isometry3 in the "
"first place.");
return X.GetAsIsometry3();
}),
"Implicit conversion from RigidTransform to Eigen::Isometry3 is "
"deprecated. You may convert explicitly by calling "
"GetAsIsometry3(), or try to avoid using Eigen::Isometry3 in the "
"first place.");
py::implicitly_convertible<RigidTransform<T>, Isometry3<T>>();
}

Expand Down
3 changes: 2 additions & 1 deletion bindings/pydrake/multibody/test/plant_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -597,7 +597,8 @@ def test_multibody_tree_kinematics(self, T):
plant.SetFreeBodyPose(
context=context, body=base, X_WB=X_WB_desired)
numpy_compare.assert_float_equal(
X_WB.matrix(), numpy_compare.to_float(X_WB_desired.matrix()))
X_WB.GetAsMatrix4(),
numpy_compare.to_float(X_WB_desired.GetAsMatrix4()))

# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
Expand Down
3 changes: 2 additions & 1 deletion bindings/pydrake/systems/perception.py
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,8 @@ def _AlignPointClouds(self, context):
X_CiSi = self.EvalAbstractInput(
context, self._transform_ports[id].get_index()).get_value()

points[id] = _TransformPoints(point_cloud.xyzs(), X_CiSi.matrix())
points[id] = _TransformPoints(
point_cloud.xyzs(), X_CiSi.GetAsMatrix4())

if point_cloud.has_rgbs():
colors[id] = point_cloud.rgbs()
Expand Down
4 changes: 2 additions & 2 deletions bindings/pydrake/systems/test/perception_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -70,15 +70,15 @@ def setUp(self):

def test_translation(self):
transformed_points = _TransformPoints(
self.points, self.translation.matrix())
self.points, self.translation.GetAsMatrix4())
expected_translated_points = np.array([[2, 3, 3], [3, 3, 3]]).T

self.assertTrue(
np.allclose(transformed_points, expected_translated_points))

def test_rotation(self):
transformed_points = _TransformPoints(
self.points, self.rotation.matrix())
self.points, self.rotation.GetAsMatrix4())
expected_rotated_points = np.array([[-1, 1, 0], [-1, 2, 0]]).T

self.assertTrue(
Expand Down
3 changes: 2 additions & 1 deletion bindings/pydrake/test/math_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -188,7 +188,8 @@ def test_isometry_implicit(self, T):
# Explicitly disabled, to mirror C++ API.
with self.assertRaises(TypeError):
self.assertTrue(mtest.TakeRigidTransform(Isometry3()))
self.assertTrue(mtest.TakeIsometry3(mut.RigidTransform()))
with catch_drake_warnings(expected_count=1):
self.assertTrue(mtest.TakeIsometry3(mut.RigidTransform()))

@numpy_compare.check_all_types
def test_rotation_matrix(self, T):
Expand Down
8 changes: 3 additions & 5 deletions examples/pendulum/test/pendulum_geometry_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,7 @@ math::RigidTransformd ExtractSinglePose(
const geometry::FramePoseVector<double>& pose_vector) {
DRAKE_THROW_UNLESS(pose_vector.size() == 1);
for (const auto& id : pose_vector.frame_ids()) {
RigidTransformd result;
result.SetFromIsometry3(pose_vector.value(id));
return result;
return pose_vector.value(id);
}
DRAKE_UNREACHABLE();
}
Expand Down Expand Up @@ -58,8 +56,8 @@ GTEST_TEST(PendulumGeometryTest, AcceptanceTest) {
const auto& geom_output_value =
geom->get_output_port(0).Eval<FramePoseVector<double>>(geom_context);
EXPECT_TRUE(CompareMatrices(
ExtractSinglePose(geom_output_value).matrix(),
RigidTransformd(RotationMatrixd::MakeYRotation(theta)).matrix(),
ExtractSinglePose(geom_output_value).GetAsMatrix4(),
RigidTransformd(RotationMatrixd::MakeYRotation(theta)).GetAsMatrix4(),
tolerance));

// Check other stuff.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -239,8 +239,8 @@ TEST_F(DifferentialInverseKinematicsTest, SimpleTracker) {
plant_->SetPositions(context_, q + v * dt);
}
X_WE = frame_E_->CalcPoseInWorld(*context_);
EXPECT_TRUE(CompareMatrices(X_WE.matrix(), X_WE_desired.matrix(), 1e-5,
MatrixCompareType::absolute));
EXPECT_TRUE(CompareMatrices(X_WE.GetAsMatrix4(), X_WE_desired.GetAsMatrix4(),
1e-5, MatrixCompareType::absolute));
}

// Test various throw conditions.
Expand Down
13 changes: 13 additions & 0 deletions math/rigid_transform.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include "drake/common/drake_assert.h"
#include "drake/common/drake_bool.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_deprecated.h"
#include "drake/common/eigen_types.h"
#include "drake/common/never_destroyed.h"
#include "drake/math/rotation_matrix.h"
Expand Down Expand Up @@ -433,8 +434,20 @@ class RigidTransform {
// possibly marked as deprecated) until at least 2020-07-01. N.B. Keep the
// deprecation date here in sync with the deprecation comment inside
// drake/bindings/pydrake/math_py.cc.
DRAKE_DEPRECATED("2020-10-01",
"Implicit conversion from RigidTransform to Eigen::Isometry3 is "
"deprecated. You may convert explicitly by calling GetAsIsometry3(), "
"or try to avoid using Eigen::Isometry3 in the first place.")
operator Isometry3<T>() const { return GetAsIsometry3(); }
DRAKE_DEPRECATED("2020-10-01",
"The RigidTransform::linear() method was added for compatibility with "
"Eigen::Isometry3, and is now deprecated. "
"Use RigidTransform::rotation().matrix() instead.")
const Matrix3<T>& linear() const { return R_AB_.matrix(); }
DRAKE_DEPRECATED("2020-10-01",
"The RigidTransform::matrix() method was added for compatibility with "
"Eigen::Isometry3, and is now deprecated. "
"Use RigidTransform::GetAsMatrix4() instead.")
Matrix4<T> matrix() const { return GetAsMatrix4(); }
#endif

Expand Down
3 changes: 3 additions & 0 deletions math/test/rigid_transform_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,8 @@ GTEST_TEST(RigidTransform, DefaultRigidTransformIsIdentity) {
EXPECT_TRUE(X.IsExactlyIdentity());
}

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
// This tests the conversion operator from a RigidTransform to an Isometry3.
// This test will fail once the conversion operator is removed per
// resolution of #9865, and thus this test should be removed as well.
Expand All @@ -101,6 +103,7 @@ GTEST_TEST(RigidTransform, ImplicitConstructionFromIsometry3) {
const Isometry3<double>& Xiso = X;
EXPECT_TRUE(Xiso.isApprox(X.GetAsIsometry3(), 0));
}
#pragma GCC diagnostic pop

// Tests constructing a RigidTransform from a RotationMatrix and Vector3.
// Also test the set method.
Expand Down
15 changes: 10 additions & 5 deletions multibody/parsing/test/detail_sdf_parser_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,8 @@ GTEST_TEST(MultibodyPlantSdfParserTest, ModelInstanceTest) {
plant.GetFrameByName(parent_name, instance1);
const RigidTransformd X_PF = plant.CalcRelativeTransform(
*context, parent_frame, frame);
EXPECT_TRUE(CompareMatrices(X_PF_expected.matrix(), X_PF.matrix(), kEps))
EXPECT_TRUE(CompareMatrices(
X_PF_expected.GetAsMatrix4(), X_PF.GetAsMatrix4(), kEps))
<< name;
};

Expand Down Expand Up @@ -265,12 +266,14 @@ GTEST_TEST(SdfParser, FloatingBodyPose) {
RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3));
const RigidTransformd X_WA =
pair.plant->GetFrameByName("a").CalcPoseInWorld(*context);
EXPECT_TRUE(CompareMatrices(X_WA_expected.matrix(), X_WA.matrix(), kEps));
EXPECT_TRUE(CompareMatrices(
X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps));
const RigidTransformd X_WB_expected(
RollPitchYawd(0.4, 0.5, 0.6), Vector3d(4, 5, 6));
const RigidTransformd X_WB =
pair.plant->GetFrameByName("b").CalcPoseInWorld(*context);
EXPECT_TRUE(CompareMatrices(X_WB_expected.matrix(), X_WB.matrix(), kEps));
EXPECT_TRUE(CompareMatrices(
X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps));
}

GTEST_TEST(SdfParser, StaticModelSupported) {
Expand All @@ -292,12 +295,14 @@ GTEST_TEST(SdfParser, StaticModelSupported) {
RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3));
const RigidTransformd X_WA =
pair.plant->GetFrameByName("a").CalcPoseInWorld(*context);
EXPECT_TRUE(CompareMatrices(X_WA_expected.matrix(), X_WA.matrix(), kEps));
EXPECT_TRUE(CompareMatrices(
X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps));
const RigidTransformd X_WB_expected(
RollPitchYawd(0.4, 0.5, 0.6), Vector3d(4, 5, 6));
const RigidTransformd X_WB =
pair.plant->GetFrameByName("b").CalcPoseInWorld(*context);
EXPECT_TRUE(CompareMatrices(X_WB_expected.matrix(), X_WB.matrix(), kEps));
EXPECT_TRUE(CompareMatrices(
X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps));
}

GTEST_TEST(SdfParser, StaticModelWithJoints) {
Expand Down
2 changes: 1 addition & 1 deletion multibody/parsing/test/detail_tinyxml_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ GTEST_TEST(TinyxmlUtilTest, OriginAttributesTest) {
const RigidTransformd actual = internal::OriginAttributesToTransform(element);
const RigidTransformd expected(RollPitchYawd(1, 2, 3), Vector3d(4, 5, 6));

EXPECT_TRUE(CompareMatrices(actual.matrix(), expected.matrix()));
EXPECT_TRUE(CompareMatrices(actual.GetAsMatrix4(), expected.GetAsMatrix4()));
}

GTEST_TEST(TinyxmlUtilTest, MalformedRpyOriginAttributesTest) {
Expand Down
5 changes: 3 additions & 2 deletions multibody/parsing/test/detail_urdf_parser_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,9 @@ GTEST_TEST(MultibodyPlantUrdfParserTest, DoublePendulum) {
math::RigidTransform<double> X_BF_expected(rpy_expected.ToRotationMatrix(),
xyz_expected);

EXPECT_TRUE(CompareMatrices(frame_on_link1.GetFixedPoseInBodyFrame().matrix(),
X_BF_expected.GetAsMatrix4(), 1e-10));
EXPECT_TRUE(CompareMatrices(
frame_on_link1.GetFixedPoseInBodyFrame().GetAsMatrix4(),
X_BF_expected.GetAsMatrix4(), 1e-10));

const Frame<double>& link2_com = plant.GetFrameByName("link2_com");
EXPECT_EQ(link2_com.body().index(), plant.GetBodyByName("link2").index());
Expand Down
8 changes: 3 additions & 5 deletions multibody/plant/test/multibody_plant_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -799,8 +799,7 @@ TEST_F(AcrobotPlantTests, VisualGeometryRegistration) {
ASSERT_TRUE(optional_id.has_value());
EXPECT_EQ(frame_id, *optional_id);
EXPECT_EQ(body_index, plant_->GetBodyFromFrameId(frame_id)->index());
const Isometry3<double>& X_WB_isometry = poses.value(frame_id);
const RigidTransform<double> X_WB(X_WB_isometry);
const RigidTransform<double>& X_WB = poses.value(frame_id);
const RigidTransform<double>& X_WB_expected =
plant_->EvalBodyPoseInWorld(*context, plant_->get_body(body_index));
EXPECT_TRUE(CompareMatrices(X_WB.GetAsMatrix34(),
Expand Down Expand Up @@ -1421,8 +1420,7 @@ GTEST_TEST(MultibodyPlantTest, CollisionGeometryRegistration) {
for (BodyIndex body_index(1);
body_index < plant.num_bodies(); ++body_index) {
const FrameId frame_id = plant.GetBodyFrameIdOrThrow(body_index);
const Isometry3<double>& X_WB_isometry = pose_data.value(frame_id);
const RigidTransform<double> X_WB(X_WB_isometry);
const RigidTransform<double>& X_WB = pose_data.value(frame_id);
const RigidTransform<double>& X_WB_expected =
plant.EvalBodyPoseInWorld(*context, plant.get_body(body_index));
EXPECT_TRUE(CompareMatrices(X_WB.GetAsMatrix34(),
Expand Down Expand Up @@ -2885,7 +2883,7 @@ GTEST_TEST(StateSelection, FloatingBodies) {
const RigidTransformd X_WM_expected = X_WT * X_TO * X_OM;

const double kTolerance = 5 * std::numeric_limits<double>::epsilon();
EXPECT_TRUE(CompareMatrices(X_WM.matrix(), X_WM_expected.matrix(),
EXPECT_TRUE(CompareMatrices(X_WM.GetAsMatrix4(), X_WM_expected.GetAsMatrix4(),
kTolerance, MatrixCompareType::relative));

// SetFreeBodyPoseInAnchoredFrame() should throw if the reference frame F is
Expand Down
3 changes: 2 additions & 1 deletion systems/controllers/test/setpoint_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,8 @@ GTEST_TEST(testQPInverseDynamicsController, testPoseSetpoint) {

for (double ang = ang_d; ang < ang_d + 2 * M_PI + 0.1; ang += 0.1) {
pose.set_rotation(AngleAxis<double>(ang, vec_d));
acc = setpoint.ComputeTargetAcceleration(pose, Vector6<double>::Zero());
acc = setpoint.ComputeTargetAcceleration(
pose.GetAsIsometry3(), Vector6<double>::Zero());

double err = ang_d - ang;
if (err > M_PI)
Expand Down
20 changes: 10 additions & 10 deletions systems/rendering/test/pose_aggregator_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -132,25 +132,25 @@ TEST_F(PoseAggregatorTest, CompositeAggregation) {
EXPECT_EQ("bundle::Sherlock", bundle.get_name(0));
EXPECT_EQ(kSherlockModelInstanceId, bundle.get_model_instance_id(0));
const RigidTransformd& generic_pose_0 = bundle.get_transform(0);
EXPECT_TRUE(CompareMatrices(RigidTransformd(translation_0).matrix(),
generic_pose_0.matrix()));
EXPECT_TRUE(CompareMatrices(RigidTransformd(translation_0).GetAsMatrix4(),
generic_pose_0.GetAsMatrix4()));
EXPECT_TRUE(CompareMatrices(sherlock_velocity.get_value(),
bundle.get_velocity(0).get_value()));

EXPECT_EQ("bundle::Mycroft", bundle.get_name(1));
EXPECT_EQ(kMycroftModelInstanceId, bundle.get_model_instance_id(1));
const RigidTransformd& generic_pose_1 = bundle.get_transform(1);
EXPECT_TRUE(CompareMatrices(RigidTransformd(translation_1).matrix(),
generic_pose_1.matrix()));
EXPECT_TRUE(CompareMatrices(RigidTransformd(translation_1).GetAsMatrix4(),
generic_pose_1.GetAsMatrix4()));

// Check that the PoseVector pose with velocity is passed through to the
// output.
EXPECT_EQ("single_xv", bundle.get_name(2));
EXPECT_EQ(42, bundle.get_model_instance_id(2));
const RigidTransformd& xv_pose = bundle.get_transform(2);
EXPECT_TRUE(CompareMatrices(
RigidTransformd(Vector3d(0.5, 1.5, 2.5)).matrix(),
xv_pose.matrix()));
RigidTransformd(Vector3d(0.5, 1.5, 2.5)).GetAsMatrix4(),
xv_pose.GetAsMatrix4()));
for (int i = 0; i < 6; ++i) {
EXPECT_EQ(1.0 + i, bundle.get_velocity(2)[i]);
}
Expand All @@ -163,8 +163,8 @@ TEST_F(PoseAggregatorTest, CompositeAggregation) {
EXPECT_TRUE(CompareMatrices(
RigidTransformd(Eigen::Quaternion<double>(0.5, 0.5, 0.5, 0.5),
Vector3d{0, 0, 0})
.matrix(),
x_pose.matrix()));
.GetAsMatrix4(),
x_pose.GetAsMatrix4()));

// The sequel checks the AutoDiffXd conversion.
auto autodiff_aggregator = aggregator_.ToAutoDiffXd();
Expand All @@ -187,9 +187,9 @@ TEST_F(PoseAggregatorTest, CompositeAggregation) {
ASSERT_EQ(bundle.get_num_poses(), autodiff_bundle.get_num_poses());
for (int i = 0; i < bundle.get_num_poses(); i++) {
EXPECT_TRUE(
CompareMatrices(bundle.get_transform(i).matrix(),
CompareMatrices(bundle.get_transform(i).GetAsMatrix4(),
math::autoDiffToValueMatrix(
autodiff_bundle.get_transform(i).matrix())));
autodiff_bundle.get_transform(i).GetAsMatrix4())));
EXPECT_TRUE(
CompareMatrices(bundle.get_velocity(i).get_value(),
math::autoDiffToValueMatrix(
Expand Down
Loading

0 comments on commit 24113fc

Please sign in to comment.