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()