def test_solve():
'''
거리를 받아 움직이어야하는 좌표값을 반환하는 코드의 동작을 테스트
:return:
'''
print('거리를 받아 움직어야하는 모터 회전값을 반환하는 코드의 동작을 테스트')
try:
while True:
inputs = raw_input("Enter point (x,y,z): (split by space)").split(
' ')[:3]
coords = [float(int(coord))
for coord in inputs] # string을 float으로 바꿈
x, y, z = coords
radians = [0, 0, 0]
if kinematics.solve(x, y, z, angles=radians):
degrees = [
kinematics.radian2degree(radian) for radian in radians
]
print(
"radian output(shoulder,right,z) : ({:.4f},{:.4f},{:.4f})"
.format(*radians))
print(
"degree output(shoulder,right,left) : ({:.4f},{:.4f},{:.4f})"
.format(*degrees))
else:
print('failed....')
except KeyboardInterrupt:
print("TEST END")
def goDirectlyTo(self, tarx, tary, tarz):
angles = [0, 0, 0]
#print("From {},{},{}".format(self.x, self.y, self.z))
#print("goto=> {},{},{}".format(tarx, tary, tarz))
#print(kinematics.cart2polar(tary, tarx))
if kinematics.solve(tarx, tary, tarz, angles):
radBase = angles[0]
radShoulder = angles[1]
radElbow = angles[2]
print("angle => {},{},{}".format(radBase, radElbow, radShoulder))
pwm_out_base = self.angle2pwm("base", radBase)
self.servoPWM["base"].ChangeDutyCycle(pwm_out_base)
pwm_out_shoulder = self.angle2pwm("shoulder", radShoulder)
self.servoPWM["shoulder"].ChangeDutyCycle(pwm_out_shoulder)
pwm_out_elbow = self.angle2pwm("elbow", radElbow)
self.servoPWM["elbow"].ChangeDutyCycle(pwm_out_elbow)
print("base=> {},{},{}".format(self.rad2deg(radBase),
self.rad2deg(radElbow),
self.rad2deg(radShoulder)))
print("pwms=> {},{},{}".format(pwm_out_base, pwm_out_elbow,
pwm_out_shoulder))
self.x = tarx
self.y = tary
self.z = tarz
def goDirectlyTo(self, x, y, z):
"""Set servo angles so as to place the gripper at a given Cartesian point as quickly as possible, without caring what path it takes to get there"""
angles = [0, 0, 0]
if kinematics.solve(x, y, z, angles):
radBase = angles[0]
radShoulder = angles[1]
radElbow = angles[2]
radBase, pwm_val = self.angle2pwm("base", radBase)
self.setPwm(self.base, pwm_val)
radShoulder, pwm_val = self.angle2pwm("shoulder", radShoulder)
self.setPwm(self.shoulder, pwm_val)
radElbow, pwm_val = self.angle2pwm("elbow", radElbow)
self.setPwm(self.elbow, pwm_val)
self.x, self.y, self.z = kinematics.unsolve(
radBase, radShoulder, radElbow)
print "goto %s (actual %s ; ang bse %s ) " % ([
int(x), int(y), int(z)
], [int(self.x), int(self.y),
int(self.z)], [
int(math.degrees(radBase)),
int(math.degrees(radShoulder)),
int(math.degrees(radElbow))
])
def goDirectlyTo(self, x, y, z):
"""Set servo angles so as to place the gripper at a given Cartesian point as quickly as possible, without caring what path it takes to get there"""
angles = [0,0,0]
if kinematics.solve(x, y, z, angles):
radBase = angles[0]
radShoulder = angles[1]
radElbow = angles[2]
PWM.set_duty_cycle(self.base, self.angle2pwm("base", radBase))
PWM.set_duty_cycle(self.shoulder, self.angle2pwm("shoulder", radShoulder))
PWM.set_duty_cycle(self.elbow, self.angle2pwm("elbow", radElbow))
self.x = x
self.y = y
self.z = z
def isReachable(self, x, y, z):
"""Returns True if the point is (theoretically) reachable """
angles = [0,0,0]
if kinematics.solve(x, y, z, angles) != None:
radBase, _ = self.angle2pwm("base", angles[0])
if radBase == angles[0]:
radShoulder, _ = self.angle2pwm("shoulder", angles[1])
if radShoulder == angles[1]:
radElbow, _ = self.angle2pwm("elbow", angles[2])
if radElbow == angles[2]:
return True
print 'Not reachable: %d %d %d'%(x,y,z)
return False
def createWorkspaceGrid(x_size, y_size, z_size, r_T_g, lenPerCell):
"""
Returns a grid (3D numpy array) with 1 being reachable and 0 otherwise
The grid's origin is described by r_T_g,
The grid has the size specified by the arguements,
The origin position is at the top left corner of the robot when looking at the robot's back
when looking at the robot's back, z-axis points to the board, y-axis downward, x-axis right
Arguments:
x_size, y_size, z_size are integers representing the number of grid cells in z, y, x directions.
r_T_g is a 4x4 numpy array representing the transformation matrix from robot to grid
"""
# Note that the convention is grid[z,y,x]
grid = np.zeros((z_size, y_size, x_size))
# this is needed to use kinematics.solve() below
dummy_angles = [0, 0, 0]
"""
The for-loop loops through each grid cell,
converts it to millimeters,
transforms it from grid coordinate system to robot coordinate system,
and calls kinematics.solve() to determine if the cell is reachable
# ix,iy,iz are the x,y,z-indices of the grid. Note that the convention is grid[z,y,x] for numpy arrays
# gx,gy,gz are the x,y,z in millimeters in the grid coordinate system. g_vec is a column vector containing them
# rx,ry,rz are the x,y,z in millimeters in the robot coordinate system . r_vec is a column vector containing them
"""
for ix in range(grid.shape[2]):
for iy in range(grid.shape[1]):
for iz in range(grid.shape[0]):
# convert them to millimeters and make a column vector
(gx, gy, gz) = (ix * lenPerCell, iy * lenPerCell,
iz * lenPerCell)
g_vec = np.array([[gx], [gy], [gz], [1]])
# transform to robot coordinate system
r_vec = r_T_g.dot(g_vec)
rx = r_vec[0, 0]
ry = r_vec[1, 0]
rz = r_vec[2, 0]
# solve: return 1 if reachable, 0 otherwise
if (not (rx == 0 and ry == 0)) and (kinematics.solve(
rx, ry, rz, dummy_angles) == True):
grid[iz, iy, ix] = 1
return grid
def goDirectlyTo(self, x, y, z):
"""Set servo angles so as to place the gripper at a given Cartesian point as quickly as possible, without caring what path it takes to get there"""
angles = [0,0,0]
if kinematics.solve(x, y, z, angles):
radBase = angles[0]
radShoulder = angles[1]
radElbow = angles[2]
self.pwm.setPWM(self.base, 0, self.angle2pwm("base", radBase))
self.pwm.setPWM(self.shoulder, 0, self.angle2pwm("shoulder", radShoulder))
self.pwm.setPWM(self.elbow, 0, self.angle2pwm("elbow", radElbow))
self.x = x
self.y = y
self.z = z
print "goto %s, radians %s" % ([x,y,z], [radBase, radShoulder, radElbow])
def goDirectlyTo(self, x, y, z):
"""Set servo angles so as to place the gripper at a given Cartesian point as quickly as possible, without caring what path it takes to get there"""
angles = [0, 0, 0]
if kinematics.solve(x, y, z, angles):
print "angles", angles
print "degs", [int(x * 180.0) / pi for x in angles]
self.radBase = angles[0]
self.radShoulder = angles[1]
self.radElbow = angles[2]
self.BasePos = self.angle2pwm("base", self.radBase)
self.ShoulderPos = self.angle2pwm("shoulder", self.radShoulder)
self.ElbowPos = self.angle2pwm("elbow", self.radElbow)
self.x = x
self.y = y
self.z = z
print "goto %s" % ([x, y, z])
def test_move_by_position():
'''
좌표값을 받아 그 위치로 움직이는 코드의 동작을 테스트
:return:
'''
print("좌표값을 받아 그 위치로 움직이는 코드의 동작을 테스트")
shoulder_servo = initialize_servo(SHOULDER_PORT_NUM)
right_arm_servo = initialize_servo(RIGHT_PORT_NUM)
left_arm_servo = initialize_servo(LEFT_PORT_NUM)
print 'init angle : 90 degree! '
angles = [90, 90, 90]
move_direct(shoulder_servo, angles[0])
move_direct(right_arm_servo, angles[1])
move_direct(left_arm_servo, angles[2])
sleep(1)
try:
while True:
inputs = raw_input("Enter position (x,y,z):").split(' ')[:3]
if len(inputs) != 3:
continue
coords = [float(int(coord))
for coord in inputs] # string을 float으로 바꿈
x, y, z = coords
radians = [0, 0, 0]
if kinematics.solve(x, y, z, angles=radians):
degrees = [
kinematics.radian2degree(radian) for radian in radians
]
else:
continue
new_angles = [int(angle) for angle in degrees]
move_smooth(shoulder_servo, angles[0], new_angles[0])
move_smooth(right_arm_servo, angles[1], new_angles[1])
move_smooth(left_arm_servo, angles[2], new_angles[2])
angles = new_angles
except KeyboardInterrupt:
print "GPIO. cleanup()"
GPIO.cleanup()
def goDirectlyTo(self, x, y, z):
"""Set servo angles so as to place the gripper at a given Cartesian point as quickly as possible, without caring what path it takes to get there"""
angles = [0,0,0]
if kinematics.solve(x, y, z, angles):
radBase = angles[0]
radShoulder = angles[1]
radElbow = angles[2]
# base
self.servo.setTarget(0, self.angle2pwm("base", radBase) )
# shoulder
self.servo.setTarget(1, self.angle2pwm("shoulder", radShoulder) )
# elbow
self.servo.setTarget(2, self.angle2pwm("elbow", radElbow) )
self.x = x
self.y = y
self.z = z
print "goto %s" % ([x,y,z])
def isReachable(self, x, y, z):
"""Returns True if the point is (theoretically) reachable by the gripper"""
radBase = 0
radShoulder = 0
radElbow = 0
return kinematics.solve(x, y, z, radBase, radShoulder, radElbow)
def isReachable(self, x, y, z):
"""Returns True if the point is (theoretically) reachable by the gripper"""
radBase = 0
radShoulder = 0
radElbow = 0
return kinematics.solve(x, y, z, radBase, radShoulder, radElbow)
