-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
planar_gripper_common.cc
226 lines (200 loc) · 9.13 KB
/
planar_gripper_common.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
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
#include "drake/examples/planar_gripper/planar_gripper_common.h"
#include <fstream>
#include <map>
#include <string>
#include <utility>
#include <vector>
#include "drake/common/drake_assert.h"
#include "drake/common/find_resource.h"
#include "drake/multibody/plant/multibody_plant.h"
namespace drake {
namespace examples {
namespace planar_gripper {
using drake::math::RigidTransformd;
using drake::math::RollPitchYawd;
using drake::multibody::MultibodyPlant;
using Eigen::Vector3d;
template <typename T>
void WeldGripperFrames(MultibodyPlant<T>* plant) {
// The finger base links are all welded a fixed distance from the gripper
// frame's origin (Go), lying on the gripper frame's Y-Z plane. We denote the
// gripper frame's Y and Z axes as Gy and Gz.
const double kGripperOriginToBaseDistance = 0.201;
const double kFinger1Angle = M_PI / 3.0;
const double kFinger2Angle = -M_PI / 3.0;
const double kFinger3Angle = M_PI;
// Note: Before welding and with all finger joint angles being zero, all
// finger base links sit at the world origin with the finger pointing along
// the world -Z axis.
// We align the planar gripper coordinate frame G with the world frame W.
const RigidTransformd X_WG = X_WGripper();
// Weld the first finger. Finger base links are arranged equidistant along the
// perimeter of a circle. The first finger is welded kFinger1Angle radians
// from the +Gz-axis. Frames F1, F2, F3 correspond to the base link finger
// frames.
RigidTransformd X_GF1 =
RigidTransformd(Eigen::AngleAxisd(kFinger1Angle, Vector3d::UnitX()),
Vector3d(0, 0, 0)) *
RigidTransformd(math::RotationMatrixd(),
Vector3d(0, 0, kGripperOriginToBaseDistance));
const multibody::Frame<T>& finger1_base_frame =
plant->GetFrameByName("finger1_base");
plant->WeldFrames(plant->world_frame(), finger1_base_frame, X_WG * X_GF1);
// Weld the second finger. The second finger is welded kFinger2Angle radians
// from the +Gz-axis.
RigidTransformd X_GF2 =
RigidTransformd(Eigen::AngleAxisd(kFinger2Angle, Vector3d::UnitX()),
Vector3d(0, 0, 0)) *
RigidTransformd(math::RotationMatrixd(),
Vector3d(0, 0, kGripperOriginToBaseDistance));
const multibody::Frame<T>& finger2_base_frame =
plant->GetFrameByName("finger2_base");
plant->WeldFrames(plant->world_frame(), finger2_base_frame, X_WG * X_GF2);
// Weld the 3rd finger. The third finger is welded kFinger3Angle radians from
// the +Gz-axis.
RigidTransformd X_GF3 =
RigidTransformd(Eigen::AngleAxisd(kFinger3Angle, Vector3d::UnitX()),
Vector3d(0, 0, 0)) *
RigidTransformd(math::RotationMatrixd(),
Vector3d(0, 0, kGripperOriginToBaseDistance));
const multibody::Frame<T>& finger3_base_frame =
plant->GetFrameByName("finger3_base");
plant->WeldFrames(plant->world_frame(), finger3_base_frame, X_WG * X_GF3);
}
// Explicit instantiations.
template void WeldGripperFrames(MultibodyPlant<double>* plant);
/// Build a keyframe matrix for joints in joint_ordering by extracting the
/// appropriate columns from all_keyframes. The interpretation of columns in
/// joint_keyframes are ordered as in joint_ordering.
/// @pre There are as many strings in headers as there are columns in
/// all_keyframes.
/// @pre Every string in joint_ordering is expected to be unique.
/// @pre Every string in joint_ordering is expected to be found in headers.
MatrixX<double> MakeKeyframes(MatrixX<double> all_keyframes,
std::vector<std::string> joint_ordering,
std::vector<std::string> headers) {
// First find the columns in the keyframe data for just the joints in
// joint_ordering.
std::map<std::string, int> joint_name_to_col_index_map;
for (const auto& header_name : joint_ordering) {
auto match_it = std::find(headers.begin(), headers.end(), header_name);
DRAKE_DEMAND(match_it != headers.end());
joint_name_to_col_index_map[header_name] = match_it - headers.begin();
}
// Now create the keyframe matrix.
const int keyframe_count = all_keyframes.rows();
const int kNumFingerJoints = joint_ordering.size();
MatrixX<double> joint_keyframes(keyframe_count, kNumFingerJoints);
for (int i = 0; i < kNumFingerJoints; ++i) {
const std::string& joint_name = joint_ordering[i];
const int all_keyframe_col_index = joint_name_to_col_index_map[joint_name];
joint_keyframes.block(0, i, keyframe_count, 1) =
all_keyframes.block(0, all_keyframe_col_index, keyframe_count, 1);
}
return joint_keyframes;
}
std::pair<MatrixX<double>, std::map<std::string, int>> ParseKeyframes(
const std::string& name, EigenPtr<Vector3<double>> brick_initial_pose) {
const std::string keyframe_path = FindResourceOrThrow(name);
std::fstream file;
file.open(keyframe_path, std::fstream::in);
DRAKE_DEMAND(file.is_open());
// Count the number of lines in the file.
std::string line;
int line_count = 0;
while (!std::getline(file, line).eof()) {
line_count++;
}
const int keyframe_count = line_count - 1;
drake::log()->info("Found {} keyframes", keyframe_count);
// Get the file headers.
file.clear();
file.seekg(0);
std::getline(file, line);
std::stringstream sstream(line);
std::vector<std::string> headers;
std::string token;
while (sstream >> token) {
headers.push_back(token);
}
// Make sure we read the correct number of headers.
const int kNumHeaders = 9;
if (headers.size() != kNumHeaders) {
throw std::runtime_error(
"Unexpected number of headers found in keyframe input file.");
}
// Extract all keyframes (finger and brick)
MatrixX<double> all_keyframes(keyframe_count, headers.size());
for (int i = 0; i < all_keyframes.rows(); ++i) {
for (int j = 0; j < all_keyframes.cols(); ++j) {
file >> all_keyframes(i, j);
}
}
// Find the columns in the keyframe data for just the finger joints and
// create the corresponding keyframe matrix.
std::vector<std::string> finger_joint_ordering = {
"finger1_BaseJoint", "finger2_BaseJoint", "finger3_BaseJoint",
"finger1_MidJoint", "finger2_MidJoint", "finger3_MidJoint"};
MatrixX<double> finger_joint_keyframes =
MakeKeyframes(all_keyframes, finger_joint_ordering, headers);
// Find the columns in the keyframe data for just the brick joints and create
// the corresponding keyframe matrix. Note: Only the first keyframe is used to
// set the brick's initial position. All other brick keyframe data is unused.
std::vector<std::string> brick_joint_ordering = {"brick_translate_y_joint",
"brick_translate_z_joint",
"brick_revolute_x_joint"};
std::map<std::string, int> brick_joint_name_to_col_index_map;
MatrixX<double> brick_joint_keyframes =
MakeKeyframes(all_keyframes, brick_joint_ordering, headers);
// Set the brick's initial pose (expressed in the gripper frame G).
if (brick_initial_pose != EigenPtr<Vector3<double>>(nullptr)) {
(*brick_initial_pose)(0) = brick_joint_keyframes(0, 0); // y-translate
(*brick_initial_pose)(1) = brick_joint_keyframes(0, 1); // z-translate
(*brick_initial_pose)(2) = brick_joint_keyframes(0, 2); // x-revolute
}
finger_joint_keyframes.transposeInPlace();
// Create the finger joint name to row index map.
std::map<std::string, int> finger_joint_name_to_row_index_map;
for (size_t i = 0; i < finger_joint_ordering.size(); i++) {
finger_joint_name_to_row_index_map[finger_joint_ordering[i]] = i;
}
return std::make_pair(finger_joint_keyframes,
finger_joint_name_to_row_index_map);
}
MatrixX<double> ReorderKeyframesForPlant(
const MultibodyPlant<double>& plant, const MatrixX<double> keyframes,
std::map<std::string, int>* finger_joint_name_to_row_index_map) {
DRAKE_DEMAND(finger_joint_name_to_row_index_map != nullptr);
if (static_cast<int>(finger_joint_name_to_row_index_map->size()) !=
keyframes.rows()) {
throw std::runtime_error(
"The number of keyframe rows must match the size of "
"finger_joint_name_to_row_index_map.");
}
if (keyframes.rows() != kNumJoints) {
throw std::runtime_error(
"The number of keyframe rows must match the number of planar-gripper "
"joints");
}
if (plant.num_positions() != kNumJoints) {
throw std::runtime_error(
"The number of plant positions must exactly match the number of "
"planar-gripper joints.");
}
std::map<std::string, int> original_map = *finger_joint_name_to_row_index_map;
MatrixX<double> reordered_keyframes(keyframes);
for (auto iter = original_map.begin(); iter != original_map.end(); ++iter) {
auto joint_vel_start_index =
plant.GetJointByName(iter->first).velocity_start();
reordered_keyframes.row(joint_vel_start_index) =
keyframes.row(iter->second);
(*finger_joint_name_to_row_index_map)[iter->first] = joint_vel_start_index;
}
return reordered_keyframes;
}
const RigidTransformd X_WGripper() {
return math::RigidTransformd::Identity();
}
} // namespace planar_gripper
} // namespace examples
} // namespace drake