Skip to content

Commit

Permalink
Added support for revolute joint spring in multibody plant parser (#1…
Browse files Browse the repository at this point in the history 
…2992)

* Added support for revolute joint spring in multibody plant parser

Reads in <spring_stiffness> and <spring_reference> tags from joints
in SDF files
  • Loading branch information
@mntan3
mntan3 authored Apr 11, 2020
1 parent 30e0138 commit 712424d
Show file tree
Hide file tree
Showing 3 changed files with 209 additions and 0 deletions.
32 changes: 32 additions & 0 deletions multibody/parsing/detail_sdf_parser.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include "drake/multibody/tree/fixed_offset_frame.h"
#include "drake/multibody/tree/prismatic_joint.h"
#include "drake/multibody/tree/revolute_joint.h"
#include "drake/multibody/tree/revolute_spring.h"
#include "drake/multibody/tree/uniform_gravity_field_element.h"
#include "drake/multibody/tree/weld_joint.h"

Expand Down Expand Up @@ -227,6 +228,36 @@ 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
// 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);

// Axis specification.
const sdf::JointAxis* axis = joint_spec.Axis();
if (axis == nullptr) {
throw std::runtime_error(
"An axis must be specified for joint '" + joint_spec.Name() + "'");
}

const double spring_reference = axis->SpringReference();
const double spring_stiffness = axis->SpringStiffness();

// We don't add a force element if stiffness is zero.
// If a negative value is passed in, RevoluteSpring will
// throw an error.
if (spring_stiffness != 0) {
plant->AddForceElement<RevoluteSpring>(
joint, spring_reference, spring_stiffness);
}
}

// Returns joint limits as the tuple (lower_limit, upper_limit,
// velocity_limit). The units of the limits depend on the particular joint
// type. For prismatic joints, units are meters for the position limits and
Expand Down Expand Up @@ -353,6 +384,7 @@ void AddJointFromSpecification(
plant->get_mutable_joint(joint.index()).set_velocity_limits(
Vector1d(-velocity_limit), Vector1d(velocity_limit));
AddJointActuatorFromSpecification(joint_spec, joint, plant);
AddRevoluteSpringFromSpecification(joint_spec, joint, plant);
break;
}
default: {
Expand Down
49 changes: 49 additions & 0 deletions multibody/parsing/test/detail_sdf_parser_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -519,6 +519,55 @@ GTEST_TEST(MultibodyPlantSdfParserTest, JointActuatorParsingTest) {
std::logic_error, "There is no joint actuator named '.*' in the model.");
}

// Verifies that the SDF parser parses the revolute spring parameters correctly.
GTEST_TEST(MultibodyPlantSdfParserTest, RevoluteSpringParsingTest) {
MultibodyPlant<double> plant(0.0);

const std::string full_name = FindResourceOrThrow(
"drake/multibody/parsing/test/sdf_parser_test/"
"revolute_spring_parsing_test.sdf");
PackageMap package_map;
package_map.PopulateUpstreamToDrake(full_name);

// Reads in the SDF file.
AddModelFromSdfFile(full_name, "", package_map, &plant, nullptr);
plant.Finalize();

// Plant should have a UniformGravityFieldElement by default.
// Our test contains two joints that have nonzero stiffness
// and two joints that have zero stiffness. We only add a
// spring for nonzero stiffness, so only two spring forces
// should have been added.
constexpr int kNumSpringForces = 2;
DRAKE_DEMAND(plant.num_force_elements() == kNumSpringForces + 1);

// In these two tests, we verify that the generalized forces are
// correct for both springs. The first spring has a nonzero reference
// of 1.0 radians so should have nonzero torque. The second spring
// has a zero reference, so it should have no applied torque.
MultibodyForces<double> forces(plant);
auto context = plant.CreateDefaultContext();
constexpr int kGeneralizedForcesSize = 10;
Matrix2X<double> 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;
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);
const auto& nonzero_reference = plant.GetForceElement(force_index);
forces.SetZero();
nonzero_reference.CalcAndAddForceContribution(
*context, plant.EvalPositionKinematics(*context),
plant.EvalVelocityKinematics(*context), &forces);

const VectorX<double>& generalized_forces = forces.generalized_forces();
EXPECT_TRUE(CompareMatrices(generalized_forces,
expected_generalized_forces.row(i).transpose(),
kEps, MatrixCompareType::relative));
}
}

GTEST_TEST(SdfParser, TestSupportedFrames) {
// Test `//link/pose[@relative_to]`.
ParseTestString(R"(
Expand Down
128 changes: 128 additions & 0 deletions multibody/parsing/test/sdf_parser_test/revolute_spring_parsing_test.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,128 @@
<?xml version="1.0" ?>

<!--
Defines an SDF model with revolute joints for testing the revolute spring
parser. The only focus is that the parser reads in the values from the
<spring_stiffness> and <spring_reference> tags properly and assigns a
default value of zero when the tag is not present. This is an accompanying
file to detail_sdf_parser_test.cc and therefore they must be kept in sync
with each other.
-->

<sdf version='1.6'>
<model name='revolute_spring_parsing_test'>
<link name='link1'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
</link>
<link name='link2'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
</link>
<joint name='revolute_spring_reference_and_stiffness' type='revolute'>
<child>link2</child>
<parent>link1</parent>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<spring_reference>1</spring_reference>
<spring_stiffness>5</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='link3'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
</link>
<joint name='revolute_spring_only_reference' type='revolute'>
<child>link3</child>
<parent>link2</parent>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<spring_reference>1</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='link4'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
</link>
<joint name='revolute_spring_only_stiffness' type='revolute'>
<child>link4</child>
<parent>link3</parent>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>5</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='link5'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1.0</iyy>
<iyz>0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
</link>
<joint name='revolute_spring_no_spring' type='revolute'>
<child>link5</child>
<parent>link4</parent>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
</model>
</sdf>

0 comments on commit 712424d

Please sign in to comment.