一种确定轨迹的方法和系统，该轨迹包括在一系列时间间隔内具有初始姿势和结束姿势的路径，该轨迹同时满足对具有连杆的机器人臂的动力学约束和几何约束。基于每个连杆的动力学确定动力学约束，并通过生成环境的坐标网格将几何约束确定为一阶可微分函数，以确定每个连杆的端点和障碍物的笛卡尔坐标。使用多连杆动力学模型制订针对机器人臂的非线性优化(NLO)程序，NLO程序使用初始姿势和结束姿势、动力学约束和几何约束作为输入，以优化轨迹的目标函数，对于每个时间间隔，所述轨迹同时满足所述动力学约束和所述几何约束以及机器人臂操作约束。控制机器人驱动器的马达，以基于轨迹移动机器人臂。