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)