def test_invalid_target(self):
invalid_target = self._mjcf_model.worldbody
with self.assertRaisesWithLiteralMatch(
TypeError,
scaled_actuators._GOT_INVALID_TARGET.format(invalid_target)):
scaled_actuators.add_position_actuator(
target=invalid_target, qposrange=(self._min, self._max))
def test_invalid_kwargs(self):
invalid_kwargs = dict(joint=self._scaled_actuator_joint, ctrllimited=False)
with self.assertRaisesWithLiteralMatch(
TypeError,
scaled_actuators._GOT_INVALID_KWARGS.format(sorted(invalid_kwargs))):
scaled_actuators.add_position_actuator(
target=self._scaled_actuator_joint,
qposrange=(self._min, self._max),
**invalid_kwargs)
def test_position_actuator(self):
scaled_actuator = scaled_actuators.add_position_actuator(
target=self._scaled_actuator_joint, kp=self._gain,
qposrange=(self._min, self._max),
ctrlrange=(self._scaled_min, self._scaled_max))
standard_actuator = self._mjcf_model.actuator.add(
'position', joint=self._standard_actuator_joint, kp=self._gain,
ctrllimited=True, ctrlrange=(self._min, self._max))
physics = mjcf.Physics.from_mjcf_model(self._mjcf_model)
# Zero torque.
physics.bind(self._scaled_actuator_joint).qpos = (
0.2345 * self._range + self._min)
self._set_actuator_controls(physics, 0.2345, scaled_actuator)
np.testing.assert_allclose(
physics.bind(self._scaled_actuator_joint).qfrc_actuator, 0, atol=1e-15)
for _ in range(100):
normalized_ctrl = self._random_state.uniform()
physics.bind(self._joints).qpos = (
self._random_state.uniform() * self._range + self._min)
self._set_actuator_controls(physics, normalized_ctrl,
scaled_actuator, standard_actuator)
self._assert_same_qfrc_actuator(
physics, self._scaled_actuator_joint, self._standard_actuator_joint)
def _build(self, *args, **kwargs):
super(CMUHumanoidPositionControlled, self)._build(*args, **kwargs)
self._mjcf_root.default.general.forcelimited = 'true'
self._mjcf_root.actuator.motor.clear()
for actuator_params in _POSITION_ACTUATORS:
associated_joint = self._mjcf_root.find('joint',
actuator_params.name)
scaled_actuators.add_position_actuator(
name=actuator_params.name,
target=associated_joint,
kp=actuator_params.kp,
qposrange=associated_joint.range,
ctrlrange=(-1, 1),
forcerange=actuator_params.forcerange)
limits = zip(*(actuator.joint.range for actuator in self.actuators)) # pylint: disable=not-an-iterable
lower, upper = (np.array(limit) for limit in limits)
self._scale = upper - lower
self._offset = upper + lower
def _build(self, model_version='2019', **kwargs):
self._version = model_version
if 'scale_default' in kwargs:
scale_default = kwargs['scale_default']
del kwargs['scale_default']
else:
scale_default = False
super()._build(**kwargs)
if scale_default:
# NOTE: This rescaling doesn't affect the attached hands
rescale.rescale_humanoid(self, 1.2, 1.2, 70)
# modify actuators
if self._version == '2020':
position_actuators = _POSITION_ACTUATORS_V2020
else:
position_actuators = _POSITION_ACTUATORS
self._mjcf_root.default.general.forcelimited = 'true'
self._mjcf_root.actuator.motor.clear()
for actuator_params in position_actuators:
associated_joint = self._mjcf_root.find('joint',
actuator_params.name)
if hasattr(actuator_params, 'damping'):
associated_joint.damping = actuator_params.damping
actuator = scaled_actuators.add_position_actuator(
name=actuator_params.name,
target=associated_joint,
kp=actuator_params.kp,
qposrange=associated_joint.range,
ctrlrange=(-1, 1),
forcerange=actuator_params.forcerange)
if self._version == '2020':
actuator.dyntype = 'filter'
actuator.dynprm = [0.030]
limits = zip(*(actuator.joint.range for actuator in self.actuators)) # pylint: disable=not-an-iterable
lower, upper = (np.array(limit) for limit in limits)
self._scale = upper - lower
self._offset = upper + lower
