Skip to content

Commit

Permalink
[parsing] Support continuous joint when parsing SDFormat
Browse files Browse the repository at this point in the history
  • Loading branch information
@xuchenhan-tri
xuchenhan-tri committed Sep 21, 2022
1 parent 35e6290 commit e30b19b
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 62 deletions.
83 changes: 54 additions & 29 deletions multibody/parsing/detail_sdf_parser.cc
View file
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 limtis to inifinities 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(lower_limit, upper_limit, 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,
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);
AddRevoluteSpringFromSpecification(joint_spec, joint, plant);
break;
}
case sdf::JointType::SCREW: {
Expand Down
50 changes: 18 additions & 32 deletions multibody/parsing/test/detail_sdf_parser_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -835,26 +835,27 @@ TEST_F(SdfParserTest, JointParsingTest) {
EXPECT_TRUE(CompareMatrices(
prismatic_joint.acceleration_upper_limits(), Vector1d(10)));

// Limitless revolute joint
// Continuous joint
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);
EXPECT_EQ(no_limit_joint.name(), "revolute_joint_no_limits");
EXPECT_EQ(no_limit_joint.parent_body().name(), "link3");
EXPECT_EQ(no_limit_joint.child_body().name(), "link4");
EXPECT_EQ(no_limit_joint.revolute_axis(), Vector3d::UnitZ());
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(), "link3");
EXPECT_EQ(continuous_joint.child_body().name(), "link4");
EXPECT_EQ(continuous_joint.revolute_axis(), Vector3d::UnitZ());
const Vector1d inf(std::numeric_limits<double>::infinity());
const Vector1d neg_inf(-std::numeric_limits<double>::infinity());
EXPECT_TRUE(CompareMatrices(no_limit_joint.position_lower_limits(), neg_inf));
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_upper_limits(), inf));
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));

// Ball joint
DRAKE_EXPECT_NO_THROW(plant_.GetJointByName<BallRpyJoint>("ball_joint",
Expand Down Expand Up @@ -1083,21 +1084,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
2 changes: 1 addition & 1 deletion multibody/parsing/test/sdf_parser_test/joint_parsing_test.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ Defines an SDF model with various types of joints used for testing the parser.
</inertia>
</inertial>
</link>
<joint name="revolute_joint_no_limits" type="revolute">
<joint name="continuous_joint" type="continuous">
<child>link4</child>
<parent>link3</parent>
<axis>
Expand Down

0 comments on commit e30b19b

Please sign in to comment.