hpp-constraints 6.0.0
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
distance-between-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_BODIES_HH
32#define HPP_CONSTRAINTS_DISTANCE_BETWEEN_BODIES_HH
33
36#include <hpp/pinocchio/collision-object.hh>
37#include <hpp/pinocchio/liegroup-element.hh>
38#include <pinocchio/multibody/geometry.hpp>
39
40namespace hpp {
41namespace constraints {
50 : public DifferentiableFunction {
51 public:
52 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53
60 static DistanceBetweenBodiesPtr_t create(const std::string& name,
61 const DevicePtr_t& robot,
62 const JointPtr_t& joint1,
63 const JointPtr_t& joint2);
64
72 const std::string& name, const DevicePtr_t& robot,
73 const JointPtr_t& joint,
74 const std::vector<CollisionObjectPtr_t>& objects);
75
77
78 protected:
85 DistanceBetweenBodies(const std::string& name, const DevicePtr_t& robot,
86 const JointPtr_t& joint1, const JointPtr_t& joint2);
87
94 DistanceBetweenBodies(const std::string& name, const DevicePtr_t& robot,
95 const JointPtr_t& joint,
96 const std::vector<CollisionObjectPtr_t>& objects);
97
98 virtual void impl_compute(LiegroupElementRef result,
99 ConfigurationIn_t argument) const;
100 virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
101
102 bool isEqual(const DifferentiableFunction& other) const {
103 const DistanceBetweenBodies& castother =
104 dynamic_cast<const DistanceBetweenBodies&>(other);
105 if (!DifferentiableFunction::isEqual(other)) return false;
106
107 if (robot_ != castother.robot_) return false;
108 if (joint1_ != castother.joint1_) return false;
109 if (joint2_ != castother.joint2_) return false;
110 if (data_ != castother.data_) return false;
111
112 return true;
113 }
114
115 private:
116 typedef ::pinocchio::GeometryData GeometryData;
117
118 DevicePtr_t robot_;
119 JointPtr_t joint1_;
120 JointPtr_t joint2_;
121 mutable GeometryData data_;
122 mutable std::size_t minIndex_;
123 mutable Configuration_t latestArgument_;
124 mutable LiegroupElement latestResult_;
125}; // class DistanceBetweenBodies
126} // namespace constraints
127} // namespace hpp
128
129#endif // HPP_CONSTRAINTS_DISTANCE_BETWEEN_BODIES_HH
Definition differentiable-function.hh:63
Definition distance-between-bodies.hh:50
static DistanceBetweenBodiesPtr_t create(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint, const std::vector< CollisionObjectPtr_t > &objects)
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW DistanceBetweenBodiesPtr_t create(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint1, const JointPtr_t &joint2)
DistanceBetweenBodies(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint, const std::vector< CollisionObjectPtr_t > &objects)
DistanceBetweenBodies(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &joint1, const JointPtr_t &joint2)
virtual ~DistanceBetweenBodies()
Definition distance-between-bodies.hh:76
bool isEqual(const DifferentiableFunction &other) const
Definition distance-between-bodies.hh:102
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
pinocchio::LiegroupElement LiegroupElement
Definition fwd.hh:65
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:105
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< DistanceBetweenBodies > DistanceBetweenBodiesPtr_t
Definition fwd.hh:117
pinocchio::LiegroupElementRef LiegroupElementRef
Definition fwd.hh:66
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:49
Definition active-set-differentiable-function.hh:36