hpp-constraints: include/hpp/constraints/com-between-feet.hh Source File

hpp-constraints 9.0.2 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
34#include <hpp/constraints/config.hh>
35#include <hpp/constraints/differentiable-function.hh>
36#include <hpp/constraints/fwd.hh>
37#include <hpp/constraints/symbolic-calculus.hh>
38#include <hpp/constraints/tools.hh>
39
40 namespace hpp {
41 namespace constraints {
42
67 class HPP_CONSTRAINTS_DLLAPI ComBetweenFeet : public DifferentiableFunction {
68 public:
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70
72 static ComBetweenFeetPtr_t create(
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
80 static ComBetweenFeetPtr_t create(
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,
91 const CenterOfMassComputationPtr_t& comc,
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;
134 typedef Difference<PointInJoint, PointInJoint> DiffPiJPiJ;
135 typedef Sum<PointInJoint, PointInJoint> SumPiJPiJ;
136 typedef CrossProduct<Difference<PointCom, ScalarMultiply<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
hpp::constraints::ComBetweenFeet
Definition com-between-feet.hh:67
hpp::constraints::ComBetweenFeet::create
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.
hpp::constraints::ComBetweenFeet::impl_compute
virtual void impl_compute(LiegroupElementRef result, ConfigurationIn_t argument) const
hpp::constraints::ComBetweenFeet::impl_jacobian
virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
hpp::constraints::ComBetweenFeet::isEqual
bool isEqual(const DifferentiableFunction &other) const
Definition com-between-feet.hh:107
hpp::constraints::ComBetweenFeet::create
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.
hpp::constraints::ComBetweenFeet::ComBetweenFeet
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)
hpp::constraints::ComBetweenFeet::~ComBetweenFeet
virtual ~ComBetweenFeet()
Definition com-between-feet.hh:88
hpp::constraints::CrossProduct
Cross product of two expressions.
Definition symbolic-calculus.hh:294
hpp::constraints::Difference
Difference of two expressions.
Definition symbolic-calculus.hh:387
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::constraints::Sum
Sum of two expressions.
Definition symbolic-calculus.hh:429
hpp::constraints::Traits
Definition symbolic-calculus.hh:126
hpp::constraints::Traits::Ptr_t
HPP_CONSTRAINTS_CB_REF< Class > Ptr_t
Definition symbolic-calculus.hh:127
hpp::pinocchio::LiegroupElementBase
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
differentiable-function.hh
fwd.hh
hpp::constraints::eigen::vector3_t
Eigen::Matrix< value_type, 3, 1 > vector3_t
Definition fwd.hh:74
hpp::constraints::eigen::matrix3_t
Eigen::Matrix< value_type, 3, 3 > matrix3_t
Definition fwd.hh:73
hpp::constraints::ComBetweenFeetPtr_t
shared_ptr< ComBetweenFeet > ComBetweenFeetPtr_t
Definition fwd.hh:123
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::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::CenterOfMassComputationPtr_t
pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t
Definition fwd.hh:114
hpp::constraints::JointPtr_t
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:50
hpp
symbolic-calculus.hh
tools.hh