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
b20f0039
Verified
Commit
b20f0039
authored
Aug 07, 2018
by
Justin Carpentier
Committed by
Justin Carpentier
Oct 29, 2018
Browse files
[All] Improve templatization of Model and Data struct
parent
cd0b8c5a
Changes
54
Hide whitespace changes
Inline
Side-by-side
bindings/python/algorithm/expose-aba.cpp
View file @
b20f0039
...
...
@@ -37,7 +37,7 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"aba"
,
&
aba
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
VectorXd
>
,
&
aba
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity v (size Model::nv)"
,
...
...
@@ -46,7 +46,7 @@ namespace se3
bp
::
return_value_policy
<
bp
::
return_by_value
>
());
bp
::
def
(
"aba"
,
&
aba
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
VectorXd
,
Force
>
,
&
aba
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
VectorXd
,
Force
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity v (size Model::nv)"
,
...
...
bindings/python/algorithm/expose-centroidal.cpp
View file @
b20f0039
...
...
@@ -28,7 +28,7 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"ccrba"
,
&
ccrba
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
ccrba
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity v (size Model::nv)"
),
...
...
@@ -36,7 +36,7 @@ namespace se3
bp
::
return_value_policy
<
bp
::
return_by_value
>
());
bp
::
def
(
"dccrba"
,
dccrba
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
dccrba
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity v (size Model::nv)"
),
...
...
bindings/python/algorithm/expose-com.cpp
View file @
b20f0039
...
...
@@ -78,7 +78,7 @@ namespace se3
"Computes the center of mass position, velocity and acceleration by storing the result in Data"
"and returns the center of mass position of the full model expressed in the world frame."
);
bp
::
def
(
"jacobianCenterOfMass"
,
&
jacobianCenterOfMass
<
JointCollectionDefault
,
VectorXd
>
,
bp
::
def
(
"jacobianCenterOfMass"
,
&
jacobianCenterOfMass
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
jacobianCenterOfMass_overload
(
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
),
"Computes the jacobian of the center of mass, puts the result in Data and return it."
)[
...
...
bindings/python/algorithm/expose-dynamics.cpp
View file @
b20f0039
...
...
@@ -28,7 +28,7 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"forwardDynamics"
,
&
forwardDynamics
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
VectorXd
,
MatrixXd
,
VectorXd
>
,
&
forwardDynamics
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
VectorXd
,
MatrixXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity v (size Model::nv)"
,
...
...
@@ -41,7 +41,7 @@ namespace se3
bp
::
return_value_policy
<
bp
::
return_by_value
>
());
bp
::
def
(
"impactDynamics"
,
&
impulseDynamics
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
MatrixXd
>
,
&
impulseDynamics
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
MatrixXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity before impact v_before (size Model::nv)"
,
...
...
bindings/python/algorithm/expose-energy.cpp
View file @
b20f0039
...
...
@@ -28,7 +28,7 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"kineticEnergy"
,
&
kineticEnergy
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
kineticEnergy
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity v (size Model::nv)"
,
...
...
@@ -38,7 +38,7 @@ namespace se3
"in data.kinetic_energy. By default, the first order kinematic quantities of the model are updated."
);
bp
::
def
(
"potentialEnergy"
,
&
potentialEnergy
<
JointCollectionDefault
,
VectorXd
>
,
&
potentialEnergy
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Update kinematics (bool)"
),
...
...
bindings/python/algorithm/expose-geometry.cpp
View file @
b20f0039
...
...
@@ -28,14 +28,14 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"updateGeometryPlacements"
,
&
updateGeometryPlacements
<
JointCollectionDefault
,
VectorXd
>
,
&
updateGeometryPlacements
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
bp
::
args
(
"model"
,
"data"
,
"geometry model"
,
"geometry data"
,
"Configuration vector q (size Model::nq)"
),
"Update the placement of the collision objects according to the current configuration."
"The algorithm also updates the current placement of the joint in Data."
);
bp
::
def
(
"updateGeometryPlacements"
,
&
updateGeometryPlacements
<
JointCollectionDefault
>
,
&
updateGeometryPlacements
<
double
,
0
,
JointCollectionDefault
Tpl
>
,
bp
::
args
(
"model"
,
"data"
,
"geometry model"
,
"geometry data"
),
"Update the placement of the collision objects according to the current joint placement stored in data."
);
...
...
@@ -54,7 +54,7 @@ namespace se3
);
bp
::
def
(
"computeCollisions"
,
&
computeCollisions
<
JointCollectionDefault
,
VectorXd
>
,
&
computeCollisions
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
bp
::
args
(
"model"
,
"data"
,
"geometry model"
,
"geometry data"
,
"Configuration q (size Model::nq)"
,
"bool"
),
"Update the geometry for a given configuration and"
"determine if all collision pairs are effectively in collision or not."
...
...
@@ -74,7 +74,7 @@ namespace se3
);
bp
::
def
(
"computeDistances"
,
&
computeDistances
<
JointCollectionDefault
,
VectorXd
>
,
&
computeDistances
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
bp
::
args
(
"model"
,
"data"
,
"geometry model"
,
"geometry data"
,
"Configuration q (size Model::nq)"
),
"Update the geometry for a given configuration and"
"compute the distance between each collision pair"
...
...
bindings/python/algorithm/expose-joints.cpp
View file @
b20f0039
...
...
@@ -41,14 +41,14 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"integrate"
,
&
integrate
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
integrate
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
),
"Integrate the model for a tangent vector during one unit time ."
);
bp
::
def
(
"interpolate"
,
&
interpolate
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
double
>
,
&
interpolate
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Configuration q1 (size Model::nq)"
,
"Configuration q2 (size Model::nq)"
,
...
...
@@ -56,7 +56,7 @@ namespace se3
"Interpolate the model between two configurations."
);
bp
::
def
(
"difference"
,
&
difference
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
difference
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Configuration q1 (size Model::nq)"
,
"Configuration q2 (size Model::nq)"
),
...
...
@@ -64,14 +64,14 @@ namespace se3
"to go from q1 to q2"
);
bp
::
def
(
"squaredDistance"
,
&
squaredDistance
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
squaredDistance
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Configuration q1 (size Model::nq)"
,
"Configuration q2 (size Model::nq)"
),
"Squared distance vector between two configurations."
);
bp
::
def
(
"distance"
,
&
distance
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
distance
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Configuration q1 (size Model::nq)"
,
"Configuration q2 (size Model::nq)"
),
...
...
@@ -83,7 +83,7 @@ namespace se3
"Generate a random configuration in the bounds given by the lower and upper limits contained in model."
);
bp
::
def
(
"randomConfiguration"
,
&
randomConfiguration
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
randomConfiguration
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Joint lower limits (size Model::nq)"
,
"Joint upper limits (size Model::nq)"
),
...
...
@@ -95,7 +95,7 @@ namespace se3
"return the configuration normalized "
);
bp
::
def
(
"isSameConfiguration"
,
&
isSameConfiguration
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
isSameConfiguration
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Configuration q1 (size Model::nq)"
,
"Configuration q2 (size Model::nq)"
,
...
...
bindings/python/algorithm/expose-kinematics-derivatives.cpp
View file @
b20f0039
...
...
@@ -66,7 +66,7 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"computeForwardKinematicsDerivatives"
,
&
computeForwardKinematicsDerivatives
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
VectorXd
>
,
&
computeForwardKinematicsDerivatives
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
,
...
...
bindings/python/algorithm/expose-kinematics.cpp
View file @
b20f0039
...
...
@@ -27,20 +27,20 @@ namespace se3
{
using
namespace
Eigen
;
bp
::
def
(
"updateGlobalPlacements"
,
&
updateGlobalPlacements
<
JointCollectionDefault
>
,
&
updateGlobalPlacements
<
double
,
0
,
JointCollectionDefault
Tpl
>
,
bp
::
args
(
"Model"
,
"Data"
),
"Updates the global placements of all the frames of the kinematic "
"tree and put the results in data according to the relative placements of the joints."
);
bp
::
def
(
"forwardKinematics"
,
&
forwardKinematics
<
JointCollectionDefault
,
VectorXd
>
,
&
forwardKinematics
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
),
"Compute the global placements of all the joints of the kinematic "
"tree and put the results in data."
);
bp
::
def
(
"forwardKinematics"
,
&
forwardKinematics
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
forwardKinematics
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
),
...
...
@@ -48,7 +48,7 @@ namespace se3
"tree and put the results in data."
);
bp
::
def
(
"forwardKinematics"
,
&
forwardKinematics
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
VectorXd
>
,
&
forwardKinematics
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
,
...
...
bindings/python/algorithm/expose-regressor.cpp
View file @
b20f0039
...
...
@@ -28,7 +28,7 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"computeStaticRegressor"
,
&
regressor
::
computeStaticRegressor
<
JointCollectionDefault
,
VectorXd
>
,
&
regressor
::
computeStaticRegressor
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
),
"Compute the static regressor that links the inertia parameters of the system to its center of mass position
\n
"
...
...
bindings/python/algorithm/expose-rnea.cpp
View file @
b20f0039
...
...
@@ -28,7 +28,7 @@ namespace se3
using
namespace
Eigen
;
bp
::
def
(
"rnea"
,
&
rnea
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
VectorXd
>
,
&
rnea
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
,
...
...
@@ -37,7 +37,7 @@ namespace se3
bp
::
return_value_policy
<
bp
::
return_by_value
>
());
bp
::
def
(
"rnea"
,
&
rnea
<
JointCollectionDefault
,
VectorXd
,
VectorXd
,
VectorXd
,
Force
>
,
&
rnea
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
,
VectorXd
,
Force
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
,
...
...
@@ -48,7 +48,7 @@ namespace se3
bp
::
def
(
"nle"
,
&
nonLinearEffects
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
nonLinearEffects
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
),
...
...
@@ -57,7 +57,7 @@ namespace se3
bp
::
def
(
"computeGeneralizedGravity"
,
&
computeGeneralizedGravity
<
JointCollectionDefault
,
VectorXd
>
,
&
computeGeneralizedGravity
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
),
"Computes the generalized gravity contribution g(q) of the Lagrangian dynamics."
,
...
...
@@ -65,7 +65,7 @@ namespace se3
bp
::
def
(
"computeCoriolisMatrix"
,
&
computeCoriolisMatrix
<
JointCollectionDefault
,
VectorXd
,
VectorXd
>
,
&
computeCoriolisMatrix
<
double
,
0
,
JointCollectionDefault
Tpl
,
VectorXd
,
VectorXd
>
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
"Velocity v (size Model::nv)"
),
...
...
src/algorithm/aba-derivatives.hpp
View file @
b20f0039
...
...
@@ -47,10 +47,10 @@ namespace se3
///
/// \sa se3::aba
///
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
typename
MatrixType1
,
typename
MatrixType2
,
typename
MatrixType3
>
inline
void
computeABADerivatives
(
const
ModelTpl
<
JointCollection
>
&
model
,
DataTpl
<
JointCollection
>
&
data
,
inline
void
computeABADerivatives
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
,
...
...
@@ -78,9 +78,9 @@ namespace se3
///
/// \sa se3::aba and \sa se3::computeABADerivatives.
///
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
>
inline
void
computeABADerivatives
(
const
ModelTpl
<
JointCollection
>
&
model
,
DataTpl
<
JointCollection
>
&
data
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
>
inline
void
computeABADerivatives
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
)
...
...
src/algorithm/aba-derivatives.hxx
View file @
b20f0039
...
...
@@ -25,12 +25,12 @@
namespace
se3
{
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType
>
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType
>
struct
ComputeABADerivativesForwardStep1
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesForwardStep1
<
JointCollection
,
ConfigVectorType
,
TangentVectorType
>
>
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesForwardStep1
<
Scalar
,
Options
,
JointCollection
Tpl
,
ConfigVectorType
,
TangentVectorType
>
>
{
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&
,
...
...
@@ -82,12 +82,12 @@ namespace se3
};
template
<
typename
JointCollection
,
typename
MatrixType
>
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
MatrixType
>
struct
ComputeABADerivativesBackwardStep1
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesBackwardStep1
<
JointCollection
,
MatrixType
>
>
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesBackwardStep1
<
Scalar
,
Options
,
JointCollection
Tpl
,
MatrixType
>
>
{
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&
,
...
...
@@ -148,7 +148,7 @@ namespace se3
{
typename
Data
::
Force
&
pa
=
data
.
f
[
i
];
pa
.
toVector
()
+=
Ia
*
data
.
a
[
i
].
toVector
()
+
jdata
.
UDinv
()
*
jmodel
.
jointVelocitySelector
(
data
.
u
);
data
.
Yaba
[
parent
]
+=
AbaBackwardStep
<
JointCollection
>::
SE3actOn
(
data
.
liMi
[
i
],
Ia
);
data
.
Yaba
[
parent
]
+=
AbaBackwardStep
<
Scalar
,
Options
,
JointCollection
Tpl
>::
SE3actOn
(
data
.
liMi
[
i
],
Ia
);
data
.
f
[
parent
]
+=
data
.
liMi
[
i
].
act
(
pa
);
}
...
...
@@ -156,12 +156,12 @@ namespace se3
};
template
<
typename
JointCollection
,
typename
MatrixType
>
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
MatrixType
>
struct
ComputeABADerivativesForwardStep2
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesForwardStep2
<
JointCollection
,
MatrixType
>
>
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesForwardStep2
<
Scalar
,
Options
,
JointCollection
Tpl
,
MatrixType
>
>
{
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&
,
...
...
@@ -248,12 +248,12 @@ namespace se3
};
template
<
typename
JointCollection
>
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
>
struct
ComputeABADerivativesBackwardStep2
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesBackwardStep2
<
JointCollection
>
>
:
public
fusion
::
JointVisitorBase
<
ComputeABADerivativesBackwardStep2
<
Scalar
,
Options
,
JointCollection
Tpl
>
>
{
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&>
ArgsType
;
...
...
@@ -332,10 +332,10 @@ namespace se3
}
};
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
typename
MatrixType1
,
typename
MatrixType2
,
typename
MatrixType3
>
inline
void
computeABADerivatives
(
const
ModelTpl
<
JointCollection
>
&
model
,
DataTpl
<
JointCollection
>
&
data
,
inline
void
computeABADerivatives
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
,
...
...
@@ -354,7 +354,7 @@ namespace se3
assert
(
aba_partial_dtau
.
rows
()
==
model
.
nv
);
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
typename
ModelTpl
<
JointCollection
>::
JointIndex
JointIndex
;
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>::
JointIndex
JointIndex
;
data
.
a
[
0
]
=
-
model
.
gravity
;
data
.
oa
[
0
]
=
-
model
.
gravity
;
...
...
@@ -363,7 +363,7 @@ namespace se3
MatrixType3
&
Minv_
=
EIGEN_CONST_CAST
(
MatrixType3
,
aba_partial_dtau
);
/// First, compute Minv and a, the joint acceleration vector
typedef
ComputeABADerivativesForwardStep1
<
JointCollection
,
ConfigVectorType
,
TangentVectorType1
>
Pass1
;
typedef
ComputeABADerivativesForwardStep1
<
Scalar
,
Options
,
JointCollection
Tpl
,
ConfigVectorType
,
TangentVectorType1
>
Pass1
;
for
(
JointIndex
i
=
1
;
i
<
(
JointIndex
)
model
.
njoints
;
++
i
)
{
Pass1
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
...
...
@@ -371,21 +371,21 @@ namespace se3
}
data
.
Fcrb
[
0
].
setZero
();
typedef
ComputeABADerivativesBackwardStep1
<
JointCollection
,
MatrixType3
>
Pass2
;
typedef
ComputeABADerivativesBackwardStep1
<
Scalar
,
Options
,
JointCollection
Tpl
,
MatrixType3
>
Pass2
;
for
(
JointIndex
i
=
(
JointIndex
)(
model
.
njoints
-
1
);
i
>
0
;
--
i
)
{
Pass2
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
typename
Pass2
::
ArgsType
(
model
,
data
,
Minv_
));
}
typedef
ComputeABADerivativesForwardStep2
<
JointCollection
,
MatrixType3
>
Pass3
;
typedef
ComputeABADerivativesForwardStep2
<
Scalar
,
Options
,
JointCollection
Tpl
,
MatrixType3
>
Pass3
;
for
(
JointIndex
i
=
1
;
i
<
(
JointIndex
)
model
.
njoints
;
++
i
)
{
Pass3
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
typename
Pass3
::
ArgsType
(
model
,
data
,
Minv_
));
}
typedef
ComputeABADerivativesBackwardStep2
<
JointCollection
>
Pass4
;
typedef
ComputeABADerivativesBackwardStep2
<
Scalar
,
Options
,
JointCollection
Tpl
>
Pass4
;
for
(
JointIndex
i
=
(
JointIndex
)(
model
.
njoints
-
1
);
i
>
0
;
--
i
)
{
Pass4
::
run
(
model
.
joints
[
i
],
...
...
src/algorithm/aba.hpp
View file @
b20f0039
...
...
@@ -40,10 +40,10 @@ namespace se3
///
/// \return The current joint acceleration stored in data.ddq.
///
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
>
inline
const
typename
DataTpl
<
JointCollection
>::
TangentVectorType
&
aba
(
const
ModelTpl
<
JointCollection
>
&
model
,
DataTpl
<
JointCollection
>
&
data
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
>
inline
const
typename
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>::
TangentVectorType
&
aba
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
);
...
...
@@ -66,10 +66,10 @@ namespace se3
///
/// \return The current joint acceleration stored in data.ddq.
///
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
typename
ForceDerived
>
inline
const
typename
DataTpl
<
JointCollection
>::
TangentVectorType
&
aba
(
const
ModelTpl
<
JointCollection
>
&
model
,
DataTpl
<
JointCollection
>
&
data
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
,
typename
ForceDerived
>
inline
const
typename
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>::
TangentVectorType
&
aba
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
,
...
...
@@ -87,10 +87,10 @@ namespace se3
///
/// \return The inverse of the joint space inertia matrix stored in data.ddq.
///
template
<
typename
JointCollection
,
typename
ConfigVectorType
>
inline
const
typename
DataTpl
<
JointCollection
>::
RowMatrixXd
&
computeMinverse
(
const
ModelTpl
<
JointCollection
>
&
model
,
DataTpl
<
JointCollection
>
&
data
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
>
inline
const
typename
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>::
RowMatrixXd
&
computeMinverse
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
);
...
...
src/algorithm/aba.hxx
View file @
b20f0039
...
...
@@ -26,12 +26,12 @@
namespace
se3
{
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType
>
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType
>
struct
AbaForwardStep1
:
public
fusion
::
JointVisitorBase
<
AbaForwardStep1
<
JointCollection
,
ConfigVectorType
,
TangentVectorType
>
>
:
public
fusion
::
JointVisitorBase
<
AbaForwardStep1
<
Scalar
,
Options
,
JointCollection
Tpl
,
ConfigVectorType
,
TangentVectorType
>
>
{
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&
,
...
...
@@ -67,12 +67,12 @@ namespace se3
};
template
<
typename
JointCollection
>
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
>
struct
AbaBackwardStep
:
public
fusion
::
JointVisitorBase
<
AbaBackwardStep
<
JointCollection
>
>
:
public
fusion
::
JointVisitorBase
<
AbaBackwardStep
<
Scalar
,
Options
,
JointCollection
Tpl
>
>
{
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&>
ArgsType
;
...
...
@@ -103,12 +103,12 @@ namespace se3
}
}
template
<
typename
S
calar
,
int
O
ptions
,
typename
Matrix6Type
>
template
<
typename
S
2
,
int
O
2
,
typename
Matrix6Type
>
inline
static
typename
EIGEN_PLAIN_TYPE
(
Matrix6Type
)
SE3actOn
(
const
SE3Tpl
<
S
calar
,
Options
>
&
M
,
SE3actOn
(
const
SE3Tpl
<
S
2
,
O2
>
&
M
,
const
Eigen
::
MatrixBase
<
Matrix6Type
>
&
I
)
{
typedef
SE3Tpl
<
S
calar
,
Options
>
SE3
;
typedef
SE3Tpl
<
S
2
,
O2
>
SE3
;
typedef
typename
SE3
::
Matrix3
Matrix3
;
typedef
typename
SE3
::
Vector3
Vector3
;
...
...
@@ -158,12 +158,12 @@ namespace se3
}
};
template
<
typename
JointCollection
>
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
>
struct
AbaForwardStep2
:
public
fusion
::
JointVisitorBase
<
AbaForwardStep2
<
JointCollection
>
>
:
public
fusion
::
JointVisitorBase
<
AbaForwardStep2
<
Scalar
,
Options
,
JointCollection
Tpl
>
>
{
typedef
ModelTpl
<
JointCollection
>
Model
;
typedef
DataTpl
<
JointCollection
>
Data
;
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&>
ArgsType
;
...
...
@@ -188,10 +188,10 @@ namespace se3
};
template
<
typename
JointCollection
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
>
inline
const
typename
DataTpl
<
JointCollection
>::
TangentVectorType
&
aba
(
const
ModelTpl
<
JointCollection
>
&
model
,
DataTpl
<
JointCollection
>
&
data
,
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollection
Tpl
,
typename
ConfigVectorType
,
typename
TangentVectorType1
,
typename
TangentVectorType2
>
inline
const
typename
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>::
TangentVectorType
&
aba
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType1
>
&
v
,
const
Eigen
::
MatrixBase
<
TangentVectorType2
>
&
tau
)
...
...
@@ -201,27 +201,27 @@ namespace se3
assert
(
v
.
size
()
==
model
.
nv
&&
"The joint velocity vector is not of right size"
);
assert
(
tau
.
size
()
==
model
.
nv
&&
"The joint acceleration vector is not of right size"
);
typedef
typename
ModelTpl
<
JointCollection
>::
JointIndex
JointIndex
;
typedef
typename
ModelTpl
<
Scalar
,
Options
,
JointCollection
Tpl
>::
JointIndex
JointIndex
;
data
.
v
[
0
].
setZero
();
data
.
a
[
0
]
=
-
model
.
gravity
;
data
.
u
=
tau
;
typedef
AbaForwardStep1
<
JointCollection
,
ConfigVectorType
,
TangentVectorType1
>
Pass1
;
typedef
AbaForwardStep1
<
Scalar
,
Options
,
JointCollection
Tpl
,
ConfigVectorType
,
TangentVectorType1
>
Pass1
;