Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allowing WORLD and LOCAL_WORLD_ALIGNED in ContactModel1D #1054

Closed
wants to merge 5 commits into from
Closed

Allowing WORLD and LOCAL_WORLD_ALIGNED in ContactModel1D #1054

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a7e87c8
added pinocchio reference frame option to contact1D + mask to select …
skleff1994 Mar 22, 2022
31acf4a
fix mistake in contact1d force computation
skleff1994 Mar 22, 2022
fd58261
removed redundant def of pinocchio ref frame in bindings + changed ar…
skleff1994 Mar 23, 2022
1fecfb4
generalized enum + added unittest + updated force computation + corre…
skleff1994 Mar 29, 2022
44b65f3
formatting
skleff1994 Mar 31, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 17 additions & 6 deletions bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,12 @@ namespace python {
void exposeContact1D() {
bp::register_ptr_to_python<boost::shared_ptr<ContactModel1D> >();

bp::enum_<pinocchio::ReferenceFrame>("ReferenceFrame")
cmastalli marked this conversation as resolved.
Show resolved Hide resolved
.value("LOCAL", pinocchio::LOCAL)
.value("WORLD", pinocchio::WORLD)
.value("LOCAL_WORLD_ALIGNED", pinocchio::LOCAL_WORLD_ALIGNED)
.export_values();

bp::class_<ContactModel1D, bp::bases<ContactModelAbstract> >(
"ContactModel1D",
"Rigid 1D contact model.\n\n"
Expand All @@ -24,21 +30,23 @@ void exposeContact1D() {
"The calc and calcDiff functions compute the contact Jacobian and drift (holonomic constraint) or\n"
"the derivatives of the holonomic constraint, respectively.",
bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex, double, std::size_t,
bp::optional<Eigen::Vector2d> >(
bp::args("self", "state", "id", "xref", "nu", "gains"),
bp::optional<Eigen::Vector2d, std::size_t, pinocchio::ReferenceFrame> >(
bp::args("self", "state", "id", "xref", "nu", "gains", "type", "pinRefFrame"),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Set default argument here.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also rename the arguments as follows:

  • type to mask
  • pinRefFrame to type

This latter follows the standard used in other classes.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jcarpent concerning the default argument I had

( bp::args("self", "state", "id", "xref", "nu", "gains", "type"), bp::arg("pinRefFrame")=pinocchio::LOCAL ),

but it seemed to make python unittest fail and caused an import error in python, I couldn't figure out why . I need to investigate this . Any clue?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@cmastalli addressed in fd58261

"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 nu: dimension of control vector\n"
":param gains: gains of the contact model (default np.matrix([0.,0.]))"))
.def(bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex, double, bp::optional<Eigen::Vector2d> >(
":param gains: gains of the contact model (default np.matrix([0.,0.]))\n"
":param type: axis of the contact constraint (0=x, 1=y or 2=z)\n"
":param pinRefFrame: pin.ReferenceFrame in {LOCAL, WORLD, LOCAL_WORLD_ALIGNED}"))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please adapt the documentation as suggested above

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261

.def(bp::init<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex, double, bp::optional<Eigen::Vector2d, pinocchio::ReferenceFrame> >(
bp::args("self", "state", "id", "xref", "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 gains: gains of the contact model (default np.matrix([0.,0.]))"))
":param gains: gains of the contact model (default np.matrix([0.,0.]))\n"))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Revert this back

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261

.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 constraint\n"
Expand Down Expand Up @@ -68,7 +76,10 @@ void exposeContact1D() {
&ContactModel1D::set_reference, "reference contact translation")
.add_property("gains",
bp::make_function(&ContactModel1D::get_gains, bp::return_value_policy<bp::return_by_value>()),
"contact gains");
"contact gains")
.add_property("pinReferenceFrame",
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  1. Renamed this as suggested above
  2. Add the getter and setter for the mask

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261

bp::make_function(&ContactModel1D::get_pinReferenceFrame, bp::return_value_policy<bp::return_by_value>()),
&ContactModel1D::set_pinReferenceFrame, "pin.ReferenceFrame");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Change documentation to reference type of contact

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261


bp::register_ptr_to_python<boost::shared_ptr<ContactData1D> >();

Expand Down
26 changes: 22 additions & 4 deletions include/crocoddyl/multibody/contacts/contact-1d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
typedef typename MathBase::Vector3s Vector3s;
typedef typename MathBase::VectorXs VectorXs;
typedef typename MathBase::Matrix3s Matrix3s;
typedef typename MathBase::Vector6s Vector6s;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove this line. It doesn't seem to be needed

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261


/**
* @brief Initialize the 1d contact model
Expand All @@ -48,7 +49,8 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
* @param[in] gains Baumgarte stabilization gains
*/
ContactModel1DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Scalar xref,
const std::size_t nu, const Vector2s& gains = Vector2s::Zero());
const std::size_t nu, const Vector2s& gains = Vector2s::Zero(), const std::size_t& type = 2,
const pinocchio::ReferenceFrame = pinocchio::LOCAL);

