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

format

parent f6855f40
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np import numpy as np
from math import pow, atanh from math import atanh
from numpy import tanh, sqrt from numpy import tanh
def enb(r): def enb(r):
return C - K * tanh(a * r + b) return C - K * tanh(a * r + b)
alphaMin = 0.2 alphaMin = 0.2
C = ( 1 + alphaMin ) / 2 C = (1 + alphaMin) / 2
K = ( 1 - alphaMin ) / 2 K = (1 - alphaMin) / 2
# N = 4 # N = 4
# M = 8 # M = 8
...@@ -17,13 +19,13 @@ K = ( 1 - alphaMin ) / 2 ...@@ -17,13 +19,13 @@ K = ( 1 - alphaMin ) / 2
# b = 2. - 4. / ( 1 - 10**(N-M) ) # b = 2. - 4. / ( 1 - 10**(N-M) )
r_half_target = 10**6 r_half_target = 10**6
delta_1 = 0.02 delta_1 = 0.02
a = atanh ((delta_1 - 1 + C) / K) / (1 - r_half_target) a = atanh((delta_1 - 1 + C) / K) / (1 - r_half_target)
b = - r_half_target * a b = -r_half_target * a
r_half = - b / a r_half = -b / a
alpha_1 = enb(1) alpha_1 = enb(1)
rs = 10**(np.linspace(-1,10,200)) rs = 10 ** (np.linspace(-1, 10, 200))
fs = enb(rs) fs = enb(rs)
# plt.plot(rs, fs) # plt.plot(rs, fs)
...@@ -33,12 +35,12 @@ ax.plot(rs, fs, label="Error norm based alpha") ...@@ -33,12 +35,12 @@ ax.plot(rs, fs, label="Error norm based alpha")
ax.set_xscale("log") ax.set_xscale("log")
ax.set_xlabel("(E / E_0)^2") ax.set_xlabel("(E / E_0)^2")
ax.set_ylabel("alpha") 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.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.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.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.legend(loc="lower left")
plt.show() plt.show()
...@@ -20,7 +20,7 @@ ...@@ -20,7 +20,7 @@
%% \import{<path to file>}{<filename>.pdf_tex} %% \import{<path to file>}{<filename>.pdf_tex}
%% Alternatively, one can specify %% Alternatively, one can specify
%% \graphicspath{{<path to file>/}} %% \graphicspath{{<path to file>/}}
%% %%
%% For more information, please see info/svg-inkscape on CTAN: %% For more information, please see info/svg-inkscape on CTAN:
%% http://tug.ctan.org/tex-archive/info/svg-inkscape %% 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} ...@@ -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) 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 {eqnarray*}
\end {document} \end {document}
...@@ -135,4 +135,3 @@ $$ ...@@ -135,4 +135,3 @@ $$
\hline \hline
\end {tabular} \end {tabular}
\end {document} \end {document}
...@@ -29,89 +29,74 @@ ...@@ -29,89 +29,74 @@
#ifndef HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH #ifndef HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH
#define 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/config.hh>
#include <hpp/constraints/differentiable-function.hh> #include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/fwd.hh>
namespace hpp { 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. /// Get the original function
/// const DifferentiableFunctionPtr_t& functionPtr() const { return 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 protected:
const DifferentiableFunction& function() const typedef std::vector<segments_t> intervalss_t;
{
return *function_;
}
/// Get the original function /// User implementation of function evaluation
const DifferentiableFunctionPtr_t& functionPtr() const virtual void impl_compute(LiegroupElementRef result,
{ vectorIn_t argument) const {
return function_; function_->value(result, argument);
} }
protected: virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const {
typedef std::vector < segments_t > intervalss_t; 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 bool isEqual(const DifferentiableFunction& other) const {
virtual void impl_compute (LiegroupElementRef result, const ActiveSetDifferentiableFunction& castother =
vectorIn_t argument) const dynamic_cast<const ActiveSetDifferentiableFunction&>(other);
{ if (!DifferentiableFunction::isEqual(other)) return false;
function_->value(result, argument);
}
virtual void impl_jacobian (matrixOut_t jacobian, if (function_ != castother.function_) return false;
vectorIn_t arg) const if (intervals_ != castother.intervals_) return false;
{
function_->jacobian(jacobian, arg);
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 { return true;
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;
}
DifferentiableFunctionPtr_t function_; DifferentiableFunctionPtr_t function_;
segments_t intervals_; segments_t intervals_;
}; // class ActiveSetDifferentiableFunction }; // class ActiveSetDifferentiableFunction
} // namespace constraints } // namespace constraints
} // namespace hpp } // namespace hpp
#endif // HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH #endif // HPP_CONSTRAINTS_ACTIVE_SET_DIFFERENTIABLE_FUNCTION_HH
...@@ -27,204 +27,176 @@ ...@@ -27,204 +27,176 @@
// DAMAGE. // DAMAGE.
#ifndef HPP_CONSTRAINTS_AFFINE_FUNCTION_HH #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/config.hh> #include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/fwd.hh>
# include <hpp/constraints/differentiable-function.hh>
namespace hpp { namespace hpp {
namespace constraints { namespace constraints {
/// \addtogroup constraints /// \addtogroup constraints
/// \{ /// \{
/// Identity function /// Identity function
/// \f$ q_{out} = q_{in} \f$ /// \f$ q_{out} = q_{in} \f$
/// ///
/// \todo should we handle specifically this function is the solvers ? /// \todo should we handle specifically this function is the solvers ?
class HPP_CONSTRAINTS_DLLAPI Identity class HPP_CONSTRAINTS_DLLAPI Identity
: public constraints::DifferentiableFunction : public constraints::DifferentiableFunction {
{ public:
public: static IdentityPtr_t create(const LiegroupSpacePtr_t space,
static IdentityPtr_t create (const LiegroupSpacePtr_t space, const std::string& name) const std::string& name) {
{ IdentityPtr_t ptr(new Identity(space, name));
IdentityPtr_t ptr (new Identity (space, name)); return ptr;
return ptr; }
}
Identity(const LiegroupSpacePtr_t space, const std::string& name)
Identity (const LiegroupSpacePtr_t space, const std::string& name) : : DifferentiableFunction(space->nq(), space->nv(), space, name) {}
DifferentiableFunction (space->nq(), space->nv(), space, name) {}
protected:
protected: void impl_compute(LiegroupElementRef y, vectorIn_t arg) const {
void impl_compute (LiegroupElementRef y, vectorIn_t arg) const y.vector() = arg;
{ }
y.vector() = arg;
} void impl_jacobian(matrixOut_t J, vectorIn_t) const { J.setIdentity(); }
void impl_jacobian (matrixOut_t J, vectorIn_t) const bool isEqual(const DifferentiableFunction& other) const {
{ dynamic_cast<const Identity&>(other);
J.setIdentity(); if (!DifferentiableFunction::isEqual(other)) return false;
}
return true;
bool isEqual(const DifferentiableFunction& other) const { }
dynamic_cast<const Identity&>(other);
if (!DifferentiableFunction::isEqual(other)) private:
return false; Identity() {}
HPP_SERIALIZABLE();
return true; }; // class Identity
}
/// Affine function
private: /// \f$ f(q) = J * q + b \f$
Identity() {} ///
HPP_SERIALIZABLE(); /// \todo should we handle specifically this function is the solvers ?
}; // class Identity class HPP_CONSTRAINTS_DLLAPI AffineFunction : public DifferentiableFunction {
public:
/// Affine function static AffineFunctionPtr_t create(const matrixIn_t& J,
/// \f$ f(q) = J * q + b \f$ const std::string name = "LinearFunction") {
/// return AffineFunctionPtr_t(new AffineFunction(J, name));
/// \todo should we handle specifically this function is the solvers ? }
class HPP_CONSTRAINTS_DLLAPI AffineFunction
: public DifferentiableFunction static AffineFunctionPtr_t create(const matrixIn_t& J, const vectorIn_t& b,
{ const std::string name = "LinearFunction") {
public: return AffineFunctionPtr_t(new AffineFunction(J, b, name));
static AffineFunctionPtr_t create }
(const matrixIn_t& J, const std::string name = "LinearFunction")
{ protected:
return AffineFunctionPtr_t(new AffineFunction(J, name)); AffineFunction(const matrixIn_t& J, const std::string name = "LinearFunction")
} : DifferentiableFunction(J.cols(), J.cols(), LiegroupSpace::Rn(J.rows()),
name),
static AffineFunctionPtr_t create J_(J),
(const matrixIn_t& J, const vectorIn_t& b, b_(vector_t::Zero(J.rows())) {
const std::string name = "LinearFunction") init();
{ }
return AffineFunctionPtr_t(new AffineFunction(J, b, name));
} AffineFunction(const matrixIn_t& J, const vectorIn_t& b,
protected: const std::string name = "LinearFunction")
AffineFunction (const matrixIn_t& J, : DifferentiableFunction(J.cols(), J.cols(), LiegroupSpace::Rn(J.rows()),
const std::string name = "LinearFunction") name),
: DifferentiableFunction (J.cols(), J.cols(), LiegroupSpace::Rn J_(J),
(J.rows()), name), b_(b) {
J_ (J), b_ (vector_t::Zero(J.rows())) init();
{ }
init();
} bool isEqual(const DifferentiableFunction& other) const {
const AffineFunction& castother =
AffineFunction (const matrixIn_t& J, const vectorIn_t& b, dynamic_cast<const AffineFunction&>(other);
const std::string name = "LinearFunction") if (!DifferentiableFunction::isEqual(other)) return false;
: DifferentiableFunction (J.cols(), J.cols(), LiegroupSpace::Rn
(J.rows()), name), if (J_ != castother.J_) return false;
J_ (J), b_ (b) if (b_ != castother.b_) return false;
{
init(); return true;
} }
bool isEqual(const DifferentiableFunction& other) const { private:
const AffineFunction& castother = dynamic_cast<const AffineFunction&>(other); /// User implementation of function evaluation
if (!DifferentiableFunction::isEqual(other)) void impl_compute(LiegroupElementRef y, vectorIn_t x) const {
return false; y.vector().noalias() = J_ * x + b_;
}
if (J_ != castother.J_)
return false; void impl_jacobian(matrixOut_t jacobian, vectorIn_t) const { jacobian = J_; }
if (b_ != castother.b_)
return false; void init() {
assert(J_.rows() == b_.rows());
return true; activeParameters_ = (J_.array() != 0).colwise().any();
} activeDerivativeParameters_ = activeParameters_;
}
private:
/// User implementation of function evaluation const matrix_t J_;
void impl_compute (LiegroupElementRef y, vectorIn_t x) const const vector_t b_;
{
y.vector ().noalias() = J_ * x + b_; AffineFunction() {}
} HPP_SERIALIZABLE();
}; // class AffineFunction
void impl_jacobian (matrixOut_t jacobian, vectorIn_t) const
{ /// Constant function
jacobian = J_; /// \f$ f(q) = C \f$
} ///
/// \todo should we handle specifically this function is the solvers ?
void init () struct HPP_CONSTRAINTS_DLLAPI ConstantFunction : public DifferentiableFunction {
{ public:
assert(J_.rows() == b_.rows()); static ConstantFunctionPtr_t create(
activeParameters_ = (J_.array() != 0).colwise().any(); const vector_t& constant, const size_type& sizeIn,
activeDerivativeParameters_ = activeParameters_; const size_type& sizeInDer, const std::string name = "ConstantFunction") {
} return ConstantFunctionPtr_t(
new ConstantFunction(constant, sizeIn, sizeInDer, name));
const matrix_t J_; }
const vector_t b_; static ConstantFunctionPtr_t create(
const LiegroupElement& element, const size_type& sizeIn,
AffineFunction() {} const size_type& sizeInDer, const std::string name = "ConstantFunction") {
HPP_SERIALIZABLE(); return ConstantFunctionPtr_t(
}; // class AffineFunction new ConstantFunction(element, sizeIn, sizeInDer, name));
}
/// Constant function
/// \f$ f(q) = C \f$ protected:
/// ConstantFunction(const vector_t& constant, const size_type& sizeIn,
/// \todo should we handle specifically this function is the solvers ? const size_type& sizeInDer,
struct HPP_CONSTRAINTS_DLLAPI ConstantFunction const std::string name = "ConstantFunction")
: public DifferentiableFunction : DifferentiableFunction(sizeIn, sizeInDer,
{ LiegroupSpace::Rn(constant.rows()), name),
public: c_(constant, outputSpace()) {}
static ConstantFunctionPtr_t create
(const vector_t& constant, const size_type& sizeIn, ConstantFunction(const LiegroupElement& element, const size_type& sizeIn,
const size_type& sizeInDer, const std::string name="ConstantFunction") const size_type& sizeInDer,
{ const std::string name = "ConstantFunction")
return ConstantFunctionPtr_t(new ConstantFunction(constant, sizeIn, : DifferentiableFunction(sizeIn, sizeInDer, element.space(), name),
sizeInDer, name)); c_(element) {}
}
static ConstantFunctionPtr_t create /// User implementation of function evaluation
(const LiegroupElement& element, const size_type& sizeIn, void impl_compute(LiegroupElementRef r, vectorIn_t) const { r = c_; }
const size_type& sizeInDer, const std::string name="ConstantFunction")
{ void impl_jacobian(matrixOut_t J, vectorIn_t) const { J.setZero(); }
return ConstantFunctionPtr_t(new ConstantFunction(element, sizeIn,
sizeInDer, name)); bool isEqual(const DifferentiableFunction& other) const {
} const ConstantFunction& castother =
protected: dynamic_cast<const ConstantFunction&>(other);
ConstantFunction (const vector_t& constant, if (!DifferentiableFunction::isEqual(other)) return false;
const size_type& sizeIn,
const size_type& sizeInDer, if (c_.vector() == castother.c_.vector()) return false;
const std::string name = "ConstantFunction") :
DifferentiableFunction (sizeIn, sizeInDer, LiegroupSpace::Rn return true;
(constant.rows()), name), }
c_ (constant, outputSpace())
{} const LiegroupElement c_;
ConstantFunction (const LiegroupElement& element, private:
const size_type& sizeIn, ConstantFunction() {}
const size_type& sizeInDer, HPP_SERIALIZABLE();
const std::string name = "ConstantFunction") : }; // class ConstantFunction
DifferentiableFunction (sizeIn, sizeInDer, element.space(), name),
c_ (element) /// \}
{} } // namespace constraints
} // namespace hpp
/// User implementation of function evaluation
void impl_compute (LiegroupElementRef r, vectorIn_t) const { r = c_; } #endif // HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
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())<