scenario.cpp 3.59 KB
Newer Older
Justin Carpentier's avatar
Justin Carpentier committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>
// Simplified BSD license :
//Redistribution and use in source and binary forms, with or without modification,
//are permitted provided that the following conditions are met:

//1. Redistributions of source code must retain the above copyright notice,
//this list of conditions and the following disclaimer.

//2. Redistributions in binary form must reproduce the above copyright notice,
//this list of conditions and the following disclaimer in the documentation
//and/or other materials provided with the distribution.

//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
//AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
//THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
//ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
//LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
//OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
//PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
//OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
//WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
//OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
//ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <iostream>

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

#include "locomote/scenario/contact-phase.hpp"
#include "locomote/scenario/contact-phase-humanoid.hpp"
#include "locomote/scenario/contact-patch.hpp"

using namespace locomote::scenario;

template<typename Scalar>
struct ATpl
{
  typedef se3::SE3Tpl<Scalar> SE3;
  
  explicit ATpl() : data() {}
  explicit ATpl(const ATpl & other) : data(other.data) {};
  
  bool operator==(const ATpl & other) { return data == other.data; }
  
protected:
  
  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;
  
  ContactModelPlanar mp1(mu,ZMP_radius);
  ContactModelPlanar mp2(mp1);
  
  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;
  
  BOOST_CHECK(cp == cp);
  BOOST_CHECK(cp == cp2);
  
  Ad a1;
  Ad a2(a1);
  
}

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);
  
  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);
//  
//  BOOST_CHECK(cp == cp);
//  BOOST_CHECK(cp == cp2);
//  
//  cp.m_reference_configurations.push_back(Eigen::VectorXd::Zero(10));
//  
////  BOOST_CHECK(cp != cp2);
//}

BOOST_AUTO_TEST_SUITE_END()