-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
populate_cylinder_plant_test.cc
90 lines (72 loc) · 3.32 KB
/
populate_cylinder_plant_test.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#include "drake/examples/multibody/cylinder_with_multicontact/populate_cylinder_plant.h"
#include <limits>
#include <memory>
#include <gtest/gtest.h>
#include "drake/common/eigen_types.h"
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/systems/framework/diagram_builder.h"
namespace drake {
namespace examples {
namespace multibody {
namespace cylinder_with_multicontact {
namespace {
using drake::multibody::AddMultibodyPlantSceneGraph;
using drake::multibody::CoulombFriction;
using drake::multibody::RigidBody;
using drake::multibody::SpatialInertia;
using drake::systems::DiagramBuilder;
using Eigen::Vector3d;
GTEST_TEST(PopulateCylinderPlant, VerifyPlant) {
// Plant's parameters.
const double radius = 0.05; // The cylinder's radius, m
const double mass = 0.1; // The cylinder's mass, kg
const double g = 9.81; // Acceleration of gravity, m/s^2
const double length = 4.0 * radius; // The cylinder's length, m.
const CoulombFriction<double> coulomb_friction(
0.5 /* static friction */,
0.5 /* dynamic friction */);
// Time stepping step size.
const double time_step = 1.0e-3;
// The builder is garbage; we won't be building the diagram, we just need
// this to use AddMultibodyPlantSceneGraph.
DiagramBuilder<double> builder;
auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, time_step);
PopulateCylinderPlant(radius, length, mass, coulomb_friction,
-g * Vector3d::UnitZ(), &plant);
plant.Finalize();
EXPECT_EQ(plant.num_velocities(), 6);
EXPECT_EQ(plant.num_positions(), 7);
EXPECT_EQ(plant.time_step(), time_step);
EXPECT_TRUE(plant.geometry_source_is_registered());
ASSERT_TRUE(plant.HasBodyNamed("Cylinder"));
const RigidBody<double>& cylinder =
plant.GetRigidBodyByName("Cylinder");
EXPECT_EQ(cylinder.default_mass(), mass);
// Verify the value of the inertial properties for the cylinder.
const SpatialInertia<double>& M_Bcm_B = cylinder.default_spatial_inertia();
// Unit inertia about the revolute axis, the z axis, of the cylinder.
const double G_axis = radius * radius / 2.0;
// Unit inertia about any axis perpendicular to the revolute axis passing
// through the COM.
const double G_perp = (3.0 * radius * radius + length * length) / 12.0;
EXPECT_EQ(M_Bcm_B.get_mass(), mass);
EXPECT_EQ(M_Bcm_B.get_com(), Vector3d::Zero());
// An empirical tolerance: two bits = 2^2 times machine epsilon.
const double kTolerance = 4 * std::numeric_limits<double>::epsilon();
EXPECT_TRUE(CompareMatrices(M_Bcm_B.get_unit_inertia().get_moments(),
Vector3d(G_perp, G_perp, G_axis), kTolerance));
EXPECT_EQ(M_Bcm_B.get_unit_inertia().get_products(), Vector3d::Zero());
// TODO(amcastro-tri): Verify geometry including:
// - The number of registered geometries.
// - Whether it is for visualization or contact modeling.
// - Obtain geometry specifics; type, radius, length, etc.
// - Obtain friction coefficients for each geometry.
// However, we need proper SceneGraph/MBP APIs to enable this.
// TODO(amcastro-tri): verify gravity is correct.
// We need an API to retrieve the gravity force element, #9253.
}
} // namespace
} // namespace cylinder_with_multicontact
} // namespace multibody
} // namespace examples
} // namespace drake