From d7202fb99d2fcc589270cd8f04faf02409d1b531 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Jun 2024 17:02:46 +0000 Subject: [PATCH] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- examples/walk_without_think/walk_ocp.py | 8 +- .../crocomplements/activation-quad-ref.hpp | 16 ++-- .../activation-weighted-log.hpp | 17 ++-- .../crocomplements/contact/contact-fwddyn.hxx | 6 +- .../crocomplements/contact/contact1d.hxx | 4 +- .../crocomplements/contact/contact3d.hxx | 4 +- .../crocomplements/contact/contact6d.hxx | 4 +- .../contact/multiple-contacts.hxx | 16 ++-- .../crocomplements/lowpassfilter/action.hxx | 83 ++++++++-------- .../crocomplements/lowpassfilter/state.hxx | 96 +++++++++---------- python/sobec/pinbullet.py | 4 +- python/sobec/walk_without_think/ocp.py | 22 +++-- .../sobec/walk_without_think/weight_share.py | 4 +- tests/python/test_mpccpp.py | 1 - 14 files changed, 140 insertions(+), 145 deletions(-) diff --git a/examples/walk_without_think/walk_ocp.py b/examples/walk_without_think/walk_ocp.py index a528592b..a825907b 100644 --- a/examples/walk_without_think/walk_ocp.py +++ b/examples/walk_without_think/walk_ocp.py @@ -81,9 +81,11 @@ q0 = robot.x0[: robot.model.nq] print( "Start from q0=", - "half_sitting" - if norm(q0 - robot.model.referenceConfigurations["half_sitting"]) < 1e-9 - else q0, + ( + "half_sitting" + if norm(q0 - robot.model.referenceConfigurations["half_sitting"]) < 1e-9 + else q0 + ), ) # ##################################################################################### diff --git a/include/sobec/crocomplements/activation-quad-ref.hpp b/include/sobec/crocomplements/activation-quad-ref.hpp index 957d2027..01cefda2 100644 --- a/include/sobec/crocomplements/activation-quad-ref.hpp +++ b/include/sobec/crocomplements/activation-quad-ref.hpp @@ -32,15 +32,15 @@ class ActivationModelQuadRefTpl : public ActivationModelAbstractTpl<_Scalar> { typedef typename MathBase::MatrixXs MatrixXs; explicit ActivationModelQuadRefTpl(const VectorXs& reference) - : Base(static_cast(reference.size())), ref(reference){}; - virtual ~ActivationModelQuadRefTpl(){}; + : Base(static_cast(reference.size())), ref(reference) {}; + virtual ~ActivationModelQuadRefTpl() {}; virtual void calc(const boost::shared_ptr& data, const Eigen::Ref& r) { if (static_cast(r.size()) != nr_) { - throw_pretty("Invalid argument: " - << "r has wrong dimension (it should be " + - std::to_string(nr_) + ")"); + throw_pretty( + "Invalid argument: " << "r has wrong dimension (it should be " + + std::to_string(nr_) + ")"); } data->a_value = (Scalar(0.5) * (r - ref).transpose() * (r - ref))[0]; }; @@ -48,9 +48,9 @@ class ActivationModelQuadRefTpl : public ActivationModelAbstractTpl<_Scalar> { virtual void calcDiff(const boost::shared_ptr& data, const Eigen::Ref& r) { if (static_cast(r.size()) != nr_) { - throw_pretty("Invalid argument: " - << "r has wrong dimension (it should be " + - std::to_string(nr_) + ")"); + throw_pretty( + "Invalid argument: " << "r has wrong dimension (it should be " + + std::to_string(nr_) + ")"); } data->Ar = r - ref; diff --git a/include/sobec/crocomplements/activation-weighted-log.hpp b/include/sobec/crocomplements/activation-weighted-log.hpp index 38ea859c..75f6cd90 100644 --- a/include/sobec/crocomplements/activation-weighted-log.hpp +++ b/include/sobec/crocomplements/activation-weighted-log.hpp @@ -37,18 +37,17 @@ class ActivationModelWeightedLogTpl const Scalar &alpha = Scalar(1.)) : Base(weights.size()), weights_(weights), alpha_(alpha) { if (alpha < Scalar(0.)) { - throw_pretty("Invalid argument: " - << "alpha should be a positive value"); + throw_pretty("Invalid argument: " << "alpha should be a positive value"); } }; - virtual ~ActivationModelWeightedLogTpl(){}; + virtual ~ActivationModelWeightedLogTpl() {}; virtual void calc(const boost::shared_ptr &data, const Eigen::Ref &r) { if (static_cast(r.size()) != nr_) { - throw_pretty("Invalid argument: " - << "r has wrong dimension (it should be " + - std::to_string(nr_) + ")"); + throw_pretty( + "Invalid argument: " << "r has wrong dimension (it should be " + + std::to_string(nr_) + ")"); } boost::shared_ptr d = boost::static_pointer_cast(data); d->Wr = weights_.cwiseProduct(r); @@ -59,9 +58,9 @@ class ActivationModelWeightedLogTpl virtual void calcDiff(const boost::shared_ptr &data, const Eigen::Ref &r) { if (static_cast(r.size()) != nr_) { - throw_pretty("Invalid argument: " - << "r has wrong dimension (it should be " + - std::to_string(nr_) + ")"); + throw_pretty( + "Invalid argument: " << "r has wrong dimension (it should be " + + std::to_string(nr_) + ")"); } boost::shared_ptr d = boost::static_pointer_cast(data); diff --git a/include/sobec/crocomplements/contact/contact-fwddyn.hxx b/include/sobec/crocomplements/contact/contact-fwddyn.hxx index 2a6c32a2..b08ec314 100644 --- a/include/sobec/crocomplements/contact/contact-fwddyn.hxx +++ b/include/sobec/crocomplements/contact/contact-fwddyn.hxx @@ -49,9 +49,9 @@ void DifferentialActionModelContactFwdDynamicsTpl::calcDiff( std::to_string(this->get_state()->get_nx()) + ")"); } if (static_cast(u.size()) != this->get_nu()) { - throw_pretty("Invalid argument: " - << "u has wrong dimension (it should be " + - std::to_string(this->get_nu()) + ")"); + throw_pretty( + "Invalid argument: " << "u has wrong dimension (it should be " + + std::to_string(this->get_nu()) + ")"); } const std::size_t nv = this->get_state()->get_nv(); diff --git a/include/sobec/crocomplements/contact/contact1d.hxx b/include/sobec/crocomplements/contact/contact1d.hxx index 4b0d8652..b51a1a9e 100644 --- a/include/sobec/crocomplements/contact/contact1d.hxx +++ b/include/sobec/crocomplements/contact/contact1d.hxx @@ -150,8 +150,8 @@ void ContactModel1DTpl::updateForce( const boost::shared_ptr>& data, const VectorXs& force) { if (force.size() != 1) { - throw_pretty("Invalid argument: " - << "lambda has wrong dimension (it should be 1)"); + throw_pretty( + "Invalid argument: " << "lambda has wrong dimension (it should be 1)"); } Data* d = static_cast(data.get()); d->oRf = d->pinocchio->oMf[id_].rotation(); diff --git a/include/sobec/crocomplements/contact/contact3d.hxx b/include/sobec/crocomplements/contact/contact3d.hxx index 7c48240b..979cea0a 100644 --- a/include/sobec/crocomplements/contact/contact3d.hxx +++ b/include/sobec/crocomplements/contact/contact3d.hxx @@ -145,8 +145,8 @@ void ContactModel3DTpl::updateForce( const boost::shared_ptr>& data, const VectorXs& force) { if (force.size() != 3) { - throw_pretty("Invalid argument: " - << "lambda has wrong dimension (it should be 3)"); + throw_pretty( + "Invalid argument: " << "lambda has wrong dimension (it should be 3)"); } Data* d = static_cast(data.get()); d->oRf = d->pinocchio->oMf[id_].rotation(); diff --git a/include/sobec/crocomplements/contact/contact6d.hxx b/include/sobec/crocomplements/contact/contact6d.hxx index 2aba8320..fc6da1dd 100644 --- a/include/sobec/crocomplements/contact/contact6d.hxx +++ b/include/sobec/crocomplements/contact/contact6d.hxx @@ -131,8 +131,8 @@ void ContactModel6DTpl::updateForce( const boost::shared_ptr>& data, const VectorXs& force) { if (force.size() != 6) { - throw_pretty("Invalid argument: " - << "lambda has wrong dimension (it should be 6)"); + throw_pretty( + "Invalid argument: " << "lambda has wrong dimension (it should be 6)"); } Data* d = static_cast(data.get()); d->oRf = d->pinocchio->oMf[id_].rotation(); diff --git a/include/sobec/crocomplements/contact/multiple-contacts.hxx b/include/sobec/crocomplements/contact/multiple-contacts.hxx index f53da898..13540287 100644 --- a/include/sobec/crocomplements/contact/multiple-contacts.hxx +++ b/include/sobec/crocomplements/contact/multiple-contacts.hxx @@ -30,17 +30,17 @@ void ContactModelMultipleTpl::updateForceDiff( const std::size_t ndx = this->get_state()->get_ndx(); if (static_cast(df_dx->rows()) != this->get_nc() || static_cast(df_dx->cols()) != ndx) { - throw_pretty("Invalid argument: " - << "df_dx has wrong dimension (it should be " + - std::to_string(this->get_nc()) + "," + - std::to_string(ndx) + ")"); + throw_pretty( + "Invalid argument: " << "df_dx has wrong dimension (it should be " + + std::to_string(this->get_nc()) + "," + + std::to_string(ndx) + ")"); } if (static_cast(df_du->rows()) != this->get_nc() || static_cast(df_du->cols()) != this->get_nu()) { - throw_pretty("Invalid argument: " - << "df_du has wrong dimension (it should be " + - std::to_string(this->get_nc()) + "," + - std::to_string(this->get_nu()) + ")"); + throw_pretty( + "Invalid argument: " << "df_du has wrong dimension (it should be " + + std::to_string(this->get_nc()) + "," + + std::to_string(this->get_nu()) + ")"); } if (static_cast(data->contacts.size()) != this->get_contacts().size()) { diff --git a/include/sobec/crocomplements/lowpassfilter/action.hxx b/include/sobec/crocomplements/lowpassfilter/action.hxx index 286606b9..d5c40e9b 100644 --- a/include/sobec/crocomplements/lowpassfilter/action.hxx +++ b/include/sobec/crocomplements/lowpassfilter/action.hxx @@ -131,14 +131,14 @@ void IntegratedActionModelLPFTpl::calc( const std::size_t& nx = differential_->get_state()->get_nx(); if (static_cast(y.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } if (static_cast(w.size()) != nw_) { - throw_pretty("Invalid argument: " - << "w has wrong dimension (it should be " + - std::to_string(nw_) + ")"); + throw_pretty( + "Invalid argument: " << "w has wrong dimension (it should be " + + std::to_string(nw_) + ")"); } // Static casting the data @@ -162,14 +162,14 @@ void IntegratedActionModelLPFTpl::calc( const Eigen::Ref& tau = d->tau_tmp; // std::cout << "[lpf.calc] tau = " << tau << std::endl; if (static_cast(x.size()) != nx) { - throw_pretty("Invalid argument: " - << "x has wrong dimension (it should be " + - std::to_string(nx) + ")"); + throw_pretty( + "Invalid argument: " << "x has wrong dimension (it should be " + + std::to_string(nx) + ")"); } if (static_cast(tau.size()) != nw_) { - throw_pretty("Invalid argument: " - << "tau has wrong dimension (it should be " + - std::to_string(nw_) + ")"); + throw_pretty( + "Invalid argument: " << "tau has wrong dimension (it should be " + + std::to_string(nw_) + ")"); } if (static_cast(d->Fy.rows()) != boost::static_pointer_cast(state_)->get_ndy()) { @@ -190,9 +190,9 @@ void IntegratedActionModelLPFTpl::calc( ")"); } if (static_cast(d->Fw.cols()) != nw_) { - throw_pretty("Invalid argument: " - << "Fw.cols() has wrong dimension (it should be " + - std::to_string(nw_) + ")"); + throw_pretty( + "Invalid argument: " << "Fw.cols() has wrong dimension (it should be " + + std::to_string(nw_) + ")"); } if (static_cast(d->r.size()) != differential_->get_nr() + 2 * ntau_) { @@ -211,9 +211,9 @@ void IntegratedActionModelLPFTpl::calc( ")"); } if (static_cast(d->Lw.size()) != nw_) { - throw_pretty("Invalid argument: " - << "Lw has wrong dimension (it should be " + - std::to_string(nw_) + ")"); + throw_pretty( + "Invalid argument: " << "Lw has wrong dimension (it should be " + + std::to_string(nw_) + ")"); } // Compute acceleration and cost (DAM, i.e. CT model) @@ -305,9 +305,9 @@ void IntegratedActionModelLPFTpl::calc( const std::size_t& nx = differential_->get_state()->get_nx(); if (static_cast(y.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } // Static casting the data @@ -335,14 +335,14 @@ void IntegratedActionModelLPFTpl::calcDiff( const std::size_t& ndx = differential_->get_state()->get_ndx(); if (static_cast(y.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } if (static_cast(w.size()) != nw_) { - throw_pretty("Invalid argument: " - << "w has wrong dimension (it should be " + - std::to_string(nw_) + ")"); + throw_pretty( + "Invalid argument: " << "w has wrong dimension (it should be " + + std::to_string(nw_) + ")"); } // Static casting the data @@ -541,7 +541,7 @@ void IntegratedActionModelLPFTpl::calcDiff( } #endif } // tauLim !=0 - } // tau integration + } // tau integration // // TAU PLUS INTEGRATION // else { @@ -741,9 +741,9 @@ void IntegratedActionModelLPFTpl::calcDiff( const std::size_t& ndx = differential_->get_state()->get_ndx(); if (static_cast(y.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } // Static casting the data boost::shared_ptr d = boost::static_pointer_cast(data); @@ -820,8 +820,7 @@ const Scalar& IntegratedActionModelLPFTpl::get_fc() const { template void IntegratedActionModelLPFTpl::set_dt(const Scalar& dt) { if (dt < 0.) { - throw_pretty("Invalid argument: " - << "dt has positive value"); + throw_pretty("Invalid argument: " << "dt has positive value"); } time_step_ = dt; time_step2_ = dt * dt; @@ -831,8 +830,7 @@ template void IntegratedActionModelLPFTpl::set_fc(const Scalar& fc) { // Set the cut-off frequency if (fc <= 0.) { - throw_pretty("Invalid argument: " - << "fc must be positive"); + throw_pretty("Invalid argument: " << "fc must be positive"); } else { fc_ = fc; } @@ -842,8 +840,7 @@ template void IntegratedActionModelLPFTpl::set_alpha(const Scalar& alpha) { // Set the cut-off frequency if (alpha < 0. || alpha > 1) { - throw_pretty("Invalid argument: " - << "alpha must be in [0,1]"); + throw_pretty("Invalid argument: " << "alpha must be in [0,1]"); } else { alpha_ = alpha; } @@ -917,14 +914,14 @@ void IntegratedActionModelLPFTpl::quasiStatic( const Eigen::Ref& x, const std::size_t maxiter, const Scalar tol) { if (static_cast(u.size()) != nu_) { - throw_pretty("Invalid argument: " - << "u has wrong dimension (it should be " + - std::to_string(nu_) + ")"); + throw_pretty( + "Invalid argument: " << "u has wrong dimension (it should be " + + std::to_string(nu_) + ")"); } if (static_cast(x.size()) != state_->get_nx()) { - throw_pretty("Invalid argument: " - << "x has wrong dimension (it should be " + - std::to_string(state_->get_nx()) + ")"); + throw_pretty( + "Invalid argument: " << "x has wrong dimension (it should be " + + std::to_string(state_->get_nx()) + ")"); } // Static casting the data diff --git a/include/sobec/crocomplements/lowpassfilter/state.hxx b/include/sobec/crocomplements/lowpassfilter/state.hxx index 67efa56c..d2781b23 100644 --- a/include/sobec/crocomplements/lowpassfilter/state.hxx +++ b/include/sobec/crocomplements/lowpassfilter/state.hxx @@ -102,19 +102,19 @@ void StateLPFTpl::diff(const Eigen::Ref& y0, const Eigen::Ref& y1, Eigen::Ref dyout) const { if (static_cast(y0.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y0 has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y0 has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } if (static_cast(y1.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y1 has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y1 has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } if (static_cast(dyout.size()) != ndy_) { - throw_pretty("Invalid argument: " - << "dyout has wrong dimension (it should be " + - std::to_string(ndy_) + ")"); + throw_pretty( + "Invalid argument: " << "dyout has wrong dimension (it should be " + + std::to_string(ndy_) + ")"); } pinocchio::difference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), @@ -128,19 +128,19 @@ void StateLPFTpl::integrate(const Eigen::Ref& y, const Eigen::Ref& dy, Eigen::Ref yout) const { if (static_cast(y.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } if (static_cast(dy.size()) != ndy_) { - throw_pretty("Invalid argument: " - << "dy has wrong dimension (it should be " + - std::to_string(ndy_) + ")"); + throw_pretty( + "Invalid argument: " << "dy has wrong dimension (it should be " + + std::to_string(ndy_) + ")"); } if (static_cast(yout.size()) != ny_) { - throw_pretty("Invalid argument: " - << "yout has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "yout has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } pinocchio::integrate(*pinocchio_.get(), y.head(nq_), dy.head(nv_), @@ -159,23 +159,23 @@ void StateLPFTpl::Jdiff(const Eigen::Ref& y0, is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}")); if (static_cast(y0.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y0 has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y0 has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } if (static_cast(y1.size()) != ny_) { - throw_pretty("Invalid argument: " - << "y1 has wrong dimension (it should be " + - std::to_string(ny_) + ")"); + throw_pretty( + "Invalid argument: " << "y1 has wrong dimension (it should be " + + std::to_string(ny_) + ")"); } if (firstsecond == first) { if (static_cast(Jfirst.rows()) != ndy_ || static_cast(Jfirst.cols()) != ndy_) { - throw_pretty("Invalid argument: " - << "Jfirst has wrong dimension (it should be " + - std::to_string(ndy_) + "," + std::to_string(ndy_) + - ")"); + throw_pretty( + "Invalid argument: " << "Jfirst has wrong dimension (it should be " + + std::to_string(ndy_) + "," + + std::to_string(ndy_) + ")"); } pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), @@ -185,10 +185,10 @@ void StateLPFTpl::Jdiff(const Eigen::Ref& y0, } else if (firstsecond == second) { if (static_cast(Jsecond.rows()) != ndy_ || static_cast(Jsecond.cols()) != ndy_) { - throw_pretty("Invalid argument: " - << "Jsecond has wrong dimension (it should be " + - std::to_string(ndy_) + "," + std::to_string(ndy_) + - ")"); + throw_pretty( + "Invalid argument: " << "Jsecond has wrong dimension (it should be " + + std::to_string(ndy_) + "," + + std::to_string(ndy_) + ")"); } pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1); @@ -197,17 +197,17 @@ void StateLPFTpl::Jdiff(const Eigen::Ref& y0, } else { // computing both if (static_cast(Jfirst.rows()) != ndy_ || static_cast(Jfirst.cols()) != ndy_) { - throw_pretty("Invalid argument: " - << "Jfirst has wrong dimension (it should be " + - std::to_string(ndy_) + "," + std::to_string(ndy_) + - ")"); + throw_pretty( + "Invalid argument: " << "Jfirst has wrong dimension (it should be " + + std::to_string(ndy_) + "," + + std::to_string(ndy_) + ")"); } if (static_cast(Jsecond.rows()) != ndy_ || static_cast(Jsecond.cols()) != ndy_) { - throw_pretty("Invalid argument: " - << "Jsecond has wrong dimension (it should be " + - std::to_string(ndy_) + "," + std::to_string(ndy_) + - ")"); + throw_pretty( + "Invalid argument: " << "Jsecond has wrong dimension (it should be " + + std::to_string(ndy_) + "," + + std::to_string(ndy_) + ")"); } pinocchio::dDifference(*pinocchio_.get(), y0.head(nq_), y1.head(nq_), Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0); @@ -235,10 +235,10 @@ void StateLPFTpl::Jintegrate(const Eigen::Ref& y, if (firstsecond == first || firstsecond == both) { if (static_cast(Jfirst.rows()) != ndy_ || static_cast(Jfirst.cols()) != ndy_) { - throw_pretty("Invalid argument: " - << "Jfirst has wrong dimension (it should be " + - std::to_string(ndy_) + "," + std::to_string(ndy_) + - ")"); + throw_pretty( + "Invalid argument: " << "Jfirst has wrong dimension (it should be " + + std::to_string(ndy_) + "," + + std::to_string(ndy_) + ")"); } switch (op) { case setto: @@ -271,10 +271,10 @@ void StateLPFTpl::Jintegrate(const Eigen::Ref& y, if (firstsecond == second || firstsecond == both) { if (static_cast(Jsecond.rows()) != ndy_ || static_cast(Jsecond.cols()) != ndy_) { - throw_pretty("Invalid argument: " - << "Jsecond has wrong dimension (it should be " + - std::to_string(ndy_) + "," + std::to_string(ndy_) + - ")"); + throw_pretty( + "Invalid argument: " << "Jsecond has wrong dimension (it should be " + + std::to_string(ndy_) + "," + + std::to_string(ndy_) + ")"); } switch (op) { case setto: diff --git a/python/sobec/pinbullet.py b/python/sobec/pinbullet.py index 9fe19795..fb7fa875 100644 --- a/python/sobec/pinbullet.py +++ b/python/sobec/pinbullet.py @@ -56,9 +56,7 @@ def setPinocchioFinalizationTricks(self): self.rmodel.upperPositionLimit[:7] = 1 self.rmodel.lowerPositionLimit[:7] = -1 - self.rmodel.armature = ( - self.rmodel.rotorInertia * self.rmodel.rotorGearRatio**2 - ) + self.rmodel.armature = self.rmodel.rotorInertia * self.rmodel.rotorGearRatio**2 self.rmodel.q0 = self.rmodel.referenceConfigurations["half_sitting"] self.rdata = self.rmodel.createData() diff --git a/python/sobec/walk_without_think/ocp.py b/python/sobec/walk_without_think/ocp.py index a8a63adb..e3225eca 100644 --- a/python/sobec/walk_without_think/ocp.py +++ b/python/sobec/walk_without_think/ocp.py @@ -434,16 +434,18 @@ def __init__(self, robotWrapper, ddp): np.concatenate( [ ( - d.differential.multibody.contacts.contacts[ - "%s_contact" % robotWrapper.model.frames[cid].name - ].jMf.inverse() - * d.differential.multibody.contacts.contacts[ - "%s_contact" % robotWrapper.model.frames[cid].name - ].f - ).vector - if "%s_contact" % robotWrapper.model.frames[cid].name - in d.differential.multibody.contacts.contacts - else np.zeros(6) + ( + d.differential.multibody.contacts.contacts[ + "%s_contact" % robotWrapper.model.frames[cid].name + ].jMf.inverse() + * d.differential.multibody.contacts.contacts[ + "%s_contact" % robotWrapper.model.frames[cid].name + ].f + ).vector + if "%s_contact" % robotWrapper.model.frames[cid].name + in d.differential.multibody.contacts.contacts + else np.zeros(6) + ) for cid in robotWrapper.contactIds ] ) diff --git a/python/sobec/walk_without_think/weight_share.py b/python/sobec/walk_without_think/weight_share.py index 8ce89bf3..b40fa0bb 100644 --- a/python/sobec/walk_without_think/weight_share.py +++ b/python/sobec/walk_without_think/weight_share.py @@ -5,9 +5,7 @@ def switch_tanh(x, **kwargs): return ( 0 if x <= 0 - else 1 - if x >= 1 - else (np.tanh(-1.0 / x + 1.0 / (1.0 - x)) + 1.0) / 2.0 + else 1 if x >= 1 else (np.tanh(-1.0 / x + 1.0 / (1.0 - x)) + 1.0) / 2.0 ) diff --git a/tests/python/test_mpccpp.py b/tests/python/test_mpccpp.py index bde242e8..074345a3 100644 --- a/tests/python/test_mpccpp.py +++ b/tests/python/test_mpccpp.py @@ -3,7 +3,6 @@ """ - import numpy as np # from numpy.linalg import norm