Skip to content

Commit

Permalink
fixed bindings and unittests for contact1d
Browse files Browse the repository at this point in the history
  • Loading branch information
@skleff1994
skleff1994 committed Oct 26, 2023
1 parent f44d414 commit fc4c5e7
Show file tree
Hide file tree
Showing 15 changed files with 116 additions and 119 deletions.
35 changes: 17 additions & 18 deletions bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,30 +29,29 @@ void exposeContact1D() {
"(holonomic constraint) or\n"
"the derivatives of the holonomic constraint, respectively.",
bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex,
Eigen::Vector3d, pinocchio::ReferenceFrame, std::size_t,
bp::optional<Eigen::Vector2d, Eigen::Matrix3d> >(
bp::args("self", "state", "id", "xref", "type", "nu", "gains", "rotation"),
double, pinocchio::ReferenceFrame, Eigen::Matrix3d,
std::size_t, bp::optional<Eigen::Vector2d> >(
bp::args("self", "state", "id", "xref", "type", "rotation", "nu", "gains"),
"Initialize the contact model.\n\n"
":param state: state of the multibody system\n"
":param id: reference frame id of the contact\n"
":param xref: contact position used for the Baumgarte stabilization\n"
":param type: type of contact\n"
":param rotation: rotation of the reference frame's z axis"
":param nu: dimension of control vector\n"
":param gains: gains of the contact model (default "
"np.matrix([0.,0.]))\n"
":param rotation: rotation of the reference frame's z axis"))
"np.matrix([0.,0.]))"))
.def(bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex,
Eigen::Vector3d, pinocchio::ReferenceFrame,
bp::optional<Eigen::Vector2d, Eigen::Matrix3d> >(
bp::args("self", "state", "id", "xref", "type", "gains", "rotation"),
double, pinocchio::ReferenceFrame,
bp::optional<Eigen::Vector2d> >(
bp::args("self", "state", "id", "xref", "type", "gains"),
"Initialize the contact model.\n\n"
":param state: state of the multibody system\n"
":param id: reference frame id of the contact\n"
":param xref: contact position used for the Baumgarte stabilization\n"
":param type: type of contact\n"
":param gains: gains of the contact model (default "
"np.matrix([0.,0.]))\n"
":param rotation: rotation of the reference frame's z axis"))
"np.matrix([0.,0.]))"))
.def("calc", &ContactModel1D::calc, bp::args("self", "data", "x"),
"Compute the 1D contact Jacobian and drift.\n\n"
"The rigid contact model throught acceleration-base holonomic "
Expand Down Expand Up @@ -82,15 +81,15 @@ void exposeContact1D() {
"returns the allocated data for a predefined cost.\n"
":param data: Pinocchio data\n"
":return contact data.")
.add_property("reference",
bp::make_function(&ContactModel1D::get_reference,
bp::return_internal_reference<>()),
&ContactModel1D::set_reference,
"reference contact translation")
.add_property("rotation",
bp::make_function(&ContactModel1D::get_rotation,
.add_property(
"reference",
bp::make_function(&ContactModel1D::get_reference,
bp::return_value_policy<bp::return_by_value>()),
&ContactModel1D::set_reference, "reference contact translation")
.add_property("Raxis",
bp::make_function(&ContactModel1D::get_axis_rotation,
bp::return_internal_reference<>()),
&ContactModel1D::set_rotation,
&ContactModel1D::set_axis_rotation,
"rotation of the reference frame's z axis")
.add_property(
"gains",
Expand Down
38 changes: 17 additions & 21 deletions include/crocoddyl/multibody/contacts/contact-1d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,15 +49,15 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
* @param[in] id Reference frame id of the contact
* @param[in] xref Contact position used for the Baumgarte stabilization
* @param[in] type Type of contact
* @param[in] rotation Rotation of the reference frame's z-axis
* @param[in] nu Dimension of the control vector
* @param[in] gains Baumgarte stabilization gains
* @param[in] rotation Rotation of the reference frame's z-axis
*/
ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex id, const Vector3s& xref,
const pinocchio::ReferenceFrame type, const std::size_t nu,
const Vector2s& gains = Vector2s::Zero(),
const Matrix3s& rotation = Matrix3s::Identity());
const pinocchio::FrameIndex id, const Scalar xref,
const pinocchio::ReferenceFrame type,
const Matrix3s& rotation, const std::size_t nu,
const Vector2s& gains = Vector2s::Zero());

/**
* @brief Initialize the 1d contact model
Expand All @@ -73,29 +73,25 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
* @param[in] xref Contact position used for the Baumgarte stabilization
* @param[in] type Type of contact
* @param[in] gains Baumgarte stabilization gains
* @param[in] rotation Rotation of the reference frame's z-axis
*/
ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex id, const Vector3s& xref,
const pinocchio::FrameIndex id, const Scalar xref,
const pinocchio::ReferenceFrame type,
const Vector2s& gains = Vector2s::Zero(),
const Matrix3s& rotation = Matrix3s::Identity());
const Vector2s& gains = Vector2s::Zero());

