GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/factory/constraint.cpp Lines: 166 173 96.0 %
Date: 2024-02-13 11:12:33 Branches: 144 267 53.9 %

Line Branch Exec Source
1
///////////////////////////////////////////////////////////////////////////////
2
// BSD 3-Clause License
3
//
4
// Copyright (C) 2020-2022, University of Edinburgh, IRI: CSIC-UPC
5
// Copyright note valid unless otherwise stated in individual files.
6
// All rights reserved.
7
///////////////////////////////////////////////////////////////////////////////
8
9
#include "constraint.hpp"
10
11
#include "crocoddyl/core/constraints/residual.hpp"
12
#include "crocoddyl/core/residuals/control.hpp"
13
#include "crocoddyl/core/utils/exception.hpp"
14
#include "crocoddyl/multibody/residuals/com-position.hpp"
15
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
16
#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
17
#include "crocoddyl/multibody/residuals/frame-translation.hpp"
18
#include "crocoddyl/multibody/residuals/frame-velocity.hpp"
19
#include "crocoddyl/multibody/residuals/state.hpp"
20
21
namespace crocoddyl {
22
namespace unittest {
23
24
const std::vector<ConstraintModelTypes::Type> ConstraintModelTypes::all(
25
    ConstraintModelTypes::init_all());
26
27
70
std::ostream& operator<<(std::ostream& os, ConstraintModelTypes::Type type) {
28




70
  switch (type) {
29
5
    case ConstraintModelTypes::ConstraintModelResidualStateEquality:
30
5
      os << "ConstraintModelResidualStateEquality";
31
5
      break;
32
5
    case ConstraintModelTypes::ConstraintModelResidualStateInequality:
33
5
      os << "ConstraintModelResidualStateInequality";
34
5
      break;
35
5
    case ConstraintModelTypes::ConstraintModelResidualControlEquality:
36
5
      os << "ConstraintModelResidualControlEquality";
37
5
      break;
38
5
    case ConstraintModelTypes::ConstraintModelResidualControlInequality:
39
5
      os << "ConstraintModelResidualControlInequality";
40
5
      break;
41
5
    case ConstraintModelTypes::ConstraintModelResidualCoMPositionEquality:
42
5
      os << "ConstraintModelResidualCoMPositionEquality";
43
5
      break;
44
5
    case ConstraintModelTypes::ConstraintModelResidualCoMPositionInequality:
45
5
      os << "ConstraintModelResidualCoMPositionInequality";
46
5
      break;
47
5
    case ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality:
48
5
      os << "ConstraintModelResidualFramePlacementEquality";
49
5
      break;
50
5
    case ConstraintModelTypes::ConstraintModelResidualFramePlacementInequality:
51
5
      os << "ConstraintModelResidualFramePlacementInequality";
52
5
      break;
53
5
    case ConstraintModelTypes::ConstraintModelResidualFrameRotationEquality:
54
5
      os << "ConstraintModelResidualFrameRotationEquality";
55
5
      break;
56
5
    case ConstraintModelTypes::ConstraintModelResidualFrameRotationInequality:
57
5
      os << "ConstraintModelResidualFrameRotationInequality";
58
5
      break;
59
5
    case ConstraintModelTypes::ConstraintModelResidualFrameTranslationEquality:
60
5
      os << "ConstraintModelResidualFrameTranslationEquality";
61
5
      break;
62
5
    case ConstraintModelTypes::
63
        ConstraintModelResidualFrameTranslationInequality:
64
5
      os << "ConstraintModelResidualFrameTranslationInequality";
65
5
      break;
66
5
    case ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality:
67
5
      os << "ConstraintModelResidualFrameVelocityEquality";
68
5
      break;
69
5
    case ConstraintModelTypes::ConstraintModelResidualFrameVelocityInequality:
70
5
      os << "ConstraintModelResidualFrameVelocityInequality";
71
5
      break;
72
    case ConstraintModelTypes::NbConstraintModelTypes:
73
      os << "NbConstraintModelTypes";
74
      break;
75
    default:
76
      break;
77
  }
78
70
  return os;
79
}
80
81
905
ConstraintModelFactory::ConstraintModelFactory() {}
82
905
ConstraintModelFactory::~ConstraintModelFactory() {}
83
84
boost::shared_ptr<crocoddyl::ConstraintModelAbstract>
85
905
ConstraintModelFactory::create(ConstraintModelTypes::Type constraint_type,
86
                               StateModelTypes::Type state_type,
87
                               std::size_t nu) const {
88
1810
  StateModelFactory state_factory;
89
905
  boost::shared_ptr<crocoddyl::ConstraintModelAbstract> constraint;
90
  boost::shared_ptr<crocoddyl::StateMultibody> state =
91
      boost::static_pointer_cast<crocoddyl::StateMultibody>(
92
1810
          state_factory.create(state_type));
93
905
  pinocchio::FrameIndex frame_index = state->get_pinocchio()->frames.size() - 1;
94
905
  pinocchio::SE3 frame_SE3 = pinocchio::SE3::Random();
95
905
  pinocchio::Motion frame_motion = pinocchio::Motion::Random();
96
905
  if (nu == std::numeric_limits<std::size_t>::max()) {
97
407
    nu = state->get_nv();
98
  }
99

1810
  Eigen::VectorXd lb, ub;
100



905
  switch (constraint_type) {
101
28
    case ConstraintModelTypes::ConstraintModelResidualStateEquality:
102
56
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
103
56
          state, boost::make_shared<crocoddyl::ResidualModelState>(
104
84
                     state, state->rand(), nu));
105
28
      break;
106
32
    case ConstraintModelTypes::ConstraintModelResidualStateInequality:
107

32
      lb = Eigen::VectorXd::Zero(state->get_ndx());
108

32
      ub = Eigen::VectorXd::Zero(state->get_ndx());
109



32
      state->diff(state->zero(), state->zero(), lb);
110



32
      state->diff(state->zero(), state->rand().cwiseAbs(), ub);
111
64
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
112
          state,
113
64
          boost::make_shared<crocoddyl::ResidualModelState>(state,
114
64
                                                            state->rand(), nu),
115
64
          lb, ub.cwiseAbs());
116
32
      break;
117
31
    case ConstraintModelTypes::ConstraintModelResidualControlEquality:
118
62
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
119
62
          state, boost::make_shared<crocoddyl::ResidualModelControl>(
120
62
                     state, Eigen::VectorXd::Random(nu)));
121
31
      break;
122
27
    case ConstraintModelTypes::ConstraintModelResidualControlInequality:
123

27
      lb = Eigen::VectorXd::Zero(nu);
124
27
      lb(0) = -INFINITY;
125

27
      ub = Eigen::VectorXd::Random(nu).cwiseAbs();
126
54
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
127
          state,
128
54
          boost::make_shared<crocoddyl::ResidualModelControl>(
129
27
              state, Eigen::VectorXd::Random(nu)),
130
27
          lb, ub);
