Commit 78121a66 authored by spykspeigel's avatar spykspeigel Committed by Carlos Mastalli
Browse files

[binding] added binding for statenumdiff

parent 02386959
......@@ -36,6 +36,7 @@ void exposeCore() {
exposeActionNumDiff();
exposeDifferentialActionNumDiff();
exposeActivationNumDiff();
exposeStateNumDiff();
exposeShootingProblem();
exposeSolverAbstract();
exposeStateEuclidean();
......
......@@ -38,6 +38,7 @@ void exposeCostControl();
void exposeActionNumDiff();
void exposeDifferentialActionNumDiff();
void exposeActivationNumDiff();
void exposeStateNumDiff();
void exposeShootingProblem();
void exposeSolverAbstract();
void exposeStateEuclidean();
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2021, University of Pisa
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#include "python/crocoddyl/core/core.hpp"
#include "python/crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/numdiff/state.hpp"
namespace crocoddyl {
namespace python {
void exposeStateNumDiff() {
bp::register_ptr_to_python<boost::shared_ptr<StateNumDiff> >();
bp::class_<StateNumDiff, bp::bases<StateAbstract> >(
"StateNumDiff", "Abstract class for computing calcDiff by using numerical differentiation.\n\n",
bp::init<boost::shared_ptr<StateAbstract> >(
bp::args("self", "state"),
"Initialize the state NumDiff.\n\n"
":param model: state where we compute the derivatives through NumDiff"))
.def("zero", &StateNumDiff::zero, bp::args("self"),
"Return a zero reference state.\n\n"
":return zero reference state")
.def("rand", &StateNumDiff::rand, bp::args("self"),
"Return a random reference state.\n\n"
":return random reference state")
.def("diff", &StateNumDiff::diff_dx, bp::args("self", "x0", "x1"),
"Operator that differentiates the two state points.\n\n"
"It returns the value of x1 [-] x0 operation. Due to a state vector lies in\n"
"the Euclidean space, this operator is defined with arithmetic subtraction.\n"
":param x0: current state (dim state.nx()).\n"
":param x1: next state (dim state.nx()).\n"
":return x1 - x0 value (dim state.nx()).")
.def("integrate", &StateNumDiff::integrate_x, bp::args("self", "x", "dx"),
"Operator that integrates the current state.\n\n"
"It returns the value of x [+] dx operation. Due to a state vector lies in\n"
"the Euclidean space, this operator is defined with arithmetic addition.\n"
"Futhermore there is no timestep here (i.e. dx = v*dt), note this if you're\n"
"integrating a velocity v during an interval dt.\n"
":param x: current state (dim state.nx()).\n"
":param dx: displacement of the state (dim state.nx()).\n"
":return x + dx value (dim state.nx()).")
.def("Jdiff", &StateNumDiff::Jdiff_Js,
Jdiffs(bp::args("self", "x0", "x1", "firstsecond"),
"Compute the partial derivatives of arithmetic substraction.\n\n"
"Both Jacobian matrices are represented throught an identity matrix, with the exception\n"
"that the first partial derivatives (w.r.t. x0) has negative signed. By default, this\n"
"function returns the derivatives of the first and second argument (i.e.\n"
"firstsecond='both'). However we ask for a specific partial derivative by setting\n"
"firstsecond='first' or firstsecond='second'.\n"
":param x0: current state (dim state.nx()).\n"
":param x1: next state (dim state.nx()).\n"
":param firstsecond: derivative w.r.t x0 or x1 or both\n"
":return the partial derivative(s) of the diff(x0, x1) function"))
.def("Jintegrate", &StateNumDiff::Jintegrate_Js,
Jintegrates(bp::args("self", "x", "dx", "firstsecond"),
"Compute the partial derivatives of arithmetic addition.\n\n"
"Both Jacobian matrices are represented throught an identity matrix. By default, this\n"
"function returns the derivatives of the first and second argument (i.e.\n"
"firstsecond='both'). However we ask for a specific partial derivative by setting\n"
"firstsecond='first' or firstsecond='second'.\n"
":param x: current state (dim state.nx()).\n"
":param dx: displacement of the state (dim state.nx()).\n"
":param firstsecond: derivative w.r.t x or dx or both\n"
":return the partial derivative(s) of the integrate(x, dx) function"))
.def("JintegrateTransport", &StateNumDiff::JintegrateTransport, bp::args("self", "x", "dx", "Jin", "firstsecond"),
"Parallel transport from integrate(x, dx) to x.\n\n"
"This function performs the parallel transportation of an input matrix whose columns\n"
"are expressed in the tangent space at integrate(x, dx) to the tangent space at x point\n"
":param x: state point (dim. state.nx).\n"
":param dx: velocity vector (dim state.ndx).\n"
":param Jin: input matrix (number of rows = state.nv).\n"
":param firstsecond: derivative w.r.t x or dx");
.add_property("disturbance", bp::make_function(&StateNumDiff::get_disturbance),
&StateNumDiff::set_disturbance,
"disturbance value used in the numerical differentiation")
}
} // namespace python
} // namespace crocoddyl
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