opencv_ik_test.py

# import the necessary packages
import argparse
import cv2
import numpy as np
import math

# initialize the list of reference points and boolean indicating
# whether cropping is being performed or not
refPt = []
cropping = False
l1_end = None

def click_and_crop(event, x, y, flags, param):
	# grab references to the global variables
	global refPt, cropping
 	global image

	# if the left mouse button was clicked, record the starting
	# (x, y) coordinates and indicate that cropping is being
	# performed
	if event == cv2.EVENT_LBUTTONDOWN:
		refPt = [(x, y)]
		cropping = True
 
	# check to see if the left mouse button was released
	elif event == cv2.EVENT_LBUTTONUP:
		# record the ending (x, y) coordinates and indicate that
		# the cropping operation is finished
		refPt.append((x, y))
		cropping = False
 
		image = copy.copy()
		c = tc1((x,y))
		result = solve(c)
		if result is None:
			print "Skip - error"
			return
			
		print "solve for " + str(tc1((x,y))) + " results = " + str(result)

		if c[0] >= 0:
			draw_l1(math.radians(result[0]))
			draw_l2(math.radians(result[0] + (180+result[1]) ))
		else:
			draw_l1(math.radians(180-result[0]))
			draw_l2(math.radians(  180  - (  result[0] + (180+result[1]))  )) 

		cv2.circle(image, (x,y) ,5, (255,0,0))	
		cv2.imshow("image", image)

L1 = 175
L2 = 200
ARM = L1 + L2 

def tc(p):
	return (int(p[0]+ARM), int(ARM - p[1]))

def tc1(p):
	return (p[0]-ARM, ARM - p[1])

def draw_l2(angle):
	global l1_end
	l1 = tc1(l1_end)
	l2 = ( int(L2*math.cos(angle)) , int(L2*math.sin(angle)) )
	cv2.line(image, tc( ( l2[0] + l1[0] , l2[1] + l1[1] )  ) ,l1_end,(0,255,0),3)

def draw_l1(angle):
	global l1_end	
	l1_end = tc((L1*math.cos(angle),L1*math.sin(angle)))
	cv2.line(image, l1_end ,tc((0,2)),(0,150,0),3)

def sq(x):
	return x*x

def solve(p):
	global L1, L2
	c = math.sqrt( sq(p[0]) + sq(p[1]) )
	val = (sq(L2)  -  sq(L1)- sq(c)) / (-2*L1*c)
	if val > 1.0 or val < -1.0:
		print "error - out of domain"
		return None

	B = (math.acos(val)) * (180/math.pi)
	C = (math.acos((sq(c)  -  sq(L2) - sq(L1)) / (-2*L1*L2))) * (180/math.pi)
	theta = math.asin(p[1] / c) * (180/math.pi)
	a0 = B + theta
	a1 = C
	return (a0, a1)

w = ARM*2
image = np.zeros((w,w,3), np.uint8)
image[:] = (200,200,200)      
copy = image

cv2.circle(image, tc((0,0)), ARM, (255,0,0))
cv2.line(image, (0,ARM), (w, ARM) ,(0,150,0),2)
cv2.line(image, (ARM,0), (ARM, w),(0,150,0),2)

for i in range(w/50):
	cv2.line(image, (0,i*50), (w, i*50) , (150,150,150),1)
	cv2.line(image, (i*50,0), (i*50, w),(150,150,150),1)

cv2.namedWindow("image")
cv2.setMouseCallback("image", click_and_crop)
 
angle = 0

# keep looping until the 'q' key is pressed
while True:
	# display the image and wait for a keypress
	#image = copy.copy()
	key = cv2.waitKey(1) & 0xFF

	if key == ord("c"):
		break

	cv2.imshow("image", image)
  
# close all open windows
cv2.destroyAllWindows()