DEPRECATED(
"Use constructor that passes the type type of contact, this assumes is "
"pinocchio::LOCAL",
ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex id, const Vector3s& xref,
const pinocchio::FrameIndex id, const Scalar xref,
const std::size_t nu,
const Vector2s& gains = Vector2s::Zero(),
const Matrix3s& rotation = Matrix3s::Identity());)
const Vector2s& gains = Vector2s::Zero());)
DEPRECATED(
"Use constructor that passes the type type of contact, this assumes is "
"pinocchio::LOCAL",
ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex id, const Vector3s& xref,
const Vector2s& gains = Vector2s::Zero(),
const Matrix3s& rotation = Matrix3s::Identity());)
const pinocchio::FrameIndex id, const Scalar xref,
const Vector2s& gains = Vector2s::Zero());)
virtual ~ContactModel1DTpl();

/**
Expand Down Expand Up @@ -136,7 +132,7 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
/**
* @brief Return the reference frame translation
*/
const Vector3s& get_reference() const;
const Scalar get_reference() const;

/**
* @brief Create the 1d contact data
Expand All @@ -146,17 +142,17 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
/**
* @brief Return the rotation of the reference frames's z axis
*/
const Matrix3s& get_rotation() const;
const Matrix3s& get_axis_rotation() const;

/**
* @brief Modify the reference frame translation
*/
void set_reference(const Vector3s& reference);
void set_reference(const Scalar reference);

/**
* @brief Modify the rotation of the reference frames's z axis
*/
void set_rotation(const Matrix3s& rotation);
void set_axis_rotation(const Matrix3s& rotation);

