hpp-constraints 6.0.0
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
com-between-feet.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_COM_BETWEEN_FEET_HH
32#define HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH
33
39
40namespace hpp {
41namespace constraints {
42
68 public:
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70
73 const std::string& name, const DevicePtr_t& robot,
74 const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
75 const vector3_t pointLeft, const vector3_t pointRight,
76 const JointPtr_t& jointReference, const vector3_t pointRef,
77 std::vector<bool> mask = {true, true, true, true});
78
81 const std::string& name, const DevicePtr_t& robot,
82 const CenterOfMassComputationPtr_t& comc, const JointPtr_t& jointLeft,
83 const JointPtr_t& jointRight, const vector3_t pointLeft,
84 const vector3_t pointRight, const JointPtr_t& jointReference,
85 const vector3_t pointRef,
86 std::vector<bool> mask = {true, true, true, true});
87
88 virtual ~ComBetweenFeet() {}
89
90 ComBetweenFeet(const std::string& name, const DevicePtr_t& robot,
92 const JointPtr_t& jointLeft, const JointPtr_t& jointRight,
93 const vector3_t pointLeft, const vector3_t pointRight,
94 const JointPtr_t& jointReference, const vector3_t pointRef,
95 std::vector<bool> mask);
96
97 protected:
102 virtual void impl_compute(LiegroupElementRef result,
103 ConfigurationIn_t argument) const;
104
105 virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
106
107 bool isEqual(const DifferentiableFunction& other) const {
108 const ComBetweenFeet& castother =
109 dynamic_cast<const ComBetweenFeet&>(other);
110 if (!DifferentiableFunction::isEqual(other)) return false;
111
112 if (robot_ != castother.robot_) return false;
113 if (com_->centerOfMassComputation() !=
114 castother.com_->centerOfMassComputation())
115 return false;
116 if (left_->joint() != castother.left_->joint()) return false;
117 if (right_->joint() != castother.right_->joint()) return false;
118 if (left_->local() != castother.left_->local()) return false;
119 if (right_->local() != castother.right_->local()) return false;
120 if (jointRef_ != castother.jointRef_) return false;
121 if (pointRef_ != castother.pointRef_) return false;
122 if (mask_ != castother.mask_) return false;
123
124 return true;
125 }
126
127 private:
128 DevicePtr_t robot_;
129 mutable Traits<PointCom>::Ptr_t com_;
130 Traits<PointInJoint>::Ptr_t left_, right_;
131 eigen::vector3_t pointRef_;
132 JointPtr_t jointRef_;
133 typedef Difference<PointCom, PointInJoint> DiffPCPiJ;
135 typedef Sum<PointInJoint, PointInJoint> SumPiJPiJ;
137 DiffPiJPiJ>
138 ECrossU_t;
139 mutable Traits<DiffPCPiJ>::Ptr_t xmxl_, xmxr_;
140 mutable Traits<DiffPiJPiJ>::Ptr_t u_;
141 mutable Traits<ECrossU_t>::Ptr_t ecrossu_;
142 mutable Traits<RotationMultiply<ECrossU_t> >::Ptr_t expr_;
143 mutable Traits<ScalarProduct<DiffPCPiJ, DiffPiJPiJ> >::Ptr_t xmxlDotu_,
144 xmxrDotu_;
145 std::vector<bool> mask_;
146 mutable eigen::matrix3_t cross_;
147}; // class ComBetweenFeet
148} // namespace constraints
149} // namespace hpp
150#endif // HPP_CONSTRAINTS_COM_BETWEEN_FEET_HH
Definition com-between-feet.hh:67
static ComBetweenFeetPtr_t create(const std::string &name, const DevicePtr_t &robot, const CenterOfMassComputationPtr_t &comc, const JointPtr_t &jointLeft, const JointPtr_t &jointRight, const vector3_t pointLeft, const vector3_t pointRight, const JointPtr_t &jointReference, const vector3_t pointRef, std::vector< bool > mask={true, true, true, true})
Return a shared pointer to a new instance.
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
bool isEqual(const DifferentiableFunction &other) const
Definition com-between-feet.hh:107
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW ComBetweenFeetPtr_t create(const std::string &name, const DevicePtr_t &robot, const JointPtr_t &jointLeft, const JointPtr_t &jointRight, const vector3_t pointLeft, const vector3_t pointRight, const JointPtr_t &jointReference, const vector3_t pointRef, std::vector< bool > mask={true, true, true, true})
Return a shared pointer to a new instance.
ComBetweenFeet(const std::string &name, const DevicePtr_t &robot, const CenterOfMassComputationPtr_t &comc, const JointPtr_t &jointLeft, const JointPtr_t &jointRight, const vector3_t pointLeft, const vector3_t pointRight, const JointPtr_t &jointReference, const vector3_t pointRef, std::vector< bool > mask)
virtual ~ComBetweenFeet()
Definition com-between-feet.hh:88
Cross product of two expressions.
Definition symbolic-calculus.hh:294
Difference of two expressions.
Definition symbolic-calculus.hh:387
Definition differentiable-function.hh:63
Sum of two expressions.
Definition symbolic-calculus.hh:429
Definition symbolic-calculus.hh:126
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition symbolic-calculus.hh:127
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition fwd.hh:73
Eigen::Matrix< value_type, 3, 3 > matrix3_t
Definition fwd.hh:72
shared_ptr< ComBetweenFeet > ComBetweenFeetPtr_t
Definition fwd.hh:121
pinocchio::vector3_t vector3_t
Definition fwd.hh:52
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
pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t
Definition fwd.hh:112
pinocchio::LiegroupElementRef LiegroupElementRef
Definition fwd.hh:66
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:49
Definition active-set-differentiable-function.hh:36