hpp-constraints 6.0.0
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
configuration-constraint.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2015 CNRS
3// Authors: Joseph Mirabel
4//
5//
6
7// Redistribution and use in source and binary forms, with or without
8// modification, are permitted provided that the following conditions are
9// met:
10//
11// 1. Redistributions of source code must retain the above copyright
12// notice, this list of conditions and the following disclaimer.
13//
14// 2. Redistributions in binary form must reproduce the above copyright
15// notice, this list of conditions and the following disclaimer in the
16// documentation and/or other materials provided with the distribution.
17//
18// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29// DAMAGE.
30
31#ifndef HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH
32#define HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH
33
34#include <Eigen/Core>
38
39namespace hpp {
40namespace constraints {
41
44 : public DifferentiableFunction {
45 public:
46 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47
50 const std::string& name, const DevicePtr_t& robot, ConfigurationIn_t goal,
51 std::vector<bool> mask = std::vector<bool>(0));
52
55 static ConfigurationConstraintPtr_t create(const std::string& name,
56 const DevicePtr_t& robot,
58 const vector_t& weights);
59
61
63 ConfigurationConstraint(const std::string& name, const DevicePtr_t& robot,
64 ConfigurationIn_t goal, const vector_t& weights);
65
66 const vector_t& weights() const { return weights_; }
67
68 void weights(const vector_t& ws);
69
70 const LiegroupElement& goal() const { return goal_; }
71
72 protected:
77 virtual void impl_compute(LiegroupElementRef result,
78 ConfigurationIn_t argument) const;
79
80 virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
81
82 std::ostream& print(std::ostream& o) const;
83
84 bool isEqual(const DifferentiableFunction& other) const {
85 const ConfigurationConstraint& castother =
86 dynamic_cast<const ConfigurationConstraint&>(other);
87 if (!DifferentiableFunction::isEqual(other)) return false;
88
89 if (robot_ != castother.robot_) return false;
90 if (goal_.vector() != castother.goal_.vector()) return false;
91 if (weights_ != castother.weights_) return false;
92
93 return true;
94 }
95
96 private:
97 typedef Eigen::Array<bool, Eigen::Dynamic, 1> EigenBoolVector_t;
98 DevicePtr_t robot_;
99 LiegroupElement goal_;
100 vector_t weights_;
101}; // class ConfigurationConstraint
102} // namespace constraints
103} // namespace hpp
104#endif // HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH
Square distance between input configuration and reference configuration.
Definition configuration-constraint.hh:44
bool isEqual(const DifferentiableFunction &other) const
Definition configuration-constraint.hh:84
std::ostream & print(std::ostream &o) const
Display object in a stream.
static ConfigurationConstraintPtr_t create(const std::string &name, const DevicePtr_t &robot, ConfigurationIn_t goal, const vector_t &weights)
const vector_t & weights() const
Definition configuration-constraint.hh:66
const LiegroupElement & goal() const
Definition configuration-constraint.hh:70
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
virtual ~ConfigurationConstraint()
Definition configuration-constraint.hh:60
ConfigurationConstraint(const std::string &name, const DevicePtr_t &robot, ConfigurationIn_t goal, const vector_t &weights)
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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.
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
Definition differentiable-function.hh:63
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
pinocchio::LiegroupElement LiegroupElement
Definition fwd.hh:65
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:109
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:106
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:58
shared_ptr< ConfigurationConstraint > ConfigurationConstraintPtr_t
Definition fwd.hh:133
pinocchio::vector_t vector_t
Definition fwd.hh:59
pinocchio::LiegroupElementRef LiegroupElementRef
Definition fwd.hh:66
Definition active-set-differentiable-function.hh:36