Commit 4c93031a authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

format

parent f6855f40
import matplotlib.pyplot as plt
import numpy as np
from math import pow, atanh
from numpy import tanh, sqrt
from math import atanh
from numpy import tanh
def enb(r):
return C - K * tanh(a * r + b)
alphaMin = 0.2
C = ( 1 + alphaMin ) / 2
K = ( 1 - alphaMin ) / 2
C = (1 + alphaMin) / 2
K = (1 - alphaMin) / 2
# N = 4
# M = 8
......@@ -17,13 +19,13 @@ K = ( 1 - alphaMin ) / 2
# b = 2. - 4. / ( 1 - 10**(N-M) )
r_half_target = 10**6
delta_1 = 0.02
a = atanh ((delta_1 - 1 + C) / K) / (1 - r_half_target)
b = - r_half_target * a
a = atanh((delta_1 - 1 + C) / K) / (1 - r_half_target)
b = -r_half_target * a
r_half = - b / a
r_half = -b / a
alpha_1 = enb(1)
rs = 10**(np.linspace(-1,10,200))
rs = 10 ** (np.linspace(-1, 10, 200))
fs = enb(rs)
# plt.plot(rs, fs)
......@@ -33,12 +35,12 @@ ax.plot(rs, fs, label="Error norm based alpha")
ax.set_xscale("log")
ax.set_xlabel("(E / E_0)^2")
ax.set_ylabel("alpha")
ax.set_ylim(0,1)
ax.set_ylim(0, 1)
# ax.annotate("Alpha_max", xy = (1, alpha_1), xytext=(0.1, alpha_1))
ax.axhline(alpha_1, ls='-.', color='g', label="Alpha_max")
ax.axhline(alpha_1, ls="-.", color="g", label="Alpha_max")
# ax.annotate("Alpha_min", xy = (1, alphaMin), xytext=(0.1, alphaMin))
ax.axhline(alphaMin, ls='-.', color='r', label="Alpha_min")
ax.axhline(alphaMin, ls="-.", color="r", label="Alpha_min")
# ax.annotate("r_h", xy = (r_half, 1), xytext=(r_half, 1.02))
ax.axvline(r_half, ls='-.', color='k', label="r_h")
ax.axvline(r_half, ls="-.", color="k", label="r_h")
plt.legend(loc="lower left")
plt.show()
......@@ -66,4 +66,3 @@ F_{2/J_2}\exp_{\reals^3\times SO(3)} (rhs_{impl}) F_{2/J_2}^{-1} &=& F_{2/J_2}
rhs_{expl} &=& \log_{SE(3)}\left(F_{2/J_2}\exp_{\reals^3\times SO(3)} (rhs_{impl}) F_{2/J_2}^{-1}\right)
\end {eqnarray*}
\end {document}
......@@ -135,4 +135,3 @@ $$
\hline
\end {tabular}
\end {document}
......@@ -29,89 +29,74 @@
#ifndef HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH
#define HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/config.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/fwd.hh>
namespace hpp {
namespace constraints {
namespace constraints {
/// Handle bounds on input variables of a differentiable function.
///
/// This class is a decorator of class DifferentiableFunction that
/// sets to 0 some columns of the Jacobian of the function.
///
/// The class is used to handle saturation of input variables of
/// the function during numerical resolution of implicit constraints
/// built with the function.
class HPP_CONSTRAINTS_DLLAPI ActiveSetDifferentiableFunction :
public DifferentiableFunction
{
/// Handle bounds on input variables of a differentiable function.
///
/// This class is a decorator of class DifferentiableFunction that
/// sets to 0 some columns of the Jacobian of the function.
///
/// The class is used to handle saturation of input variables of
/// the function during numerical resolution of implicit constraints
/// built with the function.
class HPP_CONSTRAINTS_DLLAPI ActiveSetDifferentiableFunction
: public DifferentiableFunction {
public:
/// Constructor
/// \param f initial differentiable function,
/// \param intervals set of intervals of indices corresponding to saturated
/// input variables.
ActiveSetDifferentiableFunction (const DifferentiableFunctionPtr_t& f,
ActiveSetDifferentiableFunction(const DifferentiableFunctionPtr_t& f,
segments_t intervals)
: DifferentiableFunction(
f->inputSize(), f->inputDerivativeSize(),
f->outputSpace (),
"ActiveSet_on_" + f->name ())
, function_(f)
, intervals_(intervals)
{
context (f->context());
: DifferentiableFunction(f->inputSize(), f->inputDerivativeSize(),
f->outputSpace(), "ActiveSet_on_" + f->name()),
function_(f),
intervals_(intervals) {
context(f->context());
}
/// Get the original function
const DifferentiableFunction& function() const
{
return *function_;
}
const DifferentiableFunction& function() const { return *function_; }
/// Get the original function
const DifferentiableFunctionPtr_t& functionPtr() const
{
return function_;
}
const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
protected:
typedef std::vector < segments_t > intervalss_t;
typedef std::vector<segments_t> intervalss_t;
/// User implementation of function evaluation
virtual void impl_compute (LiegroupElementRef result,
vectorIn_t argument) const
{
virtual void impl_compute(LiegroupElementRef result,
vectorIn_t argument) const {
function_->value(result, argument);
}
virtual void impl_jacobian (matrixOut_t jacobian,
vectorIn_t arg) const
{
virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const {
function_->jacobian(jacobian, arg);
for (segments_t::const_iterator _int = intervals_.begin ();
_int != intervals_.end (); ++_int)
jacobian.middleCols (_int->first, _int->second).setZero ();
for (segments_t::const_iterator _int = intervals_.begin();
_int != intervals_.end(); ++_int)
jacobian.middleCols(_int->first, _int->second).setZero();
}
bool isEqual(const DifferentiableFunction& other) const {
const ActiveSetDifferentiableFunction& castother = dynamic_cast<const ActiveSetDifferentiableFunction&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
const ActiveSetDifferentiableFunction& castother =
dynamic_cast<const ActiveSetDifferentiableFunction&>(other);
if (!DifferentiableFunction::isEqual(other)) return false;
if (function_ != castother.function_)
return false;
if (intervals_ != castother.intervals_)
return false;
if (function_ != castother.function_) return false;
if (intervals_ != castother.intervals_) return false;
return true;
}
DifferentiableFunctionPtr_t function_;
segments_t intervals_;
}; // class ActiveSetDifferentiableFunction
} // namespace constraints
}; // class ActiveSetDifferentiableFunction
} // namespace constraints
} // namespace hpp
#endif // HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH
......@@ -27,51 +27,44 @@
// DAMAGE.
#ifndef HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
# define HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
#define HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
# include <hpp/constraints/fwd.hh>
# include <hpp/constraints/config.hh>
# include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/config.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/fwd.hh>
namespace hpp {
namespace constraints {
/// \addtogroup constraints
/// \{
/// Identity function
/// \f$ q_{out} = q_{in} \f$
///
/// \todo should we handle specifically this function is the solvers ?
class HPP_CONSTRAINTS_DLLAPI Identity
: public constraints::DifferentiableFunction
{
namespace constraints {
/// \addtogroup constraints
/// \{
/// Identity function
/// \f$ q_{out} = q_{in} \f$
///
/// \todo should we handle specifically this function is the solvers ?
class HPP_CONSTRAINTS_DLLAPI Identity
: public constraints::DifferentiableFunction {
public:
static IdentityPtr_t create (const LiegroupSpacePtr_t space, const std::string& name)
{
IdentityPtr_t ptr (new Identity (space, name));
static IdentityPtr_t create(const LiegroupSpacePtr_t space,
const std::string& name) {
IdentityPtr_t ptr(new Identity(space, name));
return ptr;
}
Identity (const LiegroupSpacePtr_t space, const std::string& name) :
DifferentiableFunction (space->nq(), space->nv(), space, name) {}
Identity(const LiegroupSpacePtr_t space, const std::string& name)
: DifferentiableFunction(space->nq(), space->nv(), space, name) {}
protected:
void impl_compute (LiegroupElementRef y, vectorIn_t arg) const
{
void impl_compute(LiegroupElementRef y, vectorIn_t arg) const {
y.vector() = arg;
}
void impl_jacobian (matrixOut_t J, vectorIn_t) const
{
J.setIdentity();
}
void impl_jacobian(matrixOut_t J, vectorIn_t) const { J.setIdentity(); }
bool isEqual(const DifferentiableFunction& other) const {
dynamic_cast<const Identity&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (!DifferentiableFunction::isEqual(other)) return false;
return true;
}
......@@ -79,74 +72,62 @@ namespace hpp {
private:
Identity() {}
HPP_SERIALIZABLE();
}; // class Identity
/// Affine function
/// \f$ f(q) = J * q + b \f$
///
/// \todo should we handle specifically this function is the solvers ?
class HPP_CONSTRAINTS_DLLAPI AffineFunction
: public DifferentiableFunction
{
}; // class Identity
/// Affine function
/// \f$ f(q) = J * q + b \f$
///
/// \todo should we handle specifically this function is the solvers ?
class HPP_CONSTRAINTS_DLLAPI AffineFunction : public DifferentiableFunction {
public:
static AffineFunctionPtr_t create
(const matrixIn_t& J, const std::string name = "LinearFunction")
{
static AffineFunctionPtr_t create(const matrixIn_t& J,
const std::string name = "LinearFunction") {
return AffineFunctionPtr_t(new AffineFunction(J, name));
}
static AffineFunctionPtr_t create
(const matrixIn_t& J, const vectorIn_t& b,
const std::string name = "LinearFunction")
{
static AffineFunctionPtr_t create(const matrixIn_t& J, const vectorIn_t& b,
const std::string name = "LinearFunction") {
return AffineFunctionPtr_t(new AffineFunction(J, b, name));
}
protected:
AffineFunction (const matrixIn_t& J,
const std::string name = "LinearFunction")
: DifferentiableFunction (J.cols(), J.cols(), LiegroupSpace::Rn
(J.rows()), name),
J_ (J), b_ (vector_t::Zero(J.rows()))
{
AffineFunction(const matrixIn_t& J, const std::string name = "LinearFunction")
: DifferentiableFunction(J.cols(), J.cols(), LiegroupSpace::Rn(J.rows()),
name),
J_(J),
b_(vector_t::Zero(J.rows())) {
init();
}
AffineFunction (const matrixIn_t& J, const vectorIn_t& b,
AffineFunction(const matrixIn_t& J, const vectorIn_t& b,
const std::string name = "LinearFunction")
: DifferentiableFunction (J.cols(), J.cols(), LiegroupSpace::Rn
(J.rows()), name),
J_ (J), b_ (b)
{
: DifferentiableFunction(J.cols(), J.cols(), LiegroupSpace::Rn(J.rows()),
name),
J_(J),
b_(b) {
init();
}
bool isEqual(const DifferentiableFunction& other) const {
const AffineFunction& castother = dynamic_cast<const AffineFunction&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
const AffineFunction& castother =
dynamic_cast<const AffineFunction&>(other);
if (!DifferentiableFunction::isEqual(other)) return false;
if (J_ != castother.J_)
return false;
if (b_ != castother.b_)
return false;
if (J_ != castother.J_) return false;
if (b_ != castother.b_) return false;
return true;
}
private:
/// User implementation of function evaluation
void impl_compute (LiegroupElementRef y, vectorIn_t x) const
{
y.vector ().noalias() = J_ * x + b_;
void impl_compute(LiegroupElementRef y, vectorIn_t x) const {
y.vector().noalias() = J_ * x + b_;
}
void impl_jacobian (matrixOut_t jacobian, vectorIn_t) const
{
jacobian = J_;
}
void impl_jacobian(matrixOut_t jacobian, vectorIn_t) const { jacobian = J_; }
void init ()
{
void init() {
assert(J_.rows() == b_.rows());
activeParameters_ = (J_.array() != 0).colwise().any();
activeDerivativeParameters_ = activeParameters_;
......@@ -157,60 +138,52 @@ namespace hpp {
AffineFunction() {}
HPP_SERIALIZABLE();
}; // class AffineFunction
/// Constant function
/// \f$ f(q) = C \f$
///
/// \todo should we handle specifically this function is the solvers ?
struct HPP_CONSTRAINTS_DLLAPI ConstantFunction
: public DifferentiableFunction
{
}; // class AffineFunction
/// Constant function
/// \f$ f(q) = C \f$
///
/// \todo should we handle specifically this function is the solvers ?
struct HPP_CONSTRAINTS_DLLAPI ConstantFunction : public DifferentiableFunction {
public:
static ConstantFunctionPtr_t create
(const vector_t& constant, const size_type& sizeIn,
const size_type& sizeInDer, const std::string name="ConstantFunction")
{
return ConstantFunctionPtr_t(new ConstantFunction(constant, sizeIn,
sizeInDer, name));
static ConstantFunctionPtr_t create(
const vector_t& constant, const size_type& sizeIn,
const size_type& sizeInDer, const std::string name = "ConstantFunction") {
return ConstantFunctionPtr_t(
new ConstantFunction(constant, sizeIn, sizeInDer, name));
}
static ConstantFunctionPtr_t create
(const LiegroupElement& element, const size_type& sizeIn,
const size_type& sizeInDer, const std::string name="ConstantFunction")
{
return ConstantFunctionPtr_t(new ConstantFunction(element, sizeIn,
sizeInDer, name));
static ConstantFunctionPtr_t create(
const LiegroupElement& element, const size_type& sizeIn,
const size_type& sizeInDer, const std::string name = "ConstantFunction") {
return ConstantFunctionPtr_t(
new ConstantFunction(element, sizeIn, sizeInDer, name));
}
protected:
ConstantFunction (const vector_t& constant,
const size_type& sizeIn,
ConstantFunction(const vector_t& constant, const size_type& sizeIn,
const size_type& sizeInDer,
const std::string name = "ConstantFunction") :
DifferentiableFunction (sizeIn, sizeInDer, LiegroupSpace::Rn
(constant.rows()), name),
c_ (constant, outputSpace())
{}
ConstantFunction (const LiegroupElement& element,
const size_type& sizeIn,
const std::string name = "ConstantFunction")
: DifferentiableFunction(sizeIn, sizeInDer,
LiegroupSpace::Rn(constant.rows()), name),
c_(constant, outputSpace()) {}
ConstantFunction(const LiegroupElement& element, const size_type& sizeIn,
const size_type& sizeInDer,
const std::string name = "ConstantFunction") :
DifferentiableFunction (sizeIn, sizeInDer, element.space(), name),
c_ (element)
{}
const std::string name = "ConstantFunction")
: DifferentiableFunction(sizeIn, sizeInDer, element.space(), name),
c_(element) {}
/// User implementation of function evaluation
void impl_compute (LiegroupElementRef r, vectorIn_t) const { r = c_; }
void impl_compute(LiegroupElementRef r, vectorIn_t) const { r = c_; }
void impl_jacobian (matrixOut_t J, vectorIn_t) const { J.setZero(); }
void impl_jacobian(matrixOut_t J, vectorIn_t) const { J.setZero(); }
bool isEqual(const DifferentiableFunction& other) const {
const ConstantFunction& castother = dynamic_cast<const ConstantFunction&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
const ConstantFunction& castother =
dynamic_cast<const ConstantFunction&>(other);
if (!DifferentiableFunction::isEqual(other)) return false;
if (c_.vector() == castother.c_.vector())
return false;
if (c_.vector() == castother.c_.vector()) return false;
return true;
}
......@@ -218,13 +191,12 @@ namespace hpp {
const LiegroupElement c_;
private:
ConstantFunction () {}
ConstantFunction() {}
HPP_SERIALIZABLE();
}; // class ConstantFunction
}; // class ConstantFunction
/// \}
} // namespace constraints
/// \}
} // namespace constraints
} // namespace hpp
#endif // HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
......@@ -29,18 +29,18 @@
// DAMAGE.
#ifndef HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH
# define HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH
#define HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH
# include <hpp/constraints/differentiable-function.hh>
# include <hpp/constraints/config.hh>
# include <hpp/constraints/fwd.hh>
# include <hpp/constraints/tools.hh>
# include <hpp/constraints/symbolic-calculus.hh>
#include <hpp/constraints/config.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/symbolic-calculus.hh>
#include <hpp/constraints/tools.hh>
namespace hpp {
namespace constraints {
namespace constraints {
/**
/**
* Constraint on the relative position of the center of mass
*
* The value of the function is defined as the position of the center
......@@ -64,71 +64,62 @@ namespace hpp {
* \li \f$ u = x_R - x_L \f$
* \li \f$ e = x_{com} - (\frac{x_L + x_R}{2})\f$
**/
class HPP_CONSTRAINTS_DLLAPI ComBetweenFeet : public DifferentiableFunction
{
class HPP_CONSTRAINTS_DLLAPI ComBetweenFeet : public DifferentiableFunction {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Return a shared pointer to a new instance
static ComBetweenFeetPtr_t create (
static ComBetweenFeetPtr_t create(
const std::string& name, const DevicePtr_t& robot,
const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
const vector3_t pointLeft, const vector3_t pointRight,
const JointPtr_t& jointReference, const vector3_t pointRef,
std::vector <bool> mask = { true, true, true, true });
std::vector<bool> mask = {true, true, true, true});
/// Return a shared pointer to a new instance
static ComBetweenFeetPtr_t create (
static ComBetweenFeetPtr_t create(
const std::string& name, const DevicePtr_t& robot,
const CenterOfMassComputationPtr_t& comc,
const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
const vector3_t pointLeft, const vector3_t pointRight,
const JointPtr_t& jointReference, const vector3_t pointRef,
std::vector <bool> mask = { true, true, true, true });
const CenterOfMassComputationPtr_t& comc, const JointPtr_t& jointLeft,
const JointPtr_t& jointRight, const vector3_t pointLeft,
const vector3_t pointRight, const JointPtr_t& jointReference,
const vector3_t pointRef,
std::vector<bool> mask = {true, true, true, true});
virtual ~ComBetweenFeet () {}
virtual ~ComBetweenFeet() {}
ComBetweenFeet (const std::string& name, const DevicePtr_t& robot,
ComBetweenFeet(const std::string& name, const DevicePtr_t& robot,
const CenterOfMassComputationPtr_t& comc,
const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
const vector3_t pointLeft, const vector3_t pointRight,
const JointPtr_t& jointReference, const vector3_t pointRef,
std::vector <bool> mask);
std::vector<bool> mask);
protected:
/// Compute value of error
///
/// \param argument configuration of the robot,
/// \retval result error vector
virtual void impl_compute (LiegroupElementRef result,
virtual void impl_compute(LiegroupElementRef result,
ConfigurationIn_t argument) const;
virtual void impl_jacobian (matrixOut_t jacobian,
ConfigurationIn_t arg) const;
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
bool isEqual(const DifferentiableFunction& other) const {
const ComBetweenFeet& castother = dynamic_cast<const ComBetweenFeet&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
const ComBetweenFeet& castother =
dynamic_cast<const ComBetweenFeet&>(other);
if (!DifferentiableFunction::isEqual(other)) return false;
if (robot_ != castother.robot_)
return false;
if (com_->centerOfMassComputation() != castother.com_->centerOfMassComputation())
return false;