How to use the pybullet.getEulerFromQuaternion function in pybullet

resetPose()

# Initialise the gamepad object using the gamepad inputs Python package
gamepad = ThreadedInputs()
for gamepadInput in gamepadInputs:
    gamepad.append_command(gamepadInput, gamepadInputs[gamepadInput])
gamepad.start()
trotting=TrottingGait()


s=False
while True:

    bodyPos=robot.getPos()
    bodyOrn,_,_=robot.getIMU()
    xr,yr,_= p.getEulerFromQuaternion(bodyOrn)
    distance=math.sqrt(bodyPos[0]**2+bodyPos[1]**2)
    if distance>50:
        robot.resetBody()
   
    ir=xr/(math.pi/180)
    handleGamepad()
    d=time.time()-rtime
    height = p.readUserDebugParameter(IDheight)

    # wait 3 seconds to start
    if d>3:
        robot.feetPosition(trotting.positions(d-3))
    else:
        robot.feetPosition(Lp)
    #roll=-xr
    roll=0

def rpy(self):
		return pybullet.getEulerFromQuaternion(self.body_part.current_orientation())

def getObservation(self):
        """
        Returns the position and angle of the effector
        :return: ([float])
        """
        observation = []
        state = p.getLinkState(self.kuka_uid, self.kuka_gripper_index)
        pos = state[0]
        orn = state[1]
        euler = p.getEulerFromQuaternion(orn)

        observation.extend(list(pos))
        observation.extend(list(euler))

        return observation

def _compute_observation(self):
        cubePos, cubeOrn = p.getBasePositionAndOrientation(self.botId)
        cubeEuler = p.getEulerFromQuaternion(cubeOrn)
        linear, angular = p.getBaseVelocity(self.botId)
        return [cubeEuler[0],angular[0],self.vt]

if (v[2] == p.GEOM_SPHERE):
                urdfCollision.geom_radius = v[3][0]
            if (v[2] == p.GEOM_MESH):
                urdfCollision.geom_meshfilename = v[4].decode("utf-8")
                if urdfCollision.geom_meshfilename == UNKNOWN_FILE:
                    continue
                urdfCollision.geom_meshscale = v[3]
            if (v[2] == p.GEOM_CYLINDER):
                urdfCollision.geom_length = v[3][0]
                urdfCollision.geom_radius = v[3][1]
            if (v[2] == p.GEOM_CAPSULE):
                urdfCollision.geom_length = v[3][0]
                urdfCollision.geom_radius = v[3][1]
            pos, orn = p.multiplyTransforms(dyn[3], dyn[4], v[5], v[6])
            urdfCollision.origin_xyz = pos
            urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
            urdfLink.urdf_collision_shapes.append(urdfCollision)

# step simulation forward. at the end of each repeat we set part of the step's
			# state by capture the cart & pole state in some form.
			for r in xrange(self.repeats):
				for _ in xrange(self.steps_per_repeat):
					p.stepSimulation()
					p.applyExternalForce(self.cart, -1, (fx,fy,0), (0,0,0), p.WORLD_FRAME)
					if self.delay > 0:
						time.sleep(self.delay)
				self.set_state_element_for_repeat(r)
			self.steps += 1
	
			# Check for out of bounds by position or orientation on pole.
			# we (re)fetch pose explicitly rather than depending on fields in state.
			(x, y, _z), orient = p.getBasePositionAndOrientation(self.pole)
			(cx, cy, cz), corient = p.getBasePositionAndOrientation(self.cart)
			ox, oy, _oz = p.getEulerFromQuaternion(orient)  # roll / pitch / yaw
			if abs(x) > self.pos_threshold or abs(y) > self.pos_threshold or abs(cx) > self.pos_threshold or abs(cy) > self.pos_threshold: # cart and pole can not escape a certain box
				info['done_reason'] = 'out of position bounds'
				self.done = True
				reward = 0.0
			elif abs(ox) > self.angle_threshold or abs(oy) > self.angle_threshold:
				# TODO: probably better to do explicit angle from z?
				info['done_reason'] = 'out of orientation bounds'
				self.done = True
				reward = 0.0
			# check for end of episode (by length)
			if self.steps >= self.max_episode_len:
				info['done_reason'] = 'episode length'
				self.done = True
	
			# calc reward, fixed base of 1.0
			reward = 1.0

