visualize2Dcollision

visualize_2dof_robot_collision_area.py

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")