scenario.cpp 2.38 KB
Newer Older
Justin Carpentier's avatar
Justin Carpentier committed
1
2
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
Guilhem Saurel's avatar
Guilhem Saurel committed
3

Justin Carpentier's avatar
Justin Carpentier committed
4
5
6
7
8
9
#include <iostream>

#define BOOST_TEST_MODULE StatsTests
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

Guilhem Saurel's avatar
Guilhem Saurel committed
10
#include "multicontact-api/scenario/contact-phase.hpp"
11
#include "multicontact-api/scenario/contact-sequence.hpp"
Guilhem Saurel's avatar
Guilhem Saurel committed
12
#include "multicontact-api/scenario/contact-patch.hpp"
Justin Carpentier's avatar
Justin Carpentier committed
13

Guilhem Saurel's avatar
Guilhem Saurel committed
14
using namespace multicontact_api::scenario;
Justin Carpentier's avatar
Justin Carpentier committed
15

Guilhem Saurel's avatar
Guilhem Saurel committed
16
17
template <typename Scalar>
struct ATpl {
18
  typedef pinocchio::SE3Tpl<Scalar> SE3;
Guilhem Saurel's avatar
Guilhem Saurel committed
19

Justin Carpentier's avatar
Justin Carpentier committed
20
  explicit ATpl() : data() {}
Guilhem Saurel's avatar
Guilhem Saurel committed
21
  explicit ATpl(const ATpl& other) : data(other.data){};
Guilhem Saurel's avatar
Guilhem Saurel committed
22

Guilhem Saurel's avatar
Guilhem Saurel committed
23
  bool operator==(const ATpl& other) { return data == other.data; }
Guilhem Saurel's avatar
Guilhem Saurel committed
24

Guilhem Saurel's avatar
Guilhem Saurel committed
25
 protected:
Justin Carpentier's avatar
Justin Carpentier committed
26
27
28
29
30
31
32
  SE3 data;
};

typedef ATpl<double> Ad;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

Guilhem Saurel's avatar
Guilhem Saurel committed
33
BOOST_AUTO_TEST_CASE(contact_model) {
Justin Carpentier's avatar
Justin Carpentier committed
34
35
  const double mu = 0.3;
  const double ZMP_radius = 0.01;
Guilhem Saurel's avatar
Guilhem Saurel committed
36

Guilhem Saurel's avatar
Guilhem Saurel committed
37
  ContactModelPlanar mp1(mu, ZMP_radius);
Justin Carpentier's avatar
Justin Carpentier committed
38
  ContactModelPlanar mp2(mp1);
Guilhem Saurel's avatar
Guilhem Saurel committed
39

Justin Carpentier's avatar
Justin Carpentier committed
40
41
42
  BOOST_CHECK(mp1 == mp2);
}

Guilhem Saurel's avatar
Guilhem Saurel committed
43
BOOST_AUTO_TEST_CASE(contact_patch) {
Justin Carpentier's avatar
Justin Carpentier committed
44
45
46
47
48
49
50
51
52
53
54
55
56
57
  ContactPatch cp;
  cp.contactModel().m_mu = 0.3;
  cp.contactModel().m_ZMP_radius = 0.01;
  cp.placement().setRandom();
  cp.contactModelPlacement().setRandom();
  cp.worldContactModelPlacement().setRandom();
  ContactPatch cp2(cp);
  ContactPatch cp3 = cp2;
  BOOST_CHECK(cp == cp);
  BOOST_CHECK(cp == cp2);
  Ad a1;
  Ad a2(a1);
}

58
59
60
BOOST_AUTO_TEST_CASE(contact_phase)
{
  ContactPhase4 cp, cp_test;
61
62
63
64
65
66
67
68
  for(ContactPhase4::ContactPatchMap::iterator it = cp.contact_patches().begin();
      it !=  cp.contact_patches().end(); ++it)
  {
    it->second.contactModel().m_mu = 0.3;
    it->second.contactModel().m_ZMP_radius = 0.01;
    it->second.placement().setRandom();
    it->second.contactModelPlacement().setRandom();
    it->second.worldContactModelPlacement().setRandom();
Justin Carpentier's avatar
Justin Carpentier committed
69
70
71
  }
  ContactPhase4 cp2(cp);
  BOOST_CHECK(cp == cp);
72
73
74
75
76
77
  BOOST_CHECK(cp == cp2);
  // test serialization
  cp.saveAsText("serialization_cp_test.test");
  cp_test.loadFromText("serialization_cp_test.test");
  remove("serialization_cp_test.test");
  BOOST_CHECK(cp == cp_test);
Justin Carpentier's avatar
Justin Carpentier committed
78
79
}

80
81
82
83
84
85
86
87
88
89
90
91
92
BOOST_AUTO_TEST_CASE(contact_sequence)
{
  ContactPhase4 cp;
  ContactSequence4 cs(1);
  ContactSequence4 cs_test(0);
  cs.m_contact_phases[0] = cp;
  // test serialization
  cs.saveAsText("serialization_cs_test.test");
  cs_test.loadFromText("serialization_cs_test.test");
  remove("serialization_cp_test.test");
  BOOST_CHECK(cs == cs_test);
}

Justin Carpentier's avatar
Justin Carpentier committed
93
BOOST_AUTO_TEST_SUITE_END()