hpp-constraints  4.11.0
Definition of basic geometric constraints for motion planning
relative-pose.hh
Go to the documentation of this file.
1 // Copyright (c) 2018, LAAS-CNRS
2 // Authors: Florent Lamiraux
3 //
4 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH
18 # define HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH
19 
21 # include <pinocchio/spatial/se3.hpp>
22 
23 namespace hpp {
24  namespace constraints {
25  namespace explicit_ {
28  {
29  public:
31  virtual ImplicitPtr_t copy () const;
48  static RelativePosePtr_t create
49  (const std::string& name, const DevicePtr_t& robot,
50  const JointConstPtr_t& joint1, const JointConstPtr_t& joint2,
51  const Transform3f& frame1, const Transform3f& frame2,
52  ComparisonTypes_t comp, std::vector<bool> mask=std::vector<bool>());
53 
54  static RelativePosePtr_t createCopy (const RelativePosePtr_t& other);
55 
69  virtual void outputValue(LiegroupElementRef result, vectorIn_t qin,
70  LiegroupElementConstRef rhs) const;
71 
81  virtual void jacobianOutputValue(vectorIn_t qin, LiegroupElementConstRef
82  f_value, LiegroupElementConstRef rhs,
83  matrixOut_t jacobian) const;
84  protected:
101  RelativePose (const std::string& name, const DevicePtr_t& robot,
102  const JointConstPtr_t& joint1,
103  const JointConstPtr_t& joint2,
104  const Transform3f& frame1, const Transform3f& frame2,
106  std::vector<bool> mask = std::vector<bool>(6, true));
107 
109  RelativePose (const RelativePose& other);
110 
112  void init (RelativePoseWkPtr_t weak);
113  private:
135  void implicitToExplicitRhs (LiegroupElementConstRef implicitRhs,
136  vectorOut_t explicitRhs) const;
155  void explicitToImplicitRhs (vectorIn_t explicitRhs,
156  LiegroupElementRef implicitRhs) const;
157  // Create LiegroupSpace instances to avoid useless allocation.
158  static LiegroupSpacePtr_t SE3;
159  static LiegroupSpacePtr_t R3xSO3;
160  JointConstPtr_t joint1_, joint2_;
161  Transform3f frame1_;
162  Transform3f frame2_;
163  RelativePoseWkPtr_t weak_;
164 
165  RelativePose() {}
166  HPP_SERIALIZABLE();
167  }; // class RelativePose
168  } // namespace explicit_
169  } // namespace constraints
170 } // namespace hpp
171 
172 BOOST_CLASS_EXPORT_KEY(hpp::constraints::explicit_::RelativePose)
173 
174 #endif //HPP_CONSTRAINTS_EXPLICIT_RELATIVE_POSE_HH
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
shared_ptr< RelativePose > RelativePosePtr_t
Definition: fwd.hh:191
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:55
Definition: explicit.hh:118
Constraint of relative pose between two frames on a kinematic chain.
Definition: relative-pose.hh:27
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:176
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:39
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
pinocchio::Transform3f Transform3f
Definition: fwd.hh:52