Last active
November 16, 2022 00:41
-
-
Save ven-kyoshiro/4a0d3489cf206bac2a26a9b9986671ef to your computer and use it in GitHub Desktop.
visualize2Dcollision
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 | |
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): | |
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 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 | |
j1 = np.pi/8 #[rad] | |
j2 = np.pi/8 | |
l1=0.5 #[m] | |
l2=0.5 | |
box_x=0.0 | |
box_y=0.4 | |
box_size=0.2 | |
origin_xy = [0,0] | |
############## 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 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() | |
ax = plt.gca() | |
ax.set_aspect("equal") | |
for arm in arms: | |
arm.plot(ax,color="red") | |
for b_line in boxes: | |
b_line.plot(ax) | |
############## visualize c-space | |
import matplotlib.pyplot as plt | |
j1s = [] | |
j2s = [] | |
is_collide_list = [] | |
for _j1 in np.linspace(0,np.pi,300): | |
for _j2 in np.linspace(-np.pi,np.pi,300): | |
if is_collide(_j1,_j2,l1,l2,box_x,box_y,box_size): | |
j1s.append(_j1) | |
j2s.append(_j2) | |
plt.figure(dpi=200) | |
plt.scatter(j1s,j2s,label="collision",s=0.2) | |
plt.xlabel("j1[rad]") | |
plt.ylabel("j2[rad]") | |
plt.legend() | |
ax = plt.gca() | |
ax.set_aspect("equal") |
Author
ven-kyoshiro
commented
Nov 15, 2022
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment