def reset(self):
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=pb.GUI)
self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
else:
self._p = bc.BulletClient()
# self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
self._physics_client_id = self._p._client
p = self._p
p.resetSimulation()
urdfBotPath = gym_soccerbot.getDataPath()
self.soccerbotUid = p.loadURDF(os.path.join(urdfBotPath, "soccer_description/models/soccerbot_stl.urdf"),
useFixedBase=False,
useMaximalCoordinates=False,
basePosition=[0, 0, self._STANDING_HEIGHT],
baseOrientation=[0., 0., 0., 1.],
flags=pb.URDF_USE_INERTIA_FROM_FILE
| pb.URDF_USE_SELF_COLLISION
| pb.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
)
# load ramp
urdfRootPath = pybullet_data.getDataPath()
self.planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane_implicit.urdf"),
useMaximalCoordinates=True,
basePosition=[0, 0, 0])
# TODO change dynamics ...
# for i in range(p.getNumJoints(bodyUniqueId=self.soccerbotUid)):
# print(p.getJointInfo(bodyUniqueId=self.soccerbotUid, jointIndex=i)[1])
p.changeDynamics(self.planeUid, linkIndex=-1, lateralFriction=0.9, spinningFriction=0.9, rollingFriction=0)
# p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0.01, localInertiaDiagonal=[7e-7, 7e-7, 7e-7])
# p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0., localInertiaDiagonal=[0., 0., 0.])
'''
p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6,
frictionAnchor=1, lateralFriction=1,
rollingFriction=1, spinningFriction=1)
p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6,
frictionAnchor=1, lateralFriction=1,
rollingFriction=1, spinningFriction=1)
'''
# Simulation Physics General Settings
self.timeStep = 1. / 240
p.setTimeStep(self.timeStep)
p.setGravity(*self._GRAVITY_VECTOR)
p.setRealTimeSimulation(0) # To manually step simulation later
p = self._p
# Bring robot back to origin
p.resetBasePositionAndOrientation(self.soccerbotUid, [0, 0, self._STANDING_HEIGHT], [0., 0., 0., 1.])
p.resetBaseVelocity(self.soccerbotUid, [0, 0, 0], [0, 0, 0])
self.prev_lin_vel = np.array([0, 0, 0])
# p.resetJointStates(self.soccerbotUid, list(range(0, 18, 1)), 0)
# pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
# standing_poses = [0] * (self._JOINT_DIM + 2)
standing_poses = self._standing_poses(self.np_random)
# standing_poses = self._standing_poses()
# MX-28s:
for i in range(Joints.LEFT_LEG_1, Joints.HEAD_1):
p.resetJointState(self.soccerbotUid, i, standing_poses[i])
p.changeDynamics(self.soccerbotUid, i,
jointLowerLimit=-self._joint_limit_low[i],
jointUpperLimit=self._joint_limit_high[i],
jointLimitForce=self._MX_28_force)
# AX-12s:
for i in range(Joints.LEFT_ARM_1, Joints.LEFT_LEG_1):
p.resetJointState(self.soccerbotUid, i, standing_poses[i])
p.changeDynamics(self.soccerbotUid, i,
jointLowerLimit=-self._joint_limit_low[i],
jointUpperLimit=self._joint_limit_high[i],
jointLimitForce=self._AX_12_force)
p.resetJointState(self.soccerbotUid, Joints.HEAD_1, standing_poses[Joints.HEAD_1])
p.changeDynamics(self.soccerbotUid, Joints.HEAD_1,
jointLowerLimit=-np.pi, jointUpperLimit=np.pi,
jointLimitForce=self._AX_12_force)
p.resetJointState(self.soccerbotUid, Joints.HEAD_2, standing_poses[Joints.HEAD_2])
p.changeDynamics(self.soccerbotUid, Joints.HEAD_2,
jointLowerLimit=-np.pi, jointUpperLimit=np.pi,
jointLimitForce=self._AX_12_force)
# WARM UP SIMULATION
if self.WARM_UP:
warm_up = self.np_random.randint(0, 21)
for _ in range(warm_up):
p.stepSimulation()
p.stepSimulation()
# Get Observation
# self.st = RollingAvg(256, 0.01, 0.01)
self.prev_lin_vel = np.array([0, 0, 0])
self.imu_bias = np.concatenate((self.np_random.normal(0, self._IMU_LIN_STDDEV_BIAS, int(self._IMU_DIM / 2)),
self.np_random.normal(0, self._IMU_ANG_STDDEV_BIAS, int(self._IMU_DIM / 2))))
# Construct Observation
imu = self._imu()
feet = self._feet()
joints_pos = self._joints_pos()
joints_vel = self._joints_vel()
height = np.array([self._global_pos()[2]], dtype=self.DTYPE)
orn = self._global_orn()
observation = np.concatenate((joints_pos, joints_vel, imu, orn, height, feet))
# To keep up with the pipeline - 120Hz
p.stepSimulation()
p.stepSimulation()
if self._renders:
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
return observation
def reset(self):
if self._physics_client_id < 0:
if self._renders:
self._p = bc.BulletClient(connection_mode=pb.GUI)
self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
else:
self._p = bc.BulletClient()
# self._p.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
self._physics_client_id = self._p._client
p = self._p
p.resetSimulation()
# load ramp
# load soccerbot
# p.loadURDF("/home/shahryar/catkin_ws/src/soccer_ws/soccer_description/models/soccerbot_stl.urdf",
# p.loadURDF("/home/shahryar/PycharmProjects/DeepRL/gym-soccerbot/gym_soccerbot/soccer_description/models/soccerbot_stl.urdf",
# "/home/shahryar/PycharmProjects/DeepRL/gym-soccerbot/gym_soccerbot/soccer_description/models/soccerbot_stl.urdf",
urdfBotPath = gym_soccerbot.getDataPath()
self.soccerbotUid = p.loadURDF(os.path.join(urdfBotPath, "soccer_description/models/soccerbot_stl.urdf"),
useFixedBase=False,
useMaximalCoordinates=False,
basePosition=[0, 0, self.STANDING_HEIGHT],
baseOrientation=[0., 0., 0., 1.],
flags=pb.URDF_USE_INERTIA_FROM_FILE
| pb.URDF_USE_MATERIAL_COLORS_FROM_MTL
| pb.URDF_USE_SELF_COLLISION
| pb.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
)
# |p.URDF_USE_SELF_COLLISION|p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
urdfRootPath = pybullet_data.getDataPath()
self.planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane_implicit.urdf"),
useMaximalCoordinates=True,
basePosition=[0, 0, 0])
# TODO change dynamics ...
# for i in range(p.getNumJoints(bodyUniqueId=self.soccerbotUid)):
# print(p.getJointInfo(bodyUniqueId=self.soccerbotUid, jointIndex=i)[1])
p.changeDynamics(self.planeUid, linkIndex=-1, lateralFriction=0.9, spinningFriction=0.9, rollingFriction=0)
# p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0.01, localInertiaDiagonal=[7e-7, 7e-7, 7e-7])
# p.changeDynamics(self.soccerbotUid, linkIndex=Links.IMU, mass=0., localInertiaDiagonal=[0., 0., 0.])
'''
p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6,
frictionAnchor=1, lateralFriction=1,
rollingFriction=1, spinningFriction=1)
p.changeDynamics(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6,
frictionAnchor=1, lateralFriction=1,
rollingFriction=1, spinningFriction=1)
'''
# TODO change timestep ...
self.timeStep = 1./240
p.setTimeStep(self.timeStep)
p.setGravity(0, 0, -9.81)
self.gravity = [0, 0, -9.81]
p.setRealTimeSimulation(0) # To manually step simulation later
p = self._p
# TODO reset joint state
p.resetBasePositionAndOrientation(self.soccerbotUid, [0, 0, self.STANDING_HEIGHT], [0., 0., 0., 1.])
p.resetBaseVelocity(self.soccerbotUid, [0, 0, 0], [0, 0, 0])
self.prev_lin_vel = np.array([0, 0, 0])
#p.resetJointStates(self.soccerbotUid, list(range(0, 18, 1)), 0)
#pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
#standing_poses = [0] * (self.JOINT_DIM + 2)
standing_poses = self._standing_poses()
for i in range(self.JOINT_DIM + 2):
p.resetJointState(self.soccerbotUid, i, standing_poses[i])
# WARM UP SIMULATION
for _ in range(self.WARM_UP_STEPS):
p.stepSimulation()
# TODO set state???
# TODO get observation
feet = self._feet()
imu = self._imu()
joint_states = p.getJointStates(self.soccerbotUid, list(range(0, self.JOINT_DIM, 1)))
joints_pos = np.array([state[0] for state in joint_states], dtype=np.float32)
start_pos = np.array([0, 0, self.STANDING_HEIGHT], dtype=np.float32)
observation = np.concatenate((joints_pos, imu, start_pos, feet))
#pb.resetSimulation()
"""
From core.py in gym:
Returns:
observation (object): the initial observation.
"""
if self._renders:
pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
return observation