import numpy as np

Ad = np.array([[1,0],[0,1]])
Bd = np.array([[1,0],[0,1]])

Np = 10
x1, y1, x2, y2, x3, y3, x4, y4, x5, y5 = 0

pose = np.array([[x1, y1], [x2, y2], [x3, y3], [x4, y4], [x5, y5]])

pose_C1 = np.array([1,1])
pose_C2 = np.array([1,1])
Rp = []


# for i in Np:





print(Ad)

