hpp-constraints  4.11.0
Definition of basic geometric constraints for motion planning
affine-function.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
18 # define HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
19 
20 # include <hpp/constraints/fwd.hh>
21 # include <hpp/constraints/config.hh>
22 
24 
25 namespace hpp {
26  namespace constraints {
27 
30 
37  {
38  public:
39  static IdentityPtr_t create (const LiegroupSpacePtr_t space, const std::string& name)
40  {
41  IdentityPtr_t ptr (new Identity (space, name));
42  return ptr;
43  }
44 
45  Identity (const LiegroupSpacePtr_t space, const std::string& name) :
46  DifferentiableFunction (space->nq(), space->nv(), space, name) {}
47 
48  protected:
50  {
51  y.vector() = arg;
52  }
53 
55  {
56  J.setIdentity();
57  }
58 
59  private:
60  Identity() {}
61  HPP_SERIALIZABLE();
62  }; // class Identity
63 
69  : public DifferentiableFunction
70  {
71  public:
72  static AffineFunctionPtr_t create
73  (const matrixIn_t& J, const std::string name = "LinearFunction")
74  {
75  return AffineFunctionPtr_t(new AffineFunction(J, name));
76  }
77 
78  static AffineFunctionPtr_t create
79  (const matrixIn_t& J, const vectorIn_t& b,
80  const std::string name = "LinearFunction")
81  {
82  return AffineFunctionPtr_t(new AffineFunction(J, b, name));
83  }
84  protected:
86  const std::string name = "LinearFunction")
87  : DifferentiableFunction (J.cols(), J.cols(), LiegroupSpace::Rn
88  (J.rows()), name),
89  J_ (J), b_ (vector_t::Zero(J.rows()))
90  {
91  init();
92  }
93 
94  AffineFunction (const matrixIn_t& J, const vectorIn_t& b,
95  const std::string name = "LinearFunction")
96  : DifferentiableFunction (J.cols(), J.cols(), LiegroupSpace::Rn
97  (J.rows()), name),
98  J_ (J), b_ (b)
99  {
100  init();
101  }
102 
103  private:
105  void impl_compute (LiegroupElementRef y, vectorIn_t x) const
106  {
107  y.vector ().noalias() = J_ * x + b_;
108  }
109 
110  void impl_jacobian (matrixOut_t jacobian, vectorIn_t) const
111  {
112  jacobian = J_;
113  }
114 
115  void init ()
116  {
117  assert(J_.rows() == b_.rows());
118  activeParameters_ = (J_.array() != 0).colwise().any();
119  activeDerivativeParameters_ = activeParameters_;
120  }
121 
122  const matrix_t J_;
123  const vector_t b_;
124 
125  AffineFunction() {}
126  HPP_SERIALIZABLE();
127  }; // class AffineFunction
128 
134  : public DifferentiableFunction
135  {
136  public:
137  static ConstantFunctionPtr_t create
138  (const vector_t& constant, const size_type& sizeIn,
139  const size_type& sizeInDer, const std::string name="ConstantFunction")
140  {
141  return ConstantFunctionPtr_t(new ConstantFunction(constant, sizeIn,
142  sizeInDer, name));
143  }
144  static ConstantFunctionPtr_t create
145  (const LiegroupElement& element, const size_type& sizeIn,
146  const size_type& sizeInDer, const std::string name="ConstantFunction")
147  {
148  return ConstantFunctionPtr_t(new ConstantFunction(element, sizeIn,
149  sizeInDer, name));
150  }
151  protected:
152  ConstantFunction (const vector_t& constant,
153  const size_type& sizeIn,
154  const size_type& sizeInDer,
155  const std::string name = "ConstantFunction") :
156  DifferentiableFunction (sizeIn, sizeInDer, LiegroupSpace::Rn
157  (constant.rows()), name),
158  c_ (constant, outputSpace())
159  {}
160 
162  const size_type& sizeIn,
163  const size_type& sizeInDer,
164  const std::string name = "ConstantFunction") :
165  DifferentiableFunction (sizeIn, sizeInDer, element.space(), name),
166  c_ (element)
167  {}
168 
170  void impl_compute (LiegroupElementRef r, vectorIn_t) const { r = c_; }
171 
172  void impl_jacobian (matrixOut_t J, vectorIn_t) const { J.setZero(); }
173 
175 
176  private:
177  ConstantFunction () {}
178  HPP_SERIALIZABLE();
179  }; // class ConstantFunction
180 
182  } // namespace constraints
183 } // namespace hpp
184 
185 
186 #endif // HPP_CONSTRAINTS_AFFINE_FUNCTION_HH
pinocchio::vector_t vector_t
Definition: fwd.hh:47
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
Identity(const LiegroupSpacePtr_t space, const std::string &name)
Definition: affine-function.hh:45
void impl_compute(LiegroupElementRef y, vectorIn_t arg) const
User implementation of function evaluation.
Definition: affine-function.hh:49
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
Eigen::Ref< const matrix_t > matrixIn_t
Definition: fwd.hh:45
void impl_jacobian(matrixOut_t J, vectorIn_t) const
Definition: affine-function.hh:172
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
ConstantFunction(const vector_t &constant, const size_type &sizeIn, const size_type &sizeInDer, const std::string name="ConstantFunction")
Definition: affine-function.hh:152
Definition: affine-function.hh:68
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
static IdentityPtr_t create(const LiegroupSpacePtr_t space, const std::string &name)
Definition: affine-function.hh:39
pinocchio::LiegroupSpace LiegroupSpace
Definition: fwd.hh:56
assert(d.lhs()._blocks()==d.rhs()._blocks())
shared_ptr< ConstantFunction > ConstantFunctionPtr_t
Definition: fwd.hh:131
Definition: differentiable-function.hh:52
shared_ptr< Identity > IdentityPtr_t
Definition: fwd.hh:129
Definition: affine-function.hh:133
void impl_jacobian(matrixOut_t J, vectorIn_t) const
Definition: affine-function.hh:54
const LiegroupElement c_
Definition: affine-function.hh:174
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
void impl_compute(LiegroupElementRef r, vectorIn_t) const
User implementation of function evaluation.
Definition: affine-function.hh:170
AffineFunction(const matrixIn_t &J, const std::string name="LinearFunction")
Definition: affine-function.hh:85
shared_ptr< AffineFunction > AffineFunctionPtr_t
Definition: fwd.hh:130
AffineFunction(const matrixIn_t &J, const vectorIn_t &b, const std::string name="LinearFunction")
Definition: affine-function.hh:94
ConstantFunction(const LiegroupElement &element, const size_type &sizeIn, const size_type &sizeInDer, const std::string name="ConstantFunction")
Definition: affine-function.hh:161
Definition: affine-function.hh:35