Debian Codegraph @

unittests/integration/test_Aspect.cpp

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