Skip to content

Commit

Permalink
controllers: Reorder inverse dynamics test cases
Browse files Browse the repository at this point in the history
This is to work around a bug in kcov. See RobotLocomotion#14424 for more info.
  • Loading branch information
Andrés K. Valenzuela committed Aug 4, 2021
1 parent 54b30fc commit 10cc635
Showing 1 changed file with 32 additions and 32 deletions.
64 changes: 32 additions & 32 deletions systems/controllers/test/inverse_dynamics_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,38 @@ class InverseDynamicsTest : public ::testing::Test {
std::unique_ptr<SystemOutput<double>> output_;
};

// Tests that inverse dynamics returns the expected torque for a given state and
// desired acceleration for the iiwa arm.
TEST_F(InverseDynamicsTest, InverseDynamicsTest) {
auto mbp = std::make_unique<MultibodyPlant<double>>(0.0);
const std::string full_name = drake::FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf");
multibody::Parser(mbp.get()).AddModelFromFile(full_name);
mbp->WeldFrames(mbp->world_frame(),
mbp->GetFrameByName("iiwa_link_0"));

// Add gravitational forces, finalize the model, and transfer ownership.
mbp->mutable_gravity_field().set_gravity_vector(
-9.8 * Vector3<double>::UnitZ());
mbp->Finalize();
Init(std::move(mbp),
InverseDynamics<double>::InverseDynamicsMode::kInverseDynamics);

Eigen::VectorXd q = Eigen::VectorXd::Zero(7);
Eigen::VectorXd v = Eigen::VectorXd::Zero(7);
Eigen::VectorXd vd_d = Eigen::VectorXd::Zero(7);
for (int i = 0; i < 7; ++i) {
q[i] = i * 0.1 - 0.3;
v[i] = i - 3;
vd_d[i] = i - 3;
}

// Check that gravity is modeled.
EXPECT_TRUE(GravityModeled(q));

CheckTorque(q, v, vd_d);
}

// Tests that the expected value of the gravity compensating torque and the
// value computed by the InverseDynamics in pure gravity compensation mode
// for a given joint configuration of the KUKA IIWA Arm are identical.
Expand Down Expand Up @@ -176,38 +208,6 @@ TEST_F(InverseDynamicsTest, GravityCompensationTest) {
CheckGravityTorque(robot_position);
}

// Tests that inverse dynamics returns the expected torque for a given state and
// desired acceleration for the iiwa arm.
TEST_F(InverseDynamicsTest, InverseDynamicsTest) {
auto mbp = std::make_unique<MultibodyPlant<double>>(0.0);
const std::string full_name = drake::FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf");
multibody::Parser(mbp.get()).AddModelFromFile(full_name);
mbp->WeldFrames(mbp->world_frame(),
mbp->GetFrameByName("iiwa_link_0"));

// Add gravitational forces, finalize the model, and transfer ownership.
mbp->mutable_gravity_field().set_gravity_vector(
-9.8 * Vector3<double>::UnitZ());
mbp->Finalize();
Init(std::move(mbp),
InverseDynamics<double>::InverseDynamicsMode::kInverseDynamics);

Eigen::VectorXd q = Eigen::VectorXd::Zero(7);
Eigen::VectorXd v = Eigen::VectorXd::Zero(7);
Eigen::VectorXd vd_d = Eigen::VectorXd::Zero(7);
for (int i = 0; i < 7; ++i) {
q[i] = i * 0.1 - 0.3;
v[i] = i - 3;
vd_d[i] = i - 3;
}

// Check that gravity is modeled.
EXPECT_TRUE(GravityModeled(q));

CheckTorque(q, v, vd_d);
}

} // namespace
} // namespace controllers
} // namespace systems
Expand Down

0 comments on commit 10cc635

Please sign in to comment.