Skip to content
GitLab
Commit 4c93031a authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

format

parent f6855f40
Expand all Hide whitespace changes
Inline Side-by-side
doc/error-norm-based.py
View file @ 4c93031a
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()
doc/figures/symmetric-pregrasp.pdf_tex
View file @ 4c93031a
......@@ -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
%%
......
doc/right-hand-side.tex
View file @ 4c93031a
......@@ -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}
doc/transformation-constraints.tex
View file @ 4c93031a
......@@ -135,4 +135,3 @@ $$
\hline
\end {tabular}
\end {document}
include/hpp/constraints/active-set-differentiable-function.hh
View file @ 4c93031a
......@@ -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
include/hpp/constraints/affine-function.hh
View file @ 4c93031a
......@@ -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() {}