3912.sagews

### Calculation of the finger friction forces based on wrenches
##################################
### Fingerkinematics using twists
##################################
# Axis conventions:
# X points distal
# Z rotation is flexion

# y points palmar

#######################
# Set to true if test case
#######################
test=bool
body=bool
test=0
# Laengen
l_dist=var('l_d')
l_med=var('l_m')
l_prox=var('l_p')
l_mc=var('l_mc')

# Pulley diameter vector
d=vector([var('d_MC'),d_MC,var('d_PIP'),var('d_PIP')])
dinv=vector([1/d_MC,1/d_MC,1/d_PIP,1/d_PIP])

# Constant to convert from degree to rad
deg=pi/180
# Journal
# Kinematics checked. exponential version works as well as the rodriguez version 

#Unit Matrix
IDENT=Matrix([
        [1,0,0],
        [0,1,0],
        [0,0,1]
        ])

# Flow
# Create object for every joint holding exp(xi)
# To create the Adjuncts we need R and P of every single joint which is calculated using exp(x joint it i) of ALL previous. This can not be hold within the joint object without the joint nowing which one it is.
# Solutions: 
# 1.store all omegas and p in a matrix
# 2. Use a dictionary and iteritem
# 3. Calculate GSTs outside in an extra Object 
omega=vector([var('_o1'),var('_o2'),var('_o3')])
p=vector([var('_p1'),var('_p2'),var('_p3')])
tcp_null=vector([0,0,0])
theta=var('theta')
   
jointnumber=var('jointnumber')     

class Joint(object): #Class to calculate all terms needed to calculate exp(xi) and all joint location independent terms
     #omega is the vector of axis, q the vector to a point on the axis. Therefore a Istance for Every joint can be created
    def __init__ (self,jointnumber,omega,q,theta,tcp_null):
        self._omega=omega #Assign Input Parameters to object
        self._q=q
        self._theta=theta
#        print "instanziiert"
        
    @staticmethod
    def Skew(_a): #Return a skew symmetric Matrix for Vector _a 
        SKEW=Matrix(
            [[0    ,-_a[2],_a[1]],
             [_a[2] ,0     ,-_a[0]],
             [-_a[1],_a[0] ,0]]
            ) # prooved
        return SKEW

    def Rodriguez(self): # Calculation of the exponential of omega suding rodriguez formula
        _OMEGA_w=self.Skew(self._omega)             # Calculate Skew symmetric matrix omega_wedge
        _exp_omega=exp(_OMEGA_w*self._theta) # Should be the same!!
        _exp_omega_r=(IDENT+
                      _OMEGA_w*sin(self._theta)+
                      _OMEGA_w^2*(1-cos(self._theta)))
        return _exp_omega_r #return the result of rodriguez formula


    def ExpXI (self):      # calculate exponential: INPUT: omega, q, theta
        # checked with example SCARA from Murray
        _omega=self._omega/norm(self._omega)          #normalize _omega    
        _v=-self._omega.cross_product(self._q)         #calculate velocity vector  omega X  q 
                                         #call rodriguez to calculate exponential of omega_wedge*theta# this is a real exponential
        _E_OW_T=self.Rodriguez() 
        _hw_o_t=(IDENT-_E_OW_T)*(self._omega.cross_product(_v)+self._omega.outer_product(self._omega)*_v*self._theta) 
        
        _EXP = Matrix([ # Create Matrix for exponential see Murray p42. THIS IS NOT A MATRIX EXPONENTIAL!!!
                [ _E_OW_T[0,0], _E_OW_T[0,1], _E_OW_T[0,2],_hw_o_t[0] ], #Syntax M[row,column]
                [ _E_OW_T[1,0], _E_OW_T[1,1], _E_OW_T[1,2],_hw_o_t[1] ],
                [ _E_OW_T[2,0], _E_OW_T[2,1], _E_OW_T[2,2],_hw_o_t[2] ],
                [0            ,0            ,0            ,1          ]
                ])    # exp(_XI_Theta is not a Matrix exponential! (See Murray p 28)
        return _EXP

# Create Object for Joint transformations
class JointTransformation(object): #Class to calculate all terms needed to calculate the Kinematics and Jacobian from Twists
     #omega is the vector of axis, p the vector to a point on the axis. Therefore a Istance for Every joint can be created
    def __init__ (self,jointnumber,omega,q,theta,tcp_null):
        self._jointnumber=jointnumber #Assign Input Parameters to object
        self._tcp_null=tcp_null
        self._omega=omega
        self._q=q
        self._theta=theta
#        print "Joint instanziiert"
#        print "using omega:"
#        print self._omega
#        print "and q:"
#        print self._q
#        print "and theta:"
#        print self._theta
#        print "#############################\n"
    def GstNullR_P (self,_tcp_null):
        _GST_NULL=Matrix([ # Create Matrix of TCP at null configuration
                [ 1, 0, 0, _tcp_null[0] ], #Syntax M[row,column]
                [ 0, 1, 0, _tcp_null[1] ],
                [ 0, 0, 1, _tcp_null[2] ],
                [ 0, 0, 0, 1            ]
                ])
        return _GST_NULL
    def CalcGstTheta(self):
        # We keep it simple since it is only 4 joints        
#Calculate product of exponentials starting from first joint. First Joint is Joint 1!
#        print 'Calculating Joint:' 
#        print self._jointnumber           
        if self._jointnumber == 1:    
            GST_THETA_result=Joint1.ExpXI()
        elif self._jointnumber == 2:    
            TEMP=Joint1.ExpXI()*Joint2.ExpXI()
            GST_THETA_result=TEMP
        elif self._jointnumber == 3:
            TEMP=Joint1.ExpXI()*Joint2.ExpXI()
            TEMP=TEMP*Joint3.ExpXI()
            GST_THETA_result=TEMP
        elif self._jointnumber == 4:
            TEMP=Joint1.ExpXI()*Joint2.ExpXI()
            TEMP=TEMP*Joint3.ExpXI()
            TEMP=TEMP*Joint4.ExpXI()
            GST_THETA_result=TEMP*self.GstNullR_P(tcp)
#            print "GST_THETA="
#            print GST_THETA_result
#            print "tcp_null= \n"
#            print tcp_null
        else:
            print  'Invalid Joint Number'
            GST_THETA_RESULT=eye(6)
        print "\n"    
        return GST_THETA_result
    def GstTheta(self): #return result for Gst om 
        GST_THETA=self.CalcGstTheta()
        GST_THETA=GST_THETA.apply_map(attrcall('trig_reduce')) #Simplify trigonometric terms
        return GST_THETA
    def RTheta(self,body):        
        if (body): 
            R_THETA=self.GstTheta_b().submatrix(row=0, col=0, nrows=3, ncols=3)
        else:
            R_THETA=self.GstTheta().submatrix(row=0, col=0, nrows=3, ncols=3)
        return R_THETA
    def PTheta(self,body):        
        if (body):
            P_THETA=self.GstTheta_b().submatrix(row=0, col=3, nrows=3, ncols=1)
        else:
            P_THETA=self.GstTheta().submatrix(row=0, col=3, nrows=3, ncols=1)
        return P_THETA

    def Xi(self): #define Function to build Vector x from omega and q) [murray p87]
        v=-self._omega.cross_product(self._q) # Vector of velocities v= -omega X q
        xi=vector([v[0],v[1],v[2],self._omega[0],self._omega[1],self._omega[2]]) #index starts at zero. It is python!
        return xi # Proofed against Murray Scara

    def Adjunct(self): # Calculate column- Vector of Spatial Jacobian
        null=var('null')
        _ptheta=self.PTheta(0).column(0) #convert Matrix to a vector for Joint.Skew
        NULL_temp=Matrix(3,3,(null,null,null, #This dirty trick circumpasses sagemath
                              null,null,null, #conversion limitations
                              null,null,null))
        RpR_up=(self.RTheta(0)).augment(Joint.Skew(_ptheta())*self.RTheta(0))
        RpR_low=NULL_temp.augment(self.RTheta(0))
        ADJUNCT=(RpR_up.stack(RpR_low)).substitute(null=0) #substitute finishes dirty trick
#        print "(Pseudo) Adjunct Matrix Calculation of joint number: omega"
#        print self._jointnumber
#        print self._omega
#        print "\n"
        return ADJUNCT

# Calculation of inverse Adjunct and TCP related Transformations
    def InvAdjunct(self): # Calculate column- Vector of Body Jacobian
        null=var('null')
        _ptheta=self.PTheta(1).column(0) #convert Matrix to a vector for Joint.Skew
        NULL_temp=Matrix(3,3,(null,null,null, #This dirty trick circumpasses sagemath
                              null,null,null, #conversion limitations
                              null,null,null))
        TRTheta=self.RTheta(1).transpose() # Transpose R_b
        RpR_up=(TRTheta).augment(-TRTheta*Joint.Skew(_ptheta()))
        RpR_low=NULL_temp.augment(TRTheta)
        ADJUNCT_inv=(RpR_up.stack(RpR_low)).substitute(null=0) #substitute finishes dirty trick
        return ADJUNCT_inv
##### Routines to calculate body Jacobian

    def CalcGstTheta_b(self):
        eye=Matrix(4,4,1)
        # We keep it simple since it is only 4 joints        
#Calculate product of exponentials starting from last joint. First Joint is Joint 1!
#        print 'Calculating Joint:' 
#        print self._jointnumber           
#        print "\n"
#        print "using omega:"
#        print self._omega
#        print "and q:"
#        print self._q
        if self._jointnumber == 4:    
            GST_THETA_result_b=Joint4.ExpXI()*self.GstNullR_P(tcp)
#            print "Joint"
#            print self._jointnumber
#            print "1 Multiplication \n"
#            print self._tcp_null,tcp_null
        elif self._jointnumber == 3:    
            TEMP=Joint3.ExpXI()*Joint4.ExpXI()
            GST_THETA_result_b=TEMP*self.GstNullR_P(tcp)
#            print "Joint"
#            print self._jointnumber
#            print "2 Multiplication \n"
        elif self._jointnumber == 2:
#            print "Joint"
#           print self._jointnumber
#            print "3 Multiplications \n"                    
            TEMP=Joint2.ExpXI()*Joint3.ExpXI()
            TEMP=TEMP*Joint4.ExpXI()
            GST_THETA_result_b=TEMP*self.GstNullR_P(tcp)         
        elif self._jointnumber == 1:
#            print "Joint"
#            print self._jointnumber
#            print "4 Multiplications \n"    
            TEMP=Joint1.ExpXI()*Joint2.ExpXI()
            TEMP=TEMP*Joint3.ExpXI()
            TEMP=TEMP*Joint4.ExpXI()
            GST_THETA_result_b=TEMP*self.GstNullR_P(tcp)
            #print "GST_THETA_b="
            #print GST_THETA_result_b
        else:
            print 'invalid Joint Number'
            GST_THETA_RESULT_b=eye(6)
        return GST_THETA_result_b
    def GstTheta_b(self): #return result for Gst om 
        GST_THETA_b=self.CalcGstTheta_b()
        GST_THETA_b=GST_THETA_b.apply_map(attrcall('trig_reduce')) #Simplify trigonometric terms
        return GST_THETA_b



############################################
# Dictionaries holding joint specific values
############################################
# Joints for Testcase (Murray p 88)
t_joint1_dict={ #Values of first axis (adduction abduction MC)
"jointnumber":1,
"omega":vector([0,0,1]),
"q":vector([0,0,0]),
"theta":var('theta_1'),
"tcp_null":vector([0,0,0])
}
t_joint2_dict={ #Values of first axis (flexion extension MC)
"jointnumber":2,
"omega":vector([0,0,1]),
"q":vector([0,var('l_1'),0]), # Distance of hyperboloid axes
"theta":var('theta_2'),
"tcp_null":vector([0,var('l_1'),0])
}
t_joint3_dict={ #Values of first axis (flexion extension PIP)
"jointnumber":3,
"omega":vector([0,0,1]),
"q":vector([0,var('l_1')+var('l_2'),0]),
"theta":var('theta_3'),
"tcp_null":vector([0,var('l_1')+var('l_2'),0])
}
t_joint4_dict={ #Values of 4th axis (flexion extension DIP)
"jointnumber":4,
"omega":vector([0,1,0]),
"q":vector([1,0,0]),
"theta":var('theta_4'),
"tcp_null":vector([0,var('l_1')+var('l_2'),0])
}

###############################
# Real world case: HASY finger
##############################
mc1_dict={ #Values of first axis (adduction abduction MC)
"jointnumber":1,
"omega":vector([0,1,0]),
"q":vector([0,0,0]),
"theta":var("theta_1"),
"tcp_null":vector([0,0,0])
}

mc2_dict={ #Values of first axis (flexion extension MC)
"jointnumber":2,
"omega":vector([0,0,1]),
"q":vector([l_mc,0,0]), # Distance of hyperboloid axes
"theta":var("theta_2"),
"tcp_null":vector([l_mc,0,0])
}

pip_dict={ #Values of first axis (flexion extension PIP)
"jointnumber":3,
"omega":vector([0,0,1]),
"q":vector([l_p+l_mc,0,0]),
"theta":var("theta_3"),
"tcp_null":vector([l_p+l_mc,0,0])
}

dip_dict={ #Values of  axis: flexion extension DIP
"jointnumber":4,
"omega":vector([0,0,1]),
"q":vector([l_m+l_p+l_mc,0,0]),
"theta":var("theta_4"),
"tcp_null":vector([l_m+l_p+l_mc,0,0])
}
tcp=vector([l_d+l_m+l_p+l_mc,0,0])
##### End Dictionaries Kinematics

#########################
# Create Joint Objects
########################
if (test): # Testcase
    print "####### Calculating Murray example for testing purpose"
    # Joint instances
    Joint1=Joint(**t_joint1_dict)
    Joint2=Joint(**t_joint2_dict)
    Joint3=Joint(**t_joint3_dict)
    Joint4=Joint(**t_joint4_dict)
#JointTransfrmation Instances
    JT1=JointTransformation(**t_joint1_dict)
    JT2=JointTransformation(**t_joint2_dict)
    JT3=JointTransformation(**t_joint3_dict)
    JT4=JointTransformation(**t_joint4_dict)
else:
    print "####### Calculating WikiHand"
    # Joint instances
    Joint1=Joint(**mc1_dict)
    Joint2=Joint(**mc2_dict)
    Joint3=Joint(**pip_dict)
    Joint4=Joint(**dip_dict)
#JointTransformation Instances
    JT1=JointTransformation(**mc1_dict)
    JT2=JointTransformation(**mc2_dict)
    JT3=JointTransformation(**pip_dict)
    JT4=JointTransformation(**dip_dict)
#    tcp_null=vector([l_med+l_prox+l_mc+l_dist,0,0])

####################
# Calculate Spatial Jacobian
####################

J=vector([var('x'),var('x'),var('x'),var('x'),var('x'),var('x')]) #create symbolic vector to trick out conversion of vectors

Xi1=JT1.Xi().column() #First Column is Xi, not Xi'
Xi_s2=(JT1.Adjunct()*JT2.Xi()).column()
Xi_s3=(JT2.Adjunct()*JT3.Xi()).column()
Xi_s4=(JT3.Adjunct()*JT4.Xi()).column()

# J=J.augment(JT4.JacColumn().column())
J=(J.column()).augment(Xi1)
J=J.augment(Xi_s2)
J=J.augment(Xi_s3)
J=J.augment(Xi_s4)
J=J.submatrix(row=0, col=1) # discard first column
#J=(((JT1.JacColumn().augment(JT2.JacColumn())).augment(JT3.JacColumn())).augment(JT4.JacColumn()))
#print "Jacobian:"
#print J.apply_map(attrcall('trig_reduce'))
#print "\n"


#####Output of final Result###
#print "Jacobian Latex:"
#print latex (J.apply_map(attrcall('trig_reduce')))
#print "\n"

#print "First Column of Jacobian (Latex):"
#print latex (Xi1)
#print "\n"

#print "Second Column of Jacobian (Latex):"
#print latex(Xi_s2.apply_map(attrcall('trig_reduce')))
#print "\n"

#print "Third Column of Jacobian (Latex):"
#print latex (Xi_s3.apply_map(attrcall('trig_reduce')))
#print "\n"
#print "Fourth Column of Jacobian (Latex):"
#print latex (Xi_s4.apply_map(attrcall('trig_reduce')))

################ Stichprobe dritte  Spalte Jacobian by inspection
omega3=vector([sin(theta_1),0,cos(theta_1)])
q3=vector([cos(theta_1)*(l_mc+cos(theta_2)*l_p),
                         sin(theta_2)*l_p,
                         -sin(theta_1)*(l_mc+cos(theta_2)*l_p)])

#print "3rd column of spatial Jacobian"
#print latex ((-omega3.cross_product(q3)).column().apply_map(attrcall('trig_reduce')))
###### The above gives exactly the same result as direct calculation ###### => Prooved!
###### Nothing to do anymore until Jacobian Calculation


### Prooved

##########################
# Calculate Body- Jacobian
###########################
# !!!!!!!Might be completely crap at the Moment!!!!!!!!!!!!!!!!!!
J_b=vector([var('x'),var('x'),var('x'),var('x'),var('x'),var('x')]) #create symbolic vector to trick out conversion of vectors
##### FIXME
Xi_c1=(JT1.InvAdjunct()*JT1.Xi()).column()
Xi_c2=(JT2.InvAdjunct()*JT2.Xi()).column()
Xi_c3=(JT3.InvAdjunct()*JT3.Xi()).column()
Xi_c4=(JT4.InvAdjunct()*JT4.Xi()).column()
##### End FIXME

# J=J.augment(JT4.JacColumn().column())
J_b=(J_b.column()).augment(Xi_c1)
J_b=J_b.augment(Xi_c2)
J_b=J_b.augment(Xi_c3)
J_b=J_b.augment(Xi_c4)
J_b=J_b.submatrix(row=0, col=1) # discard first column
J_b_T=J_b.transpose()

###### Final Output of Result #####
#print "Body Jacobian Latex:"
#print latex (J_b.apply_map(attrcall('trig_reduce')))
#print "\n"

#print "Transpose of Body Jacobian Latex:"
#print latex (J_b_T.apply_map(attrcall('trig_reduce')))
#print "\n"
######################### End

#print "First Column of body Jacobian (Latex):"
#print latex (Xi_c1)
#print "\n"

#print "Second Column of body Jacobian (Latex):"
#print latex(Xi_c2.apply_map(attrcall('trig_reduce')))
#print "\n"

#print "Third Column of body Jacobian (Latex):"
#print latex (Xi_c3.apply_map(attrcall('trig_reduce')))
#print "\n"
#print "Fourth Column of body Jacobian (Latex):"
#print latex (Xi_c4.apply_map(attrcall('trig_reduce')))

################ Body Jacobian by inspection
q1c=vector([-l_d -l_m*cos(theta_4)-l_p*cos(theta_4+theta_3)-l_mc*cos(theta_2+theta_3+theta_4),
             l_m*sin(theta_4)+l_p*sin(theta_4+theta_3)+l_mc*sin(theta_2+theta_3+theta_4),
           0])
omega1c=vector([sin(theta_4+theta_3+theta_2),
                     cos(theta_4+theta_3+theta_2),
                 0])
q2c=vector([-l_d -l_m*cos(theta_4)-l_p*cos(theta_4+theta_3),
             +l_m*sin(theta_4)+l_p*sin(theta_4+theta_3),
           0])
omega2c=vector([0,0,1])

q3c=vector([-l_d -l_m*cos(theta_4),
             +l_m*sin(theta_4),
           0])
