def test_direct_torque_controller():
sim = create_sim()
options = dict()
options["action_scaling"] = 1.
controller = DirectTorqueController(sim, **options)
# Test the action space.
# import pdb; pdb.set_trace()
expected_action_limits = 300.*np.ones(7)
assert np.allclose(controller.action_space.low, -expected_action_limits)
assert np.allclose(controller.action_space.high, expected_action_limits)
# Test that the torque is linear in the action.
action_1 = np.array([1., 2., 3., 4., 5., 6., 7.])
action_2 = 2*np.array(action_1)
controller.set_action(action_1)
torque_1 = controller.get_torque()
controller.set_action(action_2)
torque_2 = controller.get_torque()
controller.set_action(action_1 + action_2)
assert np.allclose(controller.get_torque(), torque_1 + torque_2)
# Test that the action scaling works.
options_2 = dict()
options_2["action_scaling"] = 2.
sim_2 = create_sim()
controller_2 = DirectTorqueController(sim_2, **options_2)
controller_2.set_action(action_1)
torque_ratio = controller_2.get_torque()/torque_1
scaling_ratio = options_2["action_scaling"]/options["action_scaling"]
assert np.allclose(torque_ratio, scaling_ratio)
def vis_impedance_random_setpoint(collision=False):
options = dict()
options['model_path'] = 'full_kuka_no_collision.xml'
options['rot_scale'] = .3
options['stiffness'] = np.array([1., 1., 1., 3., 3., 3.])
sim = create_sim(collision=collision)
controller = ImpedanceControllerV2(sim, **options)
frame_skip = 50
high = np.array([.1, .1, .1, 2, 2, 2])
low = -np.array([.1, .1, .1, 2, 2, 2])
viewer = mujoco_py.MjViewer(sim)
for i in range(10):
# Set a different random state and run the controller.
qpos = np.random.uniform(-1., 1., size=7)
qvel = np.zeros(7)
state = np.concatenate([qpos, qvel])
sim_state = sim.get_state()
sim_state.qpos[:] = qpos
sim_state.qvel[:] = qvel
sim.set_state(sim_state)
sim.forward()
for i in range(3000):
controller.set_action(np.random.uniform(high, low))
for i in range(frame_skip):
sim.data.ctrl[:] = controller.get_torque()
sim.step()
render_frame(viewer, controller.pos_set, controller.quat_set)
viewer.render()
def test_inverse_dynamics_controller():
options = dict()
options['kp_id'] = 100.
options['kd_id'] = 'auto'
sim = create_sim()
controller = InverseDynamicsController(sim, **options)
sim.data.qpos[:] = np.zeros(7)
sim.data.qvel[:] = np.ones(7)
controller.set_action(np.zeros(7))
base_torque = controller.get_torque()
# Test that coriolis terms are being calculated.
assert not np.any(np.isclose(base_torque, np.zeros(7)))
# Test that the torque is linear in the error
controller.set_action(np.ones(7))
torque_1 = controller.get_torque()
controller.set_action(2 * np.ones(7))
torque_2 = controller.get_torque()
assert not np.allclose(torque_1, base_torque)
assert np.allclose(2 * (torque_1 - base_torque), torque_2 - base_torque)
# Test that the torque is state dependent
sim.data.qpos[:] = np.array([.1, .2, .3, .4, .5, .6, .7])
controller.set_action(np.zeros(7))
assert not np.allclose(controller.get_torque(), base_torque)
def test_sac_torque_controller():
options = dict()
options["limit_scale"] = 10.
sim = create_sim()
direct_controller = DirectTorqueController(sim)
sac_controller = SACTorqueController(sim, **options)
assert np.allclose(direct_controller.action_space.low, sac_controller.action_space.low*options["limit_scale"])
assert np.allclose(direct_controller.action_space.high, sac_controller.action_space.high*options["limit_scale"])
def test_relative_inverse_dynamics_controller():
sim = create_sim()
controller = RelativeInverseDynamicsController(sim)
sim.data.qpos[:] = np.zeros(7)
sim.data.qvel[:] = np.zeros(7)
# Test that the torques are non-zero.
setpoint = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
controller.set_action(setpoint)
torque = controller.get_torque()
assert not np.any(np.isclose(np.zeros(7), torque))
# Test that the torques are zero when the state is at the setpoint.
sim.data.qpos[:] = setpoint
torque = controller.get_torque()
assert np.allclose(np.zeros(7), torque)
def test_pd_controller():
sim = create_sim()
options = dict()
options['set_velocity'] = True
# Test the null action with velocity control enabled
controller = PDController(sim, **options)
assert len(controller.action_space.low) == 14
null_action = np.zeros(14)
controller.set_action(null_action)
assert np.all(controller.get_torque() == np.zeros(7))
# Test the null action with no velocity control
options = dict()
options['set_velocity'] = False
controller = PDController(sim, **options)
assert len(controller.action_space.low) == 7
# The null action at the origin should be zero
null_action = np.zeros(7)
controller.set_action(null_action)
assert np.all(controller.get_torque() == np.zeros(7))