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
loco-3d
Multicontact-api
Commits
affd699a
Commit
affd699a
authored
May 07, 2020
by
Pierre Fernbach
Browse files
add ContactModel::generatorMatrix method
parent
cbe03c98
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/multicontact-api/scenario/contact-model.hpp
View file @
affd699a
...
...
@@ -9,6 +9,7 @@
#include
"multicontact-api/serialization/eigen-matrix.hpp"
#include
<iostream>
#include
<Eigen/Dense>
#include
<pinocchio/spatial/skew.hpp>
namespace
multicontact_api
{
namespace
scenario
{
...
...
@@ -18,7 +19,9 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef
_Scalar
Scalar
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
3
>
Matrix3
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
Eigen
::
Dynamic
>
Matrix3X
;
typedef
Eigen
::
Matrix
<
Scalar
,
6
,
Eigen
::
Dynamic
>
Matrix6X
;
/// \brief Default constructor.
ContactModelTpl
()
...
...
@@ -87,6 +90,21 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
m_num_contact_points
=
positions
.
cols
();
}
/**
* @brief generatorMatrix Return a 6x(num_contact_points*3) matrix
* containing the generator used to compute contact forces.
* @return
*/
Matrix6X
generatorMatrix
()
const
{
Matrix6X
gen
=
Matrix6X
::
Zero
(
6
,
m_num_contact_points
*
3
);
for
(
size_t
i
=
0
;
i
<
m_num_contact_points
;
i
++
)
{
gen
.
block
(
0
,
i
*
3
,
3
,
3
)
=
Matrix3
::
Identity
();
gen
.
block
(
3
,
i
*
3
,
3
,
3
)
=
pinocchio
::
skew
(
m_contact_points_positions
.
col
(
i
));
}
return
gen
;
}
/// \brief Friction coefficient.
Scalar
m_mu
;
/// \brief ZMP radius.
...
...
unittest/scenario.cpp
View file @
affd699a
...
...
@@ -34,6 +34,7 @@ using curves::t_pointX_t;
using
namespace
multicontact_api
::
scenario
;
typedef
ContactSequence
::
ContactPhaseVector
ContactPhaseVector
;
typedef
ContactModel
::
Matrix3X
Matrix3X
;
typedef
ContactModel
::
Matrix6X
Matrix6X
;
typedef
pinocchio
::
SE3Tpl
<
double
>
SE3
;
...
...
@@ -384,6 +385,9 @@ BOOST_AUTO_TEST_CASE(contact_model_points_positions) {
BOOST_CHECK_EQUAL
(
mp
.
num_contact_points
(),
6
);
BOOST_CHECK_EQUAL
(
mp
.
contact_points_positions
().
cols
(),
6
);
BOOST_CHECK
(
mp
.
contact_points_positions
().
isApprox
(
positions
));
Matrix6X
generators
=
mp
.
generatorMatrix
();
BOOST_CHECK_EQUAL
(
generators
.
rows
(),
6
);
BOOST_CHECK_EQUAL
(
generators
.
cols
(),
6
*
3
);
mp
.
num_contact_points
(
2
);
BOOST_CHECK_EQUAL
(
mp
.
num_contact_points
(),
2
);
...
...
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