Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[parsing] Support parsing continuous joint in SDFormat #17956

Merged
merged 1 commit into from
Sep 22, 2022
Merged

[parsing] Support parsing continuous joint in SDFormat #17956

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
83 changes: 54 additions & 29 deletions multibody/parsing/detail_sdf_parser.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -318,7 +318,8 @@ Vector3d ExtractJointAxis(
const sdf::Joint& joint_spec) {
unused(model_spec);
DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::REVOLUTE ||
joint_spec.Type() == sdf::JointType::PRISMATIC);
joint_spec.Type() == sdf::JointType::PRISMATIC ||
joint_spec.Type() == sdf::JointType::CONTINUOUS);

// Axis specification.
const sdf::JointAxis* axis = joint_spec.Axis();
Expand Down Expand Up @@ -365,7 +366,8 @@ double ParseJointDamping(
DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::REVOLUTE ||
joint_spec.Type() == sdf::JointType::PRISMATIC ||
joint_spec.Type() == sdf::JointType::UNIVERSAL ||
joint_spec.Type() == sdf::JointType::BALL);
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.
Expand Down Expand Up @@ -430,11 +432,11 @@ void AddJointActuatorFromSpecification(
const sdf::Joint& joint_spec, const Joint<double>& joint,
MultibodyPlant<double>* plant) {
DRAKE_THROW_UNLESS(plant != nullptr);
DRAKE_DEMAND(
joint_spec.Type() == sdf::JointType::BALL ||
joint_spec.Type() == sdf::JointType::UNIVERSAL ||
joint_spec.Type() == sdf::JointType::PRISMATIC ||
joint_spec.Type() == sdf::JointType::REVOLUTE);
DRAKE_DEMAND(joint_spec.Type() == sdf::JointType::BALL ||
joint_spec.Type() == sdf::JointType::UNIVERSAL ||
joint_spec.Type() == sdf::JointType::PRISMATIC ||
joint_spec.Type() == sdf::JointType::REVOLUTE ||
joint_spec.Type() == sdf::JointType::CONTINUOUS);

// Ball joints cannot specify an axis (nor actuation) per SDFormat. However,
// Drake still permits the first axis in order to specify damping, but it
Expand Down Expand Up @@ -469,7 +471,7 @@ void AddJointActuatorFromSpecification(
return;
}

// Prismatic and revolute joints have a single axis.
// Prismatic, revolute, and continuous joints have a single axis.
const double effort_limit = GetEffortLimit(diagnostic, joint_spec, 0);
if (effort_limit != 0) {
const JointActuator<double>& actuator =
Expand Down Expand Up @@ -499,13 +501,14 @@ void AddJointActuatorFromSpecification(
// Extracts the spring stiffness and the spring reference from a joint
// specification and adds a revolute spring force element with the
// corresponding spring reference if the spring stiffness is nonzero.
// Only available for "revolute" joints. The units for spring
// Only available for "revolute" and "continuous" joints. The units for spring
// reference is radians and the units for spring stiffness is N⋅m/rad.
void AddRevoluteSpringFromSpecification(
const sdf::Joint &joint_spec, const RevoluteJoint<double>& joint,
MultibodyPlant<double>* plant) {
DRAKE_THROW_UNLESS(plant != nullptr);
DRAKE_THROW_UNLESS(joint_spec.Type() == sdf::JointType::REVOLUTE);
DRAKE_THROW_UNLESS(joint_spec.Type() == sdf::JointType::REVOLUTE ||
joint_spec.Type() == sdf::JointType::CONTINUOUS);

// Axis specification.
const sdf::JointAxis* axis = joint_spec.Axis();
Expand All @@ -529,15 +532,18 @@ void AddRevoluteSpringFromSpecification(
// Returns joint limits as the tuple (lower_limit, upper_limit,
// velocity_limit, acceleration_limit). The units of the limits depend on the
// particular joint type. For prismatic joints, units are meters for the
// position limits and m/s for the velocity limit. For revolute joints, units
// are radians for the position limits and rad/s for the velocity limit.
// Velocity and acceleration limits are always >= 0. This method throws an
// exception if the joint type is not one of revolute or prismatic.
// position limits and m/s for the velocity limit. For revolute, units
// are radians for the position limits and rad/s for the velocity limit. For
// continuous, positions limits are infinities and velocity limits have units
// rad/s. Velocity and acceleration limits are always >= 0. This method throws
// an exception if the joint type is not one of revolute, prismatic, or
// continuous.
std::tuple<double, double, double, double> ParseJointLimits(
const DiagnosticPolicy& 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::PRISMATIC ||
joint_spec.Type() == sdf::JointType::CONTINUOUS);
// Axis specification.
const sdf::JointAxis* axis = joint_spec.Axis();
if (axis == nullptr) {
Expand All @@ -549,16 +555,24 @@ std::tuple<double, double, double, double> ParseJointLimits(
// http://sdformat.org/spec?ver=1.6&elem=joint#axis_limit.
// Drake marks joints with no limits with ±numeric_limits<double>::infinity()
// and therefore we make the change here.
const double lower_limit =
axis->Lower() == -1.0e16 ?
-std::numeric_limits<double>::infinity() : axis->Lower();
const double upper_limit =
axis->Upper() == 1.0e16 ?
std::numeric_limits<double>::infinity() : axis->Upper();
if (lower_limit > upper_limit) {
throw std::runtime_error(
"The lower limit must be lower (or equal) than the upper limit for "
"joint '" + joint_spec.Name() + "'.");
// Default position limits to infinities which is the case for continuous
// joints.
double lower_limit = -std::numeric_limits<double>::infinity();
double upper_limit = std::numeric_limits<double>::infinity();
if (joint_spec.Type() == sdf::JointType::REVOLUTE ||
joint_spec.Type() == sdf::JointType::PRISMATIC) {
lower_limit = axis->Lower() == -1.0e16
? -std::numeric_limits<double>::infinity()
: axis->Lower();
upper_limit = axis->Upper() == 1.0e16
? std::numeric_limits<double>::infinity()
: axis->Upper();
if (lower_limit > upper_limit) {
throw std::runtime_error(
"The lower limit must be lower (or equal) than the upper limit for "
"joint '" +
joint_spec.Name() + "'.");
}
}

// SDF defaults to -1.0 for joints with no limits, see
Expand Down Expand Up @@ -761,10 +775,21 @@ void AddJointFromSpecification(
break;
}
case sdf::JointType::CONTINUOUS: {
// TODO(#14747) Use an unlimited multibody::RevoluteJoint here.
diagnostic.Error(fmt::format(
"Joint type (continuous) not supported for joint '{}'.",
joint_spec.Name()));
const double damping = ParseJointDamping(diagnostic, joint_spec);
Vector3d axis_J = ExtractJointAxis(diagnostic, model_spec, joint_spec);
std::tie(std::ignore, std::ignore, velocity_limit, acceleration_limit) =
ParseJointLimits(diagnostic, joint_spec);
const auto& joint =
plant->AddJoint<RevoluteJoint>(joint_spec.Name(), parent_body, X_PJ,
child_body, X_CJ, axis_J, 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);
AddRevoluteSpringFromSpecification(joint_spec, joint, plant);
break;
}
case sdf::JointType::SCREW: {
Expand Down
44 changes: 24 additions & 20 deletions multibody/parsing/test/detail_sdf_parser_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -836,9 +836,8 @@ TEST_F(SdfParserTest, JointParsingTest) {
prismatic_joint.acceleration_upper_limits(), Vector1d(10)));

// Limitless revolute joint
DRAKE_EXPECT_NO_THROW(
plant_.GetJointByName<RevoluteJoint>("revolute_joint_no_limits",
instance1));
DRAKE_EXPECT_NO_THROW(plant_.GetJointByName<RevoluteJoint>(
"revolute_joint_no_limits", instance1));
const RevoluteJoint<double>& no_limit_joint =
plant_.GetJointByName<RevoluteJoint>("revolute_joint_no_limits",
instance1);
Expand All @@ -852,8 +851,8 @@ TEST_F(SdfParserTest, 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));

// Ball joint
Expand Down Expand Up @@ -953,6 +952,26 @@ TEST_F(SdfParserTest, JointParsingTest) {
EXPECT_TRUE(CompareMatrices(planar_joint2.position_upper_limits(), inf3));
EXPECT_TRUE(CompareMatrices(planar_joint2.velocity_lower_limits(), neg_inf3));
EXPECT_TRUE(CompareMatrices(planar_joint2.velocity_upper_limits(), inf3));

// Continuous joint
DRAKE_EXPECT_NO_THROW(
plant_.GetJointByName<RevoluteJoint>("continuous_joint", instance1));
const RevoluteJoint<double>& continuous_joint =
plant_.GetJointByName<RevoluteJoint>("continuous_joint", instance1);
EXPECT_EQ(continuous_joint.name(), "continuous_joint");
EXPECT_EQ(continuous_joint.parent_body().name(), "link7");
EXPECT_EQ(continuous_joint.child_body().name(), "link8");
EXPECT_EQ(continuous_joint.revolute_axis(), Vector3d::UnitZ());
EXPECT_TRUE(
CompareMatrices(continuous_joint.position_lower_limits(), neg_inf));
EXPECT_TRUE(CompareMatrices(continuous_joint.position_upper_limits(), inf));
EXPECT_TRUE(
CompareMatrices(continuous_joint.velocity_lower_limits(), neg_inf));
EXPECT_TRUE(CompareMatrices(continuous_joint.velocity_upper_limits(), inf));
EXPECT_TRUE(
CompareMatrices(continuous_joint.acceleration_lower_limits(), neg_inf));
EXPECT_TRUE(
CompareMatrices(continuous_joint.acceleration_upper_limits(), inf));
}

// Tests the error handling for an unsupported joint type (when actuated).
Expand Down Expand Up @@ -1083,21 +1102,6 @@ TEST_F(SdfParserTest, ActuatedBallJointParsingTest) {
ClearDiagnostics();
}

// Tests the error handling for an unsupported joint type.
TEST_F(SdfParserTest, ContinuousJointParsingTest) {
ParseTestString(R"""(
<model name="molly">
<link name="larry" />
<joint name="jerry" type="continuous">
<parent>world</parent>
<child>larry</child>
</joint>
</model>)""");
EXPECT_THAT(FormatFirstError(), ::testing::MatchesRegex(
".*continuous.*not supported.*jerry.*"));
ClearDiagnostics();
}

// Tests the error handling for an unsupported joint type.
TEST_F(SdfParserTest, ScrewJointParsingTest) {
ParseTestString(R"""(
Expand Down
24 changes: 24 additions & 0 deletions multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -193,6 +193,30 @@ Defines an SDF model with various types of joints used for testing the parser.
<drake:parent>frame6</drake:parent>
<drake:damping>0.1 0.1 0.1</drake:damping>
</drake:joint>
<link name="link8">
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
</link>
<joint name="continuous_joint" type="continuous">
<child>link8</child>
<parent>link7</parent>
<axis>
<xyz expressed_in="__model__">0 0 1</xyz>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
</model>
<model name="joint_parsing_test2">
<link name="link6">
Expand Down