hpp-constraints  4.13.0
Definition of basic geometric constraints for motion planning
difference.hh
Go to the documentation of this file.
1 // Copyright (c) 2018, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_CONSTRAINTS_FUNCTION_DIFFERENCE_HH
30 #define HPP_CONSTRAINTS_FUNCTION_DIFFERENCE_HH
31 
34 #include <hpp/constraints/fwd.hh>
35 
36 namespace hpp {
37 namespace constraints {
38 namespace function {
39 class Difference;
40 typedef shared_ptr<Difference> DifferencePtr_t;
41 
47  public:
48  typedef shared_ptr<Difference> Ptr_t;
49  Difference(const DifferentiableFunctionPtr_t& inner, const size_type& nArgs,
50  const size_type& nDers, const segment_t& lInArgs,
51  const segment_t& lInDers, const segment_t& rInArgs,
52  const segment_t& rInDers);
53 
54  protected:
56  inner_->value(l_, arg.segment(lsa_.first, lsa_.second));
57  inner_->value(r_, arg.segment(rsa_.first, rsa_.second));
58  y.vector() = l_ - r_;
59  }
60 
61  void impl_jacobian(matrixOut_t J, vectorIn_t arg) const {
62  inner_->jacobian(J.middleCols(lsd_.first, lsd_.second),
63  arg.segment(lsa_.first, lsa_.second));
64  inner_->jacobian(J.middleCols(rsd_.first, rsd_.second),
65  arg.segment(rsa_.first, rsa_.second));
66  J.middleCols(rsd_.first, rsd_.second) *= -1;
67  }
68 
69  bool isEqual(const DifferentiableFunction& other) const {
70  const Difference& castother = dynamic_cast<const Difference&>(other);
71  if (!DifferentiableFunction::isEqual(other)) return false;
72 
73  if (inner_ != castother.inner_) return false;
74  if (lsa_ != castother.lsa_) return false;
75  if (lsd_ != castother.lsd_) return false;
76  if (rsa_ != castother.rsa_) return false;
77  if (rsd_ != castother.rsd_) return false;
78 
79  return true;
80  }
81 
82  std::ostream& print(std::ostream& os) const;
83 
85  const segment_t lsa_, lsd_;
86  const segment_t rsa_, rsd_;
87 
88  mutable LiegroupElement l_, r_;
89 }; // class Difference
90 } // namespace function
91 } // namespace constraints
92 } // namespace hpp
93 
94 #endif // HPP_CONSTRAINTS_FUNCTION_DIFFERENCE_HH
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:59
#define HPP_CONSTRAINTS_LOCAL
Definition: config.hh:66
void impl_compute(LiegroupElementRef y, vectorIn_t arg) const
User implementation of function evaluation.
Definition: difference.hh:55
Definition: active-set-differentiable-function.hh:36
DifferentiableFunctionPtr_t inner_
Definition: difference.hh:84
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:64
bool isEqual(const DifferentiableFunction &other) const
Definition: difference.hh:69
std::pair< size_type, size_type > segment_t
Definition: fwd.hh:82
void impl_jacobian(matrixOut_t J, vectorIn_t arg) const
Definition: difference.hh:61
const segment_t lsa_
Definition: difference.hh:85
const segment_t rsd_
Definition: difference.hh:86
const segment_t rsa_
Definition: difference.hh:86
virtual bool isEqual(const DifferentiableFunction &other) const
Definition: differentiable-function.hh:190
Definition: difference.hh:45
const segment_t lsd_
Definition: difference.hh:85
Definition: differentiable-function.hh:63
LiegroupElement r_
Definition: difference.hh:88
shared_ptr< Difference > DifferencePtr_t
Definition: difference.hh:39
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:111
shared_ptr< Difference > Ptr_t
Definition: difference.hh:48
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:65
pinocchio::size_type size_type
Definition: fwd.hh:47
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:57