From b0d40e79fe15049c4e7ac2d13e868d10bbf3fed2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Kleff?= Date: Wed, 25 Oct 2023 14:47:07 -0400 Subject: [PATCH] added rotation matrix for contact1D + updated bindings + unittests accordingly --- .../multibody/contacts/contact-1d.cpp | 29 +++- bindings/python/crocoddyl/utils/__init__.py | 144 ++++++++++++++++++ .../multibody/contacts/contact-1d.hpp | 47 ++++-- .../multibody/contacts/contact-1d.hxx | 48 +++--- unittest/bindings/test_contacts.py | 47 +++++- unittest/factory/contact.cpp | 15 +- 6 files changed, 288 insertions(+), 42 deletions(-) diff --git a/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp b/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp index d12a725cd9..cea32a60ec 100644 --- a/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp +++ b/bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp @@ -30,8 +30,8 @@ void exposeContact1D() { "the derivatives of the holonomic constraint, respectively.", bp::init, pinocchio::FrameIndex, Eigen::Vector3d, pinocchio::ReferenceFrame, std::size_t, - bp::optional >( - bp::args("self", "state", "id", "xref", "type", "nu", "gains"), + bp::optional >( + bp::args("self", "state", "id", "xref", "type", "nu", "gains", "rotation"), "Initialize the contact model.\n\n" ":param state: state of the multibody system\n" ":param id: reference frame id of the contact\n" @@ -39,18 +39,20 @@ void exposeContact1D() { ":param type: type of contact\n" ":param nu: dimension of control vector\n" ":param gains: gains of the contact model (default " - "np.matrix([0.,0.]))")) + "np.matrix([0.,0.]))\n" + ":param rotation: rotation of the reference frame's z axis")) .def(bp::init, pinocchio::FrameIndex, Eigen::Vector3d, pinocchio::ReferenceFrame, - bp::optional >( - bp::args("self", "state", "id", "xref", "type", "gains"), + bp::optional >( + bp::args("self", "state", "id", "xref", "type", "gains", "rotation"), "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.]))")) + "np.matrix([0.,0.]))\n" + ":param rotation: rotation of the reference frame's z axis")) .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 " @@ -85,6 +87,11 @@ void exposeContact1D() { bp::return_internal_reference<>()), &ContactModel1D::set_reference, "reference contact translation") + .add_property("rotation", + bp::make_function(&ContactModel1D::get_rotation, + bp::return_internal_reference<>()), + &ContactModel1D::set_rotation, + "rotation of the reference frame's z axis") .add_property( "gains", bp::make_function(&ContactModel1D::get_gains, @@ -112,6 +119,16 @@ void exposeContact1D() { bp::return_internal_reference<>()), bp::make_setter(&ContactData1D::a0_local), "desired local contact acceleration") + .add_property("a0_skew", + bp::make_getter(&ContactData1D::a0_skew, + bp::return_internal_reference<>()), + bp::make_setter(&ContactData1D::a0_skew), + "contact acceleration skew (local)") + .add_property("a0_world_skew", + bp::make_getter(&ContactData1D::a0_world_skew, + bp::return_internal_reference<>()), + bp::make_setter(&ContactData1D::a0_world_skew), + "contact acceleration skew (world)") .add_property( "dp", bp::make_getter(&ContactData1D::dp, diff --git a/bindings/python/crocoddyl/utils/__init__.py b/bindings/python/crocoddyl/utils/__init__.py index 01fbfc517e..9a135ffc86 100644 --- a/bindings/python/crocoddyl/utils/__init__.py +++ b/bindings/python/crocoddyl/utils/__init__.py @@ -1081,6 +1081,125 @@ def __init__(self, model, collector): self.joint = model.state.pinocchio.frames[model._frame_id].parent +class Contact1DModelDerived(crocoddyl.ContactModelAbstract): + def __init__( + self, state, id, xref, type=pinocchio.ReferenceFrame.LOCAL, gains=[0.0, 0.0], rotation=np.eye(3) + ): + crocoddyl.ContactModelAbstract.__init__(self, state, type, 1) + self.id = id + self.xref = xref + self.gains = gains + self.rotation = rotation + + def calc(self, data, x): + assert self.xref is not None or self.gains[0] == 0.0 + pinocchio.updateFramePlacement(self.state.pinocchio, data.pinocchio, self.id) + data.fJf[:, :] = pinocchio.getFrameJacobian( + self.state.pinocchio, + data.pinocchio, + self.id, + pinocchio.ReferenceFrame.LOCAL, + ) + data.v = pinocchio.getFrameVelocity( + self.state.pinocchio, data.pinocchio, self.id + ) + data.a0_local[:] = pinocchio.getFrameClassicalAcceleration( + self.state.pinocchio, data.pinocchio, self.id, pinocchio.LOCAL + ).linear + + oRf = data.pinocchio.oMf[self.id].rotation + data.vw[:] = data.v.angular + data.vv[:] = data.v.linear + data.dp[:] = data.pinocchio.oMf[self.id].translation - self.xref + data.dp_local[:] = np.dot(oRf.T, data.dp) + + if self.gains[0] != 0.0: + data.a0_local[:] += self.gains[0] * data.dp_local + if self.gains[1] != 0.0: + data.a0_local[:] += self.gains[1] * data.vv + + if self.type == pinocchio.LOCAL: + data.Jc = np.dot(self.rotation, data.fJf[:3, :])[2] + data.a0[0] = np.dot(self.rotation, data.a0_local)[2] + if self.type == pinocchio.WORLD or self.type == pinocchio.LOCAL_WORLD_ALIGNED: + data.Jc = np.dot(np.dot(self.rotation, oRf), data.fJf[:3, :])[2] + data.a0[0] = np.dot(np.dot(self.rotation, oRf), data.a0_local)[2] + + def calcDiff(self, data, x): + joint = self.state.pinocchio.frames[self.id].parent + ( + v_partial_dq, + a_partial_dq, + a_partial_dv, + _, + ) = pinocchio.getJointAccelerationDerivatives( + self.state.pinocchio, data.pinocchio, joint, pinocchio.ReferenceFrame.LOCAL + ) + nv = self.state.nv + data.vv_skew[:, :] = pinocchio.skew(data.vv) + data.vw_skew[:, :] = pinocchio.skew(data.vw) + data.dp_skew[:, :] = pinocchio.skew(data.dp_local) + data.fXjdv_dq[:] = np.dot(data.fXj, v_partial_dq) + data.fXjda_dq[:] = np.dot(data.fXj, a_partial_dq) + data.fXjda_dv[:] = np.dot(data.fXj, a_partial_dv) + data.da0_local_dx[:, :nv] = data.fXjda_dq[:3, :] + data.da0_local_dx[:, :nv] += np.dot(data.vw_skew, data.fXjdv_dq[:3, :]) + data.da0_local_dx[:, :nv] -= np.dot(data.vv_skew, data.fXjdv_dq[3:, :]) + data.da0_local_dx[:, nv:] = data.fXjda_dv[:3, :] + data.da0_local_dx[:, nv:] += np.dot(data.vw_skew, data.fJf[:3, :]) + data.da0_local_dx[:, nv:] -= np.dot(data.vv_skew, data.fJf[3:, :]) + oRf = data.pinocchio.oMf[self.id].rotation + + if self.gains[0] != 0.0: + data.da0_local_dx[:, :nv] += self.gains[0] * np.dot( + data.dp_skew, data.fJf[3:, :] + ) + data.da0_local_dx[:, :nv] += self.gains[0] * data.fJf[:3, :] + if self.gains[1] != 0.0: + data.da0_local_dx[:, :nv] += self.gains[1] * data.fXjdv_dq[:3, :] + data.da0_local_dx[:, nv:] += self.gains[1] * data.fJf[:3, :] + + if self.type == pinocchio.LOCAL: + data.da0_dx[:] = (self.rotation @ data.da0_local_dx)[2,:] + if self.type == pinocchio.WORLD or self.type == pinocchio.LOCAL_WORLD_ALIGNED: + data.a0_local[:] = pinocchio.getFrameClassicalAcceleration( + self.state.pinocchio, data.pinocchio, self.id, pinocchio.LOCAL + ).linear + if self.gains[0] != 0.0: + data.a0_local[:] += self.gains[0] * data.dp_local + if self.gains[1] != 0.0: + data.a0_local[:] += self.gains[1] * data.vv + data.a0[0] = np.dot(np.dot(self.rotation, oRf), data.a0_local)[2] + + data.a0_skew[:, :] = pinocchio.skew(np.dot(np.dot(self.rotation, oRf), data.a0_local)) + data.a0_world_skew = np.dot(data.a0_skew, np.dot(self.rotation, oRf)) + data.da0_dx[:] = np.dot(np.dot(self.rotation, oRf), data.da0_local_dx)[2] + data.da0_dx[:nv] -= np.dot(data.a0_world_skew, data.fJf[3:, :])[2] + + def createData(self, data): + data = Contact1DDataDerived(self, data) + return data + + def updateForce(self, data, force): + assert force.shape[0] == 1 + nv = self.state.nv + data.f.linear[0] = force[0] + data.f.linear[1:] = np.zeros(2) + data.f.angular = np.zeros(3) + if self.type == pinocchio.LOCAL: + data.fext.linear = data.jMf.rotation[:,2] * force[0] + data.fext.angular = np.cross(data.jMf.translation, data.fext.linear); + data.dtau_dq[:, :] = np.zeros((nv, nv)) + if self.type == pinocchio.WORLD or self.type == pinocchio.LOCAL_WORLD_ALIGNED: + oRf = data.pinocchio.oMf[self.id].rotation + data.f_local.linear = oRf.T[:,2] * force[0] + data.f_local.angular = np.zeros(3) + data.fext = data.jMf.act(data.f_local) + data.f_skew[:, :] = pinocchio.skew(data.f_local.linear) + data.fJf_df[:, :] = np.dot(data.f_skew, data.fJf[3:, :]) + data.dtau_dq[:, :] = -np.dot(data.fJf[:3, :].T, data.fJf_df) + + class Contact3DModelDerived(crocoddyl.ContactModelAbstract): def __init__( self, state, id, xref, type=pinocchio.ReferenceFrame.LOCAL, gains=[0.0, 0.0] @@ -1185,6 +1304,31 @@ def updateForce(self, data, force): data.fJf_df[:, :] = np.dot(data.f_skew, data.fJf[3:, :]) data.dtau_dq[:, :] = -np.dot(data.fJf[:3, :].T, data.fJf_df) +class Contact1DDataDerived(crocoddyl.ContactDataAbstract): + def __init__(self, model, data): + crocoddyl.ContactDataAbstract.__init__(self, model, data) + self.jMf = model.state.pinocchio.frames[model.id].placement + self.fXj = self.jMf.inverse().action + self.v = pinocchio.Motion.Zero() + self.a0_local = np.zeros(3) + self.vw = np.zeros(3) + self.vv = np.zeros(3) + self.dp = np.zeros(3) + self.dp_local = np.zeros(3) + self.f_local = pinocchio.Force.Zero() + self.da0_local_dx = np.zeros((3, model.state.ndx)) + self.fJf = np.zeros((6, model.state.nv)) + self.vv_skew = np.zeros((3, 3)) + self.vw_skew = np.zeros((3, 3)) + self.a0_skew = np.zeros((3, 3)) + self.a0_world_skew = np.zeros((3, 3)) + self.dp_skew = np.zeros((3, 3)) + self.f_skew = np.zeros((3, 3)) + self.vw_skew = np.zeros((3, 3)) + self.fXjdv_dq = np.zeros((6, model.state.nv)) + self.fXjda_dq = np.zeros((6, model.state.nv)) + self.fXjda_dv = np.zeros((6, model.state.nv)) + self.fJf_df = np.zeros((3, model.state.nv)) class Contact3DDataDerived(crocoddyl.ContactDataAbstract): def __init__(self, model, data): diff --git a/include/crocoddyl/multibody/contacts/contact-1d.hpp b/include/crocoddyl/multibody/contacts/contact-1d.hpp index a4f33a305b..df1394d6da 100644 --- a/include/crocoddyl/multibody/contacts/contact-1d.hpp +++ b/include/crocoddyl/multibody/contacts/contact-1d.hpp @@ -45,17 +45,19 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { * S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary * Frames: Application to Polishing with Talos, ICHR 2022 * - * @param[in] state State of the multibody system - * @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] nu Dimension of the control vector - * @param[in] gains Baumgarte stabilization gains + * @param[in] state State of the multibody system + * @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] 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 state, const pinocchio::FrameIndex id, const Vector3s& xref, const pinocchio::ReferenceFrame type, const std::size_t nu, - const Vector2s& gains = Vector2s::Zero()); + const Vector2s& gains = Vector2s::Zero(), + const Matrix3s& rotation = Matrix3s::Identity()); /** * @brief Initialize the 1d contact model @@ -66,16 +68,18 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { * S. Kleff et. al, On the Derivation of the Contact Dynamics in Arbitrary * Frames: Application to Polishing with Talos, ICHR 2022 * - * @param[in] state State of the multibody system - * @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] gains Baumgarte stabilization gains + * @param[in] state State of the multibody system + * @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] gains Baumgarte stabilization gains + * @param[in] rotation Rotation of the reference frame's z-axis */ ContactModel1DTpl(boost::shared_ptr state, const pinocchio::FrameIndex id, const Vector3s& xref, const pinocchio::ReferenceFrame type, - const Vector2s& gains = Vector2s::Zero()); + const Vector2s& gains = Vector2s::Zero(), + const Matrix3s& rotation = Matrix3s::Identity()); DEPRECATED( "Use constructor that passes the type type of contact, this assumes is " @@ -83,13 +87,15 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { ContactModel1DTpl(boost::shared_ptr state, const pinocchio::FrameIndex id, const Vector3s& xref, const std::size_t nu, - const Vector2s& gains = Vector2s::Zero());) + const Vector2s& gains = Vector2s::Zero(), + const Matrix3s& rotation = Matrix3s::Identity());) DEPRECATED( "Use constructor that passes the type type of contact, this assumes is " "pinocchio::LOCAL", ContactModel1DTpl(boost::shared_ptr state, const pinocchio::FrameIndex id, const Vector3s& xref, - const Vector2s& gains = Vector2s::Zero());) + const Vector2s& gains = Vector2s::Zero(), + const Matrix3s& rotation = Matrix3s::Identity());) virtual ~ContactModel1DTpl(); /** @@ -137,11 +143,21 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { */ const Vector2s& get_gains() const; + /** + * @brief Return the rotation of the reference frames's z axis + */ + const Matrix3s& get_rotation() const; + /** * @brief Modify the reference frame translation */ void set_reference(const Vector3s& reference); + /** + * @brief Modify the rotation of the reference frames's z axis + */ + void set_rotation(const Matrix3s& rotation); + /** * @brief Print relevant information of the 1d contact model * @@ -159,6 +175,7 @@ class ContactModel1DTpl : public ContactModelAbstractTpl<_Scalar> { private: Vector3s xref_; //!< Contact position used for the Baumgarte stabilization Vector2s gains_; //!< Baumgarte stabilization gains + Matrix3s rotation_; //!< Rotation of the reference frame's z-axis }; template diff --git a/include/crocoddyl/multibody/contacts/contact-1d.hxx b/include/crocoddyl/multibody/contacts/contact-1d.hxx index 9226a75459..d8c22a360b 100644 --- a/include/crocoddyl/multibody/contacts/contact-1d.hxx +++ b/include/crocoddyl/multibody/contacts/contact-1d.hxx @@ -13,8 +13,8 @@ template ContactModel1DTpl::ContactModel1DTpl( boost::shared_ptr state, const pinocchio::FrameIndex id, const Vector3s& xref, const pinocchio::ReferenceFrame type, - const std::size_t nu, const Vector2s& gains) - : Base(state, type, 1, nu), xref_(xref), gains_(gains) { + const std::size_t nu, const Vector2s& gains, const Matrix3s& rotation) + : Base(state, type, 1, nu), xref_(xref), gains_(gains), rotation_(rotation) { id_ = id; } @@ -22,8 +22,8 @@ template ContactModel1DTpl::ContactModel1DTpl( boost::shared_ptr state, const pinocchio::FrameIndex id, const Vector3s& xref, const pinocchio::ReferenceFrame type, - const Vector2s& gains) - : Base(state, type, 1), xref_(xref), gains_(gains) { + const Vector2s& gains, const Matrix3s& rotation) + : Base(state, type, 1), xref_(xref), gains_(gains), rotation_(rotation) { id_ = id; } @@ -34,10 +34,11 @@ ContactModel1DTpl::ContactModel1DTpl( template ContactModel1DTpl::ContactModel1DTpl( boost::shared_ptr state, const pinocchio::FrameIndex id, - const Vector3s& xref, const std::size_t nu, const Vector2s& gains) + const Vector3s& xref, const std::size_t nu, const Vector2s& gains, const Matrix3s& rotation) : Base(state, pinocchio::ReferenceFrame::LOCAL, 1, nu), xref_(xref), - gains_(gains) { + gains_(gains), + rotation_(rotation) { id_ = id; std::cerr << "Deprecated: Use constructor that passes the type of contact, " "this assumes is pinocchio::LOCAL." @@ -47,10 +48,11 @@ ContactModel1DTpl::ContactModel1DTpl( template ContactModel1DTpl::ContactModel1DTpl( boost::shared_ptr state, const pinocchio::FrameIndex id, - const Vector3s& xref, const Vector2s& gains) + const Vector3s& xref, const Vector2s& gains, const Matrix3s& rotation) : Base(state, pinocchio::ReferenceFrame::LOCAL, 1), xref_(xref), - gains_(gains) { + gains_(gains), + rotation_(rotation) { id_ = id; std::cerr << "Deprecated: Use constructor that passes the type of contact, " "this assumes is pinocchio::LOCAL." @@ -90,13 +92,13 @@ void ContactModel1DTpl::calc( } switch (type_) { case pinocchio::ReferenceFrame::LOCAL: - d->Jc.row(0) = d->fJf.row(2); - d->a0[0] = d->a0_local[2]; + d->Jc.row(0) = (rotation_ * d->fJf.template topRows<3>()).row(2); + d->a0[0] = (rotation_ * d->a0_local)[2]; break; case pinocchio::ReferenceFrame::WORLD: case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED: - d->Jc.row(0) = (oRf * d->fJf.template topRows<3>()).row(2); - d->a0[0] = (oRf * d->a0_local)[2]; + d->Jc.row(0) = (rotation_* oRf * d->fJf.template topRows<3>()).row(2); + d->a0[0] = (rotation_ * oRf * d->a0_local)[2]; break; } } @@ -144,7 +146,7 @@ void ContactModel1DTpl::calcDiff( } switch (type_) { case pinocchio::ReferenceFrame::LOCAL: - d->da0_dx.row(0) = d->da0_local_dx.row(2); + d->da0_dx.row(0) = (rotation_ * d->da0_local_dx).row(2); break; case pinocchio::ReferenceFrame::WORLD: case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED: @@ -160,11 +162,11 @@ void ContactModel1DTpl::calcDiff( if (gains_[1] != 0.) { d->a0_local += gains_[1] * d->v.linear(); } - d->a0[0] = (oRf * d->a0_local)[2]; + d->a0[0] = (rotation_ * oRf * d->a0_local)[2]; - pinocchio::skew((oRf * d->a0_local).template head<3>(), d->a0_skew); - d->a0_world_skew.noalias() = d->a0_skew * oRf; - d->da0_dx.row(0) = (oRf * d->da0_local_dx).row(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); d->da0_dx.leftCols(nv).row(0) -= (d->a0_world_skew * d->fJf.template bottomRows<3>()).row(2); break; @@ -229,9 +231,21 @@ ContactModel1DTpl::get_gains() const { return gains_; } +template +const typename MathBaseTpl::Matrix3s& +ContactModel1DTpl::get_rotation() const { + return rotation_; +} + template void ContactModel1DTpl::set_reference(const Vector3s& reference) { xref_ = reference; } +template +void ContactModel1DTpl::set_rotation(const Matrix3s& rotation) { + rotation_ = rotation; +} + + } // namespace crocoddyl diff --git a/unittest/bindings/test_contacts.py b/unittest/bindings/test_contacts.py index 64690dafec..377fb77e20 100644 --- a/unittest/bindings/test_contacts.py +++ b/unittest/bindings/test_contacts.py @@ -7,7 +7,7 @@ import pinocchio import crocoddyl -from crocoddyl.utils import Contact3DModelDerived, Contact6DModelDerived +from crocoddyl.utils import Contact1DModelDerived, Contact3DModelDerived, Contact6DModelDerived class ContactModelAbstractTestCase(unittest.TestCase): @@ -155,6 +155,48 @@ def test_calcDiff(self): ) +class Contact1DLocalTest(ContactModelAbstractTestCase): + ROBOT_MODEL = example_robot_data.load("hyq").model + ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) + + gains = pinocchio.utils.rand(2) + xref = pinocchio.SE3.Random().translation + rotation = pinocchio.SE3.Random().rotation + CONTACT = crocoddyl.ContactModel1D( + ROBOT_STATE, ROBOT_MODEL.getFrameId("lf_foot"), xref, pinocchio.LOCAL, gains, rotation + ) + CONTACT_DER = Contact1DModelDerived( + ROBOT_STATE, ROBOT_MODEL.getFrameId("lf_foot"), xref, pinocchio.LOCAL, gains, rotation + ) + +class Contact1DWorldTest(ContactModelAbstractTestCase): + ROBOT_MODEL = example_robot_data.load("hyq").model + ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) + + gains = pinocchio.utils.rand(2) + xref = pinocchio.SE3.Random().translation + rotation = pinocchio.SE3.Random().rotation + CONTACT = crocoddyl.ContactModel1D( + ROBOT_STATE, ROBOT_MODEL.getFrameId("lf_foot"), xref, pinocchio.WORLD, gains, rotation + ) + CONTACT_DER = Contact1DModelDerived( + ROBOT_STATE, ROBOT_MODEL.getFrameId("lf_foot"), xref, pinocchio.WORLD, gains, rotation + ) + +class Contact1DLocalWorldAlignedTest(ContactModelAbstractTestCase): + ROBOT_MODEL = example_robot_data.load("hyq").model + ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) + + gains = pinocchio.utils.rand(2) + xref = pinocchio.SE3.Random().translation + rotation = pinocchio.SE3.Random().rotation + CONTACT = crocoddyl.ContactModel1D( + ROBOT_STATE, ROBOT_MODEL.getFrameId("lf_foot"), xref, pinocchio.LOCAL_WORLD_ALIGNED, gains, rotation + ) + CONTACT_DER = Contact1DModelDerived( + ROBOT_STATE, ROBOT_MODEL.getFrameId("lf_foot"), xref, pinocchio.LOCAL_WORLD_ALIGNED, gains, rotation + ) + class Contact3DLocalTest(ContactModelAbstractTestCase): ROBOT_MODEL = example_robot_data.load("hyq").model ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) @@ -312,6 +354,9 @@ class Contact6DMultipleTest(ContactModelMultipleAbstractTestCase): if __name__ == "__main__": # test to be run test_classes_to_run = [ + Contact1DLocalTest, + Contact1DWorldTest, + Contact1DLocalWorldAlignedTest, Contact3DLocalTest, Contact3DWorldTest, Contact3DLocalWorldAlignedTest, diff --git a/unittest/factory/contact.cpp b/unittest/factory/contact.cpp index 0b79f5fa40..1456290b9f 100644 --- a/unittest/factory/contact.cpp +++ b/unittest/factory/contact.cpp @@ -84,26 +84,35 @@ boost::shared_ptr ContactModelFactory::create( } switch (contact_type) { case ContactModelTypes::ContactModel1D_LOCAL: + { + pinocchio::SE3 M = pinocchio::SE3::Random(); gains[0] = 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact contact = boost::make_shared( state, frame_id, Eigen::Vector3d::Zero(), - pinocchio::ReferenceFrame::LOCAL, nu, gains); + pinocchio::ReferenceFrame::LOCAL, nu, gains, M.rotation()); break; + } case ContactModelTypes::ContactModel1D_WORLD: + { + pinocchio::SE3 M = pinocchio::SE3::Random(); gains[0] = 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact contact = boost::make_shared( state, frame_id, Eigen::Vector3d::Zero(), - pinocchio::ReferenceFrame::WORLD, nu, gains); + pinocchio::ReferenceFrame::WORLD, nu, gains, M.rotation()); break; + } case ContactModelTypes::ContactModel1D_LWA: + { + pinocchio::SE3 M = pinocchio::SE3::Random(); gains[0] = 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact contact = boost::make_shared( state, frame_id, Eigen::Vector3d::Zero(), - pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains); + pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains, M.rotation()); break; + } case ContactModelTypes::ContactModel2D: gains[0] = 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact