scenario.cpp 2.33 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
12
#include "multicontact-api/scenario/contact-phase.hpp"
#include "multicontact-api/scenario/contact-phase-humanoid.hpp"
#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
16
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
22
  explicit ATpl() : data() {}
  explicit ATpl(const ATpl & other) : data(other.data) {};
Guilhem Saurel's avatar
Guilhem Saurel committed
23

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

Justin Carpentier's avatar
Justin Carpentier committed
26
protected:
Guilhem Saurel's avatar
Guilhem Saurel committed
27

Justin Carpentier's avatar
Justin Carpentier committed
28
29
30
31
32
33
34
35
36
37
38
39
  SE3 data;
};

typedef ATpl<double> Ad;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)


BOOST_AUTO_TEST_CASE(contact_model)
{
  const double mu = 0.3;
  const double ZMP_radius = 0.01;
Guilhem Saurel's avatar
Guilhem Saurel committed
40

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

Justin Carpentier's avatar
Justin Carpentier committed
44
45
46
47
48
49
50
51
52
53
54
55
56
  BOOST_CHECK(mp1 == mp2);
}

BOOST_AUTO_TEST_CASE(contact_patch)
{
  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
57

Justin Carpentier's avatar
Justin Carpentier committed
58
59
  BOOST_CHECK(cp == cp);
  BOOST_CHECK(cp == cp2);
Guilhem Saurel's avatar
Guilhem Saurel committed
60

Justin Carpentier's avatar
Justin Carpentier committed
61
62
  Ad a1;
  Ad a2(a1);
Guilhem Saurel's avatar
Guilhem Saurel committed
63

Justin Carpentier's avatar
Justin Carpentier committed
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
}

BOOST_AUTO_TEST_CASE(contact_phase)
{
  ContactPhase4 cp;
  for(ContactPhase4::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;
    it->placement().setRandom();
    it->contactModelPlacement().setRandom();
    it->worldContactModelPlacement().setRandom();
  }
  ContactPhase4 cp2(cp);
Guilhem Saurel's avatar
Guilhem Saurel committed
79

Justin Carpentier's avatar
Justin Carpentier committed
80
81
82
83
84
85
86
87
88
89
90
91
92
93
  BOOST_CHECK(cp == cp);
//  BOOST_CHECK(cp == cp2);
}
//
//BOOST_AUTO_TEST_CASE(contact_phase_humanoid)
//{
//  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
94
//
Justin Carpentier's avatar
Justin Carpentier committed
95
96
//  BOOST_CHECK(cp == cp);
//  BOOST_CHECK(cp == cp2);
Guilhem Saurel's avatar
Guilhem Saurel committed
97
//
Justin Carpentier's avatar
Justin Carpentier committed
98
//  cp.m_reference_configurations.push_back(Eigen::VectorXd::Zero(10));
Guilhem Saurel's avatar
Guilhem Saurel committed
99
//
Justin Carpentier's avatar
Justin Carpentier committed
100
101
102
103
////  BOOST_CHECK(cp != cp2);
//}

BOOST_AUTO_TEST_SUITE_END()