4627.sagews

############## Run this only once per worksheet:
##### load 'https://gitorious.org/sagerobotics/sagerobotics/blobs/raw/master/download_sagerobotics.sage'

The module seems to be successfully downloaded. You can import it calling ' import sagerobotics '. If you want to delete the download files call ' autodownload_remove_downloaded_files() '.

import sagerobotics

planar2 = sagerobotics.Robot(2,'2-link Planar Robot','planar2')

planar2.links_dh = [ [ 0.0, 0.2, 0.0, planar2.q[0,0] ],    # link 1
                     [ 0.0, 0.3, 0.0, planar2.q[1,0] ] ]     # link 2

# dynamic parameters vector
planar2.Parms()

\newcommand{\Bold}[1]{\mathbf{#1}}\left($$\begin{array}{r} \hat{I}_{1,xx} \\ \hat{I}_{1,xy} \\ \hat{I}_{1,xz} \\ \hat{I}_{1,yy} \\ \hat{I}_{1,yz} \\ \hat{I}_{1,zz} \\ \widehat{m_1l_{1,x}} \\ \widehat{m_1l_{1,y}} \\ \widehat{m_1l_{1,z}} \\ m_{1} \\ {f_v}_{1} \\ {f_c}_{1} \\ \hat{I}_{2,xx} \\ \hat{I}_{2,xy} \\ \hat{I}_{2,xz} \\ \hat{I}_{2,yy} \\ \hat{I}_{2,yz} \\ \hat{I}_{2,zz} \\ \widehat{m_2l_{2,x}} \\ \widehat{m_2l_{2,y}} \\ \widehat{m_2l_{2,z}} \\ m_{2} \\ {f_v}_{2} \\ {f_c}_{2} \end{array}$$\right)

planar2.gen_geometric_model()
planar2.gen_kinematic_model()

\newcommand{\Bold}[1]{\mathbf{#1}}\verb|Robot|\phantom{x}\verb|instance:|\phantom{x}\verb|2-link|\phantom{x}\verb|Planar|\phantom{x}\verb|Robot|

# link 1 transformation matrix (link frame at robot base)
planar2.Ti[1]

\newcommand{\Bold}[1]{\mathbf{#1}}\left($$\begin{array}{rrrr} \cos\left(q_{1}\right) & -\sin\left(q_{1}\right) & 0 & 0.200000000000000 \, \cos\left(q_{1}\right) \\ \sin\left(q_{1}\right) & \cos\left(q_{1}\right) & 0 & 0.200000000000000 \, \sin\left(q_{1}\right) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}$$\right)

# link 2 transformation matrix (link frame at robot base)
planar2.Ti[2]

\newcommand{\Bold}[1]{\mathbf{#1}}\left($$\begin{array}{rrrr} -\sin\left(q_{1}\right) \sin\left(q_{2}\right) + \cos\left(q_{1}\right) \cos\left(q_{2}\right) & -\sin\left(q_{1}\right) \cos\left(q_{2}\right) - \sin\left(q_{2}\right) \cos\left(q_{1}\right) & 0 & -0.300000000000000 \, \sin\left(q_{1}\right) \sin\left(q_{2}\right) + 0.300000000000000 \, \cos\left(q_{1}\right) \cos\left(q_{2}\right) + 0.200000000000000 \, \cos\left(q_{1}\right) \\ \sin\left(q_{1}\right) \cos\left(q_{2}\right) + \sin\left(q_{2}\right) \cos\left(q_{1}\right) & -\sin\left(q_{1}\right) \sin\left(q_{2}\right) + \cos\left(q_{1}\right) \cos\left(q_{2}\right) & 0 & 0.300000000000000 \, \sin\left(q_{1}\right) \cos\left(q_{2}\right) + 0.300000000000000 \, \sin\left(q_{2}\right) \cos\left(q_{1}\right) + 0.200000000000000 \, \sin\left(q_{1}\right) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}$$\right)

# linear part of Jacobinan at link 2 frame
planar2.Jpi[2]

\newcommand{\Bold}[1]{\mathbf{#1}}\left($$\begin{array}{rr} -0.300000000000000 \, \sin\left(q_{1}\right) \cos\left(q_{2}\right) - 0.300000000000000 \, \sin\left(q_{2}\right) \cos\left(q_{1}\right) - 0.200000000000000 \, \sin\left(q_{1}\right) & -0.300000000000000 \, \sin\left(q_{1}\right) \cos\left(q_{2}\right) - 0.300000000000000 \, \sin\left(q_{2}\right) \cos\left(q_{1}\right) \\ -0.300000000000000 \, \sin\left(q_{1}\right) \sin\left(q_{2}\right) + 0.300000000000000 \, \cos\left(q_{1}\right) \cos\left(q_{2}\right) + 0.200000000000000 \, \cos\left(q_{1}\right) & -0.300000000000000 \, \sin\left(q_{1}\right) \sin\left(q_{2}\right) + 0.300000000000000 \, \cos\left(q_{1}\right) \cos\left(q_{2}\right) \\ 0 & 0 \end{array}$$\right)

# full Jacobinan at last link frame (link 2)
planar2.J()

\newcommand{\Bold}[1]{\mathbf{#1}}\left($$\begin{array}{rr} -0.300000000000000 \, \sin\left(q_{1}\right) \cos\left(q_{2}\right) - 0.300000000000000 \, \sin\left(q_{2}\right) \cos\left(q_{1}\right) - 0.200000000000000 \, \sin\left(q_{1}\right) & -0.300000000000000 \, \sin\left(q_{1}\right) \cos\left(q_{2}\right) - 0.300000000000000 \, \sin\left(q_{2}\right) \cos\left(q_{1}\right) \\ -0.300000000000000 \, \sin\left(q_{1}\right) \sin\left(q_{2}\right) + 0.300000000000000 \, \cos\left(q_{1}\right) \cos\left(q_{2}\right) + 0.200000000000000 \, \cos\left(q_{1}\right) & -0.300000000000000 \, \sin\left(q_{1}\right) \sin\left(q_{2}\right) + 0.300000000000000 \, \cos\left(q_{1}\right) \cos\left(q_{2}\right) \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{array}$$\right)

# tau from EL
sagerobotics.dyn_EL.gen_tau_EL( planar2 ).simplify_trig()

\newcommand{\Bold}[1]{\mathbf{#1}}\left($$\begin{array}{r} {\left(\ddot{q}_1 + \ddot{q}_2\right)} \hat{I}_{2,zz} + \frac{3}{5} \, {\left(\ddot{q}_1 + \ddot{q}_2\right)} \widehat{m_2l_{2,x}} + \frac{1}{100} \, {\left(13 \, \ddot{q}_1 + 9 \, \ddot{q}_2\right)} m_{2} - \frac{1}{50} \, {\left(10 \, {\left(2 \, \ddot{q}_1 + \ddot{q}_2\right)} \widehat{m_2l_{2,y}} + 3 \, {\left(2 \, \dot{q}_1 \dot{q}_2 + \dot{q}_2^{2}\right)} m_{2} + 10 \, {\left(2 \, \dot{q}_1 \dot{q}_2 + \dot{q}_2^{2}\right)} \widehat{m_2l_{2,x}}\right)} \sin\left(q_{2}\right) + \frac{1}{50} \, {\left(3 \, {\left(2 \, \ddot{q}_1 + \ddot{q}_2\right)} m_{2} + 10 \, {\left(2 \, \ddot{q}_1 + \ddot{q}_2\right)} \widehat{m_2l_{2,x}} - 10 \, {\left(2 \, \dot{q}_1 \dot{q}_2 + \dot{q}_2^{2}\right)} \widehat{m_2l_{2,y}}\right)} \cos\left(q_{2}\right) + \hat{I}_{1,zz} \ddot{q}_1 + \frac{1}{25} \, \ddot{q}_1 m_{1} + \frac{2}{5} \, \ddot{q}_1 \widehat{m_1l_{1,x}} \\ {\left(\ddot{q}_1 + \ddot{q}_2\right)} \hat{I}_{2,zz} + \frac{9}{100} \, {\left(\ddot{q}_1 + \ddot{q}_2\right)} m_{2} + \frac{3}{5} \, {\left(\ddot{q}_1 + \ddot{q}_2\right)} \widehat{m_2l_{2,x}} + \frac{1}{50} \, {\left(10 \, \dot{q}_1^{2} \widehat{m_2l_{2,y}} + 3 \, \ddot{q}_1 m_{2} + 10 \, \ddot{q}_1 \widehat{m_2l_{2,x}}\right)} \cos\left(q_{2}\right) + \frac{1}{50} \, {\left(3 \, \dot{q}_1^{2} m_{2} + 10 \, \dot{q}_1^{2} \widehat{m_2l_{2,x}} - 10 \, \ddot{q}_1 \widehat{m_2l_{2,y}}\right)} \sin\left(q_{2}\right) \end{array}$$\right)

# tau, in intermediate variables form, from RNE
tau_aux, tau_exp = sagerobotics.dyn_RNE.gen_tau_RNE( True, planar2 )

tau_exp

\newcommand{\Bold}[1]{\mathbf{#1}}\left($$\begin{array}{r} \mathbf{a}_{16} + 0.200000000000000 \, \mathbf{a}_{18} \\ \mathbf{a}_{10} + 0.300000000000000 \, \mathbf{a}_{12} \end{array}$$\right)

html.table(tau_aux)

\mathbf{a}_{0}	0.200000000000000 \, \dot{q}_1
\mathbf{a}_{1}	0.200000000000000 \, \ddot{q}_1
\mathbf{a}_{2}	\dot{q}_1 + \dot{q}_2
\mathbf{a}_{3}	\mathbf{a}_{0} \sin\left(q_{2}\right)
\mathbf{a}_{4}	\mathbf{a}_{0} \cos\left(q_{2}\right) + 0.300000000000000 \, \dot{q}_1 + 0.300000000000000 \, \dot{q}_2
\mathbf{a}_{5}	\ddot{q}_1 + \ddot{q}_2
\mathbf{a}_{6}	{\left(\mathbf{a}_{0} \cos\left(q_{2}\right) + 0.300000000000000 \, \dot{q}_1\right)} \dot{q}_2 + \mathbf{a}_{1} \sin\left(q_{2}\right) - 0.300000000000000 \, \dot{q}_1 \dot{q}_2
\mathbf{a}_{7}	-\mathbf{a}_{0} \dot{q}_2 \sin\left(q_{2}\right) + \mathbf{a}_{1} \cos\left(q_{2}\right) + 0.300000000000000 \, \ddot{q}_1 + 0.300000000000000 \, \ddot{q}_2
\mathbf{a}_{8}	-{\left(\hat{I}_{2,yz} \mathbf{a}_{2} + \mathbf{a}_{3} \widehat{m_2l_{2,z}}\right)} \mathbf{a}_{2} + \hat{I}_{2,xz} \mathbf{a}_{5} - \mathbf{a}_{7} \widehat{m_2l_{2,z}} + 9.81 \, \widehat{m_2l_{2,y}}
\mathbf{a}_{9}	{\left(\hat{I}_{2,xz} \mathbf{a}_{2} - \mathbf{a}_{4} \widehat{m_2l_{2,z}}\right)} \mathbf{a}_{2} + \hat{I}_{2,yz} \mathbf{a}_{5} + \mathbf{a}_{6} \widehat{m_2l_{2,z}} - 9.81 \, \widehat{m_2l_{2,x}}
\mathbf{a}_{10}	{\left(\mathbf{a}_{2} \widehat{m_2l_{2,y}} - \mathbf{a}_{3} m_{2}\right)} \mathbf{a}_{4} + {\left(\mathbf{a}_{2} \widehat{m_2l_{2,x}} + \mathbf{a}_{4} m_{2}\right)} \mathbf{a}_{3} + \hat{I}_{2,zz} \mathbf{a}_{5} - \mathbf{a}_{6} \widehat{m_2l_{2,y}} + \mathbf{a}_{7} \widehat{m_2l_{2,x}}
\mathbf{a}_{11}	-{\left(\mathbf{a}_{2} \widehat{m_2l_{2,x}} + \mathbf{a}_{4} m_{2}\right)} \mathbf{a}_{2} - \mathbf{a}_{5} \widehat{m_2l_{2,y}} + \mathbf{a}_{6} m_{2}
\mathbf{a}_{12}	-{\left(\mathbf{a}_{2} \widehat{m_2l_{2,y}} - \mathbf{a}_{3} m_{2}\right)} \mathbf{a}_{2} + \mathbf{a}_{5} \widehat{m_2l_{2,x}} + \mathbf{a}_{7} m_{2}
\mathbf{a}_{13}	9.81 \, m_{2}
\mathbf{a}_{14}	-\hat{I}_{1,yz} \dot{q}_1^{2} + \hat{I}_{1,xz} \ddot{q}_1 - \mathbf{a}_{1} \widehat{m_1l_{1,z}} + 0.300000000000000 \, \mathbf{a}_{13} \sin\left(q_{2}\right) + \mathbf{a}_{8} \cos\left(q_{2}\right) - \mathbf{a}_{9} \sin\left(q_{2}\right) + 9.81 \, \widehat{m_1l_{1,y}}
\mathbf{a}_{15}	{\left(\hat{I}_{1,xz} \dot{q}_1 - \mathbf{a}_{0} \widehat{m_1l_{1,z}}\right)} \dot{q}_1 + \hat{I}_{1,yz} \ddot{q}_1 - 0.300000000000000 \, \mathbf{a}_{13} \cos\left(q_{2}\right) + \mathbf{a}_{8} \sin\left(q_{2}\right) + \mathbf{a}_{9} \cos\left(q_{2}\right) - 9.81 \, \widehat{m_1l_{1,x}}
\mathbf{a}_{16}	\mathbf{a}_{0} \dot{q}_1 \widehat{m_1l_{1,y}} + \hat{I}_{1,zz} \ddot{q}_1 + \mathbf{a}_{1} \widehat{m_1l_{1,x}} + \mathbf{a}_{10} + 0.300000000000000 \, \mathbf{a}_{12}
\mathbf{a}_{17}	-{\left(\mathbf{a}_{0} m_{1} + \dot{q}_1 \widehat{m_1l_{1,x}}\right)} \dot{q}_1 + \mathbf{a}_{11} \cos\left(q_{2}\right) - \mathbf{a}_{12} \sin\left(q_{2}\right) - \ddot{q}_1 \widehat{m_1l_{1,y}}
\mathbf{a}_{18}	-\dot{q}_1^{2} \widehat{m_1l_{1,y}} + \mathbf{a}_{1} m_{1} + \mathbf{a}_{11} \sin\left(q_{2}\right) + \mathbf{a}_{12} \cos\left(q_{2}\right) + \ddot{q}_1 \widehat{m_1l_{1,x}}
\mathbf{a}_{19}	\mathbf{a}_{13} + 9.81 \, m_{1}

c_code = sagerobotics.code_dyn.dyn_matrix_to_func( 'c', tau_aux, tau_exp, 'inverse_dynamics', 2, planar2.dof, planar2.Parms(False,False).list() )

print c_code

void inverse_dynamics( double* inverse_dynamics_out, const double* parms, const double* q, const double* dq, const double* ddq ) { double aux0 = 0.2*dq[0]; double aux1 = 0.2*ddq[0]; double aux2 = dq[0] + dq[1]; double aux3 = sin(q[1]); double aux4 = aux0*aux3; double aux5 = cos(q[1]); double aux6 = aux0*aux5 + 0.3*dq[0]; double aux7 = aux6 + 0.3*dq[1]; double aux8 = ddq[0] + ddq[1]; double aux9 = aux1*aux3 + aux6*dq[1] - 0.3*dq[0]*dq[1]; double aux10 = aux1*aux5 - aux4*dq[1] + 0.3*ddq[0] + 0.3*ddq[1]; double aux11 = aux2*parms[17] - aux4*parms[19]; double aux12 = aux2*parms[16] + aux7*parms[19]; double aux13 = aux10*parms[19] - aux11*aux2 + aux8*parms[16]; double aux14 = parms[15]*aux8 + aux10*parms[16] + aux11*aux7 + aux12*aux4 + 0.3*aux13 - aux9*parms[17]; inverse_dynamics_out[0] = parms[5]*ddq[0] + aux0*dq[0]*parms[7] + 0.2*aux1*parms[9] + 2*aux1*parms[6] + 0.2*aux13*aux5 + aux14 + 0.2*aux3*(-aux12*aux2 - aux8*parms[17] + aux9*parms[19]) - 0.2*pow(dq[0], 2)*parms[7]; inverse_dynamics_out[1] = aux14; }