Hello,
I have the AL5D kit from yourselves and Flowstone software, I have written the Kinematic conversion from my X,Y,Z coordinates and it all works fine on an excel spreadsheet but when used on the arm the tool tip height changes as the robot moves.
It does change linearly where the tip is about right when closest to the base and its about 2 cm too high at its reach?
my kinematics look like this:
X,Y0,Z0 are my input coordinates
L1 is shoulder to elbow
L2 is elbow to wrist
L3 is wrist to tool tip
toolangle is the angle of the tool relative to the ground
base is base height
Y1 is the Y coordinate after the base rotation
Y1 and Z1 are the coordinates of the wrist taking the tool tip length and angle into account
AB is the distance from shoulder to wrist
@x = @ins[0]
@y0 = @ins[1]
@z0 = @ins[2]
@L1 = @ins[3]
@L2 = @ins[4]
@L3 = @ins[5]
@toolangle = @ins[6]
@base = @ins[7]
@L12 = @L1*@L1
@L22 = @L2*@L2
#reach
@y1 = Math::sqrt(((@x-20)**2)+(@y0**2))
#tool angle
@toolanglerad = (@toolangle/180) * Math::PI
@c = Math::cos(@toolanglerad) * @L3 #y offset
@b = Math::sin(@toolanglerad) * @L3 #z offset
#wrist xy
@wristz = (@z0+@b)-@base
@wristy = @y1+@c
#base to wrist
@AB=Math::sqrt((@wristz**2)+(@wristy**2))
@AB2 = @AB*@AB
#shoulder angle
@alpharad = Math::acos(((@AB2+@L12)-@L22)/(2*(@AB*@L1)))
@alpha = @alpharad * (180/Math::PI)
@thetarad = Math::atan2(@wristz,@wristy)
@theta = @thetarad * (180/Math::PI)
@servo1 = @alpha+@theta
#elbow
@lambdarad = Math::acos(((@L12+@L22)-@AB2)/(2*(@L1*@L2)))
@lambda = @lambdarad * (180/Math::PI)
@servo2 = @lambda
#wrist
@G = 180-@servo1
@F = 180-@servo2
@R = 180-(@G+@F)
@servo3 = @toolangle - @R
#base rotate
@angleArad = Math::atan((@x-20)/@y0)
@angleA = @angleArad*(180/Math::PI)
@servo0 = 180-(90+@angleA)
output 0,@servo0
output 1,@servo1
output 2,@servo2
output 3,@servo3The angles output from this run into another module which converts the angles into servo position:
@angle = @ins[0]
@angle = @angle * 0.9#due to inaccuracy of servo
@llimit = @ins[1]
@ulimit = @ins[2]
@offset = @ins[3]
@range = @ulimit - @llimit
@servo = @angle *(@range/180)+@offset
output 0,@servo.ceil#ceil is round to nearestwithout the rounding and *0.9 factor the arm is in a different position but when moved along a straight line the tool tip remains too low near the base and too high when at full reach for a given height.
Can anyone see anything wrong with my calculations, or something im forgetting to do to correct this?
Many Thanks for looking 