test-op-point-modifier.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST

import unittest

import numpy as np
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier

gaze = np.array(
    (
        (
            (1.0, 0.0, 0.0, 0.025),
            (0.0, 1.0, 0.0, 0.0),
            (0.0, 0.0, 1.0, 0.648),
            (0.0, 0.0, 0.0, 1.0),
        )
    )
)

Jgaze = np.array(
    (
        (
            (1.0, 0.0, 0.0, 0.0, 0.648, 0.0),
            (0.0, 1.0, 0.0, -0.648, 0.0, 0.025),
            (0.0, 0.0, 1.0, 0.0, -0.025, 0.0),
            (0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
            (0.0, 0.0, 0.0, 0.0, 1.0, 0.0),
            (0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
        )
    )
)

I4 = np.array(
    (
        (1.0, 0.0, 0.0, 0.0),
        (0.0, 1.0, 0.0, 0.0),
        (0.0, 0.0, 1.0, 0.0),
        (0.0, 0.0, 0.0, 1.0),
    )
)

I6 = np.array(
    (
        (1.0, 0.0, 0.0, 0.0, 0.0, 0.0),
        (0.0, 1.0, 0.0, 0.0, 0.0, 0.0),
        (0.0, 0.0, 1.0, 0.0, 0.0, 0.0),
        (0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
        (0.0, 0.0, 0.0, 0.0, 1.0, 0.0),
        (0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
    )
)


class OpPointModifierTest(unittest.TestCase):
    def test_simple(self):
        op = OpPointModifier("op")
        op.setTransformation(I4)
        op.positionIN.value = I4
        op.jacobianIN.value = I6
        op.position.recompute(0)
        op.jacobian.recompute(0)

        self.assertTrue((op.getTransformation() == I4).all())
        self.assertTrue((op.position.value == I4).all())
        self.assertTrue((op.jacobian.value == I6).all())

    def test_translation(self):
        tx = 11.0
        ty = 22.0
        tz = 33.0

        T = np.array(
            (
                (1.0, 0.0, 0.0, tx),
                (0.0, 1.0, 0.0, ty),
                (0.0, 0.0, 1.0, tz),
                (0.0, 0.0, 0.0, 1.0),
            )
        )

        op = OpPointModifier("op2")
        op.setTransformation(T)
        op.positionIN.value = gaze
        op.jacobianIN.value = Jgaze
        op.position.recompute(1)
        op.jacobian.recompute(1)

        self.assertTrue((op.getTransformation() == T).all())

        # w_M_s = w_M_g * g_M_s
        w_M_g = gaze
        g_M_s = T
        w_M_s_ref = w_M_g.dot(g_M_s)
        w_M_s = op.position.value

        # Check w_M_s == w_M_s_ref
        self.assertTrue((w_M_s == w_M_s_ref).all())

        twist = np.array(
            [
                [1.0, 0.0, 0.0, 0.0, tz, -ty],
                [0.0, 1.0, 0.0, -tz, 0.0, tx],
                [0.0, 0.0, 1.0, ty, -tx, 0.0],
                [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
            ]
        )

        J = op.jacobian.value
        J_ref = twist.dot(Jgaze)

        # Check w_M_s == w_M_s_ref
        self.assertTrue((J == J_ref).all())

    def test_rotation(self):
        T = np.array(
            (
                (0.0, 0.0, 1.0, 0.0),
                (0.0, -1.0, 0.0, 0.0),
                (1.0, 0.0, 0.0, 0.0),
                (0.0, 0.0, 0.0, 1.0),
            )
        )

        op = OpPointModifier("op3")
        op.setTransformation(T)
        op.positionIN.value = gaze
        op.jacobianIN.value = Jgaze
        op.position.recompute(1)
        op.jacobian.recompute(1)

        self.assertTrue((op.getTransformation() == T).all())

        # w_M_s = w_M_g * g_M_s
        w_M_g = gaze
        g_M_s = T
        w_M_s_ref = w_M_g.dot(g_M_s)
        w_M_s = op.position.value

        # Check w_M_s == w_M_s_ref
        self.assertTrue((w_M_s == w_M_s_ref).all())

        twist = np.array(
            [
                [0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
                [0.0, -1.0, 0.0, 0.0, 0.0, 0.0],
                [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
                [0.0, 0.0, 0.0, 0.0, -1.0, 0.0],
                [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
            ]
        )

        J = op.jacobian.value
        J_ref = twist.dot(Jgaze)

        # Check w_M_s == w_M_s_ref
        self.assertTrue((J == J_ref).all())

    def test_rotation_translation(self):
        tx = 11.0
        ty = 22.0
        tz = 33.0

        T = np.array(
            (
                (0.0, 0.0, 1.0, tx),
                (0.0, -1.0, 0.0, ty),
                (1.0, 0.0, 0.0, tz),
                (0.0, 0.0, 0.0, 1.0),
            )
        )

        op = OpPointModifier("op4")
        op.setTransformation(T)
        op.positionIN.value = gaze
        op.jacobianIN.value = Jgaze
        op.position.recompute(1)
        op.jacobian.recompute(1)

        self.assertTrue((op.getTransformation() == T).all())

        # w_M_s = w_M_g * g_M_s
        w_M_g = gaze
        g_M_s = T
        w_M_s_ref = w_M_g.dot(g_M_s)
        w_M_s = op.position.value

        # Check w_M_s == w_M_s_ref
        self.assertTrue((w_M_s == w_M_s_ref).all())

        twist = np.array(
            [
                [0.0, 0.0, 1.0, ty, -tx, 0.0],
                [0.0, -1.0, 0.0, tz, 0.0, -tx],
                [1.0, 0.0, 0.0, 0.0, tz, -ty],
                [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
                [0.0, 0.0, 0.0, 0.0, -1.0, 0.0],
                [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
            ]
        )

        J = op.jacobian.value
        J_ref = twist.dot(Jgaze)

        # Check w_M_s == w_M_s_ref
        self.assertTrue((J == J_ref).all())


if __name__ == "__main__":
    unittest.main()