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