Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
pinocchio
Commits
8eace9aa
Verified
Commit
8eace9aa
authored
Aug 07, 2018
by
Justin Carpentier
Committed by
Justin Carpentier
Oct 29, 2018
Browse files
[Joint/JointTpl] Improve templatization of Joint{Model,Data}Tpl
parent
29f48d44
Changes
9
Expand all
Hide whitespace changes
Inline
Side-by-side
src/multibody/data.hpp
View file @
8eace9aa
...
...
@@ -41,10 +41,10 @@ namespace se3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum
{
Options
=
JointCollection
::
Options
};
typedef
typename
JointCollection
::
Scalar
Scalar
;
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
typename
JointCollection
::
Scalar
Scalar
;
typedef
SE3Tpl
<
Scalar
,
Options
>
SE3
;
typedef
MotionTpl
<
Scalar
,
Options
>
Motion
;
typedef
ForceTpl
<
Scalar
,
Options
>
Force
;
...
...
@@ -57,8 +57,8 @@ namespace se3
typedef
se3
::
FrameIndex
FrameIndex
;
typedef
std
::
vector
<
Index
>
IndexVector
;
typedef
JointModelTpl
<
JointCollection
>
JointModel
;
typedef
JointDataTpl
<
JointCollection
>
JointData
;
typedef
JointModelTpl
<
Scalar
,
Options
,
JointCollection
DefaultTpl
>
JointModel
;
typedef
JointDataTpl
<
Scalar
,
Options
,
JointCollection
DefaultTpl
>
JointData
;
typedef
container
::
aligned_vector
<
JointModel
>
JointModelVector
;
typedef
container
::
aligned_vector
<
JointData
>
JointDataVector
;
...
...
src/multibody/data.hxx
View file @
8eace9aa
...
...
@@ -107,7 +107,7 @@ namespace se3
/* Create data strcture associated to the joints */
for
(
JointIndex
i
=
0
;
i
<
(
JointIndex
)(
model
.
njoints
);
++
i
)
joints
.
push_back
(
CreateJointData
<
JointCollection
>::
run
(
model
.
joints
[
i
]));
joints
.
push_back
(
CreateJointData
<
Scalar
,
Options
,
JointCollection
DefaultTpl
>::
run
(
model
.
joints
[
i
]));
/* Init for CRBA */
M
.
setZero
();
Minv
.
setZero
();
...
...
src/multibody/joint/fwd.hpp
View file @
8eace9aa
...
...
@@ -86,11 +86,13 @@ namespace se3
struct
JointDataCompositeTpl
;
typedef
JointDataCompositeTpl
<
double
>
JointDataComposite
;
template
<
typename
JointCollection
>
struct
JointModelTpl
;
typedef
JointModelTpl
<
JointCollectionDefault
>
JointModel
;
template
<
typename
Scalar
,
int
Options
=
0
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
=
JointCollectionDefaultTpl
>
struct
JointModelTpl
;
typedef
JointModelTpl
<
double
>
JointModel
;
template
<
typename
JointCollection
>
struct
JointDataTpl
;
typedef
JointDataTpl
<
JointCollectionDefault
>
JointData
;
template
<
typename
Scalar
,
int
Options
=
0
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
=
JointCollectionDefaultTpl
>
struct
JointDataTpl
;
typedef
JointDataTpl
<
double
>
JointData
;
}
...
...
src/multibody/joint/joint-basic-visitors.hpp
View file @
8eace9aa
...
...
@@ -32,9 +32,9 @@ namespace se3
*
* @return The created JointDataTpl
*/
template
<
typename
JointCollection
>
inline
JointDataTpl
<
JointCollection
>
createData
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
createData
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
/**
...
...
@@ -48,9 +48,9 @@ namespace se3
* @param jdata The JointDataVariant we want to update
* @param[in] q The full model's (in which the joint belongs to) configuration vector
*/
template
<
typename
JointCollection
,
typename
ConfigVectorType
>
inline
void
calc_zero_order
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
,
JointDataTpl
<
JointCollection
>
&
jdata
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
>
inline
void
calc_zero_order
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
,
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
);
/**
...
...
@@ -65,9 +65,9 @@ namespace se3
* @param jdata The JointDataVariant we want to update
* @param[in] q The full model's (in which the joint belongs to) configuration vector
*/
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType
>
inline
void
calc_first_order
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
,
JointDataTpl
<
JointCollection
>
&
jdata
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType
>
inline
void
calc_first_order
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
,
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
);
...
...
@@ -83,9 +83,9 @@ namespace se3
* @param[inout] I Inertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix
* @param[in] update_I If I should be updated or not
*/
template
<
typename
JointCollection
,
typename
Matrix6Type
>
inline
void
calc_aba
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
,
JointDataTpl
<
JointCollection
>
&
jdata
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
,
typename
Matrix6Type
>
inline
void
calc_aba
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
,
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
,
const
Eigen
::
MatrixBase
<
Matrix6Type
>
&
I
,
const
bool
update_I
);
...
...
@@ -96,9 +96,9 @@ namespace se3
///
/// \returns The finite diffrence increment.
///
template
<
typename
JointCollection
>
inline
typename
JointCollection
::
Scalar
finiteDifferenceIncrement
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
Scalar
finiteDifferenceIncrement
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
/**
* @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
...
...
@@ -108,8 +108,8 @@ namespace se3
*
* @return The dimension of joint tangent space
*/
template
<
typename
JointCollection
>
inline
int
nv
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
int
nv
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
...
...
@@ -121,8 +121,8 @@ namespace se3
*
* @return The dimension of joint configuration space
*/
template
<
typename
JointCollection
>
inline
int
nq
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
int
nq
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
/**
...
...
@@ -134,8 +134,8 @@ namespace se3
* @return The index in the full model configuration space corresponding to the first
* degree of freedom of jmodel
*/
template
<
typename
JointCollection
>
inline
int
idx_q
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
int
idx_q
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
/**
...
...
@@ -147,8 +147,8 @@ namespace se3
* @return The index in the full model tangent space corresponding to the first
* joint tangent space degree
*/
template
<
typename
JointCollection
>
inline
int
idx_v
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
int
idx_v
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
/**
...
...
@@ -158,8 +158,8 @@ namespace se3
*
* @return The index of the joint in the kinematic chain
*/
template
<
typename
JointCollection
>
inline
JointIndex
id
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
JointIndex
id
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
/**
* @brief Visit a JointModelVariant through JointSetIndexesVisitor to set
...
...
@@ -172,8 +172,8 @@ namespace se3
*
* @return The index of the joint in the kinematic chain
*/
template
<
typename
JointCollection
>
inline
void
setIndexes
(
JointModelTpl
<
JointCollection
>
&
jmodel
,
JointIndex
id
,
int
q
,
int
v
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
void
setIndexes
(
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
,
JointIndex
id
,
int
q
,
int
v
);
/**
...
...
@@ -181,8 +181,8 @@ namespace se3
*
* @param jmodel The JointModelVariant we want the shortname of the type held in
*/
template
<
typename
JointCollection
>
inline
std
::
string
shortname
(
const
JointModelTpl
<
JointCollection
>
&
jmodel
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
std
::
string
shortname
(
const
JointModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jmodel
);
//
...
...
@@ -198,9 +198,9 @@ namespace se3
*
* @return The constraint dense corresponding to the joint derived constraint
*/
template
<
typename
JointCollection
>
inline
ConstraintTpl
<
Eigen
::
Dynamic
,
typename
JointCollection
::
Scalar
,
JointCollection
::
Options
>
constraint_xd
(
const
JointDataTpl
<
JointCollection
>
&
jdata
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
ConstraintTpl
<
Eigen
::
Dynamic
,
Scalar
,
Options
>
constraint_xd
(
const
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
);
/**
* @brief Visit a JointDataVariant through JointTransformVisitor to get the joint internal transform (transform
...
...
@@ -210,9 +210,9 @@ namespace se3
*
* @return The joint transform corresponding to the joint derived transform (sXp)
*/
template
<
typename
JointCollection
>
inline
SE3Tpl
<
typename
JointCollection
::
Scalar
,
JointCollection
::
Options
>
joint_transform
(
const
JointDataTpl
<
JointCollection
>
&
jdata
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
SE3Tpl
<
Scalar
,
Options
>
joint_transform
(
const
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
);
/**
* @brief Visit a JointDataVariant through JointMotionVisitor to get the joint internal motion
...
...
@@ -222,9 +222,9 @@ namespace se3
*
* @return The motion dense corresponding to the joint derived motion
*/
template
<
typename
JointCollection
>
inline
MotionTpl
<
typename
JointCollection
::
Scalar
,
JointCollection
::
Options
>
motion
(
const
JointDataTpl
<
JointCollection
>
&
jdata
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
MotionTpl
<
Scalar
,
Options
>
motion
(
const
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
);
/**
* @brief Visit a JointDataVariant through JointBiasVisitor to get the joint bias
...
...
@@ -234,9 +234,9 @@ namespace se3
*
* @return The motion dense corresponding to the joint derived bias
*/
template
<
typename
JointCollection
>
inline
MotionTpl
<
typename
JointCollection
::
Scalar
,
JointCollection
::
Options
>
bias
(
const
JointDataTpl
<
JointCollection
>
&
jdata
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
MotionTpl
<
Scalar
,
Options
>
bias
(
const
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
);
/**
* @brief Visit a JointDataVariant through JointUInertiaVisitor to get the U matrix of the inertia matrix
...
...
@@ -246,9 +246,9 @@ namespace se3
*
* @return The U matrix of the inertia matrix decomposition
*/
template
<
typename
JointCollection
>
inline
Eigen
::
Matrix
<
typename
JointCollection
::
Scalar
,
6
,
Eigen
::
Dynamic
,
JointCollection
::
Options
>
u_inertia
(
const
JointDataTpl
<
JointCollection
>
&
jdata
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
Eigen
::
Matrix
<
Scalar
,
6
,
Eigen
::
Dynamic
,
Options
>
u_inertia
(
const
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
);
/**
* @brief Visit a JointDataVariant through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix
...
...
@@ -258,9 +258,9 @@ namespace se3
*
* @return The D^{-1} matrix of the inertia matrix decomposition
*/
template
<
typename
JointCollection
>
inline
Eigen
::
Matrix
<
typename
JointCollection
::
Scalar
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
JointCollection
::
Options
>
dinv_inertia
(
const
JointDataTpl
<
JointCollection
>
&
jdata
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
Eigen
::
Matrix
<
Scalar
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Options
>
dinv_inertia
(
const
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
);
/**
* @brief Visit a JointDataVariant through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix
...
...
@@ -270,9 +270,9 @@ namespace se3
*
* @return The U*D^{-1} matrix of the inertia matrix decomposition
*/
template
<
typename
JointCollection
>
inline
Eigen
::
Matrix
<
typename
JointCollection
::
Scalar
,
6
,
Eigen
::
Dynamic
,
JointCollection
::
Options
>
udinv_inertia
(
const
JointDataTpl
<
JointCollection
>
&
jdata
);
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
inline
Eigen
::
Matrix
<
Scalar
,
6
,
Eigen
::
Dynamic
,
Options
>
udinv_inertia
(
const
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
jdata
);
}
// namespace se3
...
...
src/multibody/joint/joint-basic-visitors.hxx
View file @
8eace9aa
This diff is collapsed.
Click to expand it.
src/multibody/joint/joint-composite.hpp
View file @
8eace9aa
...
...
@@ -67,29 +67,21 @@ namespace se3
struct
traits
<
JointDataCompositeTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
>
{
typedef
JointCompositeTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
JointDerived
;
};
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
>
template
<
typename
_
Scalar
,
int
_
Options
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
>
struct
JointDataCompositeTpl
:
public
JointDataBase
<
JointDataCompositeTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
>
:
public
JointDataBase
<
JointDataCompositeTpl
<
_
Scalar
,
_
Options
,
JointCollectionTpl
>
>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
JointDataBase
<
JointDataCompositeTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
>
Base
;
typedef
JointCollectionTpl
<
Scalar
,
Options
>
JointCollection
;
typedef
JointDataTpl
<
JointCollection
>
JointDataVariant
;
// typedef typename JointCollection::JointDataVariant JointDataVariant;
typedef
JointDataBase
<
JointDataCompositeTpl
>
Base
;
typedef
JointCompositeTpl
<
_Scalar
,
_Options
,
JointCollectionTpl
>
JointDerived
;
SE3_JOINT_TYPEDEF_TEMPLATE
;
typedef
JointCollectionTpl
<
Scalar
,
Options
>
JointCollection
;
typedef
JointDataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
JointDataVariant
;
typedef
container
::
aligned_vector
<
JointDataVariant
>
JointDataVector
;
// typedef boost::array<JointDataVariant,njoints> JointDataVector;
typedef
typename
Base
::
Transformation_t
Transformation_t
;
typedef
typename
Base
::
Motion_t
Motion_t
;
typedef
typename
Base
::
Bias_t
Bias_t
;
typedef
typename
Base
::
Constraint_t
Constraint_t
;
typedef
typename
Base
::
U_t
U_t
;
typedef
typename
Base
::
D_t
D_t
;
typedef
typename
Base
::
UD_t
UD_t
;
// JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
JointDataCompositeTpl
(
const
JointDataVector
&
joint_data
,
const
int
/*nq*/
,
const
int
nv
)
...
...
@@ -126,30 +118,16 @@ namespace se3
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
typename
traits
<
JointModelCompositeTpl
>::
JointDerived
JointType
;
typedef
typename
traits
<
JointType
>::
JointDataDerived
JointData
;
typedef
typename
traits
<
JointType
>::
Scalar
Scalar
;
typedef
JointModelBase
<
JointModelCompositeTpl
>
Base
;
enum
{
Options
=
traits
<
JointType
>::
Options
,
NV
=
traits
<
JointType
>::
NV
,
NQ
=
traits
<
JointType
>::
NQ
};
typedef
JointCompositeTpl
<
_Scalar
,
_Options
,
JointCollectionTpl
>
JointDerived
;
SE3_JOINT_TYPEDEF_TEMPLATE
;
typedef
JointCollectionTpl
<
Scalar
,
Options
>
JointCollection
;
typedef
JointModelTpl
<
JointCollection
>
JointModelVariant
;
// typedef typename JointCollection::JointModelVariant JointModelVariant;
typedef
JointModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
JointModelVariant
;
typedef
SE3Tpl
<
Scalar
,
Options
>
SE3
;
typedef
container
::
aligned_vector
<
JointModelVariant
>
JointModelVector
;
// typedef boost::array<JointModelVariant,njoints> JointModelVector;
typedef
typename
traits
<
JointType
>::
Transformation_t
Transformation_t
;
typedef
typename
traits
<
JointType
>::
Constraint_t
Constraint_t
;
typedef
typename
traits
<
JointType
>::
ConfigVector_t
ConfigVector_t
;
typedef
typename
traits
<
JointType
>::
TangentVector_t
TangentVector_t
;
using
Base
::
id
;
using
Base
::
idx_q
;
...
...
@@ -216,29 +194,29 @@ namespace se3
updateJointIndexes
();
}
JointData
createData
()
const
JointData
Derived
createData
()
const
{
typename
JointData
::
JointDataVector
jdata
(
joints
.
size
());
typename
JointData
Derived
::
JointDataVector
jdata
(
joints
.
size
());
for
(
int
i
=
0
;
i
<
(
int
)
joints
.
size
();
++
i
)
jdata
[(
size_t
)
i
]
=
::
se3
::
createData
<
JointCollection
>
(
joints
[(
size_t
)
i
]);
return
JointData
(
jdata
,
nq
(),
nv
());
jdata
[(
size_t
)
i
]
=
::
se3
::
createData
<
Scalar
,
Options
,
JointCollection
Tpl
>
(
joints
[(
size_t
)
i
]);
return
JointData
Derived
(
jdata
,
nq
(),
nv
());
}
template
<
typename
,
int
,
template
<
typename
S
,
int
O
>
class
,
typename
>
friend
struct
JointCompositeCalcZeroOrderStep
;
template
<
typename
ConfigVectorType
>
void
calc
(
JointData
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
)
const
;
void
calc
(
JointData
Derived
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
)
const
;
template
<
typename
,
int
,
template
<
typename
S
,
int
O
>
class
,
typename
,
typename
>
friend
struct
JointCompositeCalcFirstOrderStep
;
template
<
typename
ConfigVectorType
,
typename
TangentVectorType
>
void
calc
(
JointData
&
data
,
void
calc
(
JointData
Derived
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
,
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
vs
)
const
;
void
calc_aba
(
JointData
&
data
,
Inertia
::
Matrix6
&
I
,
const
bool
update_I
)
const
void
calc_aba
(
JointData
Derived
&
data
,
Inertia
::
Matrix6
&
I
,
const
bool
update_I
)
const
{
data
.
U
.
noalias
()
=
I
*
data
.
S
;
Eigen
::
Matrix
<
Scalar
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Options
>
tmp
(
data
.
S
.
matrix
().
transpose
()
*
data
.
U
);
...
...
src/multibody/joint/joint-composite.hxx
View file @
8eace9aa
...
...
@@ -69,7 +69,7 @@ namespace se3
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
>
template
<
typename
ConfigVectorType
>
inline
void
JointModelCompositeTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
calc
(
JointData
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
)
const
calc
(
JointData
Derived
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
)
const
{
assert
(
joints
.
size
()
>
0
);
assert
(
data
.
joints
.
size
()
==
joints
.
size
());
...
...
@@ -142,7 +142,7 @@ namespace se3
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
>
template
<
typename
ConfigVectorType
,
typename
TangentVectorType
>
inline
void
JointModelCompositeTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
::
calc
(
JointData
&
jdata
,
::
calc
(
JointData
Derived
&
jdata
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
,
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
vs
)
const
{
...
...
src/multibody/joint/joint.hpp
View file @
8eace9aa
...
...
@@ -29,20 +29,24 @@
namespace
se3
{
template
<
typename
JointCollection
>
struct
JointTpl
;
typedef
JointTpl
<
JointCollectionDefault
>
Joint
;
template
<
typename
Scalar
,
int
Options
=
0
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
=
JointCollectionDefaultTpl
>
struct
JointTpl
;
typedef
JointTpl
<
double
>
Joint
;
template
<
typename
JointCollection
>
struct
traits
<
JointTpl
<
JointCollection
>
>
template
<
typename
_Scalar
,
int
_Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
struct
traits
<
JointTpl
<
_Scalar
,
_Options
,
JointCollection
Tpl
>
>
{
enum
{
Options
=
JointCollection
::
Options
,
Options
=
_
Options
,
NQ
=
Eigen
::
Dynamic
,
// Dynamic because unknown at compile time
NV
=
Eigen
::
Dynamic
};
typedef
typename
JointCollection
::
Scalar
Scalar
;
typedef
JointDataTpl
<
JointCollection
>
JointDataDerived
;
typedef
JointModelTpl
<
JointCollection
>
JointModelDerived
;
typedef
_Scalar
Scalar
;
typedef
JointCollectionTpl
<
Scalar
,
Options
>
JointCollection
;
typedef
JointDataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
JointDataDerived
;
typedef
JointModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
JointModelDerived
;
typedef
ConstraintTpl
<
Eigen
::
Dynamic
,
Scalar
,
Options
>
Constraint_t
;
typedef
SE3Tpl
<
Scalar
,
Options
>
Transformation_t
;
typedef
MotionTpl
<
Scalar
,
Options
>
Motion_t
;
...
...
@@ -58,23 +62,29 @@ namespace se3
typedef
Eigen
::
Matrix
<
Scalar
,
Eigen
::
Dynamic
,
1
,
Options
>
TangentVector_t
;
};
template
<
typename
JointCollection
>
struct
traits
<
JointDataTpl
<
JointCollection
>
>
{
typedef
JointTpl
<
JointCollection
>
JointDerived
;
};
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollection
Tpl
>
struct
traits
<
JointDataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
>
{
typedef
JointTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
JointDerived
;
};
template
<
typename
JointCollection
>
struct
traits
<
JointModelTpl
<
JointCollection
>
>
{
typedef
JointTpl
<
JointCollection
>
JointDerived
;
};
template
<
typename
JointCollection
>
struct
JointDataTpl
:
public
JointDataBase
<
JointDataTpl
<
JointCollection
>
>
,
JointCollection
::
JointDataVariant
template
<
typename
Scalar
,
int
Options
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
>
struct
traits
<
JointModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
>
{
typedef
JointTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
JointDerived
;
};
template
<
typename
_Scalar
,
int
_Options
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
>
struct
JointDataTpl
:
public
JointDataBase
<
JointDataTpl
<
_Scalar
,
_Options
,
JointCollectionTpl
>
>
,
JointCollectionTpl
<
_Scalar
,
_Options
>::
JointDataVariant
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
JointTpl
<
JointCollection
>
JointDerived
;
typedef
typename
JointCollection
::
JointDataVariant
JointDataVariant
;
typedef
JointTpl
<
_Scalar
,
_Options
,
JointCollectionTpl
>
JointDerived
;
typedef
JointDataBase
<
JointDataTpl
>
Base
;
SE3_JOINT_TYPEDEF_TEMPLATE
;
typedef
JointCollectionTpl
<
_Scalar
,
_Options
>
JointCollection
;
typedef
typename
JointCollection
::
JointDataVariant
JointDataVariant
;
JointDataVariant
&
toVariant
()
{
return
*
static_cast
<
JointDataVariant
*>
(
this
);
}
const
JointDataVariant
&
toVariant
()
const
{
return
*
static_cast
<
const
JointDataVariant
*>
(
this
);
}
...
...
@@ -104,19 +114,22 @@ namespace se3
};
template
<
typename
JointCollection
>
struct
JointModelTpl
:
JointModelBase
<
JointModelTpl
<
JointCollection
>
>
,
JointCollection
::
JointModelVariant
template
<
typename
_Scalar
,
int
_Options
,
template
<
typename
S
,
int
O
>
class
JointCollectionTpl
>
struct
JointModelTpl
:
JointModelBase
<
JointModelTpl
<
_Scalar
,
_Options
,
JointCollectionTpl
>
>
,
JointCollectionTpl
<
_Scalar
,
_Options
>::
JointModelVariant
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
JointTpl
<
_Scalar
,
_Options
,
JointCollectionTpl
>
JointDerived
;
SE3_JOINT_TYPEDEF_TEMPLATE
;
SE3_JOINT_USE_INDEXES
;
typedef
JointCollectionTpl
<
Scalar
,
Options
>
JointCollection
;
typedef
typename
JointCollection
::
JointDataVariant
JointDataVariant
;
typedef
typename
JointCollection
::
JointModelVariant
JointModelVariant
;
typedef
JointModelVariant
JointModelBoostVariant
;
typedef
JointTpl
<
JointCollection
>
JointDerived
;
SE3_JOINT_TYPEDEF_TEMPLATE
;
SE3_JOINT_USE_INDEXES
;
using
Base
::
id
;
using
Base
::
setIndexes
;
using
Base
::
operator
==
;
...
...
@@ -129,7 +142,7 @@ namespace se3
template
<
typename
JointModelDerived
>
JointModelTpl
(
const
JointModelBase
<
JointModelDerived
>
&
jmodel
)
:
JointCollection
::
JointModelVariant
((
JointModelVariant
)
jmodel
.
derived
())
:
JointModelVariant
((
JointModelVariant
)
jmodel
.
derived
())
{
BOOST_MPL_ASSERT
((
boost
::
mpl
::
contains
<
typename
JointModelVariant
::
types
,
JointModelDerived
>
));
}
...
...
src/multibody/model.hpp
View file @
8eace9aa
...
...
@@ -43,10 +43,10 @@ namespace se3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum
{
Options
=
JointCollection
::
Options
};
typedef
typename
JointCollection
::
Scalar
Scalar
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
typename
JointCollection
::
Scalar
Scalar
;
typedef
SE3Tpl
<
Scalar
,
Options
>
SE3
;
typedef
MotionTpl
<
Scalar
,
Options
>
Motion
;
typedef
ForceTpl
<
Scalar
,
Options
>
Force
;
...
...
@@ -59,8 +59,8 @@ namespace se3
typedef
se3
::
FrameIndex
FrameIndex
;
typedef
std
::
vector
<
Index
>
IndexVector
;
typedef
JointModelTpl
<
JointCollection
>
JointModel
;
typedef
JointDataTpl
<
JointCollection
>
JointData
;
typedef
JointModelTpl
<
Scalar
,
Options
,
JointCollection
DefaultTpl
>
JointModel
;
typedef
JointDataTpl
<
Scalar
,
Options
,
JointCollection
DefaultTpl
>
JointData
;
typedef
container
::
aligned_vector
<
JointModel
>
JointModelVector
;
typedef
container
::
aligned_vector
<
JointData
>
JointDataVector
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment