Loading...
Searching...
No Matches
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
6namespace quadruped_walkgen {
7template <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
53template <typename Scalar>
55
56template <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
116template <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
164template <typename Scalar>
165boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
167 return boost::make_shared<ActionDataQuadrupedTimeTpl<Scalar> >(this);
168}
169
171// get & set state weight //////
173
174template <typename Scalar>
175const typename Eigen::Matrix<Scalar, 12, 1>&
177 return state_weights_;
178}
179template <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
189template <typename Scalar>
190const typename Eigen::Matrix<Scalar, 8, 1>&
192 return heuristic_weights_;
193}
194template <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
208template <typename Scalar>
210 return symmetry_term;
211}
212template <typename Scalar>
214 const bool& sym_term) {
215 // The model need to be updated after this changed
216 symmetry_term = sym_term;
217}
218
219template <typename Scalar>
221 return centrifugal_term;
222}
223template <typename Scalar>
225 const bool& cent_term) {
226 // The model need to be updated after this changed
227 centrifugal_term = cent_term;
228}
229
230template <typename Scalar>
232 // The model need to be updated after this change
233 return T_gait;
234}
235template <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
242template <typename Scalar>
244 return dt_ref_[0];
245}
246template <typename Scalar>
248 dt_ref_[0] = dt;
249}
250
252// Upper and lower bound dt //
254template <typename Scalar>
256 return dt_min_[0];
257}
258template <typename Scalar>
260 dt_min_[0] = dt;
261}
262template <typename Scalar>
264 return dt_max_[0];
265}
266template <typename Scalar>
268 dt_max_[0] = dt;
269}
271// Get access and modify to the upper/lower bound for dt //
273template <typename Scalar>
275 const {
276 return dt_bound_weight_cmd;
277}
278template <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 //
287template <typename Scalar>
289 return dt_weight_cmd;
290}
291template <typename Scalar>
296
298// Logging cost //
300template <typename Scalar>
301const typename Eigen::Matrix<Scalar, 7, 1>&
303 return cost_;
304}
305
307// Update current model
309
310template <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
Definition quadruped_augmented_time.hpp:14
ActionModelQuadrupedAugmentedTimeTpl()
Definition quadruped_augmented_time.hxx:9
_Scalar Scalar
Definition quadruped_augmented_time.hpp:16
ActionModelQuadrupedTimeTpl()
Definition quadruped_time.hxx:8
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_time.hxx:195
~ActionModelQuadrupedTimeTpl()
Definition quadruped_time.hxx:54
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition quadruped_time.hxx:166
void set_T_gait(const Scalar &T_gait_)
Definition quadruped_time.hxx:236
const Scalar & get_T_gait() const
Definition quadruped_time.hxx:231
const Scalar & get_dt_max() const
Definition quadruped_time.hxx:263
const Scalar & get_dt_weight_cmd() const
Definition quadruped_time.hxx:288
const Scalar & get_dt_bound_weight_cmd() const
Definition quadruped_time.hxx:274
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
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
void set_dt_weight_cmd(const Scalar &weight_)
Definition quadruped_time.hxx:292
void set_dt_max(const Scalar &dt)
Definition quadruped_time.hxx:267
void set_symmetry_term(const bool &sym_term)
Definition quadruped_time.hxx:213
void set_centrifugal_term(const bool &cent_term)
Definition quadruped_time.hxx:224
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition quadruped_time.hxx:191
const Scalar & get_dt_min() const
Definition quadruped_time.hxx:255
void set_dt_ref(const Scalar &dt)
Definition quadruped_time.hxx:247
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition quadruped_time.hxx:302
_Scalar Scalar
Definition quadruped_time.hpp:16
const bool & get_symmetry_term() const
Definition quadruped_time.hxx:209
const bool & get_centrifugal_term() const
Definition quadruped_time.hxx:220
const Scalar & get_dt_ref() const
Definition quadruped_time.hxx:243
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
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_time.hxx:180
void set_dt_min(const Scalar &dt)
Definition quadruped_time.hxx:259
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition quadruped_time.hxx:176
void set_dt_bound_weight_cmd(const Scalar &weight_)
Definition quadruped_time.hxx:279
Definition quadruped.hpp:11