-
Notifications
You must be signed in to change notification settings - Fork 1.3k
/
compass_gait_geometry.cc
154 lines (133 loc) · 5.98 KB
/
compass_gait_geometry.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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
#include "drake/examples/compass_gait/compass_gait_geometry.h"
#include <memory>
#include <fmt/format.h>
#include "drake/examples/compass_gait/gen/compass_gait_params.h"
#include "drake/geometry/geometry_frame.h"
#include "drake/geometry/geometry_ids.h"
#include "drake/geometry/geometry_instance.h"
#include "drake/geometry/geometry_roles.h"
#include "drake/math/rigid_transform.h"
#include "drake/math/roll_pitch_yaw.h"
#include "drake/math/rotation_matrix.h"
namespace drake {
namespace examples {
namespace compass_gait {
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::VectorXd;
using geometry::Box;
using geometry::Cylinder;
using geometry::GeometryFrame;
using geometry::GeometryId;
using geometry::GeometryInstance;
using geometry::MakePhongIllustrationProperties;
using geometry::Sphere;
using std::make_unique;
const CompassGaitGeometry* CompassGaitGeometry::AddToBuilder(
systems::DiagramBuilder<double>* builder,
const systems::OutputPort<double>& floating_base_state_port,
const CompassGaitParams<double>& compass_gait_params,
geometry::SceneGraph<double>* scene_graph) {
DRAKE_THROW_UNLESS(builder != nullptr);
DRAKE_THROW_UNLESS(scene_graph != nullptr);
auto compass_gait_geometry =
builder->AddSystem(std::unique_ptr<CompassGaitGeometry>(
new CompassGaitGeometry(compass_gait_params, scene_graph)));
builder->Connect(floating_base_state_port,
compass_gait_geometry->get_input_port(0));
builder->Connect(
compass_gait_geometry->get_output_port(0),
scene_graph->get_source_pose_port(compass_gait_geometry->source_id_));
return compass_gait_geometry;
}
CompassGaitGeometry::CompassGaitGeometry(
const CompassGaitParams<double>& params,
geometry::SceneGraph<double>* scene_graph) {
DRAKE_THROW_UNLESS(scene_graph != nullptr);
source_id_ = scene_graph->RegisterSource();
// Note: the floating-base state output from CompassGait uses RPY (12
// dims) + 2 for the position/velocity of the right leg.
this->DeclareVectorInputPort("floating_base_state", 14);
this->DeclareAbstractOutputPort("geometry_pose",
&CompassGaitGeometry::OutputGeometryPose);
// Add the ramp.
// Positioned so that the top center of the box is at x=y=z=0.
GeometryId id = scene_graph->RegisterAnchoredGeometry(
source_id_,
make_unique<GeometryInstance>(
math::RigidTransformd(
math::RotationMatrixd::MakeYRotation(params.slope())) *
math::RigidTransformd(Vector3d(0, 0, -10. / 2.)),
make_unique<Box>(100, 1, 10), "ramp"));
// Color is "desert sand" according to htmlcsscolor.com.
scene_graph->AssignRole(
source_id_, id,
MakePhongIllustrationProperties(Vector4d(0.9297, 0.7930, 0.6758, 1)));
left_leg_frame_id_ =
scene_graph->RegisterFrame(source_id_, GeometryFrame("left_leg"));
right_leg_frame_id_ =
scene_graph->RegisterFrame(source_id_, left_leg_frame_id_,
GeometryFrame("right_leg"));
// Add the hip. Both legs have the same origin of rotation and the hip
// geometry is rotationally symmetric, so we opt to arbitrarily attach it
// to the left leg.
const double hip_mass_radius = .1;
id = scene_graph->RegisterGeometry(
source_id_, left_leg_frame_id_,
make_unique<GeometryInstance>(
math::RigidTransformd(math::RotationMatrixd::MakeXRotation(M_PI_2)),
make_unique<Sphere>(hip_mass_radius), "hip"));
scene_graph->AssignRole(
source_id_, id, MakePhongIllustrationProperties(Vector4d(0, 1, 0, 1)));
// Scale the leg mass geometry relative to the hip mass (assuming constant
// density).
const double leg_mass_radius = std::cbrt(
std::pow(hip_mass_radius, 3) * params.mass_leg() / params.mass_hip());
// Add the left leg (which is attached to the hip).
id = scene_graph->RegisterGeometry(
source_id_, left_leg_frame_id_,
make_unique<GeometryInstance>(
math::RigidTransformd(Vector3d(0, 0, -params.length_leg() / 2.)),
make_unique<Cylinder>(0.0075, params.length_leg()), "left_leg"));
scene_graph->AssignRole(
source_id_, id, MakePhongIllustrationProperties(Vector4d(1, 0, 0, 1)));
id = scene_graph->RegisterGeometry(
source_id_, left_leg_frame_id_,
make_unique<GeometryInstance>(
math::RigidTransformd(Vector3d(0, 0, -params.center_of_mass_leg())),
make_unique<Sphere>(leg_mass_radius), "left_leg_mass"));
scene_graph->AssignRole(
source_id_, id, MakePhongIllustrationProperties(Vector4d(1, 0, 0, 1)));
// Add the right leg.
id = scene_graph->RegisterGeometry(
source_id_, right_leg_frame_id_,
make_unique<GeometryInstance>(
math::RigidTransformd(Vector3d(0, 0, -params.length_leg() / 2.)),
make_unique<Cylinder>(0.0075, params.length_leg()), "right_leg"));
scene_graph->AssignRole(
source_id_, id, MakePhongIllustrationProperties(Vector4d(0, 0, 1, 1)));
id = scene_graph->RegisterGeometry(
source_id_, right_leg_frame_id_,
make_unique<GeometryInstance>(
math::RigidTransformd(Vector3d(0, 0, -params.center_of_mass_leg())),
make_unique<Sphere>(leg_mass_radius), "right_leg_mass"));
scene_graph->AssignRole(
source_id_, id, MakePhongIllustrationProperties(Vector4d(0, 0, 1, 1)));
}
CompassGaitGeometry::~CompassGaitGeometry() = default;
void CompassGaitGeometry::OutputGeometryPose(
const systems::Context<double>& context,
geometry::FramePoseVector<double>* poses) const {
DRAKE_DEMAND(left_leg_frame_id_.is_valid());
DRAKE_DEMAND(right_leg_frame_id_.is_valid());
const VectorXd& input = get_input_port(0).Eval(context);
const math::RigidTransformd left_pose(
math::RollPitchYawd(input.segment<3>(3)), input.head<3>());
const double hip_angle = input[6];
const math::RigidTransformd right_pose(
math::RotationMatrixd::MakeYRotation(hip_angle));
*poses = {{left_leg_frame_id_, left_pose}, {right_leg_frame_id_, right_pose}};
}
} // namespace compass_gait
} // namespace examples
} // namespace drake