### 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 ####### 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 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 (click to the left of this block for traceback) ... __SAGE__ 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) (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 (click to the left of this block for traceback) ... NameError: name 'theta1min' is not defined 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 (click to the left of this block for traceback) ... SyntaxError: invalid syntax 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 (click to the left of this block for traceback) ... NameError: name 'Tau_fr' is not defined 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 (click to the left of this block for traceback) ... TypeError: cannot evaluate symbolic expression numerically 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 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 (click to the left of this block for traceback) ... NameError: name 'tauFrT5data' is not defined 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 (click to the left of this block for traceback) ... IndexError: row index out of range 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"))
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) (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) 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] 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 (click to the left of this block for traceback) ... AttributeError: 'list' object has no attribute 'shape' 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\}
\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 [ 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 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) [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 (click to the left of this block for traceback) ... NameError: name 'test_dict' is not defined 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 (click to the left of this block for traceback) ... 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' (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 (click to the left of this block for traceback) ... NotImplementedError ********************
[ 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
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] [ 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 (click to the left of this block for traceback) ... NameError: name 'b' is not defined 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"))
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 (click to the left of this block for traceback) ... AttributeError: 'list' object has no attribute 'apply_map' 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"))
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)
\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)
\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)
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 -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)] [ -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:
|
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] 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 (click to the left of this block for traceback) ... NameError: name 'Joint' is not defined 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"))
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 ]))
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)
|
|
|
|