Allowing WORLD and LOCAL_WORLD_ALIGNED in ContactModel1D #1054

skleff1994 · 2022-03-22T17:44:34Z

Minor enhancements for the 1D contact model . Related to #812

Add a mask for constraint dimension selection . The user selects optionally type in {0,1,2} for {x,y,z} the z-axis being the default one
Add optional argument of pinocchio reference frame in {LOCAL, WORLD , LOCAL_WORLD_ALIGNED}, LOCAL being the default one

The mask could be extended to 2D contact , and the pinocchio reference frame option could be propagated to other contact models , if deemed useful . But I think all of this will be handled in Pinocchio in the future...

…contact constraint axis

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

jcarpent · 2022-03-22T19:31:53Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

-               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"),


Set default argument here.

Also rename the arguments as follows:

type to mask

pinRefFrame to type

This latter follows the standard used in other classes.

@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?

@cmastalli addressed in fd58261

jcarpent · 2022-03-22T19:32:53Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

+                                             const Scalar xref, 
+                                             const std::size_t nu, 
+                                             const Vector2s& gains, 
+                                             const std::size_t& type, 


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

Better, please define an enum for type.

addressed in fd58261

jcarpent · 2022-03-22T19:33:56Z

include/crocoddyl/multibody/contacts/contact-1d.hxx


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


The WORLD case has no physical meaning I would say.

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 ?

cmastalli

There are still many changes to be tackled.
However, I don't understand why you need to define a reference frame that is not LOCAL.
These classes are not aimed to track a reference acceleration for instance.
So, why?

cmastalli · 2022-03-23T23:41:12Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

-               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"),


Also rename the arguments as follows:

type to mask

pinRefFrame to type

This latter follows the standard used in other classes.

cmastalli · 2022-03-23T23:41:41Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

+          ":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}"))


Please adapt the documentation as suggested above

addressed in fd58261

cmastalli · 2022-03-23T23:41:54Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

          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"))


Revert this back

addressed in fd58261

cmastalli · 2022-03-23T23:42:44Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

@@ -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",


Renamed this as suggested above

Add the getter and setter for the mask

addressed in fd58261

cmastalli · 2022-03-23T23:43:43Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

+                    "contact gains")
+      .add_property("pinReferenceFrame",
+                    bp::make_function(&ContactModel1D::get_pinReferenceFrame, bp::return_value_policy<bp::return_by_value>()),
+                    &ContactModel1D::set_pinReferenceFrame, "pin.ReferenceFrame");


Change documentation to reference type of contact

addressed in fd58261

cmastalli · 2022-03-23T23:44:22Z

include/crocoddyl/multibody/contacts/contact-1d.hpp

@@ -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;


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

addressed in fd58261

cmastalli · 2022-03-23T23:47:12Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

+                                             const pinocchio::FrameIndex id,
+                                             const Scalar xref, 
+                                             const Vector2s& gains,
+                                             const pinocchio::ReferenceFrame pinRefFrame)


Change the name of the arguments as suggested above.

addressed in fd58261

cmastalli · 2022-03-23T23:47:29Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