def ik_random_restarts(self, body, target_joint, target_pos, target_orient, world_creation, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, ik_indices=range(29, 29+7), max_iterations=1000, max_ik_random_restarts=50, random_restart_threshold=0.01, half_range=False, step_sim=False, check_env_collisions=False):
        orient_orig = target_orient
        best_ik_joints = None
        best_ik_distance = 0
        for r in range(max_ik_random_restarts):
            target_joint_positions = self.ik(body, target_joint, target_pos, target_orient, ik_indices=ik_indices, max_iterations=max_iterations, half_range=half_range)
            world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(target_joint_positions), tool=None)
            if step_sim:
                for _ in range(5):
                    p.stepSimulation(physicsClientId=self.id)
                if len(p.getContactPoints(bodyA=body, bodyB=body, physicsClientId=self.id)) > 0 and orient_orig is not None:
                    # The robot's arm is in contact with itself. Continually randomize end effector orientation until a solution is found
                    target_orient = p.getQuaternionFromEuler(p.getEulerFromQuaternion(orient_orig, physicsClientId=self.id) + np.deg2rad(self.np_random.uniform(-45, 45, size=3)), physicsClientId=self.id)
            if check_env_collisions:
                for _ in range(25):
                    p.stepSimulation(physicsClientId=self.id)
            gripper_pos, gripper_orient = p.getLinkState(body, target_joint, computeForwardKinematics=True, physicsClientId=self.id)[:2]
            if np.linalg.norm(target_pos - np.array(gripper_pos)) < random_restart_threshold and (target_orient is None or np.linalg.norm(target_orient - np.array(gripper_orient)) < random_restart_threshold or np.isclose(np.linalg.norm(target_orient - np.array(gripper_orient)), 2, atol=random_restart_threshold)):
                return True, np.array(target_joint_positions)
            if best_ik_joints is None or np.linalg.norm(target_pos - np.array(gripper_pos)) < best_ik_distance:
                best_ik_joints = target_joint_positions
                best_ik_distance = np.linalg.norm(target_pos - np.array(gripper_pos))
        world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(best_ik_joints), tool=None)
        return False, np.array(best_ik_joints)

# step simulation forward. at the end of each repeat we set part of the step's
    # state by capture the cart & pole state in some form.
    for r in xrange(self.repeats):
      for _ in xrange(self.steps_per_repeat):
        p.stepSimulation()
        p.applyExternalForce(self.cart, -1, (fx,fy,0), (0,0,0), p.WORLD_FRAME)
        if self.delay > 0:
          time.sleep(self.delay)
      self.set_state_element_for_repeat(r)
    self.steps += 1

    # Check for out of bounds by position or orientation on pole.
    # we (re)fetch pose explicitly rather than depending on fields in state.
    (x, y, _z), orient = p.getBasePositionAndOrientation(self.pole)
    ox, oy, _oz = p.getEulerFromQuaternion(orient)  # roll / pitch / yaw
    if abs(x) > self.pos_threshold or abs(y) > self.pos_threshold:
      info['done_reason'] = 'out of position bounds'
      self.done = True
      reward = 0.0
    elif abs(ox) > self.angle_threshold or abs(oy) > self.angle_threshold:
      # TODO: probably better to do explicit angle from z?
      info['done_reason'] = 'out of orientation bounds'
      self.done = True
      reward = 0.0
    # check for end of episode (by length)
    if self.steps >= self.max_episode_len:
      info['done_reason'] = 'episode length'
      self.done = True

    # calc reward, fixed base of 1.0
    reward = 1.0

def getFrames(self):
        """Gets the available frames in the current robot model
        
        Returns:
            dict -- dict of str -> (pos, orientation)
        """
        frames = {}

        for name in self.frames.keys():
            jointState = p.getLinkState(self.robot, self.frames[name])
            pos = jointState[0]
            orientation = p.getEulerFromQuaternion(jointState[1])
            frames[name] = [pos, orientation]

        return frames