quadruped_time.hxx
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_time_hxx__
2 #define __quadruped_walkgen_quadruped_time_hxx__
3 
4 #include "crocoddyl/core/utils/exception.hpp"
5 
6 namespace quadruped_walkgen {
7 template <typename Scalar>
9  : crocoddyl::ActionModelAbstractTpl<Scalar>(
10  boost::make_shared<crocoddyl::StateVectorTpl<Scalar> >(21), 1, 22) {
11  // 4 costs
12  state_weights_ << Scalar(1.), Scalar(1.), Scalar(150.), Scalar(35.),
13  Scalar(30.), Scalar(8.), Scalar(20.), Scalar(20.), Scalar(15.),
14  Scalar(4.), Scalar(4.), Scalar(8.);
15  heuristic_weights_.setConstant(Scalar(1));
16 
17  dt_bound_weight_cmd = Scalar(100.);
18  dt_weight_cmd = Scalar(0.);
19 
20  pheuristic_.setZero();
21  gait_double_.setZero();
22 
23  // Shoulder heuristic position
24  // pshoulder_ << Scalar(0.1946) , Scalar(0.15005), Scalar(0.1946) ,
25  // Scalar(-0.15005) ,
26  // Scalar(-0.1946), Scalar(0.15005) , Scalar(-0.1946),
27  // Scalar(-0.15005) ;
28  // pshoulder_0 << Scalar(0.1946) , Scalar(0.1946) , Scalar(-0.1946),
29  // Scalar(-0.1946) ,
30  // Scalar(0.15005) , Scalar(-0.15005) , Scalar(0.15005) ,
31  // Scalar(-0.15005) ;
32  // pshoulder_tmp.setZero() ;
33  // pcentrifugal_tmp_1.setZero() ;
34  // pcentrifugal_tmp_2.setZero() ;
35  // pcentrifugal_tmp.setZero() ;
36  centrifugal_term = true;
37  symmetry_term = true;
38  T_gait = Scalar(0.64);
39  // R_tmp.setZero() ;
40 
41  // Cost relative to the command
42  rub_max_.setZero();
43  rub_max_bool.setZero();
44  dt_ref_.setConstant(Scalar(0.02));
45  dt_min_.setConstant(Scalar(0.005));
46  dt_max_.setConstant(Scalar(0.1));
47 
48  // Log cost
49  cost_.setZero();
50  log_cost = true;
51 }
52 
53 template <typename Scalar>
55 
56 template <typename Scalar>
58  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
59  const Eigen::Ref<const typename MathBase::VectorXs>& x,
60  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
61  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
62  throw_pretty("Invalid argument: "
63  << "x has wrong dimension (it should be " +
64  std::to_string(state_->get_nx()) + ")");
65  }
66  if (static_cast<std::size_t>(u.size()) != nu_) {
67  throw_pretty("Invalid argument: "
68  << "u has wrong dimension (it should be " +
69  std::to_string(nu_) + ")");
70  }
71 
73  static_cast<ActionDataQuadrupedTimeTpl<Scalar>*>(data.get());
74 
75  d->xnext.template head<12>() = x.head(12);
76  d->xnext.template segment<8>(12) = x.segment(12, 8);
77  d->xnext.template tail<1>() = u.cwiseAbs();
78 
79  // Residual cost on the state and force norm
80  // State : delta*||X-Xref||
81  d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
82  // Feet placement : delta*||P-Pref||
83  d->r.template segment<8>(12) =
84  ((heuristic_weights_.cwiseProduct(x.segment(12, 8) - pheuristic_))
85  .array() *
86  gait_double_.array())
87  .matrix();
88  // Dt command, used to fix the optimisation to dt_ref_
89  d->r.template tail<1>() << dt_weight_cmd * (u.cwiseAbs() - dt_ref_);
90 
91  // Penalisation if dt out of lower/upper bound
92  rub_max_ << dt_min_ - u.cwiseAbs(), u.cwiseAbs() - dt_max_;
93  rub_max_bool =
94  (rub_max_.array() >= Scalar(0.)).matrix().template cast<Scalar>();
95  rub_max_ = rub_max_.cwiseMax(Scalar(0.));
96 
97  d->cost = Scalar(0.5) * d->r.transpose() * d->r +
98  dt_bound_weight_cmd * Scalar(0.5) * rub_max_.squaredNorm();
99 
100  if (log_cost) {
101  // Length of the cost similar to the Augmented cost...
102  cost_[0] =
103  Scalar(0.5) * d->r.head(12).transpose() * d->r.head(12); // state
104  cost_[1] = Scalar(0.5) * d->r.segment(12, 8).transpose() *
105  d->r.segment(12, 8); // feet placement
106  cost_[2] = Scalar(0.5) * d->r.tail(1).transpose() *
107  d->r.tail(1); // 0.5*\delta*|| U - dt_ref ||^2
108  cost_[3] = dt_bound_weight_cmd * Scalar(0.5) *
109  rub_max_.squaredNorm(); // u/l bound weight
110  cost_[4] = Scalar(0);
111  cost_[5] = Scalar(0);
112  cost_[6] = Scalar(0);
113  }
114 }
115 
116 template <typename Scalar>
118  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
119  const Eigen::Ref<const typename MathBase::VectorXs>& x,
120  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
121  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
122  throw_pretty("Invalid argument: "
123  << "x has wrong dimension (it should be " +
124  std::to_string(state_->get_nx()) + ")");
125  }
126  if (static_cast<std::size_t>(u.size()) != nu_) {
127  throw_pretty("Invalid argument: "
128  << "u has wrong dimension (it should be " +
129  std::to_string(nu_) + ")");
130  }
131 
133  static_cast<ActionDataQuadrupedTimeTpl<Scalar>*>(data.get());
134 
135  // Cost derivatives : Lx
136  d->Lx.template head<12>() =
137  (state_weights_.array() * d->r.template head<12>().array()).matrix();
138  d->Lx.template segment<8>(12) =
139  (heuristic_weights_.array() * d->r.template segment<8>(12).array())
140  .matrix(); // * gait_double in d->r
141 
142  d->Lu << dt_bound_weight_cmd * std::copysign(1., u(0)) *
143  (-rub_max_[0] + rub_max_[1]);
144  d->Lu += dt_weight_cmd * std::copysign(1., u(0)) * d->r.template tail<1>();
145 
146  // Hessian : Lxx
147  d->Lxx.diagonal().head(12) =
148  (state_weights_.array() * state_weights_.array()).matrix();
149  d->Lxx.diagonal().segment(12, 8) =
150  (gait_double_.array() * heuristic_weights_.array() *
151  heuristic_weights_.array())
152  .matrix();
153 
154  d->Luu.diagonal() << dt_weight_cmd * dt_weight_cmd +
155  dt_bound_weight_cmd * rub_max_bool[0] +
156  dt_bound_weight_cmd * rub_max_bool[1];
157 
158  // Dynamic derivatives
159  d->Fx.setIdentity();
160  d->Fx(20, 20) = Scalar(0.);
161  d->Fu.block(20, 0, 1, 1) << std::copysign(1., u(0));
162 }
163 
164 template <typename Scalar>
165 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
167  return boost::make_shared<ActionDataQuadrupedTimeTpl<Scalar> >(this);
168 }
169 
171 // get & set state weight //////
173 
174 template <typename Scalar>
175 const typename Eigen::Matrix<Scalar, 12, 1>&
177  return state_weights_;
178 }
179 template <typename Scalar>
181  const typename MathBase::VectorXs& weights) {
182  if (static_cast<std::size_t>(weights.size()) != 12) {
183  throw_pretty("Invalid argument: "
184  << "Weights vector has wrong dimension (it should be 12)");
185  }
186  state_weights_ = weights;
187 }
188 
189 template <typename Scalar>
190 const typename Eigen::Matrix<Scalar, 8, 1>&
192  return heuristic_weights_;
193 }
194 template <typename Scalar>
196  const typename MathBase::VectorXs& weights) {
197  if (static_cast<std::size_t>(weights.size()) != 8) {
198  throw_pretty("Invalid argument: "
199  << "Weights vector has wrong dimension (it should be 8)");
200  }
201  heuristic_weights_ = weights;
202 }
203 
205 // Param relative to the heuristic position //
207 
208 template <typename Scalar>
210  return symmetry_term;
211 }
212 template <typename Scalar>
214  const bool& sym_term) {
215  // The model need to be updated after this changed
216  symmetry_term = sym_term;
217 }
218 
219 template <typename Scalar>
221  return centrifugal_term;
222 }
223 template <typename Scalar>
225  const bool& cent_term) {
226  // The model need to be updated after this changed
227  centrifugal_term = cent_term;
228 }
229 
230 template <typename Scalar>
232  // The model need to be updated after this change
233  return T_gait;
234 }
235 template <typename Scalar>
237  // The model need to be updated after this change
238  T_gait = T_gait_;
239 }
240 
241 // dt_ref to fix the dt optimised
242 template <typename Scalar>
244  return dt_ref_[0];
245 }
246 template <typename Scalar>
248  dt_ref_[0] = dt;
249 }
250 
252 // Upper and lower bound dt //
254 template <typename Scalar>
256  return dt_min_[0];
257 }
258 template <typename Scalar>
260  dt_min_[0] = dt;
261 }
262 template <typename Scalar>
264  return dt_max_[0];
265 }
266 template <typename Scalar>
268  dt_max_[0] = dt;
269 }
271 // Get access and modify to the upper/lower bound for dt //
273 template <typename Scalar>
275  const {
276  return dt_bound_weight_cmd;
277 }
278 template <typename Scalar>
280  const Scalar& weight_) {
281  dt_bound_weight_cmd = weight_;
282 }
283 
285 // Get access and modify to the \weight*||U-dt_ref|| weight //
287 template <typename Scalar>
289  return dt_weight_cmd;
290 }
291 template <typename Scalar>
293  const Scalar& weight_) {
294  dt_weight_cmd = weight_;
295 }
296 
298 // Logging cost //
300 template <typename Scalar>
301 const typename Eigen::Matrix<Scalar, 7, 1>&
303  return cost_;
304 }
305 
307 // Update current model
309 
310 template <typename Scalar>
312  const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
313  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
314  const Eigen::Ref<const typename MathBase::VectorXs>& S) {
315  if (static_cast<std::size_t>(l_feet.size()) != 12) {
316  throw_pretty("Invalid argument: "
317  << "l_feet matrix has wrong dimension (it should be : 3x4)");
318  }
319  if (static_cast<std::size_t>(xref.size()) != 12) {
320  throw_pretty("Invalid argument: "
321  << "Weights vector has wrong dimension (it should be " +
322  std::to_string(state_->get_nx()) + ")");
323  }
324  if (static_cast<std::size_t>(S.size()) != 4) {
325  throw_pretty("Invalid argument: "
326  << "S vector has wrong dimension (it should be 4x1)");
327  }
328  // l_feet and S useless, kept for now, to be consistent with others models
329 
330  xref_ = xref;
331  for (int i = 0; i < 4; i = i + 1) {
332  gait_double_[2 * i] = S[i];
333  gait_double_[2 * i + 1] = S[i];
334  pheuristic_[2 * i] = l_feet(0, i);
335  pheuristic_[2 * i + 1] = l_feet(1, i);
336  }
337 
338  // To compute heuristic here
339  // R_tmp << cos(xref(5,0)) ,-sin(xref(5,0)) , Scalar(0),
340  // sin(xref(5,0)), cos(xref(5,0)), Scalar(0),
341  // Scalar(0),Scalar(0),Scalar(1.0) ;
342 
343  // // Centrifual term
344  // pcentrifugal_tmp_1 = xref.block(6,0,3,1) ;
345  // pcentrifugal_tmp_2 = xref.block(9,0,3,1) ;
346  // pcentrifugal_tmp = 0.5*std::sqrt(xref(2,0)/9.81) *
347  // pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2) ;
348 
349  // for (int i=0; i<4; i=i+1){
350  // pshoulder_tmp.block(0,i,2,1) =
351  // R_tmp.block(0,0,2,2)*(pshoulder_0.block(0,i,2,1) + symmetry_term *
352  // 0.25*T_gait*xref.block(6,0,2,1) + centrifugal_term *
353  // pcentrifugal_tmp.block(0,0,2,1) ); pshoulder_[2*i] = pshoulder_tmp(0,i) +
354  // xref(0,0); pshoulder_[2*i+1] = pshoulder_tmp(1,i) + xref(1,0);
355  // }
356 }
357 } // namespace quadruped_walkgen
358 
359 #endif
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_max
const Scalar & get_dt_max() const
Definition: quadruped_time.hxx:263
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_time.hxx:220
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_weight_cmd
const Scalar & get_dt_weight_cmd() const
Definition: quadruped_time.hxx:288
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_weight_cmd
void set_dt_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:292
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_time.hxx:213
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_cost
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_time.hxx:302
quadruped_walkgen::ActionDataQuadrupedTimeTpl
Definition: quadruped_time.hpp:118
quadruped_walkgen::ActionModelQuadrupedTimeTpl::ActionModelQuadrupedTimeTpl
ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:8
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_ref
const Scalar & get_dt_ref() const
Definition: quadruped_time.hxx:243
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_time.hxx:231
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_time.hxx:236
quadruped_walkgen::ActionModelQuadrupedTimeTpl::~ActionModelQuadrupedTimeTpl
~ActionModelQuadrupedTimeTpl()
Definition: quadruped_time.hxx:54
quadruped_walkgen::ActionModelQuadrupedTimeTpl::calcDiff
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_time.hxx:117
quadruped_walkgen::ActionModelQuadrupedTimeTpl::calc
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_time.hxx:57
quadruped_walkgen::ActionModelQuadrupedTimeTpl::update_model
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::VectorXs > &S)
Definition: quadruped_time.hxx:311
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_time.hxx:224
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_bound_weight_cmd
const Scalar & get_dt_bound_weight_cmd() const
Definition: quadruped_time.hxx:274
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_heuristic_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_time.hxx:191
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_dt_min
const Scalar & get_dt_min() const
Definition: quadruped_time.hxx:255
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:180
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_min
void set_dt_min(const Scalar &dt)
Definition: quadruped_time.hxx:259
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_time.hxx:176
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_max
void set_dt_max(const Scalar &dt)
Definition: quadruped_time.hxx:267
quadruped_walkgen::ActionModelQuadrupedTimeTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_time.hxx:166
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_heuristic_weights
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_time.hxx:195
quadruped_walkgen::ActionModelQuadrupedTimeTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_time.hxx:209
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_bound_weight_cmd
void set_dt_bound_weight_cmd(const Scalar &weight_)
Definition: quadruped_time.hxx:279
quadruped_walkgen::ActionModelQuadrupedTimeTpl::set_dt_ref
void set_dt_ref(const Scalar &dt)
Definition: quadruped_time.hxx:247
quadruped_walkgen::ActionModelQuadrupedTimeTpl::Scalar
_Scalar Scalar
Definition: quadruped_time.hpp:16