Commit a5f542fa authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

format

parent cf0de97e
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()
......@@ -20,7 +20,7 @@
%% \import{<path to file>}{<filename>.pdf_tex}
%% Alternatively, one can specify
%% \graphicspath{{<path to file>/}}
%%
%%
%% For more information, please see info/svg-inkscape on CTAN:
%% http://tug.ctan.org/tex-archive/info/svg-inkscape
%%
......
......@@ -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 {
public:
/// Constructor
/// \param f initial differentiable function,
/// \param intervals set of intervals of indices corresponding to saturated
/// input variables.
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());
}
/// Get the original function
const DifferentiableFunction& function() const { return *function_; }
/// 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,
segments_t intervals)
: DifferentiableFunction(
f->inputSize(), f->inputDerivativeSize(),
f->outputSpace (),
"ActiveSet_on_" + f->name ())
, function_(f)
, intervals_(intervals)
{
context (f->context());
}
/// Get the original function
const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
/// Get the original function
const DifferentiableFunction& function() const
{
return *function_;
}
protected:
typedef std::vector<segments_t> intervalss_t;
/// Get the original function
const DifferentiableFunctionPtr_t& functionPtr() const
{
return function_;
}
/// User implementation of function evaluation
virtual void impl_compute(LiegroupElementRef result,
vectorIn_t argument) const {
function_->value(result, argument);
}
protected:
typedef std::vector < segments_t > intervalss_t;
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();
}
/// User implementation of function evaluation
virtual void impl_compute (LiegroupElementRef result,
vectorIn_t argument) const
{
function_->value(result, argument);
}
bool isEqual(const DifferentiableFunction& other) const {
const ActiveSetDifferentiableFunction& castother =
dynamic_cast<const ActiveSetDifferentiableFunction&>(other);
if (!DifferentiableFunction::isEqual(other)) return false;
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 ();
}
if (function_ != castother.function_) return false;
if (intervals_ != castother.intervals_) return false;
bool isEqual(const DifferentiableFunction& other) const {
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;
return true;
}
return true;
}
DifferentiableFunctionPtr_t function_;
segments_t intervals_;
}; // class ActiveSetDifferentiableFunction
} // namespace constraints
} // namespace hpp
DifferentiableFunctionPtr_t function_;
segments_t intervals_;
}; // class ActiveSetDifferentiableFunction
} // namespace constraints
} // namespace hpp
#endif // HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH
#endif // HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH
......@@ -27,204 +27,176 @@
// 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
{
public:
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) {}
protected:
void impl_compute (LiegroupElementRef y, vectorIn_t arg) const
{
y.vector() = arg;
}
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;
return true;
}
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
{
public:
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")
{
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()))
{
init();
}
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)
{
init();
}
bool isEqual(const DifferentiableFunction& other) const {
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;
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_jacobian (matrixOut_t jacobian, vectorIn_t) const
{
jacobian = J_;
}
void init ()
{
assert(J_.rows() == b_.rows());
activeParameters_ = (J_.array() != 0).colwise().any();
activeDerivativeParameters_ = activeParameters_;
}
const matrix_t J_;
const vector_t b_;
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
{
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 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,
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 size_type& sizeInDer,
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_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;
if (c_.vector() == castother.c_.vector())
return false;
return true;
}
const LiegroupElement c_;
private:
ConstantFunction () {}
HPP_SERIALIZABLE();
}; // class ConstantFunction
/// \}
} // namespace constraints
} // namespace hpp
#endif // HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
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));
return ptr;
}
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 {
y.vector() = arg;
}
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;
return true;
}
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 {
public:
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") {
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())) {
init();
}
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) {
init();
}
bool isEqual(const DifferentiableFunction& other) const {
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;
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_jacobian(matrixOut_t jacobian, vectorIn_t) const { jacobian = J_; }
void init() {
assert(J_.rows() == b_.rows());
activeParameters_ = (J_.array() != 0).colwise().any();
activeDerivativeParameters_ = activeParameters_;
}
const matrix_t J_;
const vector_t b_;
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 {
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 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,
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 size_type& sizeInDer,
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_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;
if (c_.vector() == castother.c_.vector()) return false;
return true;
}
const LiegroupElement c_;
private:
ConstantFunction() {}