configuration space visualization

configuration_space_visualization.py

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