def __init__(self, render=False, on_rack=False): """ Initialize the class variables, set the directory path, and load the initial trajectiories. Define the action-space and the state-space, along with its dimensions. """ self._is_render = render self._on_rack = on_rack self.robot = Simulator.StochliteLoader(frame_skip = 5) self._walkcon = walking_controller.WalkingController(gait_type='trot', spine_enable = False, planning_space = 'polar_task_space', left_to_right_switch = True, frequency=self.robot.frequency, zero_desired_velocity = True) ##Gym env related mandatory variables observation_high = np.array([10.0] * self.robot.obs_dim) observation_low = -observation_high self.observation_space = spaces.Box(observation_low, observation_high) action_high = np.array([1] * self.robot.action_dim) self.action_space = spaces.Box(-action_high, action_high) self.robot.hard_reset(render, on_rack)
def __init__(self, render = False, on_rack = False, gait = 'trot', phase = [0, no_of_points, no_of_points,0],#[FR, FL, BR, BL] action_dim = 20, end_steps = 1000, stairs = False, downhill =False, seed_value = 100, wedge = True, IMU_Noise = False, deg = 5): self._is_stairs = stairs self._is_wedge = wedge self._is_render = render self._on_rack = on_rack self.rh_along_normal = 0.24 self.seed_value = seed_value random.seed(self.seed_value) if self._is_render: self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI) else: self._pybullet_client = bullet_client.BulletClient() self._theta = 0 self._frequency = 2.5 self.termination_steps = end_steps self.downhill = downhill #PD gains self._kp = 200 self._kd = 10 self.dt = 0.005 self._frame_skip = 25 self._n_steps = 0 self._action_dim = action_dim self._obs_dim = 11 self.action = np.zeros(self._action_dim) self._last_base_position = [0, 0, 0] self.last_yaw = 0 self._distance_limit = float("inf") self.current_com_height = 0.243 #wedge_parameters self.wedge_start = 0.5 self.wedge_halflength = 2 if gait is 'trot': phase = [0, no_of_points, no_of_points, 0] elif gait is 'walk': phase = [0, no_of_points, 3*no_of_points/2 ,no_of_points/2] self._walkcon = walking_controller.WalkingController(gait_type=gait, phase=phase) self.inverse = False self._cam_dist = 1.0 self._cam_yaw = 0.0 self._cam_pitch = 0.0 self.avg_vel_per_step = 0 self.avg_omega_per_step = 0 self.linearV = 0 self.angV = 0 self.prev_vel=[0,0,0] self.x_f = 0 self.y_f = 0 self.clips=7 self.friction = 0.6 self.ori_history_length = 3 self.ori_history_queue = deque([0]*3*self.ori_history_length, maxlen=3*self.ori_history_length)#observation queue self.step_disp = deque([0]*100, maxlen=100) self.stride = 5 self.incline_deg = deg self.incline_ori = 0 self.prev_incline_vec = (0,0,1) self.terrain_pitch = [] self.add_IMU_noise = IMU_Noise self.INIT_POSITION =[0,0,0.3] self.INIT_ORIENTATION = [0, 0, 0, 1] self.support_plane_estimated_pitch = 0 self.support_plane_estimated_roll = 0 self.pertub_steps = 0 self.x_f = 0 self.y_f = 0 ## Gym env related mandatory variables self._obs_dim = 3*self.ori_history_length + 2 #[r,p,y]x previous time steps, suport plane roll and pitch observation_high = np.array([np.pi/2] * self._obs_dim) observation_low = -observation_high self.observation_space = spaces.Box(observation_low, observation_high) action_high = np.array([1] * self._action_dim) self.action_space = spaces.Box(-action_high, action_high) self.hard_reset() self.Set_Randomization(default=True, idx1=2, idx2=2) if(self._is_stairs): boxHalfLength = 0.1 boxHalfWidth = 1 boxHalfHeight = 0.015 sh_colBox = self._pybullet_client.createCollisionShape(self._pybullet_client.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight]) boxOrigin = 0.3 n_steps = 15 self.stairs = [] for i in range(n_steps): step =self._pybullet_client.createMultiBody(baseMass=0,baseCollisionShapeIndex = sh_colBox,basePosition = [boxOrigin + i*2*boxHalfLength,0,boxHalfHeight + i*2*boxHalfHeight],baseOrientation=[0.0,0.0,0.0,1]) self.stairs.append(step) self._pybullet_client.changeDynamics(step, -1, lateralFriction=0.8)
def __init__( self, render=False, on_rack=False, gait='trot', phase=[0, no_of_points, no_of_points, 0], #[FR, FL, BR, BL] action_dim=20, end_steps=1000, stairs=False, downhill=False, seed_value=100, wedge=False, IMU_Noise=False, deg=5): # deg = 5 self._is_stairs = stairs self._is_wedge = wedge self._is_render = render self._on_rack = on_rack self.rh_along_normal = 0.24 self.seed_value = seed_value random.seed(self.seed_value) if self._is_render: self._pybullet_client = bullet_client.BulletClient( connection_mode=pybullet.GUI) else: self._pybullet_client = bullet_client.BulletClient() self._theta = 0 self._frequency = 2.5 # originally 2.5, changing for stability self.termination_steps = end_steps self.downhill = downhill #PD gains self._kp = 400 self._kd = 10 self.dt = 0.01 self._frame_skip = 50 self._n_steps = 0 self._action_dim = action_dim self._obs_dim = 11 self.action = np.zeros(self._action_dim) self._last_base_position = [0, 0, 0] self.last_yaw = 0 self._distance_limit = float("inf") self.current_com_height = 0.25 # 0.243 #wedge_parameters self.wedge_start = 0.5 self.wedge_halflength = 2 if gait is 'trot': phase = [0, no_of_points, no_of_points, 0] elif gait is 'walk': phase = [0, no_of_points, 3 * no_of_points / 2, no_of_points / 2] self._walkcon = walking_controller.WalkingController(gait_type=gait, phase=phase) self.inverse = False self._cam_dist = 1.0 self._cam_yaw = 0.0 self._cam_pitch = 0.0 self.avg_vel_per_step = 0 self.avg_omega_per_step = 0 self.linearV = 0 self.angV = 0 self.prev_vel = [0, 0, 0] self.x_f = 0 self.y_f = 0 self.clips = 7 self.friction = 0.6 self.ori_history_length = 3 self.ori_history_queue = deque( [0] * 3 * self.ori_history_length, maxlen=3 * self.ori_history_length) #observation queue self.step_disp = deque([0] * 100, maxlen=100) self.stride = 5 self.incline_deg = deg self.incline_ori = 0 self.prev_incline_vec = (0, 0, 1) self.terrain_pitch = [] self.add_IMU_noise = IMU_Noise self.support_plane_estimated_pitch = 0 self.support_plane_estimated_roll = 0 self.pertub_steps = 0 self.x_f = 0 self.y_f = 0 self.state_pos = StatePositionIndex() self.reset_pos = InitPosition() self.state_vel = StateVelocityIndex() self.actuaor = ActuatorIndex() self.limits = JoinLimit() mujoco_env.MujocoEnv.__ini__(self, stoch23.xml, 1) utils.EzPickle.__init__(self) ## Gym env related mandatory variables self._obs_dim = 3 * self.ori_history_length + 2 #[r,p,y]x previous time steps, suport plane roll and pitch observation_high = np.array([np.pi / 2] * self._obs_dim) observation_low = -observation_high self.observation_space = spaces.Box(observation_low, observation_high) action_high = np.array([1] * self._action_dim) self.action_space = spaces.Box(-action_high, action_high) self.commands = np.array( [0, 0, 0] ) #Joystick commands consisting of cmd_x_velocity, cmd_y_velocity, cmd_ang_velocity self.max_linear_xvel = 0.35 # calculation is < 0.2 m steplength times the frequency 2.5 Hz self.max_linear_yvel = 0 #0.25, made zero for only x direction # calculation is < 0.14 m times the frequency 2.5 Hz self.max_ang_vel = 0 #6, made zero for only x direction # less than one complete rotation in one second self.max_steplength = 0.2 # by the kinematic limits of the robot self.max_x_shift = 0.1 #plus minus 0.1 m self.max_y_shift = 0.14 # max 30 degree abduction self.max_z_shift = 0.1 # plus minus 0.1 m self.max_incline = 15 # in deg self.hard_reset() self.Set_Randomization(default=True, idx1=2, idx2=2) self.logger = DataLog() if (self._is_stairs): boxHalfLength = 0.1 boxHalfWidth = 1 boxHalfHeight = 0.015 sh_colBox = self._pybullet_client.createCollisionShape( self._pybullet_client.GEOM_BOX, halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight]) boxOrigin = 0.3 n_steps = 15 self.stairs = [] for i in range(n_steps): step = self._pybullet_client.createMultiBody( baseMass=0, baseCollisionShapeIndex=sh_colBox, basePosition=[ boxOrigin + i * 2 * boxHalfLength, 0, boxHalfHeight + i * 2 * boxHalfHeight ], baseOrientation=[0.0, 0.0, 0.0, 1]) self.stairs.append(step) self._pybullet_client.changeDynamics(step, -1, lateralFriction=0.8)
def __init__( self, render=False, on_rack=False, gait='trot', phase=[0, no_of_points, no_of_points, 0], #[FR, FL, BR, BL] action_dim=20, collect_data=False, end_steps=1000, IMU_Noise=False, deg=5): self._is_render = render self._on_rack = on_rack if self._is_render: self._pybullet_client = bullet_client.BulletClient( connection_mode=pybullet.GUI) else: self._pybullet_client = bullet_client.BulletClient() self._theta = 0 self._update_action_every = 1 / 50 self._frequency = 2.5 self.frequency_weight = 1 self.termination_steps = end_steps #PD gains self._kp = 200 self._kd = 10 self.dt = 0.005 self._frame_skip = 25 self._n_steps = 0 self._action_dim = action_dim self._obs_dim = 11 self.action = np.zeros(self._action_dim) self._last_base_position = [0, 0, 0] self.last_yaw = 0 self._distance_limit = float("inf") self.current_com_height = 0.243 #wedge_parameters self.wedge_start = 0.5 self.wedge_halflength = 2 self.wedge = [0, 0, 0, 0, 0] self.collect_data = collect_data if gait is 'trot': phase = [0, no_of_points, no_of_points, 0] elif gait is 'walk': phase = [0, no_of_points, 3 * no_of_points / 2, no_of_points / 2] self._walkcon = walking_controller.WalkingController(gait_type=gait, phase=phase) self.inverse = False self._cam_dist = 1.0 self._cam_yaw = 0.0 self._cam_pitch = 0.0 self.avg_vel_per_step = 0 self.avg_omega_per_step = 0 self.linearV = 0 self.angV = 0 self.prev_vel = [0, 0, 0] self.x_f = 0 self.y_f = 0 self.clips = 7 self.friction = 0.6 self.ori_history_length = 3 self.ori_history_queue = deque( [0] * 3 * self.ori_history_length, maxlen=3 * self.ori_history_length) #observation queue self.step_disp = deque([0] * 100, maxlen=100) self.stride = 5 self.incline_deg = deg self.incline_ori = 0 self.prev_incline_vec = (0, 0, 1) self.terrain_pitch = [] self.add_IMU_noise = IMU_Noise self.INIT_POSITION = [0, 0, 0.58] self.INIT_ORIENTATION = [0, 0, 0, 1] self.support_plane_estimated_pitch = 0 self.support_plane_estimated_roll = 0 self.pertub_steps = 0 self.x_f = 0 self.y_f = 0 ## Gym env related mandatory variables self._obs_dim = 3 * self.ori_history_length + 2 #[r,p,y]x previous time steps, suport plane roll and pitch observation_high = np.array([np.pi / 2] * self._obs_dim) observation_low = -observation_high self.observation_space = spaces.Box(observation_low, observation_high) action_high = np.array([1] * self._action_dim) self.action_space = spaces.Box(-action_high, action_high) self.hard_reset()