#!/usr/bin/env python
import math
radians = math.radians
degrees = math.degrees
width = 700
height = 500
scale = 2.5
offset_x = 300
offset_y = 50
servo_distance = 23.0
servo_arm_length = 50.0
lower_arm_length = 50.0
elbow_pen_distance = 59.0
hands_pen_distance = 11.0
hands_pen_angle = None
hands_elbow_angle = None
lw1 = 1./scale
def sc(n):
return int(n*scale)
def ox( x ):
return offset_x + sc(x)
def oy( y ):
return offset_y - sc(y)
def length( v ):
return math.sqrt( v.x * v.x + v.y * v.y )
#def diff_length( c0, c1 ):
# return length( ( c1[0] - c0[0], c1[1] - c0[1] ) )
def sin( a ):
return math.sin( a )
def cos( a ):
return math.cos( a )
def asin( a ):
return math.asin( a )
def atan2( y, x ):
return math.atan2( y, x )
def sqrt( x ):
return math.sqrt( x )
def angle_sine_rule( alpha, a, b ):
# a / sin(alpha) = b / sin(beta)
# beta = asin( b/a * sin(alpha) )
return asin( b/a * sin(alpha) )
def angle_alpha( a, b, c ):
# cosine rule
# a^2 = b^2 + c^2 - 2*b*c * cos(alpha)
# (a^2 - b^2 - c^2) / (-2*b*c) = cos(alpha)
return math.acos( (a*a - b*b - c*c) / (-2*b*c) )
#def angle( c0, c1 ):
# return math.atan2( c1[0] - c0[0], c1[1] - c0[1] )
def circle( ofd, c, r, color="00aa00" ):
ofd.write( '\t\n' % ( ox(c.x), oy(c.y), sc(r), color, color ) )
def line( ofd, c0, c1, w, color="00aa00" ):
ofd.write( '\t\n' % ( ox(c0.x), oy(c0.y), ox(c1.x), oy(c1.y), color, sc(w) ) )
def arrow( ofd, c0, c1, w, color="00aa00" ):
line( ofd, c0, c1, w, color )
d = c0 - c1
d = d.norm() * (7*lw1)
line( ofd, c1, c1 + d.rotate( radians( 45 ) ), w, color )
line( ofd, c1, c1 + d.rotate( radians( -45 ) ), w, color )
def svg_open( filename, width, height ):
ofd = open( filename, "w" )
ofd.write( """\n' )
ofd.close()
class vect2:
def __init__( self, x, y ):
self.x = x
self.y = y
def length( self ):
return length( self )
def __add__( self, v ):
return vect2( self.x + v.x, self.y + v.y )
def __sub__( self, v ):
return vect2( self.x - v.x, self.y - v.y )
def __mul__( self, f ):
return vect2( self.x * f, self.y * f )
def rotate( self, a ):
ca = cos(a)
sa = sin(a)
return vect2(
self.x * ca - self.y * sa,
self.x * sa + self.y * ca
)
def norm( self ):
l = self.length()
return vect2( self.x / l, self.y / l )
def doit():
global hands_pen_angle, hands_elbow_angle
a0d = 0
a1d = 0
a0 = radians(-90+a0d)
a1 = radians(-90+a1d)
c0 = vect2( 0, 0 )
c1 = vect2( servo_distance, 0 )
c2 = c0 + vect2( servo_arm_length, 0 ).rotate( a0 )
c3 = c1 + vect2( servo_arm_length, 0 ).rotate( a1 )
d23 = c3 - c2
d2m = d23 * 0.5
lm4 = sqrt( lower_arm_length ** 2 - d2m.length() ** 2 )
dm4 = d2m.rotate( radians(-90) ).norm() * lm4
c4 = c2 + d2m + dm4
hands_pen_angle = angle_alpha( hands_pen_distance, lower_arm_length, elbow_pen_distance )
hands_elbow_angle = angle_alpha( lower_arm_length, elbow_pen_distance, hands_pen_distance )
#print degrees(hands_pen_angle)
c24 = c4 - c2
c5 = c2 + c24.rotate( -hands_pen_angle ) * (elbow_pen_distance/lower_arm_length)
svg = svg_open( "drawing.svg", width, height )
arrow( svg, vect2(0,0), vect2(130,0), lw1, "000000" )
arrow( svg, vect2(0,0), vect2(0,-130), lw1, "000000" )
cd = 5
lw = cd
circle( svg, c0, cd )
circle( svg, c1, cd )
circle( svg, c2, cd )
circle( svg, c3, cd )
circle( svg, c4, cd )
circle( svg, c5, cd*1.5 )
line( svg, c0, c2, lw )
line( svg, c2, c4, lw )
line( svg, c4, c5, lw )
line( svg, c1, c3, lw )
line( svg, c3, c4, lw )
circle( svg, c5, cd*0.75, "ffffff" )
line( svg, c5, c5 * 2 - c4, lw, "ffffff" )
# circle( svg, c2+d2m, cd/2, "ff0000" )
# circle( svg, c2+dm4, cd/2, "ff0000" )
wbcalc( svg, c0, c1, c5, { "c2": c2, "c3": c3, "c4": c4, } )
svg_close( svg )
def wbcalc( svg, c0, c1, c5, s ):
return wbcalc_2( svg, c0, c1, c5, s )
def wbcalc_1( svg, c0, c1, c5, s ):
a05 = atan2( c5.y, c5.x )
print "a05", degrees(a05)
c5l = c5.length()
a052 = angle_alpha( elbow_pen_distance, servo_arm_length, c5l )
print "a052", degrees(a052)
al = a05 - a052
print "al", degrees(al)
a205 = angle_alpha( c5l, servo_arm_length, elbow_pen_distance )
print "a205", degrees(a205)
a25 = al + (radians(180) - a205)
print "a25", degrees(a25)
a54 = a25 - hands_elbow_angle
print "a54", degrees(a54)
c4 = c5 - vect2( hands_pen_distance, 0 ).rotate( a54 )
print "c4", c4.x, c4.y
c14 = c4 - c1
print "c14", c14.x, c14.y
a14 = atan2( c14.y, c14.x )
print "a14", degrees(a14)
a134 = angle_alpha( lower_arm_length, servo_arm_length, c14.length() )
print "a134", degrees(a134)
ar = a134 + a14
print "ar", degrees(ar)
print "left servo angle", degrees(al)+90
print "right servo angle", degrees(ar)+90
c2 = s["c2"]
arrow( svg, c0, vect2( servo_arm_length, 0 ).rotate( al ), lw1, "000000" )
#arrow( svg, c2, c2 + vect2( elbow_pen_distance, 0 ).rotate( a25 ), lw1, "000000" )
#arrow( svg, c5, c5 - vect2( hands_pen_distance, 0 ).rotate( a54 ), lw1, "000000" )
arrow( svg, c0, c4, lw1, "ff6666" )
arrow( svg, c1, c1 + vect2( servo_arm_length, 0 ).rotate( ar ), lw1, "000000" )
def wbcalc_2( svg, c0, c1, c5, s ):
print "x", c5.x
print "y", c5.y
a05 = atan2( c5.y, c5.x )
print "a05", degrees(a05)
c5l = c5.length()
a052 = angle_alpha( elbow_pen_distance, servo_arm_length, c5l )
print "a052", degrees(a052)
al = a05 - a052
print "al", degrees(al), al
fx = cos(hands_pen_angle) * lower_arm_length / elbow_pen_distance
print "fx", fx
fy = sin(hands_pen_angle) * lower_arm_length / elbow_pen_distance
print "fy", fy
c2 = vect2( cos(al) * servo_arm_length, sin(al) * servo_arm_length )
print "c2", c2.x, c2.y
c25 = c5 - c2
c4 = c2 + vect2(
c25.x * fx - c25.y * fy,
c25.y * fx + c25.x * fy
)
print "c4", c4.x, c4.y
c14 = c4 - c1
print "c14", c14.x, c14.y
a14 = atan2( c14.y, c14.x )
print "a14", degrees(a14)
a134 = angle_alpha( lower_arm_length, servo_arm_length, c14.length() )
print "a134", degrees(a134)
ar = a134 + a14
print "ar", degrees(ar)
print "left servo angle", degrees(al)+90
print "right servo angle", degrees(ar)+90
c2 = s["c2"]
arrow( svg, c0, vect2( servo_arm_length, 0 ).rotate( al ), lw1, "000000" )
#arrow( svg, c2, c2 + vect2( elbow_pen_distance, 0 ).rotate( a25 ), lw1, "000000" )
#arrow( svg, c5, c5 - vect2( hands_pen_distance, 0 ).rotate( a54 ), lw1, "000000" )
arrow( svg, c0, c4, lw1, "ff6666" )
arrow( svg, c1, c1 + vect2( servo_arm_length, 0 ).rotate( ar ), lw1, "000000" )
if __name__ == "__main__":
doit()