GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2019 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__ |
||
6 |
#define __pinocchio_joint_revolute_unbounded_unaligned_hpp__ |
||
7 |
|||
8 |
#include "pinocchio/fwd.hpp" |
||
9 |
#include "pinocchio/spatial/inertia.hpp" |
||
10 |
#include "pinocchio/math/rotation.hpp" |
||
11 |
#include "pinocchio/math/matrix.hpp" |
||
12 |
|||
13 |
#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp" |
||
14 |
|||
15 |
namespace pinocchio |
||
16 |
{ |
||
17 |
|||
18 |
template<typename Scalar, int Options = 0> struct JointRevoluteUnboundedUnalignedTpl; |
||
19 |
|||
20 |
template<typename _Scalar, int _Options> |
||
21 |
struct traits< JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> > |
||
22 |
{ |
||
23 |
enum { |
||
24 |
NQ = 2, |
||
25 |
NV = 1 |
||
26 |
}; |
||
27 |
typedef _Scalar Scalar; |
||
28 |
enum { Options = _Options }; |
||
29 |
|||
30 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
31 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
32 |
|||
33 |
typedef JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDataDerived; |
||
34 |
typedef JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModelDerived; |
||
35 |
typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t; |
||
36 |
typedef SE3Tpl<Scalar,Options> Transformation_t; |
||
37 |
typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t; |
||
38 |
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
||
39 |
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t; |
||
40 |
|||
41 |
// [ABA] |
||
42 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
43 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
44 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
45 |
|||
46 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
47 |
}; |
||
48 |
|||
49 |
template<typename Scalar, int Options> |
||
50 |
struct traits< JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
51 |
{ typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; }; |
||
52 |
|||
53 |
template<typename Scalar, int Options> |
||
54 |
struct traits <JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
55 |
{ typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; }; |
||
56 |
|||
57 |
template<typename _Scalar, int _Options> |
||
58 |
struct JointDataRevoluteUnboundedUnalignedTpl |
||
59 |
: public JointDataBase< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> > |
||
60 |
{ |
||
61 |
22 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
62 |
typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived; |
||
63 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
64 |
646 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
65 |
|||
66 |
Transformation_t M; |
||
67 |
Constraint_t S; |
||
68 |
Motion_t v; |
||
69 |
Bias_t c; |
||
70 |
|||
71 |
// [ABA] specific data |
||
72 |
U_t U; |
||
73 |
D_t Dinv; |
||
74 |
UD_t UDinv; |
||
75 |
|||
76 |
26 |
JointDataRevoluteUnboundedUnalignedTpl() |
|
77 |
: M(Transformation_t::Identity()) |
||
78 |
, S(Constraint_t::Vector3::Zero()) |
||
79 |
, v(Constraint_t::Vector3::Zero(),(Scalar)0) |
||
80 |
, U(U_t::Zero()) |
||
81 |
, Dinv(D_t::Zero()) |
||
82 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
26 |
, UDinv(UD_t::Zero()) |
83 |
26 |
{} |
|
84 |
|||
85 |
template<typename Vector3Like> |
||
86 |
11 |
JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) |
|
87 |
: M(Transformation_t::Identity()) |
||
88 |
, S(axis) |
||
89 |
, v(axis,(Scalar)NAN) |
||
90 |
, U(U_t::Zero()) |
||
91 |
, Dinv(D_t::Zero()) |
||
92 |
✓✗✓✗ ✓✗✓✗ |
11 |
, UDinv(UD_t::Zero()) |
93 |
11 |
{} |
|
94 |
|||
95 |
✓✗ | 44 |
static std::string classname() { return std::string("JointDataRevoluteUnboundedUnalignedTpl"); } |
96 |
3 |
std::string shortname() const { return classname(); } |
|
97 |
|||
98 |
}; // struct JointDataRevoluteUnboundedUnalignedTpl |
||
99 |
|||
100 |
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl); |
||
101 |
|||
102 |
template<typename _Scalar, int _Options> |
||
103 |
struct JointModelRevoluteUnboundedUnalignedTpl |
||
104 |
: public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> > |
||
105 |
{ |
||
106 |
32 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
107 |
typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived; |
||
108 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
109 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
110 |
|||
111 |
typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base; |
||
112 |
using Base::id; |
||
113 |
using Base::idx_q; |
||
114 |
using Base::idx_v; |
||
115 |
using Base::setIndexes; |
||
116 |
|||
117 |
40 |
JointModelRevoluteUnboundedUnalignedTpl() {} |
|
118 |
|||
119 |
JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x, |
||
120 |
const Scalar & y, |
||
121 |
const Scalar & z) |
||
122 |
: axis(x,y,z) |
||
123 |
{ |
||
124 |
axis.normalize(); |
||
125 |
assert(isUnitary(axis) && "Rotation axis is not unitary"); |
||
126 |
} |
||
127 |
|||
128 |
template<typename Vector3Like> |
||
129 |
27 |
JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis) |
|
130 |
27 |
: axis(axis) |
|
131 |
{ |
||
132 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); |
||
133 |
✓✗✓✗ |
27 |
assert(isUnitary(axis) && "Rotation axis is not unitary"); |
134 |
27 |
} |
|
135 |
|||
136 |
11 |
JointDataDerived createData() const { return JointDataDerived(axis); } |
|
137 |
|||
138 |
const std::vector<bool> hasConfigurationLimit() const |
||
139 |
{ |
||
140 |
return {false, false}; |
||
141 |
} |
||
142 |
|||
143 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
||
144 |
{ |
||
145 |
return {false}; |
||
146 |
} |
||
147 |
|||
148 |
using Base::isEqual; |
||
149 |
14 |
bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const |
|
150 |
{ |
||
151 |
✓✓✓✗ |
14 |
return Base::isEqual(other) && axis == other.axis; |
152 |
} |
||
153 |
|||
154 |
template<typename ConfigVector> |
||
155 |
19 |
void calc(JointDataDerived & data, |
|
156 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
157 |
{ |
||
158 |
typedef typename ConfigVector::Scalar OtherScalar; |
||
159 |
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type |
||
160 |
✓✗✓✗ |
19 |
& q = qs.template segment<NQ>(idx_q()); |
161 |
|||
162 |
✓✗ | 19 |
const OtherScalar & ca = q(0); |
163 |
✓✗ | 19 |
const OtherScalar & sa = q(1); |
164 |
|||
165 |
✓✗✓✗ |
19 |
toRotationMatrix(axis,ca,sa,data.M.rotation()); |
166 |
19 |
} |
|
167 |
|||
168 |
template<typename ConfigVector, typename TangentVector> |
||
169 |
14 |
void calc(JointDataDerived & data, |
|
170 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
171 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
172 |
{ |
||
173 |
14 |
calc(data,qs.derived()); |
|
174 |
14 |
data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]); |
|
175 |
14 |
} |
|
176 |
|||
177 |
template<typename Matrix6Like> |
||
178 |
6 |
void calc_aba(JointDataDerived & data, |
|
179 |
const Eigen::MatrixBase<Matrix6Like> & I, |
||
180 |
const bool update_I) const |
||
181 |
{ |
||
182 |
✓✗✓✗ ✓✗ |
6 |
data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis; |
183 |
✓✗✓✗ |
6 |
data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR)); |
184 |
✓✗✓✗ |
6 |
data.UDinv.noalias() = data.U * data.Dinv; |
185 |
|||
186 |
✗✓ | 6 |
if (update_I) |
187 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose(); |
||
188 |
6 |
} |
|
189 |
|||
190 |
✓✗ | 64 |
static std::string classname() { return std::string("JointModelRevoluteUnboundedUnaligned"); } |
191 |
22 |
std::string shortname() const { return classname(); } |
|
192 |
|||
193 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
194 |
template<typename NewScalar> |
||
195 |
4 |
JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> cast() const |
|
196 |
{ |
||
197 |
typedef JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> ReturnType; |
||
198 |
4 |
ReturnType res(axis.template cast<NewScalar>()); |
|
199 |
4 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
200 |
4 |
return res; |
|
201 |
} |
||
202 |
|||
203 |
// data |
||
204 |
/// |
||
205 |
/// \brief 3d main axis of the joint. |
||
206 |
/// |
||
207 |
Vector3 axis; |
||
208 |
}; // struct JointModelRevoluteUnboundedUnalignedTpl |
||
209 |
|||
210 |
} //namespace pinocchio |
||
211 |
|||
212 |
#include <boost/type_traits.hpp> |
||
213 |
|||
214 |
namespace boost |
||
215 |
{ |
||
216 |
template<typename Scalar, int Options> |
||
217 |
struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
218 |
: public integral_constant<bool,true> {}; |
||
219 |
|||
220 |
template<typename Scalar, int Options> |
||
221 |
struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
222 |
: public integral_constant<bool,true> {}; |
||
223 |
|||
224 |
template<typename Scalar, int Options> |
||
225 |
struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
226 |
: public integral_constant<bool,true> {}; |
||
227 |
|||
228 |
template<typename Scalar, int Options> |
||
229 |
struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
230 |
: public integral_constant<bool,true> {}; |
||
231 |
} |
||
232 |
|||
233 |
|||
234 |
#endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__ |
Generated by: GCOVR (Version 4.2) |