+                                             const Vector2s& gains, 
+                                             const std::size_t& type, 
+                                             const pinocchio::ReferenceFrame pinRefFrame)
+    : Base(state, 1, nu), xref_(xref), gains_(gains), type_(type), pinReferenceFrame_(pinRefFrame) {


Change the name of the arguments as suggested above

addressed in fd58261

cmastalli · 2022-03-23T23:47:56Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

  id_ = id;
+  if(type_ < 0 || type_ > 2){


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

addressed in fd58261

cmastalli · 2022-03-23T23:50:04Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

+                                             const Scalar xref, 
+                                             const std::size_t nu, 
+                                             const Vector2s& gains, 
+                                             const std::size_t& type, 


Better, please define an enum for type.

skleff1994 · 2022-03-24T13:02:40Z

Thanks for your review , I will now implement some of the requested changes.

I also noticed that the contact force residual computation needs to be updated as well , and therefore be aware of the mask of the contact model .

@cmastalli the purpose of this enhancement is to allow force computations in the WORLD frame, which is necessary for e.g. sanding tasks where the desired force is defined in WORLD (the LOCAL z-axis is not necessarily aligned with the vertical direction). In that case , I believe we need to perform all contact-related computations in LOCAL_WORLD_ALIGNED , which is currently not allowed by Crocoddyl, as proposed in #812

…g names to mask, type + added getter/setter for mask + defined & binded enum for mask + formated code changed arg names into mask, type + updated doc + defined enum for mask corrected mistake ref -> type added enum in bindings removed default pinocchio reference arg in bindings causing unittest to fail and import error in python added getter setter for mask + formatted code

skleff1994 · 2022-03-24T18:24:01Z

Concerning the force residual, I could add the mask to ContactData1D so that it can be accessed from the force residual . Also , I can add the pinocchio reference as an optional argument in force residual constructor so we can compute the 1D force residual in any frame . From there, this could be straightforwardly extended to other contact models I believe . What do you think ?

cmastalli

Please see all the comments below.
A part of them, we also need to unittest the different mask cases.
To do so, you need to extend the contact factory.

cmastalli · 2022-03-31T08:52:02Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

+  bp::enum_<Contact1DMaskType>("Contact1DMaskType")
+      .value("X_MASK", X_MASK)
+      .value("Y_MASK", Y_MASK)
+      .value("Z_MASK", Z_MASK)
+      .export_values();


Let's make this class a bit more generic.

Suggested change

bp::enum_<Contact1DMaskType>("Contact1DMaskType")

.value("X_MASK", X_MASK)

.value("Y_MASK", Y_MASK)

.value("Z_MASK", Z_MASK)

.export_values();

bp::enum_<Vector3MaskType>("Vector3MaskType")

.value("x", x)

.value("y", y)

.value("z", z)

.export_values();

cmastalli · 2022-03-31T08:53:15Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

      "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, Contact1DMaskType, pinocchio::ReferenceFrame> >(


You will need to adapt the code as now the class is Vector3MaskType.

cmastalli · 2022-03-31T08:53:15Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

      "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, Contact1DMaskType, pinocchio::ReferenceFrame> >(


You will need to adapt the code as now the class is Vector3MaskType.

cmastalli · 2022-03-31T08:55:10Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

-          ":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 mask: axis of the contact constraint (0=x, 1=y or 2=z)\n"


Suggested change

":param mask: axis of the contact constraint (0=x, 1=y or 2=z)\n"

":param mask: axis of the contact constraint (default z)\n"

cmastalli · 2022-03-31T08:55:10Z

bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp

-          ":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 mask: axis of the contact constraint (0=x, 1=y or 2=z)\n"


Suggested change

":param mask: axis of the contact constraint (0=x, 1=y or 2=z)\n"

":param mask: axis of the contact constraint (default z)\n"

cmastalli · 2022-03-31T09:04:06Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

+      break;
+    }
+    case pinocchio::LOCAL_WORLD_ALIGNED: {
+      jMc = pinocchio::SE3(Matrix3s::Identity(), d->jMf.translation());


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

cmastalli · 2022-03-31T09:04:49Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

-  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;


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

cmastalli · 2022-03-31T09:04:49Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

-  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;


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

cmastalli · 2022-03-31T09:05:48Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

+
+template <typename Scalar>
+void ContactModel1DTpl<Scalar>::set_mask(const Contact1DMaskType mask) {
+  mask_ = mask;


If you change to LOCAL_WORLD_ALIGNED, then define the identity matrix as pointed out above.

cmastalli · 2022-03-31T09:05:48Z

include/crocoddyl/multibody/contacts/contact-1d.hxx

+
+template <typename Scalar>
+void ContactModel1DTpl<Scalar>::set_mask(const Contact1DMaskType mask) {
+  mask_ = mask;


If you change to LOCAL_WORLD_ALIGNED, then define the identity matrix as pointed out above.

…cted mistakes in contact1d calcdiff transformations

skleff1994 · 2022-03-31T16:14:25Z

Thanks for the review , I addressed the requested changes in 1fecfb4b .

In addition to those , I implemented

force computation using mask and reference type of contact 1D (copied in data for that purpose)
specific unittest for 1D contact

Currently, the unittests pass only for LOCAL and WORLD frames. There seem to be a mistake in calcDiff for LOCAL_WORLD_ALIGNED . I can't find it...

skleff1994 · 2022-04-01T13:37:19Z

So we wrote down the maths with @nmansard and derived correct LOCAL_WORLD_ALIGNED derivatives for acceleration, I will fix the current implementation asap.

Also @jcarpent since the WORLD case doesn't make sense as you say I can remove it from the API. But then I think we should also clarify the Baumgarte terms: is there a reason why they are expressed in WORLD currently?

cmastalli · 2023-05-22T18:05:34Z

I will close this issue due to a lack of activity. @skleff1994 -- feel free to re-open it anytime you wish.

skleff1994 added 2 commits March 22, 2022 18:21

added pinocchio reference frame option to contact1D + mask to select …

a7e87c8

…contact constraint axis

fix mistake in contact1d force computation

31acf4a

jcarpent requested changes Mar 22, 2022

View reviewed changes

cmastalli requested changes Mar 23, 2022

View reviewed changes

cmastalli requested changes Mar 31, 2022

View reviewed changes

skleff1994 added 2 commits March 31, 2022 18:01

generalized enum + added unittest + updated force computation + corre…

1fecfb4

…cted mistakes in contact1d calcdiff transformations

formatting

44b65f3

skleff1994 force-pushed the devel_upstream_contactReferenceFrame branch from d743533 to 44b65f3 Compare March 31, 2022 16:44

cmastalli closed this May 22, 2023

skleff1994 mentioned this pull request Oct 16, 2023

Added LWA frame convention in Contact1D #1172

Merged

		":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}"))

	":param mask: axis of the contact constraint (0=x, 1=y or 2=z)\n"
	":param mask: axis of the contact constraint (default z)\n"

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

Allowing WORLD and LOCAL_WORLD_ALIGNED in ContactModel1D #1054

Allowing WORLD and LOCAL_WORLD_ALIGNED in ContactModel1D #1054

Conversation

skleff1994 commented Mar 22, 2022 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

skleff1994 Mar 24, 2022 • edited

Choose a reason for hiding this comment

cmastalli left a comment • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

skleff1994 commented Mar 24, 2022 • edited

skleff1994 commented Mar 24, 2022 • edited

cmastalli left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

skleff1994 commented Mar 31, 2022 • edited

skleff1994 commented Apr 1, 2022

cmastalli commented May 22, 2023

skleff1994 commented Mar 22, 2022 •

edited

skleff1994 Mar 24, 2022 •

edited

cmastalli left a comment •

edited

skleff1994 commented Mar 24, 2022 •

edited

skleff1994 commented Mar 24, 2022 •

edited

skleff1994 commented Mar 31, 2022 •

edited