27 #include <SimoxUtility/math/convert/pos_mat3f_to_mat4f.h>
28 #include <SimoxUtility/math/convert/rpy_to_mat3f.h>
29 #include <SimoxUtility/math/distance/angle_between.h>
30 #include <SimoxUtility/math/project_to_plane.h>
31 #include <SimoxUtility/shapes/OrientedBox.h>
44 settings->value(
"rsc",
"Armar6StateComponent").toString().toStdString());
45 _gc_topic_name = settings->value(
"rsc",
"GraspCandidatesTopic").toString().toStdString();
54 settings->setValue(
"gct", QString::fromStdString(_gc_topic_name));
64 "rsc",
"Robot State Component",
"*Component");
65 _dialog->addLineEdit(
"gct",
"Grasp Candidates Topic",
"GraspCandidatesTopic");
67 return qobject_cast<SimpleConfigDialog*>(_dialog);
74 _gc_topic_name = _dialog->getLineEditText(
"gct");
85 _ui.doubleSpinBoxBoxTX, _ui.doubleSpinBoxBoxTY, _ui.doubleSpinBoxBoxTZ);
88 _ui.doubleSpinBoxBoxRX, _ui.doubleSpinBoxBoxRY, _ui.doubleSpinBoxBoxRZ);
90 _box_extend.
addWidget(_ui.doubleSpinBoxBoxSizeX);
91 _box_extend.
addWidget(_ui.doubleSpinBoxBoxSizeY);
92 _box_extend.
addWidget(_ui.doubleSpinBoxBoxSizeZ);
94 _transversal_l.
addWidget(_ui.doubleSpinBoxHandLTransvX);
95 _transversal_l.
addWidget(_ui.doubleSpinBoxHandLTransvY);
96 _transversal_l.
addWidget(_ui.doubleSpinBoxHandLTransvZ);
98 _transversal_r.
addWidget(_ui.doubleSpinBoxHandRTransvX);
99 _transversal_r.
addWidget(_ui.doubleSpinBoxHandRTransvY);
100 _transversal_r.
addWidget(_ui.doubleSpinBoxHandRTransvZ);
102 _up_l.
addWidget(_ui.doubleSpinBoxHandLUpX);
103 _up_l.
addWidget(_ui.doubleSpinBoxHandLUpY);
104 _up_l.
addWidget(_ui.doubleSpinBoxHandLUpZ);
106 _up_r.
addWidget(_ui.doubleSpinBoxHandRUpX);
107 _up_r.
addWidget(_ui.doubleSpinBoxHandRUpY);
108 _up_r.
addWidget(_ui.doubleSpinBoxHandRUpZ);
110 _tcp_shift_l.
addWidget(_ui.doubleSpinBoxTCPShiftLX);
111 _tcp_shift_l.
addWidget(_ui.doubleSpinBoxTCPShiftLY);
112 _tcp_shift_l.
addWidget(_ui.doubleSpinBoxTCPShiftLZ);
114 _tcp_shift_r.
addWidget(_ui.doubleSpinBoxTCPShiftRX);
115 _tcp_shift_r.
addWidget(_ui.doubleSpinBoxTCPShiftRY);
116 _tcp_shift_r.
addWidget(_ui.doubleSpinBoxTCPShiftRZ);
118 const std::vector spinboxes{
119 _ui.doubleSpinBoxBoundTransvMax, _ui.doubleSpinBoxBoundTransvMin,
120 _ui.doubleSpinBoxBoundWidthMin, _ui.doubleSpinBoxBoundWidthMax,
121 _ui.doubleSpinBoxBoundDepthMin, _ui.doubleSpinBoxBoundDepthMax,
123 _ui.doubleSpinBoxBoxTX, _ui.doubleSpinBoxBoxTY,
124 _ui.doubleSpinBoxBoxTZ, _ui.doubleSpinBoxBoxRX,
125 _ui.doubleSpinBoxBoxRY, _ui.doubleSpinBoxBoxRZ,
126 _ui.doubleSpinBoxBoxSizeX, _ui.doubleSpinBoxBoxSizeY,
127 _ui.doubleSpinBoxBoxSizeZ,
129 _ui.doubleSpinBoxHandLTransvX, _ui.doubleSpinBoxHandLTransvY,
130 _ui.doubleSpinBoxHandLTransvZ, _ui.doubleSpinBoxHandRTransvX,
131 _ui.doubleSpinBoxHandRTransvY, _ui.doubleSpinBoxHandRTransvZ,
133 _ui.doubleSpinBoxHandLUpX, _ui.doubleSpinBoxHandLUpY,
134 _ui.doubleSpinBoxHandLUpZ, _ui.doubleSpinBoxHandRUpX,
135 _ui.doubleSpinBoxHandRUpY, _ui.doubleSpinBoxHandRUpZ,
137 _ui.doubleSpinBoxTCPShiftLX, _ui.doubleSpinBoxTCPShiftLY,
138 _ui.doubleSpinBoxTCPShiftLZ, _ui.doubleSpinBoxTCPShiftRX,
139 _ui.doubleSpinBoxTCPShiftRY, _ui.doubleSpinBoxTCPShiftRZ};
140 for (QDoubleSpinBox* ptr : spinboxes)
143 qOverload<double>(&QDoubleSpinBox::valueChanged),
148 const std::vector checkboxes{
149 _ui.checkBoxGraspLPosXPosY,
150 _ui.checkBoxGraspLPosXPosZ,
151 _ui.checkBoxGraspLPosXNegY,
152 _ui.checkBoxGraspLPosXNegZ,
154 _ui.checkBoxGraspLNegXPosY,
155 _ui.checkBoxGraspLNegXPosZ,
156 _ui.checkBoxGraspLNegXNegY,
157 _ui.checkBoxGraspLNegXNegZ,
159 _ui.checkBoxGraspLPosYPosX,
160 _ui.checkBoxGraspLPosYPosZ,
161 _ui.checkBoxGraspLPosYNegX,
162 _ui.checkBoxGraspLPosYNegZ,
164 _ui.checkBoxGraspLNegYPosX,
165 _ui.checkBoxGraspLNegYPosZ,
166 _ui.checkBoxGraspLNegYNegX,
167 _ui.checkBoxGraspLNegYNegZ,
169 _ui.checkBoxGraspLPosZPosX,
170 _ui.checkBoxGraspLPosZPosY,
171 _ui.checkBoxGraspLPosZNegX,
172 _ui.checkBoxGraspLPosZNegY,
174 _ui.checkBoxGraspLNegZPosX,
175 _ui.checkBoxGraspLNegZPosY,
176 _ui.checkBoxGraspLNegZNegX,
177 _ui.checkBoxGraspLNegZNegY,
180 _ui.checkBoxGraspRPosXPosY,
181 _ui.checkBoxGraspRPosXPosZ,
182 _ui.checkBoxGraspRPosXNegY,
183 _ui.checkBoxGraspRPosXNegZ,
185 _ui.checkBoxGraspRNegXPosY,
186 _ui.checkBoxGraspRNegXPosZ,
187 _ui.checkBoxGraspRNegXNegY,
188 _ui.checkBoxGraspRNegXNegZ,
190 _ui.checkBoxGraspRPosYPosX,
191 _ui.checkBoxGraspRPosYPosZ,
192 _ui.checkBoxGraspRPosYNegX,
193 _ui.checkBoxGraspRPosYNegZ,
195 _ui.checkBoxGraspRNegYPosX,
196 _ui.checkBoxGraspRNegYPosZ,
197 _ui.checkBoxGraspRNegYNegX,
198 _ui.checkBoxGraspRNegYNegZ,
200 _ui.checkBoxGraspRPosZPosX,
201 _ui.checkBoxGraspRPosZPosY,
202 _ui.checkBoxGraspRPosZNegX,
203 _ui.checkBoxGraspRPosZNegY,
205 _ui.checkBoxGraspRNegZPosX,
206 _ui.checkBoxGraspRNegZPosY,
207 _ui.checkBoxGraspRNegZNegX,
208 _ui.checkBoxGraspRNegZNegY};
209 for (QCheckBox* ptr : checkboxes)
211 ptr->setChecked(
true);
220 std::lock_guard guard{_mutex};
221 _robot =
addRobot(
"state robot", VirtualRobot::RobotIO::eStructure);
222 getTopic(_gc_topic, _gc_topic_name);
224 QMetaObject::invokeMethod(
this,
"update", Qt::QueuedConnection);
231 std::lock_guard guard{_mutex};
238 _box_extend.
get<Eigen::Vector3f>()};
244 auto& l = b2gc.side(
"Left");
245 auto& r = b2gc.side(
"Right");
247 l.hand_transverse = _transversal_l.
get<Eigen::Vector3f>().normalized();
248 r.hand_transverse = _transversal_r.
get<Eigen::Vector3f>().normalized();
249 l.hand_up = _up_l.
get<Eigen::Vector3f>().normalized();
250 r.hand_up = _up_r.
get<Eigen::Vector3f>().normalized();
251 l.tcp_shift = _tcp_shift_l.
get<Eigen::Vector3f>().normalized();
252 r.tcp_shift = _tcp_shift_r.
get<Eigen::Vector3f>().normalized();
266 side_enabled_l[ba::pos_x_pos_y] = _ui.checkBoxGraspLPosXPosY->isChecked();
267 side_enabled_l[ba::pos_x_pos_z] = _ui.checkBoxGraspLPosXPosZ->isChecked();
268 side_enabled_l[ba::pos_x_neg_y] = _ui.checkBoxGraspLPosXNegY->isChecked();
269 side_enabled_l[ba::pos_x_neg_z] = _ui.checkBoxGraspLPosXNegZ->isChecked();
271 side_enabled_l[ba::neg_x_pos_y] = _ui.checkBoxGraspLNegXPosY->isChecked();
272 side_enabled_l[ba::neg_x_pos_z] = _ui.checkBoxGraspLNegXPosZ->isChecked();
273 side_enabled_l[ba::neg_x_neg_y] = _ui.checkBoxGraspLNegXNegY->isChecked();
274 side_enabled_l[ba::neg_x_neg_z] = _ui.checkBoxGraspLNegXNegZ->isChecked();
276 side_enabled_l[ba::pos_y_pos_x] = _ui.checkBoxGraspLPosYPosX->isChecked();
277 side_enabled_l[ba::pos_y_pos_z] = _ui.checkBoxGraspLPosYPosZ->isChecked();
278 side_enabled_l[ba::pos_y_neg_x] = _ui.checkBoxGraspLPosYNegX->isChecked();
279 side_enabled_l[ba::pos_y_neg_z] = _ui.checkBoxGraspLPosYNegZ->isChecked();
281 side_enabled_l[ba::neg_y_pos_x] = _ui.checkBoxGraspLNegYPosX->isChecked();
282 side_enabled_l[ba::neg_y_pos_z] = _ui.checkBoxGraspLNegYPosZ->isChecked();
283 side_enabled_l[ba::neg_y_neg_x] = _ui.checkBoxGraspLNegYNegX->isChecked();
284 side_enabled_l[ba::neg_y_neg_z] = _ui.checkBoxGraspLNegYNegZ->isChecked();
286 side_enabled_l[ba::pos_z_pos_x] = _ui.checkBoxGraspLPosZPosX->isChecked();
287 side_enabled_l[ba::pos_z_pos_y] = _ui.checkBoxGraspLPosZPosY->isChecked();
288 side_enabled_l[ba::pos_z_neg_x] = _ui.checkBoxGraspLPosZNegX->isChecked();
289 side_enabled_l[ba::pos_z_neg_y] = _ui.checkBoxGraspLPosZNegY->isChecked();
291 side_enabled_l[ba::neg_z_pos_x] = _ui.checkBoxGraspLNegZPosX->isChecked();
292 side_enabled_l[ba::neg_z_pos_y] = _ui.checkBoxGraspLNegZPosY->isChecked();
293 side_enabled_l[ba::neg_z_neg_x] = _ui.checkBoxGraspLNegZNegX->isChecked();
294 side_enabled_l[ba::neg_z_neg_y] = _ui.checkBoxGraspLNegZNegY->isChecked();
299 side_enabled_r[ba::pos_x_pos_y] = _ui.checkBoxGraspRPosXPosY->isChecked();
300 side_enabled_r[ba::pos_x_pos_z] = _ui.checkBoxGraspRPosXPosZ->isChecked();
301 side_enabled_r[ba::pos_x_neg_y] = _ui.checkBoxGraspRPosXNegY->isChecked();
302 side_enabled_r[ba::pos_x_neg_z] = _ui.checkBoxGraspRPosXNegZ->isChecked();
304 side_enabled_r[ba::neg_x_pos_y] = _ui.checkBoxGraspRNegXPosY->isChecked();
305 side_enabled_r[ba::neg_x_pos_z] = _ui.checkBoxGraspRNegXPosZ->isChecked();
306 side_enabled_r[ba::neg_x_neg_y] = _ui.checkBoxGraspRNegXNegY->isChecked();
307 side_enabled_r[ba::neg_x_neg_z] = _ui.checkBoxGraspRNegXNegZ->isChecked();
309 side_enabled_r[ba::pos_y_pos_x] = _ui.checkBoxGraspRPosYPosX->isChecked();
310 side_enabled_r[ba::pos_y_pos_z] = _ui.checkBoxGraspRPosYPosZ->isChecked();
311 side_enabled_r[ba::pos_y_neg_x] = _ui.checkBoxGraspRPosYNegX->isChecked();
312 side_enabled_r[ba::pos_y_neg_z] = _ui.checkBoxGraspRPosYNegZ->isChecked();
314 side_enabled_r[ba::neg_y_pos_x] = _ui.checkBoxGraspRNegYPosX->isChecked();
315 side_enabled_r[ba::neg_y_pos_z] = _ui.checkBoxGraspRNegYPosZ->isChecked();
316 side_enabled_r[ba::neg_y_neg_x] = _ui.checkBoxGraspRNegYNegX->isChecked();
317 side_enabled_r[ba::neg_y_neg_z] = _ui.checkBoxGraspRNegYNegZ->isChecked();
319 side_enabled_r[ba::pos_z_pos_x] = _ui.checkBoxGraspRPosZPosX->isChecked();
320 side_enabled_r[ba::pos_z_pos_y] = _ui.checkBoxGraspRPosZPosY->isChecked();
321 side_enabled_r[ba::pos_z_neg_x] = _ui.checkBoxGraspRPosZNegX->isChecked();
322 side_enabled_r[ba::pos_z_neg_y] = _ui.checkBoxGraspRPosZNegY->isChecked();
324 side_enabled_r[ba::neg_z_pos_x] = _ui.checkBoxGraspRNegZPosX->isChecked();
325 side_enabled_r[ba::neg_z_pos_y] = _ui.checkBoxGraspRNegZPosY->isChecked();
326 side_enabled_r[ba::neg_z_neg_x] = _ui.checkBoxGraspRNegZNegX->isChecked();
327 side_enabled_r[ba::neg_z_neg_y] = _ui.checkBoxGraspRNegZNegY->isChecked();
334 layer_box.
add(
viz::Box{
"box"}.
set(box).transformPose(_robot->getGlobalPose()));
336 gc_draw.draw(b2gc.side(
"Left"), layer_hand_vec);
337 gc_draw.draw(b2gc.side(
"Right"), layer_hand_vec);
339 grasping::GraspCandidateSeq graps;
340 const auto consume_l = [&](
const auto& g)
342 graps.emplace_back(g.make_candidate(
getName()));
343 gc_draw.draw(graps.back(), layer_l);
345 const auto consume_r = [&](
const auto& g)
347 graps.emplace_back(g.make_candidate(
getName()));
348 gc_draw.draw(graps.back(), layer_r);
350 b2gc.center_grasps(box,
"Left", lim, consume_l, side_enabled_l);
351 b2gc.center_grasps(box,
"Right", lim, consume_r, side_enabled_r);
353 arviz.
commit({layer_l, layer_r, layer_hand_vec, layer_box});
355 _gc_topic->reportGraspCandidates(
getName(), graps);