Verified Commit f3345a2d authored by Gabriele Buondonno's avatar Gabriele Buondonno Committed by Justin Carpentier
Browse files

[bindings] Fix bindings for getFrameVelocity

parent b042a796
......@@ -68,6 +68,8 @@ namespace pinocchio
return get_frame_jacobian_time_variation_proxy(model, data, frame_id, rf);
}
BOOST_PYTHON_FUNCTION_OVERLOADS(getFrameVelocity_overload, (&getFrameVelocity<double,0,JointCollectionDefaultTpl>), 3, 4)
void exposeFramesAlgo()
{
using namespace Eigen;
......@@ -86,9 +88,10 @@ namespace pinocchio
bp::def("getFrameVelocity",
&getFrameVelocity<double,0,JointCollectionDefaultTpl>,
bp::args("model","data","frame_id"),
"Returns the spatial velocity of the frame expressed in the coordinates system of the Frame itself.\n"
"forwardKinematics(model,data,q,v,[a]).");
getFrameVelocity_overload(
bp::args("model","data","frame_id","reference_frame"),
"Returns the spatial velocity of the frame expressed in the coordinate system given by reference_frame.\n"
"forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint spatial velocity stored in data.v"));
bp::def("getFrameAcceleration",
&getFrameAcceleration<double,0,JointCollectionDefaultTpl>,
......
......@@ -2,7 +2,9 @@ import unittest
import pinocchio as pin
import numpy as np
class TestFrameBindings(unittest.TestCase):
from test_case import PinocchioTestCase
class TestFrameBindings(PinocchioTestCase):
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
......@@ -42,5 +44,24 @@ class TestFrameBindings(unittest.TestCase):
f.placement = new_placement
self.assertTrue(np.allclose(f.placement.homogeneous, new_placement.homogeneous))
def test_getters(self):
data = self.model.createData()
q = pin.randomConfiguration(self.model)
v = np.random.rand(self.model.nv)
a = np.random.rand(self.model.nv)
pin.forwardKinematics(self.model, data, q, v, a)
T = pin.updateFramePlacement(self.model, data, self.frame_idx)
self.assertApprox(T, data.oMi[self.parent_idx].act(self.frame_placement))
v = pin.getFrameVelocity(self.model, data, self.frame_idx)
self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL)
self.assertApprox(v, self.frame_placement.actInv(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.WORLD)
self.assertApprox(v, data.oMi[self.parent_idx].act(data.v[self.parent_idx]))
v = pin.getFrameVelocity(self.model, data, self.frame_idx, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
self.assertApprox(v, pin.SE3(T.rotation, np.zeros(3)).act(self.frame_placement.actInv(data.v[self.parent_idx])))
if __name__ == '__main__':
unittest.main()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment