25 #include <SimoxUtility/shapes/OrientedBox.h>
26 #include <SimoxUtility/math/convert/rpy_to_mat3f.h>
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/distance/angle_between.h>
31 #include <SimoxUtility/math/project_to_plane.h>
45 _gc_topic_name = settings->value(
"rsc",
"GraspCandidatesTopic").toString().toStdString();
51 settings->setValue(
"gct", QString::fromStdString(_gc_topic_name));
60 _dialog->addLineEdit(
"gct",
"Grasp Candidates Topic",
"GraspCandidatesTopic");
62 return qobject_cast<SimpleConfigDialog*>(_dialog);
67 _gc_topic_name = _dialog->getLineEditText(
"gct");
78 _ui.doubleSpinBoxBoxTX,
79 _ui.doubleSpinBoxBoxTY,
80 _ui.doubleSpinBoxBoxTZ
84 _ui.doubleSpinBoxBoxRX,
85 _ui.doubleSpinBoxBoxRY,
86 _ui.doubleSpinBoxBoxRZ
89 _box_extend.
addWidget(_ui.doubleSpinBoxBoxSizeX);
90 _box_extend.
addWidget(_ui.doubleSpinBoxBoxSizeY);
91 _box_extend.
addWidget(_ui.doubleSpinBoxBoxSizeZ);
93 _transversal_l.
addWidget(_ui.doubleSpinBoxHandLTransvX);
94 _transversal_l.
addWidget(_ui.doubleSpinBoxHandLTransvY);
95 _transversal_l.
addWidget(_ui.doubleSpinBoxHandLTransvZ);
97 _transversal_r.
addWidget(_ui.doubleSpinBoxHandRTransvX);
98 _transversal_r.
addWidget(_ui.doubleSpinBoxHandRTransvY);
99 _transversal_r.
addWidget(_ui.doubleSpinBoxHandRTransvZ);
101 _up_l.
addWidget(_ui.doubleSpinBoxHandLUpX);
102 _up_l.
addWidget(_ui.doubleSpinBoxHandLUpY);
103 _up_l.
addWidget(_ui.doubleSpinBoxHandLUpZ);
105 _up_r.
addWidget(_ui.doubleSpinBoxHandRUpX);
106 _up_r.
addWidget(_ui.doubleSpinBoxHandRUpY);
107 _up_r.
addWidget(_ui.doubleSpinBoxHandRUpZ);
109 _tcp_shift_l.
addWidget(_ui.doubleSpinBoxTCPShiftLX);
110 _tcp_shift_l.
addWidget(_ui.doubleSpinBoxTCPShiftLY);
111 _tcp_shift_l.
addWidget(_ui.doubleSpinBoxTCPShiftLZ);
113 _tcp_shift_r.
addWidget(_ui.doubleSpinBoxTCPShiftRX);
114 _tcp_shift_r.
addWidget(_ui.doubleSpinBoxTCPShiftRY);
115 _tcp_shift_r.
addWidget(_ui.doubleSpinBoxTCPShiftRZ);
117 const std::vector spinboxes
119 _ui.doubleSpinBoxBoundTransvMax, _ui.doubleSpinBoxBoundTransvMin,
120 _ui.doubleSpinBoxBoundWidthMin, _ui.doubleSpinBoxBoundWidthMax,
121 _ui.doubleSpinBoxBoundDepthMin, _ui.doubleSpinBoxBoundDepthMax,
123 _ui.doubleSpinBoxBoxTX, _ui.doubleSpinBoxBoxTY, _ui.doubleSpinBoxBoxTZ,
124 _ui.doubleSpinBoxBoxRX, _ui.doubleSpinBoxBoxRY, _ui.doubleSpinBoxBoxRZ,
125 _ui.doubleSpinBoxBoxSizeX, _ui.doubleSpinBoxBoxSizeY, _ui.doubleSpinBoxBoxSizeZ,
127 _ui.doubleSpinBoxHandLTransvX, _ui.doubleSpinBoxHandLTransvY, _ui.doubleSpinBoxHandLTransvZ,
128 _ui.doubleSpinBoxHandRTransvX, _ui.doubleSpinBoxHandRTransvY, _ui.doubleSpinBoxHandRTransvZ,
130 _ui.doubleSpinBoxHandLUpX, _ui.doubleSpinBoxHandLUpY, _ui.doubleSpinBoxHandLUpZ,
131 _ui.doubleSpinBoxHandRUpX, _ui.doubleSpinBoxHandRUpY, _ui.doubleSpinBoxHandRUpZ,
133 _ui.doubleSpinBoxTCPShiftLX, _ui.doubleSpinBoxTCPShiftLY, _ui.doubleSpinBoxTCPShiftLZ,
134 _ui.doubleSpinBoxTCPShiftRX, _ui.doubleSpinBoxTCPShiftRY, _ui.doubleSpinBoxTCPShiftRZ
136 for (QDoubleSpinBox* ptr : spinboxes)
141 const std::vector checkboxes
144 _ui.checkBoxGraspLPosXPosY, _ui.checkBoxGraspLPosXPosZ,
145 _ui.checkBoxGraspLPosXNegY, _ui.checkBoxGraspLPosXNegZ,
147 _ui.checkBoxGraspLNegXPosY, _ui.checkBoxGraspLNegXPosZ,
148 _ui.checkBoxGraspLNegXNegY, _ui.checkBoxGraspLNegXNegZ,
150 _ui.checkBoxGraspLPosYPosX, _ui.checkBoxGraspLPosYPosZ,
151 _ui.checkBoxGraspLPosYNegX, _ui.checkBoxGraspLPosYNegZ,
153 _ui.checkBoxGraspLNegYPosX, _ui.checkBoxGraspLNegYPosZ,
154 _ui.checkBoxGraspLNegYNegX, _ui.checkBoxGraspLNegYNegZ,
156 _ui.checkBoxGraspLPosZPosX, _ui.checkBoxGraspLPosZPosY,
157 _ui.checkBoxGraspLPosZNegX, _ui.checkBoxGraspLPosZNegY,
159 _ui.checkBoxGraspLNegZPosX, _ui.checkBoxGraspLNegZPosY,
160 _ui.checkBoxGraspLNegZNegX, _ui.checkBoxGraspLNegZNegY,
163 _ui.checkBoxGraspRPosXPosY, _ui.checkBoxGraspRPosXPosZ,
164 _ui.checkBoxGraspRPosXNegY, _ui.checkBoxGraspRPosXNegZ,
166 _ui.checkBoxGraspRNegXPosY, _ui.checkBoxGraspRNegXPosZ,
167 _ui.checkBoxGraspRNegXNegY, _ui.checkBoxGraspRNegXNegZ,
169 _ui.checkBoxGraspRPosYPosX, _ui.checkBoxGraspRPosYPosZ,
170 _ui.checkBoxGraspRPosYNegX, _ui.checkBoxGraspRPosYNegZ,
172 _ui.checkBoxGraspRNegYPosX, _ui.checkBoxGraspRNegYPosZ,
173 _ui.checkBoxGraspRNegYNegX, _ui.checkBoxGraspRNegYNegZ,
175 _ui.checkBoxGraspRPosZPosX, _ui.checkBoxGraspRPosZPosY,
176 _ui.checkBoxGraspRPosZNegX, _ui.checkBoxGraspRPosZNegY,
178 _ui.checkBoxGraspRNegZPosX, _ui.checkBoxGraspRNegZPosY,
179 _ui.checkBoxGraspRNegZNegX, _ui.checkBoxGraspRNegZNegY
181 for (QCheckBox* ptr : checkboxes)
183 ptr->setChecked(
true);
191 std::lock_guard guard{_mutex};
192 _robot =
addRobot(
"state robot", VirtualRobot::RobotIO::eStructure);
193 getTopic(_gc_topic, _gc_topic_name);
195 QMetaObject::invokeMethod(
this,
"update", Qt::QueuedConnection);
201 std::lock_guard guard{_mutex};
213 auto& l = b2gc.side(
"Left");
214 auto& r = b2gc.side(
"Right");
216 l.hand_transverse = _transversal_l.
get<Eigen::Vector3f>().normalized();
217 r.hand_transverse = _transversal_r.
get<Eigen::Vector3f>().normalized();
218 l.hand_up = _up_l.
get<Eigen::Vector3f>().normalized();
219 r.hand_up = _up_r.
get<Eigen::Vector3f>().normalized();
220 l.tcp_shift = _tcp_shift_l.
get<Eigen::Vector3f>().normalized();
221 r.tcp_shift = _tcp_shift_r.
get<Eigen::Vector3f>().normalized();
235 side_enabled_l[ba::pos_x_pos_y] = _ui.checkBoxGraspLPosXPosY->isChecked();
236 side_enabled_l[ba::pos_x_pos_z] = _ui.checkBoxGraspLPosXPosZ->isChecked();
237 side_enabled_l[ba::pos_x_neg_y] = _ui.checkBoxGraspLPosXNegY->isChecked();
238 side_enabled_l[ba::pos_x_neg_z] = _ui.checkBoxGraspLPosXNegZ->isChecked();
240 side_enabled_l[ba::neg_x_pos_y] = _ui.checkBoxGraspLNegXPosY->isChecked();
241 side_enabled_l[ba::neg_x_pos_z] = _ui.checkBoxGraspLNegXPosZ->isChecked();
242 side_enabled_l[ba::neg_x_neg_y] = _ui.checkBoxGraspLNegXNegY->isChecked();
243 side_enabled_l[ba::neg_x_neg_z] = _ui.checkBoxGraspLNegXNegZ->isChecked();
245 side_enabled_l[ba::pos_y_pos_x] = _ui.checkBoxGraspLPosYPosX->isChecked();
246 side_enabled_l[ba::pos_y_pos_z] = _ui.checkBoxGraspLPosYPosZ->isChecked();
247 side_enabled_l[ba::pos_y_neg_x] = _ui.checkBoxGraspLPosYNegX->isChecked();
248 side_enabled_l[ba::pos_y_neg_z] = _ui.checkBoxGraspLPosYNegZ->isChecked();
250 side_enabled_l[ba::neg_y_pos_x] = _ui.checkBoxGraspLNegYPosX->isChecked();
251 side_enabled_l[ba::neg_y_pos_z] = _ui.checkBoxGraspLNegYPosZ->isChecked();
252 side_enabled_l[ba::neg_y_neg_x] = _ui.checkBoxGraspLNegYNegX->isChecked();
253 side_enabled_l[ba::neg_y_neg_z] = _ui.checkBoxGraspLNegYNegZ->isChecked();
255 side_enabled_l[ba::pos_z_pos_x] = _ui.checkBoxGraspLPosZPosX->isChecked();
256 side_enabled_l[ba::pos_z_pos_y] = _ui.checkBoxGraspLPosZPosY->isChecked();
257 side_enabled_l[ba::pos_z_neg_x] = _ui.checkBoxGraspLPosZNegX->isChecked();
258 side_enabled_l[ba::pos_z_neg_y] = _ui.checkBoxGraspLPosZNegY->isChecked();
260 side_enabled_l[ba::neg_z_pos_x] = _ui.checkBoxGraspLNegZPosX->isChecked();
261 side_enabled_l[ba::neg_z_pos_y] = _ui.checkBoxGraspLNegZPosY->isChecked();
262 side_enabled_l[ba::neg_z_neg_x] = _ui.checkBoxGraspLNegZNegX->isChecked();
263 side_enabled_l[ba::neg_z_neg_y] = _ui.checkBoxGraspLNegZNegY->isChecked();
268 side_enabled_r[ba::pos_x_pos_y] = _ui.checkBoxGraspRPosXPosY->isChecked();
269 side_enabled_r[ba::pos_x_pos_z] = _ui.checkBoxGraspRPosXPosZ->isChecked();
270 side_enabled_r[ba::pos_x_neg_y] = _ui.checkBoxGraspRPosXNegY->isChecked();
271 side_enabled_r[ba::pos_x_neg_z] = _ui.checkBoxGraspRPosXNegZ->isChecked();
273 side_enabled_r[ba::neg_x_pos_y] = _ui.checkBoxGraspRNegXPosY->isChecked();
274 side_enabled_r[ba::neg_x_pos_z] = _ui.checkBoxGraspRNegXPosZ->isChecked();
275 side_enabled_r[ba::neg_x_neg_y] = _ui.checkBoxGraspRNegXNegY->isChecked();
276 side_enabled_r[ba::neg_x_neg_z] = _ui.checkBoxGraspRNegXNegZ->isChecked();
278 side_enabled_r[ba::pos_y_pos_x] = _ui.checkBoxGraspRPosYPosX->isChecked();
279 side_enabled_r[ba::pos_y_pos_z] = _ui.checkBoxGraspRPosYPosZ->isChecked();
280 side_enabled_r[ba::pos_y_neg_x] = _ui.checkBoxGraspRPosYNegX->isChecked();
281 side_enabled_r[ba::pos_y_neg_z] = _ui.checkBoxGraspRPosYNegZ->isChecked();
283 side_enabled_r[ba::neg_y_pos_x] = _ui.checkBoxGraspRNegYPosX->isChecked();
284 side_enabled_r[ba::neg_y_pos_z] = _ui.checkBoxGraspRNegYPosZ->isChecked();
285 side_enabled_r[ba::neg_y_neg_x] = _ui.checkBoxGraspRNegYNegX->isChecked();
286 side_enabled_r[ba::neg_y_neg_z] = _ui.checkBoxGraspRNegYNegZ->isChecked();
288 side_enabled_r[ba::pos_z_pos_x] = _ui.checkBoxGraspRPosZPosX->isChecked();
289 side_enabled_r[ba::pos_z_pos_y] = _ui.checkBoxGraspRPosZPosY->isChecked();
290 side_enabled_r[ba::pos_z_neg_x] = _ui.checkBoxGraspRPosZNegX->isChecked();
291 side_enabled_r[ba::pos_z_neg_y] = _ui.checkBoxGraspRPosZNegY->isChecked();
293 side_enabled_r[ba::neg_z_pos_x] = _ui.checkBoxGraspRNegZPosX->isChecked();
294 side_enabled_r[ba::neg_z_pos_y] = _ui.checkBoxGraspRNegZPosY->isChecked();
295 side_enabled_r[ba::neg_z_neg_x] = _ui.checkBoxGraspRNegZNegX->isChecked();
296 side_enabled_r[ba::neg_z_neg_y] = _ui.checkBoxGraspRNegZNegY->isChecked();
305 .transformPose(_robot->getGlobalPose())
308 gc_draw.draw(b2gc.side(
"Left"), layer_hand_vec);
309 gc_draw.draw(b2gc.side(
"Right"), layer_hand_vec);
311 grasping::GraspCandidateSeq graps;
312 const auto consume_l = [&](
const auto & g)
314 graps.emplace_back(g.make_candidate(
getName()));
315 gc_draw.draw(graps.back(), layer_l);
317 const auto consume_r = [&](
const auto & g)
319 graps.emplace_back(g.make_candidate(
getName()));
320 gc_draw.draw(graps.back(), layer_r);
322 b2gc.center_grasps(box,
"Left", lim, consume_l, side_enabled_l);
323 b2gc.center_grasps(box,
"Right", lim, consume_r, side_enabled_r);
325 arviz.
commit({layer_l, layer_r, layer_hand_vec, layer_box});
327 _gc_topic->reportGraspCandidates(
getName(), graps);