def episode_restart(self):
Scene.episode_restart(self) # contains cpp_world.clean_everything()
lab_ground_pose = cpp_household.Pose()
lab_ground_pose.set_xyz(0, 0, 1)
lab_ground_pose.set_rpy(np.pi / 2, 0, 0)
# if self.zero_at_running_strip_start_line:
# lab_pose.set_xyz(0, 0, 0) # see RUN_STARTLINE, RUN_RAD constants
# scale seems not working
self.lab_ground = self.cpp_world.load_thingy(
os.path.join(os.path.dirname(__file__), "models_indoor/floor.obj"),
lab_ground_pose, 1.0, 0, 0xFFFFFF, True)
table_pose = cpp_household.Pose()
table_pose.set_rpy(0, 0, np.pi / 2)
table_pose.set_xyz(0.7, 0, 0.75)
self.table = self.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__),
"models_indoor/lab/bordered_table.urdf"), table_pose,
False, False)
slope_pose = cpp_household.Pose()
slope_pose.set_rpy(0, 0, np.pi / 2)
slope_pose.set_xyz(0.6, -0.4, 0.8)
self.slope = self.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__),
"models_indoor/lab/slope.urdf"), slope_pose, False,
False)
ball_pose = cpp_household.Pose()
ball_pose.set_xyz(0.6, -0.4, 0.88)
self.ball = self.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__),
"models_indoor/lab/ball.urdf"), ball_pose, False,
False)
self.ball_home_pose = ball_pose
self.ball_r = 0.025
frame_pose = cpp_household.Pose()
frame_pose.set_xyz(.95, 0.4, 0.88)
frame_pose.set_rpy(0, 0, np.pi / 2)
self.frame = self.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__),
"models_indoor/lab/frame.urdf"), frame_pose, False,
False)
self.frame_home_pose = frame_pose
self.frame_width = 0.14
self.frame_height = 0.07
self.ground_plane_mjcf = self.cpp_world.load_mjcf(
os.path.join(os.path.dirname(__file__),
"mujoco_assets/ground_plane.xml"))
def __init__(self, action_dim, obs_dim):
model_path = os.path.join(os.path.dirname(os.path.abspath(__file__)),
'assets/premaidai.urdf')
RoboschoolUrdfEnv.__init__(self,
model_urdf=model_path,
robot_name='base_link',
action_dim=action_dim,
obs_dim=obs_dim,
fixed_base=False,
self_collision=False)
self._last_camera_x = 0
self.rewards = []
self._last_body_rpy = None
self._last_body_speed = None
self._walk_target_distance = 0
self._walk_target_yaw = 0
self._target_xyz = (1e3, 0, 0.25) # kilometer away
self._feet_objects = []
self._head = None
scene = self.create_single_player_scene()
pose = cpp_household.Pose()
urdf = scene.cpp_world.load_urdf(model_path, pose, self.fixed_base,
self.self_collision)
self._angle_range = [[*j.limits()[:2]] for j in urdf.joints]
self._angle_range_low = [a_range[0] for a_range in self._angle_range]
self._angle_range_high = [a_range[1] for a_range in self._angle_range]
def set_initial_orientation(self, task, yaw_center, yaw_random_spread):
self.task = task
cpose = cpp_household.Pose()
yaw = yaw_center + self.np_random.uniform(low=-yaw_random_spread,
high=yaw_random_spread)
if task == self.TASK_WALK:
pitch = 0
roll = 0
cpose.set_xyz(self.start_pos_x, self.start_pos_y,
self.start_pos_z + 1.4)
elif task == self.TASK_STAND_UP:
pitch = np.pi / 2
roll = 0
cpose.set_xyz(self.start_pos_x, self.start_pos_y,
self.start_pos_z + 0.45)
elif task == self.TASK_ROLL_OVER:
pitch = np.pi * 3 / 2 - 0.15
roll = 0
cpose.set_xyz(self.start_pos_x, self.start_pos_y,
self.start_pos_z + 0.22)
else:
assert False
cpose.set_rpy(roll, pitch, yaw)
self.cpp_robot.set_pose_and_speed(cpose, 0, 0, 0)
self.initial_z = 0.8
def episode_restart(self):
Scene.episode_restart(self)
if self.score_right + self.score_left > 0:
sys.stdout.write("%i:%i " % (self.score_left, self.score_right))
sys.stdout.flush()
self.mjcf = self.cpp_world.load_mjcf(os.path.join(os.path.dirname(__file__), "models_robot/roboschool_pong.xml"))
dump = 0
for r in self.mjcf:
if dump: print("ROBOT '%s'" % r.root_part.name)
for part in r.parts:
if dump: print("\tPART '%s'" % part.name)
#if part.name==self.robot_name:
for j in r.joints:
if j.name=="p0x": self.p0x = j
if j.name=="p0y": self.p0y = j
if j.name=="p1x": self.p1x = j
if j.name=="p1y": self.p1y = j
if j.name=="ballx": self.ballx = j
if j.name=="bally": self.bally = j
self.ballx.set_motor_torque(0.0)
self.bally.set_motor_torque(0.0)
for r in self.mjcf:
r.query_position()
fpose = cpp_household.Pose()
fpose.set_xyz(0,0,-0.04)
self.field = self.cpp_world.load_thingy(
os.path.join(os.path.dirname(__file__), "models_outdoor/stadium/pong1.obj"),
fpose, 1.0, 0, 0xFFFFFF, True)
self.camera = self.cpp_world.new_camera_free_float(self.VIDEO_W, self.VIDEO_H, "video_camera")
self.camera_itertia = 0
self.frame = 0
self.jstate_for_frame = -1
self.score_left = 0
self.score_right = 0
self.restart_from_center(self.players_count==1 or self.np_random.randint(2)==0)
def alive_bonus(self, z, pitch):
if self.frame % 30 == 0 and self.frame > 100 and self.on_ground_frame_counter == 0:
target_xyz = np.array(self.body_xyz)
robot_speed = np.array(self.robot_body.speed())
angle = self.np_random.uniform(low=-3.14, high=3.14)
from_dist = 4.0
attack_speed = self.np_random.uniform(
low=20.0,
high=30.0) # speed 20..30 (* mass in cube.urdf = impulse)
time_to_travel = from_dist / attack_speed
target_xyz += robot_speed * time_to_travel # predict future position at the moment the cube hits the robot
cpose = cpp_household.Pose()
cpose.set_xyz(target_xyz[0] + from_dist * np.cos(angle),
target_xyz[1] + from_dist * np.sin(angle),
target_xyz[2] + 1.0)
attack_speed_vector = target_xyz - np.array(cpose.xyz())
attack_speed_vector *= attack_speed / np.linalg.norm(
attack_speed_vector)
attack_speed_vector += self.np_random.uniform(low=-1.0,
high=+1.0,
size=(3, ))
self.aggresive_cube.set_pose_and_speed(cpose, *attack_speed_vector)
if z < 0.8:
self.on_ground_frame_counter += 1
elif self.on_ground_frame_counter > 0:
self.on_ground_frame_counter -= 1
# End episode if the robot can't get up in 170 frames, to save computation and decorrelate observations.
return self.potential_leak(
) if self.on_ground_frame_counter < 170 else -1
def reset(self):
if self.scene is None:
self.scene = self.create_single_player_scene()
if not self.scene.multiplayer:
self.scene.episode_restart()
pose = cpp_household.Pose()
#import time
#t1 = time.time()
#print("fixed base: ", self.fixed_base)
self.urdf = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
self.model_urdf), pose, self.fixed_base,
self.self_collision)
#t2 = time.time()
#print("URDF load %0.2fms" % ((t2-t1)*1000))
self.ordered_joints = []
self.jdict = {}
self.parts = {}
self.frame = 0
self.done = 0
self.reward = 0
dump = 0
r = self.urdf
self.cpp_robot = r
if dump: print("ROBOT '%s'" % r.root_part.name)
if r.root_part.name == self.robot_name:
self.robot_body = r.root_part
for part in r.parts:
if dump: print("\tPART '%s'" % part.name)
self.parts[part.name] = part
if part.name == self.robot_name:
self.robot_body = part
for j in r.joints:
if j.name[1:7] != "Finger" and j.name[1:6] != "Thumb":
if dump:
print(
"\tALL JOINTS '%s' limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f"
% ((j.name, ) + j.limits()))
if j.name[:6] == "ignore":
j.set_motor_torque(0)
continue
j.power_coef, j.max_velocity = j.limits()[2:4]
self.ordered_joints.append(j)
self.jdict[j.name] = j
#else:
#print(j.name, "was ignored")
#print('joints length:', len(self.ordered_joints))
self.robot_specific_reset()
self.cpp_robot.query_position()
s = self.calc_state(
) # optimization: calc_state() can calculate something in self.* for calc_potential() to use
self.potential = self.calc_potential()
self.camera = self.scene.cpp_world.new_camera_free_float(
self.VIDEO_W, self.VIDEO_H, "video_camera")
return s
def episode_restart(self):
Scene.episode_restart(self) # contains cpp_world.clean_everything()
stadium_pose = cpp_household.Pose()
if self.zero_at_running_strip_start_line:
stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants
self.stadium = self.cpp_world.load_thingy(
os.path.join(os.path.dirname(__file__), "models_outdoor/stadium/stadium1.obj"),
stadium_pose, 1.0, 0, 0xFFFFFF, True)
self.ground_plane_mjcf = self.cpp_world.load_mjcf(os.path.join(os.path.dirname(__file__), "mujoco_assets/ground_plane.xml"))
def set_initial_orientation(self, yaw_center, yaw_random_spread):
cpose = cpp_household.Pose()
if not self.random_yaw:
yaw = yaw_center
else:
yaw = yaw_center + self.np_random.uniform(low=-yaw_random_spread, high=yaw_random_spread)
cpose.set_xyz(self.start_pos_x, self.start_pos_y, self.start_pos_z + self.initial_z)
cpose.set_rpy(0, 0, yaw) # just face random direction, but stay straight otherwise
self.cpp_robot.set_pose_and_speed(cpose, 0,0,0)
def robot_specific_reset(self):
for i, j in enumerate(self.ordered_joints):
j.reset_current_position(self.HOME_POSITION[i], 0)
self._feet_objects = [self.parts[name] for name in self.FOOT_NAME_LIST]
self.scene.actor_introduce(self)
cpose = cpp_household.Pose()
cpose.set_xyz(0, 0, 0.27)
cpose.set_rpy(0, 0, random.uniform(-radians(45), radians(45)))
self.cpp_robot.set_pose_and_speed(cpose, 0, 0, 0)
self._head = self.parts['head']
def step(self):
for i in range(len(self.m)):
mpose = cpp_household.Pose()
step_func = self.m[i][3]
x, y, z = self.m_list[i].pose().xyz()[0:3]
dx, dy, dz = self.m_list[i].speed()
x, y, z, dx, dy, dz = step_func(x, y, z, dx, dy, dz)
mpose.set_xyz(x, y, z)
mpose.set_rpy(0.0, 0.0, 0.0)
self.m_list[i].set_pose_and_speed(mpose, dx, dy, dz)
def robot_specific_reset(self):
pose_robot = cpp_household.Pose()
pose_robot.set_xyz(-0.8, 0, 0)
self.cpp_robot.set_pose_and_speed(pose_robot, 0, 0, 0)
# add table
pose_table = cpp_household.Pose()
self.urdf_table = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/table.urdf"),
pose_table, True, True)
# add tomato_soup_can
if "TOMATO_SOUP_CAN" in self.used_objects:
pose_tomato = cpp_household.Pose()
pose_tomato.set_xyz(0.3, 0.3, 0.5)
self.urdf_tomato = self.scene.cpp_world.load_urdf(
os.path.join(
os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/tomato_soup_can.urdf"),
pose_tomato, False, True)
# add banana
if "BANANA" in self.used_objects:
pose_banana = cpp_household.Pose()
pose_banana.set_xyz(0., 0., .35)
self.urdf_banana = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/banana.urdf"),
pose_banana, False, True)
# add hammer
if "HAMMER" in self.used_objects:
pose_hammer = cpp_household.Pose()
pose_hammer.set_xyz(0.2, -0.1, .25)
self.urdf_hammer = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/hammer.urdf"),
pose_hammer, False, True)
# add orange
if "ORANGE" in self.used_objects:
pose_orange = cpp_household.Pose()
pose_orange.set_xyz(0.2, -0.35, .3)
self.urdf_orange = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/orange.urdf"),
pose_orange, False, True)
# add mustard
if "mustard" in self.used_objects:
pose_mustard = cpp_household.Pose()
pose_mustard.set_xyz(-0.2, -0.35, .3)
self.urdf_mustard = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/mustard.urdf"),
pose_mustard, False, True)
def robot_specific_reset(self):
pose_robot = cpp_household.Pose()
pose_robot.set_xyz(-0.8, 0, 0)
self.cpp_robot.set_pose_and_speed(pose_robot, 0,0,0)
# add table
pose_table = cpp_household.Pose()
self.urdf_table = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/table.urdf"),
pose_table, True, True)
# add cube
pose_cube = cpp_household.Pose()
pose_cube.set_xyz(0.0, 0, 0.482)
self.urdf_cube = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__), "models_robot",
"kuka_gripper_description/urdf/cube.urdf"),
pose_cube, False, True)
def robot_specific_reset(self):
RoboschoolHumanoidFlagrun.robot_specific_reset(self)
cpose = cpp_household.Pose()
cpose.set_rpy(0, 0, 0)
cpose.set_xyz(-1.5, 0, 0.05)
self.aggresive_cube = self.scene.cpp_world.load_urdf(
os.path.join(os.path.dirname(__file__),
"models_household/cube.urdf"), cpose, False)
self.on_ground_frame_counter = 0
self.crawl_start_potential = None
self.crawl_ignored_potential = 0.0
self.initial_z = 0.8
def reset(self):
if self.scene is None:
self.scene = self.create_single_player_scene()
if not self.scene.multiplayer:
self.scene.episode_restart()
pose = cpp_household.Pose()
#import time
#t1 = time.time()
self.urdf = self.scene.cpp_world.load_urdf(self.model_urdf, pose,
self.fixed_base,
self.self_collision)
#t2 = time.time()
#print("URDF load %0.2fms" % ((t2-t1)*1000))
self.ordered_joints = []
self.jdict = {}
self.parts = {}
self.frame = 0
self.done = 0
self.reward = 0
dump = 0
r = self.urdf
self.cpp_robot = r
if dump: print("ROBOT '%s'" % r.root_part.name)
if r.root_part.name == self.robot_name:
self.robot_body = r.root_part
for part in r.parts:
if dump: print("\tPART '%s'" % part.name)
self.parts[part.name] = part
if part.name == self.robot_name:
self.robot_body = part
for j in r.joints:
if dump:
print(
"\tALL JOINTS '%s' limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f"
% ((j.name, ) + j.limits()))
if j.name[:6] == "ignore":
j.set_motor_torque(0)
continue
j.power_coef, j.max_velocity = j.limits()[2:4]
self.ordered_joints.append(j)
self.jdict[j.name] = j
#print("ordered_joints", len(self.ordered_joints))
self.robot_specific_reset()
self.cpp_robot.query_position()
s = self.calc_state()
self.potential = self.calc_potential()
self.camera = self.scene.cpp_world.new_camera_free_float(
self.VIDEO_W, self.VIDEO_H, "video_camera")
#self.camera.move_and_look_at(0,1,0,0,0,0)
return s
def build(self):
apose = cpp_household.Pose()
dirname = os.path.dirname
apose.set_xyz(self.ax * self.unit, self.ay * self.unit, self.az * self.unit)
self.agent = self.scene.cpp_world.load_urdf(os.path.join(
dirname(__file__), self.amodel), apose, False, False)
cpose = cpp_household.Pose()
self.b_list = []
for b in self.b:
cpose.set_xyz(b[0] * self.unit, b[1] * self.unit, b[2] * self.unit)
cpose.set_rpy(0.0, 0.0, 0.0)
self.b_list.append(self.scene.cpp_world.load_urdf(os.path.join(
dirname(__file__), b[4]
), cpose, False, False))
self.g_list, g_idx = [], 0
for g in self.g:
cpose.set_xyz(g[0] * self.unit, g[1] * self.unit, g[2] * self.unit)
cpose.set_rpy(0.0, 0.0, 0.0)
self.g_list.append(self.scene.cpp_world.load_urdf(os.path.join(
dirname(__file__), g[4]
), cpose, False, False))
self.g_list[-1].root_part.name = str(g_idx)
self.g_list[-1].alive = True
g_idx += 1
self.m_list = []
for m in self.m:
cpose.set_xyz(m[0] * self.unit, m[1] * self.unit, m[2] * self.unit)
cpose.set_rpy(0.0, 0.0, 0.0)
self.m_list.append(self.scene.cpp_world.load_urdf(os.path.join(
dirname(__file__), m[4]
), cpose, False, False))
return self.agent, apose, self.max_step, self.actions
def robot_specific_reset(self):
# load the ground
stadium_pose = cpp_household.Pose()
stadium_pose.set_xyz(0, 0, -0.05)
self.stadium = self.scene.cpp_world.load_thingy(
os.path.join(os.path.dirname(__file__),
"models_outdoor/stadium/pong1.obj"),
stadium_pose, 0.01, 0, 0xFFFFFF, True)
self.scene_builder = SceneBuilder(self.scene, self.history)
self.agent, apose, self.max_step, actions = self.scene_builder.build()
self.input_handler = InputHandler(self.agent, apose, actions)
# set agent camera_phi
goal = self.scene_builder.g[0]
dx, dy = self.scene_builder.ax - goal[0], self.scene_builder.ay - goal[1]
self.input_handler.camera_phi = np.arctan2(-dy, -dx)
self.step_count = 0
def episode_restart(self):
Scene.episode_restart(self)
stadium_pose = cpp_household.Pose()
if self.zero_at_running_strip_start_line:
stadium_pose.set_xyz(27, 21, 0)
if self.render:
if self.inclined:
self.hfield = self.cpp_world.load_thingy(
'assets/incline_grass.obj', stadium_pose, 1.0, 0, 0xFFFFFF,
True)
else:
self.stadium = self.cpp_world.load_thingy(
os.path.join(os.path.dirname(roboschool.__file__),
"models_outdoor/stadium/stadium1.obj"),
stadium_pose, 1.0, 0, 0xFFFFFF, True)
if self.inclined:
self.ground_plane_mjcf = self.cpp_world.load_mjcf(
"assets/incline_plane.mjcf")
else:
self.ground_plane_mjcf = self.cpp_world.load_mjcf(
"assets/level_plane.mjcf")
def robot_specific_reset(self):
RoboschoolForwardWalkersBase.robot_specific_reset(self)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += [
"right_hip_x", "right_hip_z", "right_hip_y", "right_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"left_hip_x", "left_hip_z", "left_hip_y", "left_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"right_shoulder1", "right_shoulder2", "right_elbow"
]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
if self.random_yaw:
cpose = cpp_household.Pose()
yaw = self.np_random.uniform(low=-3.14, high=3.14)
if self.random_lean and self.np_random.randint(2) == 0:
cpose.set_xyz(0, 0, 1.4)
if self.np_random.randint(2) == 0:
pitch = np.pi / 2
cpose.set_xyz(0, 0, 0.45)
else:
pitch = np.pi * 3 / 2
cpose.set_xyz(0, 0, 0.25)
roll = 0
cpose.set_rpy(roll, pitch, yaw)
else:
cpose.set_xyz(0, 0, 1.4)
cpose.set_rpy(
0, 0, yaw
) # just face random direction, but stay straight otherwise
self.cpp_robot.set_pose_and_speed(cpose, 0, 0, 0)
self.initial_z = 0.8
def alive_bonus(self, z, pitch):
if self.frame % 30 == 0 and self.frame > 100 and self.on_ground_frame_counter == 0:
target_xyz = np.array(self.body_xyz)
robot_speed = np.array(self.robot_body.speed())
angle = self.np_random.uniform(low=-3.14, high=3.14)
from_dist = 4.0
attack_speed = self.np_random.uniform(
low=20.0,
high=30.0) # speed 20..30 (* mass in cube.urdf = impulse)
time_to_travel = from_dist / attack_speed
target_xyz += robot_speed * time_to_travel # predict future position at the moment the cube hits the robot
cpose = cpp_household.Pose()
cpose.set_xyz(target_xyz[0] + from_dist * np.cos(angle),
target_xyz[1] + from_dist * np.sin(angle),
target_xyz[2] + 1.0)
attack_speed_vector = target_xyz - np.array(cpose.xyz())
attack_speed_vector *= attack_speed / np.linalg.norm(
attack_speed_vector)
attack_speed_vector += self.np_random.uniform(low=-1.0,
high=+1.0,
size=(3, ))
self.aggressive_cube.set_pose_and_speed(cpose,
*attack_speed_vector)
if z < 0.8:
if self.task == self.TASK_WALK:
self.on_ground_frame_counter = 10000 # This ends episode immediately
self.on_ground_frame_counter += 1
self.electricity_cost = RoboschoolHumanoid.electricity_cost / 5.0 # Don't care about electricity, just stand up!
elif self.on_ground_frame_counter > 0:
self.on_ground_frame_counter -= 1
self.electricity_cost = RoboschoolHumanoid.electricity_cost / 2.0 # Same as in Flagrun
else:
self.walking_normally += 1
self.electricity_cost = RoboschoolHumanoid.electricity_cost / 2.0
# End episode if the robot can't get up in 170 frames, to save computation and decorrelate observations.
return self.potential_leak(
) if self.on_ground_frame_counter < 170 else -1
