From f062738a338ab26776d24c14e6d5d79b2280a79c Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Tue, 14 Feb 2023 14:20:54 -0800 Subject: [PATCH] [planning] Opt-in to clang-format lint (#18701) --- .clang-format | 4 +- bindings/pydrake/.clang-format | 10 +- lcm/drake_lcm.cc | 6 +- planning/BUILD.bazel | 2 +- planning/body_shape_description.cc | 12 +- planning/collision_checker.cc | 139 ++++++++---------- planning/collision_checker_params.h | 5 +- planning/robot_diagram.cc | 18 +-- planning/robot_diagram.h | 15 +- planning/robot_diagram_builder.h | 38 ++--- planning/scene_graph_collision_checker.cc | 27 ++-- planning/test/collision_avoidance_test.cc | 32 ++-- planning/test/collision_checker_test.cc | 128 ++++++++-------- planning/test/planning_test_helpers.cc | 13 +- planning/test/planning_test_helpers.h | 5 +- planning/test/robot_diagram_test.cc | 7 +- .../scene_graph_collision_checker_test.cc | 115 +++++++-------- .../unimplemented_collision_checker_test.cc | 11 +- planning/test_utilities/BUILD.bazel | 2 +- .../collision_checker_abstract_test_suite.cc | 52 +++---- .../collision_checker_abstract_test_suite.h | 4 +- planning/unimplemented_collision_checker.cc | 4 +- 22 files changed, 299 insertions(+), 350 deletions(-) diff --git a/.clang-format b/.clang-format index 47890b7d64fc..1a2198f112d4 100644 --- a/.clang-format +++ b/.clang-format @@ -12,8 +12,8 @@ DerivePointerAlignment: false PointerAlignment: Left # Compress functions onto a single line (when they fit) iff they are defined -# inline (inside a of class). -AllowShortFunctionsOnASingleLine: InlineOnly +# inline (inside a of class) or are empty. +AllowShortFunctionsOnASingleLine: Inline # Compress lambdas onto a single line iff they are empty. AllowShortLambdasOnASingleLine: Empty diff --git a/bindings/pydrake/.clang-format b/bindings/pydrake/.clang-format index 9452816a734d..7beb34841101 100644 --- a/bindings/pydrake/.clang-format +++ b/bindings/pydrake/.clang-format @@ -12,8 +12,8 @@ DerivePointerAlignment: false PointerAlignment: Left # Compress functions onto a single line (when they fit) iff they are defined -# inline (inside a of class). -AllowShortFunctionsOnASingleLine: InlineOnly +# inline (inside a of class) or are empty. +AllowShortFunctionsOnASingleLine: Inline # Compress lambdas onto a single line iff they are empty. AllowShortLambdasOnASingleLine: Empty @@ -59,11 +59,5 @@ IncludeCategories: # Avoid widening out code horizontally a lot. AlignAfterOpenBracket: DontAlign -# Try to fit onto a single line: -# - functions with an empty body, or -# - member functions defined inside a class; -# everything else (e.g., free functions) are forced onto multiple lines. -AllowShortFunctionsOnASingleLine: Inline - # Compress lambdas as much as possible. AllowShortLambdasOnASingleLine: All diff --git a/lcm/drake_lcm.cc b/lcm/drake_lcm.cc index c3d560132fbf..343a7b30333f 100644 --- a/lcm/drake_lcm.cc +++ b/lcm/drake_lcm.cc @@ -70,12 +70,10 @@ class DrakeLcm::Impl { std::string handle_subscriptions_error_message_; }; -DrakeLcm::DrakeLcm() : DrakeLcm(std::string{}) { -} +DrakeLcm::DrakeLcm() : DrakeLcm(std::string{}) {} DrakeLcm::DrakeLcm(std::string lcm_url) - : DrakeLcm(DrakeLcmParams{.lcm_url = std::move(lcm_url)}) { -} + : DrakeLcm(DrakeLcmParams{.lcm_url = std::move(lcm_url)}) {} DrakeLcm::DrakeLcm(const DrakeLcmParams& params) : impl_(std::make_unique(params)) { diff --git a/planning/BUILD.bazel b/planning/BUILD.bazel index b3d12c705a03..22d445655fb3 100644 --- a/planning/BUILD.bazel +++ b/planning/BUILD.bazel @@ -265,4 +265,4 @@ drake_cc_googletest( ], ) -add_lint_tests() +add_lint_tests(enable_clang_format_lint = True) diff --git a/planning/body_shape_description.cc b/planning/body_shape_description.cc index e965958c0299..97e70d5dcb94 100644 --- a/planning/body_shape_description.cc +++ b/planning/body_shape_description.cc @@ -7,11 +7,10 @@ namespace drake { namespace planning { -BodyShapeDescription::BodyShapeDescription( - const geometry::Shape& shape, - const math::RigidTransformd& X_BS, - std::string model_instance_name, - std::string body_name) +BodyShapeDescription::BodyShapeDescription(const geometry::Shape& shape, + const math::RigidTransformd& X_BS, + std::string model_instance_name, + std::string body_name) : shape_(shape.Clone()), X_BS_(X_BS), model_instance_name_(std::move(model_instance_name)), @@ -37,8 +36,7 @@ BodyShapeDescription MakeBodyShapeDescription( const auto frame_id = inspector.GetFrameId(geometry_id); // inspector gives us the shape's pose w.r.t. the parent geometry frame F. We // rely on MbP registering the geometry frame F to the body B as X_BF = I. - const math::RigidTransformd& X_BS = - inspector.GetPoseInFrame(geometry_id); + const math::RigidTransformd& X_BS = inspector.GetPoseInFrame(geometry_id); const auto body = plant.GetBodyFromFrameId(frame_id); DRAKE_DEMAND(body != nullptr); const std::string& model_instance_name = diff --git a/planning/collision_checker.cc b/planning/collision_checker.cc index 5cea78c31a32..cb080568c36d 100644 --- a/planning/collision_checker.cc +++ b/planning/collision_checker.cc @@ -37,8 +37,8 @@ using multibody::JointIndex; using multibody::ModelInstanceIndex; using multibody::MultibodyPlant; using multibody::world_model_instance; -using systems::Context; using std::move; +using systems::Context; // Default interpolator; it uses SLERP for quaternion-valued groups of dofs and // LERP for everything else. See the documentation in CollisionChecker's @@ -46,8 +46,9 @@ using std::move; ConfigurationInterpolationFunction MakeDefaultConfigurationInterpolationFunction( const std::vector& quaternion_dof_start_indices) { - return [quaternion_dof_start_indices]( - const Eigen::VectorXd& q_1, const Eigen::VectorXd& q_2, double ratio) { + return [quaternion_dof_start_indices](const Eigen::VectorXd& q_1, + const Eigen::VectorXd& q_2, + double ratio) { // Start with linear interpolation between q_1 and q_2. Eigen::VectorXd interpolated_q = common_robotics_utilities::math::InterpolateXd(q_1, q_2, ratio); @@ -74,8 +75,7 @@ std::vector GetQuaternionDofStartIndices( for (BodyIndex body_index(0); body_index < plant.num_bodies(); ++body_index) { const auto& body = plant.get_body(body_index); if (body.has_quaternion_dofs()) { - quaternion_dof_start_indices.push_back( - body.floating_positions_start()); + quaternion_dof_start_indices.push_back(body.floating_positions_start()); } } return quaternion_dof_start_indices; @@ -191,13 +191,11 @@ CollisionChecker::MakeStandaloneModelContext() const { } void CollisionChecker::PerformOperationAgainstAllModelContexts( - const std::function&, - CollisionCheckerContext*)>& operation) { + const std::function&, + CollisionCheckerContext*)>& operation) { DRAKE_THROW_UNLESS(operation != nullptr); - owned_contexts_.PerformOperationAgainstAllOwnedContexts( - model(), operation); - standalone_contexts_.PerformOperationAgainstAllStandaloneContexts( + owned_contexts_.PerformOperationAgainstAllOwnedContexts(model(), operation); + standalone_contexts_.PerformOperationAgainstAllStandaloneContexts( // BR model(), operation); } @@ -234,13 +232,10 @@ std::map CollisionChecker::AddCollisionShapes( } bool CollisionChecker::AddCollisionShapeToFrame( - const std::string& group_name, - const Frame& frameA, - const Shape& shape, - const RigidTransform& X_AG) { + const std::string& group_name, const Frame& frameA, + const Shape& shape, const RigidTransform& X_AG) { const Body& bodyA = frameA.body(); - const RigidTransform& X_BA = - frameA.GetFixedPoseInBodyFrame(); + const RigidTransform& X_BA = frameA.GetFixedPoseInBodyFrame(); const RigidTransform X_BG = X_BA * X_AG; return AddCollisionShapeToBody(group_name, bodyA, shape, X_BG); } @@ -290,8 +285,8 @@ void CollisionChecker::RemoveAllAddedCollisionShapes() { geometry_groups_.clear(); } -std::optional -CollisionChecker::MaybeGetUniformRobotEnvironmentPadding() const { +std::optional CollisionChecker::MaybeGetUniformRobotEnvironmentPadding() + const { // TODO(SeanCurtis-TRI): We have three functions that walk a triangular // portion of the padding matrix. Consider unifying the logic for the // triangular indices. @@ -316,8 +311,8 @@ CollisionChecker::MaybeGetUniformRobotEnvironmentPadding() const { return check_padding; } -std::optional -CollisionChecker::MaybeGetUniformRobotRobotPadding() const { +std::optional CollisionChecker::MaybeGetUniformRobotRobotPadding() + const { std::optional check_padding; for (BodyIndex body_index(0); body_index < plant().num_bodies(); ++body_index) { @@ -342,10 +337,10 @@ CollisionChecker::MaybeGetUniformRobotRobotPadding() const { void CollisionChecker::SetPaddingBetween(BodyIndex bodyA_index, BodyIndex bodyB_index, double padding) { - DRAKE_THROW_UNLESS( - bodyA_index >= 0 && bodyA_index < collision_padding_.rows()); - DRAKE_THROW_UNLESS( - bodyB_index >= 0 && bodyB_index < collision_padding_.rows()); + DRAKE_THROW_UNLESS(bodyA_index >= 0 && + bodyA_index < collision_padding_.rows()); + DRAKE_THROW_UNLESS(bodyB_index >= 0 && + bodyB_index < collision_padding_.rows()); DRAKE_THROW_UNLESS(bodyA_index != bodyB_index); DRAKE_THROW_UNLESS(std::isfinite(padding)); DRAKE_THROW_UNLESS(IsPartOfRobot(get_body(bodyA_index)) || @@ -434,13 +429,12 @@ void CollisionChecker::SetCollisionFilterMatrix( // Now test for consistency. ValidateFilteredCollisionMatrix(filter_matrix, __func__); filtered_collisions_ = filter_matrix; - log()->debug( - "Set collision filter matrix to:\n{}", filtered_collisions_); + log()->debug("Set collision filter matrix to:\n{}", filtered_collisions_); UpdateMaxCollisionPadding(); } -bool CollisionChecker::IsCollisionFilteredBetween( - BodyIndex bodyA_index, BodyIndex bodyB_index) const { +bool CollisionChecker::IsCollisionFilteredBetween(BodyIndex bodyA_index, + BodyIndex bodyB_index) const { DRAKE_THROW_UNLESS(bodyA_index >= 0 && bodyA_index < filtered_collisions_.rows()); DRAKE_THROW_UNLESS(bodyB_index >= 0 && @@ -470,8 +464,8 @@ void CollisionChecker::SetCollisionFilteredBetween(BodyIndex bodyA_index, } void CollisionChecker::SetCollisionFilteredWithAllBodies(BodyIndex body_index) { - DRAKE_THROW_UNLESS( - body_index >= 0 && body_index < filtered_collisions_.rows()); + DRAKE_THROW_UNLESS(body_index >= 0 && + body_index < filtered_collisions_.rows()); DRAKE_THROW_UNLESS(IsPartOfRobot(body_index)); filtered_collisions_.row(body_index).setConstant(1); filtered_collisions_.col(body_index).setConstant(1); @@ -520,7 +514,7 @@ void CollisionChecker::SetConfigurationDistanceFunction( ConfigurationDistanceFunction CollisionChecker::MakeStandaloneConfigurationDistanceFunction() const { - return [this] (const Eigen::VectorXd& q_1, const Eigen::VectorXd& q_2) { + return [this](const Eigen::VectorXd& q_1, const Eigen::VectorXd& q_2) { return this->ComputeConfigurationDistance(q_1, q_2); }; } @@ -535,25 +529,25 @@ void CollisionChecker::SetConfigurationInterpolationFunction( } const Eigen::VectorXd test_interpolated_q = interpolation_function( GetZeroConfiguration(), GetZeroConfiguration(), 0.0); - DRAKE_THROW_UNLESS( - test_interpolated_q.size() == GetZeroConfiguration().size()); + DRAKE_THROW_UNLESS(test_interpolated_q.size() == + GetZeroConfiguration().size()); for (int index = 0; index < test_interpolated_q.size(); ++index) { - DRAKE_THROW_UNLESS( - test_interpolated_q(index) == GetZeroConfiguration()(index)); + DRAKE_THROW_UNLESS(test_interpolated_q(index) == + GetZeroConfiguration()(index)); } configuration_interpolation_function_ = interpolation_function; } ConfigurationInterpolationFunction CollisionChecker::MakeStandaloneConfigurationInterpolationFunction() const { - return [this] ( - const Eigen::VectorXd& q_1, const Eigen::VectorXd& q_2, double ratio) { + return [this](const Eigen::VectorXd& q_1, const Eigen::VectorXd& q_2, + double ratio) { return this->InterpolateBetweenConfigurations(q_1, q_2, ratio); }; } -bool CollisionChecker::CheckEdgeCollisionFree( - const Eigen::VectorXd& q1, const Eigen::VectorXd& q2) const { +bool CollisionChecker::CheckEdgeCollisionFree(const Eigen::VectorXd& q1, + const Eigen::VectorXd& q2) const { return CheckContextEdgeCollisionFree(&mutable_model_context(), q1, q2); } @@ -575,8 +569,8 @@ bool CollisionChecker::CheckContextEdgeCollisionFree( } const double distance = ComputeConfigurationDistance(q1, q2); - const int num_steps = static_cast( - std::max(1.0, std::ceil(distance / edge_step_size()))); + const int num_steps = + static_cast(std::max(1.0, std::ceil(distance / edge_step_size()))); for (int step = 0; step < num_steps; ++step) { const double ratio = static_cast(step) / static_cast(num_steps); @@ -607,8 +601,8 @@ bool CollisionChecker::CheckEdgeCollisionFreeParallel( } const double distance = ComputeConfigurationDistance(q1, q2); - const int num_steps = static_cast( - std::max(1.0, std::ceil(distance / edge_step_size()))); + const int num_steps = + static_cast(std::max(1.0, std::ceil(distance / edge_step_size()))); std::atomic edge_valid(true); #if defined(_OPENMP) #pragma omp parallel for @@ -653,8 +647,7 @@ std::vector CollisionChecker::CheckEdgesCollisionFree( EdgeMeasure CollisionChecker::MeasureEdgeCollisionFree( const Eigen::VectorXd& q1, const Eigen::VectorXd& q2) const { - return MeasureContextEdgeCollisionFree( - &mutable_model_context(), q1, q2); + return MeasureContextEdgeCollisionFree(&mutable_model_context(), q1, q2); } EdgeMeasure CollisionChecker::MeasureContextEdgeCollisionFree( @@ -662,8 +655,8 @@ EdgeMeasure CollisionChecker::MeasureContextEdgeCollisionFree( const Eigen::VectorXd& q2) const { DRAKE_THROW_UNLESS(model_context != nullptr); const double distance = ComputeConfigurationDistance(q1, q2); - const int num_steps = static_cast( - std::max(1.0, std::ceil(distance / edge_step_size()))); + const int num_steps = + static_cast(std::max(1.0, std::ceil(distance / edge_step_size()))); double last_valid_ratio = -1.0; for (int step = 0; step <= num_steps; ++step) { const double ratio = @@ -683,8 +676,8 @@ EdgeMeasure CollisionChecker::MeasureEdgeCollisionFreeParallel( // Only perform parallel operations if omp parallel for will use >1 thread. if (CanEvaluateInParallel()) { const double distance = ComputeConfigurationDistance(q1, q2); - const int num_steps = static_cast( - std::max(1.0, std::ceil(distance / edge_step_size()))); + const int num_steps = + static_cast(std::max(1.0, std::ceil(distance / edge_step_size()))); // The "highest" interpolant value, alpha, (uninterrupted from q1) for // which there is no collision. std::atomic alpha; @@ -722,8 +715,8 @@ EdgeMeasure CollisionChecker::MeasureEdgeCollisionFreeParallel( std::vector CollisionChecker::MeasureEdgesCollisionFree( const std::vector>& edges, const bool parallelize) const { - std::vector collision_checks( - edges.size(), EdgeMeasure(0.0, -1.0)); + std::vector collision_checks(edges.size(), + EdgeMeasure(0.0, -1.0)); const bool check_in_parallel = CanEvaluateInParallel() && parallelize; CRU_OMP_PARALLEL_FOR_IF(check_in_parallel) @@ -739,7 +732,7 @@ std::vector CollisionChecker::MeasureEdgesCollisionFree( RobotClearance CollisionChecker::CalcRobotClearance( const Eigen::VectorXd& q, const double influence_distance) const { return CalcContextRobotClearance(&mutable_model_context(), q, - influence_distance); + influence_distance); } RobotClearance CollisionChecker::CalcContextRobotClearance( @@ -786,8 +779,8 @@ CollisionChecker::CollisionChecker(CollisionCheckerParams params, const std::set sorted_set( params.robot_model_instances.begin(), params.robot_model_instances.end()); - const std::vector sorted_vec( - sorted_set.begin(), sorted_set.end()); + const std::vector sorted_vec(sorted_set.begin(), + sorted_set.end()); const ModelInstanceIndex world = world_model_instance(); for (const auto& robot_model_instance : sorted_vec) { DRAKE_THROW_UNLESS(robot_model_instance != world); @@ -816,8 +809,7 @@ CollisionChecker::CollisionChecker(CollisionCheckerParams params, // Generate the filtered collision matrix. nominal_filtered_collisions_ = GenerateFilteredCollisionMatrix(); filtered_collisions_ = nominal_filtered_collisions_; - log()->debug( - "Collision filter matrix:\n{}", filtered_collisions_); + log()->debug("Collision filter matrix:\n{}", filtered_collisions_); } CollisionChecker::CollisionChecker(const CollisionChecker&) = default; @@ -850,8 +842,7 @@ void CollisionChecker::AllocateContexts() { } void CollisionChecker::OwnedContextKeeper::AllocateOwnedContexts( - const CollisionCheckerContext& prototype_context, - const int num_contexts) { + const CollisionCheckerContext& prototype_context, const int num_contexts) { DRAKE_THROW_UNLESS(num_contexts >= 1); DRAKE_THROW_UNLESS(empty()); for (int index = 0; index < num_contexts; ++index) { @@ -866,7 +857,7 @@ void CollisionChecker::OwnedContextKeeper::AllocateOwnedContexts( bool CollisionChecker::CanEvaluateInParallel() const { return SupportsParallelChecking() && - common_robotics_utilities::openmp_helpers::GetNumOmpThreads() > 1; + common_robotics_utilities::openmp_helpers::GetNumOmpThreads() > 1; } std::string CollisionChecker::CriticizePaddingMatrix() const { @@ -1055,8 +1046,8 @@ void CollisionChecker::UpdateMaxCollisionPadding() { } } -void CollisionChecker::ValidatePaddingMatrix( - const Eigen::MatrixXd& padding, const char* func) const { +void CollisionChecker::ValidatePaddingMatrix(const Eigen::MatrixXd& padding, + const char* func) const { const std::string criticism = CriticizePaddingMatrix(padding, func); if (!criticism.empty()) { throw std::logic_error(criticism); @@ -1122,12 +1113,11 @@ CollisionChecker::OwnedContextKeeper::OwnedContextKeeper( AllocateOwnedContexts(other.prototype_context(), other.num_contexts()); } -void -CollisionChecker::OwnedContextKeeper::PerformOperationAgainstAllOwnedContexts( - const RobotDiagram& model, - const std::function&, - CollisionCheckerContext*)>& operation) { +void CollisionChecker::OwnedContextKeeper:: + PerformOperationAgainstAllOwnedContexts( + const RobotDiagram& model, + const std::function&, + CollisionCheckerContext*)>& operation) { DRAKE_DEMAND(operation != nullptr); DRAKE_THROW_UNLESS(allocated()); for (auto& model_context : model_contexts_) { @@ -1146,16 +1136,15 @@ void CollisionChecker::StandaloneContextReferenceKeeper::AddStandaloneContext( std::weak_ptr(standalone_context)); } -void CollisionChecker::StandaloneContextReferenceKeeper - ::PerformOperationAgainstAllStandaloneContexts( - const RobotDiagram& model, - const std::function&, - CollisionCheckerContext*)>& operation) { +void CollisionChecker::StandaloneContextReferenceKeeper:: + PerformOperationAgainstAllStandaloneContexts( + const RobotDiagram& model, + const std::function&, + CollisionCheckerContext*)>& operation) { DRAKE_DEMAND(operation != nullptr); std::lock_guard lock(standalone_contexts_mutex_); for (auto standalone_context = standalone_contexts_.begin(); - standalone_context != standalone_contexts_.end();) { + standalone_context != standalone_contexts_.end();) { auto maybe_context = standalone_context->lock(); if (maybe_context != nullptr) { operation(model, maybe_context.get()); diff --git a/planning/collision_checker_params.h b/planning/collision_checker_params.h index 1222e773da89..2acde3df355f 100644 --- a/planning/collision_checker_params.h +++ b/planning/collision_checker_params.h @@ -35,9 +35,8 @@ To be valid, the function must satisfy the following conditions: - interpolate(q1, q2, 1) ≡ q2 - interpolate(q, q, r) ≡ q, for all r in [0, 1] */ -using ConfigurationInterpolationFunction = - std::function; +using ConfigurationInterpolationFunction = std::function; /** A set of common constructor parameters for a CollisionChecker. Not all subclasses of CollisionChecker will necessarily support this diff --git a/planning/robot_diagram.cc b/planning/robot_diagram.cc index dba52bc834e8..84dc34306b1a 100644 --- a/planning/robot_diagram.cc +++ b/planning/robot_diagram.cc @@ -26,12 +26,10 @@ constexpr size_t kSceneGraphIndex = 1; // @tparam_default_scalar // @tparam ChildSystem the subsystem class to extract, e.g., MultibodyPlant. // @tparam DiagramOrDiagramBuilder duck type for either a diagram or a builder. -template < - typename T, - template class ChildSystem, - template class DiagramOrDiagramBuilder> -ChildSystem& DowncastSubsystem( - DiagramOrDiagramBuilder* diagram, size_t index) { +template class ChildSystem, + template class DiagramOrDiagramBuilder> +ChildSystem& DowncastSubsystem(DiagramOrDiagramBuilder* diagram, + size_t index) { DRAKE_DEMAND(diagram != nullptr); const std::vector*>& items = diagram->GetSystems(); const auto* child = dynamic_cast*>(items.at(index)); @@ -43,10 +41,10 @@ template RobotDiagram::RobotDiagram( std::unique_ptr> diagram_builder) : Diagram(SystemTypeTag{}), - plant_(DowncastSubsystem( - diagram_builder.get(), kPlantIndex)), - scene_graph_(DowncastSubsystem( - diagram_builder.get(), kSceneGraphIndex)) { + plant_(DowncastSubsystem(diagram_builder.get(), + kPlantIndex)), + scene_graph_(DowncastSubsystem(diagram_builder.get(), + kSceneGraphIndex)) { diagram_builder->BuildInto(this); // TODO(jeremy.nimmer) For convenience, we should probably re-export most (or // all) of the the subsytems' input and output ports here. diff --git a/planning/robot_diagram.h b/planning/robot_diagram.h index 26c0a05739ef..a47d9068da51 100644 --- a/planning/robot_diagram.h +++ b/planning/robot_diagram.h @@ -40,19 +40,13 @@ class RobotDiagram final : public systems::Diagram { using systems::Diagram::CreateDefaultContext; /** Gets the contained plant (readonly). */ - const multibody::MultibodyPlant& plant() const { - return plant_; - } + const multibody::MultibodyPlant& plant() const { return plant_; } /** Gets the contained scene graph (mutable). */ - geometry::SceneGraph& mutable_scene_graph() { - return scene_graph_; - } + geometry::SceneGraph& mutable_scene_graph() { return scene_graph_; } /** Gets the contained scene graph (readonly). */ - const geometry::SceneGraph& scene_graph() const { - return scene_graph_; - } + const geometry::SceneGraph& scene_graph() const { return scene_graph_; } /** Gets the contained plant's context (mutable) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to @@ -92,7 +86,8 @@ class RobotDiagram final : public systems::Diagram { private: // To access our private constructor. - template friend class RobotDiagramBuilder; + template + friend class RobotDiagramBuilder; // For use by RobotDiagramBuilder. explicit RobotDiagram(std::unique_ptr>); diff --git a/planning/robot_diagram_builder.h b/planning/robot_diagram_builder.h index 110cecf870a4..4111a42b7ca1 100644 --- a/planning/robot_diagram_builder.h +++ b/planning/robot_diagram_builder.h @@ -50,8 +50,8 @@ class RobotDiagramBuilder { /** Gets the contained Parser (mutable). @throws exception when IsDiagramBuilt() already. */ - template >* = nullptr> + template >* = nullptr> multibody::Parser& parser() { ThrowIfAlreadyBuilt(); return parser_; @@ -59,8 +59,8 @@ class RobotDiagramBuilder { /** Gets the contained Parser (readonly). @throws exception when IsDiagramBuilt() already. */ - template >* = nullptr> + template >* = nullptr> const multibody::Parser& parser() const { ThrowIfAlreadyBuilt(); return parser_; @@ -104,42 +104,34 @@ class RobotDiagramBuilder { std::unique_ptr> Build(); DRAKE_DEPRECATED("2023-06-01", "Use Build() instead of BuildDiagram().") - std::unique_ptr> BuildDiagram() { - return Build(); - } + std::unique_ptr> BuildDiagram() { return Build(); } DRAKE_DEPRECATED("2023-06-01", "Use builder() instead of mutable_builder().") - systems::DiagramBuilder& mutable_builder() { - return builder(); - } + systems::DiagramBuilder& mutable_builder() { return builder(); } - template >* = nullptr> + template >* = nullptr> DRAKE_DEPRECATED("2023-06-01", "Use parser() instead of mutable_parser().") multibody::Parser& mutable_parser() { return parser(); } DRAKE_DEPRECATED("2023-06-01", "Use plant() instead of mutable_plant().") - multibody::MultibodyPlant& mutable_plant() { - return plant(); - } + multibody::MultibodyPlant& mutable_plant() { return plant(); } DRAKE_DEPRECATED("2023-06-01", - "Use scene_graph() instead of mutable_scene_graph().") - geometry::SceneGraph& mutable_scene_graph() { - return scene_graph(); - } + "Use scene_graph() instead of mutable_scene_graph().") + geometry::SceneGraph& mutable_scene_graph() { return scene_graph(); } DRAKE_DEPRECATED("2023-06-01", - "Use plant().is_finalized() instead of IsPlantFinalized().") + "Use plant().is_finalized() instead of IsPlantFinalized().") bool IsPlantFinalized() const { ThrowIfAlreadyBuilt(); return plant_.is_finalized(); } DRAKE_DEPRECATED("2023-06-01", - "Use plant().Finalize() instead of FinalizePlant().") + "Use plant().Finalize() instead of FinalizePlant().") void FinalizePlant() { ThrowIfAlreadyBuilt(); plant_.Finalize(); @@ -156,8 +148,8 @@ class RobotDiagramBuilder { geometry::SceneGraph& scene_graph_; // The Parser object only exists when T == double. - using MaybeParser = std::conditional_t< - std::is_same_v, multibody::Parser, void*>; + using MaybeParser = + std::conditional_t, multibody::Parser, void*>; MaybeParser parser_; }; diff --git a/planning/scene_graph_collision_checker.cc b/planning/scene_graph_collision_checker.cc index 42596e6d2ec1..3308a8ce5570 100644 --- a/planning/scene_graph_collision_checker.cc +++ b/planning/scene_graph_collision_checker.cc @@ -61,10 +61,9 @@ std::optional SceneGraphCollisionChecker::DoAddCollisionShapeToBody( const GeometrySet bodyA_geometries = plant().CollectRegisteredGeometries(plant().GetBodiesWeldedTo(bodyA)); - log()->debug( - "Adding shape (group: [{}]) to {} (FrameID {}) at X_AG =\n{}", - group_name, GetScopedName(bodyA), body_frame_id, - X_AG.GetAsMatrix4()); + log()->debug("Adding shape (group: [{}]) to {} (FrameID {}) at X_AG =\n{}", + group_name, GetScopedName(bodyA), body_frame_id, + X_AG.GetAsMatrix4()); // The geometry instance template which will be copied into each per-thread // SceneGraph Context; the GeometryId will match across each thread this way. @@ -142,13 +141,11 @@ bool SceneGraphCollisionChecker::DoCheckContextConfigCollisionFree( const double padding = GetPaddingBetween(*body_A, *body_B); if (distance_pair.distance <= padding) { if (body_A_part_of_robot && body_B_part_of_robot) { - log()->trace( - "Self collision between bodies [{}] and [{}]", - GetScopedName(*body_A), GetScopedName(*body_B)); + log()->trace("Self collision between bodies [{}] and [{}]", + GetScopedName(*body_A), GetScopedName(*body_B)); } else { - log()->trace( - "Environment collision between bodies [{}] and [{}]", - GetScopedName(*body_A), GetScopedName(*body_B)); + log()->trace("Environment collision between bodies [{}] and [{}]", + GetScopedName(*body_A), GetScopedName(*body_B)); } return false; } @@ -263,16 +260,18 @@ RobotClearance SceneGraphCollisionChecker::DoCalcContextRobotClearance( // our ∂p_BA_W/∂qᵣ may include additional non-zero columns; we rely on the // parent class to zero those columns out and make the result truly ∂ϕ/∂qᵣ. if (body_A_part_of_robot) { - plant().CalcJacobianPositionVector( + plant().CalcJacobianPositionVector( // BR plant_context, frame_A, p_ACa, frame_W, frame_W, - &dp_BA_dq /* ∂p_WA/∂q after this invocation */); + // ∂p_WA/∂q after this invocation + &dp_BA_dq); } else { dp_BA_dq.setZero(); } if (body_B_part_of_robot) { - plant().CalcJacobianPositionVector( + plant().CalcJacobianPositionVector( // BR plant_context, frame_B, p_BCb, frame_W, frame_W, - &partial_temp /* ∂p_WB/∂q */); + // ∂p_WB/∂q + &partial_temp); dp_BA_dq -= partial_temp; // ∂p_WA/∂qᵣ - ∂p_WB/∂qᵣ. } ddist_dq.noalias() = ddist_dp_BA.transpose() * dp_BA_dq; diff --git a/planning/test/collision_avoidance_test.cc b/planning/test/collision_avoidance_test.cc index 9b3034be8217..60a71f676e2e 100644 --- a/planning/test/collision_avoidance_test.cc +++ b/planning/test/collision_avoidance_test.cc @@ -21,7 +21,9 @@ using Eigen::VectorXd; using std::move; using std::unique_ptr; -double ZeroDistanceFunc(const VectorXd&, const VectorXd&) { return 0.0; } +double ZeroDistanceFunc(const VectorXd&, const VectorXd&) { + return 0.0; +} /* We simply need a diagram with a non-zero number of dofs; a single floating body will give us 7 -- the semantics are unimportant. */ @@ -49,7 +51,8 @@ class DummyCollisionChecker final : public UnimplementedCollisionChecker { {.model = MakeRobot(), .robot_model_instances = {default_model_instance()}, .configuration_distance_function = ZeroDistanceFunc, - .edge_step_size = 0.1}, false /* supports_parallel */), + .edge_step_size = 0.1}, + false /* supports_parallel */), data_(plant().num_positions()) { AllocateContexts(); } @@ -57,9 +60,7 @@ class DummyCollisionChecker final : public UnimplementedCollisionChecker { /* This clearance data will define the distance/jacobian data that will be given back to ComputeCollisionAvoidanceDisplacement() when invoking CalcRobotClearance(). */ - void set_robot_clearance(RobotClearance data) { - data_ = move(data); - } + void set_robot_clearance(RobotClearance data) { data_ = move(data); } private: void DoUpdateContextPositions( @@ -117,9 +118,9 @@ GTEST_TEST(ComputeCollisionAvoidanceDisplacementTest, NoDistanceMeasurements) { /* The checker returns an empty RobotClearance by default. This should lead to zero-valued Δq. We're passing in an arbitrary, non-zero, q to eliminate the appearance that the return value copies the input value. */ - EXPECT_TRUE(CompareMatrices( - ComputeCollisionAvoidanceDisplacement(checker, q, 0, 1), - VectorXd::Zero(kQSize))); + EXPECT_TRUE( + CompareMatrices(ComputeCollisionAvoidanceDisplacement(checker, q, 0, 1), + VectorXd::Zero(kQSize))); } GTEST_TEST(ComputeCollisionAvoidanceDisplacementTest, WeightedCombinations) { @@ -205,19 +206,16 @@ GTEST_TEST(ComputeCollisionAvoidanceDisplacementTest, Errors) { const VectorXd q = VectorXd::Zero(kQSize); // The max_penetration cannot be positive. - EXPECT_THROW( - ComputeCollisionAvoidanceDisplacement(checker, q, 0.1, 1), - std::exception); + EXPECT_THROW(ComputeCollisionAvoidanceDisplacement(checker, q, 0.1, 1), + std::exception); // The max_clearance cannot be negative. - EXPECT_THROW( - ComputeCollisionAvoidanceDisplacement(checker, q, -1, -0.1), - std::exception); + EXPECT_THROW(ComputeCollisionAvoidanceDisplacement(checker, q, -1, -0.1), + std::exception); // They cannot be both zero. - EXPECT_THROW( - ComputeCollisionAvoidanceDisplacement(checker, q, 0, 0), - std::exception); + EXPECT_THROW(ComputeCollisionAvoidanceDisplacement(checker, q, 0, 0), + std::exception); } } // namespace diff --git a/planning/test/collision_checker_test.cc b/planning/test/collision_checker_test.cc index 805c02f6223b..90f7d4294d70 100644 --- a/planning/test/collision_checker_test.cc +++ b/planning/test/collision_checker_test.cc @@ -48,6 +48,9 @@ constexpr bool kHasOpenmp = true; constexpr bool kHasOpenmp = false; #endif +using Eigen::AngleAxisd; +using Eigen::Vector3d; +using Eigen::VectorXd; using geometry::CollisionFilterDeclaration; using geometry::FrameId; using geometry::GeometryId; @@ -68,14 +71,11 @@ using multibody::RevoluteJoint; using multibody::RigidBody; using multibody::SpatialInertia; using multibody::world_model_instance; -using systems::Context; -using Eigen::AngleAxisd; -using Eigen::Vector3d; -using Eigen::VectorXd; using std::move; using std::optional; using std::pair; using std::vector; +using systems::Context; using testing::ElementsAre; // Adds a new model instance consisting of a non-zero number of floating bodies. @@ -191,9 +191,7 @@ class CollisionCheckerTester : public UnimplementedCollisionChecker { //@{ // Testing knobs. - void SetCollisionFree(bool value) { - collision_free_ = value; - } + void SetCollisionFree(bool value) { collision_free_ = value; } // This ultimately controls whether *any* of the AddCollisionShape APIs will // report "shape added". All of the APIs ultimately delegate to @@ -217,9 +215,7 @@ class CollisionCheckerTester : public UnimplementedCollisionChecker { // Returns the positions available in this checker from the most recent // Check*ConfigCollisionFree() call. - const VectorXd& positions_for_check() const { - return positions_for_check_; - } + const VectorXd& positions_for_check() const { return positions_for_check_; } //@} private: @@ -240,7 +236,9 @@ std::unique_ptr MakeUnallocatedChecker( const vector& robot_indices, bool supports_parallel = true) { const ConfigurationDistanceFunction dist{ - [](const VectorXd& a, const VectorXd& b) { return (b - a).norm(); }}; + [](const VectorXd& a, const VectorXd& b) { + return (b - a).norm(); + }}; return std::make_unique( CollisionCheckerParams{move(robot), robot_indices, dist, 0.1, 0, 0}, supports_parallel); @@ -263,9 +261,8 @@ pair>, ModelInstanceIndex> MakeModel( // fixture rely on knowing body indices. auto robot_index = AddChain(&builder_plant, 3, config.per_body_geometries); if (config.weld_robot) { - builder_plant.WeldFrames( - builder_plant.get_body(BodyIndex(0)).body_frame(), - builder_plant.get_body(BodyIndex(1)).body_frame()); + builder_plant.WeldFrames(builder_plant.get_body(BodyIndex(0)).body_frame(), + builder_plant.get_body(BodyIndex(1)).body_frame()); } else if (config.on_env_base) { const ModelInstanceIndex instance = builder_plant.AddModelInstance("floating"); @@ -314,8 +311,7 @@ class CollisionCheckerThrowTest : public testing::Test { DRAKE_EXPECT_THROWS_MESSAGE( CollisionCheckerTester( {move(diagram_), robot_model_instances_, distance_fn_, - edge_step_size_, env_collision_padding_, - self_collision_padding_}), + edge_step_size_, env_collision_padding_, self_collision_padding_}), throw_message_pattern); } @@ -324,9 +320,11 @@ class CollisionCheckerThrowTest : public testing::Test { RobotDiagramBuilder builder_; std::unique_ptr> diagram_{builder_.Build()}; std::vector robot_model_instances_{ - default_model_instance()}; + default_model_instance()}; ConfigurationDistanceFunction distance_fn_{ - [](const VectorXd&, const VectorXd&) { return 0.0; }}; + [](const VectorXd&, const VectorXd&) { + return 0.0; + }}; double edge_step_size_{0.1}; double env_collision_padding_{0.0}; double self_collision_padding_{0.0}; @@ -359,12 +357,12 @@ GTEST_TEST(CollisionCheckerTest, SortedRobots) { auto& plant = builder.plant(); const ModelInstanceIndex robot1 = AddChain(&plant, 1); const ModelInstanceIndex robot2 = AddChain(&plant, 2); - auto distance_function = [](auto...) { return 0; }; - auto dut = - std::make_unique(CollisionCheckerParams{ - builder.Build(), - std::vector{robot2, robot2, robot1}, - distance_function, 0.1, 0, 0}); + auto distance_function = [](auto...) { + return 0; + }; + auto dut = std::make_unique(CollisionCheckerParams{ + builder.Build(), std::vector{robot2, robot2, robot1}, + distance_function, 0.1, 0, 0}); EXPECT_THAT(dut->robot_model_instances(), ElementsAre(robot1, robot2)); } @@ -388,7 +386,9 @@ GTEST_TEST(CollisionCheckerTest, CollisionCheckerEmpty) { RobotDiagramBuilder builder; auto diagram = builder.Build(); const ConfigurationDistanceFunction fn0 = [](const VectorXd&, - const VectorXd&) { return 0.0; }; + const VectorXd&) { + return 0.0; + }; const ModelInstanceIndex robot = default_model_instance(); const ModelInstanceIndex world = world_model_instance(); CollisionCheckerTester dut({move(diagram), {robot}, fn0, 0.1, 0, 0}); @@ -404,8 +404,8 @@ GTEST_TEST(CollisionCheckerTest, CollisionCheckerEmpty) { // Here is an operation that will help check memory safety of callbacks. auto op = [](const RobotDiagram&, CollisionCheckerContext* context) { - ASSERT_NE(context, nullptr); - }; + ASSERT_NE(context, nullptr); + }; // Examples of methods that throw with no contexts allocated. Derived classes // are required to allocate contexts during construction. This is proof that @@ -551,37 +551,35 @@ class TrivialCollisionCheckerTest : public testing::Test { }; TEST_F(TrivialCollisionCheckerTest, ClonesAndContexts) { - const auto count_contexts = - [](CollisionChecker* checker) { - int count{0}; - auto op = [&count](const RobotDiagram&, - CollisionCheckerContext*) { ++count; }; - checker->PerformOperationAgainstAllModelContexts(op); - return count; - }; + const auto count_contexts = [](CollisionChecker* checker) { + int count{0}; + auto op = [&count](const RobotDiagram&, CollisionCheckerContext*) { + ++count; + }; + checker->PerformOperationAgainstAllModelContexts(op); + return count; + }; EXPECT_EQ(count_contexts(dut_.get()), - dut_->num_allocated_contexts() - + 1); // prototype context + dut_->num_allocated_contexts() + 1 /* prototype context */); // Add one standalone context. auto context = dut_->MakeStandaloneModelContext(); - EXPECT_EQ(count_contexts(dut_.get()), - dut_->num_allocated_contexts() - + 1 // prototype context - + 1); // our standalone context + EXPECT_EQ(count_contexts(dut_.get()), dut_->num_allocated_contexts() + + 1 /* prototype context */ + + 1 /* our standalone context */); auto cloned = dut_->Clone(); EXPECT_NE(cloned, nullptr); + + // The clone's allocated context count matches. EXPECT_EQ(count_contexts(cloned.get()), - dut_->num_allocated_contexts() // Allocated context count matches. - + 1 // prototype context - + 0); // standalone context has not been carried over. + dut_->num_allocated_contexts() + 1 /* prototype context */ + + 0 /* standalone context has not been carried over. */); context.reset(); // Give up our standalone context. EXPECT_EQ(count_contexts(dut_.get()), - dut_->num_allocated_contexts() - + 1); // prototype context + dut_->num_allocated_contexts() + 1 /* prototype context */); } TEST_F(TrivialCollisionCheckerTest, Padding) { @@ -1185,8 +1183,8 @@ TEST_F(TrivialCollisionCheckerTest, SetCollisionFilteredBetween) { } // Token test against bodies, instead of indices. - EXPECT_NO_THROW(dut_->SetCollisionFilteredBetween(dut_->get_body(b[2]), - dut_->get_body(b[3]), true)); + EXPECT_NO_THROW(dut_->SetCollisionFilteredBetween( + dut_->get_body(b[2]), dut_->get_body(b[3]), true)); EXPECT_TRUE(dut_->IsCollisionFilteredBetween(b[2], b[3])); EXPECT_NO_THROW( dut_->SetCollisionFilterMatrix(dut_->GetFilteredCollisionMatrix())); @@ -1207,8 +1205,7 @@ TEST_F(TrivialCollisionCheckerTest, SetCollisionFilteredWithAllBodies) { { // Setting an environment body is simply bad. - EXPECT_THROW(dut_->SetCollisionFilteredWithAllBodies(b[4]), - std::exception); + EXPECT_THROW(dut_->SetCollisionFilteredWithAllBodies(b[4]), std::exception); } { @@ -1216,7 +1213,7 @@ TEST_F(TrivialCollisionCheckerTest, SetCollisionFilteredWithAllBodies) { // reports filters as expected. EXPECT_NO_THROW(dut_->SetCollisionFilteredWithAllBodies(b[3])); EXPECT_NO_THROW( - dut_->SetCollisionFilterMatrix(dut_->GetFilteredCollisionMatrix())); + dut_->SetCollisionFilterMatrix(dut_->GetFilteredCollisionMatrix())); for (BodyIndex i(0); i < num_bodies; ++i) { EXPECT_TRUE(dut_->IsCollisionFilteredBetween(i, b[3])); } @@ -1344,8 +1341,10 @@ GTEST_TEST(EdgeCheckTest, Configuration) { EXPECT_EQ(dut.MakeStandaloneConfigurationDistanceFunction()(q1, q2), -1.5); // Change the function via (3). - const ConfigurationDistanceFunction dist1 = - [](const VectorXd& a, const VectorXd& b) { return (b - a).norm(); }; + const ConfigurationDistanceFunction dist1 = [](const VectorXd& a, + const VectorXd& b) { + return (b - a).norm(); + }; dut.SetConfigurationDistanceFunction(dist1); // Evaluate (1) and (2) again. @@ -1397,8 +1396,10 @@ GTEST_TEST(EdgeCheckTest, Configuration) { // The default interpolation should handle quaternion and other joints in an // expected way (SLERP for the former, LERP for the latter). GTEST_TEST(EdgeCheckTest, DefaultInterpolation) { - const ConfigurationDistanceFunction dist = - [](const VectorXd& a, const VectorXd& b) { return (b - a).norm(); }; + const ConfigurationDistanceFunction dist = [](const VectorXd& a, + const VectorXd& b) { + return (b - a).norm(); + }; // We want a floating body to guarantee we have quaternions. auto dut = MakeEdgeChecker( dist, 0.1, nullptr /* default interpolator */, false /* welded */); @@ -1644,9 +1645,8 @@ std::vector MakeEdgeTestCases() { } // Edges are 100% valid. - configs.push_back({.alpha = 1.0, - .last_colliding_alpha = 2.0, - .parallel = in_parallel}); + configs.push_back( + {.alpha = 1.0, .last_colliding_alpha = 2.0, .parallel = in_parallel}); // Edges are invalid to varying degrees (this includes edges where q1 is not // valid -- alpha < 0). @@ -1722,9 +1722,9 @@ TEST_P(ParameterizedEdgeCheckTest, MeasureEdgeCollisionFree) { const VectorXd q2 = dut.EncodeConfiguration(q_size, config.alpha, config.last_colliding_alpha); - const EdgeMeasure result = - config.parallel ? dut.MeasureEdgeCollisionFreeParallel(q1, q2) - : dut.MeasureEdgeCollisionFree(q1, q2); + const EdgeMeasure result = config.parallel + ? dut.MeasureEdgeCollisionFreeParallel(q1, q2) + : dut.MeasureEdgeCollisionFree(q1, q2); const double expected_alpha = config.alpha; if (expected_alpha == 1.0) { @@ -1776,9 +1776,9 @@ TEST_P(ParameterizedEdgeCheckTest, CheckEdgeCollisionFree) { const VectorXd q2 = dut.EncodeConfiguration(q_size, config.alpha, config.last_colliding_alpha); - const bool result = - config.parallel ? dut.CheckEdgeCollisionFreeParallel(q1, q2) - : dut.CheckEdgeCollisionFree(q1, q2); + const bool result = config.parallel + ? dut.CheckEdgeCollisionFreeParallel(q1, q2) + : dut.CheckEdgeCollisionFree(q1, q2); // The encoded alpha (as documented above on EncodeConfiguration()). const bool expected_result = config.alpha == 1.0; diff --git a/planning/test/planning_test_helpers.cc b/planning/test/planning_test_helpers.cc index 433199727fe3..cbefa5cc42c1 100644 --- a/planning/test/planning_test_helpers.cc +++ b/planning/test/planning_test_helpers.cc @@ -34,13 +34,12 @@ ConfigurationDistanceFunction MakeWeightedIiwaConfigurationDistanceFunction() { Eigen::VectorXd weights = Eigen::VectorXd::Zero(7); weights << 2.0, 2.0, 2.0, 2.0, 1.0, 1.0, 1.0; - const ConfigurationDistanceFunction weighted_cspace_distance_fn = [weights] ( - const Eigen::VectorXd& q1, const Eigen::VectorXd& q2) { - const Eigen::VectorXd delta = q2 - q1; - const Eigen::VectorXd weighted_delta = - delta.cwiseProduct(weights); - return weighted_delta.norm(); - }; + const ConfigurationDistanceFunction weighted_cspace_distance_fn = + [weights](const Eigen::VectorXd& q1, const Eigen::VectorXd& q2) { + const Eigen::VectorXd delta = q2 - q1; + const Eigen::VectorXd weighted_delta = delta.cwiseProduct(weights); + return weighted_delta.norm(); + }; return weighted_cspace_distance_fn; } diff --git a/planning/test/planning_test_helpers.h b/planning/test/planning_test_helpers.h index b00a9fae4741..2eb8a97bac0c 100644 --- a/planning/test/planning_test_helpers.h +++ b/planning/test/planning_test_helpers.h @@ -23,9 +23,8 @@ ConfigurationDistanceFunction MakeWeightedIiwaConfigurationDistanceFunction(); revolute joints. The geometry for each body consists of @p num_geo small spheres. @note: the model name is based on @p n, so adding chains of the same length will fail. */ -multibody::ModelInstanceIndex -AddChain(multibody::MultibodyPlant* plant, int n, - int num_geo = 1); +multibody::ModelInstanceIndex AddChain(multibody::MultibodyPlant* plant, + int n, int num_geo = 1); } // namespace test } // namespace planning diff --git a/planning/test/robot_diagram_test.cc b/planning/test/robot_diagram_test.cc index 32729cab9236..8008735fb661 100644 --- a/planning/test/robot_diagram_test.cc +++ b/planning/test/robot_diagram_test.cc @@ -150,8 +150,7 @@ GTEST_TEST(RobotDiagramTest, ContextGetters) { std::unique_ptr> dut = MakeSampleDut()->Build(); std::unique_ptr> root_context = dut->CreateDefaultContext(); - const Context& plant_context = - dut->plant_context(*root_context); + const Context& plant_context = dut->plant_context(*root_context); const Context& scene_graph_context = dut->scene_graph_context(*root_context); Context& mutable_plant_context = @@ -276,8 +275,7 @@ GTEST_TEST(RobotDiagramTest, DeprecatedContextGetters) { std::unique_ptr> dut = MakeSampleDut()->BuildDiagram(); std::unique_ptr> root_context = dut->CreateDefaultContext(); - const Context& plant_context = - dut->plant_context(*root_context); + const Context& plant_context = dut->plant_context(*root_context); const Context& scene_graph_context = dut->scene_graph_context(*root_context); Context& mutable_plant_context = @@ -310,4 +308,3 @@ GTEST_TEST(RobotDiagramTest, DeprecatedClone) { } // namespace } // namespace planning } // namespace drake - diff --git a/planning/test/scene_graph_collision_checker_test.cc b/planning/test/scene_graph_collision_checker_test.cc index ca6fc695d618..abd18a887d42 100644 --- a/planning/test/scene_graph_collision_checker_test.cc +++ b/planning/test/scene_graph_collision_checker_test.cc @@ -13,6 +13,7 @@ namespace planning { namespace test { namespace { +using Eigen::Matrix3d; using Eigen::MatrixXd; using Eigen::Vector3d; using Eigen::VectorXd; @@ -24,14 +25,14 @@ CollisionCheckerTestParams MakeSceneGraphCollisionCheckerParams() { const CollisionCheckerConstructionParams p; auto model = MakePlanningTestModel(MakeCollisionCheckerTestScene()); const auto robot_instance = model->plant().GetModelInstanceByName("iiwa"); - result.checker.reset(new SceneGraphCollisionChecker({ - .model = std::move(model), - .robot_model_instances = {robot_instance}, - .configuration_distance_function = - MakeWeightedIiwaConfigurationDistanceFunction(), - .edge_step_size = p.edge_step_size, - .env_collision_padding = p.env_padding, - .self_collision_padding = p.self_padding})); + result.checker.reset(new SceneGraphCollisionChecker( + {.model = std::move(model), + .robot_model_instances = {robot_instance}, + .configuration_distance_function = + MakeWeightedIiwaConfigurationDistanceFunction(), + .edge_step_size = p.edge_step_size, + .env_collision_padding = p.env_padding, + .self_collision_padding = p.self_padding})); return result; } @@ -62,7 +63,7 @@ INSTANTIATE_TEST_SUITE_P( // Dallas's sphere has a radius of 1.0. GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceThreeSpheres) { RobotDiagramBuilder builder; - builder.parser().AddModelsFromString(R"""( + const std::string model_data = R"""( @@ -102,15 +103,16 @@ GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceThreeSpheres) { -)""", "sdf"); +)"""; + builder.parser().AddModelsFromString(model_data, "sdf"); + const auto& plant = builder.plant(); CollisionCheckerParams params; params.model = builder.Build(); - params.robot_model_instances.push_back( - plant.GetModelInstanceByName("robot")); - params.configuration_distance_function = - [](const VectorXd& q1, const VectorXd& q2) { + params.robot_model_instances.push_back(plant.GetModelInstanceByName("robot")); + params.configuration_distance_function = [](const VectorXd& q1, + const VectorXd& q2) { return (q1 - q2).norm(); }; params.edge_step_size = 0.05; @@ -137,14 +139,8 @@ GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceThreeSpheres) { const double tol = 1e-9; EXPECT_EQ(clearance.size(), 3); EXPECT_EQ(clearance.num_positions(), 3); - EXPECT_THAT(clearance.robot_indices(), ElementsAre( - boston, - boston, - seattle)); - EXPECT_THAT(clearance.other_indices(), ElementsAre( - seattle, - dallas, - dallas)); + EXPECT_THAT(clearance.robot_indices(), ElementsAre(boston, boston, seattle)); + EXPECT_THAT(clearance.other_indices(), ElementsAre(seattle, dallas, dallas)); EXPECT_THAT(clearance.collision_types(), ElementsAre(RobotCollisionType::kSelfCollision, RobotCollisionType::kEnvironmentCollision, @@ -154,35 +150,33 @@ GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceThreeSpheres) { // The Seattle <-> Dallas distance on-center is 5.0. // In each case, the measured distance is less than on-center by each of the // two cities' sphere radii. - EXPECT_TRUE(CompareMatrices(clearance.distances(), (VectorXd(3) << - 8.0 - 0.5 - 0.25, - 5.0 - 0.5 - 1.0, - 5.0 - 0.25 - 1.0).finished(), tol)); + Vector3d expected_distances; + expected_distances(0) = 8.0 - 0.5 - 0.25; + expected_distances(1) = 5.0 - 0.5 - 1.0; + expected_distances(2) = 5.0 - 0.25 - 1.0; + EXPECT_TRUE(CompareMatrices(clearance.distances(), expected_distances, tol)); // Recall that: // - The prismatic joint for Boston moves (+0.6, +0.8) for each +1.0 q₀. // - The prismatic joint for Seattle moves (-0.6, +0.8) for each +1.0 q₁. // - Dallas is not part of the robot, so its jacobian will always be zero. - EXPECT_TRUE(CompareMatrices(clearance.jacobians(), (MatrixXd(3, 3) << - // Increasing either q₀ or q₁ will increase the Boston-Seattle distance. - 0.8, 0.8, 0, - // Increasing q₀ will increase the Boston-Dallas distance. - 1.0, 0, 0, - // Increasing q₁ will increase the Seattle-Dallas distance. - 0, 1.0, 0).finished(), tol)); + Matrix3d expected_jacobians(3, 3); + expected_jacobians.setZero(); + // Increasing either q₀ or q₁ will increase the Boston-Seattle distance. + expected_jacobians(0, 0) = 0.8; + expected_jacobians(0, 1) = 0.8; + // Increasing q₀ will increase the Boston-Dallas distance. + expected_jacobians(1, 0) = 1.0; + // Increasing q₁ will increase the Seattle-Dallas distance. + expected_jacobians(2, 1) = 1.0; + EXPECT_TRUE(CompareMatrices(clearance.jacobians(), expected_jacobians, tol)); // Now reposture Seattle to be directly south of Boston (tectonic plates!). // The distances will change in the obvious ways; what we care about most is // that the jacobian and especially its sign update correctly. q(1) = -5.0; clearance = dut.CalcRobotClearance(q, influence); - EXPECT_THAT(clearance.robot_indices(), ElementsAre( - boston, - boston, - seattle)); - EXPECT_THAT(clearance.other_indices(), ElementsAre( - seattle, - dallas, - dallas)); + EXPECT_THAT(clearance.robot_indices(), ElementsAre(boston, boston, seattle)); + EXPECT_THAT(clearance.other_indices(), ElementsAre(seattle, dallas, dallas)); EXPECT_THAT(clearance.collision_types(), ElementsAre(RobotCollisionType::kSelfCollision, RobotCollisionType::kEnvironmentCollision, @@ -192,22 +186,24 @@ GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceThreeSpheres) { // The Seattle <-> Dallas distance on-center is 5.0. // In each case, the measured distance is less than on-center by each of the // two cities' sphere radii. - EXPECT_TRUE(CompareMatrices(clearance.distances(), (VectorXd(3) << - 6.0 - 0.5 - 0.25, - 5.0 - 0.5 - 1.0, - 5.0 - 0.25 - 1.0).finished(), tol)); + expected_distances(0) = 6.0 - 0.5 - 0.25; + expected_distances(1) = 5.0 - 0.5 - 1.0; + expected_distances(2) = 5.0 - 0.25 - 1.0; + EXPECT_TRUE(CompareMatrices(clearance.distances(), expected_distances, tol)); // Recall that: // - The prismatic joint for Boston moves (+0.6, +0.8) for each +1.0 q₀. // - The prismatic joint for Seattle moves (-0.6, +0.8) for each +1.0 q₁. // - Dallas is not part of the robot, so its jacobian will always be zero. - EXPECT_TRUE(CompareMatrices(clearance.jacobians(), (MatrixXd(3, 3) << - // Increasing q₀ will increase the Boston-Seattle distance. - // Increasing q₁ will decrease the Boston-Seattle distance. - 0.6, -0.6, 0, - // Increasing q₀ will increase the Boston-Dallas distance. - 1.0, 0, 0, - // Increasing q₁ will decrease the Seattle-Dallas distance. - 0, -1.0, 0).finished(), tol)); + expected_jacobians.setZero(); + // Increasing q₀ will increase the Boston-Seattle distance. + expected_jacobians(0, 0) = 0.6; + // Increasing q₁ will decrease the Boston-Seattle distance. + expected_jacobians(0, 1) = -0.6; + // Increasing q₀ will increase the Boston-Dallas distance. + expected_jacobians(1, 0) = 1.0; + // Increasing q₁ will decrease the Seattle-Dallas distance. + expected_jacobians(2, 1) = -1.0; + EXPECT_TRUE(CompareMatrices(clearance.jacobians(), expected_jacobians, tol)); } // Checks RobotClearance when the robot is an arm (only) but inboard of the @@ -216,7 +212,7 @@ GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceThreeSpheres) { GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceFloatingBase) { // Build a dut with a ground plane + floating chassis with welded arm. RobotDiagramBuilder builder; - builder.parser().AddModelsFromString(R"""( + const std::string model_directives = R"""( directives: - add_model: name: ground @@ -233,14 +229,15 @@ GTEST_TEST(SceneGraphCollisionCheckerTest, ClearanceFloatingBase) { - add_weld: parent: chassis::base_link_meat child: arm::base -)""", "dmd.yaml"); +)"""; + builder.parser().AddModelsFromString(model_directives, "dmd.yaml"); + const auto& plant = builder.plant(); CollisionCheckerParams params; params.model = builder.Build(); - params.robot_model_instances.push_back( - plant.GetModelInstanceByName("arm")); - params.configuration_distance_function = - [](const VectorXd& q1, const VectorXd& q2) { + params.robot_model_instances.push_back(plant.GetModelInstanceByName("arm")); + params.configuration_distance_function = [](const VectorXd& q1, + const VectorXd& q2) { return (q1 - q2).norm(); }; params.edge_step_size = 0.05; diff --git a/planning/test/unimplemented_collision_checker_test.cc b/planning/test/unimplemented_collision_checker_test.cc index 2253516794c5..b39fa7f91f14 100644 --- a/planning/test/unimplemented_collision_checker_test.cc +++ b/planning/test/unimplemented_collision_checker_test.cc @@ -25,10 +25,13 @@ class MyUnimplementedCollisionChecker : public UnimplementedCollisionChecker { GTEST_TEST(UnimplementedCollisionCheckerTest, SmokeTest) { CollisionCheckerParams params{ - .model = RobotDiagramBuilder{}.Build(), - .robot_model_instances = {default_model_instance()}, - .configuration_distance_function = [](auto...) { return 0; }, - .edge_step_size = 0.1, + .model = RobotDiagramBuilder{}.Build(), + .robot_model_instances = {default_model_instance()}, + .configuration_distance_function = + [](auto...) { + return 0; + }, + .edge_step_size = 0.1, }; // Prove that the class is concrete (no missing overrides). diff --git a/planning/test_utilities/BUILD.bazel b/planning/test_utilities/BUILD.bazel index 440742d81866..25a520a96e1b 100644 --- a/planning/test_utilities/BUILD.bazel +++ b/planning/test_utilities/BUILD.bazel @@ -35,4 +35,4 @@ drake_cc_library( ], ) -add_lint_tests() +add_lint_tests(enable_clang_format_lint = True) diff --git a/planning/test_utilities/collision_checker_abstract_test_suite.cc b/planning/test_utilities/collision_checker_abstract_test_suite.cc index 27a818ee9b96..5a078975a9d0 100644 --- a/planning/test_utilities/collision_checker_abstract_test_suite.cc +++ b/planning/test_utilities/collision_checker_abstract_test_suite.cc @@ -13,8 +13,8 @@ namespace planning { namespace test { using geometry::GeometryId; -using multibody::BodyIndex; using internal::ComputeCollisionAvoidanceDisplacement; +using multibody::BodyIndex; multibody::parsing::ModelDirectives MakeCollisionCheckerTestScene() { // Assemble model directives. @@ -29,13 +29,14 @@ multibody::parsing::ModelDirectives MakeCollisionCheckerTestScene() { multibody::parsing::ModelDirective add_robot_model; add_robot_model.add_model = multibody::parsing::AddModel{ "package://drake/manipulation/models/iiwa_description/urdf/" - "iiwa14_spheres_dense_collision.urdf", "iiwa"}; + "iiwa14_spheres_dense_collision.urdf", + "iiwa"}; multibody::parsing::ModelDirective add_robot_weld; - add_robot_weld.add_weld = multibody::parsing::AddWeld{ - "world", "iiwa::base"}; + add_robot_weld.add_weld = multibody::parsing::AddWeld{"world", "iiwa::base"}; - const multibody::parsing::ModelDirectives directives{.directives = { - add_env_model, add_env_weld, add_robot_model, add_robot_weld}}; + const multibody::parsing::ModelDirectives directives{ + .directives = {add_env_model, add_env_weld, add_robot_model, + add_robot_weld}}; return directives; } @@ -62,8 +63,7 @@ TEST_P(CollisionCheckerAbstractTestSuite, Clone) { EXPECT_EQ(checker.GetZeroConfiguration(), cloned->GetZeroConfiguration()); EXPECT_EQ(checker.edge_step_size(), cloned->edge_step_size()); EXPECT_EQ(checker.MaxNumDistances(), cloned->MaxNumDistances()); - EXPECT_EQ(checker.num_allocated_contexts(), - cloned->num_allocated_contexts()); + EXPECT_EQ(checker.num_allocated_contexts(), cloned->num_allocated_contexts()); EXPECT_EQ(checker.GetPaddingMatrix(), cloned->GetPaddingMatrix()); EXPECT_EQ(checker.GetFilteredCollisionMatrix(), cloned->GetFilteredCollisionMatrix()); @@ -92,8 +92,7 @@ TEST_P(CollisionCheckerAbstractTestSuite, AddSpheres) { auto& checker = *params.checker; const auto& dut_frame = checker.plant().GetFrameByName("iiwa_link_2"); // Add a really big ball so that collision will definitely happen. - checker.AddCollisionShapeToFrame("test", dut_frame, - geometry::Sphere(1.0), + checker.AddCollisionShapeToFrame("test", dut_frame, geometry::Sphere(1.0), math::RigidTransform()); EXPECT_FALSE(checker.CheckConfigCollisionFree(qs_.q1)); EXPECT_NE(checker.GetAllAddedCollisionShapes().size(), 0); @@ -120,8 +119,7 @@ TEST_P(CollisionCheckerAbstractTestSuite, AddSpheres) { EXPECT_EQ(checker.GetAllAddedCollisionShapes().size(), 0); // Re-add a shape to test removal of everything. - checker.AddCollisionShapeToFrame("test", dut_frame, - geometry::Sphere(1.0), + checker.AddCollisionShapeToFrame("test", dut_frame, geometry::Sphere(1.0), math::RigidTransform()); EXPECT_FALSE(checker.CheckConfigCollisionFree(qs_.q1)); EXPECT_NE(checker.GetAllAddedCollisionShapes().size(), 0); @@ -135,10 +133,9 @@ TEST_P(CollisionCheckerAbstractTestSuite, AddObstacles) { auto params = GetParam(); auto& checker = *params.checker; // Add a really big cylinder so that collision will definitely happen. - const bool added = - checker.AddCollisionShapeToFrame("test", checker.plant().world_frame(), - geometry::Cylinder(1.0, 1.0), - math::RigidTransform()); + const bool added = checker.AddCollisionShapeToFrame( + "test", checker.plant().world_frame(), geometry::Cylinder(1.0, 1.0), + math::RigidTransform()); EXPECT_EQ(params.supports_added_world_obstacles, added); if (added) { EXPECT_FALSE(checker.CheckConfigCollisionFree(qs_.q1)); @@ -280,8 +277,7 @@ void CollisionCheckerAbstractTestSuite::TestParallelizeableGradientQueries( qtest << 0.0, M_PI_2, 0.0, -M_PI_4 * 1.25, 0.0, 0.0, 0.0; const Eigen::VectorXd qtest_initial = qtest; EXPECT_FALSE(checker.CheckConfigCollisionFree(qtest)); - log()->info("Starting q: {}", - common_robotics_utilities::print::Print(qtest)); + log()->info("Starting q: {}", common_robotics_utilities::print::Print(qtest)); int current_iteration = 0; while (current_iteration < max_iterations && !checker.CheckConfigCollisionFree(qtest)) { @@ -294,20 +290,18 @@ void CollisionCheckerAbstractTestSuite::TestParallelizeableGradientQueries( const Eigen::VectorXd scaled_grad_qtest = grad_qtest.stableNormalized() * 0.05; qtest = qtest + scaled_grad_qtest; - log()->info( - "Iteration {}; raw grad_q: {}; scaled_grad_q: {}; new q: {}", - current_iteration, - common_robotics_utilities::print::Print(grad_qtest), - common_robotics_utilities::print::Print(scaled_grad_qtest), - common_robotics_utilities::print::Print(qtest)); + log()->info("Iteration {}; raw grad_q: {}; scaled_grad_q: {}; new q: {}", + current_iteration, + common_robotics_utilities::print::Print(grad_qtest), + common_robotics_utilities::print::Print(scaled_grad_qtest), + common_robotics_utilities::print::Print(qtest)); ++current_iteration; } EXPECT_TRUE(checker.CheckConfigCollisionFree(qtest)); - log()->info( - "Modified qtest {} to collision-free {} in {} iterations", - common_robotics_utilities::print::Print(qtest_initial), - common_robotics_utilities::print::Print(qtest), - current_iteration); + log()->info("Modified qtest {} to collision-free {} in {} iterations", + common_robotics_utilities::print::Print(qtest_initial), + common_robotics_utilities::print::Print(qtest), + current_iteration); } TEST_P(CollisionCheckerAbstractTestSuite, StressParallelGradientQueries) { diff --git a/planning/test_utilities/collision_checker_abstract_test_suite.h b/planning/test_utilities/collision_checker_abstract_test_suite.h index 118c0161be99..8659a41a0e28 100644 --- a/planning/test_utilities/collision_checker_abstract_test_suite.h +++ b/planning/test_utilities/collision_checker_abstract_test_suite.h @@ -21,7 +21,7 @@ namespace planning { namespace test { /* All abstract tests assume the same model; an iiwa welded to a ground plane. -*/ + */ multibody::parsing::ModelDirectives MakeCollisionCheckerTestScene(); /* All abstract tests assume some common constructor parameters for checkers. */ @@ -55,7 +55,7 @@ a collision checker of some derived type, and options that tests may need to adapt to the checker. */ struct CollisionCheckerTestParams { std::shared_ptr checker; - bool supports_added_world_obstacles{true}; // Most do, some don't. + bool supports_added_world_obstacles{true}; // Most do, some don't. // Some derived classes may benefit from more thread stress testing than // others; the default here provides relatively light stress. int thread_stress_iterations{10}; diff --git a/planning/unimplemented_collision_checker.cc b/planning/unimplemented_collision_checker.cc index 2b73b02beaa9..3539e2ab0b74 100644 --- a/planning/unimplemented_collision_checker.cc +++ b/planning/unimplemented_collision_checker.cc @@ -41,8 +41,8 @@ bool UnimplementedCollisionChecker::DoCheckContextConfigCollisionFree( std::optional UnimplementedCollisionChecker::DoAddCollisionShapeToBody( - const std::string&, const multibody::Body&, - const geometry::Shape&, const math::RigidTransform&) { + const std::string&, const multibody::Body&, const geometry::Shape&, + const math::RigidTransform&) { ThrowNotImplemented(__func__); }