# SurRoL.surrol.tasks.psm_env.PsmEnv._get_robot_state
def _get_robot_state(self, idx: int) -> np.ndarray:
	'''获取机器人的状态，返回机器人当前位姿、夹爪角度，两者拼接成一个数组（3位置+3欧拉角+1开合角度）'''
	# robot state: tip pose in the world coordinate
	psm = self.psm1 if idx == 0 else self.psm2
	pose_world = psm.pose_rcm2world(psm.get_current_position(), 'tuple')    # 机器人在世界坐标系下的位姿
	jaw_angle = psm.get_current_jaw_position()  # 夹爪角度
	return np.concatenate([
		np.array(pose_world[0]), np.array(p.getEulerFromQuaternion(pose_world[1])), np.array(jaw_angle).ravel()
	])  # 3 + 3 + 1 = 7

robot_state = [x, y, z, roll, pitch, yaw, gripper]

# SurRoL.surrol.tasks.psm_env.PsmEnv._get_obs
def _get_obs(self) -> dict:
	'''获取当前环境状态信息（机器人当前位置、目标物体位置、机器人与目标物体的相对位置）'''
	# 获取机器人当前状态
	robot_state = self._get_robot_state(idx=0)
	# TODO: may need to modify
	
	# 检查环境中是否有物体，如果有则获取目标物体的位置、姿态、相对于机器人的位置
	if self.has_object:
		pos, _ = get_link_pose(self.obj_id, -1)     # 目标物体位置
		object_pos = np.array(pos)
		pos, orn = get_link_pose(self.obj_id, self.obj_link1)   # 获取目标物体的特定link的位置和方向
		waypoint_pos = np.array(pos)    # 路径点位置为目标物体link位置
		# rotations
		waypoint_rot = np.array(p.getEulerFromQuaternion(orn))  #路径点姿态为目标物体link姿态
		# relative position state
		object_rel_pos = object_pos - robot_state[0: 3] # 相对位置为目标位置与机器人末端位置之差
	else:
		# TODO: can have a same-length state representation
		object_pos = waypoint_pos = waypoint_rot = object_rel_pos = np.zeros(0)

	# 确定使用哪个位置作为目标位置
	if self.has_object:
		# object/waypoint position，使用物体位置object_pos，或物体link位置waypoint_pos
		achieved_goal = object_pos.copy() if not self._waypoint_goal else waypoint_pos.copy()
	else:
		# tip position，如果没有目标物体，则将机器人末端的位置作为目标位置
		achieved_goal = np.array(get_link_pose(self.psm1.body, self.psm1.TIP_LINK_INDEX)[0])

	observation = np.concatenate([
		robot_state, object_pos.ravel(), object_rel_pos.ravel(),
		waypoint_pos.ravel(), waypoint_rot.ravel()  # achieved_goal.copy(),
	])
	obs = {
		'observation': observation.copy(),
		'achieved_goal': achieved_goal.copy(),
		'desired_goal': self.goal.copy()
	}
	return obs

# SurRoL.surrol.tasks.psm_env.PsmEnv._set_action
def _set_action(self, action: np.ndarray):
	"""
	delta_position (3), delta_theta (1) and open/close the gripper (1)
	in the world frame
	执行动作的过程（位置的变化，旋转变化，夹爪的开合）
	"""
	assert len(action) == self.ACTION_SIZE, "The action should have the save dim with the ACTION_SIZE" # ACTION_SIZE = 5
	# time0 = time.time()
	action = action.copy()  # ensure that we don't change the action outside of this scope
	action[:3] *= 0.01 * self.SCALING  # position, limit maximum change in position
	pose_world = self.psm1.pose_rcm2world(self.psm1.get_current_position())
	workspace_limits = self.workspace_limits1
	pose_world[:3, 3] = np.clip(pose_world[:3, 3] + action[:3],
								workspace_limits[:, 0] - [0.02, 0.02, 0.],
								workspace_limits[:, 1] + [0.02, 0.02, 0.08])  # clip to ensure convergence
	rot = get_euler_from_matrix(pose_world[:3, :3])
	if self.ACTION_MODE == 'yaw':
		action[3] *= np.deg2rad(30)  # yaw, limit maximum change in rotation
		rot = (self.psm1_eul[0], self.psm1_eul[1], wrap_angle(rot[2] + action[3]))  # only change yaw
	elif self.ACTION_MODE == 'pitch':
		action[3] *= np.deg2rad(15)  # pitch, limit maximum change in rotation
		pitch = np.clip(wrap_angle(rot[1] + action[3]), np.deg2rad(-90), np.deg2rad(90))
		rot = (self.psm1_eul[0], pitch, self.psm1_eul[2])  # only change pitch
	else:
		raise NotImplementedError
	pose_world[:3, :3] = get_matrix_from_euler(rot)
	action_rcm = self.psm1.pose_world2rcm(pose_world)
	# time1 = time.time()
	self.psm1.move(action_rcm)
	# time2 = time.time()

	# jaw
	if self.block_gripper:
		action[4] = -1
	if action[4] < 0:
		self.psm1.close_jaw()
		self._activate(0)
	else:
		self.psm1.move_jaw(np.deg2rad(40))  # open jaw angle; can tune
		self._release(0)

action[5] = [delta_x, delta_y, delta_z, yaw/pitch, gripper]

pip install hydra-core colorlog termcolor opencv-python perlin_noise
pip install "cython<3"