hpp-constraints  4.11.0
Definition of basic geometric constraints for motion planning
symbolic-function.hh
Go to the documentation of this file.
1 // Copyright (c) 2015, LAAS-CNRS
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_SYMBOLIC_FUNCTION_HH
18 # define HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH
19 
20 # include <hpp/constraints/fwd.hh>
21 # include <hpp/constraints/config.hh>
24 
25 # include <hpp/pinocchio/device.hh>
26 # include <hpp/pinocchio/liegroup-element.hh>
27 
28 namespace hpp {
29  namespace constraints {
30 
34  template <typename Expression>
36  {
37  public:
38  typedef shared_ptr<SymbolicFunction> Ptr_t;
39  typedef weak_ptr<SymbolicFunction> WkPtr_t;
40 
41  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 
44  static Ptr_t create (
45  const std::string& name, const DevicePtr_t& robot,
46  const typename Traits<Expression>::Ptr_t expr)
47  {
48  std::vector <bool> mask (expr->value ().size (), true);
49  return create (name, robot, expr, mask);
50  }
51 
53  static Ptr_t create (
54  const std::string& name, const DevicePtr_t& robot,
55  const typename Traits<Expression>::Ptr_t expr,
56  std::vector <bool> mask)
57  {
58  assert (mask.size() == (std::size_t) expr->value().size());
59  Ptr_t ptr (new SymbolicFunction (name,robot,expr,mask));
60  ptr->init (ptr);
61  return ptr;
62  }
63 
65  static Ptr_t create (const std::string& name, size_type sizeInput,
66  size_type sizeInputDerivative,
67  size_type sizeOutput,
68  const typename Traits<Expression>::Ptr_t expr)
69  {
70  std::vector<bool> mask (sizeOutput, true);
71  Ptr_t ptr (new SymbolicFunction (name,sizeInput,sizeInputDerivative,sizeOutput,expr,mask));
72  ptr->init (ptr);
73  return ptr;
74  }
75 
76  virtual ~SymbolicFunction () {}
77 
78  SymbolicFunction (const std::string& name, const DevicePtr_t& robot,
79  const typename Traits<Expression>::Ptr_t expr,
80  std::vector <bool> mask) :
81  DifferentiableFunction (robot->configSize(), robot->numberDof(),
82  LiegroupSpace::Rn (expr->value().size()),
83  name),
84  robot_ (robot), expr_ (expr), mask_ (mask)
85  {
86  size_type d = robot_->extraConfigSpace().dimension();
87  activeParameters_ .tail(d).setConstant(false);
88  activeDerivativeParameters_.tail(d).setConstant(false);
89  }
90 
91  SymbolicFunction (const std::string& name, size_type sizeInput,
92  size_type sizeInputDerivative,
93  size_type sizeOutput,
94  const typename Traits<Expression>::Ptr_t expr,
95  std::vector <bool> mask) :
96  DifferentiableFunction (sizeInput, sizeInputDerivative,
97  LiegroupSpace::Rn (sizeOutput), name),
98  robot_ (), expr_ (expr), mask_ (mask) {}
99 
100  protected:
105  virtual void impl_compute (LiegroupElementRef result,
106  ConfigurationIn_t argument) const
107  {
108  if (robot_) {
109  robot_->currentConfiguration (argument);
110  robot_->computeForwardKinematics ();
111  }
112  expr_->invalidate ();
113  expr_->computeValue (argument);
114  size_t index = 0;
115  for (std::size_t i = 0; i < mask_.size (); i++) {
116  if (mask_[i])
117  result.vector () [index++] = expr_->value () [i];
118  }
119  }
120 
121  virtual void impl_jacobian (matrixOut_t jacobian,
122  ConfigurationIn_t arg) const
123  {
124  if (robot_) {
125  robot_->currentConfiguration (arg);
126  robot_->computeForwardKinematics ();
127  }
128  expr_->invalidate ();
129  expr_->computeJacobian (arg);
130  size_t index = 0;
131 
132  const typename Expression::JacobianType_t& Je (expr_->jacobian());
133  size_type nv;
134  if (robot_) {
135  size_type d = robot_->extraConfigSpace().dimension();
136  nv = Je.cols();
137  assert(robot_->numberDof() == Je.cols() + d);
138  jacobian.rightCols(d).setZero();
139  } else {
140  nv = jacobian.cols();
141  }
142 
143  for (std::size_t i = 0; i < mask_.size (); i++) {
144  if (mask_[i])
145  jacobian.row(index++).head(nv) = Je.row (i);
146  }
147  }
148 
149  void init (const Ptr_t& self) {
150  wkPtr_ = self;
151  }
152  private:
153  WkPtr_t wkPtr_;
154  DevicePtr_t robot_;
155  typename Traits<Expression>::Ptr_t expr_;
156  std::vector <bool> mask_;
157  }; // class ComBetweenFeet
158  } // namespace constraints
159 } // namespace hpp
160 #endif // HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH
void init(const Ptr_t &self)
Definition: symbolic-function.hh:149
virtual ~SymbolicFunction()
Definition: symbolic-function.hh:76
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
Definition: symbolic-function.hh:121
SymbolicFunction(const std::string &name, size_type sizeInput, size_type sizeInputDerivative, size_type sizeOutput, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Definition: symbolic-function.hh:91
const Derived & d
Definition: matrix-view-operation.hh:126
Definition: active-set-differentiable-function.hh:24
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
SymbolicFunction(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Definition: symbolic-function.hh:78
pinocchio::LiegroupSpace LiegroupSpace
Definition: fwd.hh:56
shared_ptr< SymbolicFunction > Ptr_t
Definition: symbolic-function.hh:38
Definition: symbolic-function.hh:35
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
assert(d.lhs()._blocks()==d.rhs()._blocks())
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW Ptr_t create(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr)
Return a shared pointer to a new instance.
Definition: symbolic-function.hh:44
static Ptr_t create(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Return a shared pointer to a new instance.
Definition: symbolic-function.hh:53
Definition: differentiable-function.hh:52
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
Definition: symbolic-function.hh:105
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition: symbolic-calculus.hh:107
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
weak_ptr< SymbolicFunction > WkPtr_t
Definition: symbolic-function.hh:39
static Ptr_t create(const std::string &name, size_type sizeInput, size_type sizeInputDerivative, size_type sizeOutput, const typename Traits< Expression >::Ptr_t expr)
Return a shared pointer to a new instance.
Definition: symbolic-function.hh:65