hpp-constraints: include/hpp/constraints/symbolic-function.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
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
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_SYMBOLIC_FUNCTION_HH
30#define HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH
31
32#include <hpp/constraints/config.hh>
33#include <hpp/constraints/differentiable-function.hh>
34#include <hpp/constraints/fwd.hh>
35#include <hpp/constraints/symbolic-calculus.hh>
36#include <hpp/pinocchio/device.hh>
37#include <hpp/pinocchio/liegroup-element.hh>
38
39 namespace hpp {
40 namespace constraints {
41
46 template <typename Expression>
47 class HPP_CONSTRAINTS_DLLAPI SymbolicFunction : public DifferentiableFunction {
48 public:
49 typedef shared_ptr<SymbolicFunction> Ptr_t;
50 typedef weak_ptr<SymbolicFunction> WkPtr_t;
51
52 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53
55 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
56 const typename Traits<Expression>::Ptr_t expr) {
57 std::vector<bool> mask(expr->value().size(), true);
58 return create(name, robot, expr, mask);
59 }
60
62 static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
63 const typename Traits<Expression>::Ptr_t expr,
64 std::vector<bool> mask) {
65 assert(mask.size() == (std::size_t)expr->value().size());
66 Ptr_t ptr(new SymbolicFunction(name, robot, expr, mask));
67 ptr->init(ptr);
68 return ptr;
69 }
70
72 static Ptr_t create(const std::string& name, size_type sizeInput,
73 size_type sizeInputDerivative, size_type sizeOutput,
74 const typename Traits<Expression>::Ptr_t expr) {
75 std::vector<bool> mask(sizeOutput, true);
76 Ptr_t ptr(new SymbolicFunction(name, sizeInput, sizeInputDerivative,
77 sizeOutput, expr, mask));
78 ptr->init(ptr);
79 return ptr;
80 }
81
82 virtual ~SymbolicFunction() {}
83
84 SymbolicFunction(const std::string& name, const DevicePtr_t& robot,
85 const typename Traits<Expression>::Ptr_t expr,
86 std::vector<bool> mask)
87 : DifferentiableFunction(robot->configSize(), robot->numberDof(),
88 LiegroupSpace::Rn(expr->value().size()), name),
89 robot_(robot),
90 expr_(expr),
91 mask_(mask) {
92 size_type d = robot_->extraConfigSpace().dimension();
93 activeParameters_.tail(d).setConstant(false);
94 activeDerivativeParameters_.tail(d).setConstant(false);
95 }
96
97 SymbolicFunction(const std::string& name, size_type sizeInput,
98 size_type sizeInputDerivative, size_type sizeOutput,
99 const typename Traits<Expression>::Ptr_t expr,
100 std::vector<bool> mask)
101 : DifferentiableFunction(sizeInput, sizeInputDerivative,
102 LiegroupSpace::Rn(sizeOutput), name),
103 robot_(),
104 expr_(expr),
105 mask_(mask) {}
106
107 protected:
112 virtual void impl_compute(LiegroupElementRef result,
113 ConfigurationIn_t argument) const {
114 if (robot_) {
115 robot_->currentConfiguration(argument);
116 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION);
117 }
118 expr_->invalidate();
119 expr_->computeValue(argument);
120 size_t index = 0;
121 for (std::size_t i = 0; i < mask_.size(); i++) {
122 if (mask_[i]) result.vector()[index++] = expr_->value()[i];
123 }
124 }
125
126 virtual void impl_jacobian(matrixOut_t jacobian,
127 ConfigurationIn_t arg) const {
128 if (robot_) {
129 robot_->currentConfiguration(arg);
130 robot_->computeForwardKinematics(pinocchio::JOINT_POSITION |
131 pinocchio::JACOBIAN);
132 }
133 expr_->invalidate();
134 expr_->computeJacobian(arg);
135 size_t index = 0;
136
137 const typename Expression::JacobianType_t& Je(expr_->jacobian());
138 size_type nv;
139 if (robot_) {
140 size_type d = robot_->extraConfigSpace().dimension();
141 nv = Je.cols();
142 assert(robot_->numberDof() == Je.cols() + d);
143 jacobian.rightCols(d).setZero();
144 } else {
145 nv = jacobian.cols();
146 }
147
148 for (std::size_t i = 0; i < mask_.size(); i++) {
149 if (mask_[i]) jacobian.row(index++).head(nv) = Je.row(i);
150 }
151 }
152
153 void init(const Ptr_t& self) { wkPtr_ = self; }
154
155 bool isEqual(const DifferentiableFunction& other) const {
156 const SymbolicFunction& castother =
157 dynamic_cast<const SymbolicFunction&>(other);
158 if (!DifferentiableFunction::isEqual(other)) return false;
159
160 if (robot_ != castother.robot_) return false;
161 if (expr_ != castother.expr_) return false;
162 if (mask_ != castother.mask_) return false;
163
164 return true;
165 }
166
167 private:
168 WkPtr_t wkPtr_;
169 DevicePtr_t robot_;
170 typename Traits<Expression>::Ptr_t expr_;
171 std::vector<bool> mask_;
172}; // class SymbolicFunction
173} // namespace constraints
174} // namespace hpp
175#endif // HPP_CONSTRAINTS_SYMBOLIC_FUNCTION_HH
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::constraints::SymbolicFunction
Definition symbolic-function.hh:47
hpp::constraints::SymbolicFunction::impl_jacobian
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
Definition symbolic-function.hh:126
hpp::constraints::SymbolicFunction::create
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:55
hpp::constraints::SymbolicFunction::create
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:62
hpp::constraints::SymbolicFunction::~SymbolicFunction
virtual ~SymbolicFunction()
Definition symbolic-function.hh:82
hpp::constraints::SymbolicFunction::isEqual
bool isEqual(const DifferentiableFunction &other) const
Definition symbolic-function.hh:155
hpp::constraints::SymbolicFunction::WkPtr_t
weak_ptr< SymbolicFunction > WkPtr_t
Definition symbolic-function.hh:50
hpp::constraints::SymbolicFunction::impl_compute
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
Definition symbolic-function.hh:112
hpp::constraints::SymbolicFunction::Ptr_t
shared_ptr< SymbolicFunction > Ptr_t
Definition symbolic-function.hh:49
hpp::constraints::SymbolicFunction::init
void init(const Ptr_t &self)
Definition symbolic-function.hh:153
hpp::constraints::SymbolicFunction::SymbolicFunction
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:97
hpp::constraints::SymbolicFunction::create
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:72
hpp::constraints::SymbolicFunction::SymbolicFunction
SymbolicFunction(const std::string &name, const DevicePtr_t &robot, const typename Traits< Expression >::Ptr_t expr, std::vector< bool > mask)
Definition symbolic-function.hh:84
hpp::constraints::Traits::Ptr_t
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition symbolic-calculus.hh:127
hpp::pinocchio::LiegroupElementBase
hpp::pinocchio::LiegroupElementBase::vector
const vector_type & vector() const
hpp::pinocchio::LiegroupSpace
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
device.hh
differentiable-function.hh
fwd.hh
liegroup-element.hh
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:111
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:59
hpp
symbolic-calculus.hh