Skip to content

Commit

Permalink
add python bindings for passivity-based RNEA
Browse files Browse the repository at this point in the history
  • Loading branch information
Cfather committed Oct 2, 2024
1 parent 9701062 commit a5ce8e2
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 0 deletions.
13 changes: 13 additions & 0 deletions bindings/python/algorithm/expose-rnea.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,19 @@ namespace pinocchio
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n",
bp::return_value_policy<bp::return_by_value>());

bp::def(
"passivityRNEA", &passivityRNEA<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs, VectorXs, VectorXs>,
bp::args("model", "data", "q", "v", "v_r", "a_r"),
"Compute the passivity-based RNEA, store the result in Data and return it.\n\n"
"Parameters:\n"
"\tmodel: model of the kinematic tree\n"
"\tdata: data related to the model\n"
"\tq: the joint configuration vector (size model.nq)\n"
"\tv: the joint velocity vector (size model.nv)\n"
"\tv_r: the auxiliary joint velocity vector (size model.nv)\n"
"\ta_r: the auxiliary joint acceleration vector (size model.nv)\n",
bp::return_value_policy<bp::return_by_value>());
}

} // namespace python
Expand Down
8 changes: 8 additions & 0 deletions unittest/python/bindings_rnea.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,14 @@ def test_coriolis_matrix(self):
) - pin.rnea(model, data2, self.q, self.v * 0, self.a * 0)

self.assertApprox(tau_coriolis_ref, C.dot(self.v))

def test_passivity_rnea(self):
model = self.model

tau1 = pin.rnea(self.model, self.data, self.q, self.v, self.a)
tau2 = pin.passivityRNEA(self.model, self.data, self.q, self.v, self.v, self.a)

self.assertApprox(tau1, tau2)


if __name__ == "__main__":
Expand Down

0 comments on commit a5ce8e2

Please sign in to comment.