Skip to main content
Glama

nUR MCP Server

by nonead
manipulation.cpython-312.pyc52.1 kB
� #0hŗ���ddlZddl�d�Zd�Zd�Zd�Zd�Zd�Zd �Zd �Z d �Z d �Z d �Z d�Z d�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd�Zd.d �Zd.d!�Z d.d"�Z!d#�Z"d$�Z#d%�Z$d&�Z%d'�Z&d(�Z'd)�Z(d*�Z)d+�Z*d,�Z+d-�Z,y)/�N)�*c�4�tjdd|df�S)z\ Generates a vector of length x, each component being a random float b/w -10 and 10 i����� �)�random�uniform)�xs �VD:\mcp\Nonead-Universal-Robots-MCP\Nonead-Universal-Robots-MCP\URBasic\manipulation.py� randomVecr s�� �>�>�#�b�!�A�� '�'�c� �tjdd�}dtjz|z}tjdd�}dtjz|z}t d�}tj |�tj |�z|d<tj |�tj |�z|d<tj |�|d<dtjztjdd�z}||fS)z3 Generates a random unit axis and an angle rr���r)rr�math�pi�zeros�cos�sin)�u� longitude�v�latitude�axis�thetas r �randomUnitAxisAngler s��� ���q�!��A��$�'�'� �!� �I� ���q�!��A�����y��{�H� ��<�D��h�h�x� ����)�!4�4�D��G��h�h�x� ����)�!4�4�D��G��h�h�x� �D��G� �d�g�g�I�f�n�n�Q��*� *�E� ��;�r c�D�tj|�}|dk(r|S||z S)z8 Returns the normalized version of the vector v r)�linalg�norm)rrs r � normalizer $s&�� �;�;�q�>�D� �q�y��x� �T�6�Mr c�v�t|�t|d�k7ryt�}t|�D]|\}}t|�D]i\}}||k(r.|dkr|dkDr|jd��'|jd��9t |�dkr|jd��Y|jd��k�~t |�S)z@ Returns True if input M is close to an identity matrix rFgj�t��?g+�����?T�����MbP?)�len�list� enumerate�append�abs�all)�M�c�i�row�j�vals r �is_identity_matrixr/.s��� �1�v��Q�q�T���� ��A��A�,���3���n�F�A�s��!�t���;�3��;��H�H�T�N��H�H�U�O��s�8�e�#��H�H�T�N��H�H�U�O�%�� �q�6�Mr c��t|�}tt|j|��xr$t t j |�dz �dkS)z= Returns True if input R is a valid rotation matrix. rr")�asarrayr/�dot�Tr'r�det��Rs r � is_rot_matrixr7Fs=�� �� �A� �c�!�#�#�a�j� )� L�s�6�:�:�a�=��?�/C�e�/K�Lr c�T�t|�}t|�sJd��|jS)a Takes a rotation matrix belonging to SO(3) and returns its inverse. Example: R = [[.707,-.707,0],[.707,.707,0],[0,0,1]] RotInv(R) >> array([[ 0.707, 0.707, 0. ], [-0.707, 0.707, 0. ], [ 0. , 0. , 1. ]]) �Not a valid rotation matrix)r1r7r3r5s r �RotInvr:Os*�� �� �A� �� �:�:�:� � �3�3�Jr c��t|�}t|�dk(sJd��|j�}td|d |dg|dd|d g|d |ddgg�}|S)a  Takes a 3-vector representing angular velocity and returns the 3x3 skew-symmetric matrix version, an element of so(3). Example: w = [2, 1, -4] VecToso3(w) >> array([[ 0, 4, 1], [-4, 0, -2], [-1, 2, 0]]) r�Not a 3-vectorrrr)r1r#�flatten�array)�w�w_so3mats r �VecToso3rA`sw�� �� �A� �q�6�Q�;�(�(�(�;� � � � �A��q�1�Q�4�%��1��&��1��q�1�Q�4�%�(8�A�a�D�5�!�A�$��:J�K�L�H� �Or c�|�t|�}|jdk(sJd��t|dg|dg|dgg�}|S)a Takes a 3x3 skew-symmetric matrix (an element of so(3)) and returns the corresponding 3-vector. Example: w_so3mat = [[ 0, 4, 1],[-4, 0, -2],[-1, 2, 0]] so3ToVec(w_so3mat) >> array([[ 2], [ 1], [-4]]) �rrzNot a 3x3 matrix�rr�rr�rr)r1�shaper>)r@r?s r �so3ToVecrHtsN���x� �H� �>�>�U� "�6�$6�6� "� ��� ���#���(�3�-��A�B�A� �Hr c��t|�}t|�dk(sJd��tj|�}t |�}||fS)a Takes a 3-vector of exp coords r = w_unit*theta and returns w_unit and theta. Example: r = [2, 1, -4] w_unit, theta = AxisAng3(r) w_unit >> array([ 0.43643578, 0.21821789, -0.87287156]) theta >> 4.5825756949558398 rr<)r1r#rrr )�rr�w_units r �AxisAng3rL�sC�� �� �A� �q�6�Q�;�(�(�(�;� �K�K��N�E� �q�\�F� �5�=�r c�0�t|�}t|�dk(sJd��t|�\}}t|�}t d�t j |�|zzdt j|�z t||�zz}t|�sJd��|S)a] Takes a 3-vector of exp coords r = w_unit*theta and returns the corresponding rotation matrix R (an element of SO(3)). Example: r = [2, 1, -4] MatrixExp3(r) >> array([[ 0.08568414, -0.75796072, -0.64664811], [ 0.97309386, -0.07566572, 0.2176305 ], [-0.21388446, -0.64789679, 0.73108357]]) rr<rz'Did not produce a valid rotation matrix) r1r#rLrA�identityrrrr2r7)rJrKrr@r6s r � MatrixExp3rO�s��� �� �A� �q�6�Q�;�(�(�(�;��Q�K�M�F�E����H��� �d�h�h�u�o�h�.�.�!�D�H�H�U�O�2C�S��S[�E\�1\�\�A� �� �F�F�F� � �Hr c���t|�}t|�sJd��t|�r td�St |�dkDr�t |�dkr�t j }tt|��}||dk(r.t|dg|dgd|zgg�dzd d|zzd zz }||zS||d k(r.t|d gd|zg|d gg�dzd d|zzd zz }||zS||dk(r.td|zg|dg|dgg�dzd d|zzd zz }||zSt jt |�dz d z �}||jz d t j|�zz }tt|��}||zS)a Takes a rotation matrix R and returns the corresponding 3-vector of exp coords r = w_unit*theta. Example: R = [[.707,-.707,0],[.707,.707,0],[0,0,1]] MatrixLog3(R) >> array([[ 0. ], [ 0. ], [ 0.78554916]]) r9r�j�t���+�����)rrrE)rrrrg�?)rr)rrrD)rrrF)rr)r1r7r/r�tracerr�max�diagr>�acosr3rr rH)r6rr*rKr@s r � MatrixLog3rW�s��� �� �A� �� �:�:�:� ��!���U�|�� �Q�x�&��U�1�X��.����� ��Q��L�� ��#��;��Q�s�V�H�a��f�X�q��s�e�4�5�a�7�!�Q�q�S�'�C��H�F��%�<� � �!�C�&�[��Q�s�V�H�a��c�U�A�c�F�8�4�5�a�7�!�Q�q�S�'�C��H�F��%�<� � �!�C�&�[��Q�q�S�E�1�S�6�(�A�c�F�8�4�5�a�7�!�Q�q�S�'�C��H�F��%�<� � �I�I�u�Q�x��z�1�n� %�E��A�C�C��!�D�H�H�U�O�+�,�H� �x��)� *�F� �%�<�r c���t|�}t|�}t|�dk(sJd��t|�sJd��d|_t t ||f�t gd��f�}|S)a� Takes a rotation matrix R and a point (3-vector) p, and returns the corresponding 4x4 transformation matrix T, an element of SE(3). Example: R = [[.707,-.707,0],[.707,.707,0],[0,0,1]] p = [5,-4,9] RpToTrans(R,p) >> array([[ 0.707, -0.707, 0. , 5. ], [ 0.707, 0.707, 0. , -4. ], [ 0. , 0. , 1. , 9. ], [ 0. , 0. , 0. , 1. ]]) rzPoint not a 3-vectorzR not a valid rotation matrixr�rrrr)r1r#r7rG�vstack�hstackr>)r6�pr3s r � RpToTransr]�sh�� �� �A��� �A� �q�6�Q�;�.�.�.�;� �� �<�<�<� ��A�G����!�u� �u�Y�/�0�1�A� �Hr c���t|�}|jdk(sJd��|dd�dd�f}t|�sJd��tgd�|dd� �sJd ��|dd�df}d |_||fS) a Takes a transformation matrix T and returns the corresponding R and p. Example: T = [[0.707,-0.707,0,5],[0.707,0.707,0,-4],[0,0,1,9],[0,0,0,1]] R, p = TransToRp(T) R >> array([[ 0.707, -0.707, 0. ], [ 0.707, 0.707, 0. ], [ 0. , 0. , 1. ]]) p >> array([[ 5.], [-4.], [ 9.]]) ��r`�Input not a 4x4 matrixNrz'Input not a valid transformation matrixrY�����r")�atolz3Last row of homogenous T matrix should be [0,0,0,1]r)r1rGr7�allclose)r3r6r\s r � TransToRpre�s��� �� �A� �7�7�e� �5�5�5� � �"�1�"�R�a�R�%��A� �� �F�F�F� � �I�a��e�� /�f�1f�f� /� �"�1�"�R�%��A��A�G� �a�4�Kr c��t|�}t|�\}}tt|�t t|� |��}|S)aX Returns inverse of transformation matrix T. Example: T = [[0.707,-0.707,0,5],[0.707,0.707,0,-4],[0,0,1,9],[0,0,0,1]] TransInv(T) >> array([[ 0.707, 0.707, 0. , -0.707], [-0.707, 0.707, 0. , 6.363], [ 0. , 0. , 1. , -9. ], [ 0. , 0. , 0. , 1. ]]) )r1rer]r:r2)r3r6r\�T_invs r �TransInvrh s<�� �� �A� �Q�<�D�A�q� �f�Q�i��f�Q�i�Z��!2� 3�E� �Lr c���t|�}t|�dk(sJd��d|_|dd}t|�}|dd}t t ||f�t d�f�}|S)aC Takes a 6-vector (representing spatial velocity) and returns the corresponding 4x4 matrix, an element of se(3). Example: V = [3,2,5,-3,7,0] VecTose3(V) >> array([[ 0., -5., 2., -3.], [ 5., 0., -3., 7.], [-2., 3., 0., 0.], [ 0., 0., 0., 0.]]) ��Input not a 6-vector�rjrNrr`)r1r#rGrArZr[r)�Vr?r@r�V_se3mats r �VecTose3rosj�� �� �A� �q�6�Q�;�.�.�.�;��A�G� �"�1��A���{�H� �!�"��A��v�x��l�+�U�1�X�6�7�H� �Or c��t|�}|jdk(sJd��|dd�dd�f}t|�}|dd�df}d|_t||f�}|S)a� Takes an element of se(3) and returns the corresponding (6-vector) spatial velocity. Example: V_se3mat = [[ 0., -5., 2., -3.], [ 5., 0., -3., 7.], [-2., 3., 0., 0.], [ 0., 0., 0., 0.]] se3ToVec(V_se3mat) >> array([[ 3.], [ 2.], [ 5.], [-3.], [ 7.], [ 0.]]) r_zMatrix is not 4x4Nrrbr)r1rGrHrZ)rnr@r?rrms r �se3ToVecrq7so��"�x� �H� �>�>�U� "�7�$7�7� "�����2�A�2���H����A���!��B���A��A�G���!�u� �A� �Hr c�P�t|�}|jdk(sJd��t|�\}}|j�}t d|d |dg|dd|d g|d |ddgg�}t |t ||�f�}t td�|f�}t||f�}|S)a= Takes a transformation matrix T and returns the 6x6 adjoint representation [Ad_T] Example: T = [[0.707,-0.707,0,5],[0.707,0.707,0,-4],[0,0,1,9],[0,0,0,1]] Adjoint(T) >> array([[ 0.707, -0.707, 0. , 0. , 0. , 0. ], [ 0.707, 0.707, 0. , 0. , 0. , 0. ], [ 0. , 0. , 1. , 0. , 0. , 0. ], [-6.363, -6.363, -4. , 0.707, -0.707, 0. ], [ 6.363, -6.363, -5. , 0.707, 0.707, 0. ], [ 6.363, 0.707, 0. , 0. , 0. , 1. ]]) r_rarrrrC) r1rGrer=r>rZr2rr[)r3r6r\�p_skew�ad1�ad2�adTs r �AdjointrwUs��� �� �A� �7�7�e� �5�5�5� � �Q�<�D�A�q� � � � �A� �Q��1���q��t�$�q��t�Q��1���&6�!�A�$���!��a�8H�I� J�F� �!�S���]�#� $�C� �%��,�q�!� "�C� �#�c�� �C� �Jr c��t|�}t|�}t|�t|�cxk(r dk(sJd��Jd��ttj |�dz �dksJd��t |�sJd��|j �}|j �}t||� ||zz}d|_|}d|_t||f�}|S)a� Takes a point q (3-vector) on the screw, a unit axis s_hat (3-vector) in the direction of the screw, and a screw pitch h (scalar), and returns the corresponding 6-vector screw axis S (a normalized spatial velocity). Example: q = [3,0,0] s_hat = [0,0,1] h = 2 ScrewToAxis(q,s_hat,h) >> array([[ 0], [ 0], [ 1], [ 0], [-3], [ 2]]) rzq or s_hat not a 3-vectorrr"zs_hat not a valid unit vectorzh not a scalarr) r1r#r'rr�isscalarr=�crossrGrZ)�q�s_hat�h�v_wnormrK�Ss r � ScrewToAxisr�ps���$ �� �A� �E�N�E� �q�6�S��Z� $�1� $�A�&A�A� $�A�&A�A� $� �v�{�{�5�!�A�%� &�� .�O�0O�O� .� �A�;�(�(�(�;� � � � �A� �M�M�O�E��U�A����5��(�G��G�M� �F��F�L���w�� �A� �Hr c��t|�}t|�dk(sJd��|dd}|dd}tj|�dk(rAtj|�}t |�}d|_t td�|f�}||fStj|�}t |�}d|_||z }d|_t ||f�}||fS)aq Takes a 6-vector of exp coords STheta and returns the screw axis S and the distance traveled along/ about the screw axis theta. Example: STheta = [0,0,1,0,-3,2] S, theta = AxisAng6(STheta) S >> array([[ 0.], [ 0.], [ 1.], [ 0.], [-3.], [ 2.]]) theta >> 1.0 rjrkNrrr)r1r#rrr rGrZr)�SThetar?rr�v_unitrrKs r �AxisAng6r��s���$�V�_�F� �v�;�!� �3�3�3� ��r�� �A��q�r� �A� �{�{�1�~���� � �A����1����� � �E�%�L�&�)� *���%�x�� �K�K��N�E� �q�\�F��F�L� �u�W�F��F�L���v���A� �e�8�Or c���t|�}t|�dk(sJd��t|�\}}|dd}|dd}|dd}d|_tj |�dk(rt d�}|}t||�}|S||z} t| �}t|�} tt d�|zdtj|�z | zz|tj|�z t| | �zz|�}t||�}|S)a� Takes a 6-vector of exp coords STheta and returns the corresponding 4x4 transformation matrix T. Example: STheta = [0,0,1,0,-3,2] MatrixExp6(STheta) >> array([[ 0.54030231, -0.84147098, 0. , 1.37909308], [ 0.84147098, 0.54030231, 0. , -2.52441295], [ 0. , 0. , 1. , 2. ], [ 0. , 0. , 0. , 1. ]]) rjrkNrrrr)r1r#r�rGrrrNr]rOrAr2rrr) r�rrrKr��vThetar6r\r3rJr@s r � MatrixExp6r��s���V�_�F� �v�;�!� �3�3�3� ����H�A�u� �r��U�F� �q�r�U�F� �A�B�Z�F��F�L� �{�{�6��a�� �Q�K�� �� �a��N�����u� �A��1� �A����H� �X�a�[�� ��$�(�(�5�/� 1�8�;� ;�U�4�8�8�E�?�=R�TW�X`�ai�Tj�<j� j�ms�t�A��!�A��A� �Hr c��t|�}|jdk(sJd��t|�\}}t|�rt d�}|}t ||f�}|St |�dkDr�t |�dkr�tj}t|�}||z }t|�}td�|z |dz z d|z dtj|dz �dzz z t||�zz} t| |�} | |z}t ||f�}|Stjt |�dz dz �}||jz dtj |�zz }td�|z |dz z d|z dtj|dz �dzz z t||�zz} t|�}t| |�} | |z}t ||f�}|S) aP Takes a transformation matrix T and returns the corresponding 6-vector of exp coords STheta. Example: T = [[ 0.54030231, -0.84147098, 0. , 1.37909308], [ 0.84147098, 0.54030231, 0. , -2.52441295], [ 0. , 0. , 1. , 2. ], [ 0. , 0. , 0. , 1. ]] MatrixLog6(T): >> array([[ 0.00000000e+00], [ 0.00000000e+00], [ 9.99999995e-01], [ 1.12156694e-08], [ -2.99999999e+00], [ 2.00000000e+00]]) r_rarrQrRrrr)r1rGrer/rrZrSrrrWrArN�tanr2rVr3r) r3r6r\rKr�r�r�wThetar@�Ginvr�s r � MatrixLog6r��s���" �� �A� �7�7�e� �5�5�5� � �Q�<�D�A�q��!���u�������� �$��� � �Q�x�&��U�1�X��.������A��������F�#����{�5� �8�A�:�-��5��1�d�h�h�u�Q�w�>O�PQ�>Q�;R�1R�TW�X`�ai�Tj�0j�j���T�1����������(�)��� � �I�I�u�Q�x��z�1�n� %�E��A�C�C��!�D�H�H�U�O�+�,�H� �A�;�u� �x��z� )�Q�u�W�q�$�(�(�5��7�:K�A�:M�7N�-N�PS�T\�]e�Pf�,f� f�D� ��]�F� ��q�\�F� �E�\�F� �V�V�$� %�F� �Mr c��t|�}t|�d}|jdk(sJd��t|d�dk(sJd��t|�j}t |dd�df|dz�}t t|�dz �D],}t |dd�|dzf||dzz�}t||�}�.t||�}|S)a Takes - an element of SE(3): M representing the configuration of the end-effector frame when the manipulator is at its home position (all joint thetas = 0), - a list of screw axes Slist for the joints w.r.t fixed world frame, - a list of joint coords thetalist, and returns the T of end-effector frame w.r.t. fixed world frame when the joints are at the thetas specified. Example: S1 = [0,0,1,4,0,0] S2 = [0,0,0,0,1,0] S3 = [0,0,-1,-6,0,-0.1] Slist = [S1, S2, S3] thetalist = [math.pi/2, 3, math.pi] M = [[-1, 0, 0, 0], [ 0, 1, 0, 6], [ 0, 0, -1, 2], [ 0, 0, 0, 1]] FKinFixed(M,Slist,thetalist) >> array([[ -1.14423775e-17, 1.00000000e+00, 0.00000000e+00, -5.00000000e+00], [ 1.00000000e+00, 1.14423775e-17, 0.00000000e+00, 4.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 1.68584073e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) rr_�M not a 4x4 matrixrj�Incorrect Screw Axis lengthNr)r1rerGr#r3r��ranger2)r)�Slist� thetalist�R_Mr*r+�nex�T_ses r � FKinFixedr�s���4 �� �A� �A�,�q�/�C� �7�7�e� �1�1�1� � �u�Q�x�=�A� �<�<�<� � �E�N� � �E��5��1��:�i��l�*�+�A� �3�y�>�!�#� $����q��1��u��i��!��n�4�5�� ��3�K��%� �q�!�9�D� �Kr c��t|�}t|�d}|jdk(sJd��t|d�dk(sJd��t|�j}t |t |dd�df|dz��}tt|�dz �D],}t |dd�|dzf||dzz�}t ||�}�.|}|S)a� Same as FKinFixed, except here the screw axes are expressed in the end-effector frame. Example: B1 = [0,0,-1,2,0,0] B2 = [0,0,0,0,1,0] B3 = [0,0,1,0,0,0.1] Blist = [S1b, S2b, S3b] thetalist = [math.pi/2, 3, math.pi] M = [[-1, 0, 0, 0], [ 0, 1, 0, 6], [ 0, 0, -1, 2], [ 0, 0, 0, 1]] FKinBody(M,Blist,thetalist) >> array([[ -1.14423775e-17, 1.00000000e+00, 0.00000000e+00, -5.00000000e+00], [ 1.00000000e+00, 1.14423775e-17, 0.00000000e+00, 4.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 1.68584073e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) rr_r�rjr�Nr)r1rerGr#r3r2r�r�)r)�Blistr�r�r*r+r�r�s r �FKinBodyr�:s���( �� �A� �A�,�q�/�C� �7�7�e� �1�1�1� � �u�Q�x�=�A� �<�<�<� � �E�N� � �E� �A�z�%��!��*�Y�q�\�1�2�3�A� �3�y�>�!�#� $����q��1��u��i��!��n�4�5�� ��3�K��%� �D� �Kr c��t|�}td|f�}t|�j}|dd�df|dd�df<t d|�D]z}t |dd�df|dz�}t |dz �D],}t |dd�|dzf||dzz�}t ||�}�.t t|�|dd�|f�|dd�|f<�||S)a$ Takes a list of joint angles (thetalist) and a list of screw axes (Slist) expressed in fixed space frame, and returns the space Jacobian (a 6xN matrix, where N is the # joints). Example: S1 = [0,0,1,4,0,0] S2 = [0,0,0,0,1,0] S3 = [0,0,-1,-6,0,-0.1] Slist = [S1, S2, S3] thetalist = [math.pi/2, 3, math.pi] FixedJacobian(Slist,thetalist) >> array([[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 1.00000000e+00, 0.00000000e+00, -1.00000000e+00], [ 4.00000000e+00, -1.00000000e+00, -4.00000000e+00], [ 0.00000000e+00, 1.11022302e-16, -5.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, -1.00000000e-01]]) rjNrr�r#rr1r3r�r�r2rw)r�r��N�J�kr*r+r�s r � FixedJacobianr�bs���& �I��A� �q��e� �A� �E�N� � �E� �1�Q�3�Z�A�a��c�F� �1�Q�Z�� �u�Q�q�S�z�)�A�,�.� /���q��s��A��U�1�Q�q�S�5�\�)�A�a�C�.�8�9�C��A�s� �A���W�Q�Z��q��s��,��!�A�#�� � �Hr c��t|�}td|f�}t|�j}|dd�|dz f|dd�|dz f<t |dz �D]�}t |dd�|dzf ||dzz�}t |dzt|��D]'}t |dd�|f ||z�}t ||�}�)t t|�|dd�|f�|dd�|f<��|S)a& Takes a list of joint angles (thetalist) and a list of screw axes (Blist) expressed in end-effector body frame, and returns the body Jacobian (a 6xN matrix, where N is the # joints). Example: B1 = [0,0,-1,2,0,0] B2 = [0,0,0,0,1,0] B3 = [0,0,1,0,0,0.1] Blist = [B1, B2, B3] thetalist = [math.pi/2, 3, math.pi] BodyJacobian(Blist,thetalist) >> array([[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00], [ -1.00000000e+00, 0.00000000e+00, 1.00000000e+00], [ -5.00000000e+00, 1.22464680e-16, 0.00000000e+00], [ -6.12323400e-16, -1.00000000e+00, 0.00000000e+00], [ 0.00000000e+00, 0.00000000e+00, 1.00000000e-01]]) rjNrrr�)r�r�r�r�r�r*r+r�s r � BodyJacobianr��s���& �I��A� �q��e� �A� �E�N� � �E��Q�q��s�U�|�A�a��!��e�H� �1�Q�3�Z�� ��a��!��e� �}�Y�q��s�^�3� 4���q��s�C� �N�+�A��e�A�a�C�j�[��1��5�6�C��C�� �A�,��W�Q�Z��q��s��,��!�A�#�� � �Hr c �z�t|�}|jdk(sJd��d}t|�}t|�jd|�}t |||�} t t t| �|��} | dd�df} | dd�df} t|�} d}||k�r!tj| �|kDstj| �|kDr�| j|d�t tjt|| ��| �z}t||jd|�f�}t |||j��} t t t| �|��} |j|�} | dd�df} | dd�df} |dz }||kr2tj| �|kDr��tj| �|kDr��|S)aG A numerical inverse kinematics routine based on Newton-Raphson method. Takes a list of screw axes (Blist) expressed in end-effector body frame, the end-effector zero configuration (M), the desired end-effector configuration (T_sd), an initial guess of joint angles (thetalist_init), and small positive scalar thresholds (wthresh, vthresh) controlling how close the final solution thetas must be to the desired thetas. Example: wthresh = 0.01 vthresh = 0.001 M = [[1,0,0,-.817],[0,0,-1,-.191],[0,1,0,-.006],[0,0,0,1]] T_sd = [[0,1,0,-.6],[0,0,-1,.1],[-1,0,0,.1],[0,0,0,1]] thetalist_init = [0]*6 B1 = [0,1,0,.191,0,.817] B2 = [0,0,1,.095,-.817,0] B3 = [0,0,1,.095,-.392,0] B4 = [0,0,1,.095,0,0] B5 = [0,-1,0,-.082,0,0] B6 = [0,0,1,0,0,0] Blist = [B1,B2,B3,B4,B5,B6] round(IKinBody(Blist, M, T_sd, thetalist_init, wthresh, vthresh), 3) >> array([[ 0. , 0. , 0. , 0. , 0. , 0. ], [-0.356, -0.535, 0.468, 1.393, -0.356, -2.897], [-0.399, -1.005, 1.676, -0.434, -0.1 , -1.801], [-0.516, -1.062, 1.731, -1.63 , -0.502, -0.607], [-0.493, -0.923, 1.508, -0.73 , -0.289, -1.42 ], [-0.472, -0.818, 1.365, -0.455, -0.467, -1.662], [-0.469, -0.834, 1.395, -0.561, -0.467, -1.571]]) r_�T_sd not a 4x4 matrix�drNrr)r1rGr#�reshaper�r�r2rhrr�pinvr�rZr=)r�r)�T_sd�thetalist_init�wthresh�vthresh� maxiteratesr�� jointAngles�T_sb�Vb�wb�vb� thetalist_ir+�thetalist_nexts r �IKinBodyr��s���> �4�=�D� �:�:�� �7� 7�7� ��K� �N��A��.�)�1�1�!�A�6�K� �A�u�n� -�D� �C�����-� .�B� �B�Q�B��E��B� �A�B��E��B��.�)�K� �A� �K�-�V�[�[��_�W�4�� � �B���8O�$�,�,�Q�q�1�C�� � �L�QV�Wb�Dc�8d�fh�4i�i���k�>�+A�+A�!�A�+F�G�H� ���5�.�"8�"8�":�;�� ��H�T�N�D�1� 2��$�,�,�Q�0� � ����A��Y�� ���A��Y�� �Q��� �K�-�V�[�[��_�W�4�� � �B���8O� �r c��t|�}|jdk(sJd��d}t|�}t|�jd|�}t |||�} t t t| �|��} | dd�df} | dd�df} t|�} d}||k�rAtj| �|kDstj| �|kD�rt tt| ��t|| ��}| j|d�t tj|�| �z}t||jd|�f�}t |||j��} t t t| �|��} |j|�} | dd�df} | dd�df} |dz }||kr3tj| �|kDr��tj| �|kDr��|S)a� Similar to IKinBody, except the screw axes are in the fixed space frame. Example: M = [[1,0,0,0],[0,1,0,0],[0,0,1,0.910],[0,0,0,1]] T_sd = [[1,0,0,.4],[0,1,0,0],[0,0,1,.4],[0,0,0,1]] thetalist_init = [0]*7 S1 = [0,0,1,0,0,0] S2 = [0,1,0,0,0,0] S3 = [0,0,1,0,0,0] S4 = [0,1,0,-0.55,0,0.045] S5 = [0,0,1,0,0,0] S6 = [0,1,0,-0.85,0,0] S7 = [0,0,1,0,0,0] Slist = [S1,S2,S3,S4,S5,S6,S7] round(IKinFixed(Slist, M, T_sd, thetas, wthresh, vthresh), 3) >> array([[ 0. , 0. , 0. , 0. , 0. , 0. , 0. ], [ 0. , 4.471, -0. , -11.333, -0. , 6.863, -0. ], [ -0. , 3.153, -0. , -5.462, -0. , 2.309, 0. ], [ -0. , -1.006, 0. , 5.679, -0. , -4.673, 0. ], [ -0. , 1.757, 0. , -0.953, 0. , -0.804, -0. ], [ -0. , 1.754, 0. , -2.27 , -0. , 0.516, -0. ], [ 0. , 1.27 , 0. , -1.85 , -0. , 0.58 , -0. ], [ 0. , 1.367, 0. , -1.719, -0. , 0.351, -0. ], [ 0. , 1.354, 0. , -1.71 , -0. , 0.356, -0. ]]) r_r�r�rNrr)r1rGr#r�r�r�r2rhrrrwr�r�rZr=)r�r)r�r�r�r�r�r�r�r�r�r�r�r�r+�Jbr�s r � IKinFixedr��s���8 �4�=�D� �:�:�� �7� 7�7� ��K� �N��A��.�)�1�1�!�A�6�K� �Q��~� .�D� �C�����-� .�B� �B�Q�B��E��B� �A�B��E��B��.�)�K� �A� �K�-�V�[�[��_�W�4�� � �B���8O� ���$��(�-��k�*J� K��$�,�,�Q�q�1�C�� � �B���4L�L���k�>�+A�+A�!�A�+F�G�H� ���E�>�#9�#9�#;�<�� ��H�T�N�D�1� 2��$�,�,�Q�0� � ����A��Y�� ���A��Y�� �Q��� �K�-�V�[�[��_�W�4�� � �B���8O� �r c�b�|dk\r||ksJd��d|dzz |dzzd|dzz |dzzz }|S)z� Takes a total travel time T and the current time t satisfying 0 <= t <= T and returns the path parameter s corresponding to a motion that begins and ends at zero velocity. Example: CubicTimeScaling(10, 7) >> 0.78399 r� Invalid tg@rg@r��r3�t�ss r �CubicTimeScalingr�&sM�� ��6�a�1�f�)�k�)� � �Q��T��Q��T��b�!�Q�$�i�!�Q�$�/�/�A� �Hr c��|dk\r||ksJd��d|dzz |dzzd|dzz |dzzzd|dzz |dzzz}|S) a  Takes a total travel time T and the current time t satisfying 0 <= t <= T and returns the path parameter s corresponding to a motion that begins and ends at zero velocity and zero acceleration. Example: QuinticTimeScaling(10,7) >> 0.83692 rr�g$@rg.�r`g@�r�r�s r �QuinticTimeScalingr�5sc�� ��6�a�1�f�)�k�)� � �a��d��a��d��t�Q��T�{�Q��T�2�2�b�!�Q�$�i�!�Q�$�5G�G�A� �Hr c�R�t|�t|�k(sJd��|dk\sJd��t|t�sJd��|dk(s |dk(sJd��t|�}|dk(r`t d|�D]O}|t |�z|dz z }t ||�}t|�d|z zt|�|zz} t|| f�}�Q|S|dk(r`t d|�D]O}|t |�z|dz z }t||�}t|�d|z zt|�|zz} t|| f�}�Q|Sy ) a� Takes initial joint positions (n-dim) thetas_start, final joint positions thetas_end, the time of the motion T in seconds, the number of points N >= 2 in the discrete representation of the trajectory, and the time-scaling method (cubic or quintic) and returns a trajectory as a matrix with N rows, where each row is an n-vector of joint positions at an instant in time. The trajectory is a straight-line motion in joint space. Example: thetas_start = [0.1]*6 thetas_end = [pi/2]*6 T = 2 N = 5 JointTrajectory(thetas_start, thetas_end, T, N, 'cubic') >> array([[ 0.1 , 0.1 , 0.1 , 0.1 , 0.1 , 0.1 ], [ 0.33 , 0.33 , 0.33 , 0.33 , 0.33 , 0.33 ], [ 0.835, 0.835, 0.835, 0.835, 0.835, 0.835], [ 1.341, 1.341, 1.341, 1.341, 1.341, 1.341], [ 1.571, 1.571, 1.571, 1.571, 1.571, 1.571]]) zIncompatible thetasr�N must be >= 2�N must be an integer�cubic�quintic�&Incorrect time-scaling method argumentrN) r#� isinstance�intr1r��floatr�rZr�) � thetas_start� thetas_endr3r��method� trajectoryr+r�r��thetas_ss r �JointTrajectoryr�EsY��* �|� ��J�� /�F�1F�F� /� ��6�#�#�#�6� �a�� �5�5�5� � �W� ��)� 3�]�5]�]� 3���&�J� ����q���A��%��(� �A�a�C� �A� ��1�%�A��|�,�a��c�2�W�Z�5H��5J�J�H���X� 6�7�J� � �� �9� ��q���A��%��(� �A�a�C� �A�"�1�Q�'�A��|�,�a��c�2�W�Z�5H��5J�J�H���X� 6�7�J� � �� r c ��t|�\}}t|�\}}|dk\sJd��t|t�sJd��|dk(s |dk(sJd��|} |dk(r~td|�D]m} | t |�z|dz z } t || �} |j ttt|�j |��| z��} t| | f�} �o| S|dk(r~td|�D]m} | t |�z|dz z } t|| �} |j ttt|�j |��| z��} t| | f�} �o| Sy) a� Similar to JointTrajectory, except that it takes the initial end-effector configuration X_start in SE(3), the final configuration X_end, and returns the trajectory as a 4N x 4 matrix in which every 4 rows is an element of SE(3) separated in time by T/(N-1). This represents a discretized trajectory of the screw motion from X_start to X_end. Example: thetas_start = [0.1]*6 thetas_end = [pi/2]*6 M_ur5 = [[1,0,0,-.817],[0,0,-1,-.191],[0,1,0,-.006],[0,0,0,1]] S1_ur5 = [0,0,1,0,0,0] S2_ur5 = [0,-1,0,.089,0,0] S3_ur5 = [0,-1,0,.089,0,.425] S4_ur5 = [0,-1,0,.089,0,.817] S5_ur5 = [0,0,-1,.109,-.817,0] S6_ur5 = [0,-1,0,-.006,0,.817] Slist_ur5 = [S1_ur5,S2_ur5,S3_ur5,S4_ur5,S5_ur5,S6_ur5] X_start = FKinFixed(M_ur5, Slist_ur5, thetas_start) X_end = FKinFixed(M_ur5, Slist_ur5, thetas_end) T = 2 N = 3 ScrewTrajectory(X_start, X_end, T, N, 'quintic') >> array([[ 0.922, -0.388, 0.004, -0.764], [-0.007, -0.029, -1. , -0.268], [ 0.388, 0.921, -0.03 , -0.124], [ 0. , 0. , 0. , 1. ], [ 0.534, -0.806, 0.253, -0.275], [ 0.681, 0.234, -0.694, -0.067], [ 0.5 , 0.543, 0.674, -0.192], [ 0. , 0. , 0. , 1. ], [ 0. , -1. , -0. , 0.109], [ 1. , 0. , 0. , 0.297], [-0. , -0. , 1. , -0.254], [ 0. , 0. , 0. , 1. ]]) rr�r�r�r�r�rN) rer�r�r�r�r�r2r�r�rhrZr�)�X_start�X_endr3r�r��R_start�p_start�R_end�p_endr�r+r�r��X_ss r �ScrewTrajectoryr�rsm��J!��)��G�W��U�#�L�E�5� ��6�#�#�#�6� �a�� �5�5�5� � �W� ��)� 3�]�5]�]� 3��J� ����q���A��%��(� �A�a�C� �A� ��1�%�A��+�+�j��H�W�4E�4I�4I�%�4P�)Q�RS�)S�T�U�C���S� 1�2�J� � �� �9� ��q���A��%��(� �A�a�C� �A�"�1�Q�'�A��+�+�j��H�W�4E�4I�4I�%�4P�)Q�RS�)S�T�U�C���S� 1�2�J� � �� r c ��t|�\}}t|�\}}|dk\sJd��t|t�sJd��|dk(s |dk(sJd��|} |dk(r�td|�D]�} | t |�z|dz z } t || �} d| z |z| |zz} |j ttt|�j |��| z��}t|| �}t| |f�} ��| S|dk(r�td|�D]�} | t |�z|dz z } t|| �} d| z |z| |zz} |j ttt|�j |��| z��}t|| �}t| |f�} ��| Sy) ar Similar to ScrewTrajectory, except the origin of the end-effector frame follows a straight line, decoupled from the rotational motion. Example: thetas_start = [0.1]*6 thetas_end = [pi/2]*6 M_ur5 = [[1,0,0,-.817],[0,0,-1,-.191],[0,1,0,-.006],[0,0,0,1]] S1_ur5 = [0,0,1,0,0,0] S2_ur5 = [0,-1,0,.089,0,0] S3_ur5 = [0,-1,0,.089,0,.425] S4_ur5 = [0,-1,0,.089,0,.817] S5_ur5 = [0,0,-1,.109,-.817,0] S6_ur5 = [0,-1,0,-.006,0,.817] Slist_ur5 = [S1_ur5,S2_ur5,S3_ur5,S4_ur5,S5_ur5,S6_ur5] X_start = FKinFixed(M_ur5, Slist_ur5, thetas_start) X_end = FKinFixed(M_ur5, Slist_ur5, thetas_end) T = 2 N = 3 CartesianTrajectory(X_start, X_end, T, N, 'quintic') >> array([[ 0.922, -0.388, 0.004, -0.764], [-0.007, -0.029, -1. , -0.268], [ 0.388, 0.921, -0.03 , -0.124], [ 0. , 0. , 0. , 1. ], [ 0.534, -0.806, 0.253, -0.327], [ 0.681, 0.234, -0.694, 0.014], [ 0.5 , 0.543, 0.674, -0.189], [ 0. , 0. , 0. , 1. ], [ 0. , -1. , -0. , 0.109], [ 1. , 0. , 0. , 0.297], [-0. , -0. , 1. , -0.254], [ 0. , 0. , 0. , 1. ]]) Notice the R of every T is same for ScrewTrajectory and CartesianTrajectory, but the translations are different. rr�r�r�r�r�rN) rer�r�r�r�r�r2rOrWr:r]rZr�)r�r�r3r�r�r�r�r�r�r�r+r�r��p_s�R_sr�s r �CartesianTrajectoryr��s���L!��)��G�W��U�#�L�E�5� ��6�#�#�#�6� �a�� �5�5�5� � �W� ��)� 3�]�5]�]� 3��J� ����q���A��%��(� �A�a�C� �A� ��1�%�A��Q�3��-�!�E�'�)�C��+�+�j��F�7�O�4G�4G��4N�)O�PQ�)Q�R�S�C��C��%�C���S� 1�2�J� ��� �9� ��q���A��%��(� �A�a�C� �A�"�1�Q�'�A��Q�3��-�!�E�'�)�C��+�+�j��F�7�O�4G�4G��4N�)O�PQ�)Q�R�S�C��C��%�C���S� 1�2�J� ��� r c�B�t|�t|�cxk(r dk(sJd��Jd��d|_|dd�dd�f}|dd�dd�f}t|�}t|�}t||f�}tt d�|f�}t ||f�}|j |�S�NrjzNeeds two 6 vectorsrlrrC)r#rGrArZrr[r2� �V1�V2�w1�v1�w1_mat�v1_mat�col1�col2�mat1s r � LieBracketr��s��� �r�7�C��G� �Q� �5� 5�5� �5� 5�5� ��B�H� �B�Q�B�q�D��B� �A�B�q�D��B� �b�\�F� �b�\�F� �6�6�"� #�D� �5��<��(� )�D� �4��,� �D� �8�8�B�<�r c�V�t|�t|�cxk(r dk(sJd��Jd��d|_|dd�dd�f}|dd�dd�f}t|�}t|�}t||f�}tt d�|f�}t ||f�}|j j|�Sr�)r#rGrArZrr[r3r2r�s r � TruthBracketr�s��� �r�7�C��G� �Q� �5� 5�5� �5� 5�5� ��B�H� �B�Q�B�q�D��B� �A�B�q�D��B� �b�\�F� �b�\�F� �6�6�"� #�D� �5��<��(� )�D� �4��,� �D� �F�F�<�<�� �r c ���t|�t|�cxk(rt|�k(sJd��Jd��t|�}t|�}t|�}dg|z} dg|z} dg|z} dg|z} dg|z} dg|z}dg|z}|d| d<td|�D]}| |dz j||�| |<�!t|�D].}t t | |��j||�| |<�0t|�D]+}||jt | |||z��| |<�-td�}|| d<tgd�|z� }|| d<t|�jdd�}td|�D]�}t | |�}t |�j| |dz �| |||zjdd�z| |<t |�j| |dz �t| || |�||zz| |||zz| |<��t|dz dd�D]�}t | |�}t |�jj|�||j| |�jdd�zt| |||j| |��z ||<||jj| |�||<��t|�j�S)a� M1 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,.089159,1.])).T M2 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[.28,.13585,.089159,1.])).T M3 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[.675,.01615,.089159,1.])).T M4 = array(([-1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,-1.,0.],[.81725,.01615,.089159,1.])).T M5 = array(([-1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,-1.,0.],[.81725,.10915,.089159,1.])).T M6 = array(([-1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,-1.,0.],[.81725,.10915,-.005491,1.])).T M01 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,.089159,1.])).T M12 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[.28,.13585,0.,1.])).T M23 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,-.1197,.395,1])).T M34 = array(([0.,0.,-1.,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],[0.,0.,.14225,1.])).T M45 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,.093,0.,1.])).T M56 = array(([1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,.09465,1.])).T G1 = array(([.010267,0.,0.,0.,0.,0.],[0.,.010267,0.,0.,0.,0.],[0.,0.,.00666,0.,0.,0.],[0.,0.,0.,3.7,0.,0.],[0.,0.,0.,0.,3.7,0.],[0.,0.,0.,0.,0.,3.7])) G2 = array(([.22689,0.,0.,0.,0.,0.],[0.,.22689,0.,0.,0.,0.],[0.,0.,.0151074,0.,0.,0.],[0.,0.,0.,8.393,0.,0.],[0.,0.,0.,0.,8.393,0.],[0.,0.,0.,0.,0.,8.393])) G3 = array(([.0494433,0.,0.,0.,0.,0.],[0.,.0494433,0.,0.,0.,0.],[0.,0.,.004095,0.,0.,0.],[0.,0.,0.,2.275,0.,0.],[0.,0.,0.,0.,2.275,0.],[0.,0.,0.,0.,0.,2.275])) G4 = array(([.111172,0.,0.,0.,0.,0.],[0.,.111172,0.,0.,0.,0.],[0.,0.,.21942,0.,0.,0.],[0.,0.,0.,1.219,0.,0.],[0.,0.,0.,0.,1.219,0.],[0.,0.,0.,0.,0.,1.219])) G5 = array(([.111172,0.,0.,0.,0.,0.],[0.,.111172,0.,0.,0.,0.],[0.,0.,.21942,0.,0.,0.],[0.,0.,0.,1.219,0.,0.],[0.,0.,0.,0.,1.219,0.],[0.,0.,0.,0.,0.,1.219])) G6 = array(([.0171364,0.,0.,0.,0.,0.],[0.,.0171364,0.,0.,0.,0.],[0.,0.,.033822,.0,0.,0.],[0.,0.,0.,.1879,0.,0.],[0.,0.,0.,0.,.1879,0.],[0.,0.,0.,0.,0.,.1879])) Slist = [[0.,0.,1.,0.,0.,0.],[0.,1.,0.,-.089,0.,0.],[0.,1.,0.,-.089,0.,.425],[0.,1.,0.,-.089,0.,.817],[0.,0.,-1.,-.109,.817,.0],[0.,1.,0.,.006,0.,.817]] Glist = [G1,G2,G3,G4,G5,G6] M_rels = [M01,M12,M23,M34,M45,M56] Ftip = [0.,0.,0.,0.,0.,0.] zJoint inputs mismatchrrrl)rrrrjrb) r#r1r�r2rwrhr�rr�r�r3r�r=)�thetas� thetadots� thetadotdots�g�Ftip�M_rels�Glistr�r��Mlist�T_rels�Vlist�Vdotlist�Alist�Flist�Taulistr+�V0�Vdot0�F_ip1�T_i_im1�T_ip1_is r �InverseDynamicsrsI��8 �v�;�#�i�.� =�C� �,=� =�V�?V�V� =�V�?V�V� =� �E�N�E� �F� �A� �4�=�D��C��E�E��S��U�F� �C��E�E��s�1�u�H� �C��E�E� �C��E�E��c�!�e�G��a�y�E�!�H� �1�a�[����1��:�>�>�&��)�,��a����1�X���8�E�!�H�-�.�2�2�5��8�<��a����1�X���1�I�M�M�*�U�1�X�f�Q�i�-?�"@�A��q� �� �u��B��E�!�H� �W�Q�Y� � �E��H�Q�K� �D�M� !� !�!�A� &�E��1�a�[���6�!�9�%���7�#�'�'��a��c� �3�u�Q�x� �!� �7L�6U�6U�VW�XY�6Z�Z��a���g�&�*�*�8�A�a�C�=�9�J�u�Q�x�QV�WX�QY�<Z�[d�ef�[g�<g�g�jo�pq�jr�s�AB�tC�kC�C��� � ��1�Q�3�r�"� ���6�!�9�%���G�$�&�&�+�+�E�2�e�A�h�l�l�8�A�;�6O�5X�5X�YZ�[\�5]�]�`l�mr�st�mu�w|�}~�w�xD�xD�EJ�KL�EM�xN�aO�O��a���A�h�j�j�%�%�e�A�h�/��� �� �7� � #� #� %�%r c ���t|�}t|�}t||f�}t|�D]2}dg|z}d||<t |dgdz|dgdzdgdz|||�|dd�|f<�4|S)Nrrrjr)r1r#rr�r)r�r�r�r�r�r)r+r�s r � InertiaMatrixrns~�� �E�N�E� �F� �A� �q��e� �A� �1�X���s�1�u� �� �Q��!�&�1�#�a�%���s�1�u�q�c�!�e�V�UZ�\a�b��!�Q�$��� �Hr c �B�t||dgdzdgdzdgdz|||�}|S�Nrrjr�r)r�r�r�r�r��Cs r �CoriolisForcesr|s1���� �A�3�q�5�1�#�a�%�!��Q����u�U�A� �Hr c �B�t|dgdzdgdz|dgdz|||�}|S)Nrrjr)r�r�r�r�r��Gvecs r � GravityForcesr �s1�� �6�A�3�q�5�1�#�a�%��Q�C��E�6�5�%� P�D� �Kr c �>�t|dgdzdgdzdgdz||||�Srr)r�r�r�r�r�s r �EndEffectorForcesr �s-�� �6�A�3�q�5�1�#�a�%�!��Q���f�e�U� S�Sr c���t||||�}t|||||�} t|||||�} t|||||�} tj |�j || z | z | z �} | S�N)rrr r rr�r2) r�r��tausr�r�r�r�r�r)rr �eeFr�s r �ForwardDynamicsr�ss���f�f�e�U�3�A��v�y�&�%��?�A� ���F�E�5� 9�D� �D�&�&�%�� ?�C��K�K��N�'�'��q��4��#�(=�>�L� �r c�l�t|�}t|�}t|�}|||zz}|||zz}||fSr)r1)�thetas_t� thetadots_t�thetadotdots_t�delt� thetas_next�thetadots_nexts r � EulerStepr�sI���x� �H��+�&�K��^�,�N��T�+�-�-�K� �4��#6�6�N� �� &�&r c ��t|�}g} t|�D]1} t|| || || ||| |||�} | j| ��3t | �Sr)r#r�rr&r1) � thetas_traj�thetadots_traj�thetadotdots_traj� Ftip_trajr�r�r�r��Np1�idtrajr+rs r �InverseDynamicsTrajectoryr!�sj�� �k� �C� �F� �3�Z���{�1�~�~�a�/@�BS�TU�BV�XY�[d�ef�[g�io�qv�x}�~��� � �d��� �6�?�r c �j�t|�} t|�} t|�} t| �D]I} t|||| ||| |||�} t ||| |�\}}t | |f�} t | |f�} |}|}�K| j | dzt|��| j | dzt|��fS)Nr)r#r1r�rrr[r�)� thetas_init�thetadots_init�tau_histrr�rr�r�r�rrrr+�thetadotdots_initrrs r �ForwardDynamicsTrajectoryr'�s��� �h�-�C��+�&�K��^�,�N� �3�Z��+�K���RS��VW�Yb�cd�Ye�gm�ot�v{�|��&/� �^�M^�`d�&e�#� �^��k�;�7�8� ���� @�A��!� �'��� � � �s�1�u�c�+�&6� 7��9O�9O�PS�TU�PU�WZ�[f�Wg�9h� h�hr )r�)-r�numpyr rr r/r7r:rArHrLrOrWr]rerhrorqrwr�r�r�r�r�r�r�r�r�r�r�r�r�r�r�r�r�rrrr r rrr!r'r�r r �<module>r)s��� ��(��0��0M��"�( �$�* �.!�H �0�<�&�0 �<�6  �F%�P" �J.�b&�R �P �D �D;�|:�D  �  � *�Z;�|@�P � �U&�p  � � � T��'��ir

MCP directory API

We provide all the information about MCP servers via our MCP API.

curl -X GET 'https://glama.ai/api/mcp/v1/servers/nonead/nUR-MCP-SERVER'

If you have feedback or need assistance with the MCP directory API, please join our Discord server