/**
* @brief Initialize the 1d contact model
Expand All @@ -61,7 +63,8 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
* @param[in] gains Baumgarte stabilization gains
*/
ContactModel1DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Scalar xref,
const Vector2s& gains = Vector2s::Zero());
const Vector2s& gains = Vector2s::Zero(),
const pinocchio::ReferenceFrame = pinocchio::LOCAL);

virtual ~ContactModel1DTpl();

Expand Down Expand Up @@ -111,6 +114,18 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
*/
void set_reference(const Scalar reference);


/**
* @brief Modify pinocchio::ReferenceFrame
*/
void set_pinReferenceFrame(const pinocchio::ReferenceFrame);

/**
* @brief Get pinocchio::ReferenceFrame
*/
const pinocchio::ReferenceFrame get_pinReferenceFrame() const;


/**
* @brief Print relevant information of the 1d contact model
*
Expand All @@ -125,8 +140,11 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> {
using Base::state_;

private:
Scalar xref_; //!< Contact position used for the Baumgarte stabilization
Vector2s gains_; //!< Baumgarte stabilization gains
Scalar xref_; //!< Contact position used for the Baumgarte stabilization
Vector2s gains_; //!< Baumgarte stabilization gains
Vector3s mask_; //!< Projection matrix selecting contact constraint direction in LOCAL frame
std::size_t type_; //!< Type of the 1D contact in {0,1,2} for {'x','y','z'}
pinocchio::ReferenceFrame pinReferenceFrame_; //!< Pinocchio reference frame
};

template <typename _Scalar>
Expand Down
104 changes: 76 additions & 28 deletions include/crocoddyl/multibody/contacts/contact-1d.hxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,17 +9,36 @@
namespace crocoddyl {

template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
const Scalar xref, const std::size_t nu, const Vector2s& gains)
: Base(state, 1, nu), xref_(xref), gains_(gains) {
ContactModel1DTpl<Scalar>::ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex id,
const Scalar xref,
const std::size_t nu,
const Vector2s& gains,
const std::size_t& type,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The reference is not needed as it is a basic type.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Better, please define an enum for type.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261

const pinocchio::ReferenceFrame pinRefFrame)
: Base(state, 1, nu), xref_(xref), gains_(gains), type_(type), pinReferenceFrame_(pinRefFrame) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Change the name of the arguments as suggested above

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261

id_ = id;
if(type_ < 0 || type_ > 2){
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This doesn't follow the code format. Please run the code formatter as described in the wiki

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261

throw_pretty("Invalid argument: "
<< "Contact1D type must be in {0,1,2} for {x,y,z}");
}
mask_ = Vector3s::Zero();
mask_[type] = 1;
}

template <typename Scalar>
ContactModel1DTpl<Scalar>::ContactModel1DTpl(boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
const Scalar xref, const Vector2s& gains)
: Base(state, 1), xref_(xref), gains_(gains) {
ContactModel1DTpl<Scalar>::ContactModel1DTpl(boost::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex id,
const Scalar xref,
const Vector2s& gains,
const pinocchio::ReferenceFrame pinRefFrame)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Change the name of the arguments as suggested above.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

addressed in fd58261

: Base(state, 1), xref_(xref), gains_(gains), pinReferenceFrame_(pinRefFrame){
id_ = id;
// Default type is 2
type_ = 2;
mask_ = Vector3s::Zero();
mask_[type_] = 1;

}

template <typename Scalar>
Expand All @@ -30,22 +49,25 @@ void ContactModel1DTpl<Scalar>::calc(const boost::shared_ptr<ContactDataAbstract
const Eigen::Ref<const VectorXs>&) {
Data* d = static_cast<Data*>(data.get());
pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio, id_);
pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL, d->fJf);
d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(), *d->pinocchio, id_);
d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(), *d->pinocchio, id_);

d->Jc.row(0) = d->fJf.row(2);
pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio, id_, pinReferenceFrame_, d->fJf);
d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(), *d->pinocchio, id_, pinReferenceFrame_);
d->a = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(), *d->pinocchio, id_, pinReferenceFrame_);

d->Jc.row(0) = d->fJf.row(type_);
d->vw = d->v.angular();
d->vv = d->v.linear();

d->a0[0] = d->a.linear()[2] + d->vw[0] * d->vv[1] - d->vw[1] * d->vv[0];
d->a0[0] = mask_.dot( d->a.linear() + d->vw.cross(d->vv) );

if (gains_[0] != 0.) {
d->a0[0] += gains_[0] * (d->pinocchio->oMf[id_].translation()[2] - xref_);
if(pinReferenceFrame_ == pinocchio::WORLD){
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The WORLD case has no physical meaning I would say.

Copy link
Contributor Author

@skleff1994 skleff1994 Mar 24, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe that Baumgarte stabilizing term is already expressed in WORLD for all contact models - which I also don't really understand as the acceleration and force computations are currently in LOCAL... Wouldn't it make sense to compute the Baumgarte terms in the pinocchio reference frame ?

d->a0[0] += gains_[0] * (d->pinocchio->oMf[id_].translation()[type_] - xref_);
}
else if (pinReferenceFrame_ == pinocchio::LOCAL || pinReferenceFrame_ == pinocchio::LOCAL_WORLD_ALIGNED){
d->a0[0] += - gains_[0] * xref_;
}
}
if (gains_[1] != 0.) {
d->a0[0] += gains_[1] * d->vv[2];
d->a0[0] += gains_[1] * d->vv[type_];
}
}

Expand All @@ -54,7 +76,7 @@ void ContactModel1DTpl<Scalar>::calcDiff(const boost::shared_ptr<ContactDataAbst
const Eigen::Ref<const VectorXs>&) {
Data* d = static_cast<Data*>(data.get());
const pinocchio::JointIndex joint = state_->get_pinocchio()->frames[d->frame].parent;
pinocchio::getJointAccelerationDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
pinocchio::getJointAccelerationDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, joint, pinReferenceFrame_,
d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
const std::size_t nv = state_->get_nv();
pinocchio::skew(d->vv, d->vv_skew);
Expand All @@ -63,22 +85,22 @@ void ContactModel1DTpl<Scalar>::calcDiff(const boost::shared_ptr<ContactDataAbst
d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;

d->da0_dx.leftCols(nv).row(0).noalias() = d->fXjda_dq.row(2);
d->da0_dx.leftCols(nv).row(0).noalias() += d->vw_skew.row(2) * d->fXjdv_dq.template topRows<3>();
d->da0_dx.leftCols(nv).row(0).noalias() -= d->vv_skew.row(2) * d->fXjdv_dq.template bottomRows<3>();
d->da0_dx.leftCols(nv).row(0) = d->fXjda_dq.row(type_);
d->da0_dx.leftCols(nv).row(0).noalias() += d->vw_skew.row(type_) * d->fXjdv_dq.template topRows<3>();
d->da0_dx.leftCols(nv).row(0).noalias() -= d->vv_skew.row(type_) * d->fXjdv_dq.template bottomRows<3>();

d->da0_dx.rightCols(nv).row(0).noalias() = d->fXjda_dv.row(2);
d->da0_dx.rightCols(nv).row(0).noalias() += d->vw_skew.row(2) * d->fJf.template topRows<3>();
d->da0_dx.rightCols(nv).row(0).noalias() -= d->vv_skew.row(2) * d->fJf.template bottomRows<3>();
d->da0_dx.rightCols(nv).row(0) = d->fXjda_dv.row(type_);
d->da0_dx.rightCols(nv).row(0).noalias() += d->vw_skew.row(type_) * d->fJf.template topRows<3>();
d->da0_dx.rightCols(nv).row(0).noalias() -= d->vv_skew.row(type_) * d->fJf.template bottomRows<3>();

if (gains_[0] != 0.) {
const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
d->oRf(0, 0) = oRf(2, 2);
d->oRf(0, 0) = oRf(type_, type_);
d->da0_dx.leftCols(nv).noalias() += gains_[0] * d->oRf * d->Jc;
}
if (gains_[1] != 0.) {
d->da0_dx.leftCols(nv).row(0).noalias() += gains_[1] * d->fXj.row(2) * d->v_partial_dq;
d->da0_dx.rightCols(nv).row(0).noalias() += gains_[1] * d->fXj.row(2) * d->a_partial_da;
d->da0_dx.leftCols(nv).row(0).noalias() += gains_[1] * d->fXj.row(type_) * d->v_partial_dq;
d->da0_dx.rightCols(nv).row(0).noalias() += gains_[1] * d->fXj.row(type_) * d->a_partial_da;
}
}

Expand All @@ -90,9 +112,23 @@ void ContactModel1DTpl<Scalar>::updateForce(const boost::shared_ptr<ContactDataA
<< "lambda has wrong dimension (it should be 1)");
}
Data* d = static_cast<Data*>(data.get());
const Eigen::Ref<const Matrix3s> R = d->jMf.rotation();
data->f.linear() = R.col(2) * force[0];
data->f.angular() = d->jMf.translation().cross(data->f.linear());
pinocchio::SE3 jMc;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's define a global variable for this, and called it jMc_.
This avoids to allocate data dynamically.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's define a global variable for this, and called it jMc_.
This avoids to allocate data dynamically.

switch( pinReferenceFrame_ ){
case pinocchio::LOCAL: {
jMc = d->jMf;
break;
}
case pinocchio::WORLD: {
jMc = d->jMf.act(d->pinocchio->oMf[id_].inverse());
break;
}
case pinocchio::LOCAL_WORLD_ALIGNED: {
jMc = pinocchio::SE3(Matrix3s::Identity(), d->jMf.translation());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is allocating data dynamically.
Instead, you should modify the translation only, i.e.

Suggested change
jMc = pinocchio::SE3(Matrix3s::Identity(), d->jMf.translation());
jMc_.translation = d->jMf.translation();

Then, you have to set the identity matrix only when you have define LOCAL_WORLD_ALIGNED

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is allocating data dynamically.
Instead, you should modify the translation only, i.e.

Suggested change
jMc = pinocchio::SE3(Matrix3s::Identity(), d->jMf.translation());
jMc_.translation = d->jMf.translation();

Then, you have to set the identity matrix only when you have define LOCAL_WORLD_ALIGNED

break;
}
}
data->f.linear() = jMc.rotation().col(type_) * force[0];
data->f.angular() = jMc.translation().cross(data->f.linear());
}

template <typename Scalar>
Expand Down Expand Up @@ -121,4 +157,16 @@ void ContactModel1DTpl<Scalar>::set_reference(const Scalar reference) {
xref_ = reference;
}


template <typename Scalar>
void ContactModel1DTpl<Scalar>::set_pinReferenceFrame(const pinocchio::ReferenceFrame reference) {
pinReferenceFrame_ = reference;
}

template <typename Scalar>
const pinocchio::ReferenceFrame ContactModel1DTpl<Scalar>::get_pinReferenceFrame() const {
return pinReferenceFrame_;
}


} // namespace crocoddyl