aron_conversions.cpp
Go to the documentation of this file.
1#include "aron_conversions.h"
2
3#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
4#include <ArmarXCore/core/logging/Logging.h>
5
6#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
7#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
8#include <RobotAPI/libraries/aron/common/aron_conversions.h>
9#include <RobotAPI/libraries/core/Pose.h>
10
11void
12armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto,
13 armarx::grasping::BoundingBox& bo)
14{
15 ARMARX_TRACE;
16 bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
17}
18
19void
20armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
21 const armarx::grasping::BoundingBox& bo)
22{
23 ARMARX_TRACE;
24 dto.center = fromIce(bo.center);
25 dto.ha1 = fromIce(bo.ha1);
26 dto.ha2 = fromIce(bo.ha2);
27 dto.ha3 = fromIce(bo.ha3);
28}
29
30void
31armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
32 armarx::grasping::GraspCandidateSourceInfo& bo)
33{
34 ARMARX_TRACE;
35 bo.bbox = new BoundingBox();
36 if (dto.bbox)
37 {
38 fromAron(dto.bbox.value(), *bo.bbox);
39 }
40 else
41 bo.bbox = nullptr;
42 bo.referenceObjectName = dto.referenceObjectName;
43 if (dto.referenceObjectPose)
44 {
45 bo.referenceObjectPose = toIce(dto.referenceObjectPose.value());
46 }
47 else
48 bo.referenceObjectPose = nullptr;
49 bo.segmentationLabelID = dto.segmentationLabelID;
50}
51
52void
53armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
54 const armarx::grasping::GraspCandidateSourceInfo& bo)
55{
56 ARMARX_TRACE;
57 if (bo.bbox)
58 {
59 dto.bbox = arondto::BoundingBox();
60 toAron(dto.bbox.value(), *bo.bbox);
61 }
62 dto.referenceObjectName = bo.referenceObjectName;
63 if (bo.referenceObjectPose)
64 {
65 dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
66 }
67 dto.segmentationLabelID = bo.segmentationLabelID;
68}
69
70void
71armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
72 armarx::grasping::GraspCandidateReachabilityInfo& bo)
73{
74 ARMARX_TRACE;
75 bo = GraspCandidateReachabilityInfo(dto.reachable,
76 dto.minimumJointLimitMargin,
77 dto.jointLimitMargins,
78 dto.maxPosError,
79 dto.maxOriError);
80}
81
82void
83armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
84 const armarx::grasping::GraspCandidateReachabilityInfo& bo)
85{
86 ARMARX_TRACE;
87 dto.jointLimitMargins = bo.jointLimitMargins;
88 dto.maxOriError = bo.maxOriError;
89 dto.maxPosError = bo.maxPosError;
90 dto.minimumJointLimitMargin = bo.minimumJointLimitMargin;
91 dto.reachable = bo.reachable;
92}
93
94void
95armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
96 armarx::grasping::GraspCandidate& bo)
97{
98 ARMARX_TRACE;
99 bo = GraspCandidate();
100 bo.graspPose = toIce(dto.graspPose);
101 bo.robotPose = toIce(dto.robotPose);
102 bo.tcpPoseInHandRoot = dto.tcpPoseInHandRoot ? toIce(dto.tcpPoseInHandRoot.value()) : nullptr;
103 bo.approachVector = dto.approachVector ? toIce(dto.approachVector.value()) : nullptr;
104 bo.sourceFrame = dto.sourceFrame;
105 bo.targetFrame = dto.targetFrame;
106 bo.side = dto.side;
107 bo.graspSuccessProbability =
108 dto.graspSuccessProbability ? dto.graspSuccessProbability.value() : -1.0;
109 fromAron(dto.objectType, bo.objectType);
110 if (dto.executionHints)
111 {
112 bo.executionHints = new GraspCandidateExecutionHints();
113 fromAron(dto.executionHints.value(), *bo.executionHints);
114 }
115 else
116 {
117 bo.executionHints = nullptr;
118 }
119 bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
120 bo.providerName = dto.providerName;
121 if (dto.reachabilityInfo)
122 {
123 bo.reachabilityInfo = new GraspCandidateReachabilityInfo();
124 fromAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
125 }
126 else
127 {
128 bo.reachabilityInfo = nullptr;
129 }
130 if (dto.sourceInfo)
131 {
132 bo.sourceInfo = new GraspCandidateSourceInfo();
133 fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
134 }
135 else
136 {
137 bo.sourceInfo = nullptr;
138 }
139}
140
141void
142armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
143 const armarx::grasping::GraspCandidate& bo)
144{
145 ARMARX_TRACE;
146 if (bo.approachVector)
147 {
148 dto.approachVector = fromIce(bo.approachVector);
149 }
150 if (bo.executionHints)
151 {
152 dto.executionHints = arondto::GraspCandidateExecutionHints();
153 toAron(dto.executionHints.value(), *bo.executionHints);
154 }
155 else
156 {
157 dto.executionHints = std::nullopt;
158 }
159 dto.graspPose = fromIce(bo.graspPose);
160 if (bo.graspSuccessProbability < 0 || bo.graspSuccessProbability > 1.0)
161 {
162 dto.graspSuccessProbability = std::nullopt;
163 }
164 else
165 {
166 dto.graspSuccessProbability = bo.graspSuccessProbability;
167 }
168 if (bo.groupNr < 0)
169 {
170 dto.groupNr = std::nullopt;
171 }
172 else
173 {
174 dto.groupNr = bo.groupNr;
175 }
176 toAron(dto.objectType, bo.objectType);
177 dto.providerName = bo.providerName;
178 if (bo.reachabilityInfo)
179 {
180 dto.reachabilityInfo = arondto::GraspCandidateReachabilityInfo();
181 toAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
182 }
183 else
184 {
185 dto.reachabilityInfo = std::nullopt;
186 }
187 dto.robotPose = fromIce(bo.robotPose);
188 if (bo.tcpPoseInHandRoot)
189 {
190 dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
191 }
192
193 dto.side = bo.side;
194 dto.sourceFrame = bo.sourceFrame;
195 if (bo.sourceInfo)
196 {
197 dto.sourceInfo = arondto::GraspCandidateSourceInfo();
198 toAron(dto.sourceInfo.value(), *bo.sourceInfo);
199 }
200 else
201 {
202 dto.sourceInfo = std::nullopt;
203 }
204 dto.targetFrame = bo.targetFrame;
205}
206
207void
208armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
209 armarx::grasping::BimanualGraspCandidate& bo)
210{
211 ARMARX_TRACE;
212 bo = BimanualGraspCandidate();
213 bo.graspPoseRight = toIce(dto.graspPoseRight);
214 bo.graspPoseLeft = toIce(dto.graspPoseLeft);
215 bo.robotPose = toIce(dto.robotPose);
216 bo.tcpPoseInHandRootRight =
217 dto.tcpPoseInHandRootRight ? toIce(dto.tcpPoseInHandRootRight.value()) : nullptr;
218 bo.tcpPoseInHandRootLeft =
219 dto.tcpPoseInHandRootLeft ? toIce(dto.tcpPoseInHandRootLeft.value()) : nullptr;
220 bo.approachVectorRight =
221 dto.approachVectorRight ? toIce(dto.approachVectorRight.value()) : nullptr;
222 bo.approachVectorLeft =
223 dto.approachVectorLeft ? toIce(dto.approachVectorLeft.value()) : nullptr;
224 bo.inwardsVectorRight = toIce(dto.inwardsVectorRight);
225 bo.inwardsVectorLeft = toIce(dto.inwardsVectorLeft);
226 bo.sourceFrame = dto.sourceFrame;
227 bo.targetFrame = dto.targetFrame;
228 fromAron(dto.objectType, bo.objectType);
229 if (dto.executionHintsRight)
230 {
231 bo.executionHintsRight = new GraspCandidateExecutionHints();
232 fromAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
233 }
234 else
235 {
236 bo.executionHintsRight = nullptr;
237 }
238 if (dto.executionHintsLeft)
239 {
240 bo.executionHintsLeft = new GraspCandidateExecutionHints();
241 fromAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
242 }
243 else
244 {
245 bo.executionHintsLeft = nullptr;
246 }
247 bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
248 bo.providerName = dto.providerName;
249 if (dto.reachabilityInfoRight)
250 {
251 bo.reachabilityInfoRight = new GraspCandidateReachabilityInfo();
252 fromAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
253 }
254 else
255 {
256 bo.reachabilityInfoRight = nullptr;
257 }
258 if (dto.reachabilityInfoLeft)
259 {
260 bo.reachabilityInfoLeft = new GraspCandidateReachabilityInfo();
261 fromAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
262 }
263 else
264 {
265 bo.reachabilityInfoLeft = nullptr;
266 }
267 if (dto.sourceInfo)
268 {
269 bo.sourceInfo = new GraspCandidateSourceInfo();
270 fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
271 }
272 else
273 {
274 bo.sourceInfo = nullptr;
275 }
276 bo.graspName = dto.graspName;
277}
278
279void
280armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto,
281 const armarx::grasping::BimanualGraspCandidate& bo)
282{
283 if (bo.tcpPoseInHandRootRight)
284 {
285 dto.tcpPoseInHandRootRight = fromIce(bo.tcpPoseInHandRootRight);
286 }
287 if (bo.tcpPoseInHandRootLeft)
288 {
289 dto.tcpPoseInHandRootLeft = fromIce(bo.tcpPoseInHandRootLeft);
290 }
291 if (bo.approachVectorRight)
292 {
293 dto.approachVectorRight = fromIce(bo.approachVectorRight);
294 }
295 if (bo.approachVectorLeft)
296 {
297 dto.approachVectorLeft = fromIce(bo.approachVectorLeft);
298 }
299 if (bo.executionHintsRight)
300 {
301 dto.executionHintsRight = arondto::GraspCandidateExecutionHints();
302 toAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
303 }
304 else
305 {
306 dto.executionHintsRight = std::nullopt;
307 }
308 if (bo.executionHintsLeft)
309 {
310 dto.executionHintsLeft = arondto::GraspCandidateExecutionHints();
311 toAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
312 }
313 else
314 {
315 dto.executionHintsLeft = std::nullopt;
316 }
317 dto.graspPoseRight = fromIce(bo.graspPoseRight);
318 dto.graspPoseLeft = fromIce(bo.graspPoseLeft);
319 if (bo.groupNr < 0)
320 {
321 dto.groupNr = std::nullopt;
322 }
323 else
324 {
325 dto.groupNr = bo.groupNr;
326 }
327 toAron(dto.objectType, bo.objectType);
328 dto.providerName = bo.providerName;
329 if (bo.reachabilityInfoRight)
330 {
331 dto.reachabilityInfoRight = arondto::GraspCandidateReachabilityInfo();
332 toAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
333 }
334 else
335 {
336 dto.reachabilityInfoRight = std::nullopt;
337 }
338 if (bo.reachabilityInfoLeft)
339 {
340 dto.reachabilityInfoLeft = arondto::GraspCandidateReachabilityInfo();
341 toAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
342 }
343 else
344 {
345 dto.reachabilityInfoLeft = std::nullopt;
346 }
347 dto.robotPose = fromIce(bo.robotPose);
348 dto.sourceFrame = bo.sourceFrame;
349 if (bo.sourceInfo)
350 {
351 dto.sourceInfo = arondto::GraspCandidateSourceInfo();
352 toAron(dto.sourceInfo.value(), *bo.sourceInfo);
353 }
354 else
355 {
356 dto.sourceInfo = std::nullopt;
357 }
358 dto.targetFrame = bo.targetFrame;
359 dto.inwardsVectorRight = fromIce(bo.inwardsVectorRight);
360 dto.inwardsVectorLeft = fromIce(bo.inwardsVectorLeft);
361 dto.graspName = bo.graspName;
362}
363
364void
365armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
366 armarx::grasping::GraspCandidateExecutionHints& bo)
367{
368 bo = GraspCandidateExecutionHints();
369 fromAron(dto.approach, bo.approach);
370 fromAron(dto.preshape, bo.preshape);
371 bo.graspTrajectoryName = dto.graspTrajectoryName;
372}
373
374void
375armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
376 const armarx::grasping::GraspCandidateExecutionHints& bo)
377{
378 toAron(dto.approach, bo.approach);
379 toAron(dto.preshape, bo.preshape);
380 dto.graspTrajectoryName = bo.graspTrajectoryName;
381}
382
383void
384armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto,
385 armarx::grasping::ApproachType& bo)
386{
387 switch (dto.value)
388 {
389 case arondto::ApproachType::AnyApproach:
390 bo = ApproachType::AnyApproach;
391 return;
392 case arondto::ApproachType::TopApproach:
393 bo = ApproachType::TopApproach;
394 return;
395 case arondto::ApproachType::SideApproach:
396 bo = ApproachType::SideApproach;
397 return;
398 }
399 ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
400}
401
402void
403armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
404 const armarx::grasping::ApproachType& bo)
405{
406 switch (bo)
407 {
408 case ApproachType::AnyApproach:
409 dto.value = arondto::ApproachType::AnyApproach;
410 return;
411 case ApproachType::TopApproach:
412 dto.value = arondto::ApproachType::TopApproach;
413 return;
414 case ApproachType::SideApproach:
415 dto.value = arondto::ApproachType::SideApproach;
416 return;
417 }
418 ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
419}
420
421void
422armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto,
423 armarx::grasping::ApertureType& bo)
424{
425 switch (dto.value)
426 {
427 case arondto::ApertureType::AnyAperture:
428 bo = ApertureType::AnyAperture;
429 return;
430 case arondto::ApertureType::OpenAperture:
431 bo = ApertureType::OpenAperture;
432 return;
433 case arondto::ApertureType::PreshapedAperture:
434 bo = ApertureType::PreshapedAperture;
435 return;
436 }
437 ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
438}
439
440void
441armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto,
442 const armarx::grasping::ApertureType& bo)
443{
444 switch (bo)
445 {
446 case ApertureType::AnyAperture:
447 dto.value = arondto::ApertureType::AnyAperture;
448 return;
449 case ApertureType::OpenAperture:
450 dto.value = arondto::ApertureType::OpenAperture;
451 return;
452 case ApertureType::PreshapedAperture:
453 dto.value = arondto::ApertureType::PreshapedAperture;
454 return;
455 }
456 ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
457}
aron_conversions.h
aron_conversions.h
aron_conversions.h
aron_conversions.h
ARMARX_UNEXPECTED_ENUM_VALUE
#define ARMARX_UNEXPECTED_ENUM_VALUE(EnumType, value)
Throw an UnexpectedEnumValueException.
Definition UnexpectedEnumValueException.h:58
armarx::core::eigen::fromIce
void fromIce(Eigen::Vector2f &e, const Ice::FloatSeq &ice)
Definition ice_conversions.cpp:10
armarx::core::eigen::toIce
void toIce(const Eigen::Vector2f &e, Ice::FloatSeq &ice)
Definition ice_conversions.cpp:17
armarx::grasping::toAron
void toAron(arondto::ApproachType &dto, const ApproachType &bo)
armarx::grasping::fromAron
void fromAron(const arondto::ApproachType &dto, ApproachType &bo)
armarx::toAron
void toAron(arondto::PackagePath &dto, const PackageFileLocation &bo)
armarx::fromAron
void fromAron(const arondto::PackagePath &dto, PackageFileLocation &bo)
ARMARX_TRACE
#define ARMARX_TRACE
Definition trace.h:77