def __init__(self, render=False, on_rack=False):
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._theta0 = 0
self._update_action_every = 1. # update is every 50% of the step i.e., theta goes from 0 to pi/2
self._frequency = 2.
self._kp = 50
self._kd = 5
self.dt = 0.01
self._frame_skip = 10
self._n_steps = 0
self._action_dim = 12
self._obs_dim = 7
self._last_base_position = [0, 0, 0]
self._distance_limit = float("inf")
self._walkcon = walking_controller.WalkingController(
gait_type='trot',
spine_enable=False,
left_to_right_switch=True,
planning_space='joint_space',
frequency=self._frequency)
self._cam_dist = 1.0
self._cam_yaw = 0.0
self._cam_pitch = 0.0
## Gym env related mandatory variables
observation_high = np.array([10.0] * 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()
def __init__(self):
cwd = os.getcwd()
xml_path = cwd + '/envs/stoch2/stoch_two_urdf/stoch2.xml'
self._theta = 0
self._theta0 = 0
self._update_action_every = 1. # update is every 50% of the step i.e., theta goes from 0 to pi/2
self._frequency = 1
self._action_dim = 10
self._kp = 8.
self._kd = 0.2
self._actuator_indices = [7, 8, 10, 12, 14, 16, 17, 19, 21, 23]
self._xpos_previous = 0
self._walkcon = walking_controller.WalkingController(
gait_type='trot',
spine_enable=False,
left_to_right_switch=False,
frequency=self._frequency)
utils.EzPickle.__init__(self)
mujoco_env.MujocoEnv.__init__(self, xml_path,
1) # 2 is the frame_skip here
def __init__(self,
render = False,
on_rack = False,
gait = 'trot',
phase = [0,PI,PI,0],
action_dim = 10,
stairs = True):
self._is_stairs = stairs
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._theta0 = 0
self._update_action_every = 1. # update is every 50% of the step i.e., theta goes from 0 to pi/2
self._frequency = -2.8 #change back to 1
self._kp = 20
self._kd = 2
self.dt = 0.001
self._frame_skip = 5
self._n_steps = 0
self._action_dim = action_dim
# self._obs_dim = 7
self._obs_dim = 4
self.action = np.zeros(self._action_dim)
self._last_base_position = [0, 0, 0]
self._distance_limit = float("inf")
self._xpos_previous = 0.0
self._walkcon = walking_controller.WalkingController(gait_type=gait,
phase=phase)
self._cam_dist = 1.0
self._cam_yaw = 0.0
self._cam_pitch = 0.0
## Gym env related mandatory variables
observation_high = np.array([10.0] * 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()
if(self._is_stairs):
boxHalfLength = 0.06
boxHalfWidth = 2.5
boxHalfHeight = 0.02
sh_colBox = self._pybullet_client.createCollisionShape(self._pybullet_client.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
boxOrigin = 0.15
n_steps = 30
for i in range(n_steps):
block=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])
x = 1
def __init__(self, render=False, on_rack=False, frame_skip=1):
self._is_render = render
self._on_rack = on_rack
cwd = os.getcwd()
model_path = cwd + '/envs/stoch2/stoch_two_urdf/stoch2.xml'
self.FLx = np.array([
-0.0939, -0.0831, -0.0702, -0.0539, -0.0409, -0.0369, -0.0329,
-0.0289, -0.0248, -0.0208, -0.0180, -0.0168, -0.0157, -0.0145,
-0.0133, -0.0121, -0.0110, -0.0098, -0.0086, -0.0074, -0.0063,
-0.0051, -0.0039, -0.0027, -0.0016, -0.0004, 0.0008, 0.0020,
0.0037, 0.0055, 0.0074, 0.0092, 0.0111, 0.0129, 0.0148, 0.0168,
0.0193, 0.0217, 0.0241, 0.0265, 0.0289, 0.0305, 0.0322, 0.0338,
0.0354, 0.0370, 0.0372, 0.0370, 0.0368, 0.0365, 0.0363, 0.0361,
0.0359, 0.0357, 0.0355, 0.0353, 0.0352, 0.0347, 0.0334, 0.0321,
0.0309, 0.0294, 0.0269, 0.0244, 0.0218, 0.0195, 0.0173, 0.0152,
0.0130, 0.0109, 0.0087, 0.0066, 0.0044, 0.0022, 0.0001, -0.0021,
-0.0042, -0.0064, -0.0085, -0.0107, -0.0128, -0.0155, -0.0181,
-0.0208, -0.0234, -0.0261, -0.0287, -0.0314, -0.0363, -0.0418,
-0.0473, -0.0528, -0.0583, -0.0641, -0.0719, -0.0802, -0.0912,
-0.0949, -0.0949, -0.0939
])
self.FRx = np.array([
0.0368, 0.0365, 0.0363, 0.0361, 0.0359, 0.0357, 0.0355, 0.0353,
0.0352, 0.0347, 0.0334, 0.0321, 0.0309, 0.0294, 0.0269, 0.0244,
0.0218, 0.0195, 0.0173, 0.0152, 0.0130, 0.0109, 0.0087, 0.0066,
0.0044, 0.0022, 0.0001, -0.0021, -0.0042, -0.0064, -0.0085,
-0.0107, -0.0128, -0.0155, -0.0181, -0.0208, -0.0234, -0.0261,
-0.0287, -0.0314, -0.0363, -0.0418, -0.0473, -0.0528, -0.0583,
-0.0641, -0.0719, -0.0802, -0.0912, -0.0949, -0.0949, -0.0939,
-0.0939, -0.0831, -0.0702, -0.0539, -0.0409, -0.0369, -0.0329,
-0.0289, -0.0248, -0.0208, -0.0180, -0.0168, -0.0157, -0.0145,
-0.0133, -0.0121, -0.0110, -0.0098, -0.0086, -0.0074, -0.0063,
-0.0051, -0.0039, -0.0027, -0.0016, -0.0004, 0.0008, 0.0020,
0.0037, 0.0055, 0.0074, 0.0092, 0.0111, 0.0129, 0.0148, 0.0168,
0.0193, 0.0217, 0.0241, 0.0265, 0.0289, 0.0305, 0.0322, 0.0338,
0.0354, 0.0370, 0.0372, 0.0370
])
self.FLy = np.array([
-0.1992, -0.1941, -0.1905, -0.1896, -0.1896, -0.1892, -0.1888,
-0.1884, -0.1880, -0.1876, -0.1872, -0.1868, -0.1863, -0.1859,
-0.1854, -0.1850, -0.1845, -0.1841, -0.1836, -0.1832, -0.1827,
-0.1823, -0.1818, -0.1814, -0.1809, -0.1805, -0.1800, -0.1796,
-0.1792, -0.1788, -0.1784, -0.1780, -0.1776, -0.1772, -0.1768,
-0.1767, -0.1772, -0.1777, -0.1782, -0.1787, -0.1793, -0.1812,
-0.1831, -0.1850, -0.1869, -0.1888, -0.1911, -0.1936, -0.1960,
-0.1985, -0.2009, -0.2033, -0.2058, -0.2083, -0.2108, -0.2132,
-0.2157, -0.2181, -0.2203, -0.2226, -0.2248, -0.2268, -0.2276,
-0.2285, -0.2294, -0.2299, -0.2299, -0.2300, -0.2300, -0.2301,
-0.2302, -0.2302, -0.2303, -0.2304, -0.2304, -0.2305, -0.2305,
-0.2306, -0.2307, -0.2307, -0.2308, -0.2305, -0.2301, -0.2298,
-0.2294, -0.2291, -0.2287, -0.2284, -0.2278, -0.2272, -0.2266,
-0.2260, -0.2254, -0.2245, -0.2222, -0.2198, -0.2166, -0.2112,
-0.2052, -0.1992
])
self.FRy = np.array([
-0.1960, -0.1985, -0.2009, -0.2033, -0.2058, -0.2083, -0.2108,
-0.2132, -0.2157, -0.2181, -0.2203, -0.2226, -0.2248, -0.2268,
-0.2276, -0.2285, -0.2294, -0.2299, -0.2299, -0.2300, -0.2300,
-0.2301, -0.2302, -0.2302, -0.2303, -0.2304, -0.2304, -0.2305,
-0.2305, -0.2306, -0.2307, -0.2307, -0.2308, -0.2305, -0.2301,
-0.2298, -0.2294, -0.2291, -0.2287, -0.2284, -0.2278, -0.2272,
-0.2266, -0.2260, -0.2254, -0.2245, -0.2222, -0.2198, -0.2166,
-0.2112, -0.2052, -0.1992, -0.1992, -0.1941, -0.1905, -0.1896,
-0.1896, -0.1892, -0.1888, -0.1884, -0.1880, -0.1876, -0.1872,
-0.1868, -0.1863, -0.1859, -0.1854, -0.1850, -0.1845, -0.1841,
-0.1836, -0.1832, -0.1827, -0.1823, -0.1818, -0.1814, -0.1809,
-0.1805, -0.1800, -0.1796, -0.1792, -0.1788, -0.1784, -0.1780,
-0.1776, -0.1772, -0.1768, -0.1767, -0.1772, -0.1777, -0.1782,
-0.1787, -0.1793, -0.1812, -0.1831, -0.1850, -0.1869, -0.1888,
-0.1911, -0.1936
])
self._theta = 0
self._theta0 = 0
self._update_action_every = 1. # update is every step i.e., theta goes from 0 to pi
self._frame_skip = frame_skip
self._n_steps = 0
self._frequency = 2
self._kp = 30.
self._kd = 0.4 # DO NOT MODIFY THIS. if you change this, then the xml file needs changing as well
self._actuator_indices = [7, 8, 10, 12, 14, 16, 17, 19, 21, 23]
self._xpos_previous = 0
self._walkcon = walking_controller.WalkingController(
gait_type='trot',
spine_enable=False,
planning_space='polar_task_space',
left_to_right_switch=True,
frequency=self._frequency)
# utils.EzPickle.__init__(self)
mujoco_env.MujocoEnv.__init__(
self, model_path, 1
) # "frame_skip" option here is different. this integrates forward the simulation by "frame_skip=1" steps (for now).
## Gym env related mandatory variables
self.obs_dim = 7
self.action_dim = 10
self.action = np.zeros(self.action_dim)
observation_high = np.array([5.0] * self.obs_dim)
observation_low = -observation_high
self.observation_space = spaces.Box(observation_low, observation_high)
action_high = np.array([1] * self.action_dim)
action_low = -action_high
self.action_space = spaces.Box(action_low, action_high)
def __init__(self,
render = False,
on_rack = False):
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._theta0 = 0
self._update_action_every = 1. # update is every 50% of the step i.e., theta goes from 0 to pi/2
self._frequency = 1.
self._kp = 20
self._kd = 2.0
self.dt = 0.001
self._frame_skip = 5
self._n_steps = 0
self._action_dim = 10
self._obs_dim = 7
self.action = np.zeros(self._action_dim)
self._last_base_position = [0, 0, 0]
self._distance_limit = float("inf")
self._xpos_previous = 0.0
self._walkcon = walking_controller.WalkingController(gait_type='trot',
spine_enable = False,
planning_space = 'polar_task_space',
left_to_right_switch = True,
frequency=self._frequency)
self._cam_dist = 1.0
self._cam_yaw = 0.0
self._cam_pitch = 0.0
## Gym env related mandatory variables
observation_high = np.array([10.0] * 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.FLx=np.array([-0.0939,-0.0831,-0.0702,-0.0539,-0.0409,-0.0369,-0.0329,-0.0289,-0.0248,-0.0208,
-0.0180,-0.0168,-0.0157,-0.0145,-0.0133,-0.0121,-0.0110,-0.0098,-0.0086,-0.0074,
-0.0063,-0.0051,-0.0039,-0.0027,-0.0016,-0.0004,0.0008,0.0020,0.0037,0.0055,
0.0074,0.0092,0.0111,0.0129,0.0148,0.0168,0.0193,0.0217,0.0241,0.0265,
0.0289,0.0305,0.0322,0.0338,0.0354,0.0370,0.0372,0.0370,0.0368,0.0365,
0.0363,0.0361,0.0359,0.0357,0.0355,0.0353,0.0352,0.0347,0.0334,0.0321,
0.0309,0.0294,0.0269,0.0244,0.0218,0.0195,0.0173,0.0152,0.0130,0.0109,
0.0087,0.0066,0.0044,0.0022,0.0001,-0.0021,-0.0042,-0.0064,-0.0085,-0.0107,
-0.0128,-0.0155,-0.0181,-0.0208,-0.0234,-0.0261,-0.0287,-0.0314,-0.0363,-0.0418,
-0.0473,-0.0528,-0.0583,-0.0641,-0.0719,-0.0802,-0.0912,-0.0949,-0.0949,-0.0939])
self.FRx=np.array([0.0368,0.0365,0.0363,0.0361,0.0359,0.0357,0.0355,0.0353,0.0352,0.0347,
0.0334,0.0321,0.0309,0.0294,0.0269,0.0244,0.0218,0.0195,0.0173,0.0152,
0.0130,0.0109,0.0087,0.0066,0.0044,0.0022,0.0001,-0.0021,-0.0042,-0.0064,
-0.0085,-0.0107,-0.0128,-0.0155,-0.0181,-0.0208,-0.0234,-0.0261,-0.0287,-0.0314,
-0.0363,-0.0418,-0.0473,-0.0528,-0.0583,-0.0641,-0.0719,-0.0802,-0.0912,-0.0949,
-0.0949,-0.0939,-0.0939,-0.0831,-0.0702,-0.0539,-0.0409,-0.0369,-0.0329,-0.0289,
-0.0248,-0.0208,-0.0180,-0.0168,-0.0157,-0.0145,-0.0133,-0.0121,-0.0110,-0.0098,
-0.0086,-0.0074,-0.0063,-0.0051,-0.0039,-0.0027,-0.0016,-0.0004,0.0008,0.0020,
0.0037,0.0055,0.0074,0.0092,0.0111,0.0129,0.0148,0.0168,0.0193,0.0217,
0.0241,0.0265,0.0289,0.0305,0.0322,0.0338,0.0354,0.0370,0.0372,0.0370])
self.FLy=np.array([-0.1992,-0.1941,-0.1905,-0.1896,-0.1896,-0.1892,-0.1888,-0.1884,-0.1880,-0.1876,
-0.1872,-0.1868,-0.1863,-0.1859,-0.1854,-0.1850,-0.1845,-0.1841,-0.1836,-0.1832,
-0.1827,-0.1823,-0.1818,-0.1814,-0.1809,-0.1805,-0.1800,-0.1796,-0.1792,-0.1788,
-0.1784,-0.1780,-0.1776,-0.1772,-0.1768,-0.1767,-0.1772,-0.1777,-0.1782,-0.1787,
-0.1793,-0.1812,-0.1831,-0.1850,-0.1869,-0.1888,-0.1911,-0.1936,-0.1960,-0.1985,
-0.2009,-0.2033,-0.2058,-0.2083,-0.2108,-0.2132,-0.2157,-0.2181,-0.2203,-0.2226,
-0.2248,-0.2268,-0.2276,-0.2285,-0.2294,-0.2299,-0.2299,-0.2300,-0.2300,-0.2301,
-0.2302,-0.2302,-0.2303,-0.2304,-0.2304,-0.2305,-0.2305,-0.2306,-0.2307,-0.2307,
-0.2308,-0.2305,-0.2301,-0.2298,-0.2294,-0.2291,-0.2287,-0.2284,-0.2278,-0.2272,
-0.2266,-0.2260,-0.2254,-0.2245,-0.2222,-0.2198,-0.2166,-0.2112,-0.2052,-0.1992])
self.FRy=np.array([-0.1960,-0.1985,-0.2009,-0.2033,-0.2058,-0.2083,-0.2108,-0.2132,-0.2157,-0.2181,
-0.2203,-0.2226,-0.2248,-0.2268,-0.2276,-0.2285,-0.2294,-0.2299,-0.2299,-0.2300,
-0.2300,-0.2301,-0.2302,-0.2302,-0.2303,-0.2304,-0.2304,-0.2305,-0.2305,-0.2306,
-0.2307,-0.2307,-0.2308,-0.2305,-0.2301,-0.2298,-0.2294,-0.2291,-0.2287,-0.2284,
-0.2278,-0.2272,-0.2266,-0.2260,-0.2254,-0.2245,-0.2222,-0.2198,-0.2166,-0.2112,
-0.2052,-0.1992,-0.1992,-0.1941,-0.1905,-0.1896,-0.1896,-0.1892,-0.1888,-0.1884,
-0.1880,-0.1876,-0.1872,-0.1868,-0.1863,-0.1859,-0.1854,-0.1850,-0.1845,-0.1841,
-0.1836,-0.1832,-0.1827,-0.1823,-0.1818,-0.1814,-0.1809,-0.1805,-0.1800,-0.1796,
-0.1792,-0.1788,-0.1784,-0.1780,-0.1776,-0.1772,-0.1768,-0.1767,-0.1772,-0.1777,
-0.1782,-0.1787,-0.1793,-0.1812,-0.1831,-0.1850,-0.1869,-0.1888,-0.1911,-0.1936])
self.hard_reset()
def __init__(
self,
render=False,
on_rack=False,
gait='trot',
phase=[0, no_of_points, no_of_points, 0], # what is phase?
action_dim=10,
obs_dim=14,
scale=1.0,
roc=0.3,
stairs=True): #stairs?
self._is_stairs = stairs
self.scale = scale #INITIAL ONE FOR TRAINING
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._theta0 = 0
self._update_action_every = 1 / 20 # update is every 50% of the step i.e., theta goes from 0 to pi/2
self._frequency = 2.5 #change back to 1 .??
# self._frequency = 2.8 #change back to 1
self.frequency_weight = 1
self.prev_yaw = 0
self._kp = 150
self._kd = 5
self.dt = 0.005 # LET ME CHANGE, was 0.001
self._frame_skip = 25 # Working ratio is 5* self._dt
self._n_steps = 0
self._action_dim = action_dim
self._obs_dim = obs_dim
self.action = np.zeros(self._action_dim)
self._last_base_position = [0, 0, 0]
self._distance_limit = float("inf")
self._xpos_previous = 0.0
self._walkcon = walking_controller.WalkingController(gait_type=gait,
phase=phase,
scale=self.scale)
self._cam_dist = 1.0 #cam?
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_rpy = np.array([0, 0, 0])
self.obs_queue = deque([0] * 10, maxlen=10) #observation queue
self.termination_steps = 200
self.radius = roc
## Gym env related mandatory variables
observation_high = np.array([10.0] * self._obs_dim) #obs high?
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()
if (self._is_stairs): #stairs?
boxHalfLength = 0.06
boxHalfWidth = 2.5
boxHalfHeight = 0.02
sh_colBox = self._pybullet_client.createCollisionShape(
self._pybullet_client.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
boxOrigin = 0.15
n_steps = 30
for i in range(n_steps):
block = 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])
x = 1