omega3c=vector([0,0,1])

q4c=vector([-l_d,
             0,
           0])
omega4c=vector([0,0,1])

trick=vector([var('asdf'),var('asdf'),var('asdf')])
J_b_ins=(-omega1c.cross_product(q1c).column()).augment((-omega2c.cross_product(q2c)).column())
J_b_ins=J_b_ins.augment((-omega3c.cross_product(q3c)).column())
J_b_ins=J_b_ins.augment((-omega4c.cross_product(q4c)).column())
OMEGA=(((trick.column().augment(omega1c.column())).augment(omega2c.column())).augment(omega3c.column())).augment(omega4c.column())
J_b_ins=J_b_ins.stack(OMEGA.submatrix(row=0, col=1))
### Chrossceck final output ##
#print "body Jacobian by inspection"
#print latex (J_b_ins.apply_map(attrcall('trig_reduce')))
###### The above gives exactly the same result as direct calculation ###### => 
###### Nothing to do anymore until here!


######!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!#############
###### for debugging purpose
###### calculation of point velocities
#def velocity(_point,_Jacobian):
#    _theta_dot=vector([var("theta_1dot"),var("theta_2dot"),var("theta_3dot"),var("theta_4dot")])
#    _J_theta_dot=_Jacobian*_theta_dot
#    _up=vector([_J_theta_dot[0],_J_theta_dot[1],_J_theta_dot[2]])
#    _low=vector([_J_theta_dot[3],_J_theta_dot[4],_J_theta_dot[5]])
#    _low_skew=Joint.Skew(_low)
#    _J_Skew=_low_skew.augment(_up)
#    _J_Skew=_J_Skew.stack(Matrix(1,4,0))
#    _v=_J_Skew*_point
#    return _v


#print "#########V_p_b############"
#v_p_b=velocity(vector([0,0,0,1]),J_b_ins)
#v_p_b=v_p.apply_map(attrcall('trig_reduce'))
#print latex (v_p_b.column())

#print "#########V_p############"
#v_p=velocity(vector([l_mc+l_p+l_m+l_d,0,0,1]),J)
#v_p=v_p.apply_map(attrcall('trig_reduce'))
#print latex (v_p.column())

#print "######identical?"
#print v_p==-v_p_b

# "Standard Calculation of point velocities
#q_c_1=vector([0])
#q_c_2=vector([])
#q_c_3=vector([])
#q_c_4=vector([])


#v_m=theta_1dot.cross_product(q_c_1)+theta_2dot.cross_product(q_c_2)+theta_3dot.cross_product(q_c_3)+theta_4dot.cross_product(q_c_4)
#########################
# Results Kinematics
#########################
#print "Transformation  Joint 4 gst(theta)="
#print (JT4.GstTheta().apply_map(attrcall('trig_reduce')))
#print "\n"

#print "  Joint 4 R(theta)="
#print (JT4.RTheta().apply_map(attrcall('trig_reduce')))
#print "\n"

#print "  Joint 4 P(theta)="
#print (JT4.PTheta().apply_map(attrcall('trig_reduce')))
#print "\n"

#print "Transformation gst(theta)="
#print latex (JT4.GstTheta().apply_map(attrcall('trig_reduce')))
#print "\n"
#print "R(theta)="
#print latex  (JT4.RTheta(0).apply_map(attrcall('trig_reduce')))
#print "\n"
#print "p(theta)="
#print latex (JT4.PTheta(0).apply_map(attrcall('trig_reduce')))

##############################
# Testcase (Murray p89)
#############################
#GST_TEST_R=Matrix([
#        [cos(theta_1+theta_2+theta_3),  -sin(theta_1+theta_2+theta_3), 0],
#        [sin(theta_1+theta_2+theta_3),   cos(theta_1+theta_2+theta_3), 0],
#        [0                        ,   0                        , 1]
#        ])
#GST_TEST_P=Matrix([
#        [-l_1*sin(theta_1)-l_2*sin(theta_1+theta_2)],
#        [ l_1*cos(theta_1)+ l_2*cos(theta_1+theta_2)],
#        [ 0                                  ]
#        ])
#GST_TEST_TEMP=GST_TEST_R.augment(GST_TEST_P)
#
#GST_TEST=GST_TEST_TEMP.stack(Matrix([[0,0,0,1]]))

####################################
# Not needed but usefull Functions #
####################################
#def xi_wedge(_xi): #Is this needed anymore??
#    # Calculate Cross Product of omega and q which represents velocity v according to [murray p41]
#    # 4x4 Matrix intrduced in [murray p. 39] 
#    XI_wedge=Matrix([
#        [0     ,-_xi[2], _xi[1],_xi[0] ],
#        [_xi[2] ,0     ,-_xi[0],_xi[1] ],
#       [-_xi[1],_xi[0] ,0     ,_xi[2] ],
#       [0     ,0     ,0     ,0     ]
#       ])
#   return XI_wedge

##################################
# Matrix operations #
#####################
# Make diagonal matrix out of vector
def diag(_vector):
    n=_vector.degree()
    diagonalMatrix=Matrix(n,n,var("asdf"))
    for i in range(0,n):
        diagonalMatrix[i,i]=_vector[i]
    return diagonalMatrix



###########################
# PLots
###########################

#p_theta_4_dip={"theta_1":0*deg,"theta_2":15*deg,"theta_3":15*deg,"L":6+35+15}

#p_theta_4_pip={"theta_1":0*deg,"theta_2":15*deg,"theta_3":15*deg,"L":6+35}

#p_theta_4_mc={"theta_1":0*deg,"theta_2":15*deg,"theta_3":15*deg,"L":6}

# Kinematics
#POINT=GST_THETA*vector([0,0,0,1])
#coord3d=vector([POINT[0],POINT[1],POINT[2]]) #Full 3D Plot
#coord2d=vector([POINT[0],POINT[1]]) #Take only values from x,y plane (sagital plane)
# 2D Plots
#p_t_4=parametric_plot(coord2d(**p_theta_4),(theta_4,0,pi/2))
#p_t_4t=parametric_plot(coord2d(**p_theta_4_tip),(theta_4,0,60*deg))
#p_t_4t.show(aspect_ratio=1,xmin=0,ymin=0,xmax=70,ymax=40)

#p_t_4d=line(coord2d(**p_theta_4_tip)(theta_4=0))
#p_t_4p=parametric_plot(coord2d(**p_theta_4_tip),(theta_4,0,60*deg))
#p_t_4m=parametric_plot(coord2d(**p_theta_4_tip),(theta_4,0,60*deg))
#show(p_t_4t,p_t_4d)
#p_t_4.show(aspect_ratio=1,xmin=0,ymin=0,xmax=135,ymax=75)
# 3D Plots
#p_t_4=parametric_plot3d(coord3d(**p_theta_4),(theta_4,0,60*deg))
#p_t_4.show(aspect_ratio=1,xmin=0,ymin=0,zmin=30,xmax=135,ymax=75,zmax=30)

##### Dictionaries Plot parameters

#p_theta_4_tip={
#"theta_1":0*deg,
#"theta_2":15*deg,
#"theta_3":15*deg,
#"l_dist":15,
#"l_med":15,
#"l_prox":35,
#"l_mc":6,
#"L":15+15+35+6
#}


#############################################################
# Calculation of the Jacobian
#############################################################


#def calc_ad_exi(_xi,_i): #invoke function with joint number i to calculate ith column vektor of the Jacobian
    # Create list of all exponentials exept last joint
#    _a=calc_exp_XI(**mc1_dict) # Calculate exponential using function calc_exp_XI
#    _b=calc_exp_XI(**mc2_dict) 
#    _c=calc_exp_XI(**pip_dict)
#    list_exi=(_a,_b,_c)
    # Calculate Adjunct of actual joint
#    _TEMP=1
#    for n in range(0,_i-1): #Calculate product of exponentials starting from first joint
#        print 'We are on time %d' % (n)
#        _TEMP=_TEMP*list_exi[n]
        
#        print "_TEMP:"
#        print _TEMP

    # Mit der fake Ad Matrix: Murray p 55.
#    PSKEW=skew((P_THETA).column(0)) # calculate Skewsymetric Matrix P_THETA; Skew expects a vector not a matrix
#    AdT_up=R_THETA.augment(PSKEW*R_THETA) # Built upper part of matrix
#    AdT_low=Matrix(3,3,(var('null'))).augment(R_THETA) # Built lower part of matrix; using variable circumpasses type converion problems
# Merge Parts of Submatrices
#    AdT=AdT_up.stack(AdT_low) # Put upper part of Matrix on top of lower one
#    AdT=AdT.substitute(null=0) #Substitute null by zero
#    AdT=AdT.apply_map(attrcall('trig_reduce')) #Simplifiy Matrix
#Print Resulting Matrix
#    print "AdT:"
#    print AdT
#Print for Latex use
#    print "AdT:"
#    print latex (AdT)


 #   return d_xi

####### Calculating WikiHand

############## 2#####################
######### Setup Dictionaries needed for calculations
 
## Dictionaries holding all fixed values; do not use negative Values here!
param_dict={
"l_d":15.0,
"l_m":15.0,
"l_p":35.0,
"l_mc":6.0,
"L":15+15+35+6.0,
"r_M":8.2,
"r_P":5.3,
"r_D":3.75,
"d_i":0, #"d_i":1.5, #The asymmetric loading of the MC by DIP and PIP tendons causes an additional torque.
"d_a":0, #"d_a":3.3, #Therefore we assume the distance from the middle to be zero
"f_tipX":0.0, # We want to apply a force in +y direction
"f_tipY":30.0, # We want to apply a force in +y direction
"f_tipZ":0.0, # We want to apply a force in +y direction
"alpha_pulley":58.3*deg, #all signs have to be positive
"alpha_mc":10.0*deg, #FIXME
"alpha_guide":30.0*deg, #Checked with CAD: enlacement angle of palmar guiding at PIP
"offset_flex":0.2, #Checked with CAD: offset in y direction in MC
"offset_ext":1.1, #Checked with CAD: offset in -y direction in MC
"alpha_5_mc":1.1*deg, # Checked with CAD
"alpha_8_mc":4.5*deg, # Checked with CAD; negative direction. Adjusted in ALPHA matrix
}


pret0_dict={
"f_pre_MC13":0.0,
"f_pre_MC24":0.0,
"f_pre_PIP56":0.0,
"f_pre_DIP78":0.0
}


pret5_dict={
"f_pre_MC13":5.0,
"f_pre_MC24":5,
"f_pre_PIP56":5,
"f_pre_DIP78":5
}


pret10_dict={
"f_pre_MC13":10.0,
"f_pre_MC24":10.0,
"f_pre_PIP56":10.0,
"f_pre_DIP78":10.0
}


pret30_dict={
"f_pre_MC13":30,
"f_pre_MC24":30,
"f_pre_PIP56":30,
"f_pre_DIP78":30
}


pret50_dict={
"f_pre_MC13":50,
"f_pre_MC24":50,
"f_pre_PIP56":50,
"f_pre_DIP78":50
}

pret150_dict={
"f_pre_MC13":150,
"f_pre_MC24":150,
"f_pre_PIP56":150,
"f_pre_DIP78":150
}

############# 3 #####################


#Tendon Force Calculation
F_tip=vector([var("f_tipX"),var("f_tipY"),var("f_tipZ"),0,0,0])
F_test=vector([0,0,1,0,0,0])

#We set F_tip components in x and z to zero
f_tipX=0
f_tipZ=0 
tau_b=J_b.transpose()*F_tip
tau_b_test=J_b.transpose()*F_test

#print latex (v_p(theta_1=0,theta_2=0,theta_3=0,theta_4=0))
#print "###############"
#print latex (v_p_b(theta_1=0,theta_2=0,theta_3=0,theta_4=0))
#print "###############"
#print "###############"
#print "q generic:"
#print q_gen
#print latex (q_gen.column().apply_map(attrcall('trig_reduce')))

#print "###############"
#print "q_b:"
#print q_b
#print latex (q_b.column().apply_map(attrcall('trig_reduce')))

#print "###############"
#print "q_b_test:"
#print q_b_test
#print latex (q_b_test.column().apply_map(attrcall('trig_reduce')))


tau_b=tau_b.apply_map(attrcall('trig_reduce'))
################################################################################
# Calculation of tendon forces needed to reach a desired torque on each joint.
# Using principle of virtual work (Murray p 295-299)
################################################################################

# necessary functions
def pseudoinverse (_matrix):
    _matrix_square=(_matrix*_matrix.transpose().apply_map(attrcall('trig_reduce'))) #Dirty things are triginometric
    #_matrix_square=_matrix*_matrix.transpose()
    print "square matrix calculated"
    #    _matrix_squareinv=(_matrix_square.inverse().apply_map(attrcall('simplify')))
    _matrix_squareinv=_matrix_square.inverse()
    print "square matrix inverse calculated"
    _pseudoinv=_matrix.transpose()*_matrix_squareinv
    print "pseudoinverse calculated"
#    return _pseudoinv.apply_map(attrcall('trig_reduce'))
    return _pseudoinv


# Calculate Tendon Forces Using Virtual Work Principle

#### Tendon Mapping:
# MC_abd_flex=> 1
# MC_add_flex=> 2
# MC_add_ext => 3
# MC_abd_ext => 4
# P_flex     => 5
# P_ext      => 6
# D_flex     => 7
# D_ext      => 8
         
# The following is only necessary for formal reasons        
l1,l2,l3,l4,l5,l6,l7,l8=var("l1,l2,l3,l4,l5,l6,l7,l8")
# Attachment radius
r_M,r_P,r_D=var("r_M,r_P,r_D")
r=vector([r_M,r_M,r_P,r_D])

# radius of guidings in hyperboloids
var("d_i,d_a")

# elongation equations; ATTENTION to pull on the tendon we use NEGATIVE forces! Important with pretension calculation
h1(theta_1,theta_2,theta_3,theta_4)=l1 -r_M*theta_1 -r_M*theta_2
h2(theta_1,theta_2,theta_3,theta_4)=l2 +r_M*theta_1 -r_M*theta_2
h3(theta_1,theta_2,theta_3,theta_4)=l3 +r_M*theta_1 +r_M*theta_2
h4(theta_1,theta_2,theta_3,theta_4)=l4 -r_M*theta_1 +r_M*theta_2
h5(theta_1,theta_2,theta_3,theta_4)=l5 -d_i*theta_1 -r_P*theta_3
h6(theta_1,theta_2,theta_3,theta_4)=l6 -d_a*theta_1 +r_P*theta_3
h7(theta_1,theta_2,theta_3,theta_4)=l7 +d_i*theta_1 +r_D*theta_3  -r_D*theta_4 
h8(theta_1,theta_2,theta_3,theta_4)=l8 +d_a*theta_1 -r_D*theta_3  +r_D*theta_4

h=vector([h1,h2,h3,h4,h5,h6,h7,h8])
P_theta=jacobian(h,[theta_1,theta_2,theta_3,theta_4]).transpose()
#print "extension vector h:"
#print latex (h.column())
#print "##########################################"
#print "P(theta)"
#print P_theta
print "##########################################"
#print latex (P_theta)

##########################################

################## 4 #######################

##### Calculate all active tendon forces

#Use result for Coupling Matrix from above:
Ptheta=Matrix([[-r_M , r_M  ,r_M,-r_M,-d_i,-d_a ,d_i ,d_a ],
              [-r_M  ,-r_M  ,r_M, r_M, 0  ,0    ,0   ,0   ],
              [ 0    ,0     ,0  ,0   ,-r_P,r_P  ,r_D ,-r_D],
              [ 0    ,0     ,0  ,0   ,0   ,0    ,-r_D,r_D ]
              ]) 

# print Ptheta

# Calculate the Pseudoinverse of P_theta since P theta is not a Square Matrix
# Calculates to P^T*(P*P^T)^-1

P_pseudoinv=pseudoinverse(Ptheta)
#print "pseudoinverse:"
#print P_pseudoinv

# Active Tendonforces: pseudinverse * torque Vector
f_t_act=P_pseudoinv*tau_b # vector of 8 entries

#### The Forces we retrieved still can be in wrong (positive!) direction ####
# So we select the lowest forces and raise (or correct lower) all tendonforces so minimum pretension is achieved
# In the metacarpal pretension acts diagonally whereas joint torques are applied by non diagonal tendons.
# Therefore we raise or lower all 4 tendon loads. ATTENTION inverted sign since tendons only can pull

f_t_act_corr=f_t_act-vector([ #Checked. Sign is correct 
        max_symbolic(f_t_act[0:4]),
        max_symbolic(f_t_act[0:4]),	
        max_symbolic(f_t_act[0:4]),	
        max_symbolic(f_t_act[0:4]),
        max_symbolic(f_t_act[4:6]),
        max_symbolic(f_t_act[4:6]),
        max_symbolic(f_t_act[6:8]),
        max_symbolic(f_t_act[6:8])
	])
                            
# Done

####For Debugging Purpose

#print latex (f_t_act(**test_dict).column())
#show (f_t_act(**test_dict).column())

square matrix calculated square matrix inverse calculated pseudoinverse calculated

## 5 #####

#########################
####Friction calculation
# Note: Diagonal Matrices are used to enable elementwise multiplication of vectors
#########################

### Now we have the tendon forces. In the folowing we calculate the resulting joint torques after appying friction 

# Now we calculate the resulting normal forces at every joint. We do not need the normal forces in the sliding surfaces!
# The normal force is the vector sum of the two tendon forces. Therefore we need the angle of the tendon.

# Setup pretension forces
var("f_pre_MC13,f_pre_MC24,f_pre_PIP56,f_pre_DIP78")
f_t_pre=vector([f_pre_MC13,f_pre_MC24,f_pre_MC13,f_pre_MC24,f_pre_PIP56,f_pre_PIP56,f_pre_DIP78,f_pre_DIP78])

#Setup tendon force including active and pretension part
f_tendon=f_t_act_corr+(-1)*f_t_pre #These forces all are <0 unless pretension is >0

# Setup angles of attachment
# angles are measured using joint CoS distally  FIXME: put all signs to ALPHA Matrix
var("offset_flex,offset_ext,alpha_pulley")
var("alpha_guide,alpha_mc,alpha_8_mc,alpha_5_mc")
alpha_5_p=alpha_guide+ alpha_5_mc   # must be measured from CAD
alpha_6_p=-arcsin((-offset_ext+r_P)/l_prox)
alpha_7_p=-arcsin((+offset_flex+r_D)/l_prox) # The flexor DIP is guided on the diameter of the DIP
alpha_7_m=arcsin((2*r_D)/l_med)
alpha_8_p=alpha_pulley
alpha_8_m=-arcsin((2*r_D)/l_med)

####### Calculate Normal force Vector for every joint.
###### !! Indices start at 0!!
### Tendon Forces are already in negative z- direction, tendon angles in positive
### These forces do not include the Fingertip force. It has to be added in
## Calculation of the fingertip force has to be in local coordinate system. 
## We use simple rotation matrices calculation since it is a simple transformation

# Tip forces
f_tip_DIP=-(F_tip)[0:3] #DIP reaction force
f_tip_PIP=-(F_tip)[0:3]* Matrix([ #PIP Reaction force
        [cos(-theta_4), -sin(-theta_4),0], #we are gong bachwards so the direction is inverted!
        [sin(-theta_4),  cos(-theta_4),0],
        [0,0,1]
        ])
f_tip_MC=f_tip_PIP *Matrix([ #MC Reaction force
        [cos(-theta_3), -sin(-theta_3),0], #we are gong bachwards so the direction is inverted!
        [sin(-theta_3),  cos(-theta_3),0],
        [0,0,1]
        ])
