1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2019 CNRS INRIA |
3 |
|
|
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#ifndef __pinocchio_force_base_hpp__ |
7 |
|
|
#define __pinocchio_force_base_hpp__ |
8 |
|
|
|
9 |
|
|
namespace pinocchio |
10 |
|
|
{ |
11 |
|
|
/** |
12 |
|
|
* @brief Base interface for forces representation. |
13 |
|
|
* @details The Class implements all |
14 |
|
|
* \ingroup pinocchio_spatial |
15 |
|
|
* |
16 |
|
|
* This class hierarchy represents a spatial force, e.g. a spatial impulse or force associated to a body. |
17 |
|
|
* The spatial force is the mathematical representation of \f$ se^{*}(3) \f$, the dual of \f$ se(3) \f$. |
18 |
|
|
* |
19 |
|
|
* @tparam Derived { description } |
20 |
|
|
*/ |
21 |
|
|
template< class Derived> |
22 |
|
|
class ForceBase |
23 |
|
|
{ |
24 |
|
|
public: |
25 |
|
|
FORCE_TYPEDEF_TPL(Derived); |
26 |
|
|
|
27 |
|
2037345 |
Derived & derived() { return *static_cast<Derived*>(this); } |
28 |
|
724063 |
const Derived& derived() const { return *static_cast<const Derived*>(this); } |
29 |
|
|
|
30 |
|
|
/** |
31 |
|
|
* @brief Return the angular part of the force vector |
32 |
|
|
* |
33 |
|
|
* @return The 3D vector associated to the angular part of the 6D force vector |
34 |
|
|
*/ |
35 |
|
414983 |
ConstAngularType angular() const { return derived().angular_impl(); } |
36 |
|
|
|
37 |
|
|
/** |
38 |
|
|
* @brief Return the linear part of the force vector |
39 |
|
|
* |
40 |
|
|
* @return The 3D vector associated to the linear part of the 6D force vector |
41 |
|
|
*/ |
42 |
|
434679 |
ConstLinearType linear() const { return derived().linear_impl(); } |
43 |
|
|
|
44 |
|
|
/// \copydoc ForceBase::angular |
45 |
|
822279 |
AngularType angular() { return derived().angular_impl(); } |
46 |
|
|
|
47 |
|
|
/// \copydoc ForceBase::linear |
48 |
|
834319 |
LinearType linear() { return derived().linear_impl(); } |
49 |
|
|
|
50 |
|
|
|
51 |
|
|
/** |
52 |
|
|
* @brief Set the angular part of the force vector |
53 |
|
|
* |
54 |
|
|
* @tparam V3Like A vector 3 like type. |
55 |
|
|
* |
56 |
|
|
* @param[in] n |
57 |
|
|
*/ |
58 |
|
|
template<typename V3Like> |
59 |
|
|
void angular(const Eigen::MatrixBase<V3Like> & n) |
60 |
|
|
{ derived().angular_impl(n.derived()); } |
61 |
|
|
|
62 |
|
|
/** |
63 |
|
|
* @brief Set the linear part of the force vector |
64 |
|
|
* |
65 |
|
|
* @tparam V3Like A vector 3 like type. |
66 |
|
|
* |
67 |
|
|
* @param[in] f |
68 |
|
|
*/ |
69 |
|
|
template<typename V3Like> |
70 |
|
|
void linear(const Eigen::MatrixBase<V3Like> & f) |
71 |
|
|
{ derived().linear_impl(f.derived()); } |
72 |
|
|
|
73 |
|
|
/** |
74 |
|
|
* @brief Return the force as an Eigen vector. |
75 |
|
|
* |
76 |
|
|
* @return The 6D vector \f$ \phi \f$ such that |
77 |
|
|
* \f{equation*} |
78 |
|
|
* {}^{A}\phi = \begin{bmatrix} {}^{A}f \\ {}^{A}\tau \end{bmatrix} |
79 |
|
|
* \f} |
80 |
|
|
*/ |
81 |
|
86946 |
ToVectorConstReturnType toVector() const { return derived().toVector_impl(); } |
82 |
|
|
|
83 |
|
|
/// \copydoc ForceBase::toVector |
84 |
|
11742 |
ToVectorReturnType toVector() { return derived().toVector_impl(); } |
85 |
|
|
|
86 |
|
|
/* |
87 |
|
|
* @brief C-style cast operator |
88 |
|
|
* \copydoc ForceBase::toVector |
89 |
|
|
*/ |
90 |
|
4 |
operator Vector6() const { return toVector(); } |
91 |
|
|
|
92 |
|
|
/** \returns true if each coefficients of \c *this and \a other are all exactly equal. |
93 |
|
|
* \warning When using floating point scalar values you probably should rather use a |
94 |
|
|
* fuzzy comparison such as isApprox() |
95 |
|
|
*/ |
96 |
|
|
template<typename F2> |
97 |
|
1150 |
bool operator==(const ForceBase<F2> & other) const {return derived().isEqual_impl(other.derived());} |
98 |
|
|
|
99 |
|
|
/** \returns true if at least one coefficient of \c *this and \a other does not match. |
100 |
|
|
*/ |
101 |
|
|
template<typename F2> |
102 |
|
1 |
bool operator!=(const ForceBase<F2> & other) const { return !(derived() == other.derived()); } |
103 |
|
|
|
104 |
|
|
/** \returns true if *this is approximately equal to other, within the precision given by prec. |
105 |
|
|
*/ |
106 |
|
218 |
bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
107 |
|
218 |
{ return derived().isApprox_impl(other, prec); } |
108 |
|
|
|
109 |
|
|
/** \returns true if the component of the linear and angular part of the Spatial Force are approximately equal to zero, within the precision given by prec. |
110 |
|
|
*/ |
111 |
|
2 |
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
112 |
|
2 |
{ return derived().isZero_impl(prec); } |
113 |
|
|
|
114 |
|
|
/** \brief Copies the Derived Force into *this |
115 |
|
|
* \return a reference to *this |
116 |
|
|
*/ |
117 |
|
|
Derived & operator=(const ForceBase<Derived> & other) |
118 |
|
|
{ return derived().setFrom(other.derived()); } |
119 |
|
|
|
120 |
|
|
/** |
121 |
|
|
* \brief Replaces *this by *this + other. |
122 |
|
|
* \return a reference to *this |
123 |
|
|
*/ |
124 |
|
|
Derived & operator+= (const ForceBase<Derived> & phi) { return derived().__pequ__(phi.derived()); } |
125 |
|
|
|
126 |
|
|
/** |
127 |
|
|
* \brief Replaces *this by *this - other. |
128 |
|
|
* \return a reference to *this |
129 |
|
|
*/ |
130 |
|
|
Derived & operator-= (const ForceBase<Derived> & phi) { return derived().__mequ__(phi.derived()); } |
131 |
|
|
|
132 |
|
|
/** \return an expression of the sum of *this and other |
133 |
|
|
*/ |
134 |
|
|
Derived operator+(const ForceBase<Derived> & phi) const { return derived().__plus__(phi.derived()); } |
135 |
|
|
|
136 |
|
|
/** \return an expression of *this scaled by the factor alpha |
137 |
|
|
*/ |
138 |
|
|
template<typename OtherScalar> |
139 |
|
116 |
ForcePlain operator*(const OtherScalar & alpha) const { return derived().__mult__(alpha); } |
140 |
|
|
|
141 |
|
|
/** \return an expression of *this divided by the factor alpha |
142 |
|
|
*/ |
143 |
|
|
template<typename OtherScalar> |
144 |
|
1 |
ForcePlain operator/(const OtherScalar & alpha) const { return derived().__div__(alpha); } |
145 |
|
|
|
146 |
|
|
/** \return an expression of the opposite of *this |
147 |
|
|
*/ |
148 |
|
|
Derived operator-() const { return derived().__opposite__(); } |
149 |
|
|
|
150 |
|
|
/** \return an expression of the difference of *this and phi |
151 |
|
|
*/ |
152 |
|
|
Derived operator-(const ForceBase<Derived> & phi) const { return derived().__minus__(phi.derived()); } |
153 |
|
|
|
154 |
|
|
/** \return the dot product of *this with m * |
155 |
|
|
*/ |
156 |
|
|
template<typename MotionDerived> |
157 |
|
|
Scalar dot(const MotionDense<MotionDerived> & m) const { return derived().dot(m.derived()); } |
158 |
|
|
|
159 |
|
|
|
160 |
|
|
/** |
161 |
|
|
* @brief Transform from A to B coordinates the Force represented by *this such that |
162 |
|
|
* \f{equation*} |
163 |
|
|
* {}^{B}f = {}^{B}X_A^* * {}^{A}f |
164 |
|
|
* \f} |
165 |
|
|
* |
166 |
|
|
* @param[in] m The rigid transformation \f$ {}^{B}m_A \f$ whose coordinates transform for forces is |
167 |
|
|
* {}^{B}X_A^* |
168 |
|
|
* |
169 |
|
|
* @return an expression of the force expressed in the new coordinates |
170 |
|
|
*/ |
171 |
|
|
template<typename S2, int O2> |
172 |
|
|
typename SE3GroupAction<Derived>::ReturnType |
173 |
|
69611 |
se3Action(const SE3Tpl<S2,O2> & m) const |
174 |
|
69611 |
{ return derived().se3Action_impl(m); } |
175 |
|
|
|
176 |
|
|
/** |
177 |
|
|
* @brief Transform from B to A coordinates the Force represented by *this such that |
178 |
|
|
* \f{equation*} |
179 |
|
|
* {}^{A}f = {}^{A}X_B^* * {}^{A}f |
180 |
|
|
* \f} |
181 |
|
|
* |
182 |
|
|
* @param[in] m The rigid transformation \f$ {}^{B}m_A \f$ whose coordinates transform for forces is |
183 |
|
|
* {}^{B}X_A^* |
184 |
|
|
* |
185 |
|
|
* @return an expression of the force expressed in the new coordinates |
186 |
|
|
*/ |
187 |
|
|
template<typename S2, int O2> |
188 |
|
|
typename SE3GroupAction<Derived>::ReturnType |
189 |
|
965 |
se3ActionInverse(const SE3Tpl<S2,O2> & m) const |
190 |
|
965 |
{ return derived().se3ActionInverse_impl(m); } |
191 |
|
|
|
192 |
|
|
template<typename M1> |
193 |
|
|
typename MotionAlgebraAction<Derived,M1>::ReturnType |
194 |
|
|
motionAction(const MotionDense<M1> & v) const |
195 |
|
|
{ |
196 |
|
|
return derived().motionAction(v.derived()); |
197 |
|
|
} |
198 |
|
|
|
199 |
|
1 |
void disp(std::ostream & os) const { derived().disp_impl(os); } |
200 |
|
1 |
friend std::ostream & operator << (std::ostream & os, const ForceBase<Derived> & X) |
201 |
|
|
{ |
202 |
|
1 |
X.disp(os); |
203 |
|
1 |
return os; |
204 |
|
|
} |
205 |
|
|
|
206 |
|
|
}; // class ForceBase |
207 |
|
|
|
208 |
|
|
} // namespace pinocchio |
209 |
|
|
|
210 |
|
|
#endif // ifndef __pinocchio_force_base_hpp__ |