hpp-constraints: include/hpp/constraints/distance-between-points-in-bodies.hh Source File

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

Loading...
Searching...
No Matches
distance-between-points-in-bodies.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2015 CNRS
3// Authors: Florent Lamiraux
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_DISTANCE_BETWEEN_POINTS_IN_BODIES_HH
32#define HPP_CONSTRAINTS_DISTANCE_BETWEEN_POINTS_IN_BODIES_HH
33
34#include <hpp/constraints/differentiable-function.hh>
35#include <hpp/pinocchio/liegroup-element.hh>
36
37 namespace hpp {
38 namespace constraints {
39
47 class HPP_CONSTRAINTS_DLLAPI DistanceBetweenPointsInBodies
48 : public DifferentiableFunction {
49 public:
50 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51
60 static DistanceBetweenPointsInBodiesPtr_t create(const std::string& name,
61 const DevicePtr_t& robot,
62 const JointPtr_t& joint1,
63 const JointPtr_t& joint2,
64 const vector3_t& point1,
65 const vector3_t& point2);
66
74 static DistanceBetweenPointsInBodiesPtr_t create(const std::string& name,
75 const DevicePtr_t& robot,
76 const JointPtr_t& joint1,
77 const vector3_t& point1,
78 const vector3_t& point2);
79
80 virtual ~DistanceBetweenPointsInBodies() {}
81
82 protected:
89 DistanceBetweenPointsInBodies(const std::string& name,
90 const DevicePtr_t& robot,
91 const JointPtr_t& joint1,
92 const JointPtr_t& joint2,
93 const vector3_t& point1,
94 const vector3_t& point2);
95
101 DistanceBetweenPointsInBodies(const std::string& name,
102 const DevicePtr_t& robot,
103 const JointPtr_t& joint1,
104 const vector3_t& point1,
105 const vector3_t& point2);
106
107 virtual void impl_compute(LiegroupElementRef result,
108 ConfigurationIn_t argument) const;
109 virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
110
111 bool isEqual(const DifferentiableFunction& other) const {
112 const DistanceBetweenPointsInBodies& castother =
113 dynamic_cast<const DistanceBetweenPointsInBodies&>(other);
114 if (!DifferentiableFunction::isEqual(other)) return false;
115
116 if (robot_ != castother.robot_) return false;
117 if (joint1_ != castother.joint1_) return false;
118 if (joint2_ != castother.joint2_) return false;
119 if (point1_ != castother.point1_) return false;
120 if (point2_ != castother.point2_) return false;
121
122 return true;
123 }
124
125 private:
126 DevicePtr_t robot_;
127 JointPtr_t joint1_;
128 JointPtr_t joint2_;
129 vector3_t point1_;
130 vector3_t point2_;
131 mutable vector3_t global1_;
132 mutable vector3_t global2_;
133 mutable Configuration_t latestArgument_;
134 mutable LiegroupElement latestResult_;
135}; // class DistanceBetweenPointsInBodies
136} // namespace constraints
137} // namespace hpp
138
139#endif // HPP_CONSTRAINTS_DISTANCE_BETWEEN_POINTS_IN_BODIES_HH
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::constraints::DistanceBetweenPointsInBodies
Definition distance-between-points-in-bodies.hh:48
hpp::constraints::DistanceBetweenPointsInBodies::create
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW DistanceBetweenPointsInBodiesPtr_t create(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint1, const JointPtr_t &joint2, const vector3_t &point1, const vector3_t &point2)
hpp::constraints::DistanceBetweenPointsInBodies::impl_compute
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
hpp::constraints::DistanceBetweenPointsInBodies::impl_jacobian
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
hpp::constraints::DistanceBetweenPointsInBodies::~DistanceBetweenPointsInBodies
virtual ~DistanceBetweenPointsInBodies()
Definition distance-between-points-in-bodies.hh:80
hpp::constraints::DistanceBetweenPointsInBodies::DistanceBetweenPointsInBodies
DistanceBetweenPointsInBodies(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint1, const vector3_t &point1, const vector3_t &point2)
hpp::constraints::DistanceBetweenPointsInBodies::DistanceBetweenPointsInBodies
DistanceBetweenPointsInBodies(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint1, const JointPtr_t &joint2, const vector3_t &point1, const vector3_t &point2)
hpp::constraints::DistanceBetweenPointsInBodies::create
static DistanceBetweenPointsInBodiesPtr_t create(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint1, const vector3_t &point1, const vector3_t &point2)
hpp::constraints::DistanceBetweenPointsInBodies::isEqual
bool isEqual(const DifferentiableFunction &other) const
Definition distance-between-points-in-bodies.hh:111
hpp::pinocchio::LiegroupElementBase
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
differentiable-function.hh
liegroup-element.hh
hpp::constraints::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::constraints::vector3_t
pinocchio::vector3_t vector3_t
Definition fwd.hh:53
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:111
hpp::constraints::DistanceBetweenPointsInBodiesPtr_t
shared_ptr< DistanceBetweenPointsInBodies > DistanceBetweenPointsInBodiesPtr_t
Definition fwd.hh:121
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::constraints::JointPtr_t
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:50
hpp