/**
* @brief Print relevant information of the 1d contact model
Expand All @@ -173,9 +169,9 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
using Base::type_;

private:
Vector3s xref_; //!< Contact position used for the Baumgarte stabilization
Scalar xref_; //!< Contact position used for the Baumgarte stabilization
Vector2s gains_; //!< Baumgarte stabilization gains
Matrix3s rotation_; //!< Rotation of the reference frame's z-axis
Matrix3s Raxis_; //!< Rotation of the reference frame's z-axis
};

template <typename _Scalar>
Expand Down
63 changes: 31 additions & 32 deletions include/crocoddyl/multibody/contacts/contact-1d.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,19 @@ namespace crocoddyl {
template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
const Vector3s& xref, const pinocchio::ReferenceFrame type,
const std::size_t nu, const Vector2s& gains, const Matrix3s& rotation)
: Base(state, type, 1, nu), xref_(xref), gains_(gains), rotation_(rotation) {
Scalar xref, const pinocchio::ReferenceFrame type,
const Matrix3s& rotation, const std::size_t nu, const Vector2s& gains)
: Base(state, type, 1, nu), xref_(xref), Raxis_(rotation), gains_(gains) {
id_ = id;
}

template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
const Vector3s& xref, const pinocchio::ReferenceFrame type,
const Vector2s& gains, const Matrix3s& rotation)
: Base(state, type, 1), xref_(xref), gains_(gains), rotation_(rotation) {
Scalar xref, const pinocchio::ReferenceFrame type, const Vector2s& gains)
: Base(state, type, 1), xref_(xref), gains_(gains) {
id_ = id;
Raxis_ = Matrix3s::Identity();
}

#pragma GCC diagnostic push // TODO: Remove once the deprecated FrameXX has
Expand All @@ -34,12 +34,12 @@ ContactModel1DTpl<Scalar>::ContactModel1DTpl(
template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
const Vector3s& xref, const std::size_t nu, const Vector2s& gains, const Matrix3s& rotation)
Scalar xref, const std::size_t nu, const Vector2s& gains)
: Base(state, pinocchio::ReferenceFrame::LOCAL, 1, nu),
xref_(xref),
gains_(gains),
rotation_(rotation) {
gains_(gains) {
id_ = id;
Raxis_ = Matrix3s::Identity();
std::cerr << "Deprecated: Use constructor that passes the type of contact, "
"this assumes is pinocchio::LOCAL."
<< std::endl;
Expand All @@ -48,12 +48,12 @@ ContactModel1DTpl<Scalar>::ContactModel1DTpl(
template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
const Vector3s& xref, const Vector2s& gains, const Matrix3s& rotation)
Scalar xref, const Vector2s& gains)
: Base(state, pinocchio::ReferenceFrame::LOCAL, 1),
xref_(xref),
gains_(gains),
rotation_(rotation) {
gains_(gains) {
id_ = id;
Raxis_ = Matrix3s::Identity();
std::cerr << "Deprecated: Use constructor that passes the type of contact, "
"this assumes is pinocchio::LOCAL."
<< std::endl;
Expand Down Expand Up @@ -83,7 +83,7 @@ void ContactModel1DTpl<Scalar>::calc(

const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
if (gains_[0] != 0.) {
d->dp = d->pinocchio->oMf[id_].translation() - xref_;
d->dp = d->pinocchio->oMf[id_].translation() - (xref_ * Raxis_ * Vector3s::UnitZ());
d->dp_local.noalias() = oRf.transpose() * d->dp;
d->a0_local += gains_[0] * d->dp_local;
}
Expand All @@ -92,13 +92,13 @@ void ContactModel1DTpl<Scalar>::calc(
}
switch (type_) {
case pinocchio::ReferenceFrame::LOCAL:
d->Jc.row(0) = (rotation_ * d->fJf.template topRows<3>()).row(2);
d->a0[0] = (rotation_ * d->a0_local)[2];
d->Jc.row(0) = (Raxis_ * d->fJf.template topRows<3>()).row(2);
d->a0[0] = (Raxis_ * d->a0_local)[2];
break;
case pinocchio::ReferenceFrame::WORLD:
case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
d->Jc.row(0) = (rotation_* oRf * d->fJf.template topRows<3>()).row(2);
d->a0[0] = (rotation_ * oRf * d->a0_local)[2];
d->Jc.row(0) = (Raxis_* oRf * d->fJf.template topRows<3>()).row(2);
d->a0[0] = (Raxis_ * oRf * d->a0_local)[2];
break;
}
}
Expand Down Expand Up @@ -146,7 +146,7 @@ void ContactModel1DTpl<Scalar>::calcDiff(
}
switch (type_) {
case pinocchio::ReferenceFrame::LOCAL:
d->da0_dx.row(0) = (rotation_ * d->da0_local_dx).row(2);
d->da0_dx.row(0) = (Raxis_ * d->da0_local_dx).row(2);
break;
case pinocchio::ReferenceFrame::WORLD:
case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
Expand All @@ -162,11 +162,11 @@ void ContactModel1DTpl<Scalar>::calcDiff(
if (gains_[1] != 0.) {
d->a0_local += gains_[1] * d->v.linear();
}
d->a0[0] = (rotation_ * oRf * d->a0_local)[2];
d->a0[0] = (Raxis_ * oRf * d->a0_local)[2];

pinocchio::skew((rotation_ * oRf * d->a0_local).template head<3>(), d->a0_skew);
d->a0_world_skew.noalias() = d->a0_skew * rotation_ * oRf;
d->da0_dx.row(0) = (rotation_ * oRf * d->da0_local_dx).row(2);
pinocchio::skew((Raxis_ * oRf * d->a0_local).template head<3>(), d->a0_skew);
d->a0_world_skew.noalias() = d->a0_skew * Raxis_ * oRf;
d->da0_dx.row(0) = (Raxis_ * oRf * d->da0_local_dx).row(2);
d->da0_dx.leftCols(nv).row(0) -=
(d->a0_world_skew * d->fJf.template bottomRows<3>()).row(2);
break;
Expand All @@ -187,14 +187,14 @@ void ContactModel1DTpl<Scalar>::updateForce(
data->f.angular().setZero();
switch (type_) {
case pinocchio::ReferenceFrame::LOCAL:
data->fext.linear() = (R * rotation_.transpose()).col(2) * force[0];
data->fext.linear() = (R * Raxis_.transpose()).col(2) * force[0];
data->fext.angular() = d->jMf.translation().cross(data->fext.linear());
data->dtau_dq.setZero();
break;
case pinocchio::ReferenceFrame::WORLD:
case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
d->f_local.linear().noalias() = (oRf.transpose() * rotation_.transpose()).col(2) * force[0];
d->f_local.linear().noalias() = (oRf.transpose() * Raxis_.transpose()).col(2) * force[0];
d->f_local.angular().setZero();
data->fext = data->jMf.act(d->f_local);
pinocchio::skew(d->f_local.linear(), d->f_skew);
Expand All @@ -216,12 +216,11 @@ ContactModel1DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
template <typename Scalar>
void ContactModel1DTpl<Scalar>::print(std::ostream& os) const {
os << "ContactModel1D {frame=" << state_->get_pinocchio()->frames[id_].name
<< "}";
<< ", axis=" << (Raxis_ * Vector3s::UnitZ()).transpose() << "}";
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Vector3s&
ContactModel1DTpl<Scalar>::get_reference() const {
const Scalar ContactModel1DTpl<Scalar>::get_reference() const {
return xref_;
}

Expand All @@ -233,18 +232,18 @@ ContactModel1DTpl<Scalar>::get_gains() const {

template <typename Scalar>
const typename MathBaseTpl<Scalar>::Matrix3s&
ContactModel1DTpl<Scalar>::get_rotation() const {
return rotation_;
ContactModel1DTpl<Scalar>::get_axis_rotation() const {
return Raxis_;
}

template <typename Scalar>
void ContactModel1DTpl<Scalar>::set_reference(const Vector3s& reference) {
void ContactModel1DTpl<Scalar>::set_reference(const Scalar reference) {
xref_ = reference;
}

template <typename Scalar>
void ContactModel1DTpl<Scalar>::set_rotation(const Matrix3s& rotation) {
rotation_ = rotation;
void ContactModel1DTpl<Scalar>::set_axis_rotation(const Matrix3s& rotation) {
Raxis_ = rotation;
}


Expand Down
Loading

0 comments on commit fc4c5e7

Please sign in to comment.