From 0965a43eec9d3b8200b3050e5ee1a5fcc6e0cef4 Mon Sep 17 00:00:00 2001 From: Alejandro Castro Date: Wed, 15 Jan 2025 17:29:32 -0500 Subject: [PATCH] Clang formats some of the files in multibody/parsing (#22472) --- multibody/parsing/detail_sdf_parser.cc | 763 ++++++++--------- multibody/parsing/detail_urdf_parser.cc | 305 ++++--- .../parsing/test/detail_sdf_parser_test.cc | 785 +++++++++--------- .../parsing/test/detail_urdf_parser_test.cc | 626 ++++++++------ 4 files changed, 1299 insertions(+), 1180 deletions(-) diff --git a/multibody/parsing/detail_sdf_parser.cc b/multibody/parsing/detail_sdf_parser.cc index 48b511df6109..d8069df40758 100644 --- a/multibody/parsing/detail_sdf_parser.cc +++ b/multibody/parsing/detail_sdf_parser.cc @@ -100,8 +100,7 @@ using ModelInstanceIndexRange = // Returns the model instance name for the given `instance`, unless it's the // world model instance in which case returns the empty string. std::string GetInstanceScopeNameIgnoringWorld( - const MultibodyPlant& plant, - ModelInstanceIndex instance) { + const MultibodyPlant& plant, ModelInstanceIndex instance) { if (instance != plant.world_body().model_instance()) { return plant.GetModelInstanceName(instance); } else { @@ -141,7 +140,7 @@ std::pair GetResolvedModelInstanceAndLocalName( plant.GetModelInstanceName(model_instance), parent_name); resolved_model_instance = - plant.GetModelInstanceByName(parent_model_absolute_name); + plant.GetModelInstanceByName(parent_model_absolute_name); } } @@ -155,10 +154,9 @@ std::pair GetResolvedModelInstanceAndLocalName( // scoped name of the nested model. If the relative_to_model_instance is the // world_model_instance, the local name of the body is prefixed with the model // name. -std::string GetRelativeBodyName( - const RigidBody& body, - ModelInstanceIndex relative_to_model_instance, - const MultibodyPlant& plant) { +std::string GetRelativeBodyName(const RigidBody& body, + ModelInstanceIndex relative_to_model_instance, + const MultibodyPlant& plant) { const std::string& relative_to_model_absolute_name = plant.GetModelInstanceName(relative_to_model_instance); // If the relative_to_model instance is the world_model_instance, we need to @@ -201,27 +199,24 @@ math::RigidTransformd ResolveRigidTransform( return ToRigidTransform(pose); } -Eigen::Vector3d ResolveAxisXyz( - const SDFormatDiagnostic& diagnostic, - const sdf::JointAxis& axis) { +Eigen::Vector3d ResolveAxisXyz(const SDFormatDiagnostic& diagnostic, + const sdf::JointAxis& axis) { gz::math::Vector3d xyz; sdf::Errors errors = axis.ResolveXyz(xyz); diagnostic.PropagateErrors(errors); return ToVector3(xyz); } -std::string ResolveJointParentLinkName( - const SDFormatDiagnostic& diagnostic, - const sdf::Joint& joint) { +std::string ResolveJointParentLinkName(const SDFormatDiagnostic& diagnostic, + const sdf::Joint& joint) { std::string link; sdf::Errors errors = joint.ResolveParentLink(link); diagnostic.PropagateErrors(errors); return link; } -std::string ResolveJointChildLinkName( - const SDFormatDiagnostic& diagnostic, - const sdf::Joint& joint) { +std::string ResolveJointChildLinkName(const SDFormatDiagnostic& diagnostic, + const sdf::Joint& joint) { std::string link; sdf::Errors errors = joint.ResolveChildLink(link); diagnostic.PropagateErrors(errors); @@ -232,8 +227,7 @@ std::string ResolveJointChildLinkName( // frame origin Bo and, expressed in body frame B, from a gz::Inertial // object. SpatialInertia ExtractSpatialInertiaAboutBoExpressedInB( - const SDFormatDiagnostic& diagnostic, - const sdf::ElementPtr link_element, + const SDFormatDiagnostic& diagnostic, const sdf::ElementPtr link_element, const gz::math::Inertiald& Inertial_BBcm_Bi) { double mass = Inertial_BBcm_Bi.MassMatrix().Mass(); @@ -254,16 +248,20 @@ SpatialInertia ExtractSpatialInertiaAboutBoExpressedInB( // specified frames in the sdf file. That is, that it always returns // M_BBcm_Bi. const gz::math::Matrix3d I = Inertial_BBcm_Bi.MassMatrix().Moi(); - return ParseSpatialInertia(diagnostic.MakePolicyForNode(*link_element), - X_BBi, mass, - {.ixx = I(0, 0), .iyy = I(1, 1), .izz = I(2, 2), - .ixy = I(1, 0), .ixz = I(2, 0), .iyz = I(2, 1)}); + return ParseSpatialInertia(diagnostic.MakePolicyForNode(*link_element), X_BBi, + mass, + {.ixx = I(0, 0), + .iyy = I(1, 1), + .izz = I(2, 2), + .ixy = I(1, 0), + .ixz = I(2, 0), + .iyz = I(2, 1)}); } // Helper method to retrieve a Body given the name of the link specification. const RigidBody& GetBodyByLinkSpecificationName( - const std::string& link_name, - ModelInstanceIndex model_instance, const MultibodyPlant& plant) { + const std::string& link_name, ModelInstanceIndex model_instance, + const MultibodyPlant& plant) { // SDF's convention to indicate a joint is connected to the world is to either // name the corresponding link "world" or just leave it unnamed. // Thus this the "if" statement in the following line. @@ -278,9 +276,8 @@ const RigidBody& GetBodyByLinkSpecificationName( } // Extracts a Vector3d representation of the joint axis for joints with an axis. -Vector3d ExtractJointAxis( - const SDFormatDiagnostic& diagnostic, - const sdf::Joint& joint_spec) { +Vector3d ExtractJointAxis(const SDFormatDiagnostic& diagnostic, + const sdf::Joint& joint_spec) { DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::REVOLUTE || joint_spec.Type() == sdf::JointType::SCREW || joint_spec.Type() == sdf::JointType::PRISMATIC || @@ -290,8 +287,7 @@ Vector3d ExtractJointAxis( const sdf::JointAxis* axis = joint_spec.Axis(); if (axis == nullptr) { std::string message = fmt::format( - "An axis must be specified for joint '{}'", - joint_spec.Name()); + "An axis must be specified for joint '{}'", joint_spec.Name()); diagnostic.Error(joint_spec.Element(), std::move(message)); return Vector3d(0, 0, 1); } @@ -304,17 +300,17 @@ Vector3d ExtractJointAxis( // Extracts a Vector3d representation of `axis` and `axis2` for joints with both // attributes. Both axes are required. Otherwise, an error is triggered. std::pair ExtractJointAxisAndAxis2( - const SDFormatDiagnostic& diagnostic, const sdf::Joint& joint_spec) { + const SDFormatDiagnostic& diagnostic, const sdf::Joint& joint_spec) { DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::REVOLUTE2 || - joint_spec.Type() == sdf::JointType::UNIVERSAL); + joint_spec.Type() == sdf::JointType::UNIVERSAL); // Axis specification. const sdf::JointAxis* axis = joint_spec.Axis(0); const sdf::JointAxis* axis2 = joint_spec.Axis(1); if (axis == nullptr || axis2 == nullptr) { - std::string message = fmt::format( - "Both axis and axis2 must be specified for joint '{}'", - joint_spec.Name()); + std::string message = + fmt::format("Both axis and axis2 must be specified for joint '{}'", + joint_spec.Name()); diagnostic.Error(joint_spec.Element(), std::move(message)); return std::make_pair(Vector3d(1, 0, 0), Vector3d(0, 1, 0)); } @@ -327,15 +323,14 @@ std::pair ExtractJointAxisAndAxis2( // Helper to parse the damping for a given joint specification. // Right now we only parse the tag. -double ParseJointDamping( - const SDFormatDiagnostic& diagnostic, - const sdf::Joint& joint_spec) { +double ParseJointDamping(const SDFormatDiagnostic& diagnostic, + const sdf::Joint& joint_spec) { DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::REVOLUTE || - joint_spec.Type() == sdf::JointType::PRISMATIC || - joint_spec.Type() == sdf::JointType::SCREW || - joint_spec.Type() == sdf::JointType::UNIVERSAL || - joint_spec.Type() == sdf::JointType::BALL || - joint_spec.Type() == sdf::JointType::CONTINUOUS); + joint_spec.Type() == sdf::JointType::PRISMATIC || + joint_spec.Type() == sdf::JointType::SCREW || + joint_spec.Type() == sdf::JointType::UNIVERSAL || + joint_spec.Type() == sdf::JointType::BALL || + joint_spec.Type() == sdf::JointType::CONTINUOUS); // If the axis is missing, we'll rely on ExtractJointAxis to tell the user. // For our purposes in this function, it's OK to just bail and return zero. @@ -376,16 +371,14 @@ double ParseJointDamping( // user would say 0. The effort_limit should be non-negative. // SDFormat internally interprets a negative effort limit as infinite and only // returns non-negative values. -double GetEffortLimit( - const SDFormatDiagnostic& diagnostic, - const sdf::Joint& joint_spec, int axis_index) { +double GetEffortLimit(const SDFormatDiagnostic& diagnostic, + const sdf::Joint& joint_spec, int axis_index) { DRAKE_DEMAND(axis_index == 0 || axis_index == 1); const sdf::JointAxis* axis = joint_spec.Axis(axis_index); if (axis == nullptr) { - std::string message = fmt::format( - "An axis{} must be specified for joint '{}'", - axis_index > 0 ? "2" : "", - joint_spec.Name()); + std::string message = + fmt::format("An axis{} must be specified for joint '{}'", + axis_index > 0 ? "2" : "", joint_spec.Name()); diagnostic.Warning(joint_spec.Element(), std::move(message)); return 0.0; } @@ -397,10 +390,10 @@ double GetEffortLimit( // . In Drake, we understand that joints with an // effort limit of zero are not actuated. For joint types that do not have an // actuator implementation available in Drake, produces a diagnostic warning. -void AddJointActuatorFromSpecification( - const SDFormatDiagnostic& diagnostic, - const sdf::Joint& joint_spec, const Joint& joint, - MultibodyPlant* plant) { +void AddJointActuatorFromSpecification(const SDFormatDiagnostic& diagnostic, + const sdf::Joint& joint_spec, + const Joint& joint, + MultibodyPlant* plant) { DRAKE_THROW_UNLESS(plant != nullptr); DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::BALL || joint_spec.Type() == sdf::JointType::SCREW || @@ -416,7 +409,8 @@ void AddJointActuatorFromSpecification( if (joint_spec.Axis(0) != nullptr) { std::string message = fmt::format( "A ball joint axis will be ignored. Only the dynamic parameters" - " and limits will be considered.", joint_spec.Name()); + " and limits will be considered.", + joint_spec.Name()); diagnostic.Warning(joint_spec.Element(), std::move(message)); if (GetEffortLimit(diagnostic, joint_spec, 0) != 0) { std::string effort_message = fmt::format( @@ -429,7 +423,8 @@ void AddJointActuatorFromSpecification( if (joint_spec.Axis(1) != nullptr) { std::string message = fmt::format( "An axis2 may not be specified for ball joint '{}' and will be " - "ignored", joint_spec.Name()); + "ignored", + joint_spec.Name()); diagnostic.Warning(joint_spec.Element(), std::move(message)); } return; @@ -500,7 +495,8 @@ void AddJointActuatorFromSpecification( if (joint_spec.Axis(1) != nullptr) { std::string message = fmt::format( "An axis2 may not be specified for 1-dof joint '{}' and will be " - "ignored", joint_spec.Name()); + "ignored", + joint_spec.Name()); diagnostic.Warning(joint_spec.Element(), std::move(message)); } } @@ -521,8 +517,7 @@ void AddPrismaticSpringFromSpecification(const SDFormatDiagnostic& diagnostic, const sdf::JointAxis* axis = joint_spec.Axis(); if (axis == nullptr) { std::string message = fmt::format( - "An axis must be specified for joint '{}'.", - joint_spec.Name()); + "An axis must be specified for joint '{}'.", joint_spec.Name()); diagnostic.Error(joint_spec.Element(), std::move(message)); return; } @@ -533,8 +528,8 @@ void AddPrismaticSpringFromSpecification(const SDFormatDiagnostic& diagnostic, // We add a force element if stiffness is positive, report error // if the stiffness is negative, and pass if the stiffness is zero if (spring_stiffness > 0) { - plant->AddForceElement( - joint, spring_reference, spring_stiffness); + plant->AddForceElement(joint, spring_reference, + spring_stiffness); } else if (spring_stiffness < 0) { std::string message = fmt::format( "The stiffness specified for joint '{}' must be non-negative.", @@ -550,9 +545,10 @@ void AddPrismaticSpringFromSpecification(const SDFormatDiagnostic& diagnostic, // reference is radians and the units for spring stiffness is N⋅m/rad. // When the error diagnostic policy is not set to throw this function will // return false on errors. -bool AddRevoluteSpringFromSpecification( - const SDFormatDiagnostic& diagnostic, const sdf::Joint &joint_spec, - const RevoluteJoint& joint, MultibodyPlant* plant) { +bool AddRevoluteSpringFromSpecification(const SDFormatDiagnostic& diagnostic, + const sdf::Joint& joint_spec, + const RevoluteJoint& joint, + MultibodyPlant* plant) { DRAKE_THROW_UNLESS(plant != nullptr); DRAKE_THROW_UNLESS(joint_spec.Type() == sdf::JointType::REVOLUTE || joint_spec.Type() == sdf::JointType::CONTINUOUS); @@ -560,8 +556,8 @@ bool AddRevoluteSpringFromSpecification( // Axis specification. const sdf::JointAxis* axis = joint_spec.Axis(); if (axis == nullptr) { - std::string message = "An axis must be specified for joint '" - + joint_spec.Name() + "'"; + std::string message = + "An axis must be specified for joint '" + joint_spec.Name() + "'"; diagnostic.Error(joint_spec.Element(), std::move(message)); return false; } @@ -573,8 +569,8 @@ bool AddRevoluteSpringFromSpecification( // If a negative value is passed in, RevoluteSpring will // throw an error. if (spring_stiffness != 0) { - plant->AddForceElement( - joint, spring_reference, spring_stiffness); + plant->AddForceElement(joint, spring_reference, + spring_stiffness); } return true; @@ -591,16 +587,15 @@ bool AddRevoluteSpringFromSpecification( // continuous. When the diagnostic policy is not set to throw it will return // std::nullopt on errors. std::optional> ParseJointLimits( - const SDFormatDiagnostic& diagnostic, - const sdf::Joint& joint_spec) { + const SDFormatDiagnostic& diagnostic, const sdf::Joint& joint_spec) { DRAKE_THROW_UNLESS(joint_spec.Type() == sdf::JointType::REVOLUTE || joint_spec.Type() == sdf::JointType::PRISMATIC || joint_spec.Type() == sdf::JointType::CONTINUOUS); // Axis specification. const sdf::JointAxis* axis = joint_spec.Axis(); if (axis == nullptr) { - std::string message = "An axis must be specified for joint '" - + joint_spec.Name() + "'"; + std::string message = + "An axis must be specified for joint '" + joint_spec.Name() + "'"; diagnostic.Error(joint_spec.Element(), std::move(message)); return std::nullopt; } @@ -610,8 +605,10 @@ std::optional> ParseJointLimits( const double lower_limit = axis->Lower(); const double upper_limit = axis->Upper(); if (lower_limit > upper_limit) { - std::string message = "The lower limit must be lower (or equal) than " - "the upper limit for joint '" + joint_spec.Name() + "'."; + std::string message = + "The lower limit must be lower (or equal) than " + "the upper limit for joint '" + + joint_spec.Name() + "'."; diagnostic.Error(joint_spec.Element(), std::move(message)); return std::nullopt; } @@ -626,29 +623,25 @@ std::optional> ParseJointLimits( if (axis->Element()->HasElement("limit")) { const auto limit_element = axis->Element()->GetElement("limit"); const std::set supported_limit_elements{ - "drake:acceleration", - "effort", - "lower", - "stiffness", - "upper", - "velocity"}; - CheckSupportedElements(diagnostic, limit_element, - supported_limit_elements); + "drake:acceleration", "effort", "lower", + "stiffness", "upper", "velocity"}; + CheckSupportedElements(diagnostic, limit_element, supported_limit_elements); if (limit_element->HasElement("drake:acceleration")) { acceleration_limit = limit_element->Get("drake:acceleration"); if (acceleration_limit < 0) { - std::string message = "Acceleration limit is negative for joint '" - + joint_spec.Name() + "'. Aceleration limit must be a non-negative" - " number."; + std::string message = "Acceleration limit is negative for joint '" + + joint_spec.Name() + + "'. Aceleration limit must be a non-negative" + " number."; diagnostic.Error(limit_element, std::move(message)); return std::tuple(); } } } - return std::make_tuple( - lower_limit, upper_limit, velocity_limit, acceleration_limit); + return std::make_tuple(lower_limit, upper_limit, velocity_limit, + acceleration_limit); } // Helper method to add joints to a MultibodyPlant given an sdf::Joint @@ -657,23 +650,23 @@ std::optional> ParseJointLimits( // containing model, hence M = W. // If the diagnostic error policy is not set to throw it returns false // when an error occurs. -bool AddJointFromSpecification( - const SDFormatDiagnostic& diagnostic, const RigidTransformd& X_WM, - const sdf::Joint& joint_spec, ModelInstanceIndex model_instance, - MultibodyPlant* plant, std::set* joint_types, - bool is_model_joint = true) { - - const std::set supported_joint_elements{ - "axis", - "axis2", - "child", - "drake:rotor_inertia", - "drake:gear_ratio", - "drake:controller_gains", - "drake:mimic", - "parent", - "pose", - "screw_thread_pitch"}; +bool AddJointFromSpecification(const SDFormatDiagnostic& diagnostic, + const RigidTransformd& X_WM, + const sdf::Joint& joint_spec, + ModelInstanceIndex model_instance, + MultibodyPlant* plant, + std::set* joint_types, + bool is_model_joint = true) { + const std::set supported_joint_elements{"axis", + "axis2", + "child", + "drake:rotor_inertia", + "drake:gear_ratio", + "drake:controller_gains", + "drake:mimic", + "parent", + "pose", + "screw_thread_pitch"}; CheckSupportedElements(diagnostic, joint_spec.Element(), supported_joint_elements); @@ -721,11 +714,9 @@ bool AddJointFromSpecification( switch (joint_spec.Type()) { case sdf::JointType::FIXED: { - plant->AddJoint( - joint_spec.Name(), - parent_body, X_PJ, - child_body, X_CJ, - RigidTransformd::Identity() /* X_JpJc */); + plant->AddJoint(joint_spec.Name(), parent_body, X_PJ, + child_body, X_CJ, + RigidTransformd::Identity() /* X_JpJc */); break; } case sdf::JointType::PRISMATIC: { @@ -737,13 +728,14 @@ bool AddJointFromSpecification( std::tie(lower_limit, upper_limit, velocity_limit, acceleration_limit) = *joint_limits; const auto& joint = plant->AddJoint( - joint_spec.Name(), - parent_body, X_PJ, - child_body, X_CJ, axis_J, lower_limit, upper_limit, damping); - plant->get_mutable_joint(joint.index()).set_velocity_limits( - Vector1d(-velocity_limit), Vector1d(velocity_limit)); - plant->get_mutable_joint(joint.index()).set_acceleration_limits( - Vector1d(-acceleration_limit), Vector1d(acceleration_limit)); + joint_spec.Name(), parent_body, X_PJ, child_body, X_CJ, axis_J, + lower_limit, upper_limit, damping); + plant->get_mutable_joint(joint.index()) + .set_velocity_limits(Vector1d(-velocity_limit), + Vector1d(velocity_limit)); + plant->get_mutable_joint(joint.index()) + .set_acceleration_limits(Vector1d(-acceleration_limit), + Vector1d(acceleration_limit)); AddJointActuatorFromSpecification(diagnostic, joint_spec, joint, plant); AddPrismaticSpringFromSpecification(diagnostic, joint_spec, joint, plant); break; @@ -757,16 +749,17 @@ bool AddJointFromSpecification( std::tie(lower_limit, upper_limit, velocity_limit, acceleration_limit) = *joint_limits; const auto& joint = plant->AddJoint( - joint_spec.Name(), - parent_body, X_PJ, - child_body, X_CJ, axis_J, lower_limit, upper_limit, damping); - plant->get_mutable_joint(joint.index()).set_velocity_limits( - Vector1d(-velocity_limit), Vector1d(velocity_limit)); - plant->get_mutable_joint(joint.index()).set_acceleration_limits( - Vector1d(-acceleration_limit), Vector1d(acceleration_limit)); + joint_spec.Name(), parent_body, X_PJ, child_body, X_CJ, axis_J, + lower_limit, upper_limit, damping); + plant->get_mutable_joint(joint.index()) + .set_velocity_limits(Vector1d(-velocity_limit), + Vector1d(velocity_limit)); + plant->get_mutable_joint(joint.index()) + .set_acceleration_limits(Vector1d(-acceleration_limit), + Vector1d(acceleration_limit)); AddJointActuatorFromSpecification(diagnostic, joint_spec, joint, plant); - if (!AddRevoluteSpringFromSpecification( - diagnostic, joint_spec, joint, plant)) { + if (!AddRevoluteSpringFromSpecification(diagnostic, joint_spec, joint, + plant)) { return false; } break; @@ -794,9 +787,9 @@ bool AddJointFromSpecification( // We require that axis and axis2 are orthogonal. As a result, R_JI should // be a valid rotation matrix. if (!math::RotationMatrixd::IsValid(R_JI)) { - std::string message = fmt::format( - "axis and axis2 must be orthogonal for joint '{}'", - joint_spec.Name()); + std::string message = + fmt::format("axis and axis2 must be orthogonal for joint '{}'", + joint_spec.Name()); diagnostic.Error(joint_spec.Element(), std::move(message)); } else { const RigidTransformd X_JI(math::RotationMatrix{R_JI}); @@ -808,9 +801,7 @@ bool AddJointFromSpecification( const RigidTransformd& X_PF = X_PI; const RigidTransformd& X_CM = X_CI; const auto& joint = plant->AddJoint( - joint_spec.Name(), - parent_body, X_PF, - child_body, X_CM, damping); + joint_spec.Name(), parent_body, X_PF, child_body, X_CM, damping); // At most, this prints a warning (it does not add an actuator). AddJointActuatorFromSpecification(diagnostic, joint_spec, joint, plant); } @@ -819,9 +810,7 @@ bool AddJointFromSpecification( case sdf::JointType::BALL: { const double damping = ParseJointDamping(diagnostic, joint_spec); const auto& joint = plant->AddJoint( - joint_spec.Name(), - parent_body, X_PJ, - child_body, X_CJ, damping); + joint_spec.Name(), parent_body, X_PJ, child_body, X_CJ, damping); // At most, this prints a warning (it does not add an actuator). AddJointActuatorFromSpecification(diagnostic, joint_spec, joint, plant); break; @@ -844,8 +833,8 @@ bool AddJointFromSpecification( .set_acceleration_limits(Vector1d(-acceleration_limit), Vector1d(acceleration_limit)); AddJointActuatorFromSpecification(diagnostic, joint_spec, joint, plant); - if (!AddRevoluteSpringFromSpecification( - diagnostic, joint_spec, joint, plant)) { + if (!AddRevoluteSpringFromSpecification(diagnostic, joint_spec, joint, + plant)) { return false; } break; @@ -857,27 +846,26 @@ bool AddJointFromSpecification( const double screw_thread_pitch = joint_spec.ScrewThreadPitch(); Vector3d axis_J = ExtractJointAxis(diagnostic, joint_spec); const auto& joint = plant->AddJoint( - joint_spec.Name(), - parent_body, X_PJ, - child_body, X_CJ, axis_J, screw_thread_pitch, damping); + joint_spec.Name(), parent_body, X_PJ, child_body, X_CJ, axis_J, + screw_thread_pitch, damping); AddJointActuatorFromSpecification(diagnostic, joint_spec, joint, plant); break; } case sdf::JointType::GEARBOX: { // TODO(jwnimmer-tri) Demote this to a warning, possibly adding a // RevoluteJoint as an approximation (stopgap) in that case. - std::string message = fmt::format( - "Joint type (gearbox) not supported for joint '{}'.", - joint_spec.Name()); + std::string message = + fmt::format("Joint type (gearbox) not supported for joint '{}'.", + joint_spec.Name()); diagnostic.Error(joint_spec.Element(), std::move(message)); break; } case sdf::JointType::REVOLUTE2: { // TODO(jwnimmer-tri) Demote this to a warning, possibly adding a // UniversalJoint as an approximation (stopgap) in that case. - std::string message = fmt::format( - "Joint type (revolute2) not supported for joint '{}'.", - joint_spec.Name()); + std::string message = + fmt::format("Joint type (revolute2) not supported for joint '{}'.", + joint_spec.Name()); diagnostic.Error(joint_spec.Element(), std::move(message)); break; } @@ -984,11 +972,10 @@ bool ParseMimicTag(const SDFormatDiagnostic& diagnostic, // Helper method to load an SDF file and read the contents into an sdf::Root // object. -[[nodiscard]] sdf::Errors LoadSdf( - const SDFormatDiagnostic& diagnostic, - sdf::Root* root, - const DataSource& data_source, - const sdf::ParserConfig& parser_config) { +[[nodiscard]] sdf::Errors LoadSdf(const SDFormatDiagnostic& diagnostic, + sdf::Root* root, + const DataSource& data_source, + const sdf::ParserConfig& parser_config) { sdf::Errors errors; if (data_source.IsFilename()) { const std::string full_path = data_source.GetAbsolutePath(); @@ -998,12 +985,10 @@ bool ParseMimicTag(const SDFormatDiagnostic& diagnostic, } if (errors.empty()) { - const std::set supported_root_elements{ - "model", - "world"}; + const std::set supported_root_elements{"model", "world"}; - CheckSupportedElements( - diagnostic, root->Element(), supported_root_elements); + CheckSupportedElements(diagnostic, root->Element(), + supported_root_elements); } return errors; } @@ -1086,30 +1071,21 @@ void DrakifyModel(const SDFormatDiagnostic& diagnostic, // specification object. std::optional> AddLinksFromSpecification( const SDFormatDiagnostic& diagnostic, - const ModelInstanceIndex model_instance, - const sdf::Model& model, - const RigidTransformd& X_WM, - MultibodyPlant* plant, - const PackageMap& package_map, - const std::string& root_dir) { + const ModelInstanceIndex model_instance, const sdf::Model& model, + const RigidTransformd& X_WM, MultibodyPlant* plant, + const PackageMap& package_map, const std::string& root_dir) { std::vector link_infos; const std::set supported_link_elements{ - "drake:visual", - "collision", - "gravity", - "inertial", - "kinematic", - "pose", - "visual"}; + "drake:visual", "collision", "gravity", "inertial", + "kinematic", "pose", "visual"}; // Add all the links for (uint64_t link_index = 0; link_index < model.LinkCount(); ++link_index) { const sdf::Link& link = *model.LinkByIndex(link_index); sdf::ElementPtr link_element = link.Element(); - CheckSupportedElements( - diagnostic, link_element, supported_link_elements); + CheckSupportedElements(diagnostic, link_element, supported_link_elements); CheckSupportedElementValue(diagnostic, link_element, "kinematic", "false"); CheckSupportedElementValue(diagnostic, link_element, "gravity", "true"); @@ -1122,16 +1098,16 @@ std::optional> AddLinksFromSpecification( const gz::math::Inertiald& Inertial_Bcm_Bi = link.Inertial(); const SpatialInertia M_BBo_B = - ExtractSpatialInertiaAboutBoExpressedInB( - diagnostic, link_element, Inertial_Bcm_Bi); + ExtractSpatialInertiaAboutBoExpressedInB(diagnostic, link_element, + Inertial_Bcm_Bi); // Add a rigid body to model each link. const RigidBody& body = plant->AddRigidBody(link.Name(), model_instance, M_BBo_B); // Register information. - const RigidTransformd X_ML = ResolveRigidTransform( - diagnostic, link.SemanticPose()); + const RigidTransformd X_ML = + ResolveRigidTransform(diagnostic, link.SemanticPose()); const RigidTransformd X_WL = X_WM * X_ML; link_infos.push_back(LinkInfo{&body, X_WL}); @@ -1140,24 +1116,16 @@ std::optional> AddLinksFromSpecification( plant->SetDefaultFreeBodyPose(body, X_WL); const std::set supported_geometry_elements{ - "box", - "capsule", - "cylinder", - "drake:capsule", - "drake:ellipsoid", - "ellipsoid", - "empty", - "mesh", - "plane", - "sphere"}; + "box", "capsule", "cylinder", "drake:capsule", "drake:ellipsoid", + "ellipsoid", "empty", "mesh", "plane", "sphere"}; if (plant->geometry_source_is_registered()) { ResolveFilename resolve_filename = - [&package_map, &root_dir, &link_element]( - const SDFormatDiagnostic& inner_diagnostic, std::string uri) { - return ResolveUri(inner_diagnostic.MakePolicyForNode(*link_element), - uri, package_map, root_dir); - }; + [&package_map, &root_dir, &link_element]( + const SDFormatDiagnostic& inner_diagnostic, std::string uri) { + return ResolveUri(inner_diagnostic.MakePolicyForNode(*link_element), + uri, package_map, root_dir); + }; for (uint64_t visual_index = 0; visual_index < link.VisualCount(); ++visual_index) { @@ -1165,14 +1133,14 @@ std::optional> AddLinksFromSpecification( const sdf::Geometry& sdf_geometry = *sdf_visual.Geom(); sdf::ElementPtr geometry_element = sdf_geometry.Element(); - CheckSupportedElements( - diagnostic, geometry_element, supported_geometry_elements); + CheckSupportedElements(diagnostic, geometry_element, + supported_geometry_elements); - const RigidTransformd X_LG = ResolveRigidTransform( - diagnostic, sdf_visual.SemanticPose()); + const RigidTransformd X_LG = + ResolveRigidTransform(diagnostic, sdf_visual.SemanticPose()); unique_ptr geometry_instance = - MakeGeometryInstanceFromSdfVisual( - diagnostic, sdf_visual, resolve_filename, X_LG); + MakeGeometryInstanceFromSdfVisual(diagnostic, sdf_visual, + resolve_filename, X_LG); // No instance may simply mean there was a visual we should skip and we // move on to the next. If there is a _real_ problem, we assume an error // was reported to diagnostic (and it responds appropriately). @@ -1193,24 +1161,23 @@ std::optional> AddLinksFromSpecification( const sdf::Geometry& sdf_geometry = *sdf_collision.Geom(); sdf::ElementPtr geometry_element = sdf_geometry.Element(); - CheckSupportedElements( - diagnostic, geometry_element, supported_geometry_elements); + CheckSupportedElements(diagnostic, geometry_element, + supported_geometry_elements); std::optional> shape = - MakeShapeFromSdfGeometry( - diagnostic, sdf_geometry, resolve_filename); + MakeShapeFromSdfGeometry(diagnostic, sdf_geometry, + resolve_filename); if (!shape.has_value()) return std::nullopt; if (*shape != nullptr) { - const RigidTransformd X_LG = ResolveRigidTransform( - diagnostic, sdf_collision.SemanticPose()); + const RigidTransformd X_LG = + ResolveRigidTransform(diagnostic, sdf_collision.SemanticPose()); const RigidTransformd X_LC = MakeGeometryPoseFromSdfCollision(sdf_collision, X_LG); std::optional props = MakeProximityPropertiesForCollision(diagnostic, sdf_collision); if (!props.has_value()) return std::nullopt; - plant->RegisterCollisionGeometry(body, X_LC, **shape, - sdf_collision.Name(), - std::move(*props)); + plant->RegisterCollisionGeometry( + body, X_LC, **shape, sdf_collision.Name(), std::move(*props)); } } } @@ -1219,18 +1186,20 @@ std::optional> AddLinksFromSpecification( } const Frame& AddFrameFromSpecification( - const SDFormatDiagnostic& diagnostic, - const sdf::Frame& frame_spec, ModelInstanceIndex model_instance, - const Frame& default_frame, MultibodyPlant* plant) { + const SDFormatDiagnostic& diagnostic, const sdf::Frame& frame_spec, + ModelInstanceIndex model_instance, const Frame& default_frame, + MultibodyPlant* plant) { const Frame* parent_frame{}; const RigidTransformd X_PF = ResolveRigidTransform( diagnostic, frame_spec.SemanticPose(), frame_spec.AttachedTo()); if (frame_spec.AttachedTo().empty()) { parent_frame = &default_frame; } else { - const std::string attached_to_absolute_name = ScopedName::Join( - GetInstanceScopeNameIgnoringWorld(*plant, model_instance), - frame_spec.AttachedTo()).to_string(); + const std::string attached_to_absolute_name = + ScopedName::Join( + GetInstanceScopeNameIgnoringWorld(*plant, model_instance), + frame_spec.AttachedTo()) + .to_string(); // If the attached_to refers to a model, we use the `__model__` frame // associated with the model. @@ -1257,13 +1226,14 @@ const Frame& AddFrameFromSpecification( // whatever is referenced by the `attached_to` attribute. Since this is // a body, we're assured that its implicit frame exists in the plant. std::string resolved_attached_to_body_name; - sdf::Errors errors = frame_spec.ResolveAttachedToBody( - resolved_attached_to_body_name); + sdf::Errors errors = + frame_spec.ResolveAttachedToBody(resolved_attached_to_body_name); diagnostic.PropagateErrors(errors); const std::string resolved_attached_to_body_absolute_name = ScopedName::Join( GetInstanceScopeNameIgnoringWorld(*plant, model_instance), - resolved_attached_to_body_name).to_string(); + resolved_attached_to_body_name) + .to_string(); parent_frame = parsing::GetScopedFrameByNameMaybe( *plant, resolved_attached_to_body_absolute_name); } @@ -1283,9 +1253,9 @@ Eigen::Vector3d ParseVector3(const SDFormatDiagnostic& diagnostic, const sdf::ElementPtr node, const char* element_name) { if (!node->HasElement(element_name)) { - std::string message = fmt::format( - "<{}>: Unable to find the <{}> child tag.", node->GetName(), - element_name); + std::string message = + fmt::format("<{}>: Unable to find the <{}> child tag.", node->GetName(), + element_name); diagnostic.Error(node, message); return {}; } @@ -1301,9 +1271,9 @@ const Frame* ParseFrame(const SDFormatDiagnostic& diagnostic, MultibodyPlant* plant, const char* element_name) { if (!node->HasElement(element_name)) { - std::string message = fmt::format( - "<{}>: Unable to find the <{}> child tag.", - node->GetName(), element_name); + std::string message = + fmt::format("<{}>: Unable to find the <{}> child tag.", node->GetName(), + element_name); diagnostic.Error(node, std::move(message)); return nullptr; } @@ -1326,10 +1296,10 @@ const Frame* ParseFrame(const SDFormatDiagnostic& diagnostic, const std::string search_model_name(absolute_scoped_name.get_namespace()); if (!plant->HasModelInstanceNamed(search_model_name)) { std::string message = fmt::format( - "<{}>: Model instance name '{}' (implied by frame name '{}' in <{}>" - " within model instance '{}') does not exist in the model.", - node->GetName(), search_model_name, frame_name, element_name, - current_model_name); + "<{}>: Model instance name '{}' (implied by frame name '{}' in <{}>" + " within model instance '{}') does not exist in the model.", + node->GetName(), search_model_name, frame_name, element_name, + current_model_name); diagnostic.Error(node, std::move(message)); return nullptr; } @@ -1340,8 +1310,8 @@ const Frame* ParseFrame(const SDFormatDiagnostic& diagnostic, if (!plant->HasFrameNamed(search_frame_name, search_model_instance)) { std::string message = fmt::format( - "<{}>: Frame '{}' specified for <{}> does not exist in the model.", - node->GetName(), frame_name, element_name); + "<{}>: Frame '{}' specified for <{}> does not exist in the model.", + node->GetName(), frame_name, element_name); diagnostic.Error(node, std::move(message)); return nullptr; } @@ -1383,22 +1353,17 @@ bool AddDrakeJointFromSpecification(const SDFormatDiagnostic& diagnostic, ModelInstanceIndex model_instance, MultibodyPlant* plant) { const std::set supported_joint_elements{ - "drake:parent", - "drake:child", - "drake:damping", - "pose"}; + "drake:parent", "drake:child", "drake:damping", "pose"}; CheckSupportedElements(diagnostic, node, supported_joint_elements); if (!node->HasAttribute("type")) { - std::string message = - ": Unable to find the 'type' attribute."; + std::string message = ": Unable to find the 'type' attribute."; diagnostic.Error(node, std::move(message)); return false; } const std::string joint_type = node->Get("type"); if (!node->HasAttribute("name")) { - std::string message = - ": Unable to find the 'name' attribute."; + std::string message = ": Unable to find the 'name' attribute."; diagnostic.Error(node, std::move(message)); return false; } @@ -1414,10 +1379,14 @@ bool AddDrakeJointFromSpecification(const SDFormatDiagnostic& diagnostic, const Frame* parent_frame = ParseFrame(diagnostic, node, model_instance, plant, "drake:parent"); - if (parent_frame == nullptr) { return false; } + if (parent_frame == nullptr) { + return false; + } const Frame* child_frame = ParseFrame(diagnostic, node, model_instance, plant, "drake:child"); - if (child_frame == nullptr) { return false; } + if (child_frame == nullptr) { + return false; + } if (joint_type == "planar") { // TODO(eric.cousineau): Error out when there are unused tags. @@ -1425,7 +1394,8 @@ bool AddDrakeJointFromSpecification(const SDFormatDiagnostic& diagnostic, plant->AddJoint(std::make_unique>( joint_name, *parent_frame, *child_frame, damping)); } else { - std::string message = "ERROR: '" + joint_name + + std::string message = + "ERROR: '" + joint_name + "' has unrecognized value for 'type' attribute: " + joint_type; diagnostic.Error(node, std::move(message)); return false; @@ -1434,24 +1404,19 @@ bool AddDrakeJointFromSpecification(const SDFormatDiagnostic& diagnostic, } const LinearBushingRollPitchYaw* AddBushingFromSpecification( - const SDFormatDiagnostic& diagnostic, - const sdf::ElementPtr node, - ModelInstanceIndex model_instance, - MultibodyPlant* plant) { + const SDFormatDiagnostic& diagnostic, const sdf::ElementPtr node, + ModelInstanceIndex model_instance, MultibodyPlant* plant) { const std::set supported_bushing_elements{ - "drake:bushing_frameA", - "drake:bushing_frameC", - "drake:bushing_force_damping", - "drake:bushing_force_stiffness", - "drake:bushing_torque_damping", - "drake:bushing_torque_stiffness"}; + "drake:bushing_frameA", "drake:bushing_frameC", + "drake:bushing_force_damping", "drake:bushing_force_stiffness", + "drake:bushing_torque_damping", "drake:bushing_torque_stiffness"}; CheckSupportedElements(diagnostic, node, supported_bushing_elements); // Functor to read a vector valued child tag with tag name: `element_name` // e.g. 0 0 0 // Throws an error if the tag does not exist. - auto read_vector = [&diagnostic, node]( - const char* element_name) -> Eigen::Vector3d { + auto read_vector = [&diagnostic, + node](const char* element_name) -> Eigen::Vector3d { return ParseVector3(diagnostic, node, element_name); }; @@ -1459,8 +1424,8 @@ const LinearBushingRollPitchYaw* AddBushingFromSpecification( // frame name, e.g. frame_name // Throws an error if the tag does not exist or if the frame does not exist in // the plant. - auto read_frame = [&diagnostic, node, model_instance, plant]( - const char* element_name) -> const Frame* { + auto read_frame = [&diagnostic, node, model_instance, + plant](const char* element_name) -> const Frame* { return ParseFrame(diagnostic, node, model_instance, plant, element_name); }; @@ -1499,9 +1464,8 @@ std::optional AddBallConstraintFromSpecification( } // Helper to determine if two links are welded together. -bool AreWelded( - const MultibodyPlant& plant, const RigidBody& a, - const RigidBody& b) { +bool AreWelded(const MultibodyPlant& plant, const RigidBody& a, + const RigidBody& b) { for (auto* body : plant.GetBodiesWeldedTo(a)) { if (body == &b) { return true; @@ -1530,43 +1494,42 @@ void ParseCollisionFilterGroup(const SDFormatDiagnostic& diagnostic, return std::get(data_element) ->HasAttribute(std::string(attribute_name)); }; - auto get_string_attribute = - [&diagnostic](const ElementNode& data_element, - const char* attribute_name) -> std::string { - auto element = std::get(data_element); - if (!element->HasAttribute(attribute_name)) { - std::string message = fmt::format( - "The tag <{}> is missing the required attribute \"{}\"", - element->GetName(), attribute_name); - diagnostic.Error(element, std::move(message)); - return {}; - } - return std::get(data_element) - ->Get(attribute_name); - }; + auto get_string_attribute = [&diagnostic]( + const ElementNode& data_element, + const char* attribute_name) -> std::string { + auto element = std::get(data_element); + if (!element->HasAttribute(attribute_name)) { + std::string message = + fmt::format("The tag <{}> is missing the required attribute \"{}\"", + element->GetName(), attribute_name); + diagnostic.Error(element, std::move(message)); + return {}; + } + return std::get(data_element) + ->Get(attribute_name); + }; auto get_bool_attribute = [](const ElementNode& data_element, const char* attribute_name) { return std::get(data_element)->Get(attribute_name); }; - auto read_tag_string = - [&diagnostic](const ElementNode& data_element, const char*) - -> std::string { - auto element = std::get(data_element); - sdf::ParamPtr param = element->GetValue(); - if (param == nullptr) { - std::string message = fmt::format( - "The tag <{}> is missing a required string value.", - element->GetName()); - diagnostic.Error(element, std::move(message)); - return {}; - } - return param->GetAsString(); - }; + auto read_tag_string = [&diagnostic](const ElementNode& data_element, + const char*) -> std::string { + auto element = std::get(data_element); + sdf::ParamPtr param = element->GetValue(); + if (param == nullptr) { + std::string message = + fmt::format("The tag <{}> is missing a required string value.", + element->GetName()); + diagnostic.Error(element, std::move(message)); + return {}; + } + return param->GetAsString(); + }; ParseCollisionFilterGroupCommon( - diagnostic.MakePolicyForNode(*(model.Element())), - model_instance, model.Element(), plant, resolver, - next_child_element, next_sibling_element, has_attribute, - get_string_attribute, get_bool_attribute, read_tag_string); + diagnostic.MakePolicyForNode(*(model.Element())), model_instance, + model.Element(), plant, resolver, next_child_element, + next_sibling_element, has_attribute, get_string_attribute, + get_bool_attribute, read_tag_string); } bool CanReuseModelInstance( @@ -1645,13 +1608,13 @@ class InterfaceModelHelper { void ComputeDiffFromSnapshot() { DRAKE_DEMAND(have_snapshot); body_indices_ = - GetVectorDiff(plant_.GetBodyIndices(model_instance_), body_indices_); + GetVectorDiff(plant_.GetBodyIndices(model_instance_), body_indices_); frame_indices_ = - GetVectorDiff(plant_.GetFrameIndices(model_instance_), frame_indices_); + GetVectorDiff(plant_.GetFrameIndices(model_instance_), frame_indices_); joint_indices_ = - GetVectorDiff(plant_.GetJointIndices(model_instance_), joint_indices_); + GetVectorDiff(plant_.GetJointIndices(model_instance_), joint_indices_); model_instance_indices_ = - GetVectorDiff(GetChildModelInstanceIndices(), model_instance_indices_); + GetVectorDiff(GetChildModelInstanceIndices(), model_instance_indices_); } ModelInstanceIndex model_instance() const { return model_instance_; } @@ -1684,15 +1647,11 @@ class InterfaceModelHelper { // Helper method to add a model to a MultibodyPlant given an sdf::Model // specification object. std::vector AddModelsFromSpecification( - const SDFormatDiagnostic& diagnostic, - sdf::Model* model_ptr, - const std::string& model_name, - const RigidTransformd& X_WP, - MultibodyPlant* plant, - CollisionFilterGroupResolver* resolver, - const PackageMap& package_map, - const std::string& root_dir, - const ModelInstanceIndexRange &reusable_model_instance_range, + const SDFormatDiagnostic& diagnostic, sdf::Model* model_ptr, + const std::string& model_name, const RigidTransformd& X_WP, + MultibodyPlant* plant, CollisionFilterGroupResolver* resolver, + const PackageMap& package_map, const std::string& root_dir, + const ModelInstanceIndexRange& reusable_model_instance_range, const sdf::ParserConfig& parser_config) { DRAKE_DEMAND(model_ptr != nullptr); @@ -1703,27 +1662,26 @@ std::vector AddModelsFromSpecification( plant, model_name, reusable_model_instance_range); const std::set supported_model_elements{ - "drake:joint", - "drake:linear_bushing_rpy", - "drake:ball_constraint", - "drake:collision_filter_group", - "frame", - "include", - "joint", - "link", - "model", - "pose", - "static"}; - CheckSupportedElements( - diagnostic, model.Element(), supported_model_elements); - - std::vector added_model_instances{model_instance}; + "drake:joint", + "drake:linear_bushing_rpy", + "drake:ball_constraint", + "drake:collision_filter_group", + "frame", + "include", + "joint", + "link", + "model", + "pose", + "static"}; + CheckSupportedElements(diagnostic, model.Element(), supported_model_elements); + + std::vector added_model_instances{model_instance}; // "P" is the parent frame. If the model is in a child of //world or //sdf, // this will be the world frame. Otherwise, this will be the parent model // frame. - const RigidTransformd X_PM = ResolveRigidTransform( - diagnostic, model.SemanticPose()); + const RigidTransformd X_PM = + ResolveRigidTransform(diagnostic, model.SemanticPose()); const RigidTransformd X_WM = X_WP * X_PM; // Add nested models at root-level of . @@ -1746,8 +1704,8 @@ std::vector AddModelsFromSpecification( } drake::log()->trace("sdf_parser: Add links"); std::optional> added_link_infos = - AddLinksFromSpecification(diagnostic, model_instance, model, - X_WM, plant, package_map, root_dir); + AddLinksFromSpecification(diagnostic, model_instance, model, X_WM, plant, + package_map, root_dir); if (!added_link_infos.has_value()) return {}; // Add the SDF "model frame" given the model name so that way any frames added @@ -1763,7 +1721,7 @@ std::vector AddModelsFromSpecification( if (canonical_link != nullptr) { const auto [parent_model_instance, local_name] = GetResolvedModelInstanceAndLocalName(canonical_link_name, - model_instance, *plant); + model_instance, *plant); const Frame& canonical_link_frame = plant->GetFrameByName(local_name, parent_model_instance); const RigidTransformd X_LcM = ResolveRigidTransform( @@ -1786,10 +1744,10 @@ std::vector AddModelsFromSpecification( ++joint_index) { // Get a pointer to the SDF joint, and the joint axis information. const sdf::Joint& joint = *model.JointByIndex(joint_index); - if (!AddJointFromSpecification( - diagnostic, X_WM, joint, model_instance, plant, &joint_types)) { - return {}; - } + if (!AddJointFromSpecification(diagnostic, X_WM, joint, model_instance, + plant, &joint_types)) { + return {}; + } } // Parse drake:mimic elements only after all joints have been added. @@ -1807,8 +1765,8 @@ std::vector AddModelsFromSpecification( for (uint64_t frame_index = 0; frame_index < model.FrameCount(); ++frame_index) { const sdf::Frame& frame = *model.FrameByIndex(frame_index); - AddFrameFromSpecification( - diagnostic, frame, model_instance, model_frame, plant); + AddFrameFromSpecification(diagnostic, frame, model_instance, model_frame, + plant); } drake::log()->trace("sdf_parser: Add drake custom joints"); @@ -1816,10 +1774,10 @@ std::vector AddModelsFromSpecification( for (sdf::ElementPtr joint_node = model.Element()->GetElement("drake:joint"); joint_node; joint_node = joint_node->GetNextElement("drake:joint")) { - if (!AddDrakeJointFromSpecification( - diagnostic, joint_node, model_instance, plant)) { - return {}; - } + if (!AddDrakeJointFromSpecification(diagnostic, joint_node, + model_instance, plant)) { + return {}; + } } } @@ -1829,8 +1787,8 @@ std::vector AddModelsFromSpecification( model.Element()->GetElement("drake:linear_bushing_rpy"); bushing_node; bushing_node = bushing_node->GetNextElement( "drake:linear_bushing_rpy")) { - if (AddBushingFromSpecification( - diagnostic, bushing_node, model_instance, plant) == nullptr) { + if (AddBushingFromSpecification(diagnostic, bushing_node, model_instance, + plant) == nullptr) { return {}; } } @@ -1840,9 +1798,8 @@ std::vector AddModelsFromSpecification( if (model.Element()->HasElement("drake:ball_constraint")) { for (sdf::ElementPtr constraint_node = model.Element()->GetElement("drake:ball_constraint"); - constraint_node; - constraint_node = constraint_node->GetNextElement( - "drake:ball_constraint")) { + constraint_node; constraint_node = constraint_node->GetNextElement( + "drake:ball_constraint")) { AddBallConstraintFromSpecification(diagnostic, constraint_node, model_instance, plant); } @@ -1870,9 +1827,8 @@ std::vector AddModelsFromSpecification( const auto& B = link_info.body->body_frame(); const std::string joint_name = "sdformat_model_static_" + A.name() + "_welds_to_" + B.name(); - plant->AddJoint( - std::make_unique>( - joint_name, A, B, link_info.X_WL)); + plant->AddJoint(std::make_unique>(joint_name, A, B, + link_info.X_WL)); } } } @@ -1880,21 +1836,18 @@ std::vector AddModelsFromSpecification( // Parses the collision filter groups only if the scene graph is registered. if (plant->geometry_source_is_registered()) { drake::log()->trace("sdf_parser: Add collision filter groups"); - ParseCollisionFilterGroup( - diagnostic, model_instance, model, plant, resolver); + ParseCollisionFilterGroup(diagnostic, model_instance, model, plant, + resolver); } return added_model_instances; } // Helper function that computes the default pose of a Frame -RigidTransformd GetDefaultFramePose( - const MultibodyPlant& plant, - const Frame& frame) { - const RigidTransformd X_WB = - plant.GetDefaultFreeBodyPose(frame.body()); - const RigidTransformd X_WF = - X_WB * frame.GetFixedPoseInBodyFrame(); +RigidTransformd GetDefaultFramePose(const MultibodyPlant& plant, + const Frame& frame) { + const RigidTransformd X_WB = plant.GetDefaultFreeBodyPose(frame.body()); + const RigidTransformd X_WF = X_WB * frame.GetFixedPoseInBodyFrame(); return X_WF; } @@ -2026,11 +1979,11 @@ sdf::InterfaceModelPtr ConvertToInterfaceModel( // order. If we add support for other file formats, we should ensure that the // parsers comply with this assumption. sdf::InterfaceModelPtr ParseNestedInterfaceModel( - const ParsingWorkspace& workspace, - const sdf::NestedInclude& include, sdf::Errors* errors) { + const ParsingWorkspace& workspace, const sdf::NestedInclude& include, + sdf::Errors* errors) { const sdf::ParserConfig parser_config = MakeSdfParserConfig(workspace); - auto& [options, package_map, diagnostic, plant, - collision_resolver, parser_selector] = workspace; + auto& [options, package_map, diagnostic, plant, collision_resolver, + parser_selector] = workspace; const std::string resolved_filename{include.ResolvedFileName()}; // Do not attempt to parse anything other than URDF and MuJoCo xml files. @@ -2049,24 +2002,20 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( DataSource data_source(DataSource::kFilename, &resolved_filename); drake::internal::DiagnosticPolicy subdiagnostic; - subdiagnostic.SetActionForWarnings( - [&errors](const DiagnosticDetail& detail) { - errors->emplace_back(MakeSdfError( - sdf::ErrorCode::NONE, detail)); - }); - subdiagnostic.SetActionForErrors( - [&errors](const DiagnosticDetail& detail) { - errors->emplace_back(MakeSdfError( - sdf::ErrorCode::ELEMENT_INVALID, detail)); - }); + subdiagnostic.SetActionForWarnings([&errors](const DiagnosticDetail& detail) { + errors->emplace_back(MakeSdfError(sdf::ErrorCode::NONE, detail)); + }); + subdiagnostic.SetActionForErrors([&errors](const DiagnosticDetail& detail) { + errors->emplace_back(MakeSdfError(sdf::ErrorCode::ELEMENT_INVALID, detail)); + }); ModelInstanceIndex main_model_instance; // New instances will have indices starting from cur_num_models const bool is_merge_include = include.IsMerge().value_or(false); InterfaceModelHelper interface_model_helper(*plant); - ParsingWorkspace subworkspace{options, package_map, subdiagnostic, plant, - collision_resolver, parser_selector}; + ParsingWorkspace subworkspace{options, package_map, subdiagnostic, + plant, collision_resolver, parser_selector}; std::string model_frame_name = "__model__"; std::string model_name; @@ -2074,7 +2023,7 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( // Create the parent model instance if it hasn't been created already. // This can happen if this is the first model to be merge-included. const auto parent_model_instance = - GetOrCreateModelInstanceByName(plant, include.AbsoluteParentName()); + GetOrCreateModelInstanceByName(plant, include.AbsoluteParentName()); interface_model_helper.TakeSnapShot(parent_model_instance); auto& parser = parser_selector(diagnostic, resolved_filename); @@ -2089,9 +2038,9 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( main_model_instance = parent_model_instance; } else { const std::optional maybe_model = - parser_selector(diagnostic, resolved_filename). - AddModel(data_source, include.LocalModelName().value_or(""), - include.AbsoluteParentName(), subworkspace); + parser_selector(diagnostic, resolved_filename) + .AddModel(data_source, include.LocalModelName().value_or(""), + include.AbsoluteParentName(), subworkspace); if (maybe_model.has_value()) { main_model_instance = *maybe_model; } else { @@ -2122,10 +2071,9 @@ sdf::InterfaceModelPtr ParseNestedInterfaceModel( const Frame& canonical_link_frame = plant->GetFrameByName(canonical_link.name(), main_model_instance); - plant->AddFrame( - std::make_unique>( - model_frame_name, canonical_link_frame, RigidTransformd::Identity(), - main_model_instance)); + plant->AddFrame(std::make_unique>( + model_frame_name, canonical_link_frame, RigidTransformd::Identity(), + main_model_instance)); // Now that the model is parsed, we create interface elements to send to // libsdformat. @@ -2153,25 +2101,23 @@ sdf::ParserConfig MakeSdfParserConfig(const ParsingWorkspace& workspace) { parser_config.SetWarningsPolicy(sdf::EnforcementPolicy::ERR); parser_config.SetDeprecatedElementsPolicy(sdf::EnforcementPolicy::ERR); parser_config.SetUnrecognizedElementsPolicy(sdf::EnforcementPolicy::ERR); - parser_config.SetFindCallback( - [&workspace](const std::string &_input) { - // This callback uses an empty return value to denote errors, and then its - // caller reports its own "no such file" error directly. We'll route - // Drake's specific messages about *why* the file wasn't found into a - // debug-only log. - DiagnosticPolicy debug_log; - debug_log.SetActionForWarnings([](const DiagnosticDetail& detail) { - drake::log()->debug(detail.FormatWarning()); - }); - debug_log.SetActionForErrors([](const DiagnosticDetail& detail) { - drake::log()->debug(detail.FormatError()); - }); - return ResolveUri(debug_log, _input, workspace.package_map, "."); + parser_config.SetFindCallback([&workspace](const std::string& _input) { + // This callback uses an empty return value to denote errors, and then its + // caller reports its own "no such file" error directly. We'll route + // Drake's specific messages about *why* the file wasn't found into a + // debug-only log. + DiagnosticPolicy debug_log; + debug_log.SetActionForWarnings([](const DiagnosticDetail& detail) { + drake::log()->debug(detail.FormatWarning()); }); + debug_log.SetActionForErrors([](const DiagnosticDetail& detail) { + drake::log()->debug(detail.FormatError()); + }); + return ResolveUri(debug_log, _input, workspace.package_map, "."); + }); parser_config.RegisterCustomModelParser( - [&workspace]( - const sdf::NestedInclude& include, sdf::Errors& errors) { + [&workspace](const sdf::NestedInclude& include, sdf::Errors& errors) { return ParseNestedInterfaceModel(workspace, include, &errors); }); @@ -2210,9 +2156,8 @@ std::optional AddModelFromSdf( SDFormatDiagnostic diagnostic(&workspace.diagnostic, &data_source); const auto model_index_begin = - static_cast(workspace.plant->num_model_instances()); - sdf::Errors errors = LoadSdf( - diagnostic, &root, data_source, parser_config); + static_cast(workspace.plant->num_model_instances()); + sdf::Errors errors = LoadSdf(diagnostic, &root, data_source, parser_config); if (diagnostic.PropagateErrors(errors)) { return std::nullopt; } @@ -2259,9 +2204,8 @@ std::vector AddModelsFromSdf( SDFormatDiagnostic diagnostic(&workspace.diagnostic, &data_source); const auto model_index_begin = - static_cast(workspace.plant->num_model_instances()); - sdf::Errors errors = LoadSdf( - diagnostic, &root, data_source, parser_config); + static_cast(workspace.plant->num_model_instances()); + sdf::Errors errors = LoadSdf(diagnostic, &root, data_source, parser_config); if (diagnostic.PropagateErrors(errors)) { return {}; } @@ -2282,7 +2226,8 @@ std::vector AddModelsFromSdf( if ((model_count + world_count) != 1) { std::string message = fmt::format( "File must have exactly one or exactly one , but" - " instead has {} models and {} worlds", model_count, world_count); + " instead has {} models and {} worlds", + model_count, world_count); diagnostic.Error(root.Element(), std::move(message)); return {}; } @@ -2307,8 +2252,7 @@ std::vector AddModelsFromSdf( workspace.collision_resolver, workspace.package_map, data_source.GetRootDir(), reusable_model_instance_range, parser_config); - model_instances.insert(model_instances.end(), - added_model_instances.begin(), + model_instances.insert(model_instances.end(), added_model_instances.begin(), added_model_instances.end()); } else { DRAKE_DEMAND(model_count == 0); @@ -2316,19 +2260,16 @@ std::vector AddModelsFromSdf( DRAKE_DEMAND(root.WorldByIndex(0) != nullptr); sdf::World& world = *root.WorldByIndex(0); - const std::set supported_world_elements{ - "frame", - "include", - "joint", - "model"}; - CheckSupportedElements( - diagnostic, world.Element(), supported_world_elements); + const std::set supported_world_elements{"frame", "include", + "joint", "model"}; + CheckSupportedElements(diagnostic, world.Element(), + supported_world_elements); // TODO(eric.cousineau): Either support or explicitly prevent adding joints // via `//world/joint`, per this Bitbucket comment: https://bit.ly/2udQxhp for (uint64_t model_index = 0; model_index < world.ModelCount(); - ++model_index) { + ++model_index) { // Get the model. sdf::Model* model_ptr = world.ModelByIndex(model_index); DRAKE_DEMAND(model_ptr != nullptr); @@ -2348,23 +2289,23 @@ std::vector AddModelsFromSdf( } for (uint64_t frame_index = 0; frame_index < world.FrameCount(); - ++frame_index) { + ++frame_index) { const sdf::Frame& frame = *world.FrameByIndex(frame_index); - AddFrameFromSpecification( - diagnostic, frame, world_model_instance(), - workspace.plant->world_frame(), workspace.plant); + AddFrameFromSpecification(diagnostic, frame, world_model_instance(), + workspace.plant->world_frame(), + workspace.plant); } // Add all the joints std::set joint_types; for (uint64_t joint_index = 0; joint_index < world.JointCount(); - ++joint_index) { + ++joint_index) { const sdf::Joint& joint = *world.JointByIndex(joint_index); - if (!AddJointFromSpecification( - diagnostic, {}, joint, world_model_instance(), - workspace.plant, &joint_types, false)) { - return {}; - } + if (!AddJointFromSpecification(diagnostic, {}, joint, + world_model_instance(), workspace.plant, + &joint_types, false)) { + return {}; + } } // Parse drake:mimic elements only after all joints have been added. diff --git a/multibody/parsing/detail_urdf_parser.cc b/multibody/parsing/detail_urdf_parser.cc index c74d9f4417e8..4955fd7b72a1 100644 --- a/multibody/parsing/detail_urdf_parser.cc +++ b/multibody/parsing/detail_urdf_parser.cc @@ -44,9 +44,9 @@ using Eigen::Vector3d; using Eigen::Vector4d; using math::RigidTransformd; using math::RotationMatrixd; -using tinyxml2::XMLNode; using tinyxml2::XMLDocument; using tinyxml2::XMLElement; +using tinyxml2::XMLNode; namespace { @@ -58,14 +58,11 @@ class UrdfParser { // Note that @p data_source, @p xml_doc, and @p w are aliased for the // lifetime of this object. Their lifetimes must exceed that of the created // object. - UrdfParser( - const DataSource* data_source, - const std::string& model_name, - const std::optional& parent_model_name, - std::optional merge_into_model_instance, - const std::string& root_dir, - XMLDocument* xml_doc, - const ParsingWorkspace& w) + UrdfParser(const DataSource* data_source, const std::string& model_name, + const std::optional& parent_model_name, + std::optional merge_into_model_instance, + const std::string& root_dir, XMLDocument* xml_doc, + const ParsingWorkspace& w) : model_name_(model_name), parent_model_name_(parent_model_name), merge_into_model_instance_(merge_into_model_instance), @@ -95,10 +92,8 @@ class UrdfParser { void ParseJointDynamics(XMLElement* node, double* damping); void ParseJointLimits(XMLElement* node, double* lower, double* upper, double* velocity, double* acceleration, double* effort); - void ParseJointKeyParams(XMLElement* node, - std::string* name, - std::string* type, - std::string* parent_link_name, + void ParseJointKeyParams(XMLElement* node, std::string* name, + std::string* type, std::string* parent_link_name, std::string* child_link_name); void ParseScrewJointThreadPitch(XMLElement* node, double* screw_thread_pitch); void ParseCollisionFilterGroup(XMLElement* node); @@ -108,16 +103,13 @@ class UrdfParser { // A work-alike for internal::ParseScalarAttribute() that uses the local // diagnostic policy. - bool ParseScalarAttribute( - const tinyxml2::XMLElement* node, - const char* attribute_name, double* val) { - return internal::ParseScalarAttribute( - node, attribute_name, val, - diagnostic_.MakePolicyForNode(node)); + bool ParseScalarAttribute(const tinyxml2::XMLElement* node, + const char* attribute_name, double* val) { + return internal::ParseScalarAttribute(node, attribute_name, val, + diagnostic_.MakePolicyForNode(node)); } void ParseMechanicalReduction(const XMLElement& node); - void Warning(const XMLNode& location, std::string message) const { diagnostic_.Warning(location, std::move(message)); } @@ -178,8 +170,8 @@ SpatialInertia UrdfParser::ExtractSpatialInertiaAboutBoExpressedInB( ParseScalarAttribute(inertia, "iyz", &inputs.iyz); ParseScalarAttribute(inertia, "izz", &inputs.izz); } - return ParseSpatialInertia(diagnostic_.MakePolicyForNode(node), - X_BBi, body_mass, inputs); + return ParseSpatialInertia(diagnostic_.MakePolicyForNode(node), X_BBi, + body_mass, inputs); } void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) { @@ -208,7 +200,8 @@ void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) { // implementation? body_pointer = &w_.plant->world_body(); if (node->FirstChildElement("inertial") != nullptr) { - Warning(*node, "A URDF file declared the \"world\" link and then" + Warning(*node, + "A URDF file declared the \"world\" link and then" " attempted to assign mass properties (via the tag)." " Only geometries, and , can be assigned to" " the world link. The tag is being ignored."); @@ -229,12 +222,13 @@ void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) { std::unordered_set geometry_names; for (XMLElement* visual_node = node->FirstChildElement("visual"); - visual_node; - visual_node = visual_node->NextSiblingElement("visual")) { + visual_node; visual_node = visual_node->NextSiblingElement("visual")) { std::optional geometry_instance = ParseVisual(diagnostic_, body_name, w_.package_map, root_dir_, visual_node, materials, &geometry_names); - if (!geometry_instance) { continue; } + if (!geometry_instance) { + continue; + } // The parsing should *always* produce an IllustrationProperties // instance, even if it is empty. DRAKE_DEMAND(geometry_instance->illustration_properties() != nullptr); @@ -251,7 +245,9 @@ void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) { std::optional geometry_instance = ParseCollision(diagnostic_, body_name, w_.package_map, root_dir_, collision_node, &geometry_names); - if (!geometry_instance) { continue; } + if (!geometry_instance) { + continue; + } DRAKE_DEMAND(geometry_instance->proximity_properties() != nullptr); w_.plant->RegisterCollisionGeometry( body, geometry_instance->pose(), geometry_instance->shape(), @@ -264,8 +260,7 @@ void UrdfParser::ParseBody(XMLElement* node, MaterialMap* materials) { void UrdfParser::ParseCollisionFilterGroup(XMLElement* node) { auto next_child_element = [](const ElementNode& data_element, const char* element_name) { - return std::get(data_element) - ->FirstChildElement(element_name); + return std::get(data_element)->FirstChildElement(element_name); }; auto next_sibling_element = [](const ElementNode& data_element, const char* element_name) { @@ -284,8 +279,8 @@ void UrdfParser::ParseCollisionFilterGroup(XMLElement* node) { XMLElement* anode = std::get(data_element); if (!ParseStringAttribute(anode, attribute_name, &attribute_value)) { Error(*anode, fmt::format("The tag <{}> does not specify the required" - " attribute \"{}\".", anode->Value(), - attribute_name)); + " attribute \"{}\".", + anode->Value(), attribute_name)); // Fall through to return empty string. } return attribute_value; @@ -293,8 +288,8 @@ void UrdfParser::ParseCollisionFilterGroup(XMLElement* node) { auto get_bool_attribute = [](const ElementNode& data_element, const char* attribute_name) { std::string attribute_value; - ParseStringAttribute(std::get(data_element), - attribute_name, &attribute_value); + ParseStringAttribute(std::get(data_element), attribute_name, + &attribute_value); return attribute_value == std::string("true") ? true : false; }; ParseCollisionFilterGroupCommon( @@ -317,8 +312,7 @@ void UrdfParser::ParseCollisionFilterGroup(XMLElement* node) { // parent link should be saved. // @param[out] child_link_name A reference to a string where the name of the // child link should be saved. -void UrdfParser::ParseJointKeyParams(XMLElement* node, - std::string* name, +void UrdfParser::ParseJointKeyParams(XMLElement* node, std::string* name, std::string* type, std::string* parent_link_name, std::string* child_link_name) { @@ -340,7 +334,8 @@ void UrdfParser::ParseJointKeyParams(XMLElement* node, } if (!ParseStringAttribute(parent_node, "link", parent_link_name)) { Error(*parent_node, fmt::format("joint {}'s parent does not have a link" - " attribute!", *name)); + " attribute!", + *name)); return; } @@ -352,14 +347,15 @@ void UrdfParser::ParseJointKeyParams(XMLElement* node, } if (!ParseStringAttribute(child_node, "link", child_link_name)) { Error(*child_node, fmt::format("joint {}'s child does not have a link" - " attribute!", *name)); + " attribute!", + *name)); return; } } -void UrdfParser::ParseJointLimits( - XMLElement* node, double* lower, double* upper, - double* velocity, double* acceleration, double* effort) { +void UrdfParser::ParseJointLimits(XMLElement* node, double* lower, + double* upper, double* velocity, + double* acceleration, double* effort) { *lower = -std::numeric_limits::infinity(); *upper = std::numeric_limits::infinity(); *velocity = std::numeric_limits::infinity(); @@ -386,13 +382,15 @@ void UrdfParser::ParseJointDynamics(XMLElement* node, double* damping) { ParseScalarAttribute(dynamics_node, "damping", damping); if (ParseScalarAttribute(dynamics_node, "friction", &coulomb_friction) && coulomb_friction != 0.0) { - Warning(*dynamics_node, "A joint has specified a non-zero value for the" + Warning(*dynamics_node, + "A joint has specified a non-zero value for the" " 'friction' attribute of a joint/dynamics tag. MultibodyPlant" " does not currently support non-zero joint friction."); } - if (ParseScalarAttribute( - dynamics_node, "coulomb_window", &coulomb_window)) { - Warning(*dynamics_node, "A joint has specified a value for the" + if (ParseScalarAttribute(dynamics_node, "coulomb_window", + &coulomb_window)) { + Warning(*dynamics_node, + "A joint has specified a value for the" " 'coulomb_window' attribute of a tag. Drake no longer" " makes use of that attribute; all instances will be ignored."); } @@ -408,21 +406,21 @@ void UrdfParser::ParseScrewJointThreadPitch(XMLElement* node, if (screw_thread_pitch_node) { if (!ParseScalarAttribute(screw_thread_pitch_node, "value", screw_thread_pitch)) { - Error(*screw_thread_pitch_node, "A screw joint has a" + Error(*screw_thread_pitch_node, + "A screw joint has a" " tag that is missing the 'value'" " attribute."); return; } } else { - Error(*node, "A screw joint is missing the " - " tag."); - return; + Error(*node, + "A screw joint is missing the tag."); + return; } } const RigidBody* UrdfParser::GetBodyForElement( - const std::string& element_name, - const std::string& link_name) { + const std::string& element_name, const std::string& link_name) { auto plant = w_.plant; if (link_name == kWorldName) { return &plant->world_body(); @@ -430,16 +428,15 @@ const RigidBody* UrdfParser::GetBodyForElement( if (!plant->HasBodyNamed(link_name, model_instance_)) { Error(*xml_doc_, fmt::format("Could not find link named '{}' with model" - " instance ID {} for element '{}'.", link_name, - model_instance_, element_name)); + " instance ID {} for element '{}'.", + link_name, model_instance_, element_name)); return nullptr; } return &plant->GetBodyByName(link_name, model_instance_); } -void UrdfParser::ParseJoint( - JointEffortLimits* joint_effort_limits, - XMLElement* node) { +void UrdfParser::ParseJoint(JointEffortLimits* joint_effort_limits, + XMLElement* node) { // TODO(rpoyner-tri): legacy undocumented tag: remove, fix? std::string drake_ignore; if (ParseStringAttribute(node, "drake_ignore", &drake_ignore) && @@ -452,17 +449,19 @@ void UrdfParser::ParseJoint( // Parses the parent and child link names. std::string name, type, parent_name, child_name; ParseJointKeyParams(node, &name, &type, &parent_name, &child_name); - if (name.empty() || type.empty() || - parent_name.empty() || child_name.empty()) { + if (name.empty() || type.empty() || parent_name.empty() || + child_name.empty()) { return; } - const RigidBody* parent_body = GetBodyForElement( - name, parent_name); - if (parent_body == nullptr) { return; } - const RigidBody* child_body = GetBodyForElement( - name, child_name); - if (child_body == nullptr) { return; } + const RigidBody* parent_body = GetBodyForElement(name, parent_name); + if (parent_body == nullptr) { + return; + } + const RigidBody* child_body = GetBodyForElement(name, child_name); + if (child_body == nullptr) { + return; + } // The transform from parent to child when the joint is in its zero state. // See the Joint class documentation. @@ -478,8 +477,8 @@ void UrdfParser::ParseJoint( type.compare("floating") != 0 && type.compare("ball") != 0) { ParseVectorAttribute(axis_node, "xyz", &axis); if (axis.norm() < 1e-8) { - Error(*axis_node, fmt::format("Joint '{}' axis is zero. Don't do that.", - name)); + Error(*axis_node, + fmt::format("Joint '{}' axis is zero. Don't do that.", name)); return; } axis.normalize(); @@ -502,17 +501,19 @@ void UrdfParser::ParseJoint( // later if/when an actuator is created. double effort = std::numeric_limits::infinity(); - auto throw_on_custom_joint = - [node, name, type, this](bool want_custom_joint) { + auto throw_on_custom_joint = [node, name, type, + this](bool want_custom_joint) { const std::string node_name(node->Name()); const bool is_custom_joint = node_name == "drake:joint"; if (want_custom_joint && !is_custom_joint) { Error(*node, fmt::format("Joint {} of type {} is a custom joint type, and" - " should be a ", name, type)); + " should be a ", + name, type)); return; } else if (!want_custom_joint && is_custom_joint) { Error(*node, fmt::format("Joint {} of type {} is a standard joint type," - " and should be a ", name, type)); + " and should be a ", + name, type)); return; } }; @@ -526,21 +527,24 @@ void UrdfParser::ParseJoint( // Frame M is Frame B. Frame F and Frame M are coincident at the zero state // of the joint. See Joint class documentation. const RigidTransformd& X_PF = X_PB; - index = plant->AddJoint( - name, *parent_body, X_PF, - *child_body, std::nullopt, axis, lower, upper, damping).index(); + index = + plant + ->AddJoint(name, *parent_body, X_PF, *child_body, + std::nullopt, axis, lower, upper, damping) + .index(); Joint& joint = plant->get_mutable_joint(*index); joint.set_velocity_limits(Vector1d(-velocity), Vector1d(velocity)); - joint.set_acceleration_limits( - Vector1d(-acceleration), Vector1d(acceleration)); + joint.set_acceleration_limits(Vector1d(-acceleration), + Vector1d(acceleration)); } else if (type.compare("fixed") == 0) { throw_on_custom_joint(false); // Frame M is Frame B. Frame F and Frame M are coincident at the zero state // of the joint. See Joint class documentation. const RigidTransformd& X_PF = X_PB; - index = plant->AddJoint( - name, *parent_body, X_PF, *child_body, std::nullopt, - RigidTransformd::Identity()).index(); + index = plant + ->AddJoint(name, *parent_body, X_PF, *child_body, + std::nullopt, RigidTransformd::Identity()) + .index(); } else if (type.compare("prismatic") == 0) { throw_on_custom_joint(false); ParseJointLimits(node, &lower, &upper, &velocity, &acceleration, &effort); @@ -548,26 +552,31 @@ void UrdfParser::ParseJoint( // Frame M is Frame B. Frame F and Frame M are coincident at the zero state // of the joint. See Joint class documentation. const RigidTransformd& X_PF = X_PB; - index = plant->AddJoint( - name, *parent_body, X_PF, *child_body, std::nullopt, axis, lower, - upper, damping).index(); + index = plant + ->AddJoint(name, *parent_body, X_PF, + *child_body, std::nullopt, axis, + lower, upper, damping) + .index(); Joint& joint = plant->get_mutable_joint(*index); joint.set_velocity_limits(Vector1d(-velocity), Vector1d(velocity)); - joint.set_acceleration_limits( - Vector1d(-acceleration), Vector1d(acceleration)); + joint.set_acceleration_limits(Vector1d(-acceleration), + Vector1d(acceleration)); } else if (type.compare("floating") == 0) { throw_on_custom_joint(false); Warning(*node, fmt::format("Joint '{}' specified as type floating which is" " not supported by MultibodyPlant. Leaving '{}'" - " as a free body.", name, child_name)); + " as a free body.", + name, child_name)); } else if (type.compare("ball") == 0) { throw_on_custom_joint(true); ParseJointDynamics(node, &damping); // Frame M is Frame B. Frame F and Frame M are coincident at the zero state // of the joint. See Joint class documentation. const RigidTransformd& X_PF = X_PB; - index = plant->AddJoint( - name, *parent_body, X_PF, *child_body, std::nullopt, damping).index(); + index = plant + ->AddJoint(name, *parent_body, X_PF, *child_body, + std::nullopt, damping) + .index(); } else if (type.compare("planar") == 0) { // Permit both the standard 'joint' and custom 'drake:joint' spellings // here. The standard spelling was actually always correct, but Drake only @@ -595,8 +604,10 @@ void UrdfParser::ParseJoint( const RigidTransformd X_BM = RigidTransformd(R_BM, Vector3d::Zero()); const RigidTransformd X_PM = X_PB * X_BM; const RigidTransformd& X_PF = X_PM; - index = plant->AddJoint( - name, *parent_body, X_PF, *child_body, X_BM, damping_vec).index(); + index = plant + ->AddJoint(name, *parent_body, X_PF, *child_body, + X_BM, damping_vec) + .index(); } else if (type.compare("screw") == 0) { throw_on_custom_joint(true); ParseJointDynamics(node, &damping); @@ -605,9 +616,11 @@ void UrdfParser::ParseJoint( // Frame M is Frame B. Frame F and Frame M are coincident at the zero state // of the joint. See Joint class documentation. const RigidTransformd& X_PF = X_PB; - index = plant->AddJoint( - name, *parent_body, X_PF, *child_body, std::nullopt, axis, - screw_thread_pitch, damping).index(); + index = plant + ->AddJoint(name, *parent_body, X_PF, *child_body, + std::nullopt, axis, screw_thread_pitch, + damping) + .index(); } else if (type.compare("universal") == 0) { throw_on_custom_joint(true); ParseJointDynamics(node, &damping); @@ -615,11 +628,13 @@ void UrdfParser::ParseJoint( // Frame M is Frame B. Frame F and Frame M are coincident at the zero state // of the joint. See Joint class documentation. const RigidTransformd& X_PF = X_PB; - index = plant->AddJoint( - name, *parent_body, X_PF, *child_body, std::nullopt, damping).index(); + index = plant + ->AddJoint(name, *parent_body, X_PF, + *child_body, std::nullopt, damping) + .index(); } else { - Error(*node, fmt::format("Joint '{}' has unrecognized type: '{}'", - name, type)); + Error(*node, + fmt::format("Joint '{}' has unrecognized type: '{}'", name, type)); return; } @@ -716,21 +731,27 @@ void UrdfParser::ParseMimicTag(XMLElement* node) { void UrdfParser::ParseMechanicalReduction(const XMLElement& node) { const XMLElement* child = node.FirstChildElement("mechanicalReduction"); - if (!child) { return; } + if (!child) { + return; + } const char* text = child->GetText(); - if (!text) { return; } + if (!text) { + return; + } std::vector values = ConvertToVector(text); - if (values.size() == 1 && values[0] == 1) { return; } - Warning(*child, fmt::format( - "A '{}' element contains a mechanicalReduction element with a" - " value '{}' other than the default of 1. MultibodyPlant does" - " not currently support non-default mechanical reductions.", - node.Name(), text)); + if (values.size() == 1 && values[0] == 1) { + return; + } + Warning( + *child, + fmt::format("A '{}' element contains a mechanicalReduction element with a" + " value '{}' other than the default of 1. MultibodyPlant does" + " not currently support non-default mechanical reductions.", + node.Name(), text)); } -void UrdfParser::ParseTransmission( - const JointEffortLimits& joint_effort_limits, - XMLElement* node) { +void UrdfParser::ParseTransmission(const JointEffortLimits& joint_effort_limits, + XMLElement* node) { WarnUnsupportedElement(*node, "leftActuator"); WarnUnsupportedElement(*node, "rightActuator"); WarnUnsupportedElement(*node, "flexJoint"); @@ -757,9 +778,10 @@ void UrdfParser::ParseTransmission( // print a warning and then abort this method call since only simple // transmissions are supported at this time. if (type.find("SimpleTransmission") == std::string::npos) { - Warning(*node, "A has a type that isn't" - " 'SimpleTransmission'. Drake only supports 'SimpleTransmission';" - " all other transmission types will be ignored."); + Warning(*node, + "A has a type that isn't 'SimpleTransmission'. " + "Drake only supports 'SimpleTransmission'; all other transmission " + "types will be ignored."); return; } @@ -786,7 +808,6 @@ void UrdfParser::ParseTransmission( } // `joint/hardwareInterface` child tags are silently ignored. - std::string joint_name; if (!ParseStringAttribute(joint_node, "name", &joint_name)) { Error(*joint_node, "Transmission is missing a joint name."); @@ -796,17 +817,19 @@ void UrdfParser::ParseTransmission( auto plant = w_.plant; if (!plant->HasJointNamed(joint_name, model_instance_)) { Error(*joint_node, fmt::format("Transmission specifies joint '{}' which" - " does not exist.", joint_name)); + " does not exist.", + joint_name)); return; } - const Joint& joint = plant->GetJointByName( - joint_name, model_instance_); + const Joint& joint = + plant->GetJointByName(joint_name, model_instance_); // Checks if the actuator is attached to a fixed joint. If so, abort this // method call. if (joint.num_positions() == 0) { Warning(*joint_node, fmt::format("Skipping transmission since it's attached" - " to a fixed joint \"{}\".", joint_name)); + " to a fixed joint \"{}\".", + joint_name)); return; } @@ -814,15 +837,16 @@ void UrdfParser::ParseTransmission( DRAKE_DEMAND(effort_iter != joint_effort_limits.end()); if (effort_iter->second < 0) { Error(*joint_node, fmt::format("Transmission specifies joint '{}' which has" - " a negative effort limit.", joint_name)); + " a negative effort limit.", + joint_name)); return; } if (effort_iter->second <= 0) { Warning(*joint_node, fmt::format("Skipping transmission since it's attached" " to joint \"{}\" which has a zero effort" - " limit {}.", joint_name, - effort_iter->second)); + " limit {}.", + joint_name, effort_iter->second)); return; } @@ -837,7 +861,8 @@ void UrdfParser::ParseTransmission( if (!ParseScalarAttribute(rotor_inertia_node, "value", &rotor_inertia)) { Error(*rotor_inertia_node, fmt::format("joint actuator {}'s drake:rotor_inertia does not have" - " a \"value\" attribute!", actuator_name)); + " a \"value\" attribute!", + actuator_name)); return; } plant->get_mutable_joint_actuator(actuator.index()) @@ -852,7 +877,8 @@ void UrdfParser::ParseTransmission( if (!ParseScalarAttribute(gear_ratio_node, "value", &gear_ratio)) { Error(*gear_ratio_node, fmt::format("joint actuator {}'s drake:gear_ratio does not have a" - " \"value\" attribute!", actuator_name)); + " \"value\" attribute!", + actuator_name)); return; } plant->get_mutable_joint_actuator(actuator.index()) @@ -899,9 +925,10 @@ void UrdfParser::ParseFrame(XMLElement* node) { return; } - const RigidBody* body = GetBodyForElement( - name, body_name); - if (body == nullptr) { return; } + const RigidBody* body = GetBodyForElement(name, body_name); + if (body == nullptr) { + return; + } RigidTransformd X_BF = OriginAttributesToTransform(node); w_.plant->AddFrame(std::make_unique>( @@ -920,7 +947,8 @@ void UrdfParser::ParseBushing(XMLElement* node) { return value; } else { Error(*node, fmt::format("Unable to read the 'value' attribute for the" - " <{}> tag", element_name)); + " <{}> tag", + element_name)); return Eigen::Vector3d::Zero(); } } else { @@ -933,8 +961,8 @@ void UrdfParser::ParseBushing(XMLElement* node) { // Returns nullptr if unable to find the tag, if the name attribute is // improperly formed, or if it does not refer to a frame already in the // model. - auto read_frame = [node, this](const char* element_name) - -> const Frame* { + auto read_frame = [node, + this](const char* element_name) -> const Frame* { XMLElement* value_node = node->FirstChildElement(element_name); if (value_node != nullptr) { @@ -943,14 +971,15 @@ void UrdfParser::ParseBushing(XMLElement* node) { if (ParseStringAttribute(value_node, "name", &frame_name)) { if (!plant->HasFrameNamed(frame_name, model_instance_)) { Error(*value_node, fmt::format("Frame: {} specified for <{}> does not" - " exist in the model.", frame_name, - element_name)); + " exist in the model.", + frame_name, element_name)); return {}; } return &plant->GetFrameByName(frame_name, model_instance_); } else { Error(*value_node, fmt::format("Unable to read the 'name' attribute for" - " the <{}> tag", element_name)); + " the <{}> tag", + element_name)); return {}; } @@ -1026,14 +1055,16 @@ std::pair, std::string> UrdfParser::Parse() { Error(*xml_doc_, "URDF does not contain a robot tag."); return {}; } - // See https://github.com/ros/urdfdom/blob/dbecca0/urdf_parser/src/model.cpp#L124-L131 + // See + // https://github.com/ros/urdfdom/blob/dbecca0/urdf_parser/src/model.cpp#L124-L131 WarnUnsupportedAttribute(*node, "version"); // child tags are silently ignored. std::string model_name = model_name_; if (model_name.empty() && !ParseStringAttribute(node, "name", &model_name)) { - Error(*node, "Your robot must have a name attribute or a model name " - "must be specified."); + Error(*node, + "Your robot must have a name attribute or a model name must be " + "specified."); return {}; } @@ -1056,8 +1087,7 @@ std::pair, std::string> UrdfParser::Parse() { } // Parses the model's link elements. - for (XMLElement* link_node = node->FirstChildElement("link"); - link_node; + for (XMLElement* link_node = node->FirstChildElement("link"); link_node; link_node = link_node->NextSiblingElement("link")) { ParseBody(link_node, &materials); } @@ -1164,17 +1194,17 @@ AddOrMergeModelFromUrdf( UrdfParser parser(&data_source, model_name_in, parent_model_name, merge_into_model_instance, data_source.GetRootDir(), &xml_doc, workspace); - return parser.Parse();; + return parser.Parse(); } } // namespace std::optional AddModelFromUrdf( - const DataSource& data_source, - const std::string& model_name_in, + const DataSource& data_source, const std::string& model_name_in, const std::optional& parent_model_name, const ParsingWorkspace& workspace) { return AddOrMergeModelFromUrdf(data_source, model_name_in, parent_model_name, - workspace, std::nullopt).first; + workspace, std::nullopt) + .first; } UrdfParserWrapper::UrdfParserWrapper() {} @@ -1194,7 +1224,8 @@ std::string UrdfParserWrapper::MergeModel( ModelInstanceIndex merge_into_model_instance, const ParsingWorkspace& workspace) { return AddOrMergeModelFromUrdf(data_source, model_name, std::nullopt, - workspace, merge_into_model_instance).second; + workspace, merge_into_model_instance) + .second; } std::vector UrdfParserWrapper::AddAllModels( diff --git a/multibody/parsing/test/detail_sdf_parser_test.cc b/multibody/parsing/test/detail_sdf_parser_test.cc index 387a7f9eedd1..6707aa73b104 100644 --- a/multibody/parsing/test/detail_sdf_parser_test.cc +++ b/multibody/parsing/test/detail_sdf_parser_test.cc @@ -45,10 +45,10 @@ namespace { using ::testing::MatchesRegex; -using Eigen::Vector2d; -using Eigen::Vector3d; using drake::internal::DiagnosticDetail; using drake::internal::DiagnosticPolicy; +using Eigen::Vector2d; +using Eigen::Vector3d; using geometry::GeometryId; using geometry::GeometryInstance; using geometry::SceneGraph; @@ -68,15 +68,11 @@ const double kEps = std::numeric_limits::epsilon(); // uses the SAP solver. More specifically, we call // set_discrete_contact_approximation(DiscreteContactApproximation::kSap) on the // MultibodyPlant used for testing before parsing. -class SdfParserTest : public test::DiagnosticPolicyTestBase{ +class SdfParserTest : public test::DiagnosticPolicyTestBase { public: - SdfParserTest() { - RecordErrors(); - } + SdfParserTest() { RecordErrors(); } - void AddSceneGraph() { - plant_.RegisterAsSourceForSceneGraph(&scene_graph_); - } + void AddSceneGraph() { plant_.RegisterAsSourceForSceneGraph(&scene_graph_); } static ParserInterface& TestingSelect(const DiagnosticPolicy&, const std::string& filename) { @@ -93,15 +89,13 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ "Unsupported file format in unittest for file ({})", filename)); } - ModelInstanceIndex AddModelFromSdfFile( - const std::string& file_name, - const std::string& model_name, + const std::string& file_name, const std::string& model_name, const std::optional& parent_model_name = {}) { const DataSource data_source{DataSource::kFilename, &file_name}; internal::CollisionFilterGroupResolver resolver{&plant_}; ParsingWorkspace w{options_, package_map_, diagnostic_policy_, - &plant_, &resolver, TestingSelect}; + &plant_, &resolver, TestingSelect}; std::optional result = AddModelFromSdf(data_source, model_name, parent_model_name, w); EXPECT_TRUE(result.has_value()); @@ -116,7 +110,7 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ const DataSource data_source{DataSource::kFilename, &file_name}; internal::CollisionFilterGroupResolver resolver{&plant_}; ParsingWorkspace w{options_, package_map_, diagnostic_policy_, - &plant_, &resolver, TestingSelect}; + &plant_, &resolver, TestingSelect}; auto result = AddModelsFromSdf(data_source, parent_model_name, w); last_parsed_groups_ = ConvertInstancedNamesToStrings( resolver.Resolve(diagnostic_policy_), plant_); @@ -129,7 +123,7 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ const DataSource data_source{DataSource::kContents, &file_contents}; internal::CollisionFilterGroupResolver resolver{&plant_}; ParsingWorkspace w{options_, package_map_, diagnostic_policy_, - &plant_, &resolver, TestingSelect}; + &plant_, &resolver, TestingSelect}; auto result = AddModelsFromSdf(data_source, parent_model_name, w); last_parsed_groups_ = ConvertInstancedNamesToStrings( resolver.Resolve(diagnostic_policy_), plant_); @@ -142,15 +136,14 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ const std::optional& parent_model_name = {}) { SCOPED_TRACE(inner); FlushDiagnostics(); - const std::string file_contents = - "" - + inner + "\n\n"; + const std::string file_contents = "" + + inner + "\n\n"; AddModelsFromSdfString(file_contents, parent_model_name); } - void VerifyCollisionFilters( - const std::vector& ids, - const std::set& expected_filters) { + void VerifyCollisionFilters(const std::vector& ids, + const std::set& expected_filters) { const int num_links = ids.size(); const auto& inspector = scene_graph_.model_inspector(); for (int m = 0; m < num_links; ++m) { @@ -159,12 +152,10 @@ class SdfParserTest : public test::DiagnosticPolicyTestBase{ const std::string& n_name = inspector.GetName(ids[n]); SCOPED_TRACE(fmt::format("{}[{}] vs {}[{}]", m_name, m, n_name, n)); CollisionPair names{m_name, n_name}; - auto contains = - [&expected_filters](const CollisionPair& key) { - return expected_filters.contains(key); - }; - EXPECT_EQ(inspector.CollisionFiltered(ids[m], ids[n]), - contains(names)); + auto contains = [&expected_filters](const CollisionPair& key) { + return expected_filters.contains(key); + }; + EXPECT_EQ(inspector.CollisionFiltered(ids[m], ids[n]), contains(names)); } } } @@ -230,8 +221,7 @@ TEST_F(SdfParserTest, ModelInstanceTest) { "drake/multibody/parsing/test/" "links_with_visuals_and_collisions.sdf"); - ModelInstanceIndex instance1 = - AddModelFromSdfFile(full_name, "instance1"); + ModelInstanceIndex instance1 = AddModelFromSdfFile(full_name, "instance1"); // Check that a duplicate model names are not allowed. DRAKE_EXPECT_THROWS_MESSAGE( @@ -240,23 +230,20 @@ TEST_F(SdfParserTest, ModelInstanceTest) { "Model instance names must be unique within a given model."); // Load two acrobots to check per-model-instance items. - const std::string acrobot_sdf_name = FindResourceOrThrow( - "drake/multibody/benchmarks/acrobot/acrobot.sdf"); - ModelInstanceIndex acrobot1 = - AddModelFromSdfFile(acrobot_sdf_name, ""); + const std::string acrobot_sdf_name = + FindResourceOrThrow("drake/multibody/benchmarks/acrobot/acrobot.sdf"); + ModelInstanceIndex acrobot1 = AddModelFromSdfFile(acrobot_sdf_name, ""); // Loading the model again without specifying a different model name should // throw. - EXPECT_THROW(AddModelFromSdfFile(acrobot_sdf_name, ""), - std::logic_error); + EXPECT_THROW(AddModelFromSdfFile(acrobot_sdf_name, ""), std::logic_error); // Avoid name collisions with model renaming. ModelInstanceIndex acrobot2 = AddModelFromSdfFile(acrobot_sdf_name, "acrobot2"); // Avoid name collisions with parent model names; both entry points. - ModelInstanceIndex acrobot3 = - AddModelFromSdfFile(acrobot_sdf_name, "", "3"); + ModelInstanceIndex acrobot3 = AddModelFromSdfFile(acrobot_sdf_name, "", "3"); ModelInstanceIndex acrobot3rename = AddModelFromSdfFile(acrobot_sdf_name, "new_model_name", "3"); ModelInstanceIndex acrobot4 = @@ -280,9 +267,8 @@ TEST_F(SdfParserTest, ModelInstanceTest) { // Links which appear in multiple model instances throw if the instance // isn't specified. - DRAKE_EXPECT_THROWS_MESSAGE( - plant_.HasBodyNamed("Link1"), - ".*Body.*Link1.*multiple model instances.*"); + DRAKE_EXPECT_THROWS_MESSAGE(plant_.HasBodyNamed("Link1"), + ".*Body.*Link1.*multiple model instances.*"); EXPECT_FALSE(plant_.HasBodyNamed("Link1", instance1)); EXPECT_TRUE(plant_.HasBodyNamed("Link1", acrobot1)); @@ -296,10 +282,8 @@ TEST_F(SdfParserTest, ModelInstanceTest) { EXPECT_EQ(acrobot1_link1.model_instance(), acrobot1); EXPECT_EQ(acrobot2_link1.model_instance(), acrobot2); - DRAKE_EXPECT_THROWS_MESSAGE( - plant_.GetBodyByName("Link1"), - ".*Body.*Link1.*multiple model instances.*"); - + DRAKE_EXPECT_THROWS_MESSAGE(plant_.GetBodyByName("Link1"), + ".*Body.*Link1.*multiple model instances.*"); DRAKE_EXPECT_THROWS_MESSAGE( plant_.HasJointNamed("ShoulderJoint"), @@ -340,34 +324,32 @@ TEST_F(SdfParserTest, ModelInstanceTest) { plant_.GetFrameByName("Link1", acrobot2); EXPECT_NE(acrobot1_link1_frame.index(), acrobot2_link1_frame.index()); - DRAKE_EXPECT_THROWS_MESSAGE( - plant_.GetFrameByName("Link1"), - ".*Frame.*Link1.*multiple model instances.*"); + DRAKE_EXPECT_THROWS_MESSAGE(plant_.GetFrameByName("Link1"), + ".*Frame.*Link1.*multiple model instances.*"); // Check model scope frames. auto context = plant_.CreateDefaultContext(); auto check_frame = [this, instance1, &context]( - std::string parent_name, std::string name, - const RigidTransformd& X_PF_expected) { + std::string parent_name, std::string name, + const RigidTransformd& X_PF_expected) { const Frame& frame = plant_.GetFrameByName(name, instance1); const Frame& parent_frame = plant_.GetFrameByName(parent_name, instance1); - const RigidTransformd X_PF = plant_.CalcRelativeTransform( - *context, parent_frame, frame); - EXPECT_TRUE(CompareMatrices( - X_PF_expected.GetAsMatrix4(), X_PF.GetAsMatrix4(), kEps)) + const RigidTransformd X_PF = + plant_.CalcRelativeTransform(*context, parent_frame, frame); + EXPECT_TRUE(CompareMatrices(X_PF_expected.GetAsMatrix4(), + X_PF.GetAsMatrix4(), kEps)) << name; }; - const RigidTransformd X_L1F1( - RollPitchYawd(0.4, 0.5, 0.6), Vector3d(0.1, 0.2, 0.3)); + const RigidTransformd X_L1F1(RollPitchYawd(0.4, 0.5, 0.6), + Vector3d(0.1, 0.2, 0.3)); check_frame("link1", "model_scope_link1_frame", X_L1F1); const RigidTransformd X_F1F2(Vector3d(0.1, 0.0, 0.0)); - check_frame( - "model_scope_link1_frame", "model_scope_link1_frame_child", X_F1F2); + check_frame("model_scope_link1_frame", "model_scope_link1_frame_child", + X_F1F2); const RigidTransformd X_MF3(Vector3d(0.7, 0.8, 0.9)); - check_frame( - "__model__", "model_scope_model_frame_implicit", X_MF3); + check_frame("__model__", "model_scope_model_frame_implicit", X_MF3); } TEST_F(SdfParserTest, ParentModelNameWithString) { @@ -556,22 +538,22 @@ TEST_F(SdfParserTest, FloatingBodyPose) { plant_.Finalize(); EXPECT_GT(plant_.num_positions(), 0); auto context = plant_.CreateDefaultContext(); - const RigidTransformd X_WA_expected( - RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3)); + const RigidTransformd X_WA_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 2, 3)); const RigidTransformd X_WA = plant_.GetFrameByName("a").CalcPoseInWorld(*context); - 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)); + 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 = plant_.GetFrameByName("b").CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); } TEST_F(SdfParserTest, StaticModelSupported1) { -// Test that static models are partially supported. + // Test that static models are partially supported. ParseTestString(R"""( true @@ -585,18 +567,18 @@ TEST_F(SdfParserTest, StaticModelSupported1) { plant_.Finalize(); EXPECT_EQ(plant_.num_positions(), 0); auto context = plant_.CreateDefaultContext(); - const RigidTransformd X_WA_expected( - RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3)); + const RigidTransformd X_WA_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 2, 3)); const RigidTransformd X_WA = plant_.GetFrameByName("a").CalcPoseInWorld(*context); - 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)); + 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 = plant_.GetFrameByName("b").CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); } TEST_F(SdfParserTest, StaticModelSupported2) { @@ -606,16 +588,17 @@ TEST_F(SdfParserTest, StaticModelSupported2) { 1 2 3 0.1 0.2 0.3 true - )""", "1.8"); + )""", + "1.8"); plant_.Finalize(); auto context = plant_.CreateDefaultContext(); - const RigidTransformd X_WA_expected( - RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3)); + const RigidTransformd X_WA_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 2, 3)); const auto& frame_A = GetModelFrameByName(plant_, "a"); const RigidTransformd X_WA = frame_A.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps)); EXPECT_EQ(frame_A.body().index(), plant_.world_body().index()); } @@ -629,25 +612,26 @@ TEST_F(SdfParserTest, StaticModelSupported3) { 0 0 0 0.1 0.2 0.0 true - )""", "1.8"); + )""", + "1.8"); plant_.Finalize(); auto context = plant_.CreateDefaultContext(); - const RigidTransformd X_WA_expected( - RollPitchYawd(0.0, 0.0, 0.3), Vector3d(1, 2, 3)); + const RigidTransformd X_WA_expected(RollPitchYawd(0.0, 0.0, 0.3), + Vector3d(1, 2, 3)); const auto& frame_A = GetModelFrameByName(plant_, "a"); const RigidTransformd X_WA = frame_A.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WA_expected.GetAsMatrix4(), X_WA.GetAsMatrix4(), kEps)); EXPECT_EQ(frame_A.body().index(), plant_.world_body().index()); - const RigidTransformd X_WB_expected( - RollPitchYawd(0.1, 0.2, 0.3), Vector3d(1, 2, 3)); + const RigidTransformd X_WB_expected(RollPitchYawd(0.1, 0.2, 0.3), + Vector3d(1, 2, 3)); - const auto &frame_B = GetModelFrameByName(plant_, "a::b"); + const auto& frame_B = GetModelFrameByName(plant_, "a::b"); const RigidTransformd X_WB = frame_B.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); EXPECT_EQ(frame_B.body().index(), plant_.world_body().index()); } @@ -667,12 +651,13 @@ TEST_F(SdfParserTest, StaticFrameOnlyModelsSupported) { 0 0 0 0 0 0.3 - )""", "1.8"); + )""", + "1.8"); plant_.Finalize(); auto context = plant_.CreateDefaultContext(); auto test_frame = [&](const std::string& frame_name, - const RigidTransformd& X_WF_expected) { + const RigidTransformd& X_WF_expected) { const auto& frame = plant_.GetFrameByName(frame_name); const RigidTransformd X_WF = frame.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices(X_WF_expected.GetAsMatrix4(), @@ -719,9 +704,7 @@ TEST_F(SdfParserTest, StaticModelWithJoints) { plant_.Finalize(); }; // The message contains the elaborate joint name inserted by the parser. - DRAKE_EXPECT_THROWS_MESSAGE( - weld_and_finalize(), - ".*sdformat_model_static.*"); + DRAKE_EXPECT_THROWS_MESSAGE(weld_and_finalize(), ".*sdformat_model_static.*"); // Drake does not support "frozen" joints (#12227). ParseTestString(R"""( @@ -737,8 +720,9 @@ TEST_F(SdfParserTest, StaticModelWithJoints) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*Only fixed joints are permitted in static models.")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*Only fixed joints are permitted in static models.")); } // Revolute joints should have an axis and 1-dof joints should have no axis2. @@ -759,10 +743,12 @@ TEST_F(SdfParserTest, JointWithNoAxisError) { // not set to throw. The first error comes from ExtractJointAxis // that has no breaking behavior while the second one comes from // ParseJointLimits which mimics the throw behavior. - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*An axis must be specified for joint 'no_axis'.*")); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*An axis must be specified for joint 'no_axis'.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*An axis must be specified for joint 'no_axis'.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*An axis must be specified for joint 'no_axis'.*")); } // Ball joints should not have an axis2. @@ -782,16 +768,20 @@ TEST_F(SdfParserTest, BallJointWithAxis2Error) { )"""); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*A ball joint axis will be ignored. Only the dynamic" - " parameters and limits will be considered.*")); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - R"(.*Actuation \(via non-zero effort limits\) for ball joint )" - R"('should_not_have_axis' is not implemented yet and will be )" - R"(ignored.*)")); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*An axis2 may not be specified for ball joint 'should_not_have_axis' " - "and will be ignored.*")); + EXPECT_THAT(TakeWarning(), + ::testing::MatchesRegex( + ".*A ball joint axis will be ignored. Only the dynamic" + " parameters and limits will be considered.*")); + EXPECT_THAT( + TakeWarning(), + ::testing::MatchesRegex( + R"(.*Actuation \(via non-zero effort limits\) for ball joint )" + R"('should_not_have_axis' is not implemented yet and will be )" + R"(ignored.*)")); + EXPECT_THAT(TakeWarning(), + ::testing::MatchesRegex(".*An axis2 may not be specified for " + "ball joint 'should_not_have_axis' " + "and will be ignored.*")); } // Joint axis upper limit should be lower than upper limit. @@ -812,9 +802,10 @@ TEST_F(SdfParserTest, JointAxisLimitsError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*The lower limit must be lower \(or equal\) than the)" - R"( upper limit for joint 'no_axis'.*)")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + R"(.*The lower limit must be lower \(or equal\) than the)" + R"( upper limit for joint 'no_axis'.*)")); } // Joint axis drake:acceleration should be non negative. @@ -834,9 +825,10 @@ TEST_F(SdfParserTest, JointAxisDrakeAccelerationError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*Acceleration limit is negative for joint 'no_axis'." - " Aceleration limit must be a non-negative number.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*Acceleration limit is negative for joint 'no_axis'." + " Aceleration limit must be a non-negative number.*")); } // Check that prismatic joints must have an axis. @@ -854,10 +846,12 @@ TEST_F(SdfParserTest, PrismaticJointWithNoAxisError) { // not set to throw. The first error comes from ExtractJointAxis // that has no breaking behavior while the second one comes from // ParseJointLimits which mimics the throw behavior. - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*An axis must be specified for joint 'no_axis'.*")); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*An axis must be specified for joint 'no_axis'.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*An axis must be specified for joint 'no_axis'.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*An axis must be specified for joint 'no_axis'.*")); } // Make sure world joints are fixed. @@ -879,11 +873,11 @@ TEST_F(SdfParserTest, WorldJointNotFixedError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*Only fixed joints are permitted in world joints.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*Only fixed joints are permitted in world joints.*")); } - // drake:joint should have a type. TEST_F(SdfParserTest, DrakeJointNoTypeError) { ParseTestString(R"""( @@ -893,8 +887,9 @@ TEST_F(SdfParserTest, DrakeJointNoTypeError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*: Unable to find the 'type' attribute.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*: Unable to find the 'type' attribute.*")); } // drake:joint should have a name. @@ -906,8 +901,9 @@ TEST_F(SdfParserTest, DrakeJointNoNameError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*: Unable to find the 'name' attribute.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*: Unable to find the 'name' attribute.*")); } // drake:joint does not support pose tags. @@ -920,8 +916,10 @@ TEST_F(SdfParserTest, DrakeJointPoseError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".* does not yet support the child tag.*")); + EXPECT_THAT( + TakeError(), + ::testing::MatchesRegex( + ".* does not yet support the child tag.*")); } // Verify that drake:joint yields an error for unrecognized types. @@ -936,9 +934,10 @@ TEST_F(SdfParserTest, DrakeJointUnrecognizedTypeError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".* 'joint_name' has unrecognized value for" - " 'type' attribute: nonetype.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".* 'joint_name' has unrecognized value for" + " 'type' attribute: nonetype.*")); } // drake:joint/drake:{parent,child} can refer to nested models. @@ -973,10 +972,12 @@ TEST_F(SdfParserTest, DrakeJointNestedParentBad) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*: Model instance name 'good::nesQQQted' .*implied by" - " frame name 'nesQQQted::a' in within model instance" - " 'good'.* does not exist.*")); + EXPECT_THAT( + TakeError(), + ::testing::MatchesRegex( + ".*: Model instance name 'good::nesQQQted' .*implied by" + " frame name 'nesQQQted::a' in within model instance" + " 'good'.* does not exist.*")); } // drake:joint/drake:child yields an error on bad nested model name. @@ -994,10 +995,12 @@ TEST_F(SdfParserTest, DrakeJointNestedChildBad) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*: Model instance name 'good::nesQQQted' .*implied by" - " frame name 'nesQQQted::b' in within model instance" - " 'good'.* does not exist.*")); + EXPECT_THAT( + TakeError(), + ::testing::MatchesRegex( + ".*: Model instance name 'good::nesQQQted' .*implied by" + " frame name 'nesQQQted::b' in within model instance" + " 'good'.* does not exist.*")); } TEST_F(SdfParserTest, MimicSuccessfulParsing) { @@ -1022,7 +1025,7 @@ TEST_F(SdfParserTest, MimicSuccessfulParsing) { 0 0 1 - )""");; + )"""); // Revolute joint with mimic DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("joint_AC")); DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("joint_AB")); @@ -1062,7 +1065,7 @@ TEST_F(SdfParserTest, MimicSuccessfulParsingForwardReference) { 0 0 1 - )""");; + )"""); // Revolute joint with mimic DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("joint_AB")); DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("joint_AC")); @@ -1289,12 +1292,12 @@ TEST_F(SdfParserTest, AddModelFromSdfNoModelError) { const DataSource data_source{DataSource::kContents, &sdf_string}; internal::CollisionFilterGroupResolver resolver{&plant_}; ParsingWorkspace w{options_, package_map_, diagnostic_policy_, - &plant_, &resolver, TestingSelect}; + &plant_, &resolver, TestingSelect}; std::optional result = AddModelFromSdf(data_source, "", "", w); resolver.Resolve(diagnostic_policy_); EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*File must have a single element.*")); + ".*File must have a single element.*")); EXPECT_FALSE(result.has_value()); } @@ -1306,9 +1309,10 @@ TEST_F(SdfParserTest, MoreThanOneWorldOrModelError) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*File must have exactly one or exactly one ," - " but instead has 0 models and 2 worlds.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*File must have exactly one or exactly one ," + " but instead has 0 models and 2 worlds.*")); } // Verify that our SDF parser throws an exception when a user specifies a joint @@ -1318,8 +1322,7 @@ TEST_F(SdfParserTest, ThrowsWhenJointDampingIsNegative) { "drake/multibody/parsing/test/sdf_parser_test/" "negative_damping_joint.sdf"); AddModelFromSdfFile(sdf_file_path, ""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*damping is negative.*")); + EXPECT_THAT(TakeError(), ::testing::MatchesRegex(".*damping is negative.*")); } TEST_F(SdfParserTest, IncludeTags) { @@ -1327,7 +1330,6 @@ TEST_F(SdfParserTest, IncludeTags) { "drake/multibody/parsing/test/sdf_parser_test/" "include_models.sdf"); - // We start with the world and default model instances. ASSERT_EQ(plant_.num_model_instances(), 2); ASSERT_EQ(plant_.num_bodies(), 1); @@ -1425,18 +1427,18 @@ TEST_F(SdfParserTest, JointParsingTest) { EXPECT_EQ(revolute_joint.child_body().name(), "link2"); EXPECT_EQ(revolute_joint.revolute_axis(), Vector3d::UnitZ()); EXPECT_EQ(revolute_joint.default_damping(), 0.2); - EXPECT_TRUE(CompareMatrices( - revolute_joint.position_lower_limits(), Vector1d(-1))); - EXPECT_TRUE(CompareMatrices( - revolute_joint.position_upper_limits(), Vector1d(2))); - EXPECT_TRUE(CompareMatrices( - revolute_joint.velocity_lower_limits(), Vector1d(-100))); - EXPECT_TRUE(CompareMatrices( - revolute_joint.velocity_upper_limits(), Vector1d(100))); - EXPECT_TRUE(CompareMatrices( - revolute_joint.acceleration_lower_limits(), Vector1d(-200))); - EXPECT_TRUE(CompareMatrices( - revolute_joint.acceleration_upper_limits(), Vector1d(200))); + EXPECT_TRUE( + CompareMatrices(revolute_joint.position_lower_limits(), Vector1d(-1))); + EXPECT_TRUE( + CompareMatrices(revolute_joint.position_upper_limits(), Vector1d(2))); + EXPECT_TRUE( + CompareMatrices(revolute_joint.velocity_lower_limits(), Vector1d(-100))); + EXPECT_TRUE( + CompareMatrices(revolute_joint.velocity_upper_limits(), Vector1d(100))); + EXPECT_TRUE(CompareMatrices(revolute_joint.acceleration_lower_limits(), + Vector1d(-200))); + EXPECT_TRUE(CompareMatrices(revolute_joint.acceleration_upper_limits(), + Vector1d(200))); // Prismatic joint DRAKE_EXPECT_NO_THROW( @@ -1448,18 +1450,18 @@ TEST_F(SdfParserTest, JointParsingTest) { EXPECT_EQ(prismatic_joint.child_body().name(), "link3"); EXPECT_EQ(prismatic_joint.translation_axis(), Vector3d::UnitZ()); EXPECT_EQ(prismatic_joint.default_damping(), 0.3); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.position_lower_limits(), Vector1d(-2))); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.position_upper_limits(), Vector1d(1))); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.velocity_lower_limits(), Vector1d(-5))); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.velocity_upper_limits(), Vector1d(5))); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.acceleration_lower_limits(), Vector1d(-10))); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.acceleration_upper_limits(), Vector1d(10))); + EXPECT_TRUE( + CompareMatrices(prismatic_joint.position_lower_limits(), Vector1d(-2))); + EXPECT_TRUE( + CompareMatrices(prismatic_joint.position_upper_limits(), Vector1d(1))); + EXPECT_TRUE( + CompareMatrices(prismatic_joint.velocity_lower_limits(), Vector1d(-5))); + EXPECT_TRUE( + CompareMatrices(prismatic_joint.velocity_upper_limits(), Vector1d(5))); + EXPECT_TRUE(CompareMatrices(prismatic_joint.acceleration_lower_limits(), + Vector1d(-10))); + EXPECT_TRUE(CompareMatrices(prismatic_joint.acceleration_upper_limits(), + Vector1d(10))); // Limitless revolute joint DRAKE_EXPECT_NO_THROW(plant_.GetJointByName( @@ -1482,8 +1484,8 @@ TEST_F(SdfParserTest, JointParsingTest) { EXPECT_TRUE(CompareMatrices(no_limit_joint.acceleration_upper_limits(), inf)); // Ball joint - DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("ball_joint", - instance1)); + DRAKE_EXPECT_NO_THROW( + plant_.GetJointByName("ball_joint", instance1)); const BallRpyJoint& ball_joint = plant_.GetJointByName("ball_joint", instance1); EXPECT_EQ(ball_joint.name(), "ball_joint"); @@ -1502,9 +1504,10 @@ TEST_F(SdfParserTest, JointParsingTest) { EXPECT_TRUE(CompareMatrices(ball_joint.velocity_upper_limits(), inf3)); // Ball joints with axis produce a waring indicating it only some params // of it are used. - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*A ball joint axis will be ignored. Only the dynamic" - " parameters and limits will be considered.*")); + EXPECT_THAT(TakeWarning(), + ::testing::MatchesRegex( + ".*A ball joint axis will be ignored. Only the dynamic" + " parameters and limits will be considered.*")); FlushDiagnostics(); // Universal joint @@ -1520,11 +1523,11 @@ TEST_F(SdfParserTest, JointParsingTest) { std::numeric_limits::infinity()); const Vector2d neg_inf2(-std::numeric_limits::infinity(), -std::numeric_limits::infinity()); - EXPECT_TRUE(CompareMatrices(universal_joint.position_lower_limits(), - neg_inf2)); + EXPECT_TRUE( + CompareMatrices(universal_joint.position_lower_limits(), neg_inf2)); EXPECT_TRUE(CompareMatrices(universal_joint.position_upper_limits(), inf2)); - EXPECT_TRUE(CompareMatrices(universal_joint.velocity_lower_limits(), - neg_inf2)); + EXPECT_TRUE( + CompareMatrices(universal_joint.velocity_lower_limits(), neg_inf2)); EXPECT_TRUE(CompareMatrices(universal_joint.velocity_upper_limits(), inf2)); // axis = (0, 0, 1) and axis2 = (0, 1, 0) in the model frame (aka the world // frame in this case). So Ix, Iy, Iz are (0, 0, 1), (0, 1, 0), and (-1, 0, 0) @@ -1553,8 +1556,8 @@ TEST_F(SdfParserTest, JointParsingTest) { .IsExactlyEqualTo(X_WI)); // Planar joint - DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("planar_joint", - instance1)); + DRAKE_EXPECT_NO_THROW( + plant_.GetJointByName("planar_joint", instance1)); const PlanarJoint& planar_joint = plant_.GetJointByName("planar_joint", instance1); EXPECT_EQ(planar_joint.name(), "planar_joint"); @@ -1570,8 +1573,8 @@ TEST_F(SdfParserTest, JointParsingTest) { EXPECT_TRUE(CompareMatrices(planar_joint.velocity_upper_limits(), inf3)); const ModelInstanceIndex instance2 = instances.back(); - DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("planar_joint", - instance2)); + DRAKE_EXPECT_NO_THROW( + plant_.GetJointByName("planar_joint", instance2)); const PlanarJoint& planar_joint2 = plant_.GetJointByName("planar_joint", instance2); EXPECT_EQ(planar_joint2.name(), "planar_joint"); @@ -1617,16 +1620,13 @@ TEST_F(SdfParserTest, JointParsingTest) { EXPECT_EQ(screw_joint.screw_axis(), Vector3d::UnitX()); EXPECT_EQ(screw_joint.screw_pitch(), 0.04); EXPECT_EQ(screw_joint.default_damping(), 0.1); - EXPECT_TRUE( - CompareMatrices(screw_joint.position_lower_limits(), neg_inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.position_lower_limits(), neg_inf)); EXPECT_TRUE(CompareMatrices(screw_joint.position_upper_limits(), inf)); - EXPECT_TRUE( - CompareMatrices(screw_joint.velocity_lower_limits(), neg_inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.velocity_lower_limits(), neg_inf)); EXPECT_TRUE(CompareMatrices(screw_joint.velocity_upper_limits(), inf)); EXPECT_TRUE( CompareMatrices(screw_joint.acceleration_lower_limits(), neg_inf)); - EXPECT_TRUE( - CompareMatrices(screw_joint.acceleration_upper_limits(), inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.acceleration_upper_limits(), inf)); } // Tests the error handling for an unsupported joint type (when actuated). @@ -1651,8 +1651,9 @@ TEST_F(SdfParserTest, ActuatedUniversalJointParsingTest) { )"""); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*effort limits.*universal joint.*not implemented.*")); + EXPECT_THAT(TakeWarning(), + ::testing::MatchesRegex( + ".*effort limits.*universal joint.*not implemented.*")); } // Tests the error handling when axis2 isn't specified for universal joints. @@ -1668,10 +1669,12 @@ TEST_F(SdfParserTest, UniversalJointAxisParsingTest) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*Both axis and axis2 must be specified.*jerry.*")); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*effort limits.*universal joint.*not implemented.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*Both axis and axis2 must be specified.*jerry.*")); + EXPECT_THAT(TakeWarning(), + ::testing::MatchesRegex( + ".*effort limits.*universal joint.*not implemented.*")); } // Tests the error handling for an non-orthogonal axis and axis2 in universal @@ -1692,7 +1695,7 @@ TEST_F(SdfParserTest, UniversalJointNonOrthogonalAxisParsingTest) { )"""); EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*axis and axis2 must be orthogonal.*jerry.*")); + ".*axis and axis2 must be orthogonal.*jerry.*")); } // Tests the error handling for axis and axis2 with incompatible damping @@ -1748,11 +1751,13 @@ TEST_F(SdfParserTest, ActuatedBallJointParsingTest) { )"""); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*A ball joint axis will be ignored. Only the dynamic" - " parameters and limits will be considered.*")); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*effort limits.*ball joint.*not implemented.*")); + EXPECT_THAT(TakeWarning(), + ::testing::MatchesRegex( + ".*A ball joint axis will be ignored. Only the dynamic" + " parameters and limits will be considered.*")); + EXPECT_THAT(TakeWarning(), + ::testing::MatchesRegex( + ".*effort limits.*ball joint.*not implemented.*")); } // Tests the error handling for an unsupported joint type. @@ -1765,8 +1770,8 @@ TEST_F(SdfParserTest, GearboxJointParsingTest) { larry )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*gearbox.*not supported.*jerry.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex(".*gearbox.*not supported.*jerry.*")); } // Tests the error handling for an unsupported joint type. @@ -1779,8 +1784,8 @@ TEST_F(SdfParserTest, Revolute2JointParsingTest) { larry )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*revolute2.*not supported.*jerry.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex(".*revolute2.*not supported.*jerry.*")); } // Tests the error handling for a misspelled joint type. @@ -1793,8 +1798,8 @@ TEST_F(SdfParserTest, MisspelledJointParsingTest) { larry )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*revoluteqqq is invalid.*")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex(".*revoluteqqq is invalid.*")); } // Verifies that the SDF parser parses the joint actuator limit correctly. @@ -1876,8 +1881,10 @@ TEST_F(SdfParserTest, NegativeStiffnessPrismaticSpringParsingTest) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*The stiffness specified for joint '.*' must be non-negative.")); + EXPECT_THAT( + TakeError(), + ::testing::MatchesRegex( + ".*The stiffness specified for joint '.*' must be non-negative.")); } // Verifies that the SDF parser parses the revolute spring parameters correctly. @@ -1907,8 +1914,8 @@ TEST_F(SdfParserTest, RevoluteSpringParsingTest) { constexpr int kGeneralizedForcesSize = 10; Matrix2X expected_generalized_forces(kNumSpringForces, kGeneralizedForcesSize); - expected_generalized_forces << 0, 0, 0, 0, 0, 0, 5, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + expected_generalized_forces << 0, 0, 0, 0, 0, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0; for (int i = 0; i < kNumSpringForces; ++i) { // The ForceElement at index zero is gravity, so we skip that index. const ForceElementIndex force_index(i + 1); @@ -1993,10 +2000,12 @@ TEST_F(SdfParserTest, TestUnsupportedFrames) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*(attached_to|relative_to) name\[world\] specified by frame )" - R"(with name\[.*\] does not match a nested model, link, joint, or )" - R"(frame name in model with name\[bad\].*)")); + EXPECT_THAT( + TakeError(), + ::testing::MatchesRegex( + R"(.*(attached_to|relative_to) name\[world\] specified by frame )" + R"(with name\[.*\] does not match a nested model, link, joint, or )" + R"(frame name in model with name\[bad\].*)")); // Ignore additional errors for this test. EXPECT_EQ(NumErrors(), 5); ClearDiagnostics(); @@ -2009,10 +2018,12 @@ TEST_F(SdfParserTest, TestUnsupportedFrames) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*(attached_to|relative_to) name\[world\] specified by frame )" - R"(with name\[.*\] does not match a nested model, link, joint, or )" - R"(frame name in model with name\[bad\].*)")); + EXPECT_THAT( + TakeError(), + ::testing::MatchesRegex( + R"(.*(attached_to|relative_to) name\[world\] specified by frame )" + R"(with name\[.*\] does not match a nested model, link, joint, or )" + R"(frame name in model with name\[bad\].*)")); // Ignore additional errors for this test. EXPECT_EQ(NumErrors(), 2); ClearDiagnostics(); @@ -2024,9 +2035,11 @@ TEST_F(SdfParserTest, TestUnsupportedFrames) { -)""", bad_name)); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*The supplied frame name \[.*\] is reserved..*)")); +)""", + bad_name)); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + R"(.*The supplied frame name \[.*\] is reserved..*)")); // Ignore additional errors for this test. The number ignored varies. ClearDiagnostics(); } @@ -2036,9 +2049,10 @@ TEST_F(SdfParserTest, TestUnsupportedFrames) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*Attribute //pose\[@relative_to\] of top level model )" - R"(must be left empty.*)")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + R"(.*Attribute //pose\[@relative_to\] of top level model )" + R"(must be left empty.*)")); // Ignore additional errors for this test. EXPECT_EQ(NumErrors(), 2); ClearDiagnostics(); @@ -2050,9 +2064,10 @@ TEST_F(SdfParserTest, TestUnsupportedFrames) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*XML Attribute\[relative_to\] in element\[pose\] not )" - R"(defined in SDF.*)")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + R"(.*XML Attribute\[relative_to\] in element\[pose\] not )" + R"(defined in SDF.*)")); } // Tests Drake's usage of sdf::EnforcementPolicy. @@ -2063,9 +2078,10 @@ TEST_F(SdfParserTest, TestSdformatParserPolicies) { )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*XML Attribute\[bad_attribute\] in element\[model\] not )" - R"(defined in SDF.*)")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + R"(.*XML Attribute\[bad_attribute\] in element\[model\] not )" + R"(defined in SDF.*)")); FlushDiagnostics(); ParseTestString(R"""( @@ -2077,7 +2093,7 @@ TEST_F(SdfParserTest, TestSdformatParserPolicies) { )"""); EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*Root object can only contain one model.*")); + ".*Root object can only contain one model.*")); FlushDiagnostics(); // TODO(#15018): This throws a warning, make this an error. @@ -2088,8 +2104,8 @@ TEST_F(SdfParserTest, TestSdformatParserPolicies) { )"""); EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*XML Element\[bad_element\], child of)" - R"( element\[model\], not defined in SDF.*)")); + R"(.*XML Element\[bad_element\], child of)" + R"( element\[model\], not defined in SDF.*)")); FlushDiagnostics(); ParseTestString(R"""( @@ -2103,10 +2119,12 @@ TEST_F(SdfParserTest, TestSdformatParserPolicies) { 0 -)""", "1.9"); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - R"(.*XML Element\[initial_position\], child of element)" - R"(\[axis\], not defined in SDF.*)")); +)""", + "1.9"); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + R"(.*XML Element\[initial_position\], child of element)" + R"(\[axis\], not defined in SDF.*)")); FlushDiagnostics(); ParseTestString(R"""( @@ -2114,9 +2132,11 @@ TEST_F(SdfParserTest, TestSdformatParserPolicies) { <_drake_deprecation_unit_test_element/> -)""", "1.9"); - EXPECT_THAT(TakeWarning(), testing::MatchesRegex( - ".*drake_deprecation_unit_test_element.*is deprecated.*")); +)""", + "1.9"); + EXPECT_THAT(TakeWarning(), + testing::MatchesRegex( + ".*drake_deprecation_unit_test_element.*is deprecated.*")); FlushDiagnostics(); } @@ -2138,9 +2158,8 @@ ::testing::AssertionResult FrameHasShape(geometry::FrameId frame_id, inspector.GetShape(geometry_id).type_name(); if (shape_type != name) { return ::testing::AssertionFailure() - << "Geometry with role " << role << " has wrong shape type." - << "\n Expected: " << name - << "\n Found: " << shape_type; + << "Geometry with role " << role << " has wrong shape type." + << "\n Expected: " << name << "\n Found: " << shape_type; } } catch (const std::exception& e) { return ::testing::AssertionFailure() @@ -2153,9 +2172,7 @@ ::testing::AssertionResult FrameHasShape(geometry::FrameId frame_id, class GeometrySdfParserTest : public SdfParserTest { public: - GeometrySdfParserTest() { - AddSceneGraph(); - } + GeometrySdfParserTest() { AddSceneGraph(); } protected: // Confirms that all supported geometries in an SDF file are registered. The @@ -2184,12 +2201,12 @@ class GeometrySdfParserTest : public SdfParserTest { geometry::Cylinder{0.1, 0.1})); EXPECT_TRUE(FrameHasShape(frame_id, role, scene_graph_, geometry::Ellipsoid{0.1, 0.1, 0.1})); - EXPECT_TRUE(FrameHasShape(frame_id, role, scene_graph_, - geometry::HalfSpace{})); + EXPECT_TRUE( + FrameHasShape(frame_id, role, scene_graph_, geometry::HalfSpace{})); EXPECT_TRUE(FrameHasShape(frame_id, role, scene_graph_, geometry::Mesh{mesh_uri, 1.0})); - EXPECT_TRUE(FrameHasShape(frame_id, role, scene_graph_, - geometry::Sphere{0.1})); + EXPECT_TRUE( + FrameHasShape(frame_id, role, scene_graph_, geometry::Sphere{0.1})); } }; @@ -2411,9 +2428,10 @@ TEST_F(SdfParserTest, BushingParsingBad1) { 10 11 12 )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*: Unable to find the " - " child tag.")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*: Unable to find the " + " child tag.")); } TEST_F(SdfParserTest, BushingParsingBad2) { @@ -2435,17 +2453,19 @@ TEST_F(SdfParserTest, BushingParsingBad2) { 10 11 12 )"""); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*: Frame 'frameZ' specified for " - " does not exist in " - "the model.")); + EXPECT_THAT(TakeError(), + ::testing::MatchesRegex( + ".*: Frame 'frameZ' specified for " + " does not exist in " + "the model.")); } TEST_F(SdfParserTest, BushingParsingBad3) { AddSceneGraph(); ThrowErrors(); // Test missing constants tag - DRAKE_EXPECT_THROWS_MESSAGE(ParseTestString(R"""( + DRAKE_EXPECT_THROWS_MESSAGE( + ParseTestString(R"""( @@ -2487,13 +2507,12 @@ TEST_F(SdfParserTest, ReflectedInertiaParametersParsing) { // Test successful parsing of both parameters. { - ParseTestString(fmt::format(test_string, - "1.5", - "300.0", - "specify_both")); + ParseTestString(fmt::format( + test_string, "1.5", + "300.0", "specify_both")); - const ModelInstanceIndex model = plant_.GetModelInstanceByName( - "ReflectedInertiaModel_specify_both"); + const ModelInstanceIndex model = + plant_.GetModelInstanceByName("ReflectedInertiaModel_specify_both"); const JointActuator& actuator = plant_.GetJointActuatorByName("revolute_AB", model); @@ -2508,8 +2527,8 @@ TEST_F(SdfParserTest, ReflectedInertiaParametersParsing) { test_string, "1.5", "", "default_gear")); - const ModelInstanceIndex model = plant_.GetModelInstanceByName( - "ReflectedInertiaModel_default_gear"); + const ModelInstanceIndex model = + plant_.GetModelInstanceByName("ReflectedInertiaModel_default_gear"); const JointActuator& actuator = plant_.GetJointActuatorByName("revolute_AB", model); @@ -2520,12 +2539,12 @@ TEST_F(SdfParserTest, ReflectedInertiaParametersParsing) { // Test successful parsing of gear_ratio and default value for // rotor_inertia. { - ParseTestString(fmt::format( - test_string, "", "300.0", - "default_rotor")); + ParseTestString(fmt::format(test_string, "", + "300.0", + "default_rotor")); - const ModelInstanceIndex model = plant_.GetModelInstanceByName( - "ReflectedInertiaModel_default_rotor"); + const ModelInstanceIndex model = + plant_.GetModelInstanceByName("ReflectedInertiaModel_default_rotor"); const JointActuator& actuator = plant_.GetJointActuatorByName("revolute_AB", model); @@ -2570,16 +2589,14 @@ TEST_F(SdfParserTest, ControllerGainsParsing) { } // Test missing 'p' attribute. { - const std::string expected_message = - ".*Unable to find the 'p' attribute.*"; + const std::string expected_message = ".*Unable to find the 'p' attribute.*"; ParseTestString(fmt::format( test_string, "", "missing_p")); EXPECT_THAT(TakeError(), ::testing::MatchesRegex(expected_message)); } // Test missing 'd' attribute. { - const std::string expected_message = - ".*Unable to find the 'd' attribute.*"; + const std::string expected_message = ".*Unable to find the 'd' attribute.*"; ParseTestString(fmt::format( test_string, "", "missing_d")); EXPECT_THAT(TakeError(), ::testing::MatchesRegex(expected_message)); @@ -2674,17 +2691,16 @@ TEST_F(SdfParserTest, LoadDirectlyNestedModelsInModel) { // There should be a model instance with the name // "grand_parent_model::parent_model". This is the model "parent_model" // nested inside "grand_parent_model" - ASSERT_TRUE( - plant_.HasModelInstanceNamed("grand_parent_model::parent_model")); + ASSERT_TRUE(plant_.HasModelInstanceNamed("grand_parent_model::parent_model")); // There should be a model instance with the name // "grand_parent_model::parent_model::robot1". This is the model "robot1" // nested inside "parent_model" which itself is nested inside // grand_parent_model - ASSERT_TRUE(plant_.HasModelInstanceNamed( - "grand_parent_model::parent_model::robot1")); - ModelInstanceIndex robot1_model = plant_.GetModelInstanceByName( - "grand_parent_model::parent_model::robot1"); + ASSERT_TRUE( + plant_.HasModelInstanceNamed("grand_parent_model::parent_model::robot1")); + ModelInstanceIndex robot1_model = + plant_.GetModelInstanceByName("grand_parent_model::parent_model::robot1"); // There should be a body with the name "base_link". EXPECT_TRUE(plant_.HasBodyNamed("base_link", robot1_model)); @@ -2697,10 +2713,10 @@ TEST_F(SdfParserTest, LoadDirectlyNestedModelsInModel) { // "grand_parent_model::parent_model::robot2". This is the model "robot2" // nested inside "parent_model" which itself is nested inside // grand_parent_model - ASSERT_TRUE(plant_.HasModelInstanceNamed( - "grand_parent_model::parent_model::robot2")); - ModelInstanceIndex robot2_model = plant_.GetModelInstanceByName( - "grand_parent_model::parent_model::robot2"); + ASSERT_TRUE( + plant_.HasModelInstanceNamed("grand_parent_model::parent_model::robot2")); + ModelInstanceIndex robot2_model = + plant_.GetModelInstanceByName("grand_parent_model::parent_model::robot2"); // There should be a body with the name "base_link". EXPECT_TRUE(plant_.HasBodyNamed("base_link", robot2_model)); @@ -2770,12 +2786,12 @@ TEST_F(SdfParserTest, ModelPlacementFrame) { const RigidTransformd X_SH = frame_H.CalcPose(*context, frame_S); const RigidTransformd X_SB = frame_B.CalcPose(*context, frame_S); - EXPECT_TRUE(CompareMatrices( - X_SM_expected.GetAsMatrix4(), X_SM.GetAsMatrix4(), kEps)); - EXPECT_TRUE(CompareMatrices( - X_SB_expected.GetAsMatrix4(), X_SB.GetAsMatrix4(), kEps)); - EXPECT_TRUE(CompareMatrices( - X_SH_expected.GetAsMatrix4(), X_SH.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_SM_expected.GetAsMatrix4(), X_SM.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_SB_expected.GetAsMatrix4(), X_SB.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_SH_expected.GetAsMatrix4(), X_SH.GetAsMatrix4(), kEps)); // X_WM = X_WT * X_TM // X_TM = X_TS * X_MS^-1 @@ -2795,12 +2811,12 @@ TEST_F(SdfParserTest, ModelPlacementFrame) { const RigidTransformd X_WM = frame_M.CalcPoseInWorld(*context); const RigidTransformd X_WH = frame_H.CalcPoseInWorld(*context); const RigidTransformd X_WB = frame_B.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WM_expected.GetAsMatrix4(), X_WM.GetAsMatrix4(), kEps)); - EXPECT_TRUE(CompareMatrices( - X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); - EXPECT_TRUE(CompareMatrices( - X_WH_expected.GetAsMatrix4(), X_WH.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WM_expected.GetAsMatrix4(), X_WM.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WB_expected.GetAsMatrix4(), X_WB.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WH_expected.GetAsMatrix4(), X_WH.GetAsMatrix4(), kEps)); } // Verify that poses can be given relative to deeply nested frames. @@ -2860,7 +2876,8 @@ TEST_F(SdfParserTest, AxisXyzExperssedInMultiLevelNestedFrame) { 1 0 0 -)""", M_PI_2, M_PI_2); +)""", + M_PI_2, M_PI_2); ParseTestString(model_string, "1.8"); plant_.Finalize(); EXPECT_GT(plant_.num_positions(), 0); @@ -2912,13 +2929,13 @@ TEST_F(SdfParserTest, FrameAttachedToMultiLevelNestedFrame) { const auto& frame_E = plant_.GetFrameByName("e"); const RigidTransformd X_WE = frame_E.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WE_expected.GetAsMatrix4(), X_WE.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WE_expected.GetAsMatrix4(), X_WE.GetAsMatrix4(), kEps)); const auto& frame_F = plant_.GetFrameByName("f"); const RigidTransformd X_WF = frame_F.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WF_expected.GetAsMatrix4(), X_WF.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WF_expected.GetAsMatrix4(), X_WF.GetAsMatrix4(), kEps)); // Also check that the frame is attached to the right body ModelInstanceIndex model_c_instance = @@ -2980,20 +2997,18 @@ TEST_F(SdfParserTest, FrameAttachedToModelFrameInWorld) { const auto& frame_E = plant_.GetFrameByName("e"); const RigidTransformd X_WE = frame_E.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WE_expected.GetAsMatrix4(), X_WE.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WE_expected.GetAsMatrix4(), X_WE.GetAsMatrix4(), kEps)); const auto& frame_F = plant_.GetFrameByName("f"); const RigidTransformd X_WF = frame_F.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WF_expected.GetAsMatrix4(), X_WF.GetAsMatrix4(), kEps)); + EXPECT_TRUE( + CompareMatrices(X_WF_expected.GetAsMatrix4(), X_WF.GetAsMatrix4(), kEps)); // Also check that the frame is attached to the right body - EXPECT_EQ(frame_E.body().index(), - plant_.GetBodyByName("d").index()); + EXPECT_EQ(frame_E.body().index(), plant_.GetBodyByName("d").index()); - EXPECT_EQ(frame_F.body().index(), - plant_.GetBodyByName("d").index()); + EXPECT_EQ(frame_F.body().index(), plant_.GetBodyByName("d").index()); } // Verify frames can be attached to joint frames @@ -3038,16 +3053,14 @@ TEST_F(SdfParserTest, FrameAttachedToJointFrame) { const auto& frame_F2 = plant_.GetFrameByName("F2"); const RigidTransformd X_WF1 = frame_F1.CalcPoseInWorld(*context); const RigidTransformd X_WF2 = frame_F2.CalcPoseInWorld(*context); - EXPECT_TRUE(CompareMatrices( - X_WF1_expected.GetAsMatrix4(), X_WF1.GetAsMatrix4(), kEps)); - EXPECT_TRUE(CompareMatrices( - X_WF2_expected.GetAsMatrix4(), X_WF2.GetAsMatrix4(), kEps)); + EXPECT_TRUE(CompareMatrices(X_WF1_expected.GetAsMatrix4(), + X_WF1.GetAsMatrix4(), kEps)); + EXPECT_TRUE(CompareMatrices(X_WF2_expected.GetAsMatrix4(), + X_WF2.GetAsMatrix4(), kEps)); // Also check that the frame is attached to the right body - EXPECT_EQ(frame_F1.body().index(), - plant_.GetBodyByName("L2").index()); - EXPECT_EQ(frame_F2.body().index(), - plant_.GetBodyByName("L3").index()); + EXPECT_EQ(frame_F1.body().index(), plant_.GetBodyByName("L2").index()); + EXPECT_EQ(frame_F2.body().index(), plant_.GetBodyByName("L3").index()); } TEST_F(SdfParserTest, SupportNonDefaultCanonicalLink) { @@ -3161,9 +3174,9 @@ TEST_F(SdfParserTest, InterfaceApi) { // Verify filtering among all links. std::set expected_filters = { - {"top::arm::L1", "top::gripper::gripper_link"}, - {"top::arm::L1", "top::torso"}, - {"top::gripper::gripper_link", "top::torso"}, + {"top::arm::L1", "top::gripper::gripper_link"}, + {"top::arm::L1", "top::torso"}, + {"top::gripper::gripper_link", "top::torso"}, }; VerifyCollisionFilters(ids, expected_filters); @@ -3200,12 +3213,10 @@ TEST_F(SdfParserTest, InterfaceApi) { // Frame T represents the frame top::table_and_mug::mug::top const RigidTransformd X_WT_expected(RollPitchYawd(M_PI_2, 0.0, 0.0), Vector3d(3, 0, 0.5)); - const auto mug_model_instance = - plant_.GetModelInstanceByName("top::mug"); + const auto mug_model_instance = plant_.GetModelInstanceByName("top::mug"); const auto& mug_top_frame = plant_.GetFrameByName("top", mug_model_instance); - const RigidTransformd X_WT = - mug_top_frame.CalcPoseInWorld(*context); + const RigidTransformd X_WT = mug_top_frame.CalcPoseInWorld(*context); EXPECT_TRUE(CompareMatrices(X_WT_expected.GetAsMatrix4(), X_WT.GetAsMatrix4(), kEps)); } @@ -3222,9 +3233,9 @@ TEST_F(SdfParserTest, ErrorsFromIncludedUrdf) { package://drake/multibody/parsing/test/sdf_parser_test/bad.urdf arm -)""", "1.8"); - EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*bad.urdf.*XML_ERROR.*")); +)""", + "1.8"); + EXPECT_THAT(TakeError(), ::testing::MatchesRegex(".*bad.urdf.*XML_ERROR.*")); } // TODO(SeanCurtis-TRI) The logic testing for collision filter group parsing @@ -3260,34 +3271,34 @@ TEST_F(SdfParserTest, CollisionFilterGroupParsingTest) { // Verify filtering among all links. std::set expected_filters = { - // Filtered within robot1. - {"test::robot1::link1_sphere", "test::robot1::link3_sphere"}, - {"test::robot1::link1_sphere", "test::robot1::link4_sphere"}, - {"test::robot1::link2_sphere", "test::robot1::link3_sphere"}, - {"test::robot1::link2_sphere", "test::robot1::link5_sphere"}, - {"test::robot1::link2_sphere", "test::robot1::link6_sphere"}, - {"test::robot1::link3_sphere", "test::robot1::link4_sphere"}, - {"test::robot1::link3_sphere", "test::robot1::link5_sphere"}, - {"test::robot1::link3_sphere", "test::robot1::link6_sphere"}, - {"test::robot1::link5_sphere", "test::robot1::link6_sphere"}, - - // Filtered across both robots. - {"test::robot1::link3_sphere", "test::robot2::link3_sphere"}, - {"test::robot1::link6_sphere", "test::robot2::link6_sphere"}, - - // Filtered within robot2. - {"test::robot2::link1_sphere", "test::robot2::link3_sphere"}, - {"test::robot2::link1_sphere", "test::robot2::link4_sphere"}, - {"test::robot2::link2_sphere", "test::robot2::link3_sphere"}, - {"test::robot2::link2_sphere", "test::robot2::link5_sphere"}, - {"test::robot2::link2_sphere", "test::robot2::link6_sphere"}, - {"test::robot2::link3_sphere", "test::robot2::link4_sphere"}, - {"test::robot2::link3_sphere", "test::robot2::link5_sphere"}, - {"test::robot2::link3_sphere", "test::robot2::link6_sphere"}, - {"test::robot2::link5_sphere", "test::robot2::link6_sphere"}, - - // Filtered by group_of_groups. - {"test::robot1::link2_sphere", "test::robot2::link3_sphere"}, + // Filtered within robot1. + {"test::robot1::link1_sphere", "test::robot1::link3_sphere"}, + {"test::robot1::link1_sphere", "test::robot1::link4_sphere"}, + {"test::robot1::link2_sphere", "test::robot1::link3_sphere"}, + {"test::robot1::link2_sphere", "test::robot1::link5_sphere"}, + {"test::robot1::link2_sphere", "test::robot1::link6_sphere"}, + {"test::robot1::link3_sphere", "test::robot1::link4_sphere"}, + {"test::robot1::link3_sphere", "test::robot1::link5_sphere"}, + {"test::robot1::link3_sphere", "test::robot1::link6_sphere"}, + {"test::robot1::link5_sphere", "test::robot1::link6_sphere"}, + + // Filtered across both robots. + {"test::robot1::link3_sphere", "test::robot2::link3_sphere"}, + {"test::robot1::link6_sphere", "test::robot2::link6_sphere"}, + + // Filtered within robot2. + {"test::robot2::link1_sphere", "test::robot2::link3_sphere"}, + {"test::robot2::link1_sphere", "test::robot2::link4_sphere"}, + {"test::robot2::link2_sphere", "test::robot2::link3_sphere"}, + {"test::robot2::link2_sphere", "test::robot2::link5_sphere"}, + {"test::robot2::link2_sphere", "test::robot2::link6_sphere"}, + {"test::robot2::link3_sphere", "test::robot2::link4_sphere"}, + {"test::robot2::link3_sphere", "test::robot2::link5_sphere"}, + {"test::robot2::link3_sphere", "test::robot2::link6_sphere"}, + {"test::robot2::link5_sphere", "test::robot2::link6_sphere"}, + + // Filtered by group_of_groups. + {"test::robot1::link2_sphere", "test::robot2::link3_sphere"}, }; VerifyCollisionFilters(ids, expected_filters); @@ -3361,13 +3372,12 @@ TEST_F(SdfParserTest, CollisionFilterGroupParsingErrorsTest) { )""")); EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*The tag is " - "missing the required attribute \"name\".*")); + ".*The tag is " + "missing the required attribute \"name\".*")); FlushDiagnostics(); // Testing several errors set to keep record instead of throwing - DRAKE_EXPECT_NO_THROW( - ParseTestString(R"""( + DRAKE_EXPECT_NO_THROW(ParseTestString(R"""( @@ -3382,8 +3392,7 @@ TEST_F(SdfParserTest, CollisionFilterGroupParsingErrorsTest) { EXPECT_THAT(TakeError(), MatchesRegex(".*'error2::group_a'.*no members")); FlushDiagnostics(); - DRAKE_EXPECT_NO_THROW( - ParseTestString(R"""( + DRAKE_EXPECT_NO_THROW(ParseTestString(R"""( @@ -3392,9 +3401,10 @@ TEST_F(SdfParserTest, CollisionFilterGroupParsingErrorsTest) { )""")); EXPECT_THAT(TakeError(), MatchesRegex(".*'error3::group_a'.*no members")); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*The tag is missing" - " a required string value.*")); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*The tag is missing" + " a required string value.*")); FlushDiagnostics(); } @@ -3533,8 +3543,7 @@ TEST_F(SdfParserTest, WorldJoint) { EXPECT_EQ(parent_link.model_instance(), parent_instance); EXPECT_EQ(child_link.model_instance(), child_instance); - const Joint& joint = - plant_.GetJointByName("J1"); + const Joint& joint = plant_.GetJointByName("J1"); EXPECT_EQ(joint.name(), "J1"); EXPECT_EQ(joint.parent_body().name(), "L_P"); EXPECT_EQ(joint.child_body().name(), "L_C"); @@ -3572,8 +3581,10 @@ TEST_F(SdfParserTest, TestUnsupportedVisualGeometry) { )"""); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*Ignoring unsupported SDFormat element in geometry: heightmap.*")); + EXPECT_THAT( + TakeWarning(), + ::testing::MatchesRegex( + ".*Ignoring unsupported SDFormat element in geometry: heightmap.*")); FlushDiagnostics(); ParseTestString(R"""( @@ -3586,8 +3597,10 @@ TEST_F(SdfParserTest, TestUnsupportedVisualGeometry) { )"""); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*Ignoring unsupported SDFormat element in geometry: polyline.*")); + EXPECT_THAT( + TakeWarning(), + ::testing::MatchesRegex( + ".*Ignoring unsupported SDFormat element in geometry: polyline.*")); } // Tests the error handling for an unsupported collision geometry. @@ -3603,8 +3616,10 @@ TEST_F(SdfParserTest, TestUnsupportedCollisionGeometry) { )"""); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*Ignoring unsupported SDFormat element in geometry: heightmap.*")); + EXPECT_THAT( + TakeWarning(), + ::testing::MatchesRegex( + ".*Ignoring unsupported SDFormat element in geometry: heightmap.*")); FlushDiagnostics(); ParseTestString(R"""( @@ -3617,8 +3632,10 @@ TEST_F(SdfParserTest, TestUnsupportedCollisionGeometry) { )"""); - EXPECT_THAT(TakeWarning(), ::testing::MatchesRegex( - ".*Ignoring unsupported SDFormat element in geometry: polyline.*")); + EXPECT_THAT( + TakeWarning(), + ::testing::MatchesRegex( + ".*Ignoring unsupported SDFormat element in geometry: polyline.*")); } // Regression test for #18878. @@ -3647,14 +3664,14 @@ TEST_F(SdfParserTest, TestSingleModelEnforcement) { const DataSource data_source{DataSource::kContents, &multi_models}; internal::CollisionFilterGroupResolver resolver{&plant_}; ParsingWorkspace w{options_, package_map_, diagnostic_policy_, - &plant_, &resolver, TestingSelect}; + &plant_, &resolver, TestingSelect}; std::optional result = AddModelFromSdf(data_source, "", {}, w); resolver.Resolve(diagnostic_policy_); EXPECT_FALSE(result.has_value()); EXPECT_THAT(TakeError(), ::testing::MatchesRegex( - ".*Root object can only contain one model.*")); + ".*Root object can only contain one model.*")); } // Verify merge-include works with Interface API. diff --git a/multibody/parsing/test/detail_urdf_parser_test.cc b/multibody/parsing/test/detail_urdf_parser_test.cc index 8059071cf7fd..0cf6d12c3d6d 100644 --- a/multibody/parsing/test/detail_urdf_parser_test.cc +++ b/multibody/parsing/test/detail_urdf_parser_test.cc @@ -35,10 +35,10 @@ namespace { using ::testing::MatchesRegex; -using Eigen::Vector2d; -using Eigen::Vector3d; using drake::internal::DiagnosticDetail; using drake::internal::DiagnosticPolicy; +using Eigen::Vector2d; +using Eigen::Vector3d; using geometry::GeometryId; using geometry::SceneGraph; @@ -50,39 +50,35 @@ using geometry::SceneGraph; // MultibodyPlant used for testing before parsing. class UrdfParserTest : public test::DiagnosticPolicyTestBase { public: - UrdfParserTest() { - plant_.RegisterAsSourceForSceneGraph(&scene_graph_); - } + UrdfParserTest() { plant_.RegisterAsSourceForSceneGraph(&scene_graph_); } std::optional AddModelFromUrdfFile( - const std::string& file_name, - const std::string& model_name) { + const std::string& file_name, const std::string& model_name) { internal::CollisionFilterGroupResolver resolver{&plant_}; ParsingWorkspace w{options_, package_map_, diagnostic_policy_, - &plant_, &resolver, NoSelect}; - auto result = AddModelFromUrdf( - {DataSource::kFilename, &file_name}, model_name, {}, w); + &plant_, &resolver, NoSelect}; + auto result = AddModelFromUrdf({DataSource::kFilename, &file_name}, + model_name, {}, w); last_parsed_groups_ = ConvertInstancedNamesToStrings( resolver.Resolve(diagnostic_policy_), plant_); return result; } std::optional AddModelFromUrdfString( - const std::string& file_contents, - const std::string& model_name) { + const std::string& file_contents, const std::string& model_name) { internal::CollisionFilterGroupResolver resolver{&plant_}; ParsingWorkspace w{options_, package_map_, diagnostic_policy_, - &plant_, &resolver, NoSelect}; - auto result = AddModelFromUrdf( - {DataSource::kContents, &file_contents}, model_name, {}, w); + &plant_, &resolver, NoSelect}; + auto result = AddModelFromUrdf({DataSource::kContents, &file_contents}, + model_name, {}, w); last_parsed_groups_ = ConvertInstancedNamesToStrings( resolver.Resolve(diagnostic_policy_), plant_); return result; } // URDF cannot delegate to any other parsers. - static ParserInterface& NoSelect( - const drake::internal::DiagnosticPolicy&, const std::string&) { + static ParserInterface& NoSelect(const drake::internal::DiagnosticPolicy&, + const std::string&) { DRAKE_UNREACHABLE(); } @@ -102,34 +98,39 @@ class UrdfParserTest : public test::DiagnosticPolicyTestBase { TEST_F(UrdfParserTest, BadFilename) { EXPECT_EQ(AddModelFromUrdfFile("nonexistent.urdf", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - "/.*/nonexistent.urdf:0: error: " - "Failed to parse XML file: XML_ERROR_FILE_NOT_FOUND")); + EXPECT_THAT( + TakeError(), + MatchesRegex("/.*/nonexistent.urdf:0: error: " + "Failed to parse XML file: XML_ERROR_FILE_NOT_FOUND")); } TEST_F(UrdfParserTest, BadXmlString) { EXPECT_EQ(AddModelFromUrdfString("not proper xml content", ""), std::nullopt); - EXPECT_EQ(TakeError(), ".urdf:1: error: Failed to parse" + EXPECT_EQ(TakeError(), + ".urdf:1: error: Failed to parse" " XML string: XML_ERROR_PARSING_TEXT"); } TEST_F(UrdfParserTest, NoRobot) { EXPECT_EQ(AddModelFromUrdfString("", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*URDF does not contain a robot tag.")); + EXPECT_THAT(TakeError(), + MatchesRegex(".*URDF does not contain a robot tag.")); } TEST_F(UrdfParserTest, NoName) { EXPECT_EQ(AddModelFromUrdfString("", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( + EXPECT_THAT(TakeError(), + MatchesRegex( ".*Your robot must have a name attribute or a model name must" " be specified.")); } TEST_F(UrdfParserTest, ModelRenameWithColons) { - std::optional index = AddModelFromUrdfString(R"""( + std::optional index = + AddModelFromUrdfString(R"""( - )""", "left::robot"); + )""", + "left::robot"); ASSERT_NE(index, std::nullopt); EXPECT_EQ(plant_.GetModelInstanceName(*index), "left::robot"); } @@ -137,8 +138,9 @@ TEST_F(UrdfParserTest, ModelRenameWithColons) { TEST_F(UrdfParserTest, ObsoleteLoopJoint) { EXPECT_NE(AddModelFromUrdfString("", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*loop joints are not supported in MultibodyPlant")); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*loop joints are not supported in MultibodyPlant")); } TEST_F(UrdfParserTest, LegacyDrakeIgnoreBody) { @@ -148,16 +150,20 @@ TEST_F(UrdfParserTest, LegacyDrakeIgnoreBody) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); } TEST_F(UrdfParserTest, BodyNameBroken) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*link tag is missing name attribute.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*link tag is missing name attribute.")); } TEST_F(UrdfParserTest, LegacyDrakeIgnoreJoint) { @@ -165,25 +171,31 @@ TEST_F(UrdfParserTest, LegacyDrakeIgnoreJoint) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); } TEST_F(UrdfParserTest, JointNameBroken) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*joint tag is missing name attribute")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*joint tag is missing name attribute")); } TEST_F(UrdfParserTest, JointTypeBroken) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*joint 'a' is missing type attribute")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*joint 'a' is missing type attribute")); } TEST_F(UrdfParserTest, JointNoParent) { @@ -193,9 +205,11 @@ TEST_F(UrdfParserTest, JointNoParent) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*joint 'a' doesn't have a parent node!")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*joint 'a' doesn't have a parent node!")); } TEST_F(UrdfParserTest, JointParentLinkBroken) { @@ -207,9 +221,12 @@ TEST_F(UrdfParserTest, JointParentLinkBroken) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*joint a's parent does not have a link attribute!")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*joint a's parent does not have a link attribute!")); } TEST_F(UrdfParserTest, JointNoChild) { @@ -219,9 +236,11 @@ TEST_F(UrdfParserTest, JointNoChild) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*joint 'a' doesn't have a child node!")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*joint 'a' doesn't have a child node!")); } TEST_F(UrdfParserTest, JointChildLinkBroken) { @@ -233,9 +252,12 @@ TEST_F(UrdfParserTest, JointChildLinkBroken) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*joint a's child does not have a link attribute!")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*joint a's child does not have a link attribute!")); } TEST_F(UrdfParserTest, JointBadDynamicsAttributes) { @@ -249,13 +271,13 @@ TEST_F(UrdfParserTest, JointBadDynamicsAttributes) { )"""; - const auto attrs = std::array{"damping", "friction", - "coulomb_window"}; + const auto attrs = + std::array{"damping", "friction", "coulomb_window"}; for (const auto& attr : attrs) { EXPECT_NE(AddModelFromUrdfString(fmt::format(base, attr + "='1 2'"), attr), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Expected single value.*" + attr + ".*")); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Expected single value.*" + attr + ".*")); } // Dynamics warnings are tested elsewhere in this file. warning_records_.clear(); @@ -271,7 +293,9 @@ TEST_F(UrdfParserTest, DrakeFrictionWarning) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(".*joint friction.*")); } @@ -285,7 +309,9 @@ TEST_F(UrdfParserTest, DrakeCoulombWarning) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(".*coulomb_window.*ignored.*")); } @@ -298,10 +324,13 @@ TEST_F(UrdfParserTest, JointLinkMissing) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Could not find link named 'parent' with model instance" - " ID 2 for element 'joint'.")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*Could not find link named 'parent' with model instance" + " ID 2 for element 'joint'.")); } TEST_F(UrdfParserTest, JointZeroAxis) { @@ -314,9 +343,11 @@ TEST_F(UrdfParserTest, JointZeroAxis) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Joint 'joint' axis is zero. Don't do that.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Joint 'joint' axis is zero. Don't do that.")); } TEST_F(UrdfParserTest, JointFloatingWarning) { @@ -328,11 +359,14 @@ TEST_F(UrdfParserTest, JointFloatingWarning) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeWarning(), MatchesRegex( - ".*Joint 'joint' specified as type floating which is not" - " supported by MultibodyPlant. Leaving 'child' as a" - " free body.")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeWarning(), + MatchesRegex(".*Joint 'joint' specified as type floating which is not" + " supported by MultibodyPlant. Leaving 'child' as a" + " free body.")); } TEST_F(UrdfParserTest, JointTypeUnknown) { @@ -344,9 +378,11 @@ TEST_F(UrdfParserTest, JointTypeUnknown) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Joint 'joint' has unrecognized type: 'who'")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Joint 'joint' has unrecognized type: 'who'")); } // TODO(rpoyner-tri): Add MimicContinuousTime (which should throw the same @@ -366,7 +402,9 @@ TEST_F(UrdfParserTest, MimicNoSap) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT( TakeWarning(), MatchesRegex( @@ -386,7 +424,9 @@ TEST_F(UrdfParserTest, MimicNoJoint) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeError(), MatchesRegex(".*Joint 'joint' mimic element is missing the " "required 'joint' attribute.")); @@ -404,7 +444,9 @@ TEST_F(UrdfParserTest, MimicBadJoint) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeError(), MatchesRegex(".*Joint 'joint' mimic element specifies joint " "'nonexistent' which does not exist.")); @@ -422,7 +464,9 @@ TEST_F(UrdfParserTest, MimicSameJoint) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeError(), MatchesRegex(".*Joint 'joint' mimic element specifies joint " "'joint'. Joints cannot mimic themselves.")); @@ -445,7 +489,9 @@ TEST_F(UrdfParserTest, MimicMismatchedJoint) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeError(), MatchesRegex(".*Joint 'joint1' which has 1 DOF cannot mimic " "joint 'joint0' which has 0 DOF.")); @@ -468,7 +514,9 @@ TEST_F(UrdfParserTest, MimicOnlyOneDOFJoint) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(".*Drake only supports the mimic element for " "single-dof joints.*")); @@ -491,7 +539,9 @@ TEST_F(UrdfParserTest, MimicFloatingJoint) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); TakeWarning(); // The first warning is about not supporting floating joints. // See issue #13691. EXPECT_THAT(TakeWarning(), @@ -519,8 +569,9 @@ TEST_F(UrdfParserTest, MimicDifferentModelInstances) { - )""", ""), - std::nullopt /* valid model instance index was parsed */); + )""", + ""), + std::nullopt /* valid model instance index was parsed */); EXPECT_NE(AddModelFromUrdfString(R"""( @@ -535,8 +586,9 @@ TEST_F(UrdfParserTest, MimicDifferentModelInstances) { - )""", ""), - std::nullopt /* valid model instance index was parsed */); + )""", + ""), + std::nullopt /* valid model instance index was parsed */); EXPECT_EQ(NumErrors(), 0); EXPECT_EQ(NumWarnings(), 0); } @@ -558,14 +610,18 @@ TEST_F(UrdfParserTest, Material) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); } TEST_F(UrdfParserTest, FrameNameBroken) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeError(), MatchesRegex(".*parsing frame name.")); } @@ -573,18 +629,22 @@ TEST_F(UrdfParserTest, FrameLinkBroken) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*missing link name for frame frameA.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*missing link name for frame frameA.")); } TEST_F(UrdfParserTest, TransmissionTypeBroken) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Transmission element is missing a type.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Transmission element is missing a type.")); } TEST_F(UrdfParserTest, TransmissionTypeUnknown) { @@ -597,7 +657,9 @@ TEST_F(UrdfParserTest, TransmissionTypeUnknown) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(warning_pattern)); // Express unknown type as element. @@ -606,7 +668,9 @@ TEST_F(UrdfParserTest, TransmissionTypeUnknown) { who - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(warning_pattern)); } @@ -614,9 +678,11 @@ TEST_F(UrdfParserTest, TransmissionActuatorMissing) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Transmission is missing an actuator element.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Transmission is missing an actuator element.")); } TEST_F(UrdfParserTest, TransmissionActuatorNameBroken) { @@ -625,9 +691,11 @@ TEST_F(UrdfParserTest, TransmissionActuatorNameBroken) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Transmission is missing an actuator name.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Transmission is missing an actuator name.")); } TEST_F(UrdfParserTest, TransmissionJointMissing) { @@ -636,9 +704,11 @@ TEST_F(UrdfParserTest, TransmissionJointMissing) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Transmission is missing a joint element.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Transmission is missing a joint element.")); } TEST_F(UrdfParserTest, TransmissionJointNameBroken) { @@ -648,9 +718,11 @@ TEST_F(UrdfParserTest, TransmissionJointNameBroken) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Transmission is missing a joint name.")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Transmission is missing a joint name.")); } TEST_F(UrdfParserTest, TransmissionJointNotExist) { @@ -660,9 +732,11 @@ TEST_F(UrdfParserTest, TransmissionJointNotExist) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeError(), MatchesRegex(".*Transmission specifies joint" - " 'nowhere' which does not exist.")); + " 'nowhere' which does not exist.")); } TEST_F(UrdfParserTest, TransmissionJointBadLimits) { @@ -682,23 +756,25 @@ TEST_F(UrdfParserTest, TransmissionJointBadLimits) { )"""; EXPECT_NE(AddModelFromUrdfString(fmt::format(base, "effort='0'"), ""), std::nullopt); - EXPECT_THAT(TakeWarning(), MatchesRegex( - ".*Skipping transmission since it's attached to joint \"a\"" - " which has a zero effort limit 0.*")); + EXPECT_THAT( + TakeWarning(), + MatchesRegex(".*Skipping transmission since it's attached to joint \"a\"" + " which has a zero effort limit 0.*")); EXPECT_NE(AddModelFromUrdfString(fmt::format(base, "effort='-3'"), "b"), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Transmission specifies joint 'a' which has a negative" - " effort limit.")); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*Transmission specifies joint 'a' which has a negative" + " effort limit.")); const auto attrs = std::array{"lower", "upper", "velocity", "drake:acceleration", "effort"}; for (const auto& attr : attrs) { EXPECT_NE(AddModelFromUrdfString(fmt::format(base, attr + "='1 2'"), attr), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Expected single value.*" + attr + ".*")); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Expected single value.*" + attr + ".*")); } } @@ -749,9 +825,9 @@ TEST_F(UrdfParserTest, DoublePendulum) { math::RigidTransform X_BF_expected(rpy_expected.ToRotationMatrix(), xyz_expected); - EXPECT_TRUE(CompareMatrices( - frame_on_link1.GetFixedPoseInBodyFrame().GetAsMatrix4(), - X_BF_expected.GetAsMatrix4(), 1e-10)); + EXPECT_TRUE( + CompareMatrices(frame_on_link1.GetFixedPoseInBodyFrame().GetAsMatrix4(), + X_BF_expected.GetAsMatrix4(), 1e-10)); const Frame& link2_com = plant_.GetFrameByName("link2_com"); EXPECT_EQ(link2_com.body().index(), plant_.GetBodyByName("link2").index()); @@ -761,8 +837,8 @@ TEST_F(UrdfParserTest, DoublePendulum) { // `package://` syntax internally to the URDF (at least for packages which are // successfully found in the same directory at the URDF. TEST_F(UrdfParserTest, TestAtlasMinimalContact) { - const std::string full_name = FindRunfile( - "drake_models/atlas/atlas_minimal_contact.urdf").abspath; + const std::string full_name = + FindRunfile("drake_models/atlas/atlas_minimal_contact.urdf").abspath; AddModelFromUrdfFile(full_name, ""); for (int k = 0; k < 30; k++) { EXPECT_THAT(TakeWarning(), MatchesRegex(".*safety_controller.*ignored.*")); @@ -793,8 +869,8 @@ TEST_F(UrdfParserTest, TestAddWithQuaternionFloatingDof) { } TEST_F(UrdfParserTest, TestRegisteredSceneGraph) { - const std::string full_name = FindRunfile( - "drake_models/atlas/atlas_minimal_contact.urdf").abspath; + const std::string full_name = + FindRunfile("drake_models/atlas/atlas_minimal_contact.urdf").abspath; // Test that registration with scene graph results in visual geometries. AddModelFromUrdfFile(full_name, ""); // Mostly ignore warnings here; they are tested in detail elsewhere. @@ -815,8 +891,7 @@ TEST_F(UrdfParserTest, JointParsingTest) { plant_.Finalize(); // Revolute joint - DRAKE_EXPECT_NO_THROW( - plant_.GetJointByName("revolute_joint")); + DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("revolute_joint")); const RevoluteJoint& revolute_joint = plant_.GetJointByName("revolute_joint"); EXPECT_EQ(revolute_joint.name(), "revolute_joint"); @@ -832,10 +907,10 @@ TEST_F(UrdfParserTest, JointParsingTest) { CompareMatrices(revolute_joint.velocity_lower_limits(), Vector1d(-100))); EXPECT_TRUE( CompareMatrices(revolute_joint.velocity_upper_limits(), Vector1d(100))); - EXPECT_TRUE(CompareMatrices( - revolute_joint.acceleration_lower_limits(), Vector1d(-200))); - EXPECT_TRUE(CompareMatrices( - revolute_joint.acceleration_upper_limits(), Vector1d(200))); + EXPECT_TRUE(CompareMatrices(revolute_joint.acceleration_lower_limits(), + Vector1d(-200))); + EXPECT_TRUE(CompareMatrices(revolute_joint.acceleration_upper_limits(), + Vector1d(200))); // Revolute actuator const JointActuator& revolute_actuator = @@ -860,15 +935,14 @@ TEST_F(UrdfParserTest, JointParsingTest) { CompareMatrices(prismatic_joint.velocity_lower_limits(), Vector1d(-5))); EXPECT_TRUE( CompareMatrices(prismatic_joint.velocity_upper_limits(), Vector1d(5))); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.acceleration_lower_limits(), Vector1d(-10))); - EXPECT_TRUE(CompareMatrices( - prismatic_joint.acceleration_upper_limits(), Vector1d(10))); + EXPECT_TRUE(CompareMatrices(prismatic_joint.acceleration_lower_limits(), + Vector1d(-10))); + EXPECT_TRUE(CompareMatrices(prismatic_joint.acceleration_upper_limits(), + Vector1d(10))); EXPECT_FALSE(plant_.HasJointActuatorNamed("prismatic_actuator")); // Ball joint - DRAKE_EXPECT_NO_THROW( - plant_.GetJointByName("ball_joint")); + DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("ball_joint")); const BallRpyJoint& ball_joint = plant_.GetJointByName("ball_joint"); EXPECT_EQ(ball_joint.name(), "ball_joint"); @@ -897,8 +971,8 @@ TEST_F(UrdfParserTest, JointParsingTest) { EXPECT_TRUE(CompareMatrices(no_limit_joint.position_upper_limits(), inf)); EXPECT_TRUE(CompareMatrices(no_limit_joint.velocity_lower_limits(), neg_inf)); EXPECT_TRUE(CompareMatrices(no_limit_joint.velocity_upper_limits(), inf)); - EXPECT_TRUE(CompareMatrices( - no_limit_joint.acceleration_lower_limits(), neg_inf)); + EXPECT_TRUE( + CompareMatrices(no_limit_joint.acceleration_lower_limits(), neg_inf)); EXPECT_TRUE(CompareMatrices(no_limit_joint.acceleration_upper_limits(), inf)); EXPECT_GT(no_limit_joint.index(), ball_joint.index()); @@ -920,11 +994,11 @@ TEST_F(UrdfParserTest, JointParsingTest) { std::numeric_limits::infinity()); const Vector2d neg_inf2(-std::numeric_limits::infinity(), -std::numeric_limits::infinity()); - EXPECT_TRUE(CompareMatrices(universal_joint.position_lower_limits(), - neg_inf2)); + EXPECT_TRUE( + CompareMatrices(universal_joint.position_lower_limits(), neg_inf2)); EXPECT_TRUE(CompareMatrices(universal_joint.position_upper_limits(), inf2)); - EXPECT_TRUE(CompareMatrices(universal_joint.velocity_lower_limits(), - neg_inf2)); + EXPECT_TRUE( + CompareMatrices(universal_joint.velocity_lower_limits(), neg_inf2)); EXPECT_TRUE(CompareMatrices(universal_joint.velocity_upper_limits(), inf2)); // Planar joint @@ -962,8 +1036,7 @@ TEST_F(UrdfParserTest, JointParsingTest) { CompareMatrices(continuous_joint.acceleration_upper_limits(), inf)); // Screw joint - DRAKE_EXPECT_NO_THROW( - plant_.GetJointByName("screw_joint")); + DRAKE_EXPECT_NO_THROW(plant_.GetJointByName("screw_joint")); const ScrewJoint& screw_joint = plant_.GetJointByName("screw_joint"); EXPECT_EQ(screw_joint.name(), "screw_joint"); @@ -972,16 +1045,13 @@ TEST_F(UrdfParserTest, JointParsingTest) { EXPECT_EQ(screw_joint.screw_axis(), Vector3d::UnitX()); EXPECT_EQ(screw_joint.screw_pitch(), 0.04); EXPECT_EQ(screw_joint.default_damping(), 0.1); - EXPECT_TRUE( - CompareMatrices(screw_joint.position_lower_limits(), neg_inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.position_lower_limits(), neg_inf)); EXPECT_TRUE(CompareMatrices(screw_joint.position_upper_limits(), inf)); - EXPECT_TRUE( - CompareMatrices(screw_joint.velocity_lower_limits(), neg_inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.velocity_lower_limits(), neg_inf)); EXPECT_TRUE(CompareMatrices(screw_joint.velocity_upper_limits(), inf)); EXPECT_TRUE( CompareMatrices(screw_joint.acceleration_lower_limits(), neg_inf)); - EXPECT_TRUE( - CompareMatrices(screw_joint.acceleration_upper_limits(), inf)); + EXPECT_TRUE(CompareMatrices(screw_joint.acceleration_upper_limits(), inf)); // Revolute joint with mimic DRAKE_EXPECT_NO_THROW( @@ -1044,17 +1114,18 @@ TEST_F(UrdfParserTest, JointParsingTagMismatchTest) { "drake/multibody/parsing/test/urdf_parser_test/" "joint_parsing_test_tag_mismatch_1.urdf"); AddModelFromUrdfFile(full_name_mismatch_1, ""); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Joint fixed_joint of type fixed is a standard joint type," - " and should be a ")); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*Joint fixed_joint of type fixed is a standard joint type," + " and should be a ")); const std::string full_name_mismatch_2 = FindResourceOrThrow( "drake/multibody/parsing/test/urdf_parser_test/" "joint_parsing_test_tag_mismatch_2.urdf"); AddModelFromUrdfFile(full_name_mismatch_2, ""); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Joint ball_joint of type ball is a custom joint" - " type, and should be a ")); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Joint ball_joint of type ball is a custom joint" + " type, and should be a ")); } TEST_F(UrdfParserTest, JointParsingTagMissingScrewParametersTest) { @@ -1063,17 +1134,19 @@ TEST_F(UrdfParserTest, JointParsingTagMissingScrewParametersTest) { "drake/multibody/parsing/test/urdf_parser_test/" "joint_parsing_test_missing_screw_thread_pitch.urdf"); AddModelFromUrdfFile(full_name_missing_element, ""); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*A screw joint is missing the " - " tag.")); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*A screw joint is missing the " + " tag.")); const std::string full_name_missing_attribute = FindResourceOrThrow( "drake/multibody/parsing/test/urdf_parser_test/" "joint_parsing_test_missing_screw_thread_pitch_attribute.urdf"); AddModelFromUrdfFile(full_name_missing_attribute, ""); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*A screw joint has a tag" - " that is missing the 'value' attribute.")); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*A screw joint has a tag" + " that is missing the 'value' attribute.")); } // We allow users to declare the "world" link for the purpose of declaring @@ -1114,8 +1187,8 @@ TEST_F(UrdfParserTest, AddingGeometriesToWorldLink) { )"""; AddModelFromUrdfString(test_urdf, "urdf"); - EXPECT_THAT(TakeWarning(), MatchesRegex( - ".* tag is being ignored.*")); + EXPECT_THAT(TakeWarning(), + MatchesRegex(".* tag is being ignored.*")); const auto& inspector = scene_graph_.model_inspector(); EXPECT_EQ(inspector.num_geometries(), 2); @@ -1150,15 +1223,14 @@ ::testing::AssertionResult FrameHasShape(geometry::FrameId frame_id, inspector.GetShape(geometry_id).type_name(); if (shape_type != name) { return ::testing::AssertionFailure() - << "Geometry with role " << role << " has wrong shape type." - << "\nExpected: " << name - << "\nFound: " << shape_type; + << "Geometry with role " << role << " has wrong shape type." + << "\nExpected: " << name << "\nFound: " << shape_type; } } catch (const std::exception& e) { return ::testing::AssertionFailure() - << "Frame " << frame_id << " does not have a geometry with role " - << role << " and name " << name - << ".\n Exception message: " << e.what(); + << "Frame " << frame_id << " does not have a geometry with role " + << role << " and name " << name + << ".\n Exception message: " << e.what(); } return ::testing::AssertionSuccess(); } @@ -1193,8 +1265,8 @@ class UrdfParsedGeometryTest : public UrdfParserTest { geometry::Ellipsoid{0.1, 0.1, 0.1})); EXPECT_TRUE(FrameHasShape(frame_id, role, scene_graph_, geometry::Mesh{mesh_uri, 1.0})); - EXPECT_TRUE(FrameHasShape(frame_id, role, scene_graph_, - geometry::Sphere{0.1})); + EXPECT_TRUE( + FrameHasShape(frame_id, role, scene_graph_, geometry::Sphere{0.1})); } }; @@ -1236,7 +1308,9 @@ TEST_F(UrdfParserTest, EntireInertialTagOmitted) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); const auto& body = dynamic_cast&>( plant_.GetBodyByName("entire_inertial_tag_omitted")); @@ -1255,7 +1329,9 @@ TEST_F(UrdfParserTest, InertiaTagOmitted) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); const auto& body = dynamic_cast&>( plant_.GetBodyByName("inertia_tag_omitted")); EXPECT_EQ(body.default_mass(), 2.); @@ -1275,7 +1351,9 @@ TEST_F(UrdfParserTest, MassTagOmitted) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); const auto& body = dynamic_cast&>( plant_.GetBodyByName("mass_tag_omitted")); EXPECT_EQ(body.default_mass(), 0.); @@ -1293,7 +1371,9 @@ TEST_F(UrdfParserTest, MasslessBody) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); const auto& body = dynamic_cast&>( plant_.GetBodyByName("massless_link")); EXPECT_EQ(body.default_mass(), 0.); @@ -1311,7 +1391,9 @@ TEST_F(UrdfParserTest, PointMass) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); const auto& body = dynamic_cast&>( plant_.GetBodyByName("point_mass")); EXPECT_EQ(body.default_mass(), 1.); @@ -1332,31 +1414,38 @@ TEST_F(UrdfParserTest, BadInertiaFormats) { )"""; AddModelFromUrdfString( fmt::format(base, "value='1 2 3'", - "ixx='0' ixy='0' ixz='0' iyy='0' iyz='0' izz='0'"), ""); + "ixx='0' ixy='0' ixz='0' iyy='0' iyz='0' izz='0'"), + ""); EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*value.*")); AddModelFromUrdfString( fmt::format(base, "value='1'", - "ixx='0 2 3' ixy='0' ixz='0' iyy='0' iyz='0' izz='0'"), "a"); + "ixx='0 2 3' ixy='0' ixz='0' iyy='0' iyz='0' izz='0'"), + "a"); EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*ixx.*")); AddModelFromUrdfString( fmt::format(base, "value='1'", - "ixx='0' ixy='0 2 3' ixz='0' iyy='0' iyz='0' izz='0'"), "b"); + "ixx='0' ixy='0 2 3' ixz='0' iyy='0' iyz='0' izz='0'"), + "b"); EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*ixy.*")); AddModelFromUrdfString( fmt::format(base, "value='1'", - "ixx='0' ixy='0' ixz='0 2 3' iyy='0' iyz='0' izz='0'"), "c"); + "ixx='0' ixy='0' ixz='0 2 3' iyy='0' iyz='0' izz='0'"), + "c"); EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*ixz.*")); AddModelFromUrdfString( fmt::format(base, "value='1'", - "ixx='0' ixy='0' ixz='0' iyy='0 2 3' iyz='0' izz='0'"), "d"); + "ixx='0' ixy='0' ixz='0' iyy='0 2 3' iyz='0' izz='0'"), + "d"); EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*iyy.*")); AddModelFromUrdfString( fmt::format(base, "value='1'", - "ixx='0' ixy='0' ixz='0' iyy='0' iyz='0 2 3' izz='0'"), "e"); + "ixx='0' ixy='0' ixz='0' iyy='0' iyz='0 2 3' izz='0'"), + "e"); EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*iyz.*")); AddModelFromUrdfString( fmt::format(base, "value='1'", - "ixx='0' ixy='0' ixz='0' iyy='0' iyz='0' izz='0 2 3'"), "f"); + "ixx='0' ixy='0' ixz='0' iyy='0' iyz='0' izz='0 2 3'"), + "f"); EXPECT_THAT(TakeError(), MatchesRegex(".*Expected single value.*izz.*")); } @@ -1381,7 +1470,6 @@ TEST_F(UrdfParserTest, BushingParsing) { EXPECT_NE(AddModelFromUrdfString(good_bushing_model, "b1"), std::nullopt); EXPECT_NE(AddModelFromUrdfString(good_bushing_model, "b2"), std::nullopt); - // MBP will always create a UniformGravityField, so the only other // ForceElement should be the LinearBushingRollPitchYaw element parsed. EXPECT_EQ(plant_.num_force_elements(), 3); @@ -1420,9 +1508,11 @@ TEST_F(UrdfParserTest, BushingMissingFrameTag) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Unable to find the tag")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Unable to find the tag")); } TEST_F(UrdfParserTest, BushingFrameTagNameBroken) { @@ -1441,10 +1531,12 @@ TEST_F(UrdfParserTest, BushingFrameTagNameBroken) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Unable to read the 'name' attribute for the" - " tag")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Unable to read the 'name' attribute for the" + " tag")); } TEST_F(UrdfParserTest, BushingFrameNotExist) { @@ -1463,10 +1555,13 @@ TEST_F(UrdfParserTest, BushingFrameNotExist) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Frame: frameZ specified for does" - " not exist in the model.")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*Frame: frameZ specified for does" + " not exist in the model.")); } TEST_F(UrdfParserTest, BushingMissingDamping) { @@ -1484,9 +1579,12 @@ TEST_F(UrdfParserTest, BushingMissingDamping) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Unable to find the tag")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*Unable to find the tag")); } TEST_F(UrdfParserTest, BushingMissingValueAttribute) { @@ -1505,10 +1603,12 @@ TEST_F(UrdfParserTest, BushingMissingValueAttribute) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*Unable to read the 'value' attribute for the" - " tag")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*Unable to read the 'value' attribute for the" + " tag")); } class BallConstraintTest : public UrdfParserTest { @@ -1625,8 +1725,7 @@ class ReflectedInertiaTest : public UrdfParserTest { public: void VerifyParameters(const std::string& rotor_inertia_text, const std::string& gear_ratio_text, - double rotor_inertia, - double gear_ratio) { + double rotor_inertia, double gear_ratio) { std::string text = fmt::format(kTestString, rotor_inertia_text, gear_ratio_text); EXPECT_NE(AddModelFromUrdfString(text, ""), std::nullopt); @@ -1677,8 +1776,7 @@ class ReflectedInertiaTest : public UrdfParserTest { TEST_F(ReflectedInertiaTest, Both) { // Test successful parsing of both parameters. VerifyParameters("", - "", - 1.5, 300); + "", 1.5, 300); } TEST_F(ReflectedInertiaTest, DefaultGearRatio) { @@ -1876,10 +1974,13 @@ TEST_F(UrdfParserTest, CollisionFilterGroupMissingName) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*The tag does not specify" - " the required attribute \"name\".")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*The tag does not specify" + " the required attribute \"name\".")); } TEST_F(UrdfParserTest, CollisionFilterGroupMissingLink) { @@ -1889,10 +1990,13 @@ TEST_F(UrdfParserTest, CollisionFilterGroupMissingLink) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*The tag does not specify the required " - "attribute \"link\".")); + )""", + ""), + std::nullopt); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*The tag does not specify the required " + "attribute \"link\".")); EXPECT_THAT(TakeError(), MatchesRegex(".*'robot::group_a'.*no members")); } @@ -1903,10 +2007,12 @@ TEST_F(UrdfParserTest, CollisionFilterGroupMissingMemberGroupName) { - )""", ""), std::nullopt); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*The tag does not specify the" - " required attribute \"name\".")); + )""", + ""), + std::nullopt); + EXPECT_THAT(TakeError(), + MatchesRegex(".*The tag does not specify the" + " required attribute \"name\".")); EXPECT_THAT(TakeError(), MatchesRegex(".*'robot::group_a'.*no members")); } @@ -1917,11 +2023,14 @@ TEST_F(UrdfParserTest, IgnoredCollisionFilterGroupMissingName) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeError(), MatchesRegex(".*'robot::group_a'.*no members")); - EXPECT_THAT(TakeError(), MatchesRegex( - ".*The tag does not" - " specify the required attribute \"name\".")); + EXPECT_THAT( + TakeError(), + MatchesRegex(".*The tag does not" + " specify the required attribute \"name\".")); } // Here follow tests to verify that Drake issues a warning when it ignores @@ -1931,7 +2040,9 @@ TEST_F(UrdfParserTest, UnsupportedVersionIgnored) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(".*version.*ignored.*")); } @@ -1939,13 +2050,14 @@ TEST_F(UrdfParserTest, UnsupportedLinkTypeIgnored) { EXPECT_NE(AddModelFromUrdfString(R"""( - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(".*type.*link.*ignored.*")); } TEST_F(UrdfParserTest, UnsupportedJointStuffIgnored) { - const std::array tags{ - "calibration", "safety_controller"}; + const std::array tags{"calibration", "safety_controller"}; for (const auto& tag : tags) { EXPECT_NE(AddModelFromUrdfString(fmt::format(R"""( @@ -1956,16 +2068,23 @@ TEST_F(UrdfParserTest, UnsupportedJointStuffIgnored) { <{}/> - )""", tag), tag), std::nullopt); + )""", + tag), + tag), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(fmt::format(".*{}.*ignored.*", tag))); } } TEST_F(UrdfParserTest, UnsupportedTransmissionStuffIgnored) { - const std::array tags{ - "leftActuator", "rightActuator", "flexJoint", "rollJoint", "gap_joint", - "passive_joint", "use_simulated_gripper_joint"}; + const std::array tags{"leftActuator", + "rightActuator", + "flexJoint", + "rollJoint", + "gap_joint", + "passive_joint", + "use_simulated_gripper_joint"}; for (const auto& tag : tags) { EXPECT_NE(AddModelFromUrdfString(fmt::format(R"""( @@ -1980,7 +2099,10 @@ TEST_F(UrdfParserTest, UnsupportedTransmissionStuffIgnored) { <{}/> - )""", tag), tag), std::nullopt); + )""", + tag), + tag), + std::nullopt); EXPECT_THAT(TakeWarning(), MatchesRegex(fmt::format(".*{}.*ignored.*", tag))); } @@ -1995,7 +2117,9 @@ TEST_F(UrdfParserTest, UnsupportedGazeboIgnoredSilent) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); } TEST_F(UrdfParserTest, UnsupportedTransmissionActuatorStuffIgnoredSilent) { @@ -2013,7 +2137,9 @@ TEST_F(UrdfParserTest, UnsupportedTransmissionActuatorStuffIgnoredSilent) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); } TEST_F(UrdfParserTest, UnsupportedTransmissionJointStuffIgnoredSilent) { @@ -2031,7 +2157,9 @@ TEST_F(UrdfParserTest, UnsupportedTransmissionJointStuffIgnoredSilent) { - )""", ""), std::nullopt); + )""", + ""), + std::nullopt); } // Here follow tests to verify that Drake applies special treatment (as @@ -2064,28 +2192,30 @@ TEST_F(UrdfParserTest, UnsupportedMechanicalReductionIgnoredMaybe) { bool provokes_warning{}; }; const std::array cases{{ - {"3500.25", true}, - {"22 79 15", true}, - {"QQQ", true}, - {"", false}, - {"", false}, - {" ", false}, - {"1", false}, - {"1.0", false}, + {"3500.25", true}, + {"22 79 15", true}, + {"QQQ", true}, + {"", false}, + {"", false}, + {" ", false}, + {"1", false}, + {"1.0", false}, }}; for (const Case& acase : cases) { // Within actuator. - EXPECT_NE(AddModelFromUrdfString( - fmt::format(robot_template, acase.input, ""), - acase.input + "_actuator"), std::nullopt); + EXPECT_NE( + AddModelFromUrdfString(fmt::format(robot_template, acase.input, ""), + acase.input + "_actuator"), + std::nullopt); if (acase.provokes_warning) { EXPECT_THAT(TakeWarning(), MatchesRegex(pattern)); } // Within transmission. - EXPECT_NE(AddModelFromUrdfString( - fmt::format(robot_template, "", acase.input), - acase.input + "_transmission"), std::nullopt); + EXPECT_NE( + AddModelFromUrdfString(fmt::format(robot_template, "", acase.input), + acase.input + "_transmission"), + std::nullopt); if (acase.provokes_warning) { EXPECT_THAT(TakeWarning(), MatchesRegex(pattern)); } @@ -2119,7 +2249,7 @@ TEST_F(UrdfParserTest, PlanarJointAxisRespected) { const math::RotationMatrixd R_WF = R_BM; const Vector3d p_WB = R_WF * p_WB_F; EXPECT_TRUE(CompareMatrices(X_WB.translation(), p_WB, - 4.0 * std::numeric_limits::epsilon())); + 4.0 * std::numeric_limits::epsilon())); } TEST_F(UrdfParserTest, PlanarJointCanonicalFrame) {