Commit c9549f5f authored by Sébastien Kleff's avatar Sébastien Kleff
Browse files

separate terminal calc and calcDiff + improved derivatives w.R.T. w, stil debugging

parent e11db975
......@@ -54,9 +54,17 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y,
const Eigen::Ref<const VectorXs>& w);
virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y);
virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y,
const Eigen::Ref<const VectorXs>& w);
virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y);
virtual boost::shared_ptr<ActionDataAbstract> createData();
virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
......@@ -82,6 +90,9 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
const std::vector<int>& get_lpf_torque_ids() const {
return lpf_torque_ids_;
};
const std::vector<int>& get_non_lpf_torque_ids() const {
return non_lpf_torque_ids_;
};
void set_dt(const Scalar& dt);
void set_fc(const Scalar& fc);
......@@ -120,7 +131,7 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
Scalar alpha_;
bool with_cost_residual_;
Scalar fc_;
bool enable_integration_;
// bool enable_integration_;
Scalar tauReg_weight_; //!< Cost weight for unfiltered torque regularization
VectorXs tauReg_reference_; //!< Cost reference for unfiltered torque
//!< regularization
......@@ -135,14 +146,13 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
boost::shared_ptr<PinocchioModel> pin_model_; //!< for reg cost
bool is_terminal_; //!< is it a terminal model or not ? (deactivate cost on w
//!< if true)
std::vector<std::string>
lpf_joint_names_; //!< Vector of joint names that are low-pass filtered
std::vector<int>
lpf_joint_ids_; //!< Vector of joint ids that are low-pass filtered
// std::vector<int> nonlpf_joint_ids_; //!< Vector of joint ids that are
// NOT low-pass filtered
std::vector<int>
lpf_torque_ids_; //!< Vector of torque ids that are low-passs filtered
std::vector<std::string> lpf_joint_names_; //!< Vector of joint names that are low-pass filtered
std::vector<int> lpf_joint_ids_; //!< Vector of joint ids that are low-pass filtered
std::vector<int> lpf_torque_ids_; //!< Vector of torque ids that are low-passs filtered
// std::vector<std::string> non_lpf_joint_names_; //!< Vector of joint names that are NOT low-pass filtered
std::vector<int> non_lpf_joint_ids_; //!< Vector of joint ids that are NOT low-pass filtered
std::vector<int> non_lpf_torque_ids_; //!< Vector of torque ids that are NOT low-passs filtered
};
template <typename _Scalar>
......
......@@ -275,11 +275,13 @@ void test_calcDiff_explicit_equivalent_euler(
const Eigen::VectorXd x = y.head(nx);
Eigen::VectorXd tau = w;
const std::vector<int>& lpf_torque_ids = modelLPF->get_lpf_torque_ids();
const std::vector<int>& non_lpf_torque_ids = modelLPF->get_non_lpf_torque_ids();
for(std::size_t i=0; i<lpf_torque_ids.size();i++){
tau(lpf_torque_ids[i]) = y.tail(ntau)[i];
}
BOOST_CHECK(non_lpf_torque_ids.size() + lpf_torque_ids.size() == nu );
// Checking the partial derivatives against NumDiff
double tol = 1e-3;
double tol = 1e-3; //1e-3
// Computing the action
modelLPF->calc(dataLPF, y, w);
......@@ -303,13 +305,33 @@ void test_calcDiff_explicit_equivalent_euler(
const Eigen::MatrixXd& Fx_LPF = dataLPF->Fx.topLeftCorner(ndx, ndx);
const Eigen::MatrixXd& Lx_LPF = dataLPF->Lx.head(ndx);
const Eigen::MatrixXd& Lxx_LPF = dataLPF->Lxx.topLeftCorner(ndx, ndx);
if(! (Lxx_LPF - dataEuler->Lxx).isZero(tol) ){
std::cout << Lxx_LPF - dataEuler->Lxx << std::endl;
}
// if(! (Lxx_LPF - dataEuler->Lxx).isZero(tol) ){
// std::cout << Lxx_LPF - dataEuler->Lxx << std::endl;
// }
// Testing the partials w.r.t. u match blocks in partial w.r.t. augmented state y
BOOST_CHECK((Fx_LPF - dataEuler->Fx).isZero(tol));
BOOST_CHECK((Lx_LPF - dataEuler->Lx).isZero(tol));
BOOST_CHECK((Lxx_LPF - dataEuler->Lxx).isZero(tol));
// // Non LPF dimensions
// // Size varying stuff
// const Eigen::MatrixXd& Fu_nonLPF = dataLPF->Fu.topRows(ndx);
// // const Eigen::MatrixXd& Lu_nonLPF = dataLPF->Lw;
// // const Eigen::MatrixXd& Lxu_nonLPF = dataLPF->Lxx.topRightCorner(ndx, ntau);
// // const Eigen::MatrixXd& Luu_nonLPF = dataLPF->Lxx.bottomRightCorner(ntau, ntau);
// for(std::size_t i=0; i<lpf_torque_ids.size();i++){
// BOOST_CHECK((Fu_nonLPF.col(i) - dataEuler->Fu.col(lpf_torque_ids[i])).isZero(tol));
// // BOOST_CHECK((Lu_LPF(i) - dataEuler->Lu(lpf_torque_ids[i])) <= tol);
// // BOOST_CHECK((Lxu_LPF.col(i) - dataEuler->Lxu.col(lpf_torque_ids[i])).isZero(tol));
// }
// // Fixed size stuff
// const Eigen::MatrixXd& Fx_LPF = dataLPF->Fx.topLeftCorner(ndx, ndx);
// const Eigen::MatrixXd& Lx_LPF = dataLPF->Lx.head(ndx);
// const Eigen::MatrixXd& Lxx_LPF = dataLPF->Lxx.topLeftCorner(ndx, ndx);
// // Testing the partials w.r.t. u match blocks in partial w.r.t. augmented state y
// BOOST_CHECK((Fx_LPF - dataEuler->Fx).isZero(tol));
// BOOST_CHECK((Lx_LPF - dataEuler->Lx).isZero(tol));
// BOOST_CHECK((Lxx_LPF - dataEuler->Lxx).isZero(tol));
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment