direct.py

#!/usr/bin/env python

import os
import math
import cmath
import numpy as np
import cv2 as cv
import matplotlib.pyplot as plt

img = cv.imread('bi.jpg')

obj_x = 600
obj_y = 300

rob_x = 250
rob_y = 165

radius = math.sqrt((obj_y-rob_y)**2 + (obj_x-rob_x)**2)
tetha = 0.0
tetha_delta = 20 * np.pi/180.0
tetha_offset = math.atan2(obj_y-rob_y, obj_x-rob_x)

count = int(2* np.pi / tetha_delta) + 1

arm_max_flex = 100 ## pixels
base_offset = 20 ## pixels

reachable_points = []

for i in range(count):
    
    ox = rob_x - obj_x
    oy = rob_y - obj_y
    
    new_x = ox * np.cos(tetha) - oy * np.sin(tetha)
    new_y = ox * np.sin(tetha) + oy * np.cos(tetha)

    new_x = new_x + obj_x
    new_y = new_y + obj_y
    
    for rad in range(int(radius)):
        
        i = int(obj_x + rad * math.cos(tetha_offset-tetha))
        j = int(obj_y + rad * math.sin(tetha_offset-tetha))

        if img[j, i, 0] == 0 and rad < arm_max_flex:
            i = int(obj_x + (rad + base_offset) * math.cos(tetha_offset-tetha))
            j = int(obj_y + (rad + base_offset) * math.sin(tetha_offset-tetha))
            
            reachable_points.append((i, j))
            print (rad)
            break
        if img[j, i, 0] != 0 and rad > arm_max_flex:
            break
        
        
        cv.circle(img, (i, j), 1, (100, 100, 255), 1)
    cv.circle(img, (int(new_x), int(new_y)), 15, (127, 255, 0), -1)
    
    tetha = (tetha_delta + tetha)


min_dist = 1000.0
min_index = None
for index, rp in enumerate(reachable_points):
    dist = np.linalg.norm(np.array(rp) - np.array([rob_x, rob_y]))
    if dist < min_dist:
        min_dist = dist
        min_index = index
        
    cv.circle(img, rp, 2, (0, 255, 255), 1)
cv.circle(img, reachable_points[min_index], 4, (0, 255, 0), -1)
    
cv.circle(img, (obj_x, obj_y), int(radius), (255, 255, 0), 1)
cv.circle(img, (obj_x, obj_y), 10, (0, 255, 0), -1)
cv.circle(img, (rob_x, rob_y), 10, (0, 0, 255), -1)

cv.namedWindow('img', cv.WINDOW_NORMAL)
cv.imshow('img', img)
cv.waitKey(0)