Skip to content

Commit

Permalink
added rotation matrix for contact1D + updated bindings + unittests ac…
Browse files Browse the repository at this point in the history 
…cordingly
  • Loading branch information
@skleff1994
skleff1994 committed Oct 25, 2023
1 parent 3dea282 commit b0d40e7
Show file tree
Hide file tree
Showing 6 changed files with 288 additions and 42 deletions.
29 changes: 23 additions & 6 deletions bindings/python/crocoddyl/multibody/contacts/contact-1d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,27 +30,29 @@ void exposeContact1D() {
"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> >(
bp::args("self", "state", "id", "xref", "type", "nu", "gains"),
bp::optional<Eigen::Vector2d, Eigen::Matrix3d> >(
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"
":param xref: contact position used for the Baumgarte stabilization\n"
":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<boost::shared_ptr<StateMultibody>, pinocchio::FrameIndex,
Eigen::Vector3d, pinocchio::ReferenceFrame,
bp::optional<Eigen::Vector2d> >(
bp::args("self", "state", "id", "xref", "type", "gains"),
bp::optional<Eigen::Vector2d, Eigen::Matrix3d> >(
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 "
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
144 changes: 144 additions & 0 deletions bindings/python/crocoddyl/utils/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down Expand Up @@ -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):
Expand Down
47 changes: 32 additions & 15 deletions include/crocoddyl/multibody/contacts/contact-1d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<StateMultibody> 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
Expand All @@ -66,30 +68,34 @@ 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<StateMultibody> 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 "
"pinocchio::LOCAL",
ContactModel1DTpl(boost::shared_ptr<StateMultibody> 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<StateMultibody> 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();

/**
Expand Down Expand Up @@ -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
*
Expand All @@ -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 <typename _Scalar>
Expand Down
Loading

0 comments on commit b0d40e7

Please sign in to comment.