PathPlanning.py

from scipy.spatial.transform import Rotation as R
import numpy as np

def main(arg):
    if(arg.load_traj is not None):
        print(f'load traj file: traj/{arg.load_traj}')
        executePath(arg,None)
    else:
        depth = int(input('depth(cm):'))
        height = int(input('height(cm):'))
        width = int(input('width(cm):'))
        traj = []
        for position in offsetOrigin(getRactanglePoints(depth, height, width/2, 'left'),[0,0,0.35]):
            print(f'point: {position}')
            '''
            for orientation in getRactangleOrientations(60,60,'left'):
                print(f'orientation: {orientation}')
                traj.append(np.append(position,orientation))
            '''
            traj.append(np.append(position,getQuatFromEuler(0,np.pi/2,-np.pi/2)))
        print(f'total steps: {len(traj)}')
        '''
        for i in range(len(traj)):
            print(f"{traj[i][0]: .3f},{traj[i][1]: .3f},{traj[i][2]: .3f} : {traj[i][3]: .3f},{traj[i][4]: .3f},{traj[i][5]: .3f},{traj[i][6]: .3f},")
        '''
        ans = input("execute the path?(y/n)")
        if(ans == "y"):
            executePath(arg,traj)
def executePath(arg,trajs):
    from alicia_d_sdk.controller import create_session, SynriaRobotAPI
    from alicia_d_sdk.kinematics import AliciaFollower, IKController
    from alicia_d_sdk.utils.logger import logger
    session = create_session(port=arg.port,baudrate=arg.baudrate,debug=False)
    controller = SynriaRobotAPI(session=session)
    ik_controller = IKController(AliciaFollower())

    #joints_list = ik_controller.solve_ik_multiprocess(np.zeros(6),trajs)
    if(arg.load_traj is not None):
        with open('traj/'+arg.load_traj,'rb') as file:
            logger.info(f'file loading: traj/{arg.load_traj}')
            joints_list = np.load(file)
            logger.info('loaded')
    else:
        logger.info('solving ik in multiprocessing')
        joints_list = ik_controller.solve_ik(np.zeros(6),trajs)
        logger.info('ik solved')

        if(arg.save_traj is not None):
            with open('traj/'+arg.save_traj,'wb') as file:
                np.save(file,joints_list)
                logger.info(f'file saved: traj/{arg.save_traj}')

    last_pose = None
    for index,pose in enumerate(joints_list):
        #logger.info(f'moveJ pose: {trajs[index]}')
        controller.moveJ(target_joints=pose,start_joint_angles=last_pose,silence=True)
        last_pose = pose
    logger.info('done')

def getQuatFromEuler(x,y,z):
    # 1. 從歐拉角創建一個旋轉物件
    # 參數：[roll, pitch, yaw], 單位是弧度
    # 'xyz' 表示旋轉順序是先X, 再Y, 再Z
    euler_angles_rad = np.array([x,y,z])
    rot = R.from_euler('xyz', euler_angles_rad)
    # 2. 從旋轉物件中獲取四元數
    # 注意：scipy的順序是 [x, y, z, w]
    return rot.as_quat()
def offsetOrigin(position_list, offset):
    offset_position_list = []
    for position in position_list:
        offset_position = [position[0]+offset[0], position[1]+offset[1], position[2]+offset[2]]
        offset_position_list.append(offset_position)
    return offset_position_list
def getRactanglePoints(width, height, depth, facing):
    position_list = []
    for x_step in range(width//5+1):
        for y_step in range(height//5+1):
            y_direction = 1 if x_step%2==0 else -1
            x_start = width/2
            x_offset = x_step * -5
            y_start = height/2 * y_direction
            y_offset = y_step * -5 * y_direction

            x_meter = (x_start + x_offset)/100
            y_meter = (y_start + y_offset)/100
            depth_meter = depth/100
            
            if(facing== 'left'):
                position = [-x_meter, -depth_meter, y_meter]
                position_list.append(position)
            if(facing== 'rear'):
                position = [depth_meter, -x_meter, y_meter]
                position_list.append(position)
            if(facing=='right'):
                position = [x_meter, depth_meter, y_meter]
                position_list.append(position)
    return position_list

def getRactangleOrientations(xAxisRange, yAxisRange, facing):
    orientation_list = []
    for xAxisStep in range(xAxisRange//10+1):
        for yAxisStep in range(2):
            y_direction = 1 if xAxisStep%2 == 0 else -1
            y_start = -yAxisRange/2 * y_direction
            y_offset = yAxisRange * yAxisStep * y_direction
            x_start = xAxisRange/2
            x_offset = xAxisStep * -10
            
            y_rad = np.deg2rad(y_start + y_offset)
            x_rad = np.deg2rad(x_start + x_offset)
            if(facing == 'left'):
                orientation = getQuatFromEuler(0, np.pi/2 - x_rad, -np.pi/2 + y_rad)
                orientation_list.append(orientation)
            if(facing == 'rear'):
                orientation = getQuatFromEuler(0, np.pi/2 - x_rad, y_start + y_rad)
                orientation_list.append(orientation)
            if(facing == 'right'):
                orientation = getQuatFromEuler(0, np.pi/2 - x_rad, np.pi/2 + y_rad)
                orientation_list.append(orientation)
    return orientation_list

if __name__ == '__main__':
    import argparse
    parser = argparse.ArgumentParser()
    # !!! 如果你能够读到版本号，版本号为5.4.19以上，则使用默认波特率1000000 !!!
    # !!! 如果显示超时或者多次尝试后没有版本号输出，则使用默认波特率921600 !!!
    parser.add_argument('--baudrate', type=int, default=921600, help="波特率")
    parser.add_argument('--port', type=str, default="", help="串口端口")
    parser.add_argument('--save-traj',type=str, default=None, help="儲存ik運算結果檔名")
    parser.add_argument('--load-traj',type=str, default=None, help="讀取ik運算結果檔名")
    args = parser.parse_args()
    main(args)