hpp-constraints  4.10.1
Definition of basic geometric constraints for motion planning
implicit-function.hh
Go to the documentation of this file.
1 // Copyright (c) 2015 - 2018 LAAS-CNRS
2 // Authors: Florent Lamiraux, Joseph Mirabel
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_EXPLICIT_IMPLICIT_FUNCTION_HH
18 # define HPP_CONSTRAINTS_EXPLICIT_IMPLICIT_FUNCTION_HH
19 
22 
23 namespace hpp {
24  namespace constraints {
25  namespace explicit_ {
38  {
39  public:
40  typedef boost::shared_ptr <ImplicitFunction> Ptr_t;
51 
52  static Ptr_t create
53  (const LiegroupSpacePtr_t& configSpace,
54  const DifferentiableFunctionPtr_t& function,
55  const segments_t& inputConf, const segments_t& outputConf,
56  const segments_t& inputVelocity, const segments_t& outputVelocity);
57 
60 
61  protected:
70  ImplicitFunction (const LiegroupSpacePtr_t& configSpace,
71  const DifferentiableFunctionPtr_t& function,
72  const segments_t& inputConf,
73  const segments_t& outputConf,
74  const segments_t& inputVelocity,
75  const segments_t& outputVelocity);
77  void impl_compute (LiegroupElementRef result, vectorIn_t argument) const;
78 
80  void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const;
81 
82  private:
83  void computeJacobianBlocks ();
84 
85  DevicePtr_t robot_;
86  DifferentiableFunctionPtr_t inputToOutput_;
87  Eigen::RowBlockIndices inputConfIntervals_;
88  Eigen::RowBlockIndices outputConfIntervals_;
89  Eigen::RowBlockIndices inputDerivIntervals_;
90  Eigen::RowBlockIndices outputDerivIntervals_;
91  std::vector <Eigen::MatrixBlocks <false, false> > outJacobian_;
92  std::vector <Eigen::MatrixBlocks <false, false> > inJacobian_;
93  mutable vector_t qIn_;
94  mutable LiegroupElement f_qIn_, qOut_;
95  mutable LiegroupElement result_;
96  // Jacobian of explicit function
97  mutable matrix_t Jf_;
98 
99  ImplicitFunction() {}
100  HPP_SERIALIZABLE();
101  }; // class ImplicitFunction
102 
103  } // namespace explicit_
104  } // namespace constraints
105 } // namespace hpp
106 
107 #endif // HPP_CONSTRAINTS_EXPLICIT_IMPLICIT_FUNCTION_HH
pinocchio::vector_t vector_t
Definition: fwd.hh:47
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
const DifferentiableFunctionPtr_t & inputToOutput() const
Get function f that maps input variables to output variables.
static Ptr_t create(const LiegroupSpacePtr_t &configSpace, const DifferentiableFunctionPtr_t &function, const segments_t &inputConf, const segments_t &outputConf, const segments_t &inputVelocity, const segments_t &outputVelocity)
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const
Compute Jacobian of g (q_out) - f (q_in) with respect to q.
std::vector< segment_t > segments_t
Definition: fwd.hh:72
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
boost::shared_ptr< ImplicitFunction > Ptr_t
Definition: implicit-function.hh:40
Definition: implicit-function.hh:37
void jacobian(matrixOut_t jacobian, vectorIn_t argument) const
Definition: differentiable-function.hh:80
Definition: differentiable-function.hh:52
boost::shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:101
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
void impl_compute(LiegroupElementRef result, vectorIn_t argument) const
Compute g (q_out) - f (q_in)