#DIP
f_N_DIP=(f_tendon[8-1]*vector([cos(alpha_8_m),sin(alpha_8_m),0])+
         f_tendon[7-1]*vector([cos(alpha_7_m),sin(alpha_7_m),0])
	 +f_tip_DIP)	#Fingertip force reaction in local coordinate system

#PIP
f_N_PIP=(f_tendon[8-1]*vector([cos(alpha_8_p),sin(alpha_8_p),0])+
         f_tendon[7-1]*vector([cos(alpha_7_p),sin(alpha_7_p),0])+
         f_tendon[6-1]*vector([cos(alpha_6_p),sin(alpha_6_p),0])+
         f_tendon[5-1]*vector([cos(alpha_5_p),sin(alpha_5_p),0]) 
	 +f_tip_PIP)

#MC
f_N_MC=(vector([f_tendon[1-1],0,0])+
        vector([f_tendon[2-1],0,0])+
        vector([f_tendon[3-1],0,0])+
        vector([f_tendon[4-1],0,0])+
        vector([f_tendon[5-1],0,0])+
        vector([f_tendon[6-1],0,0])+
        vector([f_tendon[7-1],0,0])+
        vector([f_tendon[8-1],0,0])
	 +f_tip_MC)


        


#########################
####Friction calculation
# Diagonal Matrices are used to enable elementwise multiplication of vectors
#########################

var("mu_j,mu_t")

# Joint friction
Tau_fr_N=diag(vector([abs(f_N_MC),abs(f_N_MC),abs(f_N_DIP),abs(f_N_PIP)]))*mu_j*diag(r)  #only the absolute value of FN is needed.Matrix

####### FIXME ####### we take the absolut value of all torques. Therefore we have to take care doing comparison!
# Calculate signs of tau_b
# tau_b_sign=vector([0,x,x,x])
# for i in range(1,4):
#    tau_b_sign[i]=tau_b[i]/abs(tau_b[i])

tau_fr_N=vector(Tau_fr_N.diagonal())# *-1*diag(tau_b.apply_map(sign))).diagonal())


#tau_b.apply_map(sign) # Friction has to be in opposite direction to applied torque. Vector

# Enlacement friction
# Summed up enlacement angles in zero configuration
 # Angle within palmar guiding


ALPHA=Matrix([
        [-alpha_mc   ,0             ,0                  ,0], #1 CHECKED
        [ alpha_mc   ,0             ,0                  ,0], #2 CHECKED
        [ alpha_mc   ,0             ,0                  ,0], #3 CHECKED
        [-alpha_mc   ,0             ,0                  ,0], #4 CHECKED
        [0           ,alpha_5_mc     ,alpha_5_p        ,0], #5 The angles within MC hyperboloid and the guiding are summed up as alpha_5_p
        [0           ,alpha_6_p     ,0                  ,0], #6 CHECKED. Attention the distal attachment angle does have no influence!
        [0           ,alpha_7_p     ,alpha_7_p+alpha_7_m,0], #7 Angle within MC, angle PIP proximal+ angle PIP distal
        [0           ,-alpha_8_mc     ,alpha_8_p+alpha_8_m,0]  #8 Different angles for distal and proximal end of phalanx!! Angle within MC, angle PIP proximal+ angle PIP distal
        ])

##FIXME: use that matrix for h to fix signs for consistency reasons
### DANGER enlacement eg in hyperboloids increases in both direction. Use abs
# Enlacement at the "final joint" does not change the enlacement angle. Therefore theta 4 plots are not interesting at all!
THETA=Matrix([ 
[ theta_1 ,       0,        0,        0], #1
[ theta_1 ,       0,        0,        0], #2
[ theta_1 ,       0,        0,        0], #3
[ theta_1 ,       0,        0,        0], #4
[ theta_1 , theta_2,        0,        0], #5 Range of motion is symmetric. Therefore sign of theta 1 does not matter
[ theta_1 , theta_2,        0,        0], #6
[-theta_1 , theta_2,  theta_3,        0], #7
[-theta_1 , theta_2, -theta_3,        0], #8
])

ALPHA_ENL=(ALPHA+THETA)
ALPHA_ENL=ALPHA_ENL.apply_map(abs) #tendons never loose contact even if angle changes sign
alpha_enl_sum=vector([
        sum(ALPHA_ENL.row(1-1)),
        sum(ALPHA_ENL.row(2-1)),
        sum(ALPHA_ENL.row(3-1)),
        sum(ALPHA_ENL.row(4-1)),
        sum(ALPHA_ENL.row(5-1)),
        sum(ALPHA_ENL.row(6-1)),
        sum(ALPHA_ENL.row(7-1)),
        sum(ALPHA_ENL.row(8-1))
        ])

# Calculation of "friction coefficient" of every tendon Proofed! This is the relation between Frictionforce and Tendonforce
mu_enlacement=vector([
        1-e^(-1*(mu_t*alpha_enl_sum[0])),
        1-e^(-1*(mu_t*alpha_enl_sum[1])),
        1-e^(-1*(mu_t*alpha_enl_sum[2])),
        1-e^(-1*(mu_t*alpha_enl_sum[3])),
        1-e^(-1*(mu_t*alpha_enl_sum[4])),
        1-e^(-1*(mu_t*alpha_enl_sum[5])),
        1-e^(-1*(mu_t*alpha_enl_sum[6])),
        1-e^(-1*(mu_t*alpha_enl_sum[7]))])

# Calculate the tendon friction force
F_T_Fric=diag(f_tendon)*diag(mu_enlacement) #The friction is with respect to the complete tendon load; diagonal matrix

f_t_fric=vector(F_T_Fric.diagonal()) #Vector holding friction forces of all tendons
f_t_act_fr=f_t_act_corr-f_t_fric #Vector of tendon force at joint. That is all that arrives of our tendon load

# Calculate the Joint torques from tendon forces

## mu_enl_tendon=f_t_fric/vector(f_t_act_corr) #Relation of tendon friction and the active tendon load part. This is pretension dependent!
# This is only possible elementwise!
        
tau_fr_tendon=Ptheta.apply_map(abs)*(f_t_act_corr-f_t_act_fr) #Joint torque caused by tendon friction. Friction force can never be more than active force!! We use the abs of Ptheta since all tendons do friction work.

tau_fr_tendon_error=vector([0,x,x,x]) #Vector Division only componentwise since tau_b of first axis is zero
for i in range(1,4):
    tau_fr_tendon_error[i]=tau_fr_tendon[i]/tau_b[i]

# Torque Error normal force
tau_fr_N_error=vector([0,x,x,x]) #Vector Division only componentwise since tau_b of first axis is zero
for i in range(1,4):
    tau_fr_N_error[i]=tau_fr_N[i]/tau_b[i]

############### Sum of friction torques
tau_fr=tau_fr_tendon+tau_fr_N

# Error overall
tau_fr_error=vector([0,x,x,x]) #Vector Division only componentwise since tau_b of first axis is zero
for i in range(1,4):
    tau_fr_error[i]=tau_fr[i]/tau_b[i]


# Effective Torque tau_eff=tau_b-tau_fr 
# that does not work tau_eff=tau_b-max_symbolic(tau_b.apply_map(abs),tau_fr.apply_map(abs)) #Friction Torque can not be bigger than applied torque
# So we do it componentwise
tau_eff=vector([x,x,x,x]) #Vector Division only componentwise since tau_b of first axis is zero
for i in range(4):
    tau_eff[i]=tau_b[i]-max_symbolic(tau_b.apply_map(abs)[i],tau_fr.apply_map(abs)[i]) #Friction Torque can not be bigger than applied torque

Traceback (most recent call last): File "", line 1, in <module> File "/tmp/tmpfQx7Sd/___code___.py", line 164, in <module> tau_fr_tendon=Ptheta.apply_map(abs)*(f_t_act_corr-f_t_act_fr) #Joint torque caused by tendon friction. Friction force can never be more than active force!! We use the abs of Ptheta since all tendons do friction work. File "element.pyx", line 2467, in sage.structure.element.Matrix.__mul__ (sage/structure/element.c:16791) File "coerce.pyx", line 739, in sage.structure.coerce.CoercionModel_cache_maps.bin_op (sage/structure/coerce.c:6549) File "action.pyx", line 195, in sage.matrix.action.MatrixVectorAction._call_ (sage/matrix/action.c:3592) File "matrix0.pyx", line 3703, in sage.matrix.matrix0.Matrix._matrix_times_vector_ (sage/matrix/matrix0.c:20430) File "expression.pyx", line 1812, in sage.symbolic.expression.Expression.__nonzero__ (sage/symbolic/expression.cpp:8206) File "/sagenb/sage_install/sage-4.7.2/local/lib/python2.6/site-packages/sage/interfaces/maxima_lib.py", line 420, in _eval_line if statement: result = ((result + '\n') if result else '') + max_to_string(maxima_eval("#$%s$"%statement)) File "ecl.pyx", line 693, in sage.libs.ecl.EclObject.__call__ (sage/libs/ecl.c:4660) File "ecl.pyx", line 276, in sage.libs.ecl.ecl_safe_apply (sage/libs/ecl.c:2769) RuntimeError: ECL says: Console interrupt __SAGE__

print tau_b(**param_dict)(mu_t=0.1,mu_j=0.1,f_pre_MC13=0,f_pre_MC24=0,f_pre_PIP56=0,f_pre_DIP78=0,theta_4=0)

(0, 1050.00000000000*cos(theta_3) + 900.000000000000, 900.000000000000, 450.000000000000)

