Skip to content

Commit

Permalink
Merge pull request #1204 from cmastalli/topic/fix-terminal-computatio…
Browse files Browse the repository at this point in the history 
…n-python
  • Loading branch information
@cmastalli
cmastalli authored Dec 30, 2023
2 parents 08fa965 + 14fd853 commit eb01abb
Show file tree
Hide file tree
Showing 32 changed files with 453 additions and 298 deletions.
25 changes: 19 additions & 6 deletions bindings/python/crocoddyl/core/action-base.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
// Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
Expand All @@ -24,12 +24,15 @@ class ActionModelAbstract_wrap : public ActionModelAbstract,
using ActionModelAbstract::ng_;
using ActionModelAbstract::nh_;
using ActionModelAbstract::nu_;
using ActionModelAbstract::unone_;

ActionModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t nu, const std::size_t nr = 1,
const std::size_t ng = 0, const std::size_t nh = 0)
: ActionModelAbstract(state, nu, nr, ng, nh),
bp::wrapper<ActionModelAbstract>() {}
bp::wrapper<ActionModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu);
}

void calc(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x,
Expand All @@ -44,8 +47,13 @@ class ActionModelAbstract_wrap : public ActionModelAbstract,
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
Expand All @@ -61,8 +69,13 @@ class ActionModelAbstract_wrap : public ActionModelAbstract,
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

boost::shared_ptr<ActionDataAbstract> createData() {
Expand Down
36 changes: 28 additions & 8 deletions bindings/python/crocoddyl/core/constraint-base.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2020-2021, University of Edinburgh
// Copyright (C) 2020-2024, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
Expand All @@ -20,22 +20,32 @@ class ConstraintModelAbstract_wrap
: public ConstraintModelAbstract,
public bp::wrapper<ConstraintModelAbstract> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using ConstraintModelAbstract::nu_;
using ConstraintModelAbstract::unone_;

ConstraintModelAbstract_wrap(
boost::shared_ptr<StateAbstract> state,
boost::shared_ptr<ResidualModelAbstract> residual, const std::size_t ng,
const std::size_t nh)
: ConstraintModelAbstract(state, residual, ng, nh),
bp::wrapper<ConstraintModelAbstract>() {}
bp::wrapper<ConstraintModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu_);
}

ConstraintModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t nu, const std::size_t ng,
const std::size_t nh)
: ConstraintModelAbstract(state, nu, ng, nh),
bp::wrapper<ConstraintModelAbstract>() {}
bp::wrapper<ConstraintModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu);
}

ConstraintModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t ng, const std::size_t nh)
: ConstraintModelAbstract(state, ng, nh) {}
: ConstraintModelAbstract(state, ng, nh) {
unone_ = NAN * MathBase::VectorXs::Ones(nu_);
}

void calc(const boost::shared_ptr<ConstraintDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x,
Expand All @@ -50,8 +60,13 @@ class ConstraintModelAbstract_wrap
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

void calcDiff(const boost::shared_ptr<ConstraintDataAbstract>& data,
Expand All @@ -67,8 +82,13 @@ class ConstraintModelAbstract_wrap
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

boost::shared_ptr<ConstraintDataAbstract> createData(
Expand Down
49 changes: 38 additions & 11 deletions bindings/python/crocoddyl/core/cost-base.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
Expand All @@ -19,31 +20,47 @@ namespace python {
class CostModelAbstract_wrap : public CostModelAbstract,
public bp::wrapper<CostModelAbstract> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using CostModelAbstract::nu_;
using CostModelAbstract::unone_;

CostModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
boost::shared_ptr<ActivationModelAbstract> activation,
boost::shared_ptr<ResidualModelAbstract> residual)
: CostModelAbstract(state, activation, residual) {}
: CostModelAbstract(state, activation, residual) {
unone_ = NAN * MathBase::VectorXs::Ones(nu_);
}

CostModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
boost::shared_ptr<ActivationModelAbstract> activation,
const std::size_t nu)
: CostModelAbstract(state, activation, nu) {}
: CostModelAbstract(state, activation, nu) {
unone_ = NAN * MathBase::VectorXs::Ones(nu);
}

CostModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
boost::shared_ptr<ActivationModelAbstract> activation)
: CostModelAbstract(state, activation) {}
: CostModelAbstract(state, activation) {
unone_ = NAN * MathBase::VectorXs::Ones(nu_);
}

CostModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
boost::shared_ptr<ResidualModelAbstract> residual)
: CostModelAbstract(state, residual) {}
: CostModelAbstract(state, residual) {
unone_ = NAN * MathBase::VectorXs::Ones(nu_);
}

CostModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t nr, const std::size_t nu)
: CostModelAbstract(state, nr, nu), bp::wrapper<CostModelAbstract>() {}
: CostModelAbstract(state, nr, nu), bp::wrapper<CostModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu);
}

CostModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t nr)
: CostModelAbstract(state, nr), bp::wrapper<CostModelAbstract>() {}
: CostModelAbstract(state, nr), bp::wrapper<CostModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu_);
}

void calc(const boost::shared_ptr<CostDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x,
Expand All @@ -58,8 +75,13 @@ class CostModelAbstract_wrap : public CostModelAbstract,
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

void calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
Expand All @@ -75,8 +97,13 @@ class CostModelAbstract_wrap : public CostModelAbstract,
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

boost::shared_ptr<CostDataAbstract> createData(
Expand Down
26 changes: 20 additions & 6 deletions bindings/python/crocoddyl/core/diff-action-base.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
// Copyright (C) 2019-2024, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
Expand All @@ -21,14 +22,17 @@ class DifferentialActionModelAbstract_wrap
public bp::wrapper<DifferentialActionModelAbstract> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using DifferentialActionModelAbstract::unone_;

DifferentialActionModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t nu,
const std::size_t nr = 1,
const std::size_t ng = 0,
const std::size_t nh = 0)
: DifferentialActionModelAbstract(state, nu, nr, ng, nh),
bp::wrapper<DifferentialActionModelAbstract>() {}
bp::wrapper<DifferentialActionModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu);
}

void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x,
Expand All @@ -43,8 +47,13 @@ class DifferentialActionModelAbstract_wrap
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
Expand All @@ -60,8 +69,13 @@ class DifferentialActionModelAbstract_wrap
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

boost::shared_ptr<DifferentialActionDataAbstract> createData() {
Expand Down
32 changes: 25 additions & 7 deletions bindings/python/crocoddyl/core/residual-base.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021-2023, University of Edinburgh, Heriot-Watt University
// Copyright (C) 2021-2024, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
Expand All @@ -19,22 +19,30 @@ namespace python {
class ResidualModelAbstract_wrap : public ResidualModelAbstract,
public bp::wrapper<ResidualModelAbstract> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using ResidualModelAbstract::nu_;
using ResidualModelAbstract::unone_;

ResidualModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t nr, const std::size_t nu,
const bool q_dependent = true,
const bool v_dependent = true,
const bool u_dependent = true)
: ResidualModelAbstract(state, nr, nu, q_dependent, v_dependent,
u_dependent),
bp::wrapper<ResidualModelAbstract>() {}
bp::wrapper<ResidualModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu);
}

ResidualModelAbstract_wrap(boost::shared_ptr<StateAbstract> state,
const std::size_t nr,
const bool q_dependent = true,
const bool v_dependent = true,
const bool u_dependent = true)
: ResidualModelAbstract(state, nr, q_dependent, v_dependent, u_dependent),
bp::wrapper<ResidualModelAbstract>() {}
bp::wrapper<ResidualModelAbstract>() {
unone_ = NAN * MathBase::VectorXs::Ones(nu_);
}

void calc(const boost::shared_ptr<ResidualDataAbstract>& data,
const Eigen::Ref<const Eigen::VectorXd>& x,
Expand All @@ -49,8 +57,13 @@ class ResidualModelAbstract_wrap : public ResidualModelAbstract,
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calc").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

void calcDiff(const boost::shared_ptr<ResidualDataAbstract>& data,
Expand All @@ -66,8 +79,13 @@ class ResidualModelAbstract_wrap : public ResidualModelAbstract,
<< "u has wrong dimension (it should be " +
std::to_string(nu_) + ")");
}
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
if (std::isnan(u.lpNorm<Eigen::Infinity>())) {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x);
} else {
return bp::call<void>(this->get_override("calcDiff").ptr(), data,
(Eigen::VectorXd)x, (Eigen::VectorXd)u);
}
}

boost::shared_ptr<ResidualDataAbstract> createData(
Expand Down
4 changes: 2 additions & 2 deletions bindings/python/crocoddyl/utils/pendulum.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@ def __init__(self, state, activation, nu):
)
crocoddyl.CostModelAbstract.__init__(self, state, activation, nu=nu)

def calc(self, data, x, u):
def calc(self, data, x, u=None):
c1, c2 = np.cos(x[0]), np.cos(x[1])
s1, s2 = np.sin(x[0]), np.sin(x[1])
data.residual.r[:] = np.array([s1, s2, 1 - c1, 1 - c2, x[2], x[3]])
self.activation.calc(data.activation, data.residual.r)
data.cost = data.activation.a_value

def calcDiff(self, data, x, u):
def calcDiff(self, data, x, u=None):
c1, c2 = np.cos(x[0]), np.cos(x[1])
s1, s2 = np.sin(x[0]), np.sin(x[1])

Expand Down
Loading

0 comments on commit eb01abb

Please sign in to comment.