scenario.cpp 2.3 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
11
#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"
Justin Carpentier's avatar
Justin Carpentier committed
12

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

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

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

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

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

typedef ATpl<double> Ad;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
42
BOOST_AUTO_TEST_CASE(contact_patch) {
Justin Carpentier's avatar
Justin Carpentier committed
43
44
45
46
47
48
49
50
  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;
Guilhem Saurel's avatar
Guilhem Saurel committed
51

Justin Carpentier's avatar
Justin Carpentier committed
52
53
  BOOST_CHECK(cp == cp);
  BOOST_CHECK(cp == cp2);
Guilhem Saurel's avatar
Guilhem Saurel committed
54

Justin Carpentier's avatar
Justin Carpentier committed
55
56
57
58
  Ad a1;
  Ad a2(a1);
}

Guilhem Saurel's avatar
Guilhem Saurel committed
59
BOOST_AUTO_TEST_CASE(contact_phase) {
Justin Carpentier's avatar
Justin Carpentier committed
60
  ContactPhase4 cp;
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
  }
  ContactPhase4 cp2(cp);
Guilhem Saurel's avatar
Guilhem Saurel committed
71

Justin Carpentier's avatar
Justin Carpentier committed
72
  BOOST_CHECK(cp == cp);
Guilhem Saurel's avatar
Guilhem Saurel committed
73
  //  BOOST_CHECK(cp == cp2);
Justin Carpentier's avatar
Justin Carpentier committed
74
75
}
//
Guilhem Saurel's avatar
Guilhem Saurel committed
76
// BOOST_AUTO_TEST_CASE(contact_phase_humanoid)
Justin Carpentier's avatar
Justin Carpentier committed
77
78
79
80
81
82
83
84
85
//{
//  ContactPhaseHumanoid cp;
//  for(ContactPhaseHumanoid::ContactPatchArray::iterator it = cp.contact_patches().begin();
//      it !=  cp.contact_patches().end(); ++it)
//  {
//    it->contactModel().m_mu = 0.3;
//    it->contactModel().m_ZMP_radius = 0.01;
//  }
//  ContactPhaseHumanoid cp2(cp);
Guilhem Saurel's avatar
Guilhem Saurel committed
86
//
Justin Carpentier's avatar
Justin Carpentier committed
87
88
//  BOOST_CHECK(cp == cp);
//  BOOST_CHECK(cp == cp2);
Guilhem Saurel's avatar
Guilhem Saurel committed
89
//
Justin Carpentier's avatar
Justin Carpentier committed
90
//  cp.m_reference_configurations.push_back(Eigen::VectorXd::Zero(10));
Guilhem Saurel's avatar
Guilhem Saurel committed
91
//
Justin Carpentier's avatar
Justin Carpentier committed
92
93
94
95
////  BOOST_CHECK(cp != cp2);
//}

BOOST_AUTO_TEST_SUITE_END()