131
27
      break;
132
31
    case ConstraintModelTypes::ConstraintModelResidualCoMPositionEquality:
133
62
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
134
62
          state, boost::make_shared<crocoddyl::ResidualModelCoMPosition>(
135
62
                     state, Eigen::Vector3d::Random(), nu));
136
31
      break;
137
32
    case ConstraintModelTypes::ConstraintModelResidualCoMPositionInequality:
138

32
      lb = Eigen::Vector3d(0., -INFINITY, 0.);
139

32
      ub = Eigen::Vector3d::Random().cwiseAbs();
140
32
      ub(2) = INFINITY;
141
64
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
142
          state,
143
64
          boost::make_shared<crocoddyl::ResidualModelCoMPosition>(
144
32
              state, Eigen::Vector3d::Random(), nu),
145
32
          lb, ub);
146
32
      break;
147
281
    case ConstraintModelTypes::ConstraintModelResidualFramePlacementEquality:
148
562
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
149
562
          state, boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
150
281
                     state, frame_index, frame_SE3, nu));
151
281
      break;
152
28
    case ConstraintModelTypes::ConstraintModelResidualFramePlacementInequality:
153

28
      lb = MathBaseTpl<double>::Vector6s::Zero();
154


28
      lb.tail(3) << -INFINITY, -INFINITY, -INFINITY;
