scenario.cpp 3.66 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>

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

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

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

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

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

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

typedef ATpl<double> Ad;
Pierre Fernbach's avatar
Pierre Fernbach committed
31
32
typedef pinocchio::SE3Tpl<double> SE3;

Justin Carpentier's avatar
Justin Carpentier committed
33
34
35

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

Guilhem Saurel's avatar
Guilhem Saurel committed
36
BOOST_AUTO_TEST_CASE(contact_model) {
Justin Carpentier's avatar
Justin Carpentier committed
37
38
  const double mu = 0.3;
  const double ZMP_radius = 0.01;
Guilhem Saurel's avatar
Guilhem Saurel committed
39

Guilhem Saurel's avatar
Guilhem Saurel committed
40
  ContactModelPlanar mp1(mu, ZMP_radius);
Justin Carpentier's avatar
Justin Carpentier committed
41
  ContactModelPlanar mp2(mp1);
Guilhem Saurel's avatar
Guilhem Saurel committed
42

Justin Carpentier's avatar
Justin Carpentier committed
43
44
45
  BOOST_CHECK(mp1 == mp2);
}

Guilhem Saurel's avatar
Guilhem Saurel committed
46
BOOST_AUTO_TEST_CASE(contact_patch) {
Pierre Fernbach's avatar
Pierre Fernbach committed
47
  // check default constructor :
Justin Carpentier's avatar
Justin Carpentier committed
48
  ContactPatch cp;
Pierre Fernbach's avatar
Pierre Fernbach committed
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
  BOOST_CHECK(cp.placement() == SE3::Identity());
  BOOST_CHECK(cp.friction() == -1.);
  SE3 p = SE3::Identity();
  p.setRandom();
  cp.placement() = p;
  BOOST_CHECK(cp.placement() == p);
  cp.friction() = 0.7;
  BOOST_CHECK(cp.friction() == 0.7);

  // constructor with placement :
  p.setRandom();
  ContactPatch cp1(p);
  BOOST_CHECK(cp1.placement() == p);
  BOOST_CHECK(cp1.friction() == -1.);

  // constructor with placement and friction
  p.setRandom();
  ContactPatch cp2(p,0.9);
  BOOST_CHECK(cp2.placement() == p);
  BOOST_CHECK(cp2.friction() == 0.9);

  // check comparison operator
  BOOST_CHECK(cp1 != cp2);
  ContactPatch cp3(p,0.9);
  BOOST_CHECK(cp3 == cp2);
  cp2.friction() = 0.1;
  BOOST_CHECK(cp3 != cp2);

  // copy constructor
  ContactPatch cp4(cp3);
  BOOST_CHECK(cp4 == cp3);
  BOOST_CHECK(cp4.placement() == p);
  BOOST_CHECK(cp4.friction() == 0.9);
  cp4.placement() = SE3::Identity();
  BOOST_CHECK(cp3 != cp4);

  // serialization :
  std::string fileName("fileTest");
  cp3.saveAsText(fileName);
  ContactPatch cp_from_text;
  cp_from_text.loadFromText(fileName);
  BOOST_CHECK(cp3 == cp_from_text);

  cp3.saveAsXML(fileName,"ContactPatch");
  ContactPatch cp_from_xml;
  cp_from_xml.loadFromXML(fileName,"ContactPatch");
  BOOST_CHECK(cp3 == cp_from_xml);

  cp3.saveAsBinary(fileName);
  ContactPatch cp_from_bin;
  cp_from_bin.loadFromBinary(fileName);
  BOOST_CHECK(cp3 == cp_from_bin);
Justin Carpentier's avatar
Justin Carpentier committed
101
102
}

Pierre Fernbach's avatar
Pierre Fernbach committed
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
//BOOST_AUTO_TEST_CASE(contact_phase)
//{
//  ContactPhase4 cp, cp_test;
//  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();
//  }
//  ContactPhase4 cp2(cp);
//  BOOST_CHECK(cp == cp);
//  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);
//}

//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);
//}
137

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