/
configuration-constraint.hh
79 lines (69 loc) · 2.98 KB
/
configuration-constraint.hh
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
//
// Copyright (c) 2015 CNRS
// Authors: Joseph Mirabel
//
//
// This file is part of hpp-constraints.
// hpp-constraints is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-constraints is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-constraints. If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH
# define HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH
# include <boost/assign/list_of.hpp>
# include <Eigen/Core>
# include <hpp/constraints/differentiable-function.hh>
# include <hpp/constraints/config.hh>
# include <hpp/constraints/fwd.hh>
namespace hpp {
namespace constraints {
/// Square distance between input configuration and reference configuration
class HPP_CONSTRAINTS_DLLAPI ConfigurationConstraint : public DifferentiableFunction
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Return a shared pointer to a new instance
static ConfigurationConstraintPtr_t create (
const std::string& name, const DevicePtr_t& robot,
ConfigurationIn_t goal,
std::vector <bool> mask = std::vector <bool> (0));
/// Return a shared pointer to a new instance
/// \param weights vector of size robot->numberDof()
static ConfigurationConstraintPtr_t create (
const std::string& name, const DevicePtr_t& robot,
ConfigurationIn_t goal,
const vector_t& weights);
virtual ~ConfigurationConstraint () throw () {}
/// \param weights vector of size robot->numberDof()
ConfigurationConstraint (const std::string& name,
const DevicePtr_t& robot, ConfigurationIn_t goal,
const vector_t& weights);
protected:
/// Compute value of error
///
/// \param argument configuration of the robot,
/// \retval result error vector
virtual void impl_compute (LiegroupElement& result,
ConfigurationIn_t argument) const throw ();
virtual void impl_jacobian (matrixOut_t jacobian,
ConfigurationIn_t arg) const throw ();
private:
typedef Eigen::Array <bool, Eigen::Dynamic, 1> EigenBoolVector_t;
DevicePtr_t robot_;
LiegroupElement goal_;
vector_t weights_;
EigenBoolVector_t mask_;
mutable vector_t diff_;
mutable Eigen::Matrix<value_type, 1, Eigen::Dynamic> J_;
}; // class ComBetweenFeet
} // namespace constraints
} // namespace hpp
#endif // HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH