def testMaxConflictingValues(self):
model_1 = mjcf.RootElement()
model_1.size.nconmax = 123
model_1.size.njmax = 456
model_2 = mjcf.RootElement()
model_2.size.nconmax = 345
model_2.size.njmax = 234
model_1.attach(model_2)
self.assertEqual(model_1.size.nconmax, 345)
self.assertEqual(model_1.size.njmax, 456)
def testAttachmentFrameChildReference(self):
root_1 = mjcf.RootElement('model_1')
root_2 = mjcf.RootElement('model_2')
root_2_frame = root_1.attach(root_2)
root_2_joint = root_2_frame.add('joint',
name='root_x',
type='slide',
axis=[1, 0, 0])
actuator = root_1.actuator.add('position',
name='root_x',
joint=root_2_joint)
self.assertEqual(
actuator.to_xml_string(pretty_print=False),
'<position name="root_x" class="/" joint="model_2/root_x/"/>')
def make_model_with_mixed_actuators():
actuators = []
is_stateful = []
root = mjcf.RootElement()
body = root.worldbody.add('body')
body.add('geom', type='sphere', size=[0.1])
slider = body.add('joint', type='slide', name='slide_joint')
# Third-order `general` actuator.
actuators.append(
root.actuator.add('general',
dyntype='integrator',
biastype='affine',
dynprm=[1, 0, 0],
joint=slider,
name='general_act'))
is_stateful.append(True)
# Cylinder actuators are also third-order.
actuators.append(
root.actuator.add('cylinder',
joint=slider,
name='cylinder_act'))
is_stateful.append(True)
# A second-order actuator, added after the third-order actuators. The
# actuators will be automatically reordered in the generated XML so that
# the second-order actuator comes first.
actuators.append(
root.actuator.add('velocity',
joint=slider,
name='velocity_act'))
is_stateful.append(False)
return root, actuators, is_stateful
def _build(self, size=(.2, .3, .05), rgba=(0, .5, 0, 1), name='pedestal'):
self._mjcf_root = mjcf.RootElement(model=name)
self._geom = self._mjcf_root.worldbody.add('geom',
type='box',
size=size,
name='geom',
rgba=rgba)
def test_bind_mocap_body(self):
pos = [1, 2, 3]
quat = [1, 0, 0, 0]
model = mjcf.RootElement()
# Bodies are non-mocap by default.
normal_body = model.worldbody.add('body', pos=pos, quat=quat)
mocap_body = model.worldbody.add('body',
pos=pos,
quat=quat,
mocap='true')
physics = mjcf.Physics.from_xml_string(model.to_xml_string())
binding = physics.bind(mocap_body)
np.testing.assert_array_equal(pos, binding.mocap_pos)
np.testing.assert_array_equal(quat, binding.mocap_quat)
new_pos = [4, 5, 6]
new_quat = [0, 1, 0, 0]
binding.mocap_pos = new_pos
binding.mocap_quat = new_quat
np.testing.assert_array_equal(
new_pos, physics.named.data.mocap_pos[mocap_body.full_identifier])
np.testing.assert_array_equal(
new_quat,
physics.named.data.mocap_quat[mocap_body.full_identifier])
with self.assertRaises(AttributeError):
_ = physics.bind(normal_body).mocap_pos
with six.assertRaisesRegex(
self, ValueError,
'Cannot bind to a collection containing multiple element types'
):
physics.bind([mocap_body, normal_body])
def test_resolve_defaults_for_some_attributes(self):
root = mjcf.RootElement()
root.default.geom.type = 'box'
root.default.geom.pos = [0, 1, 0]
geom1 = root.worldbody.add('geom')
mjcf.commit_defaults(geom1, attributes=['pos'])
self.assert_same_attributes(geom1, dict(pos=[0, 1, 0]))
def make_model_with_mixed_actuators(name):
actuators = []
root = mjcf.RootElement(model=name)
body = root.worldbody.add('body')
body.add('geom', type='sphere', size=[0.1])
slider = body.add('joint', type='slide', name='slide_joint')
# Third-order `general` actuator.
actuators.append(
root.actuator.add('general',
dyntype='integrator',
biastype='affine',
dynprm=[1, 0, 0],
joint=slider,
name='general_act'))
# Cylinder actuators are also third-order.
actuators.append(
root.actuator.add('cylinder',
joint=slider,
name='cylinder_act'))
# A second-order actuator, added after the third-order actuators.
actuators.append(
root.actuator.add('velocity',
joint=slider,
name='velocity_act'))
return root, actuators
def __init__(self, name):
self.mjcf_model = mjcf.RootElement(model=name)
floor = Box(lx=10, ly=10, lz=0.001, color='blue')
table = Box(lx=2, ly=2, lz=1)
self.floor = self.mjcf_model.worldbody.add('body', name='floor')
self.floor.add('geom',
name='floor',
pos=[0, 0, 0],
type=floor.type,
size=floor.size,
rgba=floor.rgba)
self.table = self.floor.add('body', name='table')
self.table.add('geom',
name='table',
pos=[0, 0, table.m_lz],
type=table.type,
size=table.size,
rgba=[1, 1, 1, 1])
self.asset = self.mjcf_model.asset.add('texture',
name="skybox",
type="skybox",
builtin="gradient",
rgb1=".4 .6 .8",
rgb2="0 0 0",
width="800",
height="800",
mark="random",
markrgb="1 1 1")
def test_resolve_actuator_defaults(self):
root = mjcf.RootElement()
root.default.general.forcelimited = 'true'
root.default.motor.forcerange = [-2, 3]
root.default.position.kp = 0.1
root.default.velocity.kv = 0.2
body = root.worldbody.add('body')
joint = body.add('joint')
motor = root.actuator.add('motor', joint=joint)
mjcf.commit_defaults(motor)
self.assert_same_attributes(
motor, dict(joint=joint, forcelimited='true', forcerange=[-2, 3]))
position = root.actuator.add('position', joint=joint)
mjcf.commit_defaults(position)
self.assert_same_attributes(
position,
dict(joint=joint, kp=0.1, forcelimited='true', forcerange=[-2, 3]))
velocity = root.actuator.add('velocity', joint=joint)
mjcf.commit_defaults(velocity)
self.assert_same_attributes(
velocity,
dict(joint=joint, kv=0.2, forcelimited='true', forcerange=[-2, 3]))
def setUp(self):
super().setUp()
# build a simple three-link chain with an endpoint site
self._mjcf_model = mjcf.RootElement()
body = self._mjcf_model.worldbody.add('body', pos=[0, 0, 0])
body.add('geom',
type='capsule',
fromto=[0, 0, 0, 0, 0, -0.4],
size=[0.06])
body.add('joint', type='ball')
body = body.add('body', pos=[0, 0, -0.5])
body.add('geom', type='capsule', pos=[0, 0, -0.15], size=[0.06, 0.15])
body.add('joint', type='ball')
body = body.add('body', pos=[0, 0, -0.4])
body.add('geom',
type='capsule',
fromto=[0, 0, 0, 0.3, 0, -0.4],
size=[0.06])
body.add('joint', type='ball')
body.add('site',
name='endpoint',
type='sphere',
pos=[0.3, 0, -0.4],
size=[0.1])
def __init__(self, length, rgba):
self.model = mjcf.RootElement()
# Defaults:
self.model.default.joint.damping = 2
self.model.default.joint.type = 'hinge'
self.model.default.geom.type = 'capsule'
self.model.default.geom.rgba = rgba # Continued below...
# Thigh:
self.thigh = self.model.worldbody.add('body')
self.hip = self.thigh.add('joint', axis=[0, 0, 1])
self.thigh.add('geom',
fromto=[0, 0, 0, length, 0, 0],
size=[length / 4])
# Hip:
self.shin = self.thigh.add('body', pos=[length, 0, 0])
self.knee = self.shin.add('joint', axis=[0, 1, 0])
self.shin.add('geom',
fromto=[0, 0, 0, 0, 0, -length],
size=[length / 5])
# Position actuators:
self.model.actuator.add('position', joint=self.hip, kp=10)
self.model.actuator.add('position', joint=self.knee, kp=10)
def _build(self,
pos,
size,
inverted=False,
visible=False,
rgba=(1, 0, 0, 0.25),
detected_rgba=(0, 1, 0, 0.25),
name='position_detector'):
"""Builds the detector.
Args:
pos: The position at the center of this detector's active region. Should
be an array-like object of length 3 in 3D mode, or length 2 in 2D mode.
size: The half-lengths of this detector's active region. Should
be an array-like object of length 3 in 3D mode, or length 2 in 2D mode.
inverted: (optional) A boolean, whether to operate in inverted detection
mode. If `True`, an entity is detected when it is not in the active
region.
visible: (optional) A boolean, whether this detector is visible by
default in rendered images. If `False`, this detector's active zone
is placed in MuJoCo rendering group 4, which is not rendered by default,
but can be toggled on (e.g. in Domain Explorer) for debugging purposes.
rgba: (optional) The color to render when nothing is detected.
detected_rgba: (optional) The color to render when an entity is detected.
name: (optional) XML element name of this position detector.
Raises:
ValueError: If the `pos` and `size` arrays do not have the same length.
"""
if len(pos) != len(size):
raise ValueError('`pos` and `size` should have the same length: '
'got {!r} and {!r}'.format(pos, size))
self._inverted = inverted
self._detected = False
self._lower = np.array(pos) - np.array(size)
self._upper = np.array(pos) + np.array(size)
self._entities = []
self._entity_geoms = {}
self._rgba = np.asarray(rgba)
self._detected_rgba = np.asarray(detected_rgba)
render_pos = np.zeros(3)
render_pos[:len(pos)] = pos
render_size = np.full(3, _RENDERED_HEIGHT_IN_2D_MODE)
render_size[:len(size)] = size
self._mjcf_root = mjcf.RootElement(model=name)
self._site = self._mjcf_root.worldbody.add('site',
name='detection_zone',
type='box',
pos=render_pos,
size=render_size,
rgba=self._rgba)
if not visible:
self._site.group = composer.SENSOR_SITES_GROUP
def test_bind_worldbody(self):
expected_mass = 10
model = mjcf.RootElement()
child = model.worldbody.add('body')
child.add('geom', type='sphere', size=[0.1], mass=expected_mass)
physics = mjcf.Physics.from_mjcf_model(model)
mass = physics.bind(model.worldbody).subtreemass
self.assertEqual(mass, expected_mass)
def testRemoveElementWithRequiredAttribute(self):
root = mjcf.RootElement()
body = root.worldbody.add('body')
# `objtype` is a required attribute.
sensor = root.sensor.add('framepos', objtype='body', objname=body)
self.assertIn(sensor, root.sensor.all_children())
sensor.remove()
self.assertNotIn(sensor, root.sensor.all_children())
def construct(self, arena=None, morphology_height=None):
if morphology_height is None:
# Determine the morphology height.
# segments contains the morphology positions. Find the minimum z, add morphology height to make it > thres
min_z = min(np.min(self.segment_embeddings[:, 2]), np.min(self.segment_embeddings[:, 5]))
morphology_height = max(0.1, 0.1 - min_z) # Make morphology height at least 1, larger if needed.
if arena is None:
arena = mjcf.RootElement(model="morphology")
arena.asset.add('texture', name="texplane", builtin='checker', height=300, rgb1=[0.1, 0.2, 0.3], rgb2=[0.2, 0.3, 0.4], type="2d", width=300)
arena.asset.add('material', name="MatPlane", reflectance=0.5, shininess=1, specular=1, texrepeat=[20, 20], texture="texplane")
arena.worldbody.add('light', cutoff=100, diffuse=[1,1,1], dir=[-0, 0, -1.3], directional=True, exponent=1, pos=[0,0,1.5], specular=[.1,.1,.1])
arena.worldbody.add('geom', name='ground', type='plane', size=[15, 15, .5], rgba=(1, 1, 1, 1), material="MatPlane")
else:
if hasattr(arena, "morphology_height"):
morphology_height += arena.morphology_height
arena = arena.construct()
# TODO: Set global kwargs. Find a better way to do this.
if 'compiler.settotalmass' in self.global_kwargs:
arena.compiler.settotalmass = self.global_kwargs['compiler.settotalmass']
if 'compiler.angle' in self.global_kwargs:
arena.compiler.angle = self.global_kwargs['compiler.angle']
if 'option.timestep' in self.global_kwargs:
arena.option.timestep = self.global_kwargs['option.timestep']
if 'option.integrator' in self.global_kwargs:
arena.option.integrator = self.global_kwargs['option.integrator']
if 'compiler.angle' in self.global_kwargs:
arena.compiler.angle = self.global_kwargs['compiler.angle']
if 'compiler.coordinate' in self.global_kwargs:
arena.compiler.coordinate = self.global_kwargs['compiler.coordinate']
if 'compiler.inertiafromgeom' in self.global_kwargs:
arena.compiler.inertiafromgeom = self.global_kwargs['compiler.inertiafromgeom']
morphology_attachment_site = arena.worldbody.add('site', name='body', size=[1e-6]*3, pos=[0, 0, morphology_height])
morphology = self.root.construct(geom_kwargs=self.geom_kwargs, joint_kwargs=self.joint_kwargs, two_dim=self.two_dim)
# attach camera to morphology
if self.one_dim:
morphology.worldbody.add('camera', name="top", pos=[1.2, 0, 3.0], quat=[1, 0, 0, 0])
elif self.two_dim:
morphology.worldbody.add('camera', name="side", pos=[0.4, -4, 0.2], quat=[0.707, 0.707, 0, 0], mode="trackcom")
else: # its three dimensional
morphology.worldbody.add('camera', name="iso", pos=[-2.3, -2.3, 1.0], xyaxes=[0.45, -0.45, 0, 0.3, 0.15, 0.94], mode="trackcom")
attachment_frame = morphology_attachment_site.attach(morphology)
if self.two_dim:
attachment_frame.add('joint', name='rootx', type='slide', axis=[1, 0, 0], limited=False, damping=0, armature=0, stiffness=0)
attachment_frame.add('joint', name='rootz', type='slide', axis=[0, 0, 1], limited=False, damping=0, armature=0, stiffness=0)
attachment_frame.add('joint', name='rooty', type='hinge', axis=[0, 1, 0], limited=False, damping=0, armature=0, stiffness=0)
elif self.one_dim:
pass
else:
attachment_frame.add('freejoint')
# Add sensors, assume the last body in worldbody is the morphology root
arena.sensor.add("subtreelinvel", name="velocity", body=arena.worldbody.body[-1])
return arena
def testRemoveWithChildren(self):
root = mjcf.RootElement()
body = root.worldbody.add('body')
subbodies = []
for _ in range(5):
subbodies.append(body.add('body'))
body.remove()
for subbody in subbodies:
self.assertTrue(subbody.is_removed)
def make_creature(idx):
rgba = random_state.uniform([0, 0, 0, 1], [1, 1, 1, 1])
model = mjcf.RootElement()
model.worldbody.add('geom',
name=f'torso{idx}',
type='ellipsoid',
size=BODY_SIZE,
rgba=rgba)
return model
def testActuatorReordering(self):
def make_model_with_mixed_actuators(name):
actuators = []
root = mjcf.RootElement(model=name)
body = root.worldbody.add('body')
body.add('geom', type='sphere', size=[0.1])
slider = body.add('joint', type='slide', name='slide_joint')
# Third-order `general` actuator.
actuators.append(
root.actuator.add('general',
dyntype='integrator',
biastype='affine',
dynprm=[1, 0, 0],
joint=slider,
name='general_act'))
# Cylinder actuators are also third-order.
actuators.append(
root.actuator.add('cylinder',
joint=slider,
name='cylinder_act'))
# A second-order actuator, added after the third-order actuators.
actuators.append(
root.actuator.add('velocity',
joint=slider,
name='velocity_act'))
return root, actuators
child_1, actuators_1 = make_model_with_mixed_actuators(name='child_1')
child_2, actuators_2 = make_model_with_mixed_actuators(name='child_2')
child_3, actuators_3 = make_model_with_mixed_actuators(name='child_3')
parent = mjcf.RootElement()
parent.attach(child_1)
parent.attach(child_2)
child_2.attach(child_3)
# Check that the generated XML contains all of the actuators that we expect
# it to have.
expected_xml_strings = [
actuator.to_xml_string(prefix_root=parent.namescope)
for actuator in actuators_1 + actuators_2 + actuators_3
]
xml_strings = [
util.to_native_string(etree.tostring(node, pretty_print=True))
for node in parent.to_xml().find('actuator').getchildren()
]
self.assertSameElements(expected_xml_strings, xml_strings)
# MuJoCo requires that all 3rd-order actuators (i.e. those with internal
# dynamics) come after all 2nd-order actuators in the XML. Attempting to
# compile this model will result in an error unless PyMJCF internally
# reorders the actuators so that the 3rd-order actuator comes last in the
# generated XML.
_ = mjcf.Physics.from_mjcf_model(child_1)
# Actuator re-ordering should also work in cases where there are multiple
# attached submodels with mixed 2nd- and 3rd-order actuators.
_ = mjcf.Physics.from_mjcf_model(parent)
def make_simple_model(self):
def add_submodel(root):
body = root.worldbody.add('body')
geom = body.add('geom', type='ellipsoid', size=[0.1, 0.2, 0.3])
site = body.add('site', type='sphere', size=[0.1])
return body, geom, site
root = mjcf.RootElement()
add_submodel(root)
add_submodel(root)
return root
def make_valid_model():
# !!LINE_REF make_valid_model.mjcf_model
mjcf_model = mjcf.RootElement()
# !!LINE_REF make_valid_model.my_body
my_body = mjcf_model.worldbody.add('body', name='my_body')
my_body.add('inertial', mass=1, pos=[0, 0, 0], diaginertia=[1, 1, 1])
# !!LINE_REF make_valid_model.my_joint
my_joint = my_body.add('joint', name='my_joint', type='hinge')
# !!LINE_REF make_valid_model.my_actuator
mjcf_model.actuator.add('velocity', name='my_actuator', joint=my_joint)
return mjcf_model
def _build(self, style):
labmaze_textures = labmaze_assets.get_floor_texture_paths(style)
self._mjcf_root = mjcf.RootElement(model='labmaze_' + style)
self._textures = []
for texture_name, texture_path in six.iteritems(labmaze_textures):
self._textures.append(
self._mjcf_root.asset.add(
'texture',
type='2d',
name=texture_name,
file=texture_path.format(texture_name)))
def __init__(self, name):
self.mjcf_model = mjcf.RootElement(model=name)
world = World('world')
self.world_site = self.mjcf_model.worldbody.add('site', name='world')
self.world_site.attach(world.mjcf_model)
robot = Robot('ub10')
self.robot_site = self.mjcf_model.worldbody.add('site',
name='ub10',
pos=[0, 0, 1.0])
self.robot_site.attach(robot.mjcf_model)
def __init__(self, name='gripper'):
self.mjcf_model = mjcf.RootElement(model=name)
base = Box(lx=0.15, ly=0.05, lz=0.05, color='yellow')
finger = Box(lx=0.1, ly=0.05, lz=0.01, color='yellow')
self.box_base = base
self.box_finger = finger
self.base = self.mjcf_model.worldbody.add('body', name='base')
self.base.add('geom',
name='base',
type=base.type,
pos=[0, 0, 0],
size=base.size,
rgba=base.rgba)
self.finger_left = self.base.add('body', name='finger_left')
self.finger_left.add(
'geom',
name='finger_left',
type=finger.type,
quat=[1, 0, -1, 0],
pos=[base.m_lx * (2 / 3), 0, base.m_lz + finger.m_lx],
size=finger.size,
rgba=finger.rgba)
self.finger_left.add('joint',
name='joint_left',
type='slide',
pos=[0, 0, base.m_lz],
axis=[1, 0, 0],
limited='True',
range=[-base.m_lx * (2 / 3) + finger.m_lz, 0.],
damping=1)
self.finger_right = self.base.add('body', name='finger_right')
self.finger_right.add(
'geom',
name='finger_right',
type=finger.type,
quat=[1, 0, 1, 0],
pos=[-base.m_lx * (2 / 3), 0, base.m_lz + finger.m_lx],
size=finger.size,
rgba=finger.rgba)
self.finger_right.add('joint',
name='joint_right',
type='slide',
pos=[0, 0, base.m_lz],
axis=[1, 0, 0],
limited='True',
range=[0., base.m_lx * (2 / 3) - finger.m_lz],
damping=1)
def _build(self, style):
labmaze_textures = labmaze_assets.get_sky_texture_paths(style)
self._mjcf_root = mjcf.RootElement(model='labmaze_' + style)
self._texture = self._mjcf_root.asset.add(
'texture',
type='skybox',
name='texture',
fileleft=labmaze_textures.left,
fileright=labmaze_textures.right,
fileup=labmaze_textures.up,
filedown=labmaze_textures.down,
filefront=labmaze_textures.front,
fileback=labmaze_textures.back)
def test_bind_stateless_actuators_only(self):
actuators = []
root = mjcf.RootElement()
body = root.worldbody.add('body')
body.add('geom', type='sphere', size=[0.1])
slider = body.add('joint', type='slide', name='slide_joint')
actuators.append(
root.actuator.add('velocity', joint=slider, name='velocity_act'))
actuators.append(
root.actuator.add('motor', joint=slider, name='motor_act'))
# `act` should be an empty array if there are no stateful actuators.
physics = mjcf.Physics.from_mjcf_model(root)
self.assertEqual(physics.bind(actuators).act.shape, (0, ))
def test_resolve_hierarchies_of_defaults(self):
root = mjcf.RootElement()
root.default.geom.type = 'box'
root.default.joint.pos = [0, 1, 0]
top1 = root.default.add('default', dclass='top1')
top1.geom.pos = [0.1, 0, 0]
top1.joint.pos = [1, 0, 0]
top1.joint.axis = [0, 0, 1]
sub1 = top1.add('default', dclass='sub1')
sub1.geom.size = [0.5]
top2 = root.default.add('default', dclass='top2')
top2.joint.pos = [0, 0, 1]
top2.joint.axis = [0, 1, 0]
top2.geom.type = 'sphere'
body = root.worldbody.add('body')
geom1 = body.add('geom', dclass=sub1)
mjcf.commit_defaults(geom1)
self.assert_same_attributes(
geom1, dict(dclass=sub1, type='box', size=[0.5], pos=[0.1, 0, 0]))
geom2 = body.add('geom', dclass=top1)
mjcf.commit_defaults(geom2)
self.assert_same_attributes(
geom2, dict(dclass=top1, type='box', pos=[0.1, 0, 0]))
geom3 = body.add('geom', dclass=top2)
mjcf.commit_defaults(geom3)
self.assert_same_attributes(geom3, dict(dclass=top2, type='sphere'))
geom4 = body.add('geom')
mjcf.commit_defaults(geom4)
self.assert_same_attributes(geom4, dict(type='box'))
joint1 = body.add('joint', dclass=sub1)
mjcf.commit_defaults(joint1)
self.assert_same_attributes(
joint1, dict(dclass=sub1, pos=[1, 0, 0], axis=[0, 0, 1]))
joint2 = body.add('joint', dclass=top2)
mjcf.commit_defaults(joint2)
self.assert_same_attributes(
joint2, dict(dclass=top2, pos=[0, 0, 1], axis=[0, 1, 0]))
joint3 = body.add('joint')
mjcf.commit_defaults(joint3)
self.assert_same_attributes(joint3, dict(pos=[0, 1, 0]))
def testCopyingWithAssets(self):
mjcf_model = parser.from_path(_MODEL_WITH_ASSETS_XML)
copied = mjcf.RootElement()
copied.include_copy(mjcf_model)
original_assets = (mjcf_model.find_all('mesh') +
mjcf_model.find_all('texture') +
mjcf_model.find_all('hfield'))
copied_assets = (copied.find_all('mesh') + copied.find_all('texture') +
copied.find_all('hfield'))
self.assertLen(copied_assets, len(original_assets))
for original_asset, copied_asset in zip(original_assets,
copied_assets):
self.assertIs(copied_asset.file, original_asset.file)
def __init__(self, num_legs):
self._model = mjcf.RootElement()
self._model.compiler.angle = 'radian'
rgba = np.random.uniform([0, 0, 0, 1], [1, 1, 1, 1])
# Torso
self._torso = self._model.worldbody.add('body', name='torso')
self._torso.add('geom', name='torso_geom', type='ellipsoid', size=_BODY_SIZE, rgba=rgba)
# Attach legs
for theta_idx in range(num_legs):
theta = theta_idx * 2 * np.pi / num_legs
leg_pos = _BODY_RADIUS * np.array([np.cos(theta), np.sin(theta), 0])
leg_site = self._torso.add('site', pos=leg_pos, euler=[0, 0, theta])
leg = Leg(length=_BODY_RADIUS, rgba=rgba)
leg_site.attach(leg.mjcf_model)
def test_grip_force(self):
arena = composer.Arena()
hand = kinova.JacoHand()
arena.attach(hand)
# A sphere with a touch sensor for measuring grip force.
prop_model = mjcf.RootElement(model='grip_target')
prop_model.worldbody.add('geom', type='sphere', size=[0.02])
touch_site = prop_model.worldbody.add('site',
type='sphere',
size=[0.025])
touch_sensor = prop_model.sensor.add('touch', site=touch_site)
prop = composer.ModelWrapperEntity(prop_model)
# Add some slide joints to allow movement of the target in the XY plane.
# This helps the contact solver to converge more reliably.
prop_frame = arena.attach(prop)
prop_frame.add('joint', name='slide_x', type='slide', axis=(1, 0, 0))
prop_frame.add('joint', name='slide_y', type='slide', axis=(0, 1, 0))
physics = mjcf.Physics.from_mjcf_model(arena.mjcf_model)
bound_pinch_site = physics.bind(hand.pinch_site)
bound_actuators = physics.bind(hand.actuators)
bound_joints = physics.bind(hand.joints)
bound_touch = physics.bind(touch_sensor)
# Position the grip target at the pinch site.
prop.set_pose(physics, position=bound_pinch_site.xpos)
# Close the fingers with as much force as the actuators will allow.
bound_actuators.ctrl = bound_actuators.ctrlrange[:, 1]
# Run the simulation forward until the joints stop moving.
physics.step()
qvel_thresh = 1e-3 # radians / s
while max(abs(bound_joints.qvel)) > qvel_thresh:
physics.step()
expected_min_grip_force = 20.
expected_max_grip_force = 30.
grip_force = bound_touch.sensordata
self.assertBetween(
grip_force,
expected_min_grip_force,
expected_max_grip_force,
msg='Expected grip force to be between {} and {} N, got {} N.'.
format(expected_min_grip_force, expected_max_grip_force,
grip_force))
def setUp(self):
super(ScaledActuatorsTest, self).setUp()
self._mjcf_model = mjcf.RootElement()
self._min = -1.4
self._max = 2.3
self._gain = 1.7
self._scaled_min = -0.8
self._scaled_max = 1.3
self._range = self._max - self._min
self._scaled_range = self._scaled_max - self._scaled_min
self._joints = []
for _ in range(2):
body = self._mjcf_model.worldbody.add('body')
body.add('geom', type='sphere', size=[1])
self._joints.append(body.add('joint', type='hinge'))
self._scaled_actuator_joint = self._joints[0]
self._standard_actuator_joint = self._joints[1]
self._random_state = np.random.RandomState(3474)