# Debuggin normal force
# Theta 1
p1=plot(vector(f_N_MC(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_1,theta1min,theta1max),label="MC_x")
p12=plot(vector(f_N_MC(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_1,theta1min,theta1max),label="MC_y")

p2=plot(vector(f_N_PIP(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_1,theta1min,theta1max),label="PIP_x")
p21=plot(vector(f_N_PIP(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_1,theta1min,theta1max),label="PIP_y")

p3=plot(vector(f_N_DIP(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_1,theta1min,theta1max),label="DIP_x")
p31=plot(vector(f_N_DIP(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_1,theta1min,theta1max),label="DIP_y")
show(p1+p12+p2+p21+p3+p31)

# Theta 2
p1=plot(vector(f_N_MC(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_2,theta2min,theta2max),label="MC_y")
p12=plot(vector(f_N_MC(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_2,theta2min,theta2max),label="MC_y")

p2=plot(vector(f_N_PIP(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_2,theta2min,theta2max),label="PIP_y")
p21=plot(vector(f_N_PIP(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_2,theta2min,theta2max),label="PIP_y")

p3=plot(vector(f_N_DIP(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_2,theta2min,theta2max),label="DIP_x")
p31=plot(vector(f_N_DIP(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_2,theta2min,theta2max),label="DIP_y")
show(p1+p12+p2+p21+p3+p31)

# Theta 3
p1=plot(vector(f_N_MC(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_3,theta3min,theta3max),label="MC_x")
p12=plot(vector(f_N_MC(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_3,theta3min,theta3max),label="MC_y")

p2=plot(vector(f_N_PIP(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_3,theta3min,theta3max),label="PIP_x")
p21=plot(vector(f_N_PIP(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_3,theta3min,theta3max),label="PIP_y")

p3=plot(vector(f_N_DIP(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_3,theta3min,theta3max),label="DIP_x")
p31=plot(vector(f_N_DIP(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_3,theta3min,theta3max),label="DIP_y")
show(p1+p12+p2+p21+p3+p31)

# Theta 4
p1=plot(vector(f_N_MC(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_4,theta4min,theta4max),label="MC_x")
p12=plot(vector(f_N_MC(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_4,theta4min,theta4max),label="MC_y")

p2=plot(vector(f_N_PIP(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_4,theta4min,theta4max),label="PIP_x")
p21=plot(vector(f_N_PIP(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_4,theta4min,theta4max),label="PIP_y")

p3=plot(vector(f_N_DIP(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[0],(theta_4,theta4min,theta4max),label="DIP_x")
p31=plot(vector(f_N_DIP(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_4,theta4min,theta4max),label="DIP_y")
show(p1+p12+p2+p21+p3+p31)

############################################################################
# Debuggin joint Torque
# Theta 1
p1=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[0],(theta_1,theta1min,theta1max),label="MC_x")
p12=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[1],(theta_1,theta1min,theta1max),label="MC_y")

p2=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[2],(theta_1,theta1min,theta1max),label="PIP_x")
p21=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[3],(theta_1,theta1min,theta1max),label="PIP_y")

show(p1+p12+p2+p21)

# Theta 2
p1=plot(tau_b(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[0],(theta_2,theta2min,theta2max),label="MC_y")
p12=plot(tau_b(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[1],(theta_2,theta2min,theta2max),label="MC_y")

p2=plot(tau_b(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[2],(theta_2,theta2min,theta2max),label="PIP_y")
p21=plot(tau_b(**param_dict)(theta_1=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[3],(theta_2,theta2min,theta2max),label="PIP_y")
show(p1+p12+p2+p21)

# Theta 3
p1=plot(tau_b(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[0],(theta_3,theta3min,theta3max),label="MC_x")
p12=plot(tau_b(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[1],(theta_3,theta3min,theta3max),label="MC_y")

p2=plot(tau_b(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[2],(theta_3,theta3min,theta3max),label="PIP_x")
p21=plot(tau_b(**param_dict)(theta_2=0,theta_1=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[3],(theta_3,theta3min,theta3max),label="PIP_y")
show(p1+p12+p2+p21)

# Theta 4
p1=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[0],(theta_4,theta4min,theta4max),label="MC_x")
p12=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[1],(theta_4,theta4min,theta4max),label="MC_y")

p2=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[2],(theta_4,theta4min,theta4max),label="PIP_x")
p21=plot(tau_b(**param_dict)(theta_2=0,theta_3=0,theta_1=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)[3],(theta_4,theta4min,theta4max),label="PIP_y")
show(p1+p12+p2+p21)

Traceback (most recent call last): p21=plot(vector(f_N_PIP(**param_dict)(theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,mu_j=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))[1],(theta_1,theta1min,theta1max),label="PIP_y") File "", line 1, in <module> File "/tmp/tmpfaGnzo/___code___.py", line 4, in <module> p1=plot(vector(f_N_MC(**param_dict)(theta_2=_sage_const_0 ,theta_3=_sage_const_0 ,theta_4=_sage_const_0 ,mu_t=_sage_const_0p1 ,mu_j=_sage_const_0p1 ,f_pre_MC13=_sage_const_10 ,f_pre_MC24=_sage_const_10 ,f_pre_PIP56=_sage_const_10 ,f_pre_DIP78=_sage_const_10 ))[_sage_const_0 ],(theta_1,theta1min,theta1max),label="MC_x") NameError: name 'theta1min' is not defined

####Debugging stuff only

# print (vector(f_t_act_corr)).column()
print "Friction Torque on tendon level:"
# print (vector(f_t_act_corr(**param_dict)(theta_1=0,theta_2=0,theta_3=0,theta_4=0))).column()
print (vector(tau_fr_tendon(**param_dict)(theta_1=0,theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10))).column().n()


#print (vector(tau_error_tendon(**param_dict)(theta_1=0,theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,f_pre_MC13=50,f_pre_MC24=50,f_pre_PIP56=50,f_pre_DIP78=50))).column().n()

print "Desired Torque:"
plot(vector(tau_b(**param_dict)(theta_1=0,theta_2=0,theta_3=0,theta_4=0,mu_t=0.1,f_pre_MC13=50,f_pre_MC24=50,f_pre_PIP56=50,f_pre_DIP78=50)[0])

Traceback (most recent call last): File "", line 1, in <module> File "/tmp/tmp9hWakE/___code___.py", line 14 ^ SyntaxError: invalid syntax

#######6#####

### Set Parameters for plots and plot
### make the results objects
#### All values doube checked!
theta1min=-30*deg
theta1max=30*deg

theta2min=-30*deg
theta2max=90*deg

theta3min=-10*deg
theta3max=135*deg

theta4min=-30*deg
theta4max=135*deg


print ((Tau_fr(**param_dict))(f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)).apply_map(attrcall('simplify')).n()
print "#############"
print (TauTest(**param_dict)).n()
print "########"
print (tau_fr_N(**param_dict))(f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10)
etha=vector([Tau_fr[0]/TauTest[0],Tau_fr[1]/TauTest[1],Tau_fr[2]/TauTest[2],Tau_fr[3]/TauTest[3]])

print (etha(**param_dict))(f_pre_MC13=10,f_pre_MC24=10,f_pre_PIP56=10,f_pre_DIP78=10)

Traceback (most recent call last): theta1max=30*deg File "", line 1, in <module> File "/tmp/tmppU5Akt/___code___.py", line 20, in <module> print ((Tau_fr(**param_dict))(f_pre_MC13=_sage_const_10 ,f_pre_MC24=_sage_const_10 ,f_pre_PIP56=_sage_const_10 ,f_pre_DIP78=_sage_const_10 )).apply_map(attrcall('simplify')).n() NameError: name 'Tau_fr' is not defined

###### 7 ########
#######################################################################################
#######################################################################################
# The second chance ;-)
# Concept: Build a Matrix and aply map of my functions to it. Since the Matrix is maximum 2D we can vary two values
# We do not stick to the concept to have everything in vector format.
# We use updating of dictionaries to keep the number of dictionaries low
class plotData(object):
    def __init__ (self,_var1,_start_x,_end_x,_points_x,_var2,_start_y,_end_y,_points_y):        
#    def __init__ (self):        
        self._points_x=_points_x #we need these values in subroutines
        self._points_y=_points_y #we need these values in subroutines
        self._start_x=_start_x
        self._start_y=_start_y
        self._end_x=_end_x
        self._end_y=_end_y
        self._var1=_var1
        self._var2=_var2
        self.th1_dict={
            "theta_2":0.0,
            "theta_3":0.0,
            "theta_4":0.0
            }
        
        self.th2_dict={
            "theta_1":0.0,
            "theta_3":0.0,
            "theta_4":0.0
            }
        self.th3_dict={
            "theta_1":0.0,
            "theta_2":0.0,
            "theta_4":0.0
            }
        
        self.th4_dict={
            "theta_1":0,
            "theta_2":0,
            "theta_3":0
            }
        
        self.th23_dict={
            "theta_1":0.0,
            "theta_4":0.0
            }
        self.th2wc_dict={
            "theta_1":theta1max,
            "theta_3":0.0,
            "theta_4":0.0
            }
        self.th3wc_dict={
            "theta_1":theta1max,
            "theta_2":theta2max,
            "theta_4":0.0
            }
        self.mu_dict={
            "mu_t":0.1, #friction coefficient of tendon
            "mu_j":0.1 #friction coefficient of joint surfaces
            }
        self.pre10_dict={ #set pretension values to some value for calculations. Do not use for joint torques"
            "f_pre_MC13": 10.0,
            "f_pre_MC24": 10.0,
            "f_pre_PIP56": 10.0,
            "f_pre_DIP78": 10.0
            }
        
        self.preEq_dict={ #set pretension values to some value for calculations. Do not use for joint torquesto same value"
            "f_pre_MC13": var("f_pre"),
            "f_pre_MC24": f_pre,
            "f_pre_PIP56": f_pre,
            "f_pre_DIP78": f_pre
            }
        self.constants={
            "pi": 3.141592654,
            "e":2.718281828
            }
        
        self._vx=self.createVector(self._start_x,self._end_x,self._points_x) #this vector is the basis for the x coordinates
        self._vy=self.createVector(self._start_y,self._end_y,self._points_y) #this vector is the basis for the y coordinates
        self.createMatrix() #Create the Matrix of input values
        
    def calcValue(self,_value):
        self._value=_value
        self._input=self._tuples
        self._result=Matrix(QQ,self._points_x*self._points_y,1)
        self._k=0
        # end init section
        # Select usecase and set remaining parameters
        if self._var1 == "theta_1" and self._var2 == "mu":
            self._value=self._value(**self.th1_dict)(**self.pre10_dict)(**self.constants).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                print "::::::::::::::::::::::debugme::::::::::::::::"
                print item[0].n(), item[1].n()
                print self._value(theta_1=item[0],mu_t=item[1],mu_j=item[1])
                self._result[self._k,0]=self._value(theta_1=item[0],mu_t=item[1],mu_j=item[1]).n()
                self._k=self._k+1

        elif self._var1 == "theta_2" and self._var2 == "mu":
            self._value=self._value(**self.th2_dict)(**self.pre10_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_2=item[0],mu_t=item[1],mu_j=item[1]).n()
                self._k=self._k+1
                
        elif self._var1 == "theta_3" and self._var2 == "mu":
            self._value=self._value(**self.th3_dict)(**self.pre10_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_3=item[0],mu_t=item[1],mu_j=item[1]).n()
                self._k=self._k+1
                
        elif self._var1 == "theta_4" and self._var2 == "mu":
            self._value=self._value(**self.th4_dict)(**self.pre10_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_4=item[0],mu_t=item[1],mu_j=item[1]).n()
                self._k=self._k+1           
                
        elif self._var1 == "theta_23" and self._var2 == "mu":
            self._value=self._value(**self.th23_dict)(**self.pre10_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_2=item[0],theta_3=item[0],mu_t=item[1],mu_j=item[1]).n()
                self._k=self._k+1
#####
        elif self._var1 == "theta_2wc" and self._var2 == "mu":
            self._value=self._value(**self.th2wc_dict)(**self.pre10_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_2=item[0],mu_t=item[1],mu_j=item[1]).n()
                self._k=self._k+1

        elif self._var1 == "theta_3wc" and self._var2 == "mu":
            self._value=self._value(**self.th3wc_dict)(**self.pre10_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_3=item[0],mu_t=item[1],mu_j=item[1]).n()
                self._k=self._k+1
#####                
        elif self._var1 == "theta_1" and self._var2 == "f_pre":
            self._value=self._value(**self.th1_dict)(**self.preEq_dict)(**self.mu_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_1=item[0],f_pre=item[1]).n()
                self._k=self._k+1
                
        elif self._var1 == "theta_2" and self._var2 == "f_pre":
            self._value=self._value(**self.th2_dict)(**self.preEq_dict)(**self.mu_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_2=item[0],f_pre=item[1]).n()
                self._k=self._k+1
                
        elif self._var1 == "theta_3" and self._var2 == "f_pre":
            # ATTENTION: very dirty trick to circumvent the signum() problem
            self._value=self._value(**self.th3_dict)(**self.preEq_dict)(**self.mu_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_3=item[0],f_pre=item[1]).n()
                self._k=self._k+1
 
        elif self._var1 == "theta_4" and self._var2 == "f_pre":
            self._value=self._value(**self.th4_dict)(**self.preEq_dict)(**self.mu_dict).trig_reduce().simplify() #set dictionary values
            print "value is:"
            print self._value
            for item in self._input:
                self._result[self._k,0]=self._value(theta_4=item[0],f_pre=item[1]).n()
                self._k=self._k+1
        else:
            print "invalid parameters. Please select from"
            print "theta_1, mu"
            print "theta_2, mu" 
            print "theta_3, mu" 
            print "theta_4, mu" 
            print "theta_23, mu" 
            print "theta_1, f_pre"
            print "theta_2, f_pre" 
            print "theta_3, f_pre" 
            print "theta_4, f_pre" 
            
        self._result=Matrix(self._points_x,self._points_y,self._result.list()) #Shape a nice matrix from the results
        return self._vx.n(),self._vy.n(),self._result.n() #Give back the two vectors and the Z- Matrix
    def createMatrix(self):
        self._tuples=Matrix(QQ,self._points_x*self._points_y,2) #create 1 dimensional array of right size to speed up
        self._i=0
        self._j=0
        for item in self._vx:
            for subitem in self._vy:
                #_tuples.append([item,subitem])
                self._tuples[self._i,0]=item.n() #replace element by tuple
                self._tuples[self._i,1]=subitem.n()
                self._i=self._i+1
## End of createMatrix

    def createVector(self,_start,_end,_points):
        _vector=vector(range(_points))*(_end-_start)/(_points-1) #build vector and adjust stepsize
        _vector=_vector+_start*vector((identity_matrix(QQ,_points)).diagonal()) #shift vector to initial value. Dirty trick for unit vector     
        return _vector

#    def __del__(self): # Destructor
#        class_name = self.__class__.__name__
#        print class_name, "Plot Data destroyed"   
         
         
         
#######################################################################################
# Works like a charm
#######################################################################################

#### Calculate all the values we need and export it.

### Routine to save the files
import scipy.io as sio #We need that to export .mat files for external plotting
#import numpy as np

class saveToMat(object):
    def __init__ (self,_valuename,_value):        
#        self._path="/home/grebi/sagedata/"
        self._path="./"
        self._xFilename= self._path+_valuename+"X"+".mat"
        self._yFilename= self._path+_valuename+"Y"+".mat"
        self._zFilename= self._path+_valuename+"Z"+".mat"
        self._X=self.makeList(_value[0]) # eval uses not the string but the variable itself 
        self._Y=self.makeList(_value[1])
        self._Z=self.makeArray(_value[2])
        self.write(self._X,self._xFilename,"x")
        self.write(self._Y,self._yFilename,"y")
        self.write(self._Z,self._zFilename,"z")
    def makeArray(self,_value):
        _value=np.array(_value.transpose()) #convert to a numpy array
        return _value
    def makeList(self,_value):
        _value=_value.list()
        return _value
    def write(self,_value,_filename,_tag): # write a file containing X coordinates 
        sio.savemat(_filename,{_tag:_value})  
    def __del__(self):
        class_name = self.__class__.__name__
        print class_name, "destroyed"      

var("x,y,z")


#print "mu_enlacement"
#for item in  mu_enlacement:
#    print item(**param_dict).trig_reduce().simplify()

#print "##################################################"
#print "tau_b"
#for item in tau_b:
#    print item(**param_dict).trig_reduce().simplify()
#    print "---------"
#print "##################################################"
#print "tau_fr_tendon"
#for item in tau_fr_tendon:
#    print item(**param_dict).trig_reduce().simplify()
#    print "---------"
#print "##################################################"
#print "tau_fr_N"
#for item in tau_fr_N:
#    print item(**param_dict).trig_reduce().simplify()
#    print "---------"

## Plot resolutions
res_x=20
res_y=51
res_mu=res_y
## dependency of friction coefficient and angles
theta1mu=plotData("theta_1",theta1min,theta1max,res_x,"mu",0.1,0.3,res_mu)
theta2mu=plotData("theta_2",theta2min,theta2max,res_x,"mu",0.1,0.3,res_mu)
theta3mu=plotData("theta_3",theta3min,theta3max,res_x,"mu",0.1,0.3,res_mu)
theta4mu=plotData("theta_4",theta4min,theta4max,res_x,"mu",0.1,0.3,res_mu)
theta23mu=plotData("theta_23",theta2min,theta2max,res_x,"mu",0.1,0.3,res_mu)
theta2WCmu=plotData("theta_2wc",theta1min,theta1max,res_x,"mu",0.1,0.3,res_mu)
theta3WCmu=plotData("theta_3wc",theta1min,theta1max,res_x,"mu",0.1,0.3,res_mu)

muValues=[
    ("theta_1",theta1min,theta1max,res_x,"mu",0.1,0.3,res_mu),
    ("theta_2",theta2min,theta2max,res_x,"mu",0.1,0.3,res_mu),
    ("theta_3",theta3min,theta3max,res_x,"mu",0.1,0.3,res_mu),
    ("theta_4",theta4min,theta4max,res_x,"mu",0.1,0.3,res_mu),
    ]

muWCValues=[
#["theta_23",theta2min,theta2max,res_x,"mu",0.1,0.3,res_mu),
    ("theta_2wc",theta1min,theta1max,res_x,"mu",0.1,0.3,res_mu),
    ("theta_3wc",theta1min,theta1max,res_x,"mu",0.1,0.3,res_mu)
    ]


fPreValues=[
    ("theta_1",theta1min,theta1max,res_x,"f_pre",0,100,res_y),
    ("theta_2",theta2min,theta2max,res_x,"f_pre",0,100,res_y),
    ("theta_3",theta3min,theta3max,res_x,"f_pre",0,100,res_y),
    ("theta_4",theta4min,theta4max,res_x,"f_pre",0,100,res_y)
    ]

fPreWCValues=[
    ("theta_23",theta2min,theta2max,res_x,"f_pre",0,100,res_y),
    ("theta_1wc",theta1min,theta1max,res_x,"f_pre",0,100,res_y)
    ]


#######################################
##### ENLACEMENT  #####################
#######################################
####### Calculate all joint angle  dependencies that are dendent of mu 


for nJoint in range (4):
    muWCObjectName="theta"+str(nJoint+1)+"WCmu" #n is joint angle number
    muObjectName="theta"+str(nJoint+1)+"mu" #n is joint angle number
    fPreObjectName="theta"+str(nJoint+1)+"WCmu" #n is joint angle number
    

    print "calculating joint: '%s'"%nJoint
#    MuObject=plotData(*(muValues[nJoint]))
#    fPreObject=plotData(*(fPreValues[nJoint]))
#    if nJoint in range(2,4):
#        MuWCObject=plotData(*(muWCValues[nJoint]))
#        fPreWCObject=plotData(*(fPreWCValues[nJoint]))

####    calculate and save mu_enlacement data
    for i in range (8):
        name="muEnl"+str(nJoint+1)+"t"+str(i+1) #n is joint angle number
        print "name:"
        print name
        __temp=eval(muObjectName).calcValue((mu_enlacement[i])(**param_dict))
        print "calculated __temp"
        saveToMat(name,__temp)
        print "****   succeeded in using object name!"

# Worstcases
    if nJoint in range(2,4): # Calculate only joint angle dependcencies for joints 2 and 3
        for i in range (8):
            name="muEnl"+str(nJoint+1)+"WCt"+str(i+1) #n is joint angle number
            print "name:"
            print name
            saveToMat(name,MuObject.calcValue(mu_enlacement[i](**param_dict))) 
### tau b
    for i in range (4): # only 4 torques
        name="TauB"+str(nJoint+1)+"j"+str(i+1) #n is joint angle number
        objectName="theta"+str(nJoint+1)+"mu" #n is joint angle number
        print "name:"
        print name        
        saveToMat(name,MuObject.calcValue(tau_b[i](**param_dict)))
##### Tendon Friction Torque #################
    for i in range (4): # only 4 torques
        name="frTT"+str(nJoint+1)+"j"+str(i+1) #n is joint angle number
        print "name:"
        print name
        saveToMat(name,MuObject.calcValue(tau_fr_tendon[i](**param_dict)))

    if nJoint in range(2,4): # Calculate only joint angle dependcencies for joints 2 and 3
        for i in range (4):
            name="frTT"+str(nJoint+1)+"WCj"+str(i+1) #n is joint angle number
            objectName="theta"+str(nJoint+1)+"WCmu" #n is joint angle number
            print "name:"
            print name
            saveToMat(name,MuWCObject.calcValue(tau_fr_tendon[i](**param_dict))) 

##########################################################################################################
################ Joint Friction
##########################################################################################################
## Calculate dependency of preload and angles

####    calculate and save enlacement data
    for i in range (4):
        name="frTN"+str(nJoint+1)+"j"+str(i+1) #n is joint angle number
        objectName="theta"+str(nJoint+1)+"f_pre" #n is joint angle number
        print "name:"
        print name
        saveToMat(name,fPreObject.calcValue(tau_fr_N[i](**param_dict)))

### joint friction error normal force
        name="frTNErr"+str(nJoint+1)+"j"+str(i+1) #n is joint angle numbe
        objectName="theta"+str(nJoint+1)+"f_pre" #n is joint angle number
        print "name:"
        print name
        saveToMat(name,fPreObject.calcValue(tau_fr_N_error[i](**param_dict)))

calculating joint: '0' name: muEnl1t1 value is: -1/e^(mu_t*abs(-0.0555555555556*pi + theta_1)) + 1 ::::::::::::::::::::::debugme:::::::::::::::: -0.523598775598299 0.100000000000000 -1/e^(1/10*abs(-0.0555555555556*pi - 26714619/51021164)) + 1

Traceback (most recent call last): class saveToMat(object): File "", line 1, in <module> File "/tmp/tmpUVwm_R/___code___.py", line 120, in <module> __temp=eval(muObjectName).calcValue((mu_enlacement[i])(**param_dict)) File "/tmp/tmpe3l3WH/___code___.py", line 97, in calcValue self._result[self._k,_sage_const_0 ]=self._value(theta_1=item[_sage_const_0 ],mu_t=item[_sage_const_1 ],mu_j=item[_sage_const_1 ]).n() File "expression.pyx", line 4053, in sage.symbolic.expression.Expression._numerical_approx (sage/symbolic/expression.cpp:17894) TypeError: cannot evaluate symbolic expression numerically

print (-1/e^(1/10*abs(-0.0555555555556*pi - 26714619/51021164)) + 1).n()

0.0674319647572598

########################
# plots starting here
########################

import matplotlib.pyplot as plt #use matplotlib without braking all sage stuff

imagepath="/home_local/grebenst/images/"
imagetype=".svg"
############Enlacement friction 
# Theta 1 Dependency
plt.figure() #instantiate figure
plt.plot(muEnl1t1[0].list(),muEnl1t1[2].column(0).list(),label="MC 1",c='black')
plt.plot(muEnl1t2[0].list(),muEnl1t2[2].column(0).list(),label="MC 2",c='black',ls='-.')
plt.plot(muEnl1t3[0].list(),muEnl1t3[2].column(0).list(),label="MC 3",c='grey')
plt.plot(muEnl1t4[0].list(),muEnl1t4[2].column(0).list(),label="MC 4",c='grey',ls='-.')
plt.plot(muEnl1t5[0].list(),muEnl1t5[2].column(0).list(),label="flexor PIP",c='r')
plt.plot(muEnl1t6[0].list(),muEnl1t6[2].column(0).list(),label="extensor PIP",c='g')
plt.plot(muEnl1t7[0].list(),muEnl1t7[2].column(0).list(),label="flexor DIP",c='b')
plt.plot(muEnl1t8[0].list(),muEnl1t8[2].column(0).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction coefficient over $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()


plt.savefig(imagepath + 'theta1mu' + imagetype)

# Theta 2 Dependency
plt.figure() #instantiate figure
plt.plot(muEnl2t1[0].list(),muEnl2t1[2].column(0).list(),label="flexor PIP",c='r')
plt.plot(muEnl2t2[0].list(),muEnl2t2[2].column(0).list(),label="extensor PIP",c='g')
plt.plot(muEnl2t3[0].list(),muEnl2t3[2].column(0).list(),label="flexor DIP",c='b')
plt.plot(muEnl2t4[0].list(),muEnl2t4[2].column(0).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction coefficient over $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta2mu' + imagetype)


# Theta 3 Dependency
plt.figure() #instantiate figure
plt.plot(muEnl3t5[0].list(),muEnl3t5[2].column(0).list(),label="flexor PIP",c='r')
plt.plot(muEnl3t5[0].list(),muEnl3t6[2].column(0).list(),label="extensor PIP",c='g')
plt.plot(muEnl3t5[0].list(),muEnl3t7[2].column(0).list(),label="flexor DIP",c='b')
plt.plot(muEnl3t5[0].list(),muEnl3t8[2].column(0).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction coefficient over $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta3mu' + imagetype)


# Theta 2 Worstcase Dependency

# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure
plt.plot(muEnl2WCt5[0].list(),muEnl2WCt5[2].column(0).list(),label="flexor PIP",c='r')
plt.plot(muEnl2WCt6[0].list(),muEnl2WCt6[2].column(0).list(),label="extensor PIP",c='g')
plt.plot(muEnl2WCt7[0].list(),muEnl2WCt7[2].column(0).list(),label="flexor DIP",c='b')
plt.plot(muEnl2WCt8[0].list(),muEnl2WCt8[2].column(0).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction coefficient over $\\theta_2$ at $\\mu=0.1$ (worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta2WorstCaseMu' + imagetype)

# Theta 3 Dependency Worstcase
### DIP
# Flex DIP maximum enlacement at: theta_1= 30, theta_2=theta2max theta_3=theta3max
# Ext DIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!) theta_3=theta3min
plt.figure() #instantiate figure
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt5[2].column(0).list(),label="flexor PIP",c='r')
plt.plot(muEnl3WCt6[0].list(),muEnl3WCt6[2].column(0).list(),label="extensor PIP",c='g')
plt.plot(muEnl3WCt7[0].list(),muEnl3WCt7[2].column(0).list(),label="flexor DIP",c='b')
plt.plot(muEnl3WCt8[0].list(),muEnl3WCt8[2].column(0).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction coefficient over $\\theta_3$ at $\\mu=0.1$ (worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta3WorstCaseMu' + imagetype)
 
# Theta 3 Dependency Worstcase
### DIP
# Flex DIP maximum enlacement at: theta_1= 30, theta_2=theta2max theta_3=theta3max
# Ext DIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!) theta_3=theta3min
plt.figure() #instantiate figure
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt5[2].column(0).list(),label="flexor PIP  ($\\mu=0.1$)",c='r')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt6[2].column(0).list(),label="extensor PIP ($\\mu=0.1$)",c='g')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt7[2].column(0).list(),label="flexor DIP ($\\mu=0.1$)",c='b')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt8[2].column(0).list(),label="extensor DIP ($\\mu=0.1$)",c='y')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt5[2].column(1).list(),c='r')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt6[2].column(1).list(),c='g')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt7[2].column(1).list(),c='b')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt8[2].column(1).list(),c='y')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt5[2].column(2).list(),c='r')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt6[2].column(2).list(),c='g')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt7[2].column(2).list(),c='b')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt8[2].column(2).list(),c='y')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt5[2].column(3).list(),c='r')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt6[2].column(3).list(),c='g')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt7[2].column(3).list(),c='b')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt8[2].column(3).list(),c='y')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt5[2].column(4).list(),c='r')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt6[2].column(4).list(),c='g')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt7[2].column(4).list(),c='b')
plt.plot(muEnl3WCt5[0].list(),muEnl3WCt8[2].column(4).list(),c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction over $\\theta_3$ at $\\mu=0.1,0.15,0.2,0.25,0.3 $ worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta3WorstCasMuVar' + imagetype)

##################################
#### Tendon Friction torques ############
##################################

plt.figure() #instantiate figure
plt.plot(frTT1j1[0].list(),frTT1j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTT1j2[0].list(),frTT1j2[2].column(0).list(),label="MC2",c='g')
plt.plot(frTT1j3[0].list(),frTT1j3[2].column(0).list(),label="PIP",c='b')
plt.plot(frTT1j4[0].list(),frTT1j4[2].column(0).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('tendon friction torque over $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()
plt.savefig(imagepath + 'theta1TorqueTendon' + imagetype)


# Theta 2 Dependency
plt.figure() #instantiate figure
plt.plot(frTT2j1[0].list(),frTT2j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTT2j2[0].list(),frTT2j2[2].column(0).list(),label="MC2",c='g')
plt.plot(frTT2j3[0].list(),frTT2j3[2].column(0).list(),label="PIP",c='b')
plt.plot(frTT2j4[0].list(),frTT2j4[2].column(0).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('tendon friction torque over $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()
plt.savefig(imagepath + 'theta2torqueTendon' + imagetype)



# Theta 3 Dependency
plt.figure() #instantiate figure
plt.plot(frTT3j1[0].list(),frTT3j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTT3j2[0].list(),frTT3j2[2].column(0).list(),label="MC2",c='g')
plt.plot(frTT3j3[0].list(),frTT3j3[2].column(0).list(),label="PIP",c='b')
plt.plot(frTT3j4[0].list(),frTT3j4[2].column(0).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('tendon friction torque over $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta3torqueTendon' + imagetype)

# Theta 4

# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure
plt.plot(frTT4j1[0].list(),frTT4j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTT4j2[0].list(),frTT4j2[2].column(0).list(),label="MC2",c='g')
plt.plot(frTT4j3[0].list(),frTT4j3[2].column(0).list(),label="PIP",c='b')
plt.plot(frTT4j4[0].list(),frTT4j4[2].column(0).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('tendon friction torque over $\\theta_4$ at $\\mu=0.1$ (worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta4TorqueTendon' + imagetype)

##################################
#### Joint Friction torques ############
##################################

plt.figure() #instantiate figure
plt.plot(frTN1j1[0].list(),frTN1j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTN1j2[0].list(),frTN1j2[2].column(1).list(),label="MC2",c='g')
plt.plot(frTN1j3[0].list(),frTN1j3[2].column(2).list(),label="PIP",c='b')
plt.plot(frTN1j4[0].list(),frTN1j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('joint friction torque over $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()
plt.savefig(imagepath + 'theta1TorqueJointFriction' + imagetype)


# Theta 2 Dependency
plt.figure() #instantiate figure
plt.plot(frTN2j1[0].list(),frTN2j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTN2j2[0].list(),frTN2j2[2].column(1).list(),label="MC2",c='g')
plt.plot(frTN2j3[0].list(),frTN2j3[2].column(2).list(),label="PIP",c='b')
plt.plot(frTN2j4[0].list(),frTN2j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('joint friction torque over $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()
plt.savefig(imagepath + 'theta2TorqueJointFriction' + imagetype)



# Theta 3 Dependency
plt.figure() #instantiate figure
plt.plot(frTN3j1[0].list(),frTN3j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTN3j2[0].list(),frTN3j2[2].column(1).list(),label="MC2",c='g')
plt.plot(frTN3j3[0].list(),frTN3j3[2].column(2).list(),label="PIP",c='b')
plt.plot(frTN3j4[0].list(),frTN3j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('joint friction torque over $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta3TorqueJointFriction' + imagetype)

# Theta 4

# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure
plt.plot(frTN4j1[0].list(),frTN4j1[2].column(0).list(),label="MC1",c='r')
plt.plot(frTN4j2[0].list(),frTN4j2[2].column(1).list(),label="MC2",c='g')
plt.plot(frTN4j3[0].list(),frTN4j3[2].column(2).list(),label="PIP",c='b')
plt.plot(frTN4j4[0].list(),frTN4j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('joint friction torque over $\\theta_4$ at $\\mu=0.1$ (worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta1TorqueJointFrictiion' + imagetype)



##################################
#### Desired Joint Torques ############
##################################

plt.figure() #instantiate figure
plt.plot(TauB1j1[0].list(),TauB1j1[2].column(0).list(),label="MC1",c='r')
plt.plot(TauB1j2[0].list(),TauB1j2[2].column(1).list(),label="MC2",c='g')
plt.plot(TauB1j3[0].list(),TauB1j3[2].column(2).list(),label="PIP",c='b')
plt.plot(TauB1j4[0].list(),TauB1j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('desired joint torque over $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()
plt.savefig(imagepath + 'theta1TorqueDesired' + imagetype)


# Theta 2 Dependency
plt.figure() #instantiate figure
plt.plot(TauB2j1[0].list(),TauB2j1[2].column(0).list(),label="MC1",c='r')
plt.plot(TauB2j2[0].list(),TauB2j2[2].column(1).list(),label="MC2",c='g')
plt.plot(TauB2j3[0].list(),TauB2j3[2].column(2).list(),label="PIP",c='b')
plt.plot(TauB2j4[0].list(),TauB2j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('desired joint torque over $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()
plt.savefig(imagepath + 'theta2TorqueDesired' + imagetype)



# Theta 3 Dependency
plt.figure() #instantiate figure
plt.plot(TauB3j1[0].list(),TauB3j1[2].column(0).list(),label="MC1",c='r')
plt.plot(TauB3j2[0].list(),TauB3j2[2].column(1).list(),label="MC2",c='g')
plt.plot(TauB3j3[0].list(),TauB3j3[2].column(2).list(),label="PIP",c='b')
plt.plot(TauB3j4[0].list(),TauB3j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('desired joint torque over $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta3TorqueDesired' + imagetype)

# Theta 4

# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure
plt.plot(TauB4j1[0].list(),TauB4j1[2].column(0).list(),label="MC1",c='r')
plt.plot(TauB4j2[0].list(),TauB4j2[2].column(1).list(),label="MC2",c='g')
plt.plot(TauB4j3[0].list(),TauB4j3[2].column(2).list(),label="PIP",c='b')
plt.plot(TauB4j4[0].list(),TauB4j4[2].column(3).list(),label="DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('desired joint torque over $\\theta_4$ at $\\mu=0.1$ (worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + 'theta1TorqueDesired' + imagetype)

##################################################
##################################################
##################################################
##################################################
##################################################
##################################################
##### Kopierte Dinge

###################################

####### Worstcase Szenarios

### PIP
# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure

plt.plot(tauFrT5data.row(5).list(),tauFrT5data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauFrT5data.row(7).list(),tauFrT5data.row(6).list(),label="$\\tau_3$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon friction over $\\theta_2$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + 'b5_th5torque' + imagetype)

### DIP
# Flex DIP maximum enlacement at: theta_1= 30, theta_2=theta2max theta_3=theta3max

# Ext DIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!) theta_3=theta3min

plt.figure() #instantiate figure

plt.plot(tauFrT6data.row(5).list(),tauFrT6data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauFrT6data.row(7).list(),tauFrT6data.row(6).list(),label="$\\tau_4$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon friction over $\\theta_3$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/b7_th6torque' + imagetype)

#### Dependency of friction coefficient. Only interesting for DIP and PIP tendons in worstcase szenario
plt.figure() #instantiate figure
plt.plot(tauFrT6data.row(5).list(),tauFrT6data.row(4).list(),label="$\\tau_3 (\\mu=0.1$)",c='r')
plt.plot(tauFrT6data.row(7).list(),tauFrT6data.row(6).list(),label="$\\tau_4 (\\mu=0.1$)",c='r',ls='-.')

plt.plot(tauFrT6data15.row(5).list(),tauFrT6data15.row(4).list(),label="$\\tau_3 (\\mu=0.15$)",c='b')
plt.plot(tauFrT6data15.row(7).list(),tauFrT6data15.row(6).list(),label="$\\tau_4 (\\mu=0.15$)",c='b',ls='-.')

plt.plot(tauFrT6data2.row(5).list(),tauFrT6data2.row(4).list(),label="$\\tau_3 (\\mu=0.2$)" ,c='g')
plt.plot(tauFrT6data2.row(7).list(),tauFrT6data2.row(6).list(),label="$\\tau_4 (\\mu=0.2$)" ,c='g',ls='-.')

plt.plot(tauFrT6data25.row(5).list(),tauFrT6data25.row(4).list(),label="$\\tau_3 (\\mu=0.25$)",c='y')
plt.plot(tauFrT6data25.row(7).list(),tauFrT6data25.row(6).list(),label="$\\tau_4 (\\mu=0.25$)" ,c='y',ls='-.')

plt.plot(tauFrT6data3.row(5).list(),tauFrT6data3.row(4).list(),label="$\\tau_3 (\\mu=0.3$)",c='grey')
plt.plot(tauFrT6data3.row(7).list(),tauFrT6data3.row(6).list(),label="$\\tau_4 (\\mu=0.3$)",c='grey',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon friction over $\\theta_3$ at $\\mu=0.1,0.15,0.2,0.25,0.3 $ worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/b8_thtorquevar' + imagetype)


#########################
# Torque Error
##########################

# Theta 1 Dependency

plt.figure() #instantiate figure
plt.plot(tauErrT1data.row(1).list(),tauErrT1data.row(0).list(),label="$\\tau_1$",c='r')
plt.plot(tauErrT1data.row(3).list(),tauErrT1data.row(2).list(),label="$\\tau_2$",c='g',ls='-.')
plt.plot(tauErrT1data.row(5).list(),tauErrT1data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT1data.row(7).list(),tauErrT1data.row(6).list(),label="$\\tau_4$",c='black',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/c1_th1torque' + imagetype)


# Theta 2 Dependency

plt.figure() #instantiate figure

plt.plot(tauErrT2data.row(1).list(),tauErrT2data.row(0).list(),label="$\\tau_1$",c='r')
plt.plot(tauErrT2data.row(3).list(),tauErrT2data.row(2).list(),label="$\\tau_2$",c='g')
plt.plot(tauErrT2data.row(5).list(),tauErrT2data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT2data.row(7).list(),tauErrT2data.row(6).list(),label="$\\tau_4$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon frictionover $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/c2_th2torque' + imagetype)


# Theta 3 Dependency

plt.figure() #instantiate figure

plt.plot(tauErrT3data.row(5).list(),tauErrT3data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT3data.row(7).list(),tauErrT3data.row(6).list(),label="$\\tau_3$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon frictionover $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/c3_th3torque' + imagetype)

# Theta 4 Dependency

plt.figure() #instantiate figure

plt.plot(tauErrT4data.row(5).list(),tauErrT4data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT4data.row(7).list(),tauErrT4data.row(6).list(),label="$\\tau_4$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon frictiono ver $\\theta_2=\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/c4_th23mu' + imagetype)

####### Worstcase Szenarios

### PIP
# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure

plt.plot(tauErrT5data.row(5).list(),tauErrT5data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauErrT5data.row(7).list(),tauErrT5data.row(6).list(),label="$\\tau_3$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_2$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/c5_th5torque' + imagetype)

### DIP
# Flex DIP maximum enlacement at: theta_1= 30, theta_2=theta2max theta_3=theta3max

# Ext DIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!) theta_3=theta3min

plt.figure() #instantiate figure

plt.plot(tauErrT6data.row(5).list(),tauErrT6data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauErrT6data.row(7).list(),tauErrT6data.row(6).list(),label="$\\tau_4$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_3$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/c7_th6torque' + imagetype)

#### Dependency of friction coefficient. Only interesting for DIP and PIP tendons in worstcase szenario
plt.figure() #instantiate figure
plt.plot(tauErrT6data.row(5).list(),tauErrT6data.row(4).list(),label="$\\tau_3 (\\mu=0.1$)",c='r')
plt.plot(tauErrT6data.row(7).list(),tauErrT6data.row(6).list(),label="$\\tau_4 (\\mu=0.1$)",c='r',ls='-.')

plt.plot(tauErrT6data15.row(5).list(),tauErrT6data15.row(4).list(),label="$\\tau_3 (\\mu=0.15$)",c='b')
plt.plot(tauErrT6data15.row(7).list(),tauErrT6data15.row(6).list(),label="$\\tau_4 (\\mu=0.15$)",c='b',ls='-.')

plt.plot(tauErrT6data2.row(5).list(),tauErrT6data2.row(4).list(),label="$\\tau_3 (\\mu=0.2$)" ,c='g')
plt.plot(tauErrT6data2.row(7).list(),tauErrT6data2.row(6).list(),label="$\\tau_4 (\\mu=0.2$)" ,c='g',ls='-.')

plt.plot(tauErrT6data25.row(5).list(),tauErrT6data25.row(4).list(),label="$\\tau_3 (\\mu=0.25$)",c='y')
plt.plot(tauErrT6data25.row(7).list(),tauErrT6data25.row(6).list(),label="$\\tau_4 (\\mu=0.25$)" ,c='y',ls='-.')

plt.plot(tauErrT6data3.row(5).list(),tauErrT6data3.row(4).list(),label="$\\tau_3 (\\mu=0.3$)",c='grey')
plt.plot(tauErrT6data3.row(7).list(),tauErrT6data3.row(6).list(),label="$\\tau_4 (\\mu=0.3$)",c='grey',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_3$ at $\\mu=0.1,0.15,0.2,0.25,0.3 $ worstcase')
plt.legend()
plt.show()

plt.savefig(imagepath + '/home_local/grebenst/c8_thtorquevar' + imagetype)

Traceback (most recent call last): imagepath="/home_local/grebenst/images/" File "", line 1, in <module> File "/tmp/tmp6e3xaG/___code___.py", line 345, in <module> plt.plot(tauFrT5data.row(_sage_const_5 ).list(),tauFrT5data.row(_sage_const_4 ).list(),label="$\\tau_3$",c='r') NameError: name 'tauFrT5data' is not defined

plt.figure()
plt.plot(muEnl1[2].row(30))
plt.show()
plt.savefig("asdssf")

Traceback (most recent call last): File "<stdin>", line 1, in <module> File "_sage_input_96.py", line 10, in <module> exec compile(u'open("___code___.py","w").write("# -*- coding: utf-8 -*-\\n" + _support_.preparse_worksheet_cell(base64.b64decode("cGx0LmZpZ3VyZSgpCnBsdC5wbG90KG11RW5sMVsyXS5yb3coMzApKQpwbHQuc2hvdygpCnBsdC5zYXZlZmlnKCJhc2Rzc2YiKQ=="),globals())+"\\n"); execfile(os.path.abspath("___code___.py"))' + '\n', '', 'single') File "", line 1, in <module> File "/tmp/tmpjrE0Wo/___code___.py", line 4, in <module> plt.plot(muEnl1[_sage_const_2 ].row(_sage_const_30 )) File "matrix1.pyx", line 971, in sage.matrix.matrix1.Matrix.row (sage/matrix/matrix1.c:7275) IndexError: row index out of range

print muEnl1[2].column(0)
print "********"
print muEnl1[0]

(24963200/370198319, 104071777/1671087775, 23157137/405587608, 23061319/444478022, 26009970/557627143, 10991449/265654191, 12121631/335994122, 9532833/310015664, 27052127/1065351238, 15311608/765341581, 12011285/823253029, 4765247/521140500, 9685085/2640689358, 5202201/2834211431, 4570627/624247500, 29374166/2298811351, 6906098/379369377, 5840302/247465177, 34551244/1192771779, 33278901/970106540) ******** (-1/6*pi, -17/114*pi, -5/38*pi, -13/114*pi, -11/114*pi, -3/38*pi, -7/114*pi, -5/114*pi, -1/38*pi, -1/114*pi, 1/114*pi, 1/38*pi, 5/114*pi, 7/114*pi, 3/38*pi, 11/114*pi, 13/114*pi, 5/38*pi, 17/114*pi, 1/6*pi)

####### Old Plotting Stuff for Debugging and comparison


import numpy as ny #use numpy to create the data arrays Syntax:linspace(a,b,c) (last number is number of steps)

class plotdata:
#    var("_mu_j,_mu_t")
    def __init__ (self,_mu_t,_mu_j):
        
        self.th1_dict={
            "theta_2":0,
            "theta_3":0,
            "theta_4":0
            }
        self.th2_dict={
            "theta_1":0,
            "theta_3":0,
            "theta_4":0
            }
        self.th3_dict={
            "theta_1":0,
            "theta_2":0,
            "theta_4":0
            }
        self.th23_dict={
            "theta_1":0,
            "theta_4":0
            }
        self.th5_dict={
            "theta_1":theta1max,
            "theta_3":0,
            "theta_4":0
            }
        self.th6_dict={
            "theta_1":30*deg,
            "theta_2":theta2max,
            "theta_4":0
            }
        self.mu_dict={
            "mu_t":_mu_t, #friction coefficient of tendon
            "mu_j":_mu_j #friction coefficient of joint surfaces
            }
        self.pre_dict={ #set pretension values to some value for calculations. Do not use for joint torques"
            "f_pre_MC13": 10,
            "f_pre_MC24": 10,
            "f_pre_PIP56": 10,
            "f_pre_DIP78": 10
            }
        print "instance created with parameters:"
        print (_mu_t,_mu_j)
        
    def muEnl(self,_plotcase,_points): #Calculate the enlacement friction coefficient
        self._plotcase=_plotcase
        self._points=_points
        self._func=Matrix(RR,16,self._points) #Create Matrix for values. 2 rows per tendon
                                              # x values are in odd rows
        for i in range(8): # Calculate for all 8 tendons
 #           print "Run number:"
 #           print i+1
            if self._plotcase == 1:  #Theta 1 is varied
                self.data = ny.linspace(theta1min,theta1max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=mu_enlacement(**param_dict)(**self.th1_dict)(**self.mu_dict)[i](theta_1=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1
                
            if self._plotcase == 2: #Theta 2 is varied
                self.data = ny.linspace(theta2min,theta2max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=mu_enlacement(**param_dict)(**self.th2_dict)(**self.mu_dict)[i](theta_2=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 3: #Theta 3 is varied
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=mu_enlacement(**param_dict)(**self.th3_dict)(**self.mu_dict)[i](theta_3=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 4:
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=mu_enlacement(**param_dict)(**self.th23_dict)(**self.mu_dict)[i](theta_2=item,theta_3=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 5:  #Worstcase PIP tendons
                self.data = ny.linspace(theta2min,theta2max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=mu_enlacement(**param_dict)(**self.th5_dict)(**self.mu_dict)[i](theta_2=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 6: #Worstcase DIP tendons
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=mu_enlacement(**param_dict)(**self.th6_dict)(**self.mu_dict)[i](theta_3=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1
        return self._func
                    

    def tauFrT(self,_plotcase,_points): #tendon friction torque
        self._plotcase=_plotcase
        self._points=_points
        self._func=Matrix(RR,16,self._points) #Create Matrix for values. 2 rows per tendon
                                              # x values are in odd rows
        for i in range(4): # Calculate for all 8 tendons
 #           print "Run number:"
 #           print i+1
            if self._plotcase == 1:  #Theta 1 is varied
                self.data = ny.linspace(theta1min,theta1max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th1_dict)(**self.mu_dict)[i](theta_1=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1
                
            if self._plotcase == 2: #Theta 2 is varied
                self.data = ny.linspace(theta2min,theta2max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th2_dict)(**self.mu_dict)[i](theta_2=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 3: #Theta 3 is varied
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th3_dict)(**self.mu_dict)[i](theta_3=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 4:
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th23_dict)(**self.mu_dict)[i](theta_2=item,theta_3=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 5:  #Worstcase PIP tendons
                self.data = ny.linspace(theta2min,theta2max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th5_dict)(**self.mu_dict)[i](theta_2=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 6: #Worstcase DIP tendons
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th6_dict)(**self.mu_dict)[i](theta_3=item).n()
                    self._func[2*i+1,j]=item
                    j=j+1
        return self._func
  
    def tauErrT(self,_plotcase,_points): #tendon friction torque related to desired torque
        self._plotcase=_plotcase
        self._points=_points
        self._func=Matrix(RR,16,self._points) #Create Matrix for values. 2 rows per tendon
                                              # x values are in odd rows
        for i in range(4): # Calculate for all 4 joints
 #           print "Run number:"
 #           print i+1
            if self._plotcase == 1:  #Theta 1 is varied
                self.data = ny.linspace(theta1min,theta1max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=(tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th1_dict)(**self.mu_dict)[i](theta_1=item).n()/
                                       tau_b(**param_dict)(**self.pre_dict)(**self.th1_dict)(**self.mu_dict)[i](theta_1=item).n())
                    self._func[2*i+1,j]=item
                    j=j+1
                
            if self._plotcase == 2: #Theta 2 is varied
                self.data = ny.linspace(theta2min,theta2max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=(tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th2_dict)(**self.mu_dict)[i](theta_2=item).n()/
                                       tau_b(**param_dict)(**self.pre_dict)(**self.th2_dict)(**self.mu_dict)[i](theta_2=item).n())
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 3: #Theta 3 is varied
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=(tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th3_dict)(**self.mu_dict)[i](theta_3=item).n()/
                                       tau_b(**param_dict)(**self.pre_dict)(**self.th3_dict)(**self.mu_dict)[i](theta_3=item).n())
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 4:
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=(tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th23_dict)(**self.mu_dict)[i](theta_2=item,theta_3=item).n()/
                                       tau_b(**param_dict)(**self.pre_dict)(**self.th23_dict)(**self.mu_dict)[i](theta_2=item,theta_3=item).n())
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 5:  #Worstcase PIP tendons
                self.data = ny.linspace(theta2min,theta2max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=(tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th5_dict)(**self.mu_dict)[i](theta_2=item).n()/
                                       tau_b(**param_dict)(**self.pre_dict)(**self.th5_dict)(**self.mu_dict)[i](theta_2=item).n())
                    self._func[2*i+1,j]=item
                    j=j+1

            if self._plotcase == 6: #Worstcase DIP tendons
                self.data = ny.linspace(theta3min,theta3max,self._points)
                j=0
                for item in self.data:
                    self._func[2*i,j]=(tau_fr_tendon(**param_dict)(**self.pre_dict)(**self.th6_dict)(**self.mu_dict)[i](theta_3=item).n()/
                                       tau_b(**param_dict)(**self.pre_dict)(**self.th6_dict)(**self.mu_dict)[i](theta_3=item).n())
                    self._func[2*i+1,j]=item
                    j=j+1
        return self._func
# ####Calculate the data needed for plotting

#### Enlacement friction
## Enlacement friction at tendon level
resolution=15

# friction coeficient = 0.1
pre0k1=plotdata(0.1,0.1) #Instantiate an object
muE1data=pre0k1.muEnl(1,resolution)
muE2data=pre0k1.muEnl(2,resolution)
muE3data=pre0k1.muEnl(3,resolution)
muE4data=pre0k1.muEnl(4,resolution)
muE5data=pre0k1.muEnl(5,resolution)
muE6data=pre0k1.muEnl(6,resolution)

# friction coeficient = 0.2. We are only interested in worstcases
pre0k15=plotdata(0.15,0.15) #Instantiate an object
muE5data15=pre0k15.muEnl(5,resolution)
muE6data15=pre0k15.muEnl(6,resolution)

# friction coeficient = 0.2. We are only interested in worstcases
pre0k2=plotdata(0.2,0.2) #Instantiate an object
muE5data2=pre0k2.muEnl(5,resolution)
muE6data2=pre0k2.muEnl(6,resolution)

# friction coeficient = 0.2. We are only interested in worstcases
pre0k25=plotdata(0.25,0.25) #Instantiate an object
muE5data25=pre0k25.muEnl(5,resolution)
muE6data25=pre0k25.muEnl(6,resolution)

# friction coeficient = 0.3  We are only interested in worstcases
pre0k3=plotdata(0.3,0.3) #Instantiate an object
muE5data3=pre0k3.muEnl(5,resolution)
muE6data3=pre0k3.muEnl(6,resolution)

import scipy.io as sio #We need that to export .mat files for external plotting

result=(zip(muE3data.row(15),muE3data.row(14))) #this saves the result to a file
sio.savemat('testdata.mat',{'result':result})


## Resulting friction torque
# The object already exists
tauFrT1data=pre0k1.tauFrT(1,resolution)
tauFrT2data=pre0k1.tauFrT(2,resolution)
tauFrT3data=pre0k1.tauFrT(3,resolution)
tauFrT4data=pre0k1.tauFrT(4,resolution)
tauFrT5data=pre0k1.tauFrT(5,resolution)
tauFrT6data=pre0k1.tauFrT(6,resolution)

tauFrT5data15=pre0k15.tauFrT(5,resolution)
tauFrT6data15=pre0k15.tauFrT(6,resolution)

tauFrT5data2=pre0k2.tauFrT(5,resolution)
tauFrT6data2=pre0k2.tauFrT(6,resolution)

tauFrT5data25=pre0k25.tauFrT(5,resolution)
tauFrT6data25=pre0k25.tauFrT(6,resolution)

tauFrT5data3=pre0k3.tauFrT(5,resolution)
tauFrT6data3=pre0k3.tauFrT(6,resolution)


## Resulting Torque error
# The object already exists
tauErrT1data=pre0k1.tauErrT(1,resolution)
tauErrT2data=pre0k1.tauErrT(2,resolution)
tauErrT3data=pre0k1.tauErrT(3,resolution)
tauErrT4data=pre0k1.tauErrT(4,resolution)
tauErrT5data=pre0k1.tauErrT(5,resolution)
tauErrT6data=pre0k1.tauErrT(6,resolution)

tauErrT5data15=pre0k15.tauErrT(5,resolution)
tauErrT6data15=pre0k15.tauErrT(6,resolution)

tauErrT5data2=pre0k2.tauErrT(5,resolution)
tauErrT6data2=pre0k2.tauErrT(6,resolution)

tauErrT5data25=pre0k25.tauErrT(5,resolution)
tauErrT6data25=pre0k25.tauErrT(6,resolution)

tauErrT5data3=pre0k3.tauErrT(5,resolution)
tauErrT6data3=pre0k3.tauErrT(6,resolution)

#### Joint friction

instance created with parameters: (0.100000000000000, 0.100000000000000) instance created with parameters: (0.150000000000000, 0.150000000000000) instance created with parameters: (0.200000000000000, 0.200000000000000) instance created with parameters: (0.250000000000000, 0.250000000000000) instance created with parameters: (0.300000000000000, 0.300000000000000)

##### 8 #####


#Plot Enlacement stuff

### What do we want to see

## Tendon friction
# Done

# dependencies
# Done

# worstcase values
# Done


## Joint Friction

# Joint Friction dependent of pretension and angle

#should be 3D!

## Overall friction

# Joint torque errors


import matplotlib.pyplot as plt #use matplotlib without braking all sage stuff

# Theta 1 Dependency

plt.figure() #instantiate figure
plt.plot(muE1data.row(1).list(),muE1data.row(0).list(),label="MC 1",c='black')
plt.plot(muE1data.row(3).list(),muE1data.row(2).list(),label="MC 2",c='black',ls='-.')
plt.plot(muE1data.row(5).list(),muE1data.row(4).list(),label="MC 3",c='grey')
plt.plot(muE1data.row(7).list(),muE1data.row(6).list(),label="MC 4",c='grey',ls='-.')
plt.plot(muE1data.row(9).list(),muE1data.row(8).list(),label="flexor PIP",c='r')
plt.plot(muE1data.row(11).list(),muE1data.row(10).list(),label="extensor PIP",c='g')
plt.plot(muE1data.row(13).list(),muE1data.row(12).list(),label="flexor DIP",c='b')
plt.plot(muE1data.row(15).list(),muE1data.row(14).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction coefficient over $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'a1_th1mu.svg')


# Theta 2 Dependency

plt.figure() #instantiate figure

plt.plot(muE2data.row(9).list(),muE2data.row(8).list(),label="flexor PIP",c='r')
plt.plot(muE2data.row(11).list(),muE2data.row(10).list(),label="extensor PIP",c='g')
plt.plot(muE2data.row(13).list(),muE2data.row(12).list(),label="flexor DIP",c='b')
plt.plot(muE2data.row(15).list(),muE2data.row(14).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction over $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'a2_th2mu.svg')


# Theta 3 Dependency

plt.figure() #instantiate figure

plt.plot(muE3data.row(13).list(),muE3data.row(12).list(),label="flexor DIP",c='b')
plt.plot(muE3data.row(15).list(),muE3data.row(14).list(),label="extensor DIP",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction over $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'a3_th3mu.svg')

# Theta 4 Dependency

plt.figure() #instantiate figure

plt.plot(muE4data.row(13).list(),muE4data.row(12).list(),label="flexor DIP",c='b')
plt.plot(muE4data.row(15).list(),muE4data.row(14).list(),label="extensor DIP",c='y')
plt.xlabel('$\\alpha [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction over $\\theta_2=\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'a4_th23mu.svg')

####### Worstcase Szenarios

### PIP
# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure

plt.plot(muE5data.row(9).list(),muE5data.row(8).list(),label="flexor PIP",c='r')
plt.plot(muE5data.row(11).list(),muE5data.row(10).list(),label="extensor PIP",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction over $\\theta_2$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig( 'a5_th5mu.svg')

### DIP
# Flex DIP maximum enlacement at: theta_1= 30, theta_2=theta2max theta_3=theta3max

# Ext DIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!) theta_3=theta3min

plt.figure() #instantiate figure

plt.plot(muE6data.row(13).list(),muE6data.row(12).list(),label="flexor DIP",c='r')
plt.plot(muE6data.row(15).list(),muE6data.row(14).list(),label="extensor DIP",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction over $\\theta_3$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig( 'a6_th6mu.svg')

#### Dependency of friction coefficient. Only interesting for DIP and PIP tendons in worstcase szenario
plt.figure() #instantiate figure
plt.plot(muE6data.row(13).list(),muE6data.row(12).list(),label="flexor DIP ($\\mu=0.1$)",c='r')
plt.plot(muE6data.row(15).list(),muE6data.row(14).list(),label="extensor DIP ($\\mu=0.1$)",c='r',ls='-.')

plt.plot(muE6data15.row(13).list(),muE6data15.row(12).list(),label="flexor DIP ($\\mu=0.15$)",c='b')
plt.plot(muE6data15.row(15).list(),muE6data15.row(14).list(),label="extensor DIP ($\\mu=0.15$)",c='b',ls='-.')

plt.plot(muE6data2.row(13).list(),muE6data2.row(12).list(),label="flexor DIP ($\\mu=0.2$)" ,c='g')
plt.plot(muE6data2.row(15).list(),muE6data2.row(14).list(),label="extensor DIP ($\\mu=0.2$)" ,c='g',ls='-.')

plt.plot(muE6data25.row(13).list(),muE6data25.row(12).list(),label="flexor DIP ($\\mu=0.25$)",c='y')
plt.plot(muE6data25.row(15).list(),muE6data25.row(14).list(),label="extensor DIP ($\\mu=0.25$)" ,c='y',ls='-.')

plt.plot(muE6data3.row(13).list(),muE6data3.row(12).list(),label="flexor DIP ($\\mu=0.3$)",c='grey')
plt.plot(muE6data3.row(15).list(),muE6data3.row(14).list(),label="extensor DIP ($\\mu=0.3$)",c='grey',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$\\mu$')
plt.title('friction over $\\theta_3$ at $\\mu=0.1,0.15,0.2,0.25,0.3 $ worstcase')
plt.legend()
plt.show()

plt.savefig( 'a7_thmuvar.svg')

##################################
#### Friction torques ############
##################################
# Theta 1 Dependency

plt.figure() #instantiate figure
plt.plot(tauFrT1data.row(1).list(),tauFrT1data.row(0).list(),label="$\\tau_1$",c='r')
plt.plot(tauFrT1data.row(3).list(),tauFrT1data.row(2).list(),label="$\\tau_2$",c='g',ls='-.')
plt.plot(tauFrT1data.row(5).list(),tauFrT1data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauFrT1data.row(7).list(),tauFrT1data.row(6).list(),label="$\\tau_4$",c='black',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon friction in dependency of $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'b1_th1torque.svg')


# Theta 2 Dependency

plt.figure() #instantiate figure

plt.plot(tauFrT2data.row(1).list(),tauFrT2data.row(0).list(),label="$\\tau_1$",c='r')
plt.plot(tauFrT2data.row(3).list(),tauFrT2data.row(2).list(),label="$\\tau_2$",c='g')
plt.plot(tauFrT2data.row(5).list(),tauFrT2data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauFrT2data.row(7).list(),tauFrT2data.row(6).list(),label="$\\tau_4$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon frictionover $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'b2_th2torque.svg')


# Theta 3 Dependency

plt.figure() #instantiate figure

plt.plot(tauFrT3data.row(5).list(),tauFrT3data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauFrT3data.row(7).list(),tauFrT3data.row(6).list(),label="$\\tau_3$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon frictionover $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'b3_th3torque.svg')

# Theta 4 Dependency

plt.figure() #instantiate figure

plt.plot(tauFrT4data.row(5).list(),tauFrT4data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauFrT4data.row(7).list(),tauFrT4data.row(6).list(),label="$\\tau_4$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon frictiono ver $\\theta_2=\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'b4_th23mu.svg')

####### Worstcase Szenarios

### PIP
# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure

plt.plot(tauFrT5data.row(5).list(),tauFrT5data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauFrT5data.row(7).list(),tauFrT5data.row(6).list(),label="$\\tau_3$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon friction over $\\theta_2$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig( 'b5_th5torque.svg')

### DIP
# Flex DIP maximum enlacement at: theta_1= 30, theta_2=theta2max theta_3=theta3max

# Ext DIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!) theta_3=theta3min

plt.figure() #instantiate figure

plt.plot(tauFrT6data.row(5).list(),tauFrT6data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauFrT6data.row(7).list(),tauFrT6data.row(6).list(),label="$\\tau_4$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon friction over $\\theta_3$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig( 'b7_th6torque.svg')

#### Dependency of friction coefficient. Only interesting for DIP and PIP tendons in worstcase szenario
plt.figure() #instantiate figure
plt.plot(tauFrT6data.row(5).list(),tauFrT6data.row(4).list(),label="$\\tau_3 (\\mu=0.1$)",c='r')
plt.plot(tauFrT6data.row(7).list(),tauFrT6data.row(6).list(),label="$\\tau_4 (\\mu=0.1$)",c='r',ls='-.')

plt.plot(tauFrT6data15.row(5).list(),tauFrT6data15.row(4).list(),label="$\\tau_3 (\\mu=0.15$)",c='b')
plt.plot(tauFrT6data15.row(7).list(),tauFrT6data15.row(6).list(),label="$\\tau_4 (\\mu=0.15$)",c='b',ls='-.')

plt.plot(tauFrT6data2.row(5).list(),tauFrT6data2.row(4).list(),label="$\\tau_3 (\\mu=0.2$)" ,c='g')
plt.plot(tauFrT6data2.row(7).list(),tauFrT6data2.row(6).list(),label="$\\tau_4 (\\mu=0.2$)" ,c='g',ls='-.')

plt.plot(tauFrT6data25.row(5).list(),tauFrT6data25.row(4).list(),label="$\\tau_3 (\\mu=0.25$)",c='y')
plt.plot(tauFrT6data25.row(7).list(),tauFrT6data25.row(6).list(),label="$\\tau_4 (\\mu=0.25$)" ,c='y',ls='-.')

plt.plot(tauFrT6data3.row(5).list(),tauFrT6data3.row(4).list(),label="$\\tau_3 (\\mu=0.3$)",c='grey')
plt.plot(tauFrT6data3.row(7).list(),tauFrT6data3.row(6).list(),label="$\\tau_4 (\\mu=0.3$)",c='grey',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Torque [Nmm]$')
plt.title('Torque caused by tendon friction over $\\theta_3$ at $\\mu=0.1,0.15,0.2,0.25,0.3 $ worstcase')
plt.legend()
plt.show()

plt.savefig( 'b8_thtorquevar.svg')


#########################
# Torque Error
##########################

# Theta 1 Dependency

plt.figure() #instantiate figure
plt.plot(tauErrT1data.row(1).list(),tauErrT1data.row(0).list(),label="$\\tau_1$",c='r')
plt.plot(tauErrT1data.row(3).list(),tauErrT1data.row(2).list(),label="$\\tau_2$",c='g',ls='-.')
plt.plot(tauErrT1data.row(5).list(),tauErrT1data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT1data.row(7).list(),tauErrT1data.row(6).list(),label="$\\tau_4$",c='black',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_1$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'c1_th1torque.svg')


# Theta 2 Dependency

plt.figure() #instantiate figure

plt.plot(tauErrT2data.row(1).list(),tauErrT2data.row(0).list(),label="$\\tau_1$",c='r')
plt.plot(tauErrT2data.row(3).list(),tauErrT2data.row(2).list(),label="$\\tau_2$",c='g')
plt.plot(tauErrT2data.row(5).list(),tauErrT2data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT2data.row(7).list(),tauErrT2data.row(6).list(),label="$\\tau_4$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon frictionover $\\theta_2$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'c2_th2torque.svg')


# Theta 3 Dependency

plt.figure() #instantiate figure

plt.plot(tauErrT3data.row(5).list(),tauErrT3data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT3data.row(7).list(),tauErrT3data.row(6).list(),label="$\\tau_3$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon frictionover $\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'c3_th3torque.svg')

# Theta 4 Dependency

plt.figure() #instantiate figure

plt.plot(tauErrT4data.row(5).list(),tauErrT4data.row(4).list(),label="$\\tau_3$",c='b')
plt.plot(tauErrT4data.row(7).list(),tauErrT4data.row(6).list(),label="$\\tau_4$",c='y')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon frictiono ver $\\theta_2=\\theta_3$ at $\\mu=0.1$')
plt.legend()
plt.show()

plt.savefig( 'c4_th23mu.svg')

####### Worstcase Szenarios

### PIP
# Flex PIP maximum enlacement at: theta_1= 30, theta_2=theta2max
# Identical Values. Therefore only one plot
# Ext PIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!)

plt.figure() #instantiate figure

plt.plot(tauErrT5data.row(5).list(),tauErrT5data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauErrT5data.row(7).list(),tauErrT5data.row(6).list(),label="$\\tau_3$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_2$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig( 'c5_th5torque.svg')

### DIP
# Flex DIP maximum enlacement at: theta_1= 30, theta_2=theta2max theta_3=theta3max

# Ext DIP maximum enlacement at: theta_1= 30, theta_2=theta2max (range of motion at MC2 is asymmetric!) theta_3=theta3min

plt.figure() #instantiate figure

plt.plot(tauErrT6data.row(5).list(),tauErrT6data.row(4).list(),label="$\\tau_3$",c='r')
plt.plot(tauErrT6data.row(7).list(),tauErrT6data.row(6).list(),label="$\\tau_4$",c='g')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_3$ at $\\mu=0.1$ worstcase')
plt.legend()
plt.show()

plt.savefig( 'c7_th6torque.svg')

#### Dependency of friction coefficient. Only interesting for DIP and PIP tendons in worstcase szenario
plt.figure() #instantiate figure
plt.plot(tauErrT6data.row(5).list(),tauErrT6data.row(4).list(),label="$\\tau_3 (\\mu=0.1$)",c='r')
plt.plot(tauErrT6data.row(7).list(),tauErrT6data.row(6).list(),label="$\\tau_4 (\\mu=0.1$)",c='r',ls='-.')

plt.plot(tauErrT6data15.row(5).list(),tauErrT6data15.row(4).list(),label="$\\tau_3 (\\mu=0.15$)",c='b')
plt.plot(tauErrT6data15.row(7).list(),tauErrT6data15.row(6).list(),label="$\\tau_4 (\\mu=0.15$)",c='b',ls='-.')

plt.plot(tauErrT6data2.row(5).list(),tauErrT6data2.row(4).list(),label="$\\tau_3 (\\mu=0.2$)" ,c='g')
plt.plot(tauErrT6data2.row(7).list(),tauErrT6data2.row(6).list(),label="$\\tau_4 (\\mu=0.2$)" ,c='g',ls='-.')

plt.plot(tauErrT6data25.row(5).list(),tauErrT6data25.row(4).list(),label="$\\tau_3 (\\mu=0.25$)",c='y')
plt.plot(tauErrT6data25.row(7).list(),tauErrT6data25.row(6).list(),label="$\\tau_4 (\\mu=0.25$)" ,c='y',ls='-.')

plt.plot(tauErrT6data3.row(5).list(),tauErrT6data3.row(4).list(),label="$\\tau_3 (\\mu=0.3$)",c='grey')
plt.plot(tauErrT6data3.row(7).list(),tauErrT6data3.row(6).list(),label="$\\tau_4 (\\mu=0.3$)",c='grey',ls='-.')
plt.xlabel('$\\theta [rad]$')
plt.ylabel('$Error $')
plt.title('Torque error caused by tendon friction over $\\theta_3$ at $\\mu=0.1,0.15,0.2,0.25,0.3 $ worstcase')
plt.legend()
plt.show()

plt.savefig( 'c8_thtorquevar.svg')

# Testing of tendin friction plotting

var("f_pre_all")


bsdf_dict={
    "f_pre_MC13":10,
    "f_pre_MC24":10,
#    "f_pre_PIP56":10,
    "f_pre_DIP78":10,
    "theta_1":0,
    "theta_2":0,
    "theta_4":0
    }
    
print "Desired Torque:"
print tau_b(**param_dict)(**bsdf_dict)(theta_3=10,f_pre_PIP56=15,mu_j=0.1).column().n()

print "Friction Torque normal force:"
print tau_fr_N(**param_dict)(**bsdf_dict)(theta_3=10,f_pre_PIP56=15,mu_j=0.1).column().n()

print "Friction Torque tendon force:"
print tau_fr_tendon(**param_dict)(**bsdf_dict)(theta_3=10,f_pre_PIP56=15,mu_j=0.1,mu_t=0.1).column().n()

############# FIXME tendon introduced friction has to sum up !!!!!!! #####################

print "Friction Torque over all:"
print tau_fr(**param_dict)(**bsdf_dict)(theta_3=10,f_pre_PIP56=15,mu_j=0.1,mu_t=0.1).column().n()
plot3d(tau_fr[3](**param_dict)(**bsdf_dict)(mu_j=0.1,mu_t=0.1)/tau_b[3](**param_dict)(**bsdf_dict)(mu_j=0.1),(theta_3,theta3min,theta3max),(f_pre_PIP56,0,150),adaptive=True)

Desired Torque: [0.000000000000000] [ 18.9748944697250] [ 900.000000000000] [ 450.000000000000] Friction Torque normal force: [0.000000000000000] [-396.885447934855] [-80.0282450138699] [-154.905944060097] Friction Torque tendon force: [-6.00331314798648] [-6.00331314798648] [-419.499005322727] [-340.426859564694] Friction Torque over all: [-6.00331314798648] [-402.888761082842] [-499.527250336597] [-495.332803624791]

#### that is a plotting sandbox


from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter
import matplotlib.pyplot as plt

fig = plt.figure()
ax = fig.gca(projection='3d')
X = ny.arange(-5, 5, 0.5)
Y = ny.arange(-5, 5, 0.5)
X, Y = ny.meshgrid(X, Y)
R = ny.sqrt(X**2 + Y**2)
Z = ny.sin(R)
surf = ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.jet,
        linewidth=0, antialiased=False)
#ax.set_zlim(-1.01, 1.01)

#ax.zaxis.set_major_locator(LinearLocator(10))
#ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))

fig.colorbar(surf, shrink=0.5, aspect=5)

plt.show()
plt.savefig( 'muell.svg')


##############################
##### Do that later ##########
##############################

# now my figure

fig2 = plt.figure()
ax2 = fig.gca(projection='3d')
X2 = muE6data25.row(13)
Y2 = muE6data25.row(13)
X2, Y2 = ny.meshgrid(X2, Y2)
Z2 = muE6data25.row(12).list()
surf2 = ax.plot_surface(X2, Y2, Z2, rstride=1, cstride=1, cmap=cm.jet,
        linewidth=0, antialiased=False)
#ax.set_zlim(-1.01, 1.01)

#ax.zaxis.set_major_locator(LinearLocator(10))
#ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f'))

fig2.colorbar(surf2, shrink=0.5, aspect=5)

plt.show()
plt.savefig( 'muell2.svg')

asdf=list_plot3d([muE6data25.row(13),muE6data25.row(12),muE6data25.row(16)])
# save that figure
filename = 'asdf.svg'
show(asdf)
# filename = 'alpha3.svg'
asdf.save(filename)

Traceback (most recent call last): ax = fig.gca(projection='3d') File "", line 1, in <module> File "/tmp/tmp9cxbgn/___code___.py", line 36, in <module> linewidth=_sage_const_0 , antialiased=False) File "/home/grebi/bin/sage-4.7.1-linux-64bit-ubuntu_8.04.4_lts-x86_64-Linux/local/lib/python2.6/site-packages/mpl_toolkits/mplot3d/axes3d.py", line 663, in plot_surface rows, cols = Z.shape AttributeError: 'list' object has no attribute 'shape'

asdf_dict={
"a":5}

bsdf_dict={
"c":5}

csdf_dict={
"v":5}

asdf_dict.update(bsdf_dict)
asdf_dict.update(csdf_dict)
asdf_dict

\newcommand{\Bold}[1]{\mathbf{#1}}\left\{\verb|a| : 5, \verb|c| : 5, \verb|v| : 5\right\}

var("fein,faus,mualpha,mue")

eq=(fein/faus==e^(mualpha))
print solve(eq,faus)
eq2=(mue==(fein-faus)/fein)
(solve((eq+eq2),mue))

print (simplify(eq2(faus=fein*e^(-mualpha))))

[ faus == fein*e^(-mualpha) ] mue == -(fein*e^(-mualpha) - fein)/fein

(mue == (faus*fein*e^mualpha - fein^2 - faus + fein)/fein).full_simplify()

mue == (faus*fein*e^mualpha - fein^2 - faus + fein)/fein

###### 9 #####

### Theta 1

# plot tendon friction coefficient due to enlacement
mu1=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[0],(theta_1,theta1min,theta1max),color="red",linestyle="--",legend_label="$tendon 1$",axes_labels=['$\\theta_1[rad]$','$\\mu$'])
mu2=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[1],(theta_1,theta1min,theta1max),color="red",legend_label="$tendon 2$")
mu3=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[2],(theta_1,theta1min,theta1max),color="green",linestyle="--",legend_label="$tendon 3$")
mu4=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[3],(theta_1,theta1min,theta1max),color="green",legend_label="$tendon 4$")
mu5=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[4],(theta_1,theta1min,theta1max),color="blue",linestyle="--",legend_label="$flex_{DIP}$")
mu6=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[5],(theta_1,theta1min,theta1max),color="blue",legend_label="$ext_{PIP}$")
mu7=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[6],(theta_1,theta1min,theta1max),color="black",linestyle="--",legend_label="$flex_{DIP}$")
mu8=plot(mu_enlacement(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[7],(theta_1,theta1min,theta1max),color="black",legend_label="$ext_{DIP}$")
show(mu1+mu2+mu3+mu4+mu5+mu6+mu7+mu8)


# plot tendon friction coefficient due to enlacement
alpha1=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[0],(theta_1,theta1min,theta1max),color="red",linestyle="--",legend_label="$tendon 1$",axes_labels=['$\\theta_1[rad]$','$\\alpha[rad]$'])
alpha2=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[1],(theta_1,theta1min,theta1max),color="red",legend_label="$tendon 2$")
alpha3=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[2],(theta_1,theta1min,theta1max),color="blue",linestyle="--",legend_label="$tendon 3$")
alpha4=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[3],(theta_1,theta1min,theta1max),color="blue",legend_label="$tendon 4$")
alpha5=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[4],(theta_1,theta1min,theta1max),color="green",linestyle="--",legend_label="$flex_{PIP}$")
alpha6=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[5],(theta_1,theta1min,theta1max),color="green",legend_label="$ext_{PIP}$")
alpha7=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[6],(theta_1,theta1min,theta1max),color="black",linestyle="--",legend_label="$flex_{DIP}$")
alpha8=plot(alpha_enl_sum(**param_dict)(theta_2=0,theta_3=0,theta_4=0)[7],(theta_1,theta1min,theta1max),color="black",legend_label="$ext_{DIP}$")
show(alpha1+alpha2+alpha3+alpha4+alpha5+alpha6+alpha7+alpha8)

# save that figure
filename = 'alpha1.eps'
(alpha1+alpha2+alpha3+alpha4+alpha5+alpha6+alpha7+alpha8).save(filename)

### Theta 2

# plot tendon friction coefficient due to enlacement

mu5=plot(mu_enlacement(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[4],(theta_2,theta2min,theta2max),color="blue",linestyle="--",legend_label="$flex_{DIP}$")
mu6=plot(mu_enlacement(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[5],(theta_2,theta2min,theta2max),color="blue",legend_label="$ext_{PIP}$")
mu7=plot(mu_enlacement(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[6],(theta_2,theta2min,theta2max),color="black",linestyle="--",legend_label="$flex_{DIP}$")
mu8=plot(mu_enlacement(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[7],(theta_2,theta2min,theta2max),color="black",legend_label="$ext_{DIP}$",axes_labels=['$\\theta_2[rad]$','$\\mu$'])
show(mu5+mu6+mu7+mu8)

# plot tendon friction coefficient due to enlacement
alpha5=plot(alpha_enl_sum(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[4],(theta_2,theta2min,theta2max),color="blue",linestyle="--",legend_label="$flex_{DIP}$")
alpha6=plot(alpha_enl_sum(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[5],(theta_2,theta2min,theta2max),color="blue",legend_label="$ext_{PIP}$")
alpha7=plot(alpha_enl_sum(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[6],(theta_2,theta2min,theta2max),color="black",linestyle="--",legend_label="$flex_{DIP}$")
alpha8=plot(alpha_enl_sum(**param_dict)(theta_1=0,theta_3=0,theta_4=0)[7],(theta_2,theta2min,theta2max),color="black",legend_label="$ext_{DIP}$",axes_labels=['$\\theta_2[rad]$','$\\alpha[rad]$'])
show(alpha5+alpha6+alpha7+alpha8)


# save that figure
filename = 'alpha2.eps'
(alpha5+alpha6+alpha7+alpha8).save(filename)


### Theta 3

# plot tendon friction coefficient due to enlacement
mu7=plot(mu_enlacement(**param_dict)(theta_1=0,theta_2=0,theta_4=0)[6],(theta_3,theta3min,theta3max),color="black",linestyle="--",legend_label="$flex_{DIP}$")
mu8=plot(mu_enlacement(**param_dict)(theta_1=0,theta_2=0,theta_4=0)[7],(theta_3,theta3min,theta3max),color="black",legend_label="$ext_{DIP}$",axes_labels=['$\\theta_3[rad]$','$\\mu$'])
show(mu7+mu8)

# plot tendon friction coefficient due to enlacement
alpha7=plot(alpha_enl_sum(**param_dict)(theta_1=0,theta_2=0,theta_4=0)[6],(theta_3,theta3min,theta3max),color="black",linestyle="--",legend_label="$flex_{DIP}$")
alpha8=plot(alpha_enl_sum(**param_dict)(theta_1=0,theta_2=0,theta_4=0)[7],(theta_3,theta3min,theta3max),color="black",legend_label='$ext_{DIP}$',axes_labels=['$\\theta_3[rad]$','$\\alpha[rad]$'])
show(alpha7+alpha8)

# save that figure
filename = 'alpha3.eps'
# filename = 'alpha3.svg'
(alpha7+alpha8).save(filename)

#print Tau_fr(**param_dict)(**pret5_dict)(theta_2=0,theta_3=0,theta_4=0)[3]

print tau_b(**param_dict)(**pret0_dict)(theta_1=0,theta_2=0,theta_3=0,theta_4=0).n().column()
print "###########################"

print f_t_act(**param_dict)(**pret5_dict)(theta_2=0,theta_3=0,theta_4=0).n().column()
print "###########################"
print f_t_act_corr(**param_dict)(**pret0_dict)(theta_1=0,theta_2=0,theta_3=0,theta_4=0).n().column()
print "###########################"
print f_tendon(**param_dict)(**pret5_dict)(theta_2=0,theta_3=0,theta_4=0).n().column()
print "######Tau_test#####################"
print TauTest(**param_dict)(**pret0_dict)(theta_2=0,theta_3=15*deg,theta_4=0).n()
print "######Tau_test2#####################"
print TauTest2(**param_dict)(**pret0_dict)(theta_2=0,theta_3=15*deg,theta_4=0).n()

#tau1=plot(Tau_fr(**param_dict)(**pret5_dict)(theta_2=0,theta_3=0,theta_4=0)[3],(theta_1,theta1min,theta1max))
#tau2=plot(tau_b(**param_dict)(**pret5_dict)(theta_2=0,theta_3=0,theta_4=0)[3],(theta_1,theta1min,theta1max))
#show(tau1+tau2)

[0.000000000000000] [ 1950.00000000000] [ 900.000000000000] [ 450.000000000000] ########################### [-59.4512195121951] [-59.4512195121951] [ 59.4512195121951] [ 59.4512195121951] [-127.358490566038] [ 127.358490566038] [-60.0000000000000] [ 60.0000000000000] ########################### [-118.902439024390] [-118.902439024390] [0.000000000000000] [0.000000000000000] [-254.716981132075] [0.000000000000000] [-120.000000000000] [0.000000000000000] ########################### [-123.902439024390] [-123.902439024390] [-5.00000000000000] [-5.00000000000000] [-259.716981132075] [-5.00000000000000] [-125.000000000000] [-5.00000000000000] ######Tau_test##################### (0.000000000000000, 1914.22211760352, 900.000000000000, 450.000000000000) ######Tau_test2##################### (0.000000000000000, 1914.22211760352, 900.000000000000, 450.000000000000)

#Test calculation: I want to know what happens to the tipforce when joint torques do not fit


# J_b_inv=pseudoinverse(J_b)
print J_b(**test_dict)
print pseudoinverse(J_b(**test_dict))

print "tip force:"

print J_b(**test_dict).transpose()*F_tip

# But we need the tip force with respect to the torques. So use the pseudoinverse

Traceback (most recent call last): File "", line 1, in <module> File "/tmp/tmphjiOQu/___code___.py", line 6, in <module> print J_b(**test_dict) NameError: name 'test_dict' is not defined

##########################################################
###### Get it together. Tendon forces including friction
##########################################################
# Get signs of joint torques since friction adds a necessary torque to the joint torque sice we want to be active
TAU=diag(tau_b)
print(tau_b)
SIGN_TAU=TAU.apply_map(sign)

F_tendon=diag(f_tendon)*diag(mu_enlacement)+Tau_fr   # The tendonforces contain the join torques!

### FIXME The torque of course adds to the joint torque BEFORE h is calculated!

(0, f_tip*l_m*cos(theta_4) + f_tip*l_p*cos(theta_3 + theta_4) + f_tip*l_d, f_tip*l_m*cos(theta_4) + f_tip*l_d, f_tip*l_d)

Traceback (most recent call last): SIGN_TAU=TAU.apply_map(sign) File "", line 1, in <module> File "/tmp/tmpMGjnJ1/___code___.py", line 10, in <module> F_tendon=diag(f_tendon)*diag(mu_enlacement)+Tau_fr # The tendonforces contain the join torques! File "element.pyx", line 1302, in sage.structure.element.RingElement.__add__ (sage/structure/element.c:11504) File "coerce.pyx", line 766, in sage.structure.coerce.CoercionModel_cache_maps.bin_op (sage/structure/coerce.c:7337) TypeError: unsupported operand parent(s) for '+': 'Full MatrixSpace of 8 by 8 dense matrices over Symbolic Ring' and 'Vector space of dimension 4 over Symbolic Ring'

var("asdf,u,v,w,x,y,z")
def testit(a_number):
    if a_number >=0:
        a_number=v
    else:
        a_number=-v
             
    return a_number

w=-99999

dict={
"x": w
    }
A=Matrix([[asdf,1,2,8,9],
[0,1,v,8,2*x],
[0,1,w,8,9],
[0,1,w,8,7],
[0,1,2,8*x,9]])

a=vector([0,1,2,8,99,22,333,444])
b=vector([1,-3,2,7,91,-3,2,7])
#B=diag(b); B
print "********************"
print A
#print "********************"


#reset("asdf")

print a*b^-1

******************** [ asdf 1 2 8 9] [ 0 1 v 8 2*x] [ 0 1 -99999 8 9] [ 0 1 -99999 8 7] [ 0 1 2 8*x 9]

Traceback (most recent call last): File "", line 1, in <module> File "/tmp/tmpLvpqcK/___code___.py", line 33, in <module> exec compile(u'print a*b**-_sage_const_1 ' + '\n', '', 'single') File "", line 1, in <module> File "free_module_element.pyx", line 1525, in sage.modules.free_module_element.FreeModuleElement.__pow__ (sage/modules/free_module_element.c:8101) NotImplementedError

from numpy import r_
a=0
b=2



from pylab import *

figure()

asdf = arange(a, b, (b-a)/100)
s = 4*tan(2*pi*asdf)
P = plot(asdf, s, linewidth=1.0)
xl = xlabel('time (s)')
yl = ylabel('voltage (mV)')
t = title('About as simple as it gets, folks')
grid(True)
savefig( 'sage.svg')

####### XX ################

###### Old Calculation of tendon forces below ############ we can reuse plenty of that

# Calculate tendon force needed to actuate finger
f_t_act=(diag(q_b)*(diag(d).inverse())).diagonal() #Using Matrices a inverse of vector d is given but result should be a vector
print latex (f_t_act)
F_t_act=Matrix([f_t_act]).transpose() #create Matrix from list f_t_act
F_t_act=F_t_act.augment(vector([0,0,0,0]))# add an empty column as second column of tendon force matrix

print "Active Tendon  force Matrix:"
print F_t_act


# Setup pretension forces
f_t_pre=vector([var("f_pre_MC1"),var("f_pre_MC2"),var("f_pre_PIP"),var("f_pre_PIP")])
F_t_pre=f_t_pre.column().augment(f_t_pre.column())

print "Pretension force Matrix:"
print F_t_pre

# Setup angle of attachment
alpha=Matrix([
        [var("alpha_MC1_add"),var("alpha_MC1_abd")],
        [var("alpha_MC2_flex"),var("alpha_MC2_ext")],
        [var("alpha_P_fflex"),var("alpha_P_ext")],
        [var("alpha_D_flex"),var("alpha_D_ext")]
         ])

# Calculate Tendon forces
# First Column is flexor, 2nd extensor
F_t=F_t_act+F_t_pre
print "##################"
print "Tendon forces:"
print F_t
print latex (F_t)

########################################
# Calculation of forces at every segment
########################################
#DIP
# Deviation forces at DIP do not produce any torque at the DIP
# Sum of forces at DIP
# Generalized force vectors of DIP forces


F_D_flex=F_t[3,0]*vector([
        cos(alpha_D_flex),
        sin(alpha_D_flex),
        0,
        0,
        0 ])

F_D_ext=F_t[3,1]*vector([
        cos(alpha_D_ext),
        -sin(alpha_D_ext),
        0,
        0,
        0 ])
F_n_dip=-F_tip-F_D_ext-F_E_flex

a=Matrix([f_t_act])
print a

[ 0 (f_tip*l_m*cos(theta_4) + f_tip*l_p*cos(theta_3 + theta_4) + f_tip*l_d)/d_MC (f_tip*l_m*cos(theta_4) + f_tip*l_d)/d_PIP f_tip*l_d/d_PIP]

print a*b.inverse()
print d[1]

Traceback (most recent call last): File "<stdin>", line 1, in <module> File "_sage_input_6.py", line 10, in <module> exec compile(u'open("___code___.py","w").write("# -*- coding: utf-8 -*-\\n" + _support_.preparse_worksheet_cell(base64.b64decode("cHJpbnQgYSpiLmludmVyc2UoKQpwcmludCBkWzFd"),globals())+"\\n"); execfile(os.path.abspath("___code___.py"))' + '\n', '', 'single') File "", line 1, in <module> File "/tmp/tmpuJ8uoI/___code___.py", line 3, in <module> print a*b.inverse() NameError: name 'b' is not defined

print (f_t_act).apply_map(attrcall('trig_reduce'))

Traceback (most recent call last): File "<stdin>", line 1, in <module> File "_sage_input_7.py", line 10, in <module> exec compile(u'open("___code___.py","w").write("# -*- coding: utf-8 -*-\\n" + _support_.preparse_worksheet_cell(base64.b64decode("cHJpbnQgKGZfdF9hY3QpLmFwcGx5X21hcChhdHRyY2FsbCgndHJpZ19yZWR1Y2UnKSk="),globals())+"\\n"); execfile(os.path.abspath("___code___.py"))' + '\n', '', 'single') File "", line 1, in <module> File "/tmp/tmpq_FojX/___code___.py", line 2, in <module> exec compile(u"print (f_t_act).apply_map(attrcall('trig_reduce'))" + '\n', '', 'single') File "", line 1, in <module> AttributeError: 'list' object has no attribute 'apply_map'

print latex ((f_t_act).apply_map(attrcall('trig_reduce')))

\left(0,\,f_{\mbox{tip}} l_{m} \cos\left(\theta_{4}\right) + f_{\mbox{tip}} l_{p} \cos\left(\theta_{3} + \theta_{4}\right) + f_{\mbox{tip}} l_{d},\,f_{\mbox{tip}} l_{m} \cos\left(\theta_{4}\right) + f_{\mbox{tip}} l_{d},\,f_{\mbox{tip}} l_{d}\right)

omega=vector([-var('s2'),0,var('c2')])
lmc=var('lmc')
lp=var('lp')
c1=var('c1')
s1=var('s1')

qs=vector([c1*lmc+lp*c1*c2,0,-s1*lmc+lp*(-s1*s2)])

print latex ((omega.cross_product(qs)).column())

\left(\begin{array}{r} 0 \\ -{\left(\mbox{lp} s_{1} s_{2} + \mbox{lmc} s_{1}\right)} s_{2} + {\left(c_{1} c_{2} \mbox{lp} + c_{1} \mbox{lmc}\right)} c_{2} \\ 0 \end{array}\right)

extension vector h: \left(\begin{array}{r} \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{M} \theta_{1} - r_{M} \theta_{2} + l_{1} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{M} \theta_{1} - r_{M} \theta_{2} + l_{2} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{M} \theta_{1} + r_{M} \theta_{2} + l_{3} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{M} \theta_{1} + r_{M} \theta_{2} + l_{4} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -d_{i} \theta_{1} + r_{P} \theta_{3} + l_{5} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -d_{a} \theta_{1} + r_{P} \theta_{3} + l_{6} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ d_{i} \theta_{1} + r_{D} \theta_{3} - r_{D} \theta_{4} + l_{7} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ d_{a} \theta_{1} - r_{D} \theta_{3} + r_{D} \theta_{4} + l_{8} \end{array}\right) ########################################## P(theta) [(theta_1, theta_2, theta_3, theta_4) |--> -r_M (theta_1, theta_2, theta_3, theta_4) |--> r_M (theta_1, theta_2, theta_3, theta_4) |--> r_M (theta_1, theta_2, theta_3, theta_4) |--> -r_M (theta_1, theta_2, theta_3, theta_4) |--> -d_i (theta_1, theta_2, theta_3, theta_4) |--> -d_a (theta_1, theta_2, theta_3, theta_4) |--> d_i (theta_1, theta_2, theta_3, theta_4) |--> d_a] [(theta_1, theta_2, theta_3, theta_4) |--> -r_M (theta_1, theta_2, theta_3, theta_4) |--> -r_M (theta_1, theta_2, theta_3, theta_4) |--> r_M (theta_1, theta_2, theta_3, theta_4) |--> r_M (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0] [ (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> r_P (theta_1, theta_2, theta_3, theta_4) |--> r_P (theta_1, theta_2, theta_3, theta_4) |--> r_D (theta_1, theta_2, theta_3, theta_4) |--> -r_D] [ (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> 0 (theta_1, theta_2, theta_3, theta_4) |--> -r_D (theta_1, theta_2, theta_3, theta_4) |--> r_D] ########################################## \left(\begin{array}{rrrrrrrr} \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -d_{i} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -d_{a} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ d_{i} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ d_{a} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{M} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{P} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{P} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{D} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{D} \\ \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ 0 & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ -r_{D} & \left( \theta_{1}, \theta_{2}, \theta_{3}, \theta_{4} \right) \ {\mapsto} \ r_{D} \end{array}\right)

sign(-5)

-1

P_pseudoinv.apply_map(attrcall('simplify'))

[ -r_M/(d_a^2 + d_i^2 + 4*r_M^2) -1/4/r_M -1/2*(d_a + d_i)*r_M/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) 1/2*((d_a - d_i)*r_M*r_P - (d_a + d_i)*r_D*r_M)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P)] [ r_M/(d_a^2 + d_i^2 + 4*r_M^2) -1/4/r_M 1/2*(d_a + d_i)*r_M/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) -1/2*((d_a - d_i)*r_M*r_P - (d_a + d_i)*r_D*r_M)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P)] [ r_M/(d_a^2 + d_i^2 + 4*r_M^2) 1/4/r_M 1/2*(d_a + d_i)*r_M/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) -1/2*((d_a - d_i)*r_M*r_P - (d_a + d_i)*r_D*r_M)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P)] [ -r_M/(d_a^2 + d_i^2 + 4*r_M^2) 1/4/r_M -1/2*(d_a + d_i)*r_M/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) 1/2*((d_a - d_i)*r_M*r_P - (d_a + d_i)*r_D*r_M)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P)] [ 1/2*(d_a + d_i)/(d_a^2 + d_i^2 + 4*r_M^2) - d_i/(d_a^2 + d_i^2 + 4*r_M^2) 0 -1/2*(d_a*d_i + d_i^2)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) + 1/4*(3*d_a^2 + 2*d_a*d_i + 3*d_i^2 + 8*r_M^2)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) 1/2*((d_a*d_i - d_i^2)*r_P - (d_a*d_i + d_i^2)*r_D)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P) + 1/4*(8*r_D*r_M^2 - (d_a^2 - d_i^2)*r_P + (3*d_a^2 + 2*d_a*d_i + 3*d_i^2)*r_D)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P)] [ 1/2*(d_a + d_i)/(d_a^2 + d_i^2 + 4*r_M^2) - d_a/(d_a^2 + d_i^2 + 4*r_M^2) 0 -1/2*(d_a^2 + d_a*d_i)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) + 1/4*(3*d_a^2 + 2*d_a*d_i + 3*d_i^2 + 8*r_M^2)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) 1/2*((d_a^2 - d_a*d_i)*r_P - (d_a^2 + d_a*d_i)*r_D)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P) + 1/4*(8*r_D*r_M^2 - (d_a^2 - d_i^2)*r_P + (3*d_a^2 + 2*d_a*d_i + 3*d_i^2)*r_D)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P)] [ d_i/(d_a^2 + d_i^2 + 4*r_M^2) + 1/2*(d_a*r_D + d_i*r_D)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) + 1/2*((d_a - d_i)*r_P - d_a*r_D - d_i*r_D)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) 0 1/2*(d_a*d_i + d_i^2)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) + 1/4*(3*d_a^2*r_D + 2*d_a*d_i*r_D + 3*d_i^2*r_D + 8*r_D*r_M^2)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P^2) - 1/4*(3*d_a^2*r_D + 2*d_a*d_i*r_D + 3*d_i^2*r_D + 8*r_D*r_M^2 - (d_a^2 - d_i^2)*r_P)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P^2) -1/2*((d_a*d_i - d_i^2)*r_P - (d_a*d_i + d_i^2)*r_D)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P) - 1/4*(8*r_D^2*r_M^2 - 2*(d_a^2 - d_i^2)*r_D*r_P + (3*d_a^2 + 2*d_a*d_i + 3*d_i^2)*r_D^2 + (3*d_a^2 - 2*d_a*d_i + 3*d_i^2 + 8*r_M^2)*r_P^2)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P^2) + 1/4*(3*d_a^2*r_D + 2*d_a*d_i*r_D + 3*d_i^2*r_D + 8*r_D*r_M^2 - (d_a^2 - d_i^2)*r_P)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P^2)] [ d_a/(d_a^2 + d_i^2 + 4*r_M^2) - 1/2*(d_a*r_D + d_i*r_D)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) - 1/2*((d_a - d_i)*r_P - d_a*r_D - d_i*r_D)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) 0 1/2*(d_a^2 + d_a*d_i)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P) - 1/4*(3*d_a^2*r_D + 2*d_a*d_i*r_D + 3*d_i^2*r_D + 8*r_D*r_M^2)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P^2) + 1/4*(3*d_a^2*r_D + 2*d_a*d_i*r_D + 3*d_i^2*r_D + 8*r_D*r_M^2 - (d_a^2 - d_i^2)*r_P)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P^2) -1/2*((d_a^2 - d_a*d_i)*r_P - (d_a^2 + d_a*d_i)*r_D)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P) + 1/4*(8*r_D^2*r_M^2 - 2*(d_a^2 - d_i^2)*r_D*r_P + (3*d_a^2 + 2*d_a*d_i + 3*d_i^2)*r_D^2 + (3*d_a^2 - 2*d_a*d_i + 3*d_i^2 + 8*r_M^2)*r_P^2)/((4*r_D*r_M^2 + (d_a^2 + d_i^2)*r_D)*r_P^2) - 1/4*(3*d_a^2*r_D + 2*d_a*d_i*r_D + 3*d_i^2*r_D + 8*r_D*r_M^2 - (d_a^2 - d_i^2)*r_P)/((d_a^2 + d_i^2 + 4*r_M^2)*r_P^2)]

File: /sage/local/lib/python2.6/site-packages/sage/rings/arith.py

Type: <type ‘function’>

Definition: rational_reconstruction(a, m, algorithm=’fast’)

Docstring:

This function tries to compute x/y, where x/y is a rational number in lowest terms such that the reduction of x/y modulo m is equal to a and the absolute values of x and y are both \le \sqrt{m/2}. If such x/y exists, that pair is unique and this function returns it. If no such pair exists, this function raises ZeroDivisionError.

An efficient algorithm for computing rational reconstruction is very similar to the extended Euclidean algorithm. For more details, see Knuth, Vol 2, 3rd ed, pages 656-657.

INPUT:

• a - an integer
• m - a modulus
• algorithm - (default: ‘fast’)
  • 'fast' - a fast compiled implementation
  • 'python' - a slow pure python implementation

OUTPUT:

Numerator and denominator n, d of the unique rational number r=n/d, if it exists, with n and |d| \le \sqrt{N/2}. Return (0,0) if no such number exists.

The algorithm for rational reconstruction is described (with a complete nontrivial proof) on pages 656-657 of Knuth, Vol 2, 3rd ed. as the solution to exercise 51 on page 379. See in particular the conclusion paragraph right in the middle of page 657, which describes the algorithm thus:

This discussion proves that the problem can be solved efficiently by applying Algorithm 4.5.2X with u=m and v=a, but with the following replacement for step X2: If v3 \le \sqrt{m/2}, the algorithm terminates. The pair (x,y)=(|v2|,v3*\mathrm{sign}(v2)) is then the unique solution, provided that x and y are coprime and x \le \sqrt{m/2}; otherwise there is no solution. (Alg 4.5.2X is the extended Euclidean algorithm.)

Knuth remarks that this algorithm is due to Wang, Kornerup, and Gregory from around 1983.

EXAMPLES:

sage: m = 100000
sage: (119*inverse_mod(53,m))%m
11323
sage: rational_reconstruction(11323,m)
119/53

sage: rational_reconstruction(400,1000)
Traceback (most recent call last):
...
ValueError: Rational reconstruction of 400 (mod 1000) does not exist.

sage: rational_reconstruction(3,292393, algorithm='python')
3
sage: a = Integers(292393)(45/97); a
204977
sage: rational_reconstruction(a,292393, algorithm='python')
45/97
sage: a = Integers(292393)(45/97); a
204977
sage: rational_reconstruction(a,292393, algorithm='fast')
45/97
sage: rational_reconstruction(293048,292393, algorithm='fast')
Traceback (most recent call last):
...
ValueError: Rational reconstruction of 655 (mod 292393) does not exist.
sage: rational_reconstruction(293048,292393, algorithm='python')
Traceback (most recent call last):
...
ValueError: Rational reconstruction of 655 (mod 292393) does not exist.

TESTS:

Check that ticket #9345 is fixed:

sage: rational_reconstruction(1, 0)
Traceback (most recent call last):
...
ZeroDivisionError: The modulus cannot be zero
sage: rational_reconstruction(randint(-10^6, 10^6), 0)
Traceback (most recent call last):
...
ZeroDivisionError: The modulus cannot be zero

JT4.JacColumn().column()

WARNING: Output truncated!

full_output.txt

Calculating Joint: 4 4 Multiplications [ -((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*sin(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*cos(theta_4) -((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*cos(theta_4) + ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*sin(theta_4) 0 (cos(theta_4) - 1)*((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*(l_1 + l_2 + l_3) + (cos(theta_3) - 1)*(l_1 + l_2)*(sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1)) + (cos(theta_2) - 1)*l_1*sin(theta_1) - (l_1 + l_2)*(sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*(l_1 + l_2 + l_3)*sin(theta_4) + l_1*sin(theta_2)*cos(theta_1) - (((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*cos(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*sin(theta_4))*(l_1 + l_2 + l_3)] [ ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*cos(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*sin(theta_4) -((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*sin(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*cos(theta_4) 0 (cos(theta_4) - 1)*((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*(l_1 + l_2 + l_3) + (cos(theta_3) - 1)*(l_1 + l_2)*(sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2)) - (cos(theta_2) - 1)*l_1*cos(theta_1) + (l_1 + l_2)*(sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*(l_1 + l_2 + l_3)*sin(theta_4) + l_1*sin(theta_1)*sin(theta_2) - (((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*sin(theta_4) + ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*cos(theta_4))*(l_1 + l_2 + l_3)] [ 0 0 1 0] [ 0 0 0 1] Calculating Joint: 4 4 Multiplications [ -((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*sin(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*cos(theta_4) ... 1] Calculating Joint: 4 4 Multiplications [ -((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*sin(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*cos(theta_4) -((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*cos(theta_4) + ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*sin(theta_4) 0 (cos(theta_4) - 1)*((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*(l_1 + l_2 + l_3) + (cos(theta_3) - 1)*(l_1 + l_2)*(sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1)) + (cos(theta_2) - 1)*l_1*sin(theta_1) - (l_1 + l_2)*(sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*(l_1 + l_2 + l_3)*sin(theta_4) + l_1*sin(theta_2)*cos(theta_1) - (((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*cos(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*sin(theta_4))*(l_1 + l_2 + l_3)] [ ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*cos(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*sin(theta_4) -((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*sin(theta_4) - ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*cos(theta_4) 0 (cos(theta_4) - 1)*((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*(l_1 + l_2 + l_3) + (cos(theta_3) - 1)*(l_1 + l_2)*(sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2)) - (cos(theta_2) - 1)*l_1*cos(theta_1) + (l_1 + l_2)*(sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*(l_1 + l_2 + l_3)*sin(theta_4) + l_1*sin(theta_1)*sin(theta_2) - (((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*cos(theta_3) - (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*sin(theta_3))*sin(theta_4) + ((sin(theta_1)*cos(theta_2) + sin(theta_2)*cos(theta_1))*sin(theta_3) + (sin(theta_1)*sin(theta_2) - cos(theta_1)*cos(theta_2))*cos(theta_3))*cos(theta_4))*(l_1 + l_2 + l_3)] [ 0 0 1 0] [ 0 0 0 1] Jacobian column vector [(l_1 + l_2 + l_3)*cos(theta_1 + theta_2 + theta_3 + theta_4) + l_1 + l_2 + l_3] [ (l_1 + l_2 + l_3)*sin(theta_1 + theta_2 + theta_3 + theta_4)] [ 0] [ (l_1 + l_2 + l_3)*null] [ (l_1 + l_2 + l_3)*null] [ (l_1 + l_2 + l_3)*null + 1]

Traceback (most recent call last): File "<stdin>", line 1, in <module> File "_sage_input_3.py", line 10, in <module> exec compile(u'open("___code___.py","w").write("# -*- coding: utf-8 -*-\\n" + _support_.preparse_worksheet_cell(base64.b64decode("Sm9pbnQuU2tldyh2ZWN0b3IoWzAsbF8xLDBdKSk="),globals())+"\\n"); execfile(os.path.abspath("___code___.py"))' + '\n', '', 'single') File "", line 1, in <module> File "/tmp/tmpcdaOhc/___code___.py", line 3, in <module> exec compile(u'Joint.Skew(vector([_sage_const_0 ,l_1,_sage_const_0 ]))' + '\n', '', 'single') File "", line 1, in <module> NameError: name 'Joint' is not defined

omega=vector([var('_o1'),var('_o2'),var('_o3')])
p=vector([var('_p1'),var('_p2'),var('_p3')])
theta=var('theta')        

class Gst(object): #Class to calculate all Terms needed to calculate the Kinematics and Jacobian from Twists
     #omega is the vector of axis, p the vector to a point on the axis. Therefore a Istance for Every joint can be created
    def __init__ (self,omega,p,theta):
        self._omega=omega #Assign Input Parameters to object
        self._p=p
        self._theta=theta
        print "instanziiert"

    def Rodriguez(self): # Calculation of the exponential of omega suding rodriguez formula
        _OMEGA_w=Skew(self._omega)             # Calculate Skew symmetric matrix omega_wedge
        _exp_omega=exp(_OMEGA_w*self._theta) # Should be the same!!
        _exp_omega_r=(IDENT+
                      _OMEGA_w*sin(self._theta)+
                      _OMEGA_w^2*(1-cos(self._theta)))
        print _exp_omega_r #return the result of rodriguez formula
        print "Schrott"

    def Asdf(self):
        #omega=self.omega
        print self._omega.cross_product(self._p)