def _go_to_relative_pose()

in src/pyrobot/habitat/base.py [0:0]

• 34 lines of code
• 10 McCabe index (conditional complexity)

    def _go_to_relative_pose(self, rel_x, rel_y, abs_yaw):
        # clip relative movements beyond 10 micrometer precision
        # this is done to improve determinism, as habitat-sim doesn't
        # seem to precisely move the robot beyond sub milimeter precision anyways
        if abs(rel_x) < 1e-5:
            rel_x = 0
        if abs(rel_y) < 1e-5:
            rel_y = 0

        if math.sqrt(rel_x ** 2 + rel_y ** 2) > 0.0:
            # rotate to point to (x, y) point
            action_name = "turn_left"
            if rel_y < 0.0:
                action_name = "turn_right"

            v1 = np.asarray([1, 0], dtype=np.float64)
            v2 = np.asarray([rel_x, rel_y], dtype=np.float64)
            cosine_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
            angle = np.arccos(cosine_angle)

            did_collide = self._act(action_name, math.degrees(angle))

            if did_collide:
                print("Error: Collision accured while 1st rotating!")
                return False

            # move to (x,y) point
            did_collide = self._act("move_forward", math.sqrt(rel_x ** 2 + rel_y ** 2))
            if did_collide:
                print("Error: Collision accured while moving straight!")
                return False
        # rotate to match the final yaw!
        (cur_x, cur_y, cur_yaw) = self.get_state()
        rel_yaw = abs_yaw - cur_yaw

        # clip to micro-degree precision to preserve determinism
        if abs(rel_yaw) < 1e-4:
            rel_yaw = 0

        action_name = "turn_left"
        if rel_yaw < 0.0:
            action_name = "turn_right"
            rel_yaw *= -1

        did_collide = self._act(action_name, math.degrees(rel_yaw))
        if did_collide:
            print("Error: Collision accured while rotating!")
            return False

        return True