1 /*
2 * Copyright (c) 2011-2021, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 // Use this preprocessor token to allow us to test that the specialized versions
34 // of Aspect functions are being used appropriately. This preprocessor token
35 // should NOT be used anywhere outside of this file (testAspect.cpp).
36 #define DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS
37
38 #include <gtest/gtest.h>
39
40 #include "TestHelpers.hpp"
41
42 #include "dart/common/Composite.hpp"
43 #include "dart/common/EmbeddedAspect.hpp"
44 #include "dart/common/SpecializedForAspect.hpp"
45 #include "dart/common/Subject.hpp"
46 #include "dart/common/sub_ptr.hpp"
47
48 #include "dart/dynamics/BoxShape.hpp"
49 #include "dart/dynamics/EulerJoint.hpp"
50
51 using namespace dart::common;
52
53 struct EmbeddedStateData
54 {
55 double d;
56 int i;
57
EmbeddedStateDataEmbeddedStateData58 EmbeddedStateData() : d(0.0), i(0)
59 {
60 // Do nothing
61 }
62
operator ==EmbeddedStateData63 bool operator==(const EmbeddedStateData& other) const
64 {
65 if (other.d != d)
66 return false;
67
68 if (other.i != i)
69 return false;
70
71 return true;
72 }
73
operator !=EmbeddedStateData74 bool operator!=(const EmbeddedStateData& other) const
75 {
76 return !(*this == other);
77 }
78 };
79
80 struct SecondEmbeddedStateData
81 {
82 };
83
84 struct EmbeddedPropertiesData
85 {
86 bool b;
87 float f;
88
EmbeddedPropertiesDataEmbeddedPropertiesData89 EmbeddedPropertiesData() : b(false), f(0.0)
90 {
91 // Do nothing
92 }
93
operator ==EmbeddedPropertiesData94 bool operator==(const EmbeddedPropertiesData& other) const
95 {
96 if (other.b != b)
97 return false;
98
99 if (other.f != f)
100 return false;
101
102 return true;
103 }
104
operator !=EmbeddedPropertiesData105 bool operator!=(const EmbeddedPropertiesData& other) const
106 {
107 return !(*this == other);
108 }
109 };
110
111 struct SecondEmbeddedPropertiesData
112 {
113 };
114
115 class EmbeddedStateComposite
116 : public EmbedState<EmbeddedStateComposite, EmbeddedStateData>
117 {
118 public:
EmbeddedStateComposite(const EmbeddedStateData & state=EmbeddedStateData ())119 EmbeddedStateComposite(const EmbeddedStateData& state = EmbeddedStateData())
120 {
121 createAspect<Aspect>(static_cast<const Aspect::StateData&>(state));
122 }
123
setAspectState(const AspectState & state)124 void setAspectState(const AspectState& state)
125 {
126 mAspectState = state;
127 }
128 };
129
130 class EmbeddedPropertiesComposite
131 : public EmbedProperties<EmbeddedPropertiesComposite, EmbeddedPropertiesData>
132 {
133 public:
EmbeddedPropertiesComposite(const EmbeddedPropertiesData & properties=EmbeddedPropertiesData ())134 EmbeddedPropertiesComposite(
135 const EmbeddedPropertiesData& properties = EmbeddedPropertiesData())
136 {
137 createAspect<Aspect>(properties);
138 }
139
setAspectProperties(const AspectProperties & properties)140 void setAspectProperties(const AspectProperties& properties)
141 {
142 mAspectProperties = properties;
143 }
144 };
145
146 class EmbeddedStateAndPropertiesComposite
147 : public EmbedStateAndProperties<
148 EmbeddedStateAndPropertiesComposite,
149 EmbeddedStateData,
150 EmbeddedPropertiesData>
151 {
152 public:
EmbeddedStateAndPropertiesComposite()153 EmbeddedStateAndPropertiesComposite()
154 {
155 createAspect<Aspect>();
156 }
157
setAspectState(const AspectState & state)158 void setAspectState(const AspectState& state)
159 {
160 mAspectState = state;
161 }
162
setAspectProperties(const AspectProperties & properties)163 void setAspectProperties(const AspectProperties& properties)
164 {
165 mAspectProperties = properties;
166 }
167 };
168
169 class InheritAndEmbedStateComposite : public EmbedStateOnTopOf<
170 InheritAndEmbedStateComposite,
171 SecondEmbeddedStateData,
172 EmbeddedStateComposite>
173 {
174 public:
InheritAndEmbedStateComposite()175 InheritAndEmbedStateComposite()
176 {
177 createAspect<Aspect>();
178 }
179
setAspectState(const AspectState & state)180 void setAspectState(const AspectState& state)
181 {
182 mAspectState = state;
183 }
184 };
185
186 class InheritAndEmbedPropertiesComposite
187 : public EmbedPropertiesOnTopOf<
188 InheritAndEmbedPropertiesComposite,
189 SecondEmbeddedPropertiesData,
190 EmbeddedPropertiesComposite>
191 {
192 public:
InheritAndEmbedPropertiesComposite()193 InheritAndEmbedPropertiesComposite()
194 {
195 createAspect<Aspect>();
196 }
197
setAspectProperties(const AspectProperties & properties)198 void setAspectProperties(const AspectProperties& properties)
199 {
200 mAspectProperties = properties;
201 }
202 };
203
204 class InheritAndEmbedStateAndPropertiesComposite
205 : public EmbedStateAndPropertiesOnTopOf<
206 InheritAndEmbedStateAndPropertiesComposite,
207 SecondEmbeddedStateData,
208 SecondEmbeddedPropertiesData,
209 EmbeddedStateAndPropertiesComposite>
210 {
211 public:
InheritAndEmbedStateAndPropertiesComposite()212 InheritAndEmbedStateAndPropertiesComposite()
213 {
214 createAspect<Aspect>();
215 }
216
setAspectState(const AspectState & state)217 void setAspectState(const AspectState& state)
218 {
219 mAspectState = state;
220 }
221
setAspectProperties(const AspectProperties & properties)222 void setAspectProperties(const AspectProperties& properties)
223 {
224 mAspectProperties = properties;
225 }
226 };
227
228 // Testing the creation of an Aspect using the AspectWithState template class
229 class StateAspectTest
230 : public dart::common::AspectWithState<StateAspectTest, dart::common::Empty>
231 {
232 public:
StateAspectTest(const StateData & state=StateData ())233 StateAspectTest(const StateData& state = StateData())
234 : dart::common::AspectWithState<StateAspectTest, dart::common::Empty>(state)
235 {
236 }
237 };
238
239 class GenericAspect : public Aspect, public Subject
240 {
241 public:
GenericAspect()242 GenericAspect() : Aspect()
243 {
244 // Do nothing
245 }
246
cloneAspect() const247 std::unique_ptr<Aspect> cloneAspect() const override final
248 {
249 return std::make_unique<GenericAspect>();
250 }
251 };
252
253 class SpecializedAspect : public Aspect, public Subject
254 {
255 public:
SpecializedAspect()256 SpecializedAspect() : Aspect()
257 {
258 // Do nothing
259 }
260
cloneAspect() const261 std::unique_ptr<Aspect> cloneAspect() const override final
262 {
263 return std::make_unique<SpecializedAspect>();
264 }
265 };
266
267 template <typename T>
268 class StatefulAspect : public Aspect, public Subject
269 {
270 public:
271 struct Data
272 {
273 T val;
274
DataStatefulAspect::Data275 Data(const T& someVal = 0) : val(someVal)
276 {
277 // Do nothing
278 }
279 };
280
281 using State = Aspect::MakeState<Data>;
282 using Properties = Aspect::MakeProperties<Data>;
283
StatefulAspect()284 StatefulAspect() : Aspect()
285 {
286 // Do nothing
287 }
288
StatefulAspect(const StatefulAspect & other)289 StatefulAspect(const StatefulAspect& other)
290 : Aspect(), mState(other.mState), mProperties(other.mProperties)
291 {
292 // Do nothing
293 }
294
StatefulAspect(const T & state)295 StatefulAspect(const T& state) : Aspect(), mState(state)
296 {
297 // Do nothing
298 }
299
StatefulAspect(const T & state,const T & properties)300 StatefulAspect(const T& state, const T& properties)
301 : Aspect(), mState(state), mProperties(properties)
302 {
303 // Do nothing
304 }
305
cloneAspect() const306 std::unique_ptr<Aspect> cloneAspect() const override final
307 {
308 return std::make_unique<StatefulAspect>(*this);
309 }
310
setAspectState(const Aspect::State & otherState)311 void setAspectState(const Aspect::State& otherState) override
312 {
313 mState.copy(otherState);
314 }
315
getAspectState() const316 const Aspect::State* getAspectState() const override
317 {
318 return &mState;
319 }
320
setAspectProperties(const Aspect::Properties & someProperties)321 void setAspectProperties(const Aspect::Properties& someProperties) override
322 {
323 mProperties.copy(someProperties);
324 }
325
getAspectProperties() const326 const Aspect::Properties* getAspectProperties() const override
327 {
328 return &mProperties;
329 }
330
randomize()331 void randomize()
332 {
333 mState.val = dart::math::Random::uniform<T>(0, 100);
334 mProperties.val = dart::math::Random::uniform<T>(0, 100);
335 }
336
337 State mState;
338
339 Properties mProperties;
340 };
341
342 typedef StatefulAspect<double> DoubleAspect;
343 typedef StatefulAspect<float> FloatAspect;
344 typedef StatefulAspect<char> CharAspect;
345 typedef StatefulAspect<int> IntAspect;
346
347 class CustomSpecializedComposite
348 : public SpecializedForAspect<SpecializedAspect>
349 {
350 };
351
TEST(Aspect,Generic)352 TEST(Aspect, Generic)
353 {
354 Composite comp;
355
356 EXPECT_TRUE(comp.get<GenericAspect>() == nullptr);
357 EXPECT_FALSE(comp.has<GenericAspect>());
358
359 comp.set<GenericAspect>(nullptr);
360 EXPECT_TRUE(comp.get<GenericAspect>() == nullptr);
361 EXPECT_FALSE(comp.has<GenericAspect>());
362
363 sub_ptr<GenericAspect> aspect = comp.createAspect<GenericAspect>();
364 GenericAspect* rawAspect = aspect;
365 EXPECT_FALSE(nullptr == aspect);
366 EXPECT_FALSE(nullptr == rawAspect);
367 EXPECT_TRUE(comp.get<GenericAspect>() == aspect);
368 EXPECT_TRUE(comp.get<GenericAspect>() == rawAspect);
369
370 GenericAspect* newAspect = comp.createAspect<GenericAspect>();
371 EXPECT_FALSE(nullptr == newAspect);
372 EXPECT_FALSE(nullptr == rawAspect);
373 EXPECT_FALSE(rawAspect == newAspect);
374 EXPECT_TRUE(nullptr == aspect);
375 }
376
TEST(Aspect,Specialized)377 TEST(Aspect, Specialized)
378 {
379 usedSpecializedAspectAccess = false;
380 CustomSpecializedComposite comp;
381
382 EXPECT_TRUE(comp.get<SpecializedAspect>() == nullptr);
383 EXPECT_TRUE(usedSpecializedAspectAccess);
384 usedSpecializedAspectAccess = false;
385 // EXPECT_TRUE( comp.getSpecializedAspect() == nullptr );
386 EXPECT_TRUE(comp.get<GenericAspect>() == nullptr);
387 EXPECT_FALSE(usedSpecializedAspectAccess);
388 usedSpecializedAspectAccess = false;
389
390 sub_ptr<SpecializedAspect> spec = comp.createAspect<SpecializedAspect>();
391 EXPECT_TRUE(usedSpecializedAspectAccess);
392 usedSpecializedAspectAccess = false;
393 SpecializedAspect* rawSpec = spec;
394
395 sub_ptr<GenericAspect> generic = comp.createAspect<GenericAspect>();
396 EXPECT_FALSE(usedSpecializedAspectAccess);
397 usedSpecializedAspectAccess = false;
398 GenericAspect* rawGeneric = generic;
399
400 EXPECT_TRUE(comp.get<SpecializedAspect>() == spec);
401 EXPECT_TRUE(usedSpecializedAspectAccess);
402 usedSpecializedAspectAccess = false;
403 // EXPECT_TRUE( comp.getSpecializedAspect() == spec );
404
405 EXPECT_TRUE(comp.get<GenericAspect>() == generic);
406 EXPECT_FALSE(usedSpecializedAspectAccess);
407 usedSpecializedAspectAccess = false;
408
409 // SpecializedAspect* newSpec = comp.createSpecializedAspect();
410 SpecializedAspect* newSpec = comp.createAspect<SpecializedAspect>();
411 EXPECT_TRUE(usedSpecializedAspectAccess);
412 usedSpecializedAspectAccess = false;
413
414 GenericAspect* newGeneric = comp.createAspect<GenericAspect>();
415 EXPECT_FALSE(usedSpecializedAspectAccess);
416 usedSpecializedAspectAccess = false;
417
418 EXPECT_TRUE(nullptr == spec);
419 EXPECT_TRUE(nullptr == generic);
420
421 EXPECT_FALSE(comp.get<SpecializedAspect>() == rawSpec);
422 EXPECT_TRUE(usedSpecializedAspectAccess);
423 usedSpecializedAspectAccess = false;
424 // EXPECT_FALSE( comp.getSpecializedAspect() == rawSpec );
425
426 EXPECT_FALSE(comp.get<GenericAspect>() == rawGeneric);
427 EXPECT_FALSE(usedSpecializedAspectAccess);
428 usedSpecializedAspectAccess = false;
429
430 EXPECT_TRUE(comp.get<SpecializedAspect>() == newSpec);
431 EXPECT_TRUE(usedSpecializedAspectAccess);
432 usedSpecializedAspectAccess = false;
433 // EXPECT_TRUE( comp.getSpecializedAspect() == newSpec );
434
435 EXPECT_TRUE(comp.get<GenericAspect>() == newGeneric);
436 EXPECT_FALSE(usedSpecializedAspectAccess);
437 usedSpecializedAspectAccess = false;
438 }
439
TEST(Aspect,Releasing)440 TEST(Aspect, Releasing)
441 {
442 Composite sender;
443 CustomSpecializedComposite receiver;
444
445 // ---- Test generic releases ----
446 {
447 EXPECT_TRUE(sender.get<GenericAspect>() == nullptr);
448 EXPECT_TRUE(receiver.get<GenericAspect>() == nullptr);
449
450 sub_ptr<GenericAspect> aspect = sender.createAspect<GenericAspect>();
451
452 EXPECT_TRUE(sender.get<GenericAspect>() == aspect);
453 EXPECT_TRUE(receiver.get<GenericAspect>() == nullptr);
454
455 receiver.set<GenericAspect>(sender.releaseAspect<GenericAspect>());
456
457 EXPECT_FALSE(nullptr == aspect);
458
459 EXPECT_TRUE(sender.get<GenericAspect>() == nullptr);
460 EXPECT_TRUE(receiver.get<GenericAspect>() == aspect);
461
462 sender.set<GenericAspect>(sender.releaseAspect<GenericAspect>());
463
464 EXPECT_FALSE(nullptr == aspect);
465
466 EXPECT_TRUE(sender.get<GenericAspect>() == nullptr);
467 EXPECT_TRUE(receiver.get<GenericAspect>() == aspect);
468
469 receiver.set<GenericAspect>(receiver.releaseAspect<GenericAspect>());
470
471 EXPECT_FALSE(nullptr == aspect);
472
473 EXPECT_TRUE(sender.get<GenericAspect>() == nullptr);
474 EXPECT_TRUE(receiver.get<GenericAspect>() == aspect);
475
476 sender.set<GenericAspect>(receiver.releaseAspect<GenericAspect>());
477
478 EXPECT_FALSE(nullptr == aspect);
479
480 EXPECT_TRUE(sender.get<GenericAspect>() == aspect);
481 EXPECT_TRUE(receiver.get<GenericAspect>() == nullptr);
482
483 sender.releaseAspect<GenericAspect>();
484
485 EXPECT_TRUE(nullptr == aspect);
486 EXPECT_TRUE(sender.get<GenericAspect>() == nullptr);
487 EXPECT_TRUE(receiver.get<GenericAspect>() == nullptr);
488 }
489
490 // ---- Test specialized releases ----
491 {
492 EXPECT_TRUE(sender.get<SpecializedAspect>() == nullptr);
493 // EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr );
494
495 sub_ptr<SpecializedAspect> spec = sender.createAspect<SpecializedAspect>();
496
497 EXPECT_TRUE(sender.get<SpecializedAspect>() == spec);
498 // EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr );
499
500 // receiver.setSpecializedAspect(sender.release<SpecializedAspect>());
501 receiver.set<SpecializedAspect>(sender.releaseAspect<SpecializedAspect>());
502
503 EXPECT_FALSE(nullptr == spec);
504
505 EXPECT_TRUE(sender.get<SpecializedAspect>() == nullptr);
506 // EXPECT_TRUE( receiver.getSpecializedAspect() == spec );
507
508 // sender.set<SpecializedAspect>(receiver.releaseSpecializedAspect());
509 sender.set<SpecializedAspect>(receiver.releaseAspect<SpecializedAspect>());
510
511 EXPECT_FALSE(nullptr == spec);
512
513 EXPECT_TRUE(sender.get<SpecializedAspect>() == spec);
514 // EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr );
515
516 sender.releaseAspect<SpecializedAspect>();
517
518 EXPECT_TRUE(nullptr == spec);
519 EXPECT_TRUE(sender.get<SpecializedAspect>() == nullptr);
520 // EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr );
521 }
522
523 // ---- Test non-moving set method ----
524 {
525 // The set() methods being used in this block of code will make clones of
526 // the aspects that are being passed in instead of transferring their
527 // ownership like the previous blocks of code were.
528 sub_ptr<GenericAspect> aspect = sender.createAspect<GenericAspect>();
529
530 // This should create a copy of the GenericAspect without taking the aspect
531 // away from 'sender'
532 receiver.set<GenericAspect>(sender.get<GenericAspect>());
533
534 EXPECT_FALSE(nullptr == aspect);
535 EXPECT_FALSE(receiver.get<GenericAspect>() == aspect);
536 EXPECT_FALSE(receiver.get<GenericAspect>() == nullptr);
537 EXPECT_TRUE(sender.get<GenericAspect>() == aspect);
538
539 sub_ptr<GenericAspect> rec_aspect = receiver.get<GenericAspect>();
540 EXPECT_FALSE(nullptr == rec_aspect);
541
542 // This should replace the first GenericAspect that was created in 'sender'
543 sender.set<GenericAspect>(receiver.get<GenericAspect>());
544
545 EXPECT_TRUE(nullptr == aspect);
546 EXPECT_FALSE(nullptr == rec_aspect);
547 EXPECT_FALSE(sender.get<GenericAspect>() == receiver.get<GenericAspect>());
548
549 sub_ptr<GenericAspect> aspect2 = sender.get<GenericAspect>();
550 EXPECT_FALSE(nullptr == aspect2);
551
552 sender.set<GenericAspect>(receiver.releaseAspect<GenericAspect>());
553 EXPECT_TRUE(nullptr == aspect2);
554 EXPECT_FALSE(nullptr == rec_aspect);
555 EXPECT_TRUE(receiver.get<GenericAspect>() == nullptr);
556 EXPECT_TRUE(sender.get<GenericAspect>() == rec_aspect);
557 }
558 }
559
560 template <class AspectT>
makeStatesDifferent(AspectT * aspect,const AspectT * differentFrom)561 void makeStatesDifferent(AspectT* aspect, const AspectT* differentFrom)
562 {
563 std::size_t counter = 0;
564 while (aspect->mState.val == differentFrom->mState.val)
565 {
566 aspect->randomize();
567 ++counter;
568 if (counter > 10)
569 {
570 dtwarn
571 << "[testAspect::makeStatesDifferent] Randomization failed to make "
572 << "the states different after " << counter << " attempts!\n";
573 break;
574 }
575 }
576 }
577
578 template <class AspectT>
makePropertiesDifferent(AspectT * aspect,const AspectT * differentFrom)579 void makePropertiesDifferent(AspectT* aspect, const AspectT* differentFrom)
580 {
581 std::size_t counter = 0;
582 while (aspect->mProperties.val == differentFrom->mProperties.val)
583 {
584 aspect->randomize();
585 ++counter;
586 if (counter > 10)
587 {
588 dtwarn << "[testAspect::makePropertiesDifferent] Randomization failed to "
589 << "make the states different after " << counter << " attempts!\n";
590 break;
591 }
592 }
593 }
594
TEST(Aspect,StateAndProperties)595 TEST(Aspect, StateAndProperties)
596 {
597 Composite comp1;
598 comp1.createAspect<DoubleAspect>();
599 comp1.createAspect<FloatAspect>();
600 comp1.createAspect<CharAspect>();
601 comp1.createAspect<IntAspect>();
602
603 Composite comp2;
604 comp2.createAspect<DoubleAspect>();
605 comp2.createAspect<FloatAspect>();
606
607 comp1.createAspect<StateAspectTest>();
608
609 // ---- Test State Transfer ----
610
611 comp2.setCompositeState(comp1.getCompositeState());
612
613 EXPECT_EQ(
614 comp1.get<DoubleAspect>()->mState.val,
615 comp2.get<DoubleAspect>()->mState.val);
616
617 EXPECT_EQ(
618 comp1.get<FloatAspect>()->mState.val,
619 comp2.get<FloatAspect>()->mState.val);
620
621 makeStatesDifferent(comp2.get<DoubleAspect>(), comp1.get<DoubleAspect>());
622 makeStatesDifferent(comp2.get<FloatAspect>(), comp1.get<FloatAspect>());
623
624 EXPECT_NE(
625 comp1.get<DoubleAspect>()->mState.val,
626 comp2.get<DoubleAspect>()->mState.val);
627
628 EXPECT_NE(
629 comp1.get<FloatAspect>()->mState.val,
630 comp2.get<FloatAspect>()->mState.val);
631
632 comp1.setCompositeState(comp2.getCompositeState());
633
634 EXPECT_EQ(
635 comp1.get<DoubleAspect>()->mState.val,
636 comp2.get<DoubleAspect>()->mState.val);
637
638 EXPECT_EQ(
639 comp1.get<FloatAspect>()->mState.val,
640 comp2.get<FloatAspect>()->mState.val);
641
642 EXPECT_TRUE(nullptr == comp2.get<CharAspect>());
643 EXPECT_TRUE(nullptr == comp2.get<IntAspect>());
644
645 // ---- Test Property Transfer ----
646
647 comp2.setCompositeProperties(comp1.getCompositeProperties());
648
649 EXPECT_EQ(
650 comp1.get<DoubleAspect>()->mProperties.val,
651 comp2.get<DoubleAspect>()->mProperties.val);
652
653 EXPECT_EQ(
654 comp1.get<FloatAspect>()->mProperties.val,
655 comp2.get<FloatAspect>()->mProperties.val);
656
657 makePropertiesDifferent(comp2.get<DoubleAspect>(), comp1.get<DoubleAspect>());
658 makePropertiesDifferent(comp2.get<FloatAspect>(), comp1.get<FloatAspect>());
659
660 EXPECT_NE(
661 comp1.get<DoubleAspect>()->mProperties.val,
662 comp2.get<DoubleAspect>()->mProperties.val);
663
664 EXPECT_NE(
665 comp1.get<FloatAspect>()->mProperties.val,
666 comp2.get<FloatAspect>()->mProperties.val);
667
668 comp1.setCompositeProperties(comp2.getCompositeProperties());
669
670 EXPECT_EQ(
671 comp1.get<DoubleAspect>()->mProperties.val,
672 comp2.get<DoubleAspect>()->mProperties.val);
673
674 EXPECT_EQ(
675 comp1.get<FloatAspect>()->mProperties.val,
676 comp2.get<FloatAspect>()->mProperties.val);
677
678 EXPECT_TRUE(nullptr == comp2.get<CharAspect>());
679 EXPECT_TRUE(nullptr == comp2.get<IntAspect>());
680
681 // ---- Test Data Containers ----
682 Composite::MakeState<DoubleAspect, IntAspect, FloatAspect> state(
683 comp1.getCompositeState());
684
685 EXPECT_EQ(
686 comp1.get<DoubleAspect>()->mState.val, state.DoubleAspect::State::val);
687 EXPECT_EQ(
688 comp1.get<FloatAspect>()->mState.val, state.FloatAspect::State::val);
689 EXPECT_EQ(comp1.get<IntAspect>()->mState.val, state.IntAspect::State::val);
690
691 Composite::MakeProperties<DoubleAspect, CharAspect, FloatAspect> properties(
692 comp2.getCompositeProperties());
693
694 EXPECT_EQ(
695 comp1.get<DoubleAspect>()->mProperties.val,
696 properties.DoubleAspect::Properties::val);
697 EXPECT_EQ(
698 comp1.get<CharAspect>()->mProperties.val,
699 properties.CharAspect::Properties::val);
700 EXPECT_EQ(
701 comp1.get<FloatAspect>()->mProperties.val,
702 properties.FloatAspect::Properties::val);
703
704 DoubleAspect::State doubleState(2.5);
705 FloatAspect::State floatState(4.7);
706 IntAspect::State intState(7);
707 CharAspect::State charState('h');
708
709 // The constructor arguments should match the type order
710 Composite::MakeState<DoubleAspect, IntAspect, CharAspect, FloatAspect>(
711 doubleState, intState, charState, floatState);
712
713 // ---- Test copying and merging ----
714 Composite::Properties c_properties_1(properties);
715 EXPECT_FALSE(c_properties_1.has<IntAspect>());
716
717 Composite::Properties c_properties_2;
718 c_properties_2.create<IntAspect>();
719 EXPECT_TRUE(c_properties_2.has<IntAspect>());
720
721 c_properties_2.merge(c_properties_1);
722 EXPECT_TRUE(c_properties_2.has<IntAspect>());
723
724 c_properties_2.copy(c_properties_1);
725 EXPECT_FALSE(c_properties_2.has<IntAspect>());
726 }
727
TEST(Aspect,Construction)728 TEST(Aspect, Construction)
729 {
730 Composite comp;
731
732 comp.createAspect<DoubleAspect>();
733
734 double s1 = dart::math::Random::uniform<double>(0, 100);
735 comp.createAspect<DoubleAspect>(s1);
736 EXPECT_EQ(comp.get<DoubleAspect>()->mState.val, s1);
737
738 double s2 = dart::math::Random::uniform<double>(0, 100);
739 double p = dart::math::Random::uniform<double>(0, 100);
740 comp.createAspect<DoubleAspect>(s2, p);
741 EXPECT_NE(comp.get<DoubleAspect>()->mState.val, s1);
742 EXPECT_EQ(comp.get<DoubleAspect>()->mState.val, s2);
743 EXPECT_EQ(comp.get<DoubleAspect>()->mProperties.val, p);
744 }
745
TEST(Aspect,Joints)746 TEST(Aspect, Joints)
747 {
748 usedSpecializedAspectAccess = false;
749
750 dart::dynamics::SkeletonPtr skel = Skeleton::create();
751
752 dart::dynamics::EulerJoint* euler
753 = skel->createJointAndBodyNodePair<dart::dynamics::EulerJoint>().first;
754 euler->getGenericJointAspect();
755 EXPECT_TRUE(usedSpecializedAspectAccess);
756 usedSpecializedAspectAccess = false;
757 euler->getEulerJointAspect();
758 EXPECT_TRUE(usedSpecializedAspectAccess);
759 usedSpecializedAspectAccess = false;
760
761 dart::dynamics::PlanarJoint* planar
762 = skel->createJointAndBodyNodePair<dart::dynamics::PlanarJoint>().first;
763 planar->getGenericJointAspect();
764 EXPECT_TRUE(usedSpecializedAspectAccess);
765 usedSpecializedAspectAccess = false;
766 planar->getPlanarJointAspect();
767 EXPECT_TRUE(usedSpecializedAspectAccess);
768 usedSpecializedAspectAccess = false;
769
770 dart::dynamics::PrismaticJoint* prismatic
771 = skel->createJointAndBodyNodePair<dart::dynamics::PrismaticJoint>()
772 .first;
773 prismatic->getGenericJointAspect();
774 EXPECT_TRUE(usedSpecializedAspectAccess);
775 usedSpecializedAspectAccess = false;
776 prismatic->getPrismaticJointAspect();
777 EXPECT_TRUE(usedSpecializedAspectAccess);
778 usedSpecializedAspectAccess = false;
779
780 dart::dynamics::RevoluteJoint* revolute
781 = skel->createJointAndBodyNodePair<dart::dynamics::RevoluteJoint>().first;
782 revolute->getGenericJointAspect();
783 EXPECT_TRUE(usedSpecializedAspectAccess);
784 usedSpecializedAspectAccess = false;
785 revolute->getRevoluteJointAspect();
786 EXPECT_TRUE(usedSpecializedAspectAccess);
787 usedSpecializedAspectAccess = false;
788
789 dart::dynamics::ScrewJoint* screw
790 = skel->createJointAndBodyNodePair<dart::dynamics::ScrewJoint>().first;
791 screw->getGenericJointAspect();
792 EXPECT_TRUE(usedSpecializedAspectAccess);
793 usedSpecializedAspectAccess = false;
794 screw->getScrewJointAspect();
795 EXPECT_TRUE(usedSpecializedAspectAccess);
796 usedSpecializedAspectAccess = false;
797
798 dart::dynamics::UniversalJoint* universal
799 = skel->createJointAndBodyNodePair<dart::dynamics::UniversalJoint>()
800 .first;
801 universal->getGenericJointAspect();
802 EXPECT_TRUE(usedSpecializedAspectAccess);
803 usedSpecializedAspectAccess = false;
804 universal->getUniversalJointAspect();
805 EXPECT_TRUE(usedSpecializedAspectAccess);
806 usedSpecializedAspectAccess = false;
807
808 // Regression test for issue #645
809 universal->getGenericJointAspect(true);
810 }
811
TEST(Aspect,BodyNodes)812 TEST(Aspect, BodyNodes)
813 {
814 SkeletonPtr skel = Skeleton::create();
815
816 BodyNode* bn
817 = skel->createJointAndBodyNodePair<dart::dynamics::RevoluteJoint>()
818 .second;
819
820 bn->createShapeNodeWith<
821 dart::dynamics::VisualAspect,
822 dart::dynamics::CollisionAspect,
823 dart::dynamics::DynamicsAspect>(
824 std::make_shared<dart::dynamics::BoxShape>(Eigen::Vector3d::Ones()));
825
826 EXPECT_EQ(bn->getNumShapeNodes(), 1u);
827 EXPECT_EQ(bn->getNumShapeNodesWith<dart::dynamics::VisualAspect>(), 1u);
828 EXPECT_EQ(bn->getNumShapeNodesWith<dart::dynamics::CollisionAspect>(), 1u);
829 EXPECT_EQ(bn->getNumShapeNodesWith<dart::dynamics::DynamicsAspect>(), 1u);
830 }
831
TEST(Aspect,Duplication)832 TEST(Aspect, Duplication)
833 {
834 Composite comp1, comp2;
835
836 comp1.createAspect<DoubleAspect>();
837 comp1.createAspect<IntAspect>();
838 comp1.createAspect<FloatAspect>();
839 comp1.createAspect<CharAspect>();
840
841 comp2.createAspect<DoubleAspect>();
842
843 comp2.duplicateAspects(&comp1);
844
845 EXPECT_FALSE(comp2.get<DoubleAspect>() == nullptr);
846 EXPECT_FALSE(comp2.get<IntAspect>() == nullptr);
847 EXPECT_FALSE(comp2.get<FloatAspect>() == nullptr);
848 EXPECT_FALSE(comp2.get<CharAspect>() == nullptr);
849
850 Composite::MakeState<DoubleAspect, IntAspect, FloatAspect> state;
851 state.DoubleAspect::State::val = 1e-6;
852 state.FloatAspect::State::val = 1.5;
853 state.IntAspect::State::val = 456;
854
855 comp1.setCompositeState(state);
856
857 EXPECT_EQ(comp1.get<DoubleAspect>()->mState.val, 1e-6);
858 EXPECT_EQ(comp1.get<FloatAspect>()->mState.val, 1.5);
859 EXPECT_EQ(comp1.get<IntAspect>()->mState.val, 456);
860
861 state = comp2.getCompositeState();
862
863 EXPECT_EQ(state.DoubleAspect::State::val, 0);
864 EXPECT_EQ(state.FloatAspect::State::val, 0);
865 EXPECT_EQ(state.IntAspect::State::val, 0);
866
867 state = comp1.getCompositeState();
868
869 EXPECT_EQ(state.DoubleAspect::State::val, 1e-6);
870 EXPECT_EQ(state.FloatAspect::State::val, 1.5);
871 EXPECT_EQ(state.IntAspect::State::val, 456);
872 }
873
TEST(Aspect,Embedded)874 TEST(Aspect, Embedded)
875 {
876 EmbeddedStateComposite s;
877 EXPECT_TRUE(s.has<EmbeddedStateComposite::Aspect>());
878 EXPECT_TRUE(s.get<EmbeddedStateComposite::Aspect>() != nullptr);
879
880 EmbeddedPropertiesComposite p;
881 EXPECT_TRUE(p.has<EmbeddedPropertiesComposite::Aspect>());
882 EXPECT_TRUE(p.get<EmbeddedPropertiesComposite::Aspect>() != nullptr);
883
884 EmbeddedStateAndPropertiesComposite sp;
885 EXPECT_TRUE(sp.has<EmbeddedStateAndPropertiesComposite::Aspect>());
886 EXPECT_TRUE(sp.get<EmbeddedStateAndPropertiesComposite::Aspect>() != nullptr);
887
888 // --------- Test Embedded State -----------
889 EmbeddedStateComposite::AspectState state = s.getAspectState();
890 EmbeddedStateComposite::AspectState a_state
891 = s.get<EmbeddedStateComposite::Aspect>()->getState();
892
893 EXPECT_TRUE(state == a_state);
894
895 state.d = 3.5;
896 state.i = 750;
897 s.setAspectState(state);
898
899 state = s.getAspectState();
900 a_state = s.get<EmbeddedStateComposite::Aspect>()->getState();
901 EXPECT_EQ(3.5, state.d);
902 EXPECT_EQ(750, state.i);
903 EXPECT_TRUE(state == a_state);
904
905 EXPECT_EQ(
906 &s.get<EmbeddedStateComposite::Aspect>()->getState(),
907 s.get<EmbeddedStateComposite::Aspect>()->getAspectState());
908 EXPECT_EQ(
909 &s.getAspectState(),
910 s.get<EmbeddedStateComposite::Aspect>()->getAspectState());
911
912 state.d = -4e-3;
913 state.i = -18;
914 s.get<EmbeddedStateComposite::Aspect>()->setAspectState(state);
915
916 state = s.getAspectState();
917 a_state = s.get<EmbeddedStateComposite::Aspect>()->getState();
918 EXPECT_EQ(-4e-3, state.d);
919 EXPECT_EQ(-18, state.i);
920 EXPECT_TRUE(state == a_state);
921
922 // --------- Test Embedded Properties -----------
923 EmbeddedPropertiesComposite::AspectProperties prop = p.getAspectProperties();
924 EmbeddedPropertiesComposite::AspectProperties a_prop
925 = p.get<EmbeddedPropertiesComposite::Aspect>()->getProperties();
926
927 EXPECT_TRUE(prop == a_prop);
928
929 prop.f = 7.5;
930 prop.b = true;
931 p.setAspectProperties(prop);
932
933 prop = p.getAspectProperties();
934 a_prop = p.get<EmbeddedPropertiesComposite::Aspect>()->getProperties();
935 EXPECT_EQ(7.5, prop.f);
936 EXPECT_EQ(true, prop.b);
937 EXPECT_TRUE(prop == a_prop);
938
939 // Make sure the pointers are consistent
940 EXPECT_EQ(
941 &p.get<EmbeddedPropertiesComposite::Aspect>()->getProperties(),
942 p.get<EmbeddedPropertiesComposite::Aspect>()->getAspectProperties());
943 EXPECT_EQ(
944 &p.getAspectProperties(),
945 p.get<EmbeddedPropertiesComposite::Aspect>()->getAspectProperties());
946
947 prop.f = -7e5;
948 prop.b = false;
949 p.get<EmbeddedPropertiesComposite::Aspect>()->setAspectProperties(prop);
950
951 prop = p.getAspectProperties();
952 a_prop = p.get<EmbeddedPropertiesComposite::Aspect>()->getProperties();
953 EXPECT_EQ(-7e5, prop.f);
954 EXPECT_EQ(false, prop.b);
955 EXPECT_TRUE(prop == a_prop);
956
957 // --------- Test Embedded State and Properties Combination -----------
958 // Make sure the pointers are consistent
959 EXPECT_EQ(
960 &sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getState(),
961 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getAspectState());
962 EXPECT_EQ(
963 &sp.getAspectState(),
964 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getAspectState());
965
966 EXPECT_EQ(
967 &sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getProperties(),
968 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()
969 ->getAspectProperties());
970 EXPECT_EQ(
971 &sp.getAspectProperties(),
972 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()
973 ->getAspectProperties());
974
975 sp.setAspectState(s.getAspectState());
976 sp.setAspectProperties(p.getAspectProperties());
977
978 // --------- Test Inheritance -----------
979 InheritAndEmbedStateComposite s_derived;
980 EXPECT_NE(
981 s_derived.get<EmbeddedStateComposite::Aspect>()->getState(),
982 s.get<EmbeddedStateComposite::Aspect>()->getState());
983 s_derived.setCompositeState(s.getCompositeState());
984 EXPECT_EQ(
985 s_derived.get<EmbeddedStateComposite::Aspect>()->getState(),
986 s.get<EmbeddedStateComposite::Aspect>()->getState());
987
988 InheritAndEmbedPropertiesComposite p_derived;
989 EXPECT_NE(
990 p_derived.get<EmbeddedPropertiesComposite::Aspect>()->getProperties(),
991 p.get<EmbeddedPropertiesComposite::Aspect>()->getProperties());
992 p_derived.setCompositeProperties(p.getCompositeProperties());
993 EXPECT_EQ(
994 p_derived.get<EmbeddedPropertiesComposite::Aspect>()->getProperties(),
995 p.get<EmbeddedPropertiesComposite::Aspect>()->getProperties());
996
997 InheritAndEmbedStateAndPropertiesComposite sp_derived;
998 EXPECT_NE(
999 sp_derived.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getState(),
1000 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getState());
1001 EXPECT_NE(
1002 sp_derived.get<EmbeddedStateAndPropertiesComposite::Aspect>()
1003 ->getProperties(),
1004 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getProperties());
1005 sp_derived.setCompositeState(sp.getCompositeState());
1006 EXPECT_EQ(
1007 sp_derived.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getState(),
1008 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getState());
1009 EXPECT_NE(
1010 sp_derived.get<EmbeddedStateAndPropertiesComposite::Aspect>()
1011 ->getProperties(),
1012 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getProperties());
1013 sp_derived.setCompositeProperties(sp.getCompositeProperties());
1014 EXPECT_EQ(
1015 sp_derived.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getState(),
1016 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getState());
1017 EXPECT_EQ(
1018 sp_derived.get<EmbeddedStateAndPropertiesComposite::Aspect>()
1019 ->getProperties(),
1020 sp.get<EmbeddedStateAndPropertiesComposite::Aspect>()->getProperties());
1021
1022 // --------- Test Construction -----------
1023 EmbeddedStateComposite s_constructed(state);
1024 EXPECT_EQ(
1025 s_constructed.get<EmbeddedStateComposite::Aspect>()->getState(), state);
1026 }
1027