def place_soft():
name = "simple_peg_board"
# objpose = [[-0.41, -0.25, 0.816082], [0, 0.0344632, 0, 0.999406]]
# objpose = [[-0.386844, -0.337578, 0.955348], [0, 0.0344632, 0, 0.999406]]
objpose = [[-0.386844, -0.317578, 0.975348], [3.141590, 0.068940, 0]]
objpose = [[-0.00314217, -0.33487248, 0.45300905],
[-0.00198517, -0.70632716, 0.70786236, -0.005]]
objpose = [[-0.18978424, -0.17577134, 0.38183659],
[-0.50218089, 0.48843603, 0.51447979, -0.494]]
objpose = [[-0.3349516, 0.00327044, 0.45290458],
[-0.50128434, -0.49779569, 0.50398595, 0.496]]
stiffness = 1e5
g = 0
if stiffness > 1e5:
g = np.interp(stiffness, [1e5, 1e6], [1, 0])
else:
g = np.interp(stiffness, [1e4, 1e5], [0, 1])
b = np.interp(stiffness, [1e4, 1e5], [1, 0])
r = np.interp(stiffness, [1e5, 1e6], [0, 1])
string_model = PEG_BOARD.format(r, g, b, stiffness)
models = [[
Model(name,
objpose[0],
orientation=objpose[1],
file_type='string',
string_model=string_model,
reference_frame="base_link"),
create_box()
]]
GazeboModels(models, 'ur3_gazebo')
def place_models():
model_names = ["multi_peg_board"]
model_names = ["simple_peg_board"]
objpose = [[0.217947, 0.367654, 0.75], None]
objpose = [[0.402628, 0.275193, 0.807442], [0, -0.3556907, 0, 0.9346037]]
objpose = [[0.198303, 0.244189, 0.75], None]
objpose = [[-0.45, -0.20, 0.86], [0, 0.1986693, 0, 0.9800666]]
objpose = [[-0.381961, -0.250909, 0.816082], None] #experiments so far
models = [[Model(model_names[0], objpose[0], orientation=objpose[1])]]
GazeboModels(models, 'ur3_gazebo')
def create_box():
obj = BOX % ("box", "0.1", "0.1", "0.01", "Black", "0.1", "0.1", "0.01")
model_names = ["box"]
# objpose = [[-0.41, -0.25, 0.811082], [0, 0.0344632, 0, 0.999406]]
objpose = [[-0.386844, -0.337578, 0.950348], [0, 0.0344632, 0, 0.999406]]
return Model(model_names[0],
objpose[0],
orientation=objpose[1],
file_type='string',
string_model=obj,
reference_frame="world")
def place_ball():
sphere = SPHERE_COLLISION.format("ball", "0.1", "Yellow", "0.1", 2e5)
model_names = ["ball"]
objpose = [[0.608678, 0.0, (0.25939 + 0.685)], [0, 0, 0]]
# 0.25939 + 0.685
models = [
Model(model_names[0],
objpose[0],
file_type='string',
string_model=sphere,
reference_frame="world")
]
spawner.load_models(models)
def place_aruco():
name = "Apriltag36_11_00000"
# objpose = [[-0.53, 0.25, 0.82], [0,0,0]] # world coordinates
objpose = [[0.20, -0.30, 1.00], [0, 0, 0]] # world coordinates
objpose = [[-0.65, 0.0, 0.78], [0, 0, np.pi / 8.0]] # world coordinates
models = [
Model(name,
objpose[0],
orientation=objpose[1],
file_type='sdf',
reference_frame="world")
]
spawner.load_models(models)
def place_target():
sphere = SPHERE % ("target", "0.02", "GreenTransparent")
model_names = ["target"]
objpose = [[0.0131, 0.4019, 0.3026]]
objpose = [[-0.13101034, 0.37818616, 0.50852045], [0, 0, 0]]
models = [
Model(model_names[0],
objpose[0],
file_type='string',
string_model=sphere,
reference_frame="world")
]
spawner.load_models(models)
def place_target():
sphere = SPHERE % ("target", "0.025", "GreenTransparent")
model_names = ["target"]
objpose = [[0.0131, 0.4019, 0.3026]]
objpose = [[-0.13101034, 0.37818616, 0.50852045]]
models = [[
Model(model_names[0],
objpose[0],
file_type='string',
string_model=sphere,
reference_frame="base_link")
]]
GazeboModels(models, 'ur3_gazebo')
def place_cube():
cube_lenght = "0.2"
obj = BOX % ("box", cube_lenght, cube_lenght, cube_lenght, "Yellow",
cube_lenght, cube_lenght, cube_lenght)
model_names = ["box"]
objpose = [[0.618678, 0.0, 0.955148], [0, 0.0, 0, 0.0]]
models = [
Model(model_names[0],
objpose[0],
file_type='string',
string_model=obj,
reference_frame="world")
]
spawner.load_models(models)
def place_soft():
name = "simple_peg_board"
objpose = [[-0.3349516, 0.00327044, 0.45290458],
[-0.50128434, -0.49779569, 0.50398595, 0.496]]
stiffness = 1e5
g = 0
if stiffness > 1e5:
g = np.interp(stiffness, [1e5, 1e6], [1, 0])
else:
g = np.interp(stiffness, [1e4, 1e5], [0, 1])
b = np.interp(stiffness, [1e4, 1e5], [1, 0])
r = np.interp(stiffness, [1e5, 1e6], [0, 1])
string_model = PEG_BOARD.format(r, g, b, stiffness)
models = [[
Model(name,
objpose[0],
orientation=objpose[1],
file_type='string',
string_model=string_model,
reference_frame="base_link")
]]
spawner.load_models(models)
def create_gazebo_marker(pose, reference_frame, marker_id=None):
marker_pose = [pose[:3].tolist(), pose[3:].tolist()]
return Model("visual_marker", marker_pose[0], orientation=marker_pose[1], reference_frame=reference_frame, model_id=marker_id)
def place_door():
name = "hinged_door"
objpose = [[-0.40, 0.20, 0.76], [0, 0, 0.9238795, 0.3826834]]
models = [[Model(name, objpose[0], orientation=objpose[1])]]
spawner.load_models(models)
def place_models():
model_names = ["multi_peg_board"]
model_names = ["simple_peg_board"]
objpose = [[-0.45, -0.20, 0.86], [0, 0.1986693, 0, 0.9800666]]
models = [Model(model_names[0], objpose[0], orientation=objpose[1])]
spawner.load_models(models)
def create_gazebo_marker(pose, reference_frame, marker_id=None):
from ur_gazebo.model import Model
pose = np.array(pose)
marker_pose = [pose[:3].tolist(), pose[3:].tolist()]
return Model("visual_marker", marker_pose[0], orientation=marker_pose[1], reference_frame=reference_frame, model_id=marker_id)
def place_ring():
name = "lens_ring"
objpose = [[-0.370381, -0.23, 0.82], [0, 0, -0.7071068, 0.7071068]]
models = [[Model(name, objpose[0], orientation=objpose[1])]]
GazeboModels(models, 'ur3_gazebo')
def place_door():
name = "hinged_door"
objpose = [[-0.40, 0.20, 0.76], [0, 0, 0.9238795, 0.3826834]]
models = [[Model(name, objpose[0], orientation=objpose[1])]]
GazeboModels(models, 'ur3_gazebo')