Commit 44b65f36 authored by Sébastien Kleff's avatar Sébastien Kleff
Browse files

formatting

parent 1fecfb4b
Pipeline #17997 failed with stage
in 29 minutes and 9 seconds
......@@ -15,11 +15,7 @@ namespace python {
void exposeContact1D() {
bp::register_ptr_to_python<boost::shared_ptr<ContactModel1D> >();
bp::enum_<Vector3MaskType>("Vector3MaskType")
.value("x", x)
.value("y", y)
.value("z", z)
.export_values();
bp::enum_<Vector3MaskType>("Vector3MaskType").value("x", x).value("y", y).value("z", z).export_values();
bp::class_<ContactModel1D, bp::bases<ContactModelAbstract> >(
"ContactModel1D",
......
......@@ -53,7 +53,8 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
*/
ContactModel1DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Scalar xref,
const std::size_t nu, const Vector2s& gains = Vector2s::Zero(),
const Vector3MaskType& mask = Vector3MaskType::z, const pinocchio::ReferenceFrame ref = pinocchio::LOCAL);
const Vector3MaskType& mask = Vector3MaskType::z,
const pinocchio::ReferenceFrame ref = pinocchio::LOCAL);
/**
* @brief Initialize the 1d contact model
......@@ -153,7 +154,7 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
private:
Scalar xref_; //!< Contact position used for the Baumgarte stabilization
Vector2s gains_; //!< Baumgarte stabilization gains
Vector3MaskType mask_; //!< Axis of the 1D contact in (x,y,z)
Vector3MaskType mask_; //!< Axis of the 1D contact in (x,y,z)
pinocchio::ReferenceFrame type_; //!< Reference type of contact
};
......@@ -182,7 +183,7 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
fXjda_dv(6, model->get_state()->get_nv()) {
frame = model->get_id();
jMf = model->get_state()->get_pinocchio()->frames[frame].placement;
fXj = pinocchio::SE3::Identity().toActionMatrix(); //jMf.inverse().toActionMatrix();
fXj = pinocchio::SE3::Identity().toActionMatrix(); // jMf.inverse().toActionMatrix();
fJf.setZero();
v_partial_dq.setZero();
a_partial_dq.setZero();
......@@ -200,7 +201,7 @@ struct ContactData1DTpl : public ContactDataAbstractTpl<_Scalar> {
mask = model->get_mask();
jMc_ = pinocchio::SE3::Identity();
wMlwa_ = pinocchio::SE3::Identity();
lwaMj_ = pinocchio::SE3::Identity();
lwaMj_ = pinocchio::SE3::Identity();
}
using Base::a0;
......
......@@ -64,13 +64,11 @@ void ContactModel1DTpl<Scalar>::calcDiff(const boost::shared_ptr<ContactDataAbst
pinocchio::skew(d->vv, d->vv_skew);
pinocchio::skew(d->vw, d->vw_skew);
// Matrix6s fXj;
if (type_ == pinocchio::LOCAL){
if (type_ == pinocchio::LOCAL) {
d->fXj = d->jMf.inverse().toActionMatrix();
}
else if (type_ == pinocchio::WORLD) {
} else if (type_ == pinocchio::WORLD) {
d->fXj = d->fXj;
}
else if (type_ == pinocchio::LOCAL_WORLD_ALIGNED) {
} else if (type_ == pinocchio::LOCAL_WORLD_ALIGNED) {
d->lwaMj_.translation(d->jMf.inverse().translation());
d->fXj = d->lwaMj_.toActionMatrix();
}
......
......@@ -40,17 +40,20 @@ void ResidualModelContactForceTpl<Scalar>::calc(const boost::shared_ptr<Residual
// We transform the force to the contact frame
switch (d->contact_type) {
case Contact1D:{
case Contact1D: {
ContactData1DTpl<Scalar>* d1d = static_cast<ContactData1DTpl<Scalar>*>(d->contact.get());
if(d1d->type == pinocchio::LOCAL){
if (d1d->type == pinocchio::LOCAL) {
data->r = ((d->contact->jMf.actInv(d->contact->f) - fref_).linear()).row(d1d->mask);
}
else if(d1d->type == pinocchio::WORLD) {
data->r = ((d->contact->pinocchio->oMf[id_].act(d->contact->jMf.actInv(d->contact->f)) - fref_).linear()).row(d1d->mask);
}
else if(d1d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
} else if (d1d->type == pinocchio::WORLD) {
data->r = ((d->contact->pinocchio->oMf[id_].act(d->contact->jMf.actInv(d->contact->f)) - fref_).linear())
.row(d1d->mask);
} else if (d1d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
d1d->wMlwa_.translation_impl(d->contact->pinocchio->oMf[id_].translation());
data->r = (( d1d->wMlwa_.actInv( d->contact->pinocchio->oMf[id_].act(d->contact->jMf.inverse()) ).act(d->contact->f) - fref_).linear()).row(d1d->mask);
data->r =
((d1d->wMlwa_.actInv(d->contact->pinocchio->oMf[id_].act(d->contact->jMf.inverse())).act(d->contact->f) -
fref_)
.linear())
.row(d1d->mask);
}
break;
}
......
......@@ -62,11 +62,9 @@ std::ostream& operator<<(std::ostream& os, const PinocchioReferenceTypes::Type&
ContactModel1DFactory::ContactModel1DFactory() {}
ContactModel1DFactory::~ContactModel1DFactory() {}
boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactModel1DFactory::create(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type,
const std::string frame_name,
std::size_t nu) const {
boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactModel1DFactory::create(
ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type, const std::string frame_name, std::size_t nu) const {
PinocchioModelFactory model_factory(model_type);
boost::shared_ptr<crocoddyl::StateMultibody> state =
boost::make_shared<crocoddyl::StateMultibody>(model_factory.create());
......@@ -84,33 +82,42 @@ boost::shared_ptr<crocoddyl::ContactModelAbstract> ContactModel1DFactory::create
Eigen::Vector2d gains = Eigen::Vector2d::Zero();
switch (mask_type) {
case ContactModelMaskTypes::X: {
if(reference_type == PinocchioReferenceTypes::LOCAL){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::LOCAL);
} else if(reference_type == PinocchioReferenceTypes::WORLD){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::WORLD);
} else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x, pinocchio::LOCAL_WORLD_ALIGNED);
if (reference_type == PinocchioReferenceTypes::LOCAL) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x,
pinocchio::LOCAL);
} else if (reference_type == PinocchioReferenceTypes::WORLD) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x,
pinocchio::WORLD);
} else if (reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::x,
pinocchio::LOCAL_WORLD_ALIGNED);
}
break;
}
case ContactModelMaskTypes::Y: {
if(reference_type == PinocchioReferenceTypes::LOCAL){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::LOCAL);
} else if(reference_type == PinocchioReferenceTypes::WORLD){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::WORLD);
} else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y, pinocchio::LOCAL_WORLD_ALIGNED);
if (reference_type == PinocchioReferenceTypes::LOCAL) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y,
pinocchio::LOCAL);
} else if (reference_type == PinocchioReferenceTypes::WORLD) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y,
pinocchio::WORLD);
} else if (reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::y,
pinocchio::LOCAL_WORLD_ALIGNED);
}
break;
}
case ContactModelMaskTypes::Z:
if(reference_type == PinocchioReferenceTypes::LOCAL){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::LOCAL);
} else if(reference_type == PinocchioReferenceTypes::WORLD){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::WORLD);
} else if(reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED){
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z, pinocchio::LOCAL_WORLD_ALIGNED);
}
if (reference_type == PinocchioReferenceTypes::LOCAL) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z,
pinocchio::LOCAL);
} else if (reference_type == PinocchioReferenceTypes::WORLD) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z,
pinocchio::WORLD);
} else if (reference_type == PinocchioReferenceTypes::LOCAL_WORLD_ALIGNED) {
contact = boost::make_shared<crocoddyl::ContactModel1D>(state, frame_id, 0., nu, gains, Vector3MaskType::z,
pinocchio::LOCAL_WORLD_ALIGNED);
}
break;
default:
throw_pretty(__FILE__ ": Wrong ContactModelMaskTypes::Type given");
......@@ -128,11 +135,14 @@ boost::shared_ptr<crocoddyl::ContactModelAbstract> create_random_contact1d() {
boost::shared_ptr<crocoddyl::ContactModelAbstract> contact;
ContactModel1DFactory factory;
if (rand() % 3 == 0) {
contact = factory.create(ContactModelMaskTypes::X, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL);
contact =
factory.create(ContactModelMaskTypes::X, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL);
} else if (rand() % 3 == 1) {
contact = factory.create(ContactModelMaskTypes::Y, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL);
contact =
factory.create(ContactModelMaskTypes::Y, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL);
} else {
contact = factory.create(ContactModelMaskTypes::Z, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL);
contact =
factory.create(ContactModelMaskTypes::Z, PinocchioModelTypes::RandomHumanoid, PinocchioReferenceTypes::LOCAL);
}
return contact;
}
......
......@@ -20,9 +20,8 @@
namespace crocoddyl {
namespace unittest {
struct ContactModelMaskTypes {
enum Type {X, Y, Z, NbMaskTypes};
enum Type { X, Y, Z, NbMaskTypes };
static std::vector<Type> init_all() {
std::vector<Type> v;
v.clear();
......@@ -35,7 +34,7 @@ struct ContactModelMaskTypes {
};
struct PinocchioReferenceTypes {
enum Type { LOCAL, WORLD, LOCAL_WORLD_ALIGNED, NbPinRefTypes};
enum Type { LOCAL, WORLD, LOCAL_WORLD_ALIGNED, NbPinRefTypes };
static std::vector<Type> init_all() {
std::vector<Type> v;
v.clear();
......@@ -47,12 +46,10 @@ struct PinocchioReferenceTypes {
static const std::vector<Type> all;
};
std::ostream& operator<<(std::ostream& os, const ContactModelMaskTypes::Type& type);
std::ostream& operator<<(std::ostream& os, const PinocchioReferenceTypes::Type& type);
class ContactModel1DFactory {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -62,8 +59,7 @@ class ContactModel1DFactory {
boost::shared_ptr<crocoddyl::ContactModelAbstract> create(
ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type,
const std::string frame_name = std::string(""),
PinocchioReferenceTypes::Type reference_type, const std::string frame_name = std::string(""),
const std::size_t nu = std::numeric_limits<std::size_t>::max()) const;
};
......
......@@ -25,8 +25,7 @@ using namespace boost::unit_test;
//----------------------------------------------------------------------------//
void test_construct_data(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
void test_construct_data(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
// create the model
ContactModel1DFactory factory;
......@@ -42,9 +41,8 @@ void test_construct_data(ContactModelMaskTypes::Type mask_type,
boost::shared_ptr<crocoddyl::ContactDataAbstract> data = model->createData(&pinocchio_data);
}
void test_calc_fetch_jacobians(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
void test_calc_fetch_jacobians(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
// create the model
ContactModel1DFactory factory;
boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(mask_type, model_type, reference_type);
......@@ -70,9 +68,8 @@ void test_calc_fetch_jacobians(ContactModelMaskTypes::Type mask_type,
BOOST_CHECK(data->df_du.isZero());
}
void test_calc_diff_fetch_derivatives(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
void test_calc_diff_fetch_derivatives(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
// create the model
ContactModel1DFactory factory;
boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(mask_type, model_type, reference_type);
......@@ -99,9 +96,8 @@ void test_calc_diff_fetch_derivatives(ContactModelMaskTypes::Type mask_type,
BOOST_CHECK(data->df_du.isZero());
}
void test_update_force(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
void test_update_force(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
// create the model
ContactModel1DFactory factory;
boost::shared_ptr<crocoddyl::ContactModelAbstract> model = factory.create(mask_type, model_type, reference_type);
......@@ -114,7 +110,7 @@ void test_update_force(ContactModelMaskTypes::Type mask_type,
// Create a random force and update it
Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
model->updateForce(data, f);
// boost::shared_ptr<crocoddyl::ContactModel3D> m = boost::static_pointer_cast<crocoddyl::ContactModel3D>(model);
// boost::shared_ptr<crocoddyl::ContactModel3D> m = boost::static_pointer_cast<crocoddyl::ContactModel3D>(model);
// Check that nothing has been computed and that all value are initialized to 0
BOOST_CHECK(data->Jc.isZero());
......@@ -125,8 +121,7 @@ void test_update_force(ContactModelMaskTypes::Type mask_type,
BOOST_CHECK(data->df_du.isZero());
}
void test_update_force_diff(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
void test_update_force_diff(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
// create the model
ContactModel1DFactory factory;
......@@ -151,8 +146,8 @@ void test_update_force_diff(ContactModelMaskTypes::Type mask_type,
BOOST_CHECK(!data->df_du.isZero());
}
void test_partial_derivatives_against_numdiff(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
void test_partial_derivatives_against_numdiff(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
#if BOOST_VERSION / 100 % 1000 >= 60
using namespace boost::placeholders;
......@@ -196,9 +191,8 @@ void test_partial_derivatives_against_numdiff(ContactModelMaskTypes::Type mask_t
//----------------------------------------------------------------------------//
void register_contact_model_unit_tests(ContactModelMaskTypes::Type mask_type,
PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
void register_contact_model_unit_tests(ContactModelMaskTypes::Type mask_type, PinocchioModelTypes::Type model_type,
PinocchioReferenceTypes::Type reference_type) {
boost::test_tools::output_test_stream test_name;
test_name << "test_" << mask_type << "_" << model_type << "_" << reference_type;
std::cout << "Running " << test_name.str() << std::endl;
......@@ -208,7 +202,8 @@ void register_contact_model_unit_tests(ContactModelMaskTypes::Type mask_type,
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives, mask_type, model_type, reference_type)));
ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force, mask_type, model_type, reference_type)));
ts->add(BOOST_TEST_CASE(boost::bind(&test_update_force_diff, mask_type, model_type, reference_type)));
ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, mask_type, model_type, reference_type)));
ts->add(
BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, mask_type, model_type, reference_type)));
framework::master_test_suite().add(ts);
}
......@@ -216,9 +211,8 @@ bool init_function() {
for (size_t mask_type = 0; mask_type < ContactModelMaskTypes::all.size(); ++mask_type) {
for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size(); ++model_type) {
for (size_t reference_type = 0; reference_type < PinocchioReferenceTypes::all.size(); ++reference_type) {
register_contact_model_unit_tests(ContactModelMaskTypes::all[mask_type],
PinocchioModelTypes::all[model_type],
PinocchioReferenceTypes::all[reference_type]);
register_contact_model_unit_tests(ContactModelMaskTypes::all[mask_type], PinocchioModelTypes::all[model_type],
PinocchioReferenceTypes::all[reference_type]);
}
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment