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