# Copyright 2019 The dm_control Authors.

#

# Licensed under the Apache License, Version 2.0 (the "License");

# you may not use this file except in compliance with the License.

# You may obtain a copy of the License at

#

# http://www.apache.org/licenses/LICENSE-2.0

#

# Unless required by applicable law or agreed to in writing, software

# distributed under the License is distributed on an "AS IS" BASIS,

# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

# See the License for the specific language governing permissions and

# limitations under the License.

# ============================================================================

"""Tests for dm_control.locomotion.walkers.base."""

from absl.testing import absltest

from dm_control import mjcf

from dm_control.locomotion.walkers import base

import numpy as np

class FakeWalker(base.Walker):

def _build(self):

self._mjcf_root = mjcf.RootElement(model='walker')

self._torso_body = self._mjcf_root.worldbody.add(

'body', name='torso', xyaxes=[0, 1, 0, -1, 0, 0])

@property

def mjcf_model(self):

return self._mjcf_root

@property

def actuators(self):

return []

@property

def root_body(self):

return self._torso_body

@property

def observable_joints(self):

return []

class BaseWalkerTest(absltest.TestCase):

def testTransformVectorToEgocentricFrame(self):

walker = FakeWalker()

physics = mjcf.Physics.from_mjcf_model(walker.mjcf_model)

# 3D vectors

np.testing.assert_allclose(

walker.transform_vec_to_egocentric_frame(physics, [0, 1, 0]), [1, 0, 0],

atol=1e-10)

np.testing.assert_allclose(

walker.transform_vec_to_egocentric_frame(physics, [-1, 0, 0]),

[0, 1, 0],

atol=1e-10)

np.testing.assert_allclose(

walker.transform_vec_to_egocentric_frame(physics, [0, 0, 1]), [0, 0, 1],

atol=1e-10)

# 2D vectors; z-component is ignored

np.testing.assert_allclose(

walker.transform_vec_to_egocentric_frame(physics, [0, 1]), [1, 0],

atol=1e-10)

np.testing.assert_allclose(

walker.transform_vec_to_egocentric_frame(physics, [-1, 0]), [0, 1],

atol=1e-10)

def testTransformMatrixToEgocentricFrame(self):

walker = FakeWalker()

physics = mjcf.Physics.from_mjcf_model(walker.mjcf_model)

rotation_atob = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])

ego_rotation_atob = np.array([[0, 0, -1], [0, -1, 0], [-1, 0, 0]])

np.testing.assert_allclose(

walker.transform_xmat_to_egocentric_frame(physics, rotation_atob),

ego_rotation_atob, atol=1e-10)

flat_rotation_atob = np.reshape(rotation_atob, -1)

flat_rotation_ego_atob = np.reshape(ego_rotation_atob, -1)

np.testing.assert_allclose(

walker.transform_xmat_to_egocentric_frame(physics, flat_rotation_atob),

flat_rotation_ego_atob, atol=1e-10)

if __name__ == '__main__':

absltest.main()