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
b35c281c
Commit
b35c281c
authored
Jan 18, 2018
by
jcarpent
Browse files
[Algo] Add algo to compute the inverse of the joint space inertia matrix
parent
6db2169c
Changes
5
Hide whitespace changes
Inline
Side-by-side
src/algorithm/aba.hpp
View file @
b35c281c
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016
-2018
CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
...
...
@@ -60,6 +60,20 @@ namespace se3
const
Eigen
::
VectorXd
&
v
,
const
Eigen
::
VectorXd
&
tau
,
const
container
::
aligned_vector
<
Force
>
&
fext
);
///
/// \brief Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
///
/// \return The inverse of the joint space inertia matrix stored in data.ddq.
///
inline
const
Data
::
RowMatrixXd
&
computeMinverse
(
const
Model
&
model
,
Data
&
data
,
const
Eigen
::
VectorXd
&
q
);
DEFINE_ALGO_CHECKER
(
ABA
);
...
...
src/algorithm/aba.hxx
View file @
b35c281c
...
...
@@ -18,6 +18,7 @@
#ifndef __se3_aba_hxx__
#define __se3_aba_hxx__
#include
"pinocchio/spatial/act-on-set.hpp"
#include
"pinocchio/multibody/visitor.hpp"
#include
"pinocchio/algorithm/check.hpp"
...
...
@@ -257,8 +258,174 @@ namespace se3
return
data
.
ddq
;
}
struct
computeMinverseForwardStep1
:
public
fusion
::
JointVisitor
<
computeMinverseForwardStep1
>
{
typedef
boost
::
fusion
::
vector
<
const
se3
::
Model
&
,
se3
::
Data
&
,
const
Eigen
::
VectorXd
&
>
ArgsType
;
JOINT_VISITOR_INIT
(
computeMinverseForwardStep1
);
template
<
typename
JointModel
>
static
void
algo
(
const
se3
::
JointModelBase
<
JointModel
>
&
jmodel
,
se3
::
JointDataBase
<
typename
JointModel
::
JointDataDerived
>
&
jdata
,
const
se3
::
Model
&
model
,
se3
::
Data
&
data
,
const
Eigen
::
VectorXd
&
q
)
{
const
Model
::
JointIndex
&
i
=
jmodel
.
id
();
jmodel
.
calc
(
jdata
.
derived
(),
q
);
const
Model
::
Index
&
parent
=
model
.
parents
[
i
];
data
.
liMi
[
i
]
=
model
.
jointPlacements
[
i
]
*
jdata
.
M
();
if
(
parent
>
0
)
data
.
oMi
[
i
]
=
data
.
oMi
[
parent
]
*
data
.
liMi
[
i
];
else
data
.
oMi
[
i
]
=
data
.
liMi
[
i
];
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
Data
::
Matrix6x
>
::
Type
ColsBlock
;
ColsBlock
J_cols
=
jmodel
.
jointCols
(
data
.
J
);
J_cols
=
data
.
oMi
[
i
].
act
(
jdata
.
S
());
data
.
Yaba
[
i
]
=
model
.
inertias
[
i
].
matrix
();
}
};
struct
computeMinverseBackwardStep
:
public
fusion
::
JointVisitor
<
computeMinverseBackwardStep
>
{
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&>
ArgsType
;
JOINT_VISITOR_INIT
(
computeMinverseBackwardStep
);
template
<
typename
JointModel
>
static
void
algo
(
const
JointModelBase
<
JointModel
>
&
jmodel
,
JointDataBase
<
typename
JointModel
::
JointDataDerived
>
&
jdata
,
const
Model
&
model
,
Data
&
data
)
{
const
Model
::
JointIndex
&
i
=
jmodel
.
id
();
const
Model
::
Index
&
parent
=
model
.
parents
[
i
];
Inertia
::
Matrix6
&
Ia
=
data
.
Yaba
[
i
];
Data
::
RowMatrixXd
&
Minv
=
data
.
Minv
;
Data
::
Matrix6x
&
Fcrb
=
data
.
Fcrb
[
0
];
Data
::
Matrix6x
&
FcrbTmp
=
data
.
Fcrb
.
back
();
Data
::
RowMatrix6
&
M6tmpR
=
data
.
M6tmpR
;
jmodel
.
calc_aba
(
jdata
.
derived
(),
Ia
,
parent
>
0
);
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
Data
::
Matrix6x
>
::
Type
ColsBlock
;
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
Data
::
RowMatrix6
>
::
Type
RowColsBlock
;
// RowColsBlock U_cols = M6tmpR.leftCols<JointModel::NV>(jmodel.nv());
ColsBlock
U_cols
=
jmodel
.
jointCols
(
data
.
IS
);
forceSet
::
se3Action
(
data
.
oMi
[
i
],
jdata
.
U
(),
U_cols
);
// expressed in the world frame
Minv
.
block
(
jmodel
.
idx_v
(),
jmodel
.
idx_v
(),
jmodel
.
nv
(),
jmodel
.
nv
())
=
jdata
.
Dinv
();
const
int
nv_children
=
data
.
nvSubtree
[
i
]
-
jmodel
.
nv
();
if
(
nv_children
>
0
)
{
ColsBlock
J_cols
=
jmodel
.
jointCols
(
data
.
J
);
ColsBlock
SDinv_cols
=
jmodel
.
jointCols
(
data
.
SDinv
);
SDinv_cols
.
noalias
()
=
J_cols
*
jdata
.
Dinv
();
Minv
.
block
(
jmodel
.
idx_v
(),
jmodel
.
idx_v
()
+
jmodel
.
nv
(),
jmodel
.
nv
(),
nv_children
).
noalias
()
=
-
SDinv_cols
.
transpose
()
*
Fcrb
.
middleCols
(
jmodel
.
idx_v
()
+
jmodel
.
nv
(),
nv_children
);
if
(
parent
>
0
)
{
FcrbTmp
.
leftCols
(
data
.
nvSubtree
[
i
]).
noalias
()
=
U_cols
*
Minv
.
block
(
jmodel
.
idx_v
(),
jmodel
.
idx_v
(),
jmodel
.
nv
(),
data
.
nvSubtree
[
i
]);
Fcrb
.
middleCols
(
jmodel
.
idx_v
(),
data
.
nvSubtree
[
i
])
+=
FcrbTmp
.
leftCols
(
data
.
nvSubtree
[
i
]);
// Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i])
// += U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]);
}
}
else
{
Fcrb
.
middleCols
(
jmodel
.
idx_v
(),
data
.
nvSubtree
[
i
]).
noalias
()
=
U_cols
*
Minv
.
block
(
jmodel
.
idx_v
(),
jmodel
.
idx_v
(),
jmodel
.
nv
(),
data
.
nvSubtree
[
i
]);
}
if
(
parent
>
0
)
data
.
Yaba
[
parent
]
+=
AbaBackwardStep
::
SE3actOn
(
data
.
liMi
[
i
],
Ia
);
}
};
struct
computeMinverseForwardStep2
:
public
fusion
::
JointVisitor
<
computeMinverseForwardStep2
>
{
typedef
boost
::
fusion
::
vector
<
const
se3
::
Model
&
,
se3
::
Data
&
>
ArgsType
;
JOINT_VISITOR_INIT
(
computeMinverseForwardStep2
);
template
<
typename
JointModel
>
static
void
algo
(
const
se3
::
JointModelBase
<
JointModel
>
&
jmodel
,
se3
::
JointDataBase
<
typename
JointModel
::
JointDataDerived
>
&
jdata
,
const
se3
::
Model
&
model
,
se3
::
Data
&
data
)
{
const
Model
::
JointIndex
&
i
=
jmodel
.
id
();
const
Model
::
Index
&
parent
=
model
.
parents
[
i
];
Data
::
RowMatrixXd
&
Minv
=
data
.
Minv
;
Data
::
Matrix6x
&
Fcrb
=
data
.
Fcrb
[
0
];
Data
::
Matrix6x
&
FcrbTmp
=
data
.
Fcrb
.
back
();
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
Data
::
Matrix6x
>
::
Type
ColsBlock
;
ColsBlock
UDinv_cols
=
jmodel
.
jointCols
(
data
.
UDinv
);
forceSet
::
se3Action
(
data
.
oMi
[
i
],
jdata
.
UDinv
(),
UDinv_cols
);
// expressed in the world frame
ColsBlock
J_cols
=
jmodel
.
jointCols
(
data
.
J
);
if
(
parent
>
0
)
{
FcrbTmp
.
topRows
(
jmodel
.
nv
()).
rightCols
(
model
.
nv
-
jmodel
.
idx_v
()).
noalias
()
=
UDinv_cols
.
transpose
()
*
data
.
Fcrb
[
parent
].
rightCols
(
model
.
nv
-
jmodel
.
idx_v
());
Minv
.
middleRows
(
jmodel
.
idx_v
(),
jmodel
.
nv
()).
rightCols
(
model
.
nv
-
jmodel
.
idx_v
())
-=
FcrbTmp
.
topRows
(
jmodel
.
nv
()).
rightCols
(
model
.
nv
-
jmodel
.
idx_v
());
// Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v())
// -= UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
}
data
.
Fcrb
[
i
].
rightCols
(
model
.
nv
-
jmodel
.
idx_v
()).
noalias
()
=
J_cols
*
Minv
.
middleRows
(
jmodel
.
idx_v
(),
jmodel
.
nv
()).
rightCols
(
model
.
nv
-
jmodel
.
idx_v
());
if
(
parent
>
0
)
data
.
Fcrb
[
i
].
rightCols
(
model
.
nv
-
jmodel
.
idx_v
())
+=
data
.
Fcrb
[
parent
].
rightCols
(
model
.
nv
-
jmodel
.
idx_v
());
}
};
inline
const
Data
::
RowMatrixXd
&
computeMinverse
(
const
Model
&
model
,
Data
&
data
,
const
Eigen
::
VectorXd
&
q
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
for
(
Model
::
Index
i
=
1
;
i
<
(
Model
::
Index
)
model
.
njoints
;
++
i
)
{
computeMinverseForwardStep1
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
computeMinverseForwardStep1
::
ArgsType
(
model
,
data
,
q
));
}
data
.
Fcrb
[
0
].
setZero
();
for
(
Model
::
Index
i
=
(
Model
::
Index
)
model
.
njoints
-
1
;
i
>
0
;
--
i
)
{
computeMinverseBackwardStep
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
computeMinverseBackwardStep
::
ArgsType
(
model
,
data
));
}
for
(
Model
::
Index
i
=
1
;
i
<
(
Model
::
Index
)
model
.
njoints
;
++
i
)
{
computeMinverseForwardStep2
::
run
(
model
.
joints
[
i
],
data
.
joints
[
i
],
computeMinverseForwardStep2
::
ArgsType
(
model
,
data
));
}
return
data
.
Minv
;
}
// --- CHECKER ---------------------------------------------------------------
...
...
src/multibody/model.hpp
View file @
b35c281c
...
...
@@ -360,6 +360,8 @@ namespace se3
typedef
SE3
::
Vector3
Vector3
;
typedef
Eigen
::
Matrix
<
double
,
6
,
6
,
Eigen
::
RowMajor
>
RowMatrix6
;
typedef
Eigen
::
Matrix
<
double
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Eigen
::
RowMajor
>
RowMatrixXd
;
/// \brief Vector of se3::JointData associated to the se3::JointModel stored in model,
/// encapsulated in JointDataAccessor.
JointDataVector
joints
;
...
...
@@ -425,6 +427,9 @@ namespace se3
/// \brief The joint space inertia matrix (a square matrix of dim model.nv).
Eigen
::
MatrixXd
M
;
/// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
RowMatrixXd
Minv
;
/// \brief The Coriolis matrix (a square matrix of dim model.nv).
Eigen
::
MatrixXd
C
;
...
...
@@ -437,6 +442,15 @@ namespace se3
/// \brief Variation of the forceset with respect to the joint acceleration.
Matrix6x
dFda
;
/// \brief Used in computeMinverse
Matrix6x
SDinv
;
/// \brief Used in computeMinverse
Matrix6x
UDinv
;
/// \brief Used in computeMinverse
Matrix6x
IS
;
/// \brief Right variation of the inertia matrix
container
::
aligned_vector
<
Inertia
::
Matrix6
>
vxI
;
...
...
src/multibody/model.hxx
View file @
b35c281c
...
...
@@ -252,10 +252,14 @@ namespace se3
,
Ycrb
((
std
::
size_t
)
model
.
njoints
)
,
dYcrb
((
std
::
size_t
)
model
.
njoints
)
,
M
(
model
.
nv
,
model
.
nv
)
,
Minv
(
model
.
nv
,
model
.
nv
)
,
C
(
model
.
nv
,
model
.
nv
)
,
dFdq
(
6
,
model
.
nv
)
,
dFdv
(
6
,
model
.
nv
)
,
dFda
(
6
,
model
.
nv
)
,
SDinv
(
6
,
model
.
nv
)
,
UDinv
(
6
,
model
.
nv
)
,
IS
(
6
,
model
.
nv
)
,
vxI
((
std
::
size_t
)
model
.
njoints
)
,
Ivx
((
std
::
size_t
)
model
.
njoints
)
,
oYcrb
((
std
::
size_t
)
model
.
njoints
)
...
...
@@ -301,7 +305,7 @@ namespace se3
joints
.
push_back
(
CreateJointData
::
run
(
model
.
joints
[
i
]));
/* Init for CRBA */
M
.
fill
(
0
);
M
.
fill
(
0
);
Minv
.
setZero
();
for
(
Model
::
Index
i
=
0
;
i
<
(
Model
::
Index
)(
model
.
njoints
);
++
i
)
{
Fcrb
[
i
].
resize
(
6
,
model
.
nv
);
}
computeLastChild
(
model
);
...
...
unittest/aba.cpp
View file @
b35c281c
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016
-2018
CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
...
...
@@ -22,6 +22,7 @@
#include
"pinocchio/algorithm/aba.hpp"
#include
"pinocchio/algorithm/rnea.hpp"
#include
"pinocchio/algorithm/jacobian.hpp"
#include
"pinocchio/algorithm/joint-configuration.hpp"
#include
"pinocchio/algorithm/crba.hpp"
#include
"pinocchio/parsers/sample-models.hpp"
...
...
@@ -125,5 +126,45 @@ BOOST_AUTO_TEST_CASE ( test_aba_vs_rnea )
BOOST_CHECK
(
data
.
ddq
.
isApprox
(
a
,
1e-12
));
}
BOOST_AUTO_TEST_CASE
(
test_computeMinverse
)
{
using
namespace
Eigen
;
using
namespace
se3
;
se3
::
Model
model
;
buildModels
::
humanoidSimple
(
model
);
model
.
gravity
.
setZero
();
se3
::
Data
data
(
model
);
se3
::
Data
data_ref
(
model
);
model
.
lowerPositionLimit
.
head
<
3
>
().
fill
(
-
1.
);
model
.
upperPositionLimit
.
head
<
3
>
().
fill
(
1.
);
VectorXd
q
=
randomConfiguration
(
model
);
VectorXd
v
=
VectorXd
::
Random
(
model
.
nv
);
crba
(
model
,
data_ref
,
q
);
data_ref
.
M
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
data_ref
.
M
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
MatrixXd
Minv_ref
(
data_ref
.
M
.
inverse
());
computeMinverse
(
model
,
data
,
q
);
BOOST_CHECK
(
data
.
Minv
.
topRows
<
6
>
().
isApprox
(
Minv_ref
.
topRows
<
6
>
()));
data
.
Minv
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
data
.
Minv
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
BOOST_CHECK
(
data
.
Minv
.
isApprox
(
Minv_ref
));
// std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
// std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
//
// std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
// std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;
}
BOOST_AUTO_TEST_SUITE_END
()
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