Skip to content

Commit 7e344bd

Browse files
jsmerelDMcopybara-github
jsmerelDM
authored and
copybara-github
committed
Release of V2020 cmu humanoid body.
PiperOrigin-RevId: 305302995 Change-Id: Ie480fc8dc5106b80fa361a9d5b8dbd83b989023e
1 parent f5574cc commit 7e344bd

7 files changed

Lines changed: 567 additions & 10 deletions

File tree

‎dm_control/locomotion/examples/__init__.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,3 +12,8 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414
# ============================================================================
15+
"""Walkers for Locomotion tasks."""
16+
17+
from dm_control.locomotion.walkers.cmu_humanoid import CMUHumanoid
18+
from dm_control.locomotion.walkers.cmu_humanoid import CMUHumanoidPositionControlled
19+
from dm_control.locomotion.walkers.cmu_humanoid import CMUHumanoidPositionControlledV2020

dm_control/locomotion/walkers/assets/humanoid_CMU.xml renamed to dm_control/locomotion/walkers/assets/humanoid_CMU_V2019.xml

File renamed without changes.

‎dm_control/locomotion/walkers/assets/humanoid_CMU_V2020.xml‎

Lines changed: 303 additions & 0 deletions
Large diffs are not rendered by default.

‎dm_control/locomotion/walkers/cmu_humanoid.py‎

Lines changed: 133 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
# Copyright 2019 The dm_control Authors.
1+
# Copyright 2020 The dm_control Authors.
22
#
33
# Licensed under the Apache License, Version 2.0 (the "License");
44
# you may not use this file except in compliance with the License.
@@ -12,7 +12,6 @@
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
1414
# ============================================================================
15-
1615
"""A CMU humanoid walker."""
1716

1817
from __future__ import absolute_import
@@ -28,15 +27,17 @@
2827
from dm_control.composer.observation import observable
2928
from dm_control.locomotion.walkers import base
3029
from dm_control.locomotion.walkers import legacy_base
30+
from dm_control.locomotion.walkers import rescale
3131
from dm_control.locomotion.walkers import scaled_actuators
3232
from dm_control.mujoco import wrapper as mj_wrapper
3333
import numpy as np
3434
import six
3535
from six.moves import zip
3636

37-
_XML_PATH = os.path.join(os.path.dirname(__file__), 'assets/humanoid_CMU.xml')
38-
37+
_XML_PATH = os.path.join(os.path.dirname(__file__),
38+
'assets/humanoid_CMU_V{model_version}.xml')
3939
_WALKER_GEOM_GROUP = 2
40+
_WALKER_INVIS_GROUP = 1
4041

