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
d78d951d
Unverified
Commit
d78d951d
authored
Oct 10, 2020
by
Justin Carpentier
Committed by
GitHub
Oct 10, 2020
Browse files
Merge pull request #1311 from jcarpent/topic/coriolis
Add getCoriolisMatrix
parents
1b10650f
0ca05e2f
Pipeline
#11625
passed with stage
in 215 minutes and 34 seconds
Changes
12
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
bindings/python/algorithm/expose-rnea.cpp
View file @
d78d951d
...
...
@@ -82,6 +82,15 @@ namespace pinocchio
"
\t
q: the joint configuration vector (size model.nq)
\n
"
"
\t
v: the joint velocity vector (size model.nv)
\n
"
,
bp
::
return_value_policy
<
bp
::
return_by_value
>
());
bp
::
def
(
"getCoriolisMatrix"
,
&
getCoriolisMatrix
<
double
,
0
,
JointCollectionDefaultTpl
>
,
bp
::
args
(
"model"
,
"Data"
),
"Retrives the Coriolis Matrix C(q,v) of the Lagrangian dynamics after calling one of the derivative algorithms, store the result in data.C and return it.
\n\n
"
"Parameters:
\n
"
"
\t
model: model of the kinematic tree
\n
"
"
\t
data: data related to the model
\n
"
,
bp
::
return_value_policy
<
bp
::
return_by_value
>
());
}
...
...
cmake
@
95917116
Compare
e4f507fb
...
95917116
Subproject commit
e4f507fb7e688c5d2dcf8f2d5c1af388596ba91c
Subproject commit
95917116c0f1b0b0690a22e12d3871b3bb7cae27
src/algorithm/aba-derivatives.hxx
View file @
d78d951d
...
...
@@ -55,7 +55,7 @@ namespace pinocchio
ov
=
data
.
oMi
[
i
].
act
(
data
.
v
[
i
]);
data
.
a_gf
[
i
]
=
jdata
.
c
()
+
(
data
.
v
[
i
]
^
jdata
.
v
());
data
.
Yaba
[
i
]
=
model
.
inertias
[
i
].
matrix
();
data
.
oYcrb
[
i
]
=
data
.
oMi
[
i
].
act
(
model
.
inertias
[
i
]);
data
.
oYcrb
[
i
]
=
data
.
oinertias
[
i
]
=
data
.
oMi
[
i
].
act
(
model
.
inertias
[
i
]);
data
.
oh
[
i
]
=
data
.
oYcrb
[
i
]
*
ov
;
data
.
of
[
i
]
=
ov
.
cross
(
data
.
oh
[
i
]);
...
...
src/algorithm/rnea-derivatives.hxx
View file @
d78d951d
//
// Copyright (c) 2017-20
19
CNRS INRIA
// Copyright (c) 2017-20
20
CNRS INRIA
//
#ifndef __pinocchio_rnea_derivatives_hxx__
...
...
@@ -46,7 +46,7 @@ namespace pinocchio
else
data
.
oMi
[
i
]
=
data
.
liMi
[
i
];
data
.
oYcrb
[
i
]
=
data
.
oMi
[
i
].
act
(
model
.
inertias
[
i
]);
data
.
oYcrb
[
i
]
=
data
.
oinertias
[
i
]
=
data
.
oMi
[
i
].
act
(
model
.
inertias
[
i
]);
data
.
of
[
i
]
=
data
.
oYcrb
[
i
]
*
minus_gravity
;
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
typename
Data
::
Matrix6x
>
::
Type
ColsBlock
;
...
...
@@ -242,7 +242,7 @@ namespace pinocchio
data
.
a
[
i
]
+=
data
.
liMi
[
i
].
actInv
(
data
.
a
[
parent
]);
}
data
.
oYcrb
[
i
]
=
data
.
oMi
[
i
].
act
(
model
.
inertias
[
i
]);
data
.
oYcrb
[
i
]
=
data
.
oinertias
[
i
]
=
data
.
oMi
[
i
].
act
(
model
.
inertias
[
i
]);
ov
=
data
.
oMi
[
i
].
act
(
data
.
v
[
i
]);
oa
=
data
.
oMi
[
i
].
act
(
data
.
a
[
i
]);
oa_gf
=
oa
-
model
.
gravity
;
// add gravity contribution
...
...
src/algorithm/rnea.hpp
View file @
d78d951d
//
// Copyright (c) 2015-20
19
CNRS INRIA
// Copyright (c) 2015-20
20
CNRS INRIA
//
#ifndef __pinocchio_rnea_hpp__
#define __pinocchio_rnea_hpp__
#ifndef __pinocchio_
algorithm_
rnea_hpp__
#define __pinocchio_
algorithm_
rnea_hpp__
#include
"pinocchio/multibody/model.hpp"
#include
"pinocchio/multibody/data.hpp"
...
...
@@ -154,9 +154,28 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
q
,
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
v
);
///
/// \brief Retrives the Coriolis Matrix \f$ C(q,\dot{q}) \f$ of the Lagrangian dynamics:
/// <CENTER> \f$ \begin{eqnarray} M \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau \end{eqnarray} \f$ </CENTER> <BR>
/// after a call to the dynamics derivatives.
///
/// \note In the previous equation, \f$ c(q, \dot{q}) = C(q, \dot{q})\dot{q} \f$.
///
/// \tparam JointCollection Collection of Joint types.
///
/// \param[in] model The model structure of the rigid body system.
/// \param[in] data The data structure of the rigid body system.
///
/// \return The Coriolis matrix stored in data.C.
///
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
inline
const
typename
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
MatrixXs
&
getCoriolisMatrix
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
data
);
}
// namespace pinocchio
/* --- Details -------------------------------------------------------------------- */
#include
"pinocchio/algorithm/rnea.hxx"
#endif // ifndef __pinocchio_rnea_hpp__
#endif // ifndef __pinocchio_
algorithm_
rnea_hpp__
src/algorithm/rnea.hxx
View file @
d78d951d
//
// Copyright (c) 2015-20
19
CNRS INRIA
// Copyright (c) 2015-20
20
CNRS INRIA
//
#ifndef __pinocchio_rnea_hxx__
#define __pinocchio_rnea_hxx__
#ifndef __pinocchio_
algorithm_
rnea_hxx__
#define __pinocchio_
algorithm_
rnea_hxx__
/// @cond DEV
...
...
@@ -473,8 +473,8 @@ namespace pinocchio
{
typedef
typename
Model
::
JointIndex
JointIndex
;
const
JointIndex
&
i
=
jmodel
.
id
();
const
JointIndex
&
parent
=
model
.
parents
[
i
];
const
JointIndex
i
=
jmodel
.
id
();
const
JointIndex
parent
=
model
.
parents
[
i
];
typename
Data
::
RowMatrix6
&
M6tmpR
=
data
.
M6tmpR
;
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
typename
Data
::
Matrix6x
>
::
Type
ColsBlock
;
...
...
@@ -543,9 +543,84 @@ namespace pinocchio
return
data
.
C
;
}
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
struct
GetCoriolisMatrixBackwardStep
:
public
fusion
::
JointUnaryVisitorBase
<
GetCoriolisMatrixBackwardStep
<
Scalar
,
Options
,
JointCollectionTpl
>
>
{
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Data
;
typedef
boost
::
fusion
::
vector
<
const
Model
&
,
Data
&
>
ArgsType
;
template
<
typename
JointModel
>
static
void
algo
(
const
JointModelBase
<
JointModel
>
&
jmodel
,
const
Model
&
model
,
Data
&
data
)
{
typedef
typename
Model
::
JointIndex
JointIndex
;
typedef
CoriolisMatrixBackwardStep
<
Scalar
,
Options
,
JointCollectionTpl
>
EquivalentPass
;
const
JointIndex
i
=
jmodel
.
id
();
const
JointIndex
parent
=
model
.
parents
[
i
];
typename
Data
::
RowMatrix6
&
M6tmpR
=
data
.
M6tmpR
;
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
typename
Data
::
Matrix6x
>
::
Type
ColsBlock
;
ColsBlock
dJ_cols
=
jmodel
.
jointCols
(
data
.
dJ
);
ColsBlock
J_cols
=
jmodel
.
jointCols
(
data
.
J
);
typename
Data
::
Matrix6x
&
dFdv
=
data
.
Fcrb
[
0
];
ColsBlock
dFdv_cols
=
jmodel
.
jointCols
(
dFdv
);
motionSet
::
inertiaAction
(
data
.
oYcrb
[
i
],
dJ_cols
,
dFdv_cols
);
dFdv_cols
.
noalias
()
+=
data
.
vxI
[
i
]
*
J_cols
;
data
.
C
.
block
(
jmodel
.
idx_v
(),
jmodel
.
idx_v
(),
jmodel
.
nv
(),
data
.
nvSubtree
[
i
]).
noalias
()
=
J_cols
.
transpose
()
*
dFdv
.
middleCols
(
jmodel
.
idx_v
(),
data
.
nvSubtree
[
i
]);
EquivalentPass
::
lhsInertiaMult
(
data
.
oYcrb
[
i
],
J_cols
.
transpose
(),
M6tmpR
.
topRows
(
jmodel
.
nv
()));
for
(
int
j
=
data
.
parents_fromRow
[(
JointIndex
)
jmodel
.
idx_v
()];
j
>=
0
;
j
=
data
.
parents_fromRow
[(
JointIndex
)
j
])
data
.
C
.
middleRows
(
jmodel
.
idx_v
(),
jmodel
.
nv
()).
col
(
j
).
noalias
()
=
M6tmpR
.
topRows
(
jmodel
.
nv
())
*
data
.
dJ
.
col
(
j
);
M6tmpR
.
topRows
(
jmodel
.
nv
()).
noalias
()
=
J_cols
.
transpose
()
*
data
.
vxI
[
i
];
for
(
int
j
=
data
.
parents_fromRow
[(
JointIndex
)
jmodel
.
idx_v
()];
j
>=
0
;
j
=
data
.
parents_fromRow
[(
JointIndex
)
j
])
data
.
C
.
middleRows
(
jmodel
.
idx_v
(),
jmodel
.
nv
()).
col
(
j
).
noalias
()
+=
M6tmpR
.
topRows
(
jmodel
.
nv
())
*
data
.
J
.
col
(
j
);
if
(
parent
>
0
)
{
data
.
vxI
[
parent
]
+=
data
.
vxI
[
i
];
}
}
};
template
<
typename
Scalar
,
int
Options
,
template
<
typename
,
int
>
class
JointCollectionTpl
>
inline
const
typename
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
MatrixXs
&
getCoriolisMatrix
(
const
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
model
,
DataTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
&
data
)
{
assert
(
model
.
check
(
data
)
&&
"data is not consistent with model."
);
typedef
ModelTpl
<
Scalar
,
Options
,
JointCollectionTpl
>
Model
;
typedef
typename
Model
::
JointIndex
JointIndex
;
for
(
JointIndex
i
=
1
;
i
<
(
JointIndex
)
model
.
njoints
;
++
i
)
{
Inertia
::
vxi
(
data
.
ov
[
i
],
data
.
oinertias
[
i
],
data
.
vxI
[
i
]);
}
typedef
GetCoriolisMatrixBackwardStep
<
Scalar
,
Options
,
JointCollectionTpl
>
Pass2
;
for
(
JointIndex
i
=
(
JointIndex
)(
model
.
njoints
-
1
);
i
>
0
;
--
i
)
{
Pass2
::
run
(
model
.
joints
[
i
],
typename
Pass2
::
ArgsType
(
model
,
data
));
}
return
data
.
C
;
}
}
// namespace pinocchio
/// @endcond
#endif // ifndef __pinocchio_rnea_hxx__
#endif // ifndef __pinocchio_
algorithm_
rnea_hxx__
src/multibody/data.hpp
View file @
d78d951d
...
...
@@ -183,10 +183,13 @@ namespace pinocchio
/// \brief Left variation of the inertia matrix
PINOCCHIO_ALIGNED_STD_VECTOR
(
Matrix6
)
Ivx
;
/// \brief Inertia quantities expressed in the world frame
/// \brief Rigid Body Inertia supported by the joint expressed in the world frame
PINOCCHIO_ALIGNED_STD_VECTOR
(
Inertia
)
oinertias
;
/// \brief Composite Rigid Body Inertia expressed in the world frame
PINOCCHIO_ALIGNED_STD_VECTOR
(
Inertia
)
oYcrb
;
/// \brief Time variation of
the inertia quantities
expressed in the world frame
/// \brief Time variation of
Composite Rigid Body Inertia
expressed in the world frame
PINOCCHIO_ALIGNED_STD_VECTOR
(
Matrix6
)
doYcrb
;
/// \brief Temporary for derivative algorithms
...
...
src/multibody/data.hxx
View file @
d78d951d
...
...
@@ -53,6 +53,7 @@ namespace pinocchio
,
IS
(
MatrixXs
::
Zero
(
6
,
model
.
nv
))
,
vxI
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Matrix6
::
Zero
())
,
Ivx
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Matrix6
::
Zero
())
,
oinertias
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Zero
())
,
oYcrb
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Zero
())
,
doYcrb
((
std
::
size_t
)
model
.
njoints
,
Inertia
::
Matrix6
::
Zero
())
,
ddq
(
VectorXs
::
Zero
(
model
.
nv
))
...
...
@@ -248,6 +249,7 @@ namespace pinocchio
&&
data1
.
IS
==
data2
.
IS
&&
data1
.
vxI
==
data2
.
vxI
&&
data1
.
Ivx
==
data2
.
Ivx
&&
data1
.
oinertias
==
data2
.
oinertias
&&
data1
.
oYcrb
==
data2
.
oYcrb
&&
data1
.
doYcrb
==
data2
.
doYcrb
&&
data1
.
ddq
==
data2
.
ddq
...
...
src/multibody/joint/joint-spherical.hpp
View file @
d78d951d
...
...
@@ -419,7 +419,7 @@ namespace pinocchio
{
typedef
typename
ConfigVectorLike
::
Scalar
Scalar
;
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE
(
ConfigVector_t
,
ConfigVectorLike
);
typedef
typename
Eigen
::
Quaternion
<
typename
ConfigVectorLike
::
Scalar
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
ConfigVectorLike
)
::
Options
>
Quaternion
;
typedef
typename
Eigen
::
Quaternion
<
Scalar
,
PINOCCHIO_EIGEN_PLAIN_TYPE
(
ConfigVectorLike
)
::
Options
>
Quaternion
;
typedef
Eigen
::
Map
<
const
Quaternion
>
ConstQuaternionMap
;
ConstQuaternionMap
quat
(
q_joint
.
derived
().
data
());
...
...
src/serialization/data.hpp
View file @
d78d951d
...
...
@@ -57,6 +57,7 @@ namespace boost
PINOCCHIO_MAKE_DATA_NVP
(
ar
,
data
,
IS
);
PINOCCHIO_MAKE_DATA_NVP
(
ar
,
data
,
vxI
);
PINOCCHIO_MAKE_DATA_NVP
(
ar
,
data
,
Ivx
);
PINOCCHIO_MAKE_DATA_NVP
(
ar
,
data
,
oinertias
);
PINOCCHIO_MAKE_DATA_NVP
(
ar
,
data
,
oYcrb
);
PINOCCHIO_MAKE_DATA_NVP
(
ar
,
data
,
doYcrb
);
PINOCCHIO_MAKE_DATA_NVP
(
ar
,
data
,
ddq
);
...
...
unittest/rnea-derivatives.cpp
View file @
d78d951d
...
...
@@ -471,4 +471,39 @@ BOOST_AUTO_TEST_CASE(test_multiple_calls)
BOOST_CHECK
(
data1
.
M
.
isApprox
(
data2
.
M
));
}
BOOST_AUTO_TEST_CASE
(
test_get_coriolis
)
{
using
namespace
Eigen
;
using
namespace
pinocchio
;
Model
model
;
buildModels
::
humanoidRandom
(
model
);
model
.
lowerPositionLimit
.
head
<
3
>
().
fill
(
-
1.
);
model
.
upperPositionLimit
.
head
<
3
>
().
fill
(
1.
);
Data
data_ref
(
model
);
Data
data
(
model
);
VectorXd
q
=
randomConfiguration
(
model
);
VectorXd
v
=
VectorXd
::
Random
(
model
.
nv
);
VectorXd
tau
=
VectorXd
::
Random
(
model
.
nv
);
computeCoriolisMatrix
(
model
,
data_ref
,
q
,
v
);
computeRNEADerivatives
(
model
,
data
,
q
,
v
,
tau
);
getCoriolisMatrix
(
model
,
data
);
BOOST_CHECK
(
data
.
J
.
isApprox
(
data_ref
.
J
));
BOOST_CHECK
(
data
.
dJ
.
isApprox
(
data_ref
.
dJ
));
BOOST_CHECK
(
data
.
Fcrb
[
0
].
isApprox
(
data_ref
.
dFdv
));
for
(
JointIndex
k
=
1
;
k
<
model
.
joints
.
size
();
++
k
)
{
BOOST_CHECK
(
data
.
vxI
[
k
].
isApprox
(
data_ref
.
vxI
[
k
]));
BOOST_CHECK
(
data
.
oYcrb
[
k
].
isApprox
(
data_ref
.
oYcrb
[
k
]));
}
BOOST_CHECK
(
data
.
C
.
isApprox
(
data_ref
.
C
));
}
BOOST_AUTO_TEST_SUITE_END
()
unittest/rnea.cpp
View file @
d78d951d
...
...
@@ -243,90 +243,70 @@ BOOST_AUTO_TEST_CASE(test_compute_static_torque)
BOOST_CHECK
(
static_torque_ref
.
isApprox
(
data
.
tau
));
}
BOOST_AUTO_TEST_CASE
(
test_compute_coriolis
)
BOOST_AUTO_TEST_CASE
(
test_compute_coriolis
)
{
using
namespace
Eigen
;
using
namespace
pinocchio
;
const
double
prec
=
Eigen
::
NumTraits
<
double
>::
dummy_precision
();
Model
model
;
buildModels
::
humanoidRandom
(
model
);
model
.
lowerPositionLimit
.
head
<
3
>
().
fill
(
-
1.
);
model
.
upperPositionLimit
.
head
<
3
>
().
fill
(
1.
);
Data
data_ref
(
model
);
Data
data
(
model
);
VectorXd
q
=
randomConfiguration
(
model
);
VectorXd
v
(
VectorXd
::
Random
(
model
.
nv
));
computeCoriolisMatrix
(
model
,
data
,
q
,
Eigen
::
VectorXd
::
Zero
(
model
.
nv
));
BOOST_CHECK
(
data
.
C
.
isZero
(
prec
));
model
.
gravity
.
setZero
();
rnea
(
model
,
data_ref
,
q
,
v
,
VectorXd
::
Zero
(
model
.
nv
));
computeJointJacobiansTimeVariation
(
model
,
data_ref
,
q
,
v
);
computeCoriolisMatrix
(
model
,
data
,
q
,
v
);
BOOST_CHECK
(
data
.
dJ
.
isApprox
(
data_ref
.
dJ
));
BOOST_CHECK
(
data
.
J
.
isApprox
(
data_ref
.
J
));
VectorXd
tau
=
data
.
C
*
v
;
BOOST_CHECK
(
tau
.
isApprox
(
data_ref
.
tau
,
prec
));
dccrba
(
model
,
data_ref
,
q
,
v
);
crba
(
model
,
data_ref
,
q
);
const
Data
::
Vector3
&
com
=
data_ref
.
com
[
0
];
Motion
vcom
(
data_ref
.
vcom
[
0
],
Data
::
Vector3
::
Zero
());
SE3
cM1
(
data
.
oMi
[
1
]);
cM1
.
translation
()
-=
com
;
BOOST_CHECK
((
cM1
.
toDualActionMatrix
()
*
data_ref
.
M
.
topRows
<
6
>
()).
isApprox
(
data_ref
.
Ag
,
prec
));
Force
dh_ref
=
cM1
.
act
(
Force
(
data_ref
.
tau
.
head
<
6
>
()));
Force
dh
(
data_ref
.
dAg
*
v
);
BOOST_CHECK
(
dh
.
isApprox
(
dh_ref
,
prec
));
{
using
namespace
Eigen
;
using
namespace
pinocchio
;
const
double
prec
=
Eigen
::
NumTraits
<
double
>::
dummy_precision
();
Model
model
;
buildModels
::
humanoidRandom
(
model
);
model
.
lowerPositionLimit
.
head
<
3
>
().
fill
(
-
1.
);
model
.
upperPositionLimit
.
head
<
3
>
().
fill
(
1.
);
Data
data_ref
(
model
);
Data
data
(
model
);
VectorXd
q
=
randomConfiguration
(
model
);
VectorXd
v
(
VectorXd
::
Random
(
model
.
nv
));
computeCoriolisMatrix
(
model
,
data
,
q
,
Eigen
::
VectorXd
::
Zero
(
model
.
nv
));
BOOST_CHECK
(
data
.
C
.
isZero
(
prec
));
model
.
gravity
.
setZero
();
rnea
(
model
,
data_ref
,
q
,
v
,
VectorXd
::
Zero
(
model
.
nv
));
computeJointJacobiansTimeVariation
(
model
,
data_ref
,
q
,
v
);
computeCoriolisMatrix
(
model
,
data
,
q
,
v
);
BOOST_CHECK
(
data
.
dJ
.
isApprox
(
data_ref
.
dJ
));
BOOST_CHECK
(
data
.
J
.
isApprox
(
data_ref
.
J
));
Data
data_ref
(
model
),
data_ref_plus
(
model
);
Eigen
::
MatrixXd
dM
(
data
.
C
+
data
.
C
.
transpose
());
VectorXd
tau
=
data
.
C
*
v
;
BOOST_CHECK
(
tau
.
isApprox
(
data_ref
.
tau
,
prec
));
const
double
alpha
=
1e-8
;
Eigen
::
VectorXd
q_plus
(
model
.
nq
);
q_plus
=
integrate
(
model
,
q
,
alpha
*
v
);
dccrba
(
model
,
data_ref
,
q
,
v
);
crba
(
model
,
data_ref
,
q
);
data_ref
.
M
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
data_ref
.
M
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
crba
(
model
,
data_ref_plus
,
q_plus
);
data_ref_plus
.
M
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
data_ref_plus
.
M
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
const
Data
::
Vector3
&
com
=
data_ref
.
com
[
0
];
Motion
vcom
(
data_ref
.
vcom
[
0
],
Data
::
Vector3
::
Zero
());
SE3
cM1
(
data
.
oMi
[
1
]);
cM1
.
translation
()
-=
com
;
BOOST_CHECK
((
cM1
.
toDualActionMatrix
()
*
data_ref
.
M
.
topRows
<
6
>
()).
isApprox
(
data_ref
.
Ag
,
prec
));
Force
dh_ref
=
cM1
.
act
(
Force
(
data_ref
.
tau
.
head
<
6
>
()));
Force
dh
(
data_ref
.
dAg
*
v
);
BOOST_CHECK
(
dh
.
isApprox
(
dh_ref
,
prec
));
{
Data
data_ref
(
model
),
data_ref_plus
(
model
);
Eigen
::
MatrixXd
dM
(
data
.
C
+
data
.
C
.
transpose
());
const
double
alpha
=
1e-8
;
Eigen
::
VectorXd
q_plus
(
model
.
nq
);
q_plus
=
integrate
(
model
,
q
,
alpha
*
v
);
crba
(
model
,
data_ref
,
q
);
data_ref
.
M
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
data_ref
.
M
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
crba
(
model
,
data_ref_plus
,
q_plus
);
data_ref_plus
.
M
.
triangularView
<
Eigen
::
StrictlyLower
>
()
=
data_ref_plus
.
M
.
transpose
().
triangularView
<
Eigen
::
StrictlyLower
>
();
Eigen
::
MatrixXd
dM_ref
=
(
data_ref_plus
.
M
-
data_ref
.
M
)
/
alpha
;
BOOST_CHECK
(
dM
.
isApprox
(
dM_ref
,
sqrt
(
alpha
)));
}
// {
// //v.setZero();
// v.setOnes();
// Eigen::VectorXd v_fd(v);
// Eigen::MatrixXd drnea_dv(model.nv,model.nv);
// Data data_fd(model);
//
// const VectorXd tau0 = rnea(model,data_fd,q,v,0*v);
// const double eps = 1e-8;
// for(int i = 0; i < model.nv; ++i)
// {
// v_fd[i] += eps;
// rnea(model,data_fd,q,v_fd,0*v);
// drnea_dv.col(i) = (data_fd.tau - tau0)/eps;
// v_fd[i] -= eps;
// }
// BOOST_CHECK(v_fd.isApprox(v));
// std::cout << "drnea_dv:\n" << drnea_dv.block<6,6>(0,0) << std::endl;
// std::cout << "C:\n" << data.C.block<6,6>(0,0) << std::endl;
// }
Eigen
::
MatrixXd
dM_ref
=
(
data_ref_plus
.
M
-
data_ref
.
M
)
/
alpha
;
BOOST_CHECK
(
dM
.
isApprox
(
dM_ref
,
sqrt
(
alpha
)));
}
BOOST_AUTO_TEST_SUITE_END
()
}
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