def reset(self, force_randomize=None):
if hasattr(self, 'terrain'):
p.removeBody(self.terrain, physicsClientId=self.client_ID)
if hasattr(self, 'robot'):
p.removeBody(self.robot, physicsClientId=self.client_ID)
del self.robot
del self.terrain
del self.target_body
self.target = None
p.resetSimulation(physicsClientId=self.client_ID)
p.setGravity(0, 0, -9.8, physicsClientId=self.client_ID)
p.setRealTimeSimulation(0, physicsClientId=self.client_ID)
self.robot = self.load_robot()
if not self.config["terrain_name"] == "flat":
self.terrain = self.generate_rnd_env()
else:
self.terrain = p.loadURDF("plane.urdf", physicsClientId=self.client_ID)
self.create_targets()
# Reset episodal vars
self.step_ctr = 0
self.episode_ctr += 1
self.prev_yaw_dev = 0
# Get heightmap height at robot position
if self.terrain is None or self.config["terrain_name"] == "flat":
spawn_height = 0
else:
spawn_height = 0.5 * np.max(
self.terrain_hm[self.config["env_length"] // 2 - 3:self.config["env_length"] // 2 + 3,
self.config["env_width"] // 2 - 3: self.config["env_width"] // 2 + 3]) * self.config["mesh_scale_vert"]
# Random initial rotation
rnd_rot = np.random.rand() * 0.3 - 0.15
rnd_quat = p.getQuaternionFromAxisAngle([0, 0, 1], rnd_rot)
joint_init_pos_list = self.norm_to_rads([0] * 18)
[p.resetJointState(self.robot, i, joint_init_pos_list[i], 0, physicsClientId=self.client_ID) for i in range(18)]
p.resetBasePositionAndOrientation(self.robot, [0, 0, spawn_height + 0.15], rnd_quat,
physicsClientId=self.client_ID)
p.setJointMotorControlArray(bodyUniqueId=self.robot,
jointIndices=range(18),
controlMode=p.POSITION_CONTROL,
targetPositions=[0] * 18,
forces=[self.config["max_joint_force"]] * 18,
physicsClientId=self.client_ID)
self.update_targets()
self.prev_target_dist = np.sqrt((0 - self.target[0]) ** 2 + (0 - self.target[1]) ** 2)
tar_angle = np.arctan2(self.target[1] - 0, self.target[0] - 0)
self.prev_yaw_deviation = np.min((abs((rnd_rot % 6.283) - (tar_angle % 6.283)), abs(rnd_rot - tar_angle)))
for i in range(10):
p.stepSimulation(physicsClientId=self.client_ID)
obs, _, _, _ = self.step(np.zeros(self.act_dim))
return obs
def __init__(self,
initial_pos=[0, 0, 0],
inital_orn=bullet.getQuaternionFromAxisAngle([0, 0, 0]),
time_step=0.01):
super().__init__(self.model_path,
initial_pos,
inital_orn,
time_step=time_step)
self.arm_link_num = 8
self.gripper_link_num = 10
self.arm_dof_num = 6
self.dof_num = self.arm_dof_num + self.gripper_dof_num
self.gripper_dof_num = 6
self.arm_idx_range = range(0, self.arm_dof_num)
self.griper_idx_range = range(self.arm_dof_num,
self.arm_link_num + self.gripper_dof_num)
self.joint_indices = range(self.arm_dof_num + self.gripper_dof_num)
self.init_action_space()
self.init_observation_space()
bullet.enableJointForceTorqueSensor(self.id,
range(self.dof_num),
enableSensor=True)
def box(size=8):
orientation_y = p.getQuaternionFromAxisAngle(axis=[0, 1, 0],
angle=math.pi / 2)
cap(width=size, position=[0, 0, size], orientation=orientation_y)
tunnel(length=size,
width=size,
position=[0, 0, size / 2],
orientation=orientation_y)
def w2quat(q):
angle = np.linalg.norm(q)
if abs(angle) < 1e-7:
ax = np.array([1, 0, 0])
else:
ax, angle = normalize(q), np.linalg.norm(q)
w = p.getQuaternionFromAxisAngle(ax, angle)
return np.array(w)
def reset(self, force_randomize=None):
if self.config["randomize_env"]:
self.robot = self.load_robot()
# Reset episodal vars
self.step_ctr = 0
self.angle = 0
# Change heightmap with small probability
if np.random.rand(
) < self.config["env_change_prob"] and not self.config[
"terrain_name"] == "flat":
self.terrain = self.generate_rnd_env()
# Get heightmap height at robot position
if self.terrain is None or self.config["terrain_name"] == "flat":
spawn_height = 0
else:
spawn_height = 0.5 * np.max(
self.terrain_hm[self.config["env_length"] // 2 -
3:self.config["env_length"] // 2 + 3,
self.config["env_width"] // 2 -
3:self.config["env_width"] // 2 +
3]) * self.config["mesh_scale_vert"]
# Random initial rotation
rnd_rot = 0 #np.random.rand() * 0.3 - 0.15
rnd_quat = p.getQuaternionFromAxisAngle([0, 0, 1], rnd_rot)
[
p.resetJointState(self.robot,
i,
0,
0,
physicsClientId=self.client_ID)
for i in range(18)
]
p.resetBasePositionAndOrientation(self.robot,
[0, 0, spawn_height + 0.3],
rnd_quat,
physicsClientId=self.client_ID)
p.setJointMotorControlArray(bodyUniqueId=self.robot,
jointIndices=range(18),
controlMode=p.POSITION_CONTROL,
targetPositions=[0] * 18,
forces=[self.config["max_joint_force"]] *
18,
physicsClientId=self.client_ID)
self.update_targets()
self.prev_target_dist = np.sqrt((0 - self.target[0])**2 +
(0 - self.target[1])**2)
tar_angle = np.arctan2(self.target[1] - 0, self.target[0] - 0)
for i in range(10):
p.stepSimulation(physicsClientId=self.client_ID)
return np.zeros(self.obs_dim)
def parcour():
# create an obstacle parcour
# width of the tunnels
width = 1
# create some orientation quaternions to rotate tunnels and corners
tunnel_ornx = [0., 0., 0., 1.]
tunnel_orny = p.getQuaternionFromAxisAngle(axis=[0, 0, 1],
angle=math.pi / 2)
tunnel_ornz = p.getQuaternionFromAxisAngle(axis=[0, 1, 0],
angle=math.pi / 2)
corner_negxposz = [0., 0., 0., 1.]
corner_negzposy = p.getQuaternionFromEuler([math.pi, 0.0, -math.pi / 2])
corner_negyposx = p.getQuaternionFromEuler([math.pi / 2, 0.0, math.pi / 2])
# create the parcour
cap(width=width, position=[-1., 0., 2.])
tunnel(length=5.,
width=width,
position=[1.5, 0., 2.],
orientation=tunnel_ornx)
corner(width=width, position=[4.5, 0., 2.], orientation=corner_negxposz)
tunnel(length=2.,
width=width,
position=[4.5, 0.0, 3.5],
orientation=tunnel_ornz)
corner(width=width, position=[4.5, 0., 5.0], orientation=corner_negzposy)
tunnel(length=4.,
width=width,
position=[4.5, 2.5, 5],
orientation=tunnel_orny)
corner(width=width, position=[4.5, 5., 5.], orientation=corner_negyposx)
tunnel(length=4.,
width=width,
position=[7.0, 5.0, 5.],
orientation=tunnel_ornx)
cap(width=width, position=[9., 5., 5.])
def create_bullet_body(self):
self._shape_id = pybullet.createCollisionShape(
shapeType=pybullet.GEOM_BOX,
#radius=.5,
halfExtents=[.5, .5, .5],
physicsClientId=self._physics_client_id,
)
self._body_id = pybullet.createMultiBody(
baseMass=1.,
baseCollisionShapeIndex=self._shape_id,
basePosition=self._location + glm.vec3(0, 0, .5),
baseOrientation=pybullet.getQuaternionFromAxisAngle([1, 0, 0],
-math.pi / 2),
physicsClientId=self._physics_client_id,
)
def _convert_to_pose(self, action):
# convert action to desired pose of humanoid
pose = [[] for _ in range(1, NLINK)]
action_ind = 0
for link_ind, i in zip(range(1, NLINK), range(0, NLINK - 1)):
if link_ind in JOINT_SPHERICAL:
# todo change q to euler for smaller params
angle = action[action_ind]
axis = [
action[action_ind + 1], action[action_ind + 2],
action[action_ind + 3]
]
pose_q = p.getQuaternionFromAxisAngle(axis, angle)
pose[i] = [pose_q[0], pose_q[1], pose_q[2], pose_q[3]]
action_ind += 4
elif link_ind in JOINT_REVOLUTE:
angle = action[action_ind]
pose[i] = [angle]
action_ind += 1
return pose
def reset(self, sim_id: int):
s = self.settings_
# Add boxes.
for d, p, r in zip(s.box_dim, s.box_pos, s.box_rot):
d = np.asarray(d)
p = np.asarray(p)
r = np.asarray(r)
box_shape = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=0.5 * d)
pb.createMultiBody(0, box_shape,
basePosition=p,
baseOrientation=pb.getQuaternionFromAxisAngle((0, 0, 1), r))
# Add ground plane.
plane_shape = pb.createCollisionShape(
pb.GEOM_PLANE, physicsClientId=sim_id)
plane_index = pb.createMultiBody(
0, plane_shape, physicsClientId=sim_id)
pb.resetBasePositionAndOrientation(
plane_index, [0, 0, 0.0], [0, 0, 0, 1], physicsClientId=sim_id)
def rotate_z(angle):
return p.getQuaternionFromAxisAngle(axis=[0, 0, 1], angle=angle)
def axis_angle2quat(axis, angle):
return pb.getQuaternionFromAxisAngle(axis, angle)