4142
_CMU_MOCAP_JOINTS = (
4243
'lfemurrz', 'lfemurry', 'lfemurrx', 'ltibiarx', 'lfootrz', 'lfootrx',
@@ -113,6 +114,67 @@
113114
PositionActuatorParams('upperneckry', [-20, 20 ], 20 ),
114115
PositionActuatorParams('upperneckrz', [-20, 20 ], 20 ),
115116
]
117+
PositionActuatorParamsV2020 = collections.namedtuple(
118+
'PositionActuatorParams', ['name', 'forcerange', 'kp', 'damping'])
119+
_POSITION_ACTUATORS_V2020 = [
120+
PositionActuatorParamsV2020('headrx', [-40, 40 ], 40 , 2 ),
121+
PositionActuatorParamsV2020('headry', [-40, 40 ], 40 , 2 ),
122+
PositionActuatorParamsV2020('headrz', [-40, 40 ], 40 , 2 ),
123+
PositionActuatorParamsV2020('lclaviclery', [-80, 80 ], 80 , 20),
124+
PositionActuatorParamsV2020('lclaviclerz', [-80, 80 ], 80 , 20),
125+
PositionActuatorParamsV2020('lfemurrx', [-300, 300], 300, 15),
126+
PositionActuatorParamsV2020('lfemurry', [-200, 200], 200, 10),
127+
PositionActuatorParamsV2020('lfemurrz', [-200, 200], 200, 10),
128+
PositionActuatorParamsV2020('lfingersrx', [-20, 20 ], 20 , 1 ),
129+
PositionActuatorParamsV2020('lfootrx', [-120, 120], 120, 6 ),
130+
PositionActuatorParamsV2020('lfootrz', [-50, 50 ], 50 , 3 ),
131+
PositionActuatorParamsV2020('lhandrx', [-20, 20 ], 20 , 1 ),
132+
PositionActuatorParamsV2020('lhandrz', [-20, 20 ], 20 , 1 ),
133+
PositionActuatorParamsV2020('lhumerusrx', [-120, 120], 120, 6 ),
134+
PositionActuatorParamsV2020('lhumerusry', [-120, 120], 120, 6 ),
135+
PositionActuatorParamsV2020('lhumerusrz', [-120, 120], 120, 6 ),
136+
PositionActuatorParamsV2020('lowerbackrx', [-300, 300], 300, 15),
137+
PositionActuatorParamsV2020('lowerbackry', [-180, 180], 180, 20),
138+
PositionActuatorParamsV2020('lowerbackrz', [-200, 200], 200, 20),
139+
PositionActuatorParamsV2020('lowerneckrx', [-120, 120 ],120, 20),
140+
PositionActuatorParamsV2020('lowerneckry', [-120, 120 ],120, 20),
141+
PositionActuatorParamsV2020('lowerneckrz', [-120, 120 ],120, 20),
142+
PositionActuatorParamsV2020('lradiusrx', [-90, 90 ], 90 , 5 ),
143+
PositionActuatorParamsV2020('lthumbrx', [-20, 20 ], 20 , 1 ),
144+
PositionActuatorParamsV2020('lthumbrz', [-20, 20 ], 20 , 1 ),
145+
PositionActuatorParamsV2020('ltibiarx', [-160, 160], 160, 8 ),
146+
PositionActuatorParamsV2020('ltoesrx', [-20, 20 ], 20 , 1 ),
147+
PositionActuatorParamsV2020('lwristry', [-20, 20 ], 20 , 1 ),
148+
PositionActuatorParamsV2020('rclaviclery', [-80, 80 ], 80 , 20),
149+
PositionActuatorParamsV2020('rclaviclerz', [-80, 80 ], 80 , 20),
150+
PositionActuatorParamsV2020('rfemurrx', [-300, 300], 300, 15),
151+
PositionActuatorParamsV2020('rfemurry', [-200, 200], 200, 10),
152+
PositionActuatorParamsV2020('rfemurrz', [-200, 200], 200, 10),
153+
PositionActuatorParamsV2020('rfingersrx', [-20, 20 ], 20 , 1 ),
154+
PositionActuatorParamsV2020('rfootrx', [-120, 120], 120, 6 ),
155+
PositionActuatorParamsV2020('rfootrz', [-50, 50 ], 50 , 3 ),
156+
PositionActuatorParamsV2020('rhandrx', [-20, 20 ], 20 , 1 ),
157+
PositionActuatorParamsV2020('rhandrz', [-20, 20 ], 20 , 1 ),
158+
PositionActuatorParamsV2020('rhumerusrx', [-120, 120], 120, 6 ),
159+
PositionActuatorParamsV2020('rhumerusry', [-120, 120], 120, 6 ),
160+
PositionActuatorParamsV2020('rhumerusrz', [-120, 120], 120, 6 ),
161+
PositionActuatorParamsV2020('rradiusrx', [-90, 90 ], 90 , 5 ),
162+
PositionActuatorParamsV2020('rthumbrx', [-20, 20 ], 20 , 1 ),
163+
PositionActuatorParamsV2020('rthumbrz', [-20, 20 ], 20 , 1 ),
164+
PositionActuatorParamsV2020('rtibiarx', [-160, 160], 160, 8 ),
165+
PositionActuatorParamsV2020('rtoesrx', [-20, 20 ], 20 , 1 ),
166+
PositionActuatorParamsV2020('rwristry', [-20, 20 ], 20 , 1 ),
167+
PositionActuatorParamsV2020('thoraxrx', [-300, 300], 300, 15),
168+
PositionActuatorParamsV2020('thoraxry', [-80, 80], 80 , 8 ),
169+
PositionActuatorParamsV2020('thoraxrz', [-200, 200], 200, 12),
170+
PositionActuatorParamsV2020('upperbackrx', [-300, 300], 300, 15),
171+
PositionActuatorParamsV2020('upperbackry', [-80, 80], 80 , 8 ),
172+
PositionActuatorParamsV2020('upperbackrz', [-200, 200], 200, 12),
173+
PositionActuatorParamsV2020('upperneckrx', [-60, 60 ], 60 , 10),
174+
PositionActuatorParamsV2020('upperneckry', [-60, 60 ], 60 , 10),
175+
PositionActuatorParamsV2020('upperneckrz', [-60, 60 ], 60 , 10),
176+
]
177+
116178
# pylint: enable=bad-whitespace
117179

118180
_UPRIGHT_POS = (0.0, 0.0, 0.94)
@@ -131,6 +193,7 @@ class _CMUHumanoidBase(legacy_base.Walker):
131193
def _build(self,
132194
name='walker',
133195
marker_rgba=None,
196+
include_face=False,
134197
initializer=None):
135198
self._mjcf_root = mjcf.from_path(self._xml_path)
136199
if name:
@@ -146,6 +209,24 @@ def _build(self,
146209

147210
super(_CMUHumanoidBase, self)._build(initializer=initializer)
148211

212+
if include_face:
213+
head = self._mjcf_root.find('body', 'head')
214+
face_forwardness = head.pos[1]-.02
215+
head_geom = self._mjcf_root.find('geom', 'head')
216+
nose_size = head_geom.size[0] / 4.75
217+
face = head.add(
218+
'body', name='face', pos=(0.0, 0.039, face_forwardness))
219+
face.add('geom',
220+
type='capsule',
221+
name='nose',
222+
size=(nose_size, 0.01),
223+
pos=(0.0, 0.0, 0.0),
224+
quat=(1, 0.7, 0, 0),
225+
mass=0.,
226+
contype=0,
227+
conaffinity=0,
228+
group=_WALKER_INVIS_GROUP)
229+
149230
def _build_observables(self):
150231
return CMUHumanoidObservables(self)
151232

@@ -259,30 +340,56 @@ class CMUHumanoid(_CMUHumanoidBase):
259340

260341
@property
261342
def _xml_path(self):
262-
return _XML_PATH
343+
return _XML_PATH.format(model_version='2019')
263344

264345

265346
class CMUHumanoidPositionControlled(CMUHumanoid):
266347
"""A position-controlled CMU humanoid with control range scaled to [-1, 1]."""
267348

268-
def _build(self, *args, **kwargs):
269-
super(CMUHumanoidPositionControlled, self)._build(*args, **kwargs)
349+
def _build(self, model_version='2019', **kwargs):
350+
self._version = model_version
351+
if 'scale_default' in kwargs:
352+
scale_default = kwargs['scale_default']
353+
del kwargs['scale_default']
354+
else:
355+
scale_default = False
356+
357+
super(CMUHumanoidPositionControlled, self)._build(**kwargs)
358+
359+
if scale_default:
360+
# NOTE: This rescaling doesn't affect the attached hands
361+
rescale.rescale_humanoid(self, 1.2, 1.2, 70)
362+
363+
# modify actuators
364+
if self._version == '2020':
365+
position_actuators = _POSITION_ACTUATORS_V2020
366+
else:
367+
position_actuators = _POSITION_ACTUATORS
270368
self._mjcf_root.default.general.forcelimited = 'true'
271369
self._mjcf_root.actuator.motor.clear()
272-
for actuator_params in _POSITION_ACTUATORS:
370+
for actuator_params in position_actuators:
273371
associated_joint = self._mjcf_root.find('joint', actuator_params.name)
274-
scaled_actuators.add_position_actuator(
372+
if hasattr(actuator_params, 'damping'):
373+
associated_joint.damping = actuator_params.damping
374+
actuator = scaled_actuators.add_position_actuator(
275375
name=actuator_params.name,
276376
target=associated_joint,
277377
kp=actuator_params.kp,
278378
qposrange=associated_joint.range,
279379
ctrlrange=(-1, 1),
280380
forcerange=actuator_params.forcerange)
381+
if self._version == '2020':
382+
actuator.dyntype = 'filter'
383+
actuator.dynprm = [0.030]
281384
limits = zip(*(actuator.joint.range for actuator in self.actuators)) # pylint: disable=not-an-iterable
282385
lower, upper = (np.array(limit) for limit in limits)
283386
self._scale = upper - lower
284387
self._offset = upper + lower
285388

389+
@property
390+
def _xml_path(self):
391+
return _XML_PATH.format(model_version=self._version)
392+
286393
def cmu_pose_to_actuation(self, target_pose):
287394
"""Creates the control signal corresponding a CMU mocap joints pose.
288395
@@ -300,6 +407,14 @@ def cmu_pose_to_actuation(self, target_pose):
300407
return (2 * target_pose[self.actuator_order] - self._offset) / self._scale
301408

302409

410+
class CMUHumanoidPositionControlledV2020(CMUHumanoidPositionControlled):
411+
"""A 2020 updated CMU humanoid walker; includes nose for head orientation."""
412+
413+
def _build(self, **kwargs):
414+
super(CMUHumanoidPositionControlledV2020, self)._build(
415+
model_version='2020', scale_default=True, include_face=True, **kwargs)
416+
417+
303418
class CMUHumanoidObservables(legacy_base.WalkerObservables):
304419
"""Observables for the Humanoid."""
305420

@@ -312,6 +427,15 @@ def body_camera(self):
312427
return observable.MJCFCamera(
313428
self._entity.body_camera, width=64, height=64, scene_option=options)
314429

430+
@composer.observable
431+
def egocentric_camera(self):
432+
options = mj_wrapper.MjvOption()
433+
434+
# Don't render this walker's geoms.
435+
options.geomgroup[_WALKER_INVIS_GROUP] = 0
436+
return observable.MJCFCamera(self._entity.egocentric_camera,
437+
width=64, height=64, scene_option=options)
438+
315439
@composer.observable
316440
def head_height(self):
317441
return observable.MJCFFeature('xpos', self._entity.head)[2]

‎dm_control/locomotion/walkers/rescale.py‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
# Copyright 2020 The dm_control Authors.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
# ============================================================================
15+
"""Function to rescale the walkers."""
16+
17+
from __future__ import absolute_import
18+
from __future__ import division
19+
from __future__ import print_function
20+
21+
from dm_control import mjcf
22+
23+
24+
def rescale_subtree(body, position_factor, size_factor):
25+
"""Recursively rescales an entire subtree of an MJCF model."""
26+
for child in body.all_children():
27+
if getattr(child, 'fromto', None) is not None:
28+
new_pos = position_factor * 0.5 * (child.fromto[3:] + child.fromto[:3])
29+
new_size = size_factor * 0.5 * (child.fromto[3:] - child.fromto[:3])
30+
child.fromto[:3] = new_pos - new_size
31+
child.fromto[3:] = new_pos + new_size
32+
if getattr(child, 'pos', None) is not None:
33+
child.pos *= position_factor
34+
if getattr(child, 'size', None) is not None:
35+
child.size *= size_factor
36+
if child.tag == 'body' or child.tag == 'worldbody':
37+
rescale_subtree(child, position_factor, size_factor)
38+
39+
40+
def rescale_humanoid(walker, position_factor, size_factor=None, mass=None):
41+
"""Rescales a humanoid walker's lengths, sizes, and masses."""
42+
body = walker.mjcf_model.find('body', 'root')
43+
subtree_root = body.parent
44+
if size_factor is None:
45+
size_factor = position_factor
46+
rescale_subtree(subtree_root, position_factor, size_factor)
47+
48+
if mass is not None:
49+
physics = mjcf.Physics.from_mjcf_model(walker.mjcf_model.root_model)
50+
current_mass = physics.bind(walker.root_body).subtreemass
51+
mass_factor = mass / current_mass
52+
for body in walker.root_body.find_all('body'):
53+
inertial = getattr(body, 'inertial', None)
54+
if inertial:
55+
inertial.mass *= mass_factor
56+
for geom in walker.root_body.find_all('geom'):
57+
if geom.mass is not None:
58+
geom.mass *= mass_factor
59+
else:
60+
current_density = geom.density if geom.density is not None else 1000
61+
geom.density = current_density * mass_factor

‎dm_control/locomotion/walkers/rescale_test.py‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
# Copyright 2020 The dm_control Authors.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
# ============================================================================
15+
16+
"""Tests for rescaling bodies."""
17+
18+
from __future__ import absolute_import
19+
from __future__ import division
20+
from __future__ import print_function
21+
22+
from absl.testing import absltest
23+
from dm_control import mjcf
24+
from dm_control.locomotion.walkers import rescale
25+
import numpy as np
26+
27+
28+
class RescaleTest(absltest.TestCase):
29+
30+
def setUp(self):
31+
super(RescaleTest, self).setUp()
32+
33+
# build a simple three-link chain with an endpoint site
34+
self._mjcf_model = mjcf.RootElement()
35+
body = self._mjcf_model.worldbody.add('body', pos=[0, 0, 0])
36+
body.add('geom', type='capsule', fromto=[0, 0, 0, 0, 0, -0.4], size=[0.06])
37+
body.add('joint', type='ball')
38+
body = body.add('body', pos=[0, 0, -0.5])
39+
body.add('geom', type='capsule', pos=[0, 0, -0.15], size=[0.06, 0.15])
40+
body.add('joint', type='ball')
41+
body = body.add('body', pos=[0, 0, -0.4])
42+
body.add('geom', type='capsule', fromto=[0, 0, 0, 0.3, 0, -0.4],
43+
size=[0.06])
44+
body.add('joint', type='ball')
45+
body.add('site', name='endpoint', type='sphere', pos=[0.3, 0, -0.4],
46+
size=[0.1])
47+
48+
def test_rescale(self):
49+
# verify endpoint is where expected
50+
physics = mjcf.Physics.from_mjcf_model(self._mjcf_model)
51+
np.testing.assert_allclose(physics.named.data.site_xpos['endpoint'],
52+
np.array([0.3, 0., -1.3]), atol=1e-15)
53+
54+
# rescale chain and verify endpoint is where expected after modification
55+
subtree_root = self._mjcf_model
56+
position_factor = .5
57+
size_factor = .5
58+
rescale.rescale_subtree(subtree_root, position_factor, size_factor)
59+
physics = mjcf.Physics.from_mjcf_model(self._mjcf_model)
60+
np.testing.assert_allclose(physics.named.data.site_xpos['endpoint'],
61+
np.array([0.15, 0., -0.65]), atol=1e-15)
62+
63+
if __name__ == '__main__':
64+
absltest.main()

‎setup.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -166,7 +166,7 @@ def find_data_files(package_dir, patterns):
166166

167167
setup(
168168
name='dm_control',
169-
version='0.0.305010918',
169+
version='0.0.305302995',
170170
description='Continuous control environments and MuJoCo Python bindings.',
171171
author='DeepMind',
172172
license='Apache License, Version 2.0',

0 commit comments

Comments
 (0)