Last active
August 3, 2023 04:43
-
-
Save ven-kyoshiro/608b69e19a92b89efa1c9adff6ed4267 to your computer and use it in GitHub Desktop.
configuration space visualization
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
import matplotlib.pyplot as plt | |
def ccw(A,B,C): | |
return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0]) | |
def intersect(A,B,C,D): | |
# check AB vs CD | |
return ccw(A,C,D) != ccw(B,C,D) and ccw(A,B,C) != ccw(A,B,D) | |
class LineSegment: | |
def __init__(self,xy1,xy2): | |
self.xy1 = xy1 | |
self.xy2 = xy2 | |
def plot(self,ax,color="black"): | |
ax.plot( | |
[self.xy1[0],self.xy2[0]], | |
[self.xy1[1],self.xy2[1]], | |
color=color) | |
def is_cross(self,ls): | |
return intersect(self.xy1,self.xy2,ls.xy1,ls.xy2) | |
def is_collide(j1,j2,l1=0.5,l2=0.5,box_x=0.0,box_y=0.4,box_size=0.2,box_rot=0.0): | |
origin_xy = [0,0] | |
elbow_xy = [l1*np.cos(j1),l1*np.sin(j1)] | |
hand_xy = [elbow_xy[0]+l2*np.cos(j2+j1),elbow_xy[1]+l2*np.sin(j2+j1)] | |
arm1 = LineSegment(origin_xy,elbow_xy) | |
arm2 = LineSegment(elbow_xy,hand_xy) | |
arms = [arm1,arm2] | |
box_center = [box_x,box_y] | |
ths = [(ii/2 + 1/4) * np.pi + box_rot for ii in range(4)] | |
boxes = [LineSegment( | |
[box_size*np.cos(th)+box_center[0], box_size*np.sin(th)+box_center[1]], | |
[box_size*np.cos(th+np.pi/2)+box_center[0],box_size*np.sin(th+np.pi/2)+box_center[1]] | |
) for th in ths] | |
for arm in arms: | |
for b_line in boxes: | |
if arm.is_cross(b_line): | |
return True | |
return False | |
def generate_image(box_x,box_y,box_rot,box_size,save_fig_name,resolution=200): | |
j1 = np.pi/2-np.pi/8#[rad] | |
j2 = np.pi/8 | |
l1=1#0.5 #[m] | |
l2=1#0.5 | |
origin_xy = [0,0] | |
#print(is_collide(j1,j2,l1,l2,box_x,box_y,box_size)) | |
############## visualize robot | |
elbow_xy = [l1*np.cos(j1),l1*np.sin(j1)] | |
hand_xy = [elbow_xy[0]+l2*np.cos(j2+j1),elbow_xy[1]+l2*np.sin(j2+j1)] | |
arm1 = LineSegment(origin_xy,elbow_xy) | |
arm2 = LineSegment(elbow_xy,hand_xy) | |
arms = [arm1,arm2] | |
box_center = [box_x,box_y] | |
ths = [(ii/2 + 1/4) * np.pi + box_rot for ii in range(4)] | |
boxes = [LineSegment( | |
[box_size*np.cos(th)+box_center[0], box_size*np.sin(th)+box_center[1]], | |
[box_size*np.cos(th+np.pi/2)+box_center[0],box_size*np.sin(th+np.pi/2)+box_center[1]] | |
) for th in ths] | |
plt.figure() | |
fig,ax = plt.subplots(1,2,dpi=200,figsize=(10,10)) | |
ax[0].set_aspect("equal") | |
for arm in arms: | |
arm.plot(ax[0],color="red") | |
for b_line in boxes: | |
b_line.plot(ax[0]) | |
ax[0].set_xlim(-1,1) | |
ax[0].set_ylim(0,4) | |
############## visualize c-space | |
j1s = [] | |
j2s = [] | |
is_collide_list = [] | |
for _j1 in np.linspace(0,np.pi,resolution): | |
for _j2 in np.linspace(-np.pi,np.pi,resolution*2): | |
if is_collide(_j1,_j2,l1,l2,box_x,box_y,box_size,box_rot): | |
j1s.append(_j1) | |
j2s.append(_j2) | |
ax[1].scatter(j1s,j2s,label="collision",s=0.2) | |
ax[1].set_xlabel("j1[rad]") | |
ax[1].set_ylabel("j2[rad]") | |
ax[1].legend() | |
ax[1].set_aspect("equal") | |
ax[1].set_xlim(0,np.pi) | |
ax[1].set_ylim(-np.pi,np.pi) | |
plt.savefig(save_fig_name) | |
if __name__ == "__main__": | |
generate_image(box_x =0.0,box_y=2.0,box_rot=np.pi/3,box_size=0.3,save_fig_name="test.png") |
Author
ven-kyoshiro
commented
Aug 3, 2023
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment