def _scene_objects(self):
z_offset = 0.2
tray = MujocoObject(object_name='tray',
pos=[0.5, 0, z_offset],
quat=[0, 0, 0, 0])
red_box = MujocoPrimitiveObject(obj_name='red_box',
obj_pos=[0.55, 0.1, z_offset + 0.2],
geom_rgba=[1, 0, 0, 1])
blue_box = MujocoPrimitiveObject(obj_name='blue_box',
obj_pos=[0.55, -0.1, z_offset + 0.2],
geom_rgba=[0, 0, 1, 1])
return [tray, red_box, blue_box]
def _goal_posts(center, rgba, team_name):
assert len(rgba) == 4
left_goal_post = MujocoPrimitiveObject(obj_pos=[center, 0.1, 0.35],
obj_name=f'left_goal_post_{team_name}',
geom_rgba=rgba,
geom_size=[0.005, 0.005, 0.02])
right_goal_post = MujocoPrimitiveObject(obj_pos=[center, -0.1, 0.35],
obj_name=f'right_goal_post_{team_name}',
geom_rgba=rgba,
geom_size=[0.005, 0.005, 0.02])
return [left_goal_post, right_goal_post]
def _scene_objects(self):
z_offset = 0.2
friction = [0.001, 0.001, 0.0001]
tray = MujocoObject(object_name='tray',
pos=[0.5, 0, z_offset],
quat=[0, 0, 0, 0],
friction=friction)
red_box = MujocoPrimitiveObject(obj_name='red_box',
obj_pos=[0.55, 0, z_offset + 0.2],
geom_rgba=[1, 0, 0, 1],
geom_friction=friction)
blue_box = MujocoPrimitiveObject(obj_name='blue_box',
obj_pos=[0.7, 0.5, z_offset + 0.2],
geom_rgba=[0, 0, 1, 1])
return [tray, red_box, blue_box]
def _boundaries():
left_boundary = MujocoPrimitiveObject(obj_pos=[0.8, 0.2, 0.35],
obj_name='left_boundary',
geom_rgba=BLACK,
geom_size=[0.5, 0.005, 0.02])
left_back_boundary = MujocoPrimitiveObject(obj_pos=[1.3, 0.15, 0.35],
obj_name='left_back_boundary',
geom_rgba=BLACK,
geom_size=[0.005, 0.05, 0.02])
right_boundary = MujocoPrimitiveObject(obj_pos=[0.8, -0.2, 0.35],
obj_name='right_boundary',
geom_rgba=BLACK,
geom_size=[0.5, 0.005, 0.02])
right_back_boundary = MujocoPrimitiveObject(obj_pos=[1.3, -0.15, 0.35],
obj_name='right_back_boundary',
geom_rgba=BLACK,
geom_size=[0.005, 0.05, 0.02])
return [left_boundary, right_boundary, left_back_boundary, right_back_boundary]
def _scene_objects(self):
z_offset = 0.2
tray = MujocoObject(object_name='tray',
pos=[0.5, 0, z_offset],
quat=[0, 0, 0, 0])
obj1 = MujocoPrimitiveObject(obj_name='box',
obj_pos=[0.5, 0, z_offset + 0.2],
geom_rgba=[1, 0, 0, 1])
return [tray, obj1]
def _scene_objects_new(self):
tray = MujocoObject(object_name='tray',
pos=[0.5, 0, Z_OFFSET],
quat=[0, 0, 0, 0])
ball = MujocoPrimitiveObject(obj_pos=[0.5, 0.0, Z_OFFSET + 0.2],
obj_name='ball',
mass=0.01,
geom_type='sphere',
geom_rgba=RED,
geom_size=[0.015, 0.015, 0.015])
return [tray, ball]
def _scene_objects(self):
table = MujocoPrimitiveObject(obj_pos=[0.85, 0.0, 0.2],
obj_name="table",
geom_size=[0.5, 0.5, 0.2],
mass=2000,
geom_material="table_mat")
player = MujocoPrimitiveObject(obj_pos=[0.4, 0.0, 0.35],
obj_name='player',
mass=0.01,
geom_rgba=BLUE,
geom_size=[0.015, 0.015, 0.015])
goalie = MujocoPrimitiveObject(obj_pos=[1.3, 0.0, 0.35],
obj_name='goalie',
mass=1,
geom_rgba=RED,
geom_size=[0.015, 0.015, 0.015])
ball = MujocoPrimitiveObject(obj_pos=[0.5, 0.0, 0.35],
obj_name='ball',
mass=0.01,
geom_type='sphere',
geom_rgba=WHITE,
geom_size=[0.015, 0.015, 0.015])
left_goal_post = MujocoPrimitiveObject(obj_pos=[1.3, 0.1, 0.35],
obj_name=f'left_goal_post',
geom_rgba=RED,
geom_size=[0.005, 0.005, 0.02])
right_goal_post = MujocoPrimitiveObject(obj_pos=[1.3, -0.1, 0.35],
obj_name=f'right_goal_post',
geom_rgba=RED,
geom_size=[0.005, 0.005, 0.02])
left_boundary = MujocoPrimitiveObject(obj_pos=[0.8, 0.2, 0.35],
obj_name='left_boundary',
geom_rgba=BLACK,
geom_size=[0.5, 0.005, 0.02])
left_back_boundary = MujocoPrimitiveObject(
obj_pos=[1.3, 0.15, 0.35],
obj_name='left_back_boundary',
geom_rgba=BLACK,
geom_size=[0.005, 0.05, 0.02])
right_boundary = MujocoPrimitiveObject(obj_pos=[0.8, -0.2, 0.35],
obj_name='right_boundary',
geom_rgba=BLACK,
geom_size=[0.5, 0.005, 0.02])
right_back_boundary = MujocoPrimitiveObject(
obj_pos=[1.3, -0.15, 0.35],
obj_name='right_back_boundary',
geom_rgba=BLACK,
geom_size=[0.005, 0.05, 0.02])
if self.include_objects:
obj_list = [
table, player, goalie, ball, left_goal_post, right_goal_post,
left_boundary, right_boundary, left_back_boundary,
right_back_boundary
]
else:
obj_list = []
return obj_list
def run_simple_demo():
table = MujocoPrimitiveObject(obj_pos=[0.9, 0.0, 0.2],
obj_name="table",
geom_size=[0.6, 0.5, 0.2],
mass=2000,
geom_material="table_mat")
cue_blue = MujocoPrimitiveObject(obj_pos=[0.4, 0.0, 0.35],
obj_name='cue_blue',
mass=0.01,
geom_rgba=BLUE,
geom_size=[0.015, 0.015, 0.015])
cue_red = MujocoPrimitiveObject(obj_pos=[1.3, 0.0, 0.35],
obj_name='cue_red',
mass=1,
geom_rgba=RED,
geom_size=[0.015, 0.015, 0.015])
ball1 = MujocoPrimitiveObject(obj_pos=[0.5, 0.0, 0.35],
obj_name='ball1',
mass=0.01,
geom_type='sphere',
geom_rgba=WHITE,
geom_size=[0.015, 0.015, 0.015])
ball2 = MujocoPrimitiveObject(obj_pos=[0.45, 0.07, 0.35],
obj_name='ball2',
mass=0.01,
geom_type='sphere',
geom_rgba=WHITE,
geom_size=[0.015, 0.015, 0.015])
goal_red = _goal_posts(1.3, RED, 'red')
boundaries = _boundaries()
object_list = [table, cue_red, ball1, ball2] + goal_red + boundaries
scene = Scene(object_list=object_list)
robot = Robots.MuJoCoRobot(scene, gravity_comp=True, num_DoF=7)
duration = 2
robot.ctrl_duration = duration
robot.startLogging()
home_position = robot.current_c_pos.copy()
home_orientation = robot.current_c_quat.copy()
# grab cue
# robot.gotoCartPositionAndQuat([0.4, 0.0, 0.5], [0, 1, 0, 0], duration=duration)
# robot.set_gripper_width = 0.04
# robot.gotoCartPositionAndQuat([0.4, 0.0, 0.41], [0, 1, 0, 0], duration=duration)
# robot.set_gripper_width = 0.0
# shoot ball1
robot.gotoCartPositionAndQuat([0.4, -0.02, 0.42], [0, 1, 0, 0], duration=duration)
robot.gotoCartPositionAndQuat([0.5, 0.03, 0.41], [0, 1, 0, 0], duration=0.18)
# shoot ball2
robot.gotoCartPositionAndQuat([0.35, 0.0, 0.42], [0, 1, 0, 0], duration=duration)
robot.gotoCartPositionAndQuat([0.35, 0.09, 0.42], [0, 1, 0, 0], duration=duration)
robot.gotoCartPositionAndQuat([0.45, 0.04, 0.41], [0, 1, 0, 0], duration=0.19)
# go home
robot.gotoCartPositionAndQuat(home_position, home_orientation, duration=duration)
robot.stopLogging()