Verified Commit 7a1c11ab authored by Justin Carpentier's avatar Justin Carpentier
Browse files

python/test: add test for kinematic regressors

parent 45d604e4
Pipeline #11897 passed with stage
in 307 minutes and 11 seconds
......@@ -24,6 +24,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
# Algo
bindings_com
bindings_kinematic_regressor
bindings_regressor
bindings_dynamics
bindings_kinematics
......
import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
class TestKinematicRegressorBindings(TestCase):
def test_all(self):
model = pin.buildSampleModelHumanoidRandom()
joint_name = "larm6_joint"
joint_id = model.getJointId(joint_name)
frame_id = model.addBodyFrame("test_body", joint_id, pin.SE3.Identity(), -1)
data = model.createData()
model.lowerPositionLimit[:7] = -1.
model.upperPositionLimit[:7] = 1.
q = pin.randomConfiguration(model)
pin.forwardKinematics(model,data,q)
R1 = pin.computeJointKinematicRegressor(model,data,joint_id,pin.ReferenceFrame.LOCAL,pin.SE3.Identity())
R2 = pin.computeJointKinematicRegressor(model,data,joint_id,pin.ReferenceFrame.LOCAL)
self.assertApprox(R1,R2)
R3 = pin.computeFrameKinematicRegressor(model,data,frame_id,pin.ReferenceFrame.LOCAL)
self.assertApprox(R1,R3)
if __name__ == '__main__':
unittest.main()
Markdown is supported
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