def createSingleBody(name, shape): _collisionData = tysoc_bindings.PyCollisionData() _visualData = tysoc_bindings.PyVisualData() _bodyData = tysoc_bindings.PyBodyData() if shape == 'box': _collisionData.type = tysoc_bindings.eShapeType.BOX _collisionData.size = [0.2, 0.2, 0.2] _visualData.type = tysoc_bindings.eShapeType.BOX _visualData.size = [0.2, 0.2, 0.2] elif shape == 'sphere': _collisionData.type = tysoc_bindings.eShapeType.SPHERE _collisionData.size = [0.1, 0.1, 0.1] _visualData.type = tysoc_bindings.eShapeType.SPHERE _visualData.size = [0.1, 0.1, 0.1] elif shape == 'cylinder': _collisionData.type = tysoc_bindings.eShapeType.CYLINDER _collisionData.size = [0.1, 0.2, 0.1] _visualData.type = tysoc_bindings.eShapeType.CYLINDER _visualData.size = [0.1, 0.2, 0.1] elif shape == 'capsule': _collisionData.type = tysoc_bindings.eShapeType.CAPSULE _collisionData.size = [0.1, 0.2, 0.1] _visualData.type = tysoc_bindings.eShapeType.CAPSULE _visualData.size = [0.1, 0.2, 0.1] elif shape == 'mesh': _collisionData.type = tysoc_bindings.eShapeType.MESH _collisionData.size = [0.2, 0.2, 0.2] _collisionData.filename = pytysoc.PATHS.RESOURCES_DIR + "meshes/monkey.stl" _visualData.type = tysoc_bindings.eShapeType.MESH _visualData.size = [0.2, 0.2, 0.2] _visualData.filename = pytysoc.PATHS.RESOURCES_DIR + "meshes/monkey.stl" _visualData.ambient = [0.7, 0.5, 0.3] _visualData.diffuse = [0.7, 0.5, 0.3] _visualData.specular = [0.7, 0.5, 0.3] _visualData.shininess = 50.0 _bodyData = tysoc_bindings.PyBodyData() _bodyData.dyntype = tysoc_bindings.eDynamicsType.DYNAMIC _bodyData.addCollision(_collisionData) _bodyData.addVisual(_visualData) _position = 4.0 * (np.random.rand(3) - 0.5) _position[2] = 3.0 _rotation = np.pi * (np.random.rand(3) - 0.5) _body = tysoc_bindings.PyBody(name, _bodyData, _position, _rotation) return _body
def createHfield(name, position): ## nxSamples = 50 ## nySamples = 50 ## xExtent = 5.0 ## yExtent = 5.0 ## _x = xExtent * np.linspace( -0.5, 0.5, nxSamples ) ## _y = yExtent * np.linspace( -0.5, 0.5, nySamples ) ## _xx, _yy = np.meshgrid( _x, _y ) ## _zz = 10.0 * ( _xx ** 2 + _yy ** 2 ) / ( xExtent ** 2 + yExtent ** 2 ) # Adapted from dm_control quadruped.py # https://github.com/deepmind/dm_control/blob/master/dm_control/suite/quadruped.py nxSamples = 50 nySamples = 50 xExtent = 10.0 yExtent = 10.0 _TERRAIN_SMOOTHNESS = 0.15 # 0.0: maximally bumpy; 1.0: completely smooth. _TERRAIN_BUMP_SCALE = 2 # Spatial scale of terrain bumps (in meters). # Sinusoidal bowl shape. _xx, _yy = np.ogrid[-1:1:nxSamples * 1j, -1:1:nySamples * 1j] radius = np.clip(np.sqrt(_xx**2 + _yy**2), .04, 1) bowl_shape = .5 - np.cos(2 * np.pi * radius) / 2 # Random smooth bumps. terrain_size = 2 * xExtent bump_res = int(terrain_size / _TERRAIN_BUMP_SCALE) bumps = np.random.uniform(_TERRAIN_SMOOTHNESS, 1, (bump_res, bump_res)) smooth_bumps = ndimage.zoom(bumps, nxSamples / float(bump_res)) # Terrain is elementwise product. _zz = bowl_shape * smooth_bumps _maxHeight = np.max(_zz) _heightData = (_zz / _maxHeight).ravel() _heightFieldData = tysoc_bindings.PyHeightFieldData() _heightFieldData.nWidthSamples = nxSamples _heightFieldData.nDepthSamples = nySamples _heightFieldData.heightData = _heightData _collisionData = tysoc_bindings.PyCollisionData() _collisionData.type = tysoc_bindings.eShapeType.HFIELD _collisionData.size = [xExtent, yExtent, _maxHeight] _collisionData.hdata = _heightFieldData _visualData = tysoc_bindings.PyVisualData() _visualData.type = tysoc_bindings.eShapeType.HFIELD _visualData.size = [xExtent, yExtent, _maxHeight] _visualData.hdata = _heightFieldData _visualData.ambient = [0.2, 0.3, 0.4] _visualData.diffuse = [0.2, 0.3, 0.4] _visualData.specular = [0.2, 0.3, 0.4] _visualData.shininess = 50.0 _bodyData = tysoc_bindings.PyBodyData() _bodyData.dyntype = tysoc_bindings.eDynamicsType.STATIC _bodyData.addCollision(_collisionData) _bodyData.addVisual(_visualData) _body = tysoc_bindings.PyBody(name, _bodyData, position, [0.0, 0.0, 0.0]) return _body
def createDoorVersion1( name, position ) : _compound = tysoc.PyCompound( name, position, [0.1,0.2,0.3], tysoc.eDynamicsType.DYNAMIC ) ## create compound bodies separately, compose them and add to the compound afterwards _frame_0_coldata = tysoc.PyCollisionData() _frame_0_coldata.type = tysoc.eShapeType.BOX _frame_0_coldata.size = [ 0.2, 0.2, 2.0 ] _frame_0_visdata = tysoc.PyVisualData() _frame_0_visdata.type = tysoc.eShapeType.BOX _frame_0_visdata.size = [ 0.2, 0.2, 2.0 ] _frame_0_visdata.ambient = [ 0.4, 0.3, 0.2 ] _frame_0_visdata.diffuse = [ 0.4, 0.3, 0.2 ] _frame_0_visdata.specular = [ 0.4, 0.3, 0.2 ] _frame_0_visdata.shininess = 50.0 _frame_0_body_data = tysoc.PyBodyData() _frame_0_body_data.collision = _frame_0_coldata _frame_0_body_data.visual = _frame_0_visdata _frame_0_local_tf = fromPositionAndRotation( [ 0.0, -0.5, 1.0 ], np.identity(3) ) _frame_0_body = _compound.createRootBody( "frame_0", _frame_0_body_data, _frame_0_local_tf ) _frame_1_coldata = tysoc.PyCollisionData() _frame_1_coldata.type = tysoc.eShapeType.BOX _frame_1_coldata.size = [ 0.2, 0.8, 0.2 ] _frame_1_visdata = tysoc.PyVisualData() _frame_1_visdata.type = tysoc.eShapeType.BOX _frame_1_visdata.size = [ 0.2, 0.8, 0.2 ] _frame_1_visdata.ambient = [ 0.4, 0.3, 0.2 ] _frame_1_visdata.diffuse = [ 0.4, 0.3, 0.2 ] _frame_1_visdata.specular = [ 0.4, 0.3, 0.2 ] _frame_1_visdata.shininess = 50.0 _frame_1_body_data = tysoc.PyBodyData() _frame_1_body_data.collision = _frame_1_coldata _frame_1_body_data.visual = _frame_1_visdata _frame_1_joint_data = tysoc.PyJointData() _frame_1_joint_data.type = tysoc.eJointType.FIXED _frame_1_joint_data.localTransform = np.identity( 4 ) ## identity (same ref-frame as body's frame) _frame_1_local_tf = fromPositionAndRotation( [ 0.0, 0.5, 0.9 ], np.identity(3) ) _frame_1_body = tysoc.PyCompoundBody( "frame_1", _frame_1_body_data, _frame_1_joint_data, _frame_0_body, _frame_1_local_tf[:3,3], _frame_1_local_tf[:3,:3] ) _frame_2_coldata = tysoc.PyCollisionData() _frame_2_coldata.type = tysoc.eShapeType.BOX _frame_2_coldata.size = [ 0.2, 0.2, 2.0 ] _frame_2_visdata = tysoc.PyVisualData() _frame_2_visdata.type = tysoc.eShapeType.BOX _frame_2_visdata.size = [ 0.2, 0.2, 2.0 ] _frame_2_visdata.ambient = [ 0.4, 0.3, 0.2 ] _frame_2_visdata.diffuse = [ 0.4, 0.3, 0.2 ] _frame_2_visdata.specular = [ 0.4, 0.3, 0.2 ] _frame_2_visdata.shininess = 50.0 _frame_2_body_data = tysoc.PyBodyData() _frame_2_body_data.collision = _frame_2_coldata _frame_2_body_data.visual = _frame_2_visdata _frame_2_joint_data = tysoc.PyJointData() _frame_2_joint_data.type = tysoc.eJointType.FIXED _frame_2_joint_data.localTransform = np.identity( 4 ) ## identity (same ref-frame as body's frame) _frame_2_local_tf = fromPositionAndRotation( [ 0.0, 0.5, -0.9 ], np.identity(3) ) _frame_2_body = tysoc.PyCompoundBody( "frame_2", _frame_2_body_data, _frame_2_joint_data, _frame_1_body, _frame_2_local_tf[:3,3], _frame_2_local_tf[:3,:3] ) _panel_coldata = tysoc.PyCollisionData() _panel_coldata.type = tysoc.eShapeType.BOX _panel_coldata.size = [ 1.2, 0.1, 2.0 ] _panel_visdata = tysoc.PyVisualData() _panel_visdata.type = tysoc.eShapeType.BOX _panel_visdata.size = [ 1.2, 0.1, 2.0 ] _panel_visdata.ambient = [ 0.4, 0.3, 0.5 ] _panel_visdata.diffuse = [ 0.4, 0.3, 0.5 ] _panel_visdata.specular = [ 0.4, 0.3, 0.5 ] _panel_visdata.shininess = 50.0 _panel_body_data = tysoc.PyBodyData() _panel_body_data.collision = _panel_coldata _panel_body_data.visual = _panel_visdata _panel_joint_data = tysoc.PyJointData() _panel_joint_data.type = tysoc.eJointType.REVOLUTE _panel_joint_data.axis = [ 0.0, 0.0, 1.0 ] _panel_joint_data.limits = [ -0.5 * np.pi, 0.5 * np.pi ] _panel_joint_data.localTransform = fromPositionAndRotation( [ -0.6, -0.05, 0.0 ], np.identity(3) ) _panel_local_tf = fromPositionAndRotation( [ 0.7, -0.15, 0.0 ], np.identity(3) ) _panel_body = tysoc.PyCompoundBody( "panel", _panel_body_data, _panel_joint_data, _frame_0_body, _panel_local_tf[:3,3], _panel_local_tf[:3,:3] ) ## add all bodies (the compound is just a handy container) _compound.addCompoundBody( _frame_1_body ) _compound.addCompoundBody( _frame_2_body ) _compound.addCompoundBody( _panel_body ) ##********************************************************************************************** return _compound