{"id":23335,"date":"2025-07-25T18:49:01","date_gmt":"2025-07-25T16:49:01","guid":{"rendered":"https:\/\/www.astor.com.pl\/poradnikautomatyka\/?p=23335"},"modified":"2025-09-24T10:14:03","modified_gmt":"2025-09-24T08:14:03","slug":"sterowanie-robotem-astorino-po-kuli","status":"publish","type":"post","link":"https:\/\/www.astor.com.pl\/poradnikautomatyka\/sterowanie-robotem-astorino-po-kuli\/","title":{"rendered":"Sterowanie robotem Astorino po kuli"},"content":{"rendered":"\n
<\/div>\n\n\n\n
\n

Z tego artyku\u0142u dowiesz si\u0119:<\/h3>\n
    \n
  • jak zasymulowa\u0107 ruch robota Astorino po powierzchni kuli za pomoc\u0105 oprogramowania astorinoIDE,<\/li>\n
  • jak wykorzysta\u0107 matematyczny opis kuli do generowania punkt\u00f3w na jej powierzchni,<\/li>\n
  • jak stworzy\u0107 program w Pythonie, kt\u00f3ry automatycznie oblicza po\u0142o\u017cenie i orientacj\u0119 robota na kuli,<\/li>\n
  • jak zaimplementowa\u0107 sterowanie ruchem robota po powierzchni kuli w astorinoIDE.<\/li>\n<\/ul>\n<\/div>\n\n\n\n
    <\/div>\n\n\n\n

    Dzi\u0119ki temu poradnikowi dowiesz si\u0119, jak zasymulowa\u0107 ruch robota Astorino po powierzchni kuli przy pomocy astorinoIDE, a nast\u0119pnie zaimplementowa\u0107 i przetestowa\u0107 program sterowania w rzeczywistych warunkach. Zaczniemy od matematycznego opisu trajektorii ruchu robota, a nast\u0119pnie stworzymy kod w Pythonie, kt\u00f3ry obliczy po\u0142o\u017cenie i orientacj\u0119 na powierzchni kuli. W ko\u0144cu przeniesiemy program do astorinoIDE i przetestujemy go na robocie edukacyjnym Astorino, sprawdzaj\u0105c poprawno\u015b\u0107 wykonania ruchu.<\/p>\n\n\n\n

    <\/div>\n\n\n\n

    Potrzebne urz\u0105dzenia oraz oprogramowanie<\/h2>\n\n\n\n
      \n
    • Robot Astorino wraz z oprogramowaniem.<\/li>\n\n\n\n
    • Oprogramowanie astorinoIDE.<\/li>\n\n\n\n
    • ASTORINO-OPT-SIMBOX (do testowania symulacji).<\/li>\n\n\n\n
    • \u015arodowisko programistyczne Python (wraz z bibliotek\u0105 NumPy).<\/li>\n\n\n\n
    • Komputer PC z systemem operacyjnym Windows.<\/li>\n<\/ul>\n\n\n\n
      <\/div>\n\n\n\n

      Wst\u0119p<\/h2>\n\n\n\n

      Poruszanie robotem w przestrzeni tr\u00f3jwymiarowej stanowi wyzwanie, szczeg\u00f3lnie gdy wymagana jest precyzyjna nawigacja po powierzchni sferycznej. Roboty musz\u0105 radzi\u0107 sobie z realizacj\u0105 skomplikowanych trajektorii, kt\u00f3re mog\u0105 obejmowa\u0107 zar\u00f3wno ruchy po prostych, jak i bardziej z\u0142o\u017conych powierzchniach, takich jak kula.<\/p>\n\n\n\n

      W artykule przedstawiono dwie metody sterowania robotem Astorino, umo\u017cliwiaj\u0105ce realizacj\u0119 ruchu po powierzchni kuli. Pierwsza z nich opiera si\u0119 na przesuni\u0119ciu punktu TCP (Tool Center Point) do \u015brodka kuli, podczas gdy druga metoda wykorzystuje matematyczny opis kuli oraz odpowiedni\u0105 transformacj\u0119 uk\u0142adu wsp\u00f3\u0142rz\u0119dnych. W dalszej cz\u0119\u015bci artyku\u0142u przedstawiono szczeg\u00f3\u0142y implementacji tych metod w oprogramowaniu astorinoIDE oraz spos\u00f3b generowania trajektorii robota.<\/p>\n\n\n\n

      <\/div>\n\n\n\n

      Sterowanie ruchem po kuli na podstawie punktu TCP<\/h2>\n\n\n\n

      Pierwsza metoda sterowania po kuli polega na przesuni\u0119ciu punktu TCP do \u015brodka kuli. Aby to osi\u0105gn\u0105\u0107, nale\u017cy najpierw oddali\u0107 uk\u0142ad narz\u0119dzia (TOOL) od ko\u0144c\u00f3wki robota (narz\u0119dzia lub flanszy), a nast\u0119pnie dojecha\u0107 robotem w taki spos\u00f3b, aby punkt TCP pokry\u0142 si\u0119 z centrum kuli. Po wykonaniu tego ruchu, rotacja robota wzgl\u0119dem osi X i Y uk\u0142adu TOOL zapewnia ruch robota po powierzchni kuli. Tego rodzaju sterowanie jest stosunkowo proste, ale mo\u017ce wymaga\u0107 du\u017cej precyzji w ustawieniach pocz\u0105tkowych uk\u0142adu.<\/p>\n\n\n\n

      <\/div>\n\n\n\n

      Sterowanie na podstawie opisu matematycznego kuli<\/h2>\n\n\n\n

      Wielko\u015bci s\u0142u\u017c\u0105ce do opisu po\u0142o\u017cenia na powierzchni kuli<\/strong><\/p>\n\n\n\n

      Alternatywnym podej\u015bciem do poruszania robotem po kuli jest wykorzystanie jej matematycznego opisu w przestrzeni tr\u00f3jwymiarowej. Mo\u017cna to zrobi\u0107 poprzez okre\u015blenie odpowiednich parametr\u00f3w kuli i obliczenie pozycji oraz orientacji punkt\u00f3w na jej powierzchni. B\u0119dziemy u\u017cywali oznacze\u0144:<\/p>\n\n\n\n

        \n
      • S(x0<\/sub>,y0<\/sub>,z0<\/sub>) \u2013 \u015brodek kuli,<\/li>\n\n\n\n
      • P(x,y,z) \u2013 dowolny punkt na powierzchni kuli,<\/li>\n\n\n\n
      • Rk<\/sub> \u2013 promie\u0144 kuli<\/li>\n\n\n\n
      • \u03b8 \u2013 k\u0105t zenitalny<\/li>\n\n\n\n
      • \u03a6 \u2013 azymut<\/li>\n<\/ul>\n\n\n\n

        K\u0105t zenitalny to k\u0105t mi\u0119dzy osi\u0105 globalnego pionu (osi\u0105 Z<\/em>) a wektorem prowadz\u0105cym od \u015brodka kuli do punktu na jej powierzchni. K\u0105t ten jest dodatni, gdy mierzony jest od osi Z<\/em> w kierunku promienia prowadz\u0105cego do danego punktu na kuli.<\/p>\n\n\n\n

        Azymut to k\u0105t mi\u0119dzy osi\u0105 X<\/em> a rzutem wektora prowadz\u0105cego od \u015brodka kuli do punktu na jej powierzchni na p\u0142aszczyzn\u0119 poziom\u0105 XY<\/em>. K\u0105t ten jest dodatni, gdy mierzony jest od osi X<\/em> w kierunku osi Y<\/em>.<\/p>\n\n\n\n

        Powy\u017csze definicje k\u0105t\u00f3w zosta\u0142y zobrazowane poni\u017cej (rys. 1).<\/p>\n\n\n\n

        <\/div>\n\n\n
        \n
        \"\"
        Rys. 1. Ilustracja przedstawiaj\u0105ca k\u0105t zenitalny oraz k\u0105t azymutu. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
        <\/div>\n\n\n\n

        Na przyk\u0142adzie kuli ziemskiej:<\/p>\n\n\n\n

          \n
        • k\u0105t zenitalny \u2013 okre\u015bla szeroko\u015b\u0107 geograficzn\u0105,<\/li>\n\n\n\n
        • azymut \u2013 okre\u015bla d\u0142ugo\u015b\u0107 geograficzn\u0105.<\/li>\n<\/ul>\n\n\n\n
          <\/div>\n\n\n\n

          Matematyczny opis po\u0142o\u017cenia i orientacji na kuli<\/strong><\/p>\n\n\n\n

          Wykorzystuj\u0105c powy\u017csze oznaczenia mo\u017cna zapisa\u0107 wzory na wsp\u00f3\u0142rz\u0119dne po\u0142o\u017cenia na powierzchni kuli:<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Wyznaczenie orientacji narz\u0119dzia nale\u017cy przeprowadzi\u0107 w taki spos\u00f3b, aby o\u015b Z<\/em> uk\u0142adu TOOL by\u0142a skierowana prostopadle do powierzchni kuli, a jej przed\u0142u\u017cenie przechodzi\u0142o przez \u015brodek kuli.<\/p>\n\n\n\n

           Na pocz\u0105tek nale\u017cy okre\u015bli\u0107 o\u015b Z<\/em> narz\u0119dzia jako:<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Jest to znormalizowany wektor wskazuj\u0105cy w stron\u0119 \u015brodka kuli.<\/p>\n\n\n\n

          Nast\u0119pnie konieczne b\u0119dzie wyznaczenie osi X<\/em>. Zostanie ona okre\u015blona jako styczna do powierzchni kuli, wyznaczona na podstawie pochodnej po\u0142o\u017cenia wzgl\u0119dem k\u0105ta zenitalnego. Je\u015bli k\u0105t zenitalny jest sta\u0142y (co skutkuje zerow\u0105 pochodn\u0105), o\u015b X<\/em> b\u0119dzie wyznaczona jako pochodna po\u0142o\u017cenia wzgl\u0119dem k\u0105ta azymutu. W celu obliczenia tej orientacji skorzystano ze wzor\u00f3w:<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Je\u015bli jednak pochodna jest bliska zeru, to przyjmujemy kierunek osi X<\/em> jako:<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          O\u015b Y <\/em>musi dope\u0142nia\u0107 utworzony uk\u0142ad prawoskr\u0119tny:<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          W ostatnim kroku nale\u017cy jeszcze raz wyznaczy\u0107 kierunek osi X, aby zapewni\u0107 ortonormalno\u015b\u0107 uk\u0142adu wsp\u00f3\u0142rz\u0119dnych narz\u0119dzia.<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Maj\u0105c wektory:<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          budujemy macierz rotacji:<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"<\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Na jej podstawie mo\u017cemy wyznaczy\u0107 k\u0105ty Eulera (w konwencji ZYZ<\/em>), kt\u00f3re jednoznacznie opisuj\u0105 orientacj\u0119 narz\u0119dzia w przestrzeni 3D.<\/p>\n\n\n\n

          <\/div>\n\n\n\n

          Implementacja sterowania w programie astorinoIDE<\/h2>\n\n\n\n

          Na pocz\u0105tek w\u0142\u0105czamy program astorinoIDE. Wybieramy rodzaj po\u0142\u0105czenia z listy rozwijanej (w przyk\u0142adzie wybrano po\u0142\u0105czenie USB) oraz klikamy przycisk Connect\/Disconnect <\/strong>(rys.2).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 2. Widok g\u00f3rnego paska narz\u0119dzia astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Wybieramy kierunek przesy\u0142anych danych z komputera do robota oraz klikamy SYNC<\/strong> (rys. 3).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 3. Widok okna synchronizacji po\u0142\u0105czenia mi\u0119dzy komputerem a robotem. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          W kolejnym kroku otwieramy lub tworzymy nowy projekt z g\u00f3rnego paska. W tym celu klikamy Project <\/strong>i wybieramy odpowiedni\u0105 opcj\u0119 z listy rozwijanej (rys. 4).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 4. Widok menu tworzenia lub otwierania projekt\u00f3w. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Nast\u0119pnie otwieramy lub tworzymy nowy program. Z lewego drzewa folder\u00f3w mo\u017cemy wybra\u0107 istniej\u0105cy plik z zak\u0142adki Program Files<\/strong> lub utworzy\u0107 nowy program za pomoc\u0105 opcji w g\u00f3rnym pasku i zak\u0142adce File<\/strong>. Po wybraniu nowego programu (rys. 5) wy\u015bwietli si\u0119 okno, w kt\u00f3rym wpisujemy nazw\u0119 pliku i zatwierdzamy przyciskiem OK<\/strong> (rys. 6).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 5. Widok menu edycji programu. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n
          \n
          \"\"
          Rys. 6. Widok okna tworzenia nowego programu. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Po po\u0142\u0105czeniu z robotem i stworzeniu w\u0142asnego programu mo\u017cna zacz\u0105\u0107 tworzy\u0107 scen\u0119 (uk\u0142ad robota z kul\u0105 w przestrzeni). W celu otwarcia symulacji nale\u017cy klikn\u0105\u0107 przycisk Visualization <\/strong>(rys. 7), oznaczony na pasku g\u0142\u00f3wnym programu symbolem sze\u015bcianu.<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 7. Widok g\u00f3rnego paska narz\u0119dzia astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Po klikni\u0119ciu powinno pojawi\u0107 si\u0119 okno z robotem (rys. 8).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 8. Widok okna symulacji robota. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          W celu u\u0142atwienia sterowania robotem mo\u017cemy sprowadzi\u0107 go do pozycji domowej. Jest to procedura, kt\u00f3ra umo\u017cliwia szybkie przywr\u00f3cenie robota do wyj\u015bciowej pozycji, co u\u0142atwia dalsze programowanie i wykonywanie polece\u0144. W tym celu w terminalu w dolnej cz\u0119\u015bci programu astorinoIDE (rys. 9) wpisujemy komend\u0119 do home<\/strong> i zatwierdzamy wciskaj\u0105c Enter<\/strong> na klawiaturze.<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 9. Widok okna terminala z wpisan\u0105 ju\u017c komend\u0105 do home<\/strong> (kliknij, aby powi\u0119kszy\u0107). \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Po takim wywo\u0142aniu komendy terminal powinien odpowiedzie\u0107 tekstem „DO motion Completed” <\/em>(rys.10), <\/em>a robot powinien znale\u017a\u0107 si\u0119 w pozycji domowej (rys. 11).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 10. Widok okna terminala po wykonaniu ruchu do pozycji domowej (kliknij, aby powi\u0119kszy\u0107)<\/em>. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n
          \n
          \"\"
          Rys. 11. Widok okna symulacji robota po wykonaniu ruchu do pozycji domowej. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          UWAGA! Nale\u017cy pami\u0119ta\u0107, \u017ce aby ruch zosta\u0142 wykonany, robot musi znajdowa\u0107 si\u0119 w trybie Repeat Mode<\/strong>. Zmieni\u0107 go mo\u017cna w g\u00f3rnym pasku narz\u0119dzi: Mode -> Switch to Repeat<\/strong> (ikona robota) (rys. 12).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 12. Widok g\u00f3rnego paska narz\u0119dzia astorinoIDE przy zmianie trybu ruchu. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Gdy mamy ju\u017c otwarty projekt z naszym programem oraz okno symulacji, mo\u017cemy doda\u0107 przyk\u0142adow\u0105 kul\u0119, po kt\u00f3rej p\u00f3\u017aniej b\u0119dzie porusza\u0142 si\u0119 robot. W tym celu w oknie wizualizacji wybieramy opcj\u0119 Open Shape Generator <\/strong>(rys. 13).<\/p>\n\n\n\n

          <\/div>\n\n\n
          \n
          \"\"
          Rys. 13. Widok g\u00f3rnego paska okna symulacji robota. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
          <\/div>\n\n\n\n

          Otworzy nam si\u0119 po prawej stronie symulacji okno tworzenia wybranego kszta\u0142tu w przestrzeni. W celu dodania kuli wybieramy zak\u0142adk\u0119 Sphere <\/strong>(rys. 14). Nast\u0119pnie mo\u017cemy wprowadzi\u0107 jej parametry: nazw\u0119, promie\u0144, po\u0142o\u017cenie w trzech osiach uk\u0142adu globalnego, kolor oraz rodzaj obiektu:<\/p>\n\n\n\n

            \n
          • Obstacle<\/strong> \u2013 obiekty tego typu s\u0105 statycznymi obiektami sceny,<\/li>\n\n\n\n
          • Work<\/strong> \u2013 obiekty tego typu mog\u0105 by\u0107 przenoszone przez robota,<\/li>\n\n\n\n
          • Tool<\/strong> \u2013 obiekty tego typu zawsze poruszaj\u0105 si\u0119 zgodnie z ki\u015bci\u0105 robota. <\/li>\n<\/ul>\n\n\n\n
            <\/div>\n\n\n
            \n
            \"\"
            Rys. 14. Widok okna generatora kszta\u0142t\u00f3w w programie astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n\n

            Gdy wszystkie parametry zosta\u0142y ju\u017c ustawione, nale\u017cy klikn\u0105\u0107 przycisk Add.<\/strong><\/p>\n\n\n\n

            <\/div>\n\n\n\n

            Ruch po kuli przez rotacj\u0119 wok\u00f3\u0142 oddalonego punktu TCP<\/h2>\n\n\n\n

            Pierwszym sposobem na ruch po kuli jest oddalenie punktu TCP na odleg\u0142o\u015b\u0107 r\u00f3wn\u0105 jej promieniowi. W tym celu otwieramy okno Robot Manager<\/strong> (rys. 15).<\/p>\n\n\n\n

            <\/div>\n\n\n
            \n
            \"\"
            Rys. 15. Widok g\u00f3rnego paska narz\u0119dzia astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n\n

            Po pojawieniu si\u0119 okna nale\u017cy wej\u015b\u0107 w zak\u0142adk\u0119 TOOL<\/strong> z prawej strony (rys. 16) i jako parametr TOOL Z<\/em><\/strong>, <\/em>wpisa\u0107 warto\u015b\u0107 wcze\u015bniej ustawionego promienia kuli.<\/p>\n\n\n\n

            <\/div>\n\n\n
            \n
            \"\"
            Rys. 16. Widok zak\u0142adki TOOL okna Robot Manager. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n\n

            Klikamy Upload Tool <\/strong>oraz zatwierdzamy zmian\u0119 narz\u0119dzia po wy\u015bwietleniu ostrze\u017cenia.<\/p>\n\n\n\n

            W kolejnym kroku doje\u017cd\u017camy do kuli, tak by punkt TCP pokry\u0142 si\u0119 z \u015brodkiem kuli, a orientacja narz\u0119dzia by\u0142a prostopad\u0142a do powierzchni kuli. Wykorzysta\u0107 do tego mo\u017cemy zak\u0142adk\u0119 JOG<\/strong> w oknie Robot Manager<\/strong> (rys. 17). <\/em>Wybieramy uk\u0142ad, w kt\u00f3rym chcemy poruszy\u0107 robotem z listy rozwijanej (Jogging -> Mode<\/strong>) i klikaj\u0105c odpowiednie przyciski doje\u017cd\u017camy nim do wcze\u015bniej wspomnianej pozycji (rys.18). Jest to mo\u017cliwe dopiero po zmianie trybu pracy na Teach Mode<\/strong>.<\/em><\/p>\n\n\n\n

            <\/div>\n\n\n
            \n
            \"\"
            Rys. 17. Widok zak\u0142adki JOG okna Robot Manager. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n
            \n
            \"\"
            Rys. 18. Widok symulacji robota po doje\u017adzie do powierzchni kuli. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n\n

            Nast\u0119pnie mo\u017cemy ju\u017c porusza\u0107 po kuli. W tym celu wybierany w zak\u0142adce JOG<\/strong> ruch robotem w uk\u0142adzie TOOL<\/strong> i wykonujemy rotacj\u0119 wok\u00f3\u0142 osi X oraz Y (rotacja wok\u00f3\u0142 osi Z nie ma sensu, poniewa\u017c wcze\u015bniej skierowali\u015bmy j\u0105 prostopadle do powierzchni kuli). Poni\u017cej przedstawiono kilka przyk\u0142adowych pozycji robota.<\/p>\n\n\n\n

            <\/div>\n\n\n
            \n
            \"\"
            Rys. 19. Przyk\u0142adowa pozycja robota nr 1. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n
            \n
            \"\"
            Rys. 20. Przyk\u0142adowa pozycja robota nr 2. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n
            \n
            \"\"
            Rys. 21. Przyk\u0142adowa pozycja robota nr 3. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n\n

            Ruch po kuli przez wykorzystanie jej matematycznego opisu<\/h2>\n\n\n\n

            Drug\u0105 metod\u0105 na poruszaniem robotem po kuli jest wygenerowanie punkt\u00f3w (po\u0142o\u017cenie i orientacja), zgodnie z opisem zamieszczonym we wcze\u015bniejszej cz\u0119\u015bci artyku\u0142u. Napisano do tego celu program w j\u0119zyku Python, kt\u00f3ry generuje punkty na powierzchni kuli wraz z orientacj\u0105 uk\u0142adu TOOL<\/strong>.<\/em>.<\/p>\n\n\n\n

            Uwaga! Je\u015bli po\u0142o\u017cenie lub orientacja uk\u0142adu TOOL<\/strong> zosta\u0142y zmienione, tak jak chocia\u017cby w poprzednim paragrafie, nale\u017cy przywr\u00f3ci\u0107 uk\u0142ad do ustawie\u0144 podstawowych (zeruj\u0105c ka\u017cd\u0105 ze sk\u0142adowych po\u0142o\u017cenia i orientacji \u2013 rys. 22).<\/p>\n\n\n\n

            <\/div>\n\n\n
            \n
            \"\"
            Rys. 22. Widok zak\u0142adki Tool okna Robot Manager. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
            <\/div>\n\n\n\n

            Uniwersalno\u015b\u0107 opracowanego kodu pozwala na wyznaczanie po\u0142o\u017cenia oraz orientacji uk\u0142adu narz\u0119dzia w przestrzeni kulistej, niezale\u017cnie od po\u0142o\u017cenia kuli oraz jej promienia. Dzi\u0119ki temu mo\u017cliwe jest obliczenie wsp\u00f3\u0142rz\u0119dnych punkt\u00f3w na powierzchni kuli w r\u00f3\u017cnych konfiguracjach.<\/p>\n\n\n\n

            Warto jednak zauwa\u017cy\u0107, \u017ce program nie uwzgl\u0119dnia rzeczywistych ogranicze\u0144 robota, takich jak fizyczne mo\u017cliwo\u015bci jego ruchu, zasi\u0119g czy przechy\u0142y. W praktyce oznacza to, \u017ce wskazane przez program punkty mog\u0105 nie zawsze by\u0107 osi\u0105galne przez robota w rzeczywistych warunkach. Program dostarcza jedynie wynik\u00f3w matematycznych, kt\u00f3re odnosz\u0105 si\u0119 do idealnego modelu matematycznego kuli, nie bior\u0105c pod uwag\u0119 fizycznych ogranicze\u0144 robota, takich jak kinematyka, zasi\u0119g, czy ograniczenia mechaniczne.<\/p>\n\n\n\n

            Istnieje zatem potrzeba dalszej weryfikacji wynik\u00f3w w kontek\u015bcie realnych trajektorii robota dla konkretnego zastosowania. W artykule skupiono si\u0119 jednak na implementacji samego ruchu, a nie na konkretnych zastosowaniach.<\/p>\n\n\n\n

            Aby u\u0142atwi\u0107 implementacj\u0119 kodu oraz unikn\u0105\u0107 nadmiaru z\u0142o\u017conych nazw, w kodzie zastosowane zosta\u0142y nast\u0119puj\u0105ce oznaczenia:<\/p>\n\n\n\n

              \n
            • k\u0105t zenitalny: \u03b8 = u <\/li>\n\n\n\n
            • azymut: \u03a6 = v<\/li>\n<\/ul>\n\n\n\n

              Poni\u017cej przedstawiony zosta\u0142 kod w j\u0119zyku Python, kt\u00f3ry pozwala na generowanie wybranej liczby punkt\u00f3w na powierzchni kuli o dowolnych – wybranych wcze\u015bniej – parametrach.<\/p>\n\n\n\n

              <\/div>\n\n\n\n
              import numpy as np\nfrom scipy.spatial.transform import Rotation as R\n\n# Parametry kuli\nx0, y0, z0 = 0.0, 500.0, 250.0  # \u015brodek kuli\nR_kuli = 50.0  # promie\u0144\nN = 20  # liczba punkt\u00f3w\n\nfor i in range(N):\n    u = i * np.pi \/ (N - 1)  # k\u0105t zenitalny\n    v = np.pi  # azymut\n\n    # Pozycja na powierzchni kuli\n    x = x0 + R_kuli * np.sin(u) * np.cos(v)\n    y = y0 + R_kuli * np.sin(u) * np.sin(v)\n    z = z0 + R_kuli * np.cos(u)\n    position = np.array([x, y, z])\n    center = np.array([x0, y0, z0])\n\n    # O\u015b Z narz\u0119dzia (do \u015brodka kuli)\n    Z_tool = center - position\n    Z_tool \/= np.linalg.norm(Z_tool)\n\n    # Dwa wektory styczne do powierzchni (pochodne po u i v)\n    dP_du = np.array([\n        R_kuli * np.cos(u) * np.cos(v),\n        R_kuli * np.cos(u) * np.sin(v),\n        -R_kuli * np.sin(u)\n    ])\n    dP_dv = np.array([\n        -R_kuli * np.sin(u) * np.sin(v),\n        R_kuli * np.sin(u) * np.cos(v),\n        0.0\n    ])\n\n    # Wybierz jeden z wektor\u00f3w stycznych (np. dP_dv)\n    tangent = dP_dv\n    if np.linalg.norm(tangent) < 1e-6:  # zabezpieczenie przed zerowym wektorem\n        tangent = dP_du\n    tangent \/= np.linalg.norm(tangent)\n\n    # Budowa uk\u0142adu wsp\u00f3\u0142rz\u0119dnych\n    X_tool = tangent\n    Y_tool = np.cross(Z_tool, X_tool)\n    Y_tool \/= np.linalg.norm(Y_tool)\n    X_tool = np.cross(Y_tool, Z_tool)\n    X_tool \/= np.linalg.norm(X_tool)\n\n    # Macierz rotacji i konwersja do k\u0105t\u00f3w Eulera\n    R_matrix = np.column_stack((X_tool, Y_tool, Z_tool))\n    rot = R.from_matrix(R_matrix)\n    euler_deg = rot.as_euler('zyz', degrees=True)\n\n    # Format wyj\u015bcia: x,y,z,roll,pitch,yaw\n    print(f\"{x:.3f},{y:.3f},{z:.3f},{euler_deg[2]:.3f},{euler_deg[1]:.3f},{euler_deg[0]:.3f}\")\n<\/code><\/pre>\n\n\n\n
              <\/div>\n\n\n\n
              Program s\u0142u\u017cy do wyznaczenia pozycji i orientacji zestawu punkt\u00f3w znajduj\u0105cych si\u0119 na powierzchni kuli. Na pocz\u0105tku definiowane s\u0105 podstawowe parametry kuli: jej \u015brodek w przestrzeni tr\u00f3jwymiarowej, promie\u0144 oraz liczba punkt\u00f3w, kt\u00f3re maj\u0105 zosta\u0107 rozmieszczone na powierzchni.<\/p>\n\n\n\n
              Nast\u0119pnie, w p\u0119tli, dla ka\u017cdego z tych punkt\u00f3w wyliczana jest jego dok\u0142adna pozycja na kuli, przy czym zmieniany jest tylko k\u0105t zenitalny, a azymut pozostaje sta\u0142y. Dzi\u0119ki temu punkty s\u0105 rozmieszczone wzd\u0142u\u017c jednego po\u0142udnika. Dla ka\u017cdego punktu na powierzchni kuli wyznaczany jest wektor skierowany od tego punktu do \u015brodka kuli. Ten wektor okre\u015bla kierunek, w kt\u00f3rym \u201epatrzy\u201d narz\u0119dzie lub uk\u0142ad wsp\u00f3\u0142rz\u0119dnych przypisany do punktu – jest to o\u015b Z lokalnego uk\u0142adu.<\/p>\n\n\n\n
              Aby okre\u015bli\u0107 pe\u0142n\u0105 orientacj\u0119 w przestrzeni, potrzebne s\u0105 jeszcze osie X i Y. Do tego wykorzystywane s\u0105 dwa wektory styczne do powierzchni kuli, obliczane jako pochodne pozycji punktu wzgl\u0119dem k\u0105t\u00f3w sferycznych. Jeden z tych wektor\u00f3w jest wybierany jako podstawa do okre\u015blenia osi X, a je\u015bli jego d\u0142ugo\u015b\u0107 by\u0142aby zbyt ma\u0142a, wybierany jest drugi. Wektor ten jest nast\u0119pnie normalizowany, czyli przeliczany tak, aby mia\u0142 d\u0142ugo\u015b\u0107 r\u00f3wn\u0105 1.<\/p>\n\n\n\n
              Dalej budowany jest ortogonalny uk\u0142ad wsp\u00f3\u0142rz\u0119dnych. O\u015b Y wyznaczana jest jako wektor prostopad\u0142y do osi Z i wybranego wektora stycznego, a nast\u0119pnie ponownie wyznaczana jest o\u015b X, by zagwarantowa\u0107, \u017ce wszystkie trzy osie tworz\u0105 sp\u00f3jny, prawid\u0142owy uk\u0142ad. Z tych trzech wektor\u00f3w tworzona jest macierz rotacji, kt\u00f3ra opisuje, jak lokalny uk\u0142ad wsp\u00f3\u0142rz\u0119dnych punktu jest ustawiony wzgl\u0119dem uk\u0142adu globalnego. Na koniec z tej macierzy wyci\u0105gane s\u0105 k\u0105ty Eulera, kt\u00f3re opisuj\u0105 orientacj\u0119 punktu jako sekwencj\u0119 obrot\u00f3w wok\u00f3\u0142 osi. Wypisywana jest pozycja punktu oraz te trzy k\u0105ty, co razem stanowi pe\u0142n\u0105 informacj\u0119 o po\u0142o\u017ceniu i ustawieniu w przestrzeni.<\/p>\n\n\n\n
              Aby zweryfikowa\u0107 poprawno\u015b\u0107 generowanych punkt\u00f3w, opracowano prosty program w astorinoIDE, kt\u00f3ry wykorzystuje trzy z wcze\u015bniej obliczonych punkt\u00f3w na powierzchni kuli. Program ten wykonuje ruch robota po \u0142uku, w kt\u00f3rym pierwszy punkt pe\u0142ni rol\u0119 punktu pocz\u0105tkowego, drugi – punktu pomocniczego, a trzeci – punktu ko\u0144cowego. Dzi\u0119ki tej metodzie mo\u017cna wizualnie sprawdzi\u0107, czy robot prawid\u0142owo pod\u0105\u017ca wyznaczon\u0105 trajektori\u0105. Poni\u017cej przedstawiono kod programu realizuj\u0105cego ten ruch:<\/p>\n\n\n\n
              <\/div>\n\n\n\n
              .PROGRAM KULA\n  HOME\n  lappro P1, 100\n  lmove P1\n  c1move P2\n  c2move P3\n  ldepart 100\n.END<\/code><\/pre>\n\n\n\n
              <\/div>\n\n\n\n
              Do przeniesienia obliczonych punkt\u00f3w do programu w astorinoIDE wystarczy skopiowa\u0107 wybrane punkty, kt\u00f3re zosta\u0142y zwr\u00f3cone w wyniku kompilacji programu w Pythonie. Po wygenerowaniu punkt\u00f3w, nale\u017cy je r\u0119cznie wprowadzi\u0107 do \u015brodowiska robota, co umo\u017cliwia ich dalsze wykorzystanie w kolejnych etapach aplikacji. Poni\u017cej przedstawiono przyk\u0142adowe wyniki uzyskane po kompilacji kodu (rys. 23).<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 23. Przyk\u0142adowy wynik kompilacji programu w j\u0119zyku Python dla parametr\u00f3w z przyk\u0142adowego kodu (kliknij, aby powi\u0119kszy\u0107)<\/em>. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Warto tu zwr\u00f3ci\u0107 uwag\u0119 na ostrze\u017cenie, kt\u00f3re informuje, \u017ce wykryto tzw. „gimbal lock” przy konwersji rotacji na k\u0105ty Eulera w uk\u0142adzie 'zyz’, co oznacza, \u017ce jedna z osi obrotu pokrywa si\u0119 z inn\u0105, przez co jeden z k\u0105t\u00f3w (trzeci) nie mo\u017ce by\u0107 jednoznacznie okre\u015blony i zostaje ustawiony na zero.<\/p>\n\n\n\n
              Je\u015bli chcemy przenie\u015b\u0107 pierwszy punkt do astorinoIDE, wystarczy wykorzysta\u0107 w terminalu (rys. 24) polecenie POINT<\/strong> oraz TRANS<\/strong>.<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 24. Widok okna terminala z instrukcj\u0105 do tworzenia nowego punktu (kliknij, aby powi\u0119kszy\u0107)<\/em>. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Po zatwierdzeniu klawiszem Enter<\/strong> punkt zostanie zapisany. Je\u015bli chcieliby\u015bmy sprawdzi\u0107, czy rzeczywi\u015bcie tak si\u0119 sta\u0142o, nale\u017cy wej\u015b\u0107 w zak\u0142adk\u0119 Points -> Transformation<\/strong>. Wy\u015bwietli si\u0119 w\u00f3wczas okno z zapisanymi punktami (rys. 25), w\u015br\u00f3d kt\u00f3rych powinni\u015bmy dostrzec nasz zapisany punkt, tak jak poni\u017cej:<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 25. Widok okna punkt\u00f3w kartezja\u0144skich w programie astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Proces ten nale\u017cy powt\u00f3rzy\u0107 dla dw\u00f3ch pozosta\u0142ych punkt\u00f3w. Wa\u017cne jest, aby ka\u017cdy z tych punkt\u00f3w zosta\u0142 zadeklarowany pod innym numerem porz\u0105dkowym, np. p2, p3, itd., aby zachowa\u0107 sp\u00f3jno\u015b\u0107 w dalszej cz\u0119\u015bci programu i umo\u017cliwi\u0107 prawid\u0142owe przypisanie do odpowiednich pozycji w przestrzeni robota.<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys.26. Widok okna punkt\u00f3w kartezja\u0144skich w programie astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              W celu przetestowania programu nale\u017cy rozpocz\u0105\u0107 cykl, klikaj\u0105c przycisk Cycle Start.<\/strong><\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 27. Widok g\u00f3rnego paska narz\u0119dzia astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Je\u015bli chcemy uruchomi\u0107 program od pocz\u0105tku lub od jakiego\u015b konkretnego kroku (kt\u00f3ry nie jest pierwszym), nale\u017cy wcisn\u0105\u0107 kolejno Pause<\/strong> oraz Cycle Stop<\/strong>. Nast\u0119pnie z listy rozwijanej Step<\/strong> nale\u017cy wybra\u0107 linijk\u0119 kodu, od kt\u00f3rej chcemy zacz\u0105\u0107 wykonywanie ruchu. Rozpocz\u0119cie ruchu zaczynamy klikni\u0119ciem przycisku Pause<\/strong>, a nast\u0119pnie przyciskiem Cycle Start.<\/strong><\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 28. Widok g\u00f3rnego paska narz\u0119dzia astorinoIDE. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Wynik programu przedstawiono na rysunku poni\u017cej:<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 29. Widok wywo\u0142ania przyk\u0142adowego programu realizuj\u0105cego ruch po kuli wraz z zaznaczon\u0105 trajektori\u0105 nr 1. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Z analizy wynika, \u017ce warto\u015b\u0107 azymutu pozostaje sta\u0142a (rys. 29), podczas gdy k\u0105t zenitalny zmienia si\u0119 w zakresie od 0do oko\u0142o 90, przy czym ostatni punkt charakteryzuje si\u0119 w\u0142a\u015bnie takim k\u0105tem. Ruch odbywa si\u0119 wy\u0142\u0105cznie w p\u0142aszczy\u017anie XZ <\/em>(dla y = const. = y0<\/sub>).<\/p>\n\n\n\n
              Program przetestowano r\u00f3wnie\u017c dla innych warto\u015bci k\u0105ta zenitalnego i azymutu. Tym razem przyj\u0119to nast\u0119puj\u0105ce warto\u015bci konta zenitalnego \u03b8 i azymutu \u03a6:<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"<\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Warto\u015b\u0107 azymutu b\u0119dzie zmienia\u0142a si\u0119 w p\u0119tli zgodnie z nast\u0119puj\u0105cym wzorem:<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"<\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              Odpowiadaj\u0105ca temu linijka kodu w Pythonie to:<\/p>\n\n\n\n
              <\/div>\n\n\n\n
              v = -np.pi \/ 2 + i * (np.pi \/ 2) \/ (N - 1))<\/code><\/pre>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 30. Widok wywo\u0142ania przyk\u0142adowego programu realizuj\u0105cego ruch po kuli wraz z zaznaczon\u0105 trajektori\u0105 nr 2. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              W tym przypadku (rys. 30) k\u0105t zenitalny pozostaje sta\u0142y, co powoduje, \u017ce ruch odbywa si\u0119 w p\u0142aszczy\u017anie XY (dla z = const. = z0<\/sub>).<\/p>\n\n\n\n
              Poni\u017cej przedstawiono dwie inne wygenerowane trajektorie:<\/p>\n\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 31. Widok wywo\u0142ania przyk\u0142adowego programu realizuj\u0105cego ruch po kuli wraz z zaznaczon\u0105 trajektori\u0105 nr 3. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n
              \n
              \"\"
              Rys. 32. Widok wywo\u0142ania przyk\u0142adowego programu realizuj\u0105cego ruch po kuli wraz z zaznaczon\u0105 trajektori\u0105 nr 4. \u0179r\u00f3d\u0142o: ASTOR<\/em><\/figcaption><\/figure>\n<\/div>\n\n\n
              <\/div>\n\n\n\n
              W tym artykule zaprezentowano dwie metody sterowania robotem Astorino po powierzchni kuli, z kt\u00f3rych ka\u017cda ma swoje zalety i ograniczenia. Pierwsza metoda jest prostsza i bardziej intuicyjna, opiera si\u0119 na prostych obliczeniach geometrycznych. Druga metoda, cho\u0107 bardziej skomplikowana, pozwala na wi\u0119ksz\u0105 elastyczno\u015b\u0107 i precyzj\u0119, poniewa\u017c umo\u017cliwia generowanie punkt\u00f3w i orientacji narz\u0119dzia na podstawie matematycznego opisu kuli.<\/p>\n\n\n\n
              Praktyczna realizacja sterowania w oprogramowaniu astorinoIDE pozwala na symulacj\u0119 ruch\u00f3w robota, co jest niezb\u0119dne do testowania poprawno\u015bci trajektorii przed ich rzeczywistym wykonaniem. Zar\u00f3wno metody sterowania, jak i implementacja w astorinoIDE stanowi\u0105 solidn\u0105 podstaw\u0119 do dalszego rozwoju technologii sterowania robotem Astorino po powierzchniach sferycznych. W przysz\u0142o\u015bci mo\u017cna je dostosowa\u0107 do bardziej zaawansowanych aplikacji, wymagaj\u0105cych precyzyjnego ruchu po bardziej z\u0142o\u017conych trajektoriach, czy nawet w przestrzeniach o bardziej z\u0142o\u017conych kszta\u0142tach.<\/p>\n\n\n\n
              <\/div>\n\n\n\n
              \n
              Autor artyku\u0142u:<\/p><\/br>\n
              Daniel Michalak<\/strong><\/h2>\n
              Praktykant ASTOR<\/p>\n
              Student III roku kierunku Automatyka i robotyka na Politechnice Pozna\u0144skiej<\/p>\n<\/div>\n\n\n\n
              <\/div>\n\n\n\n            
              \n                
              \n                    
              \n                                                    
              Newsletter Poradnika Automatyka<\/h3><\/div>\n                                                
              Czytaj trendy i inspiracje, podstawy automatyki, automatyk\u0119 w praktyce<\/p>\n<\/div>\n                        
              \n                                                                                            
              \n