155

28
      ub = MathBaseTpl<double>::Vector6s::Random().cwiseAbs();
156


28
      ub.head(3) << INFINITY, INFINITY, INFINITY;
157
56
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
158
          state,
159
56
          boost::make_shared<crocoddyl::ResidualModelFramePlacement>(
160
              state, frame_index, frame_SE3, nu),
161
28
          lb, ub);
162
28
      break;
163
26
    case ConstraintModelTypes::ConstraintModelResidualFrameRotationEquality:
164
52
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
165
26
          state, boost::make_shared<crocoddyl::ResidualModelFrameRotation>(
166

52
                     state, frame_index, frame_SE3.rotation(), nu));
167
26
      break;
168
30
    case ConstraintModelTypes::ConstraintModelResidualFrameRotationInequality:
169

30
      lb = Eigen::Vector3d::Zero();
170
30
      lb(1) = -INFINITY;
171

30
      ub = Eigen::Vector3d::Random().cwiseAbs();
172
30
      ub(2) = INFINITY;
173
60
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
174
          state,
175
30
          boost::make_shared<crocoddyl::ResidualModelFrameRotation>(
176

30
              state, frame_index, frame_SE3.rotation(), nu),
177
30
          lb, ub);
178
30
      break;
179
25
    case ConstraintModelTypes::ConstraintModelResidualFrameTranslationEquality:
180
50
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
181
25
          state, boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
182

50
                     state, frame_index, frame_SE3.translation(), nu));
183
25
      break;
184
27
    case ConstraintModelTypes::
185
        ConstraintModelResidualFrameTranslationInequality:
186


27
      lb = -1 * Eigen::Vector3d::Random().cwiseAbs();
187

27
      ub = Eigen::Vector3d::Random().cwiseAbs();
188
54
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
189
          state,
190
27
          boost::make_shared<crocoddyl::ResidualModelFrameTranslation>(
191

27
              state, frame_index, frame_SE3.translation(), nu),
192
27
          lb, ub);
193
27
      break;
194
278
    case ConstraintModelTypes::ConstraintModelResidualFrameVelocityEquality:
195
556
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
196
278
          state, boost::make_shared<crocoddyl::ResidualModelFrameVelocity>(
197
556
                     state, frame_index, frame_motion, pinocchio::LOCAL, nu));
198
278
      break;
199
29
    case ConstraintModelTypes::ConstraintModelResidualFrameVelocityInequality:
200


29
      lb = -1 * MathBaseTpl<double>::Vector6s::Random().cwiseAbs();
201
29
      lb(0) = -INFINITY;
202

29
      ub = MathBaseTpl<double>::Vector6s::Random().cwiseAbs();
203
29
      ub(0) = INFINITY;
204
58
      constraint = boost::make_shared<crocoddyl::ConstraintModelResidual>(
205
          state,
206
29
          boost::make_shared<crocoddyl::ResidualModelFrameVelocity>(
207
29
              state, frame_index, frame_motion, pinocchio::LOCAL, nu),
208
29
          lb, ub);
209
29
      break;
210
    default:
211
      throw_pretty(__FILE__ ": Wrong ConstraintModelType::Type given");
212
      break;
213
  }
214
1810
  return constraint;
215
}
216
217
57
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> create_random_constraint(
218
    StateModelTypes::Type state_type) {
219
  static bool once = true;
220
57
  if (once) {
221
1
    srand((unsigned)time(NULL));
222
1
    once = false;
223
  }
224
225
114
  ConstraintModelFactory factory;
226
  ConstraintModelTypes::Type rand_type =
227
      static_cast<ConstraintModelTypes::Type>(
228
57
          rand() % ConstraintModelTypes::NbConstraintModelTypes);
229
114
  return factory.create(rand_type, state_type);
230
}
231
232
}  // namespace unittest
233
}  // namespace crocoddyl