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