|
| 1 | +# Copyright 2017 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 | +"""Props made of a single primitive MuJoCo geom.""" |
| 16 | +import itertools |
| 17 | + |
| 18 | +from dm_control import composer |
| 19 | +from dm_control import mjcf |
| 20 | +from dm_control.composer import define |
| 21 | +from dm_control.composer.observation import observable |
| 22 | +import numpy as np |
| 23 | +_DEFAULT_HALF_LENGTHS = [0.05, 0.1, 0.15] |
| 24 | + |
| 25 | + |
| 26 | +class Primitive(composer.Entity): |
| 27 | + """A primitive MuJoCo geom prop.""" |
| 28 | + |
| 29 | + def _build(self, geom_type, size, mass=None, name=None): |
| 30 | + """Initializes this prop. |
| 31 | +
|
| 32 | + Args: |
| 33 | + geom_type: a string, one of the types supported by MuJoCo. |
| 34 | + size: a list or numpy array of up to 3 numbers, depending on the type. |
| 35 | + mass: The mass for the primitive geom. |
| 36 | + name: (optional) A string, the name of this prop. |
| 37 | + """ |
| 38 | + size = np.reshape(np.asarray(size), -1) |
| 39 | + self._mjcf_root = mjcf.element.RootElement(model=name) |
| 40 | + |
| 41 | + self._geom = self._mjcf_root.worldbody.add( |
| 42 | + 'geom', name='body_geom', type=geom_type, size=size, mass=mass) |
| 43 | + |
| 44 | + touch_sensor = self._mjcf_root.worldbody.add( |
| 45 | + 'site', type=geom_type, name='touch_sensor', size=size*1.05, |
| 46 | + rgba=[1, 1, 1, 0.1], # touch sensor site is almost transparent |
| 47 | + group=composer.SENSOR_SITES_GROUP) |
| 48 | + |
| 49 | + self._touch = self._mjcf_root.sensor.add( |
| 50 | + 'touch', site=touch_sensor) |
| 51 | + |
| 52 | + self._position = self._mjcf_root.sensor.add( |
| 53 | + 'framepos', name='position', objtype='geom', objname=self.geom) |
| 54 | + |
| 55 | + self._orientation = self._mjcf_root.sensor.add( |
| 56 | + 'framequat', name='orientation', objtype='geom', |
| 57 | + objname=self.geom) |
| 58 | + |
| 59 | + self._linear_velocity = self._mjcf_root.sensor.add( |
| 60 | + 'framelinvel', name='linear_velocity', objtype='geom', |
| 61 | + objname=self.geom) |
| 62 | + |
| 63 | + self._angular_velocity = self._mjcf_root.sensor.add( |
| 64 | + 'frameangvel', name='angular_velocity', objtype='geom', |
| 65 | + objname=self.geom) |
| 66 | + |
| 67 | + self._name = name |
| 68 | + |
| 69 | + def _build_observables(self): |
| 70 | + return PrimitiveObservables(self) |
| 71 | + |
| 72 | + @property |
| 73 | + def geom(self): |
| 74 | + """Returns the primitive's geom, e.g., to change color or friction.""" |
| 75 | + return self._geom |
| 76 | + |
| 77 | + @property |
| 78 | + def touch(self): |
| 79 | + """Exposing the touch sensor for observations and reward.""" |
| 80 | + return self._touch |
| 81 | + |
| 82 | + @property |
| 83 | + def position(self): |
| 84 | + """Ground truth pos sensor.""" |
| 85 | + return self._position |
| 86 | + |
| 87 | + @property |
| 88 | + def orientation(self): |
| 89 | + """Ground truth angular position sensor.""" |
| 90 | + return self._orientation |
| 91 | + |
| 92 | + @property |
| 93 | + def linear_velocity(self): |
| 94 | + """Ground truth velocity sensor.""" |
| 95 | + return self._linear_velocity |
| 96 | + |
| 97 | + @property |
| 98 | + def angular_velocity(self): |
| 99 | + """Ground truth angular velocity sensor.""" |
| 100 | + return self._angular_velocity |
| 101 | + |
| 102 | + @property |
| 103 | + def mjcf_model(self): |
| 104 | + return self._mjcf_root |
| 105 | + |
| 106 | + @property |
| 107 | + def name(self): |
| 108 | + return self._name |
| 109 | + |
| 110 | + |
| 111 | +class PrimitiveObservables(composer.Observables, |
| 112 | + composer.FreePropObservableMixin): |
| 113 | + """Primitive entity's observables.""" |
| 114 | + |
| 115 | + @define.observable |
| 116 | + def position(self): |
| 117 | + return observable.MJCFFeature('sensordata', self._entity.position) |
| 118 | + |
| 119 | + @define.observable |
| 120 | + def orientation(self): |
| 121 | + return observable.MJCFFeature('sensordata', self._entity.orientation) |
| 122 | + |
| 123 | + @define.observable |
| 124 | + def linear_velocity(self): |
| 125 | + return observable.MJCFFeature('sensordata', self._entity.linear_velocity) |
| 126 | + |
| 127 | + @define.observable |
| 128 | + def angular_velocity(self): |
| 129 | + return observable.MJCFFeature('sensordata', self._entity.angular_velocity) |
| 130 | + |
| 131 | + @define.observable |
| 132 | + def touch(self): |
| 133 | + return observable.MJCFFeature('sensordata', self._entity.touch) |
| 134 | + |
| 135 | + |
| 136 | +class Sphere(Primitive): |
| 137 | + """A class representing a sphere prop.""" |
| 138 | + |
| 139 | + def _build(self, radius=0.05, mass=None, name='sphere'): |
| 140 | + super(Sphere, self)._build( |
| 141 | + geom_type='sphere', size=radius, mass=mass, name=name) |
| 142 | + |
| 143 | + |
| 144 | +class Box(Primitive): |
| 145 | + """A class representing a box prop.""" |
| 146 | + |
| 147 | + def _build(self, half_lengths=None, mass=None, name='box'): |
| 148 | + half_lengths = half_lengths or _DEFAULT_HALF_LENGTHS |
| 149 | + super(Box, self)._build(geom_type='box', |
| 150 | + size=half_lengths, |
| 151 | + mass=mass, |
| 152 | + name=name) |
| 153 | + |
| 154 | + |
| 155 | +class BoxWithSites(Box): |
| 156 | + """A class representing a box prop with sites on the corners.""" |
| 157 | + |
| 158 | + def _build(self, half_lengths=None, mass=None, name='box'): |
| 159 | + half_lengths = half_lengths or _DEFAULT_HALF_LENGTHS |
| 160 | + super(BoxWithSites, self)._build(half_lengths=half_lengths, mass=mass, |
| 161 | + name=name) |
| 162 | + |
| 163 | + corner_positions = itertools.product([half_lengths[0], -half_lengths[0]], |
| 164 | + [half_lengths[1], -half_lengths[1]], |
| 165 | + [half_lengths[2], -half_lengths[2]]) |
| 166 | + corner_sites = [] |
| 167 | + for i, corner_pos in enumerate(corner_positions): |
| 168 | + corner_sites.append( |
| 169 | + self._mjcf_root.worldbody.add( |
| 170 | + 'site', |
| 171 | + type='sphere', |
| 172 | + name='corner_{}'.format(i), |
| 173 | + size=[0.1], |
| 174 | + pos=corner_pos, |
| 175 | + rgba=[1, 0, 0, 1.0], |
| 176 | + group=composer.SENSOR_SITES_GROUP)) |
| 177 | + self._corner_sites = tuple(corner_sites) |
| 178 | + |
| 179 | + @property |
| 180 | + def corner_sites(self): |
| 181 | + return self._corner_sites |
| 182 | + |
| 183 | + |
| 184 | +class Ellipsoid(Primitive): |
| 185 | + """A class representing an ellipsoid prop.""" |
| 186 | + |
| 187 | + def _build(self, radii=None, mass=None, name='ellipsoid'): |
| 188 | + radii = radii or _DEFAULT_HALF_LENGTHS |
| 189 | + super(Ellipsoid, self)._build(geom_type='ellipsoid', |
| 190 | + size=radii, |
| 191 | + mass=mass, |
| 192 | + name=name) |
| 193 | + |
| 194 | + |
| 195 | +class Cylinder(Primitive): |
| 196 | + """A class representing a cylinder prop.""" |
| 197 | + |
| 198 | + def _build(self, radius=0.05, half_length=0.15, mass=None, name='cylinder'): |
| 199 | + super(Cylinder, self)._build(geom_type='cylinder', |
| 200 | + size=[radius, half_length], |
| 201 | + mass=mass, |
| 202 | + name=name) |
| 203 | + |
| 204 | + |
| 205 | +class Capsule(Primitive): |
| 206 | + """A class representing a capsule prop.""" |
| 207 | + |
| 208 | + def _build(self, radius=0.05, half_length=0.15, mass=None, name='capsule'): |
| 209 | + super(Capsule, self)._build(geom_type='capsule', |
| 210 | + size=[radius, half_length], |
| 211 | + mass=mass, |
| 212 | + name=name) |
0 commit comments