A computing system comprising: a processor and an accessible memory, the computing system configured to: receive inputs including sensor data, vehicle operation and dynamics, VOD, data, a vehicle dynamic, VD, model, a nonlinear optimization, NLO, program, and an objective function; determine trajectories including a path with a starting pose and an ending pose over a sequence of time intervals while satisfying dynamic constraints and geometric constraints on a vehicle, the vehicle having a motor and a steering drive system for directing the vehicle along the path, wherein the determination of trajectories includes: determining the dynamic constraints based on dynamics of the vehicle operation and dynamics, VOD, from the VOD data, and determine the geometric constraints as a first-order differentiable function by generating a coordinate grid of an environment from the sensor data, to determine Cartesian coordinates of at least one obstacle and end-points of the vehicle; and using the VD model to formulate the NLO program for the vehicle, the NLO program uses as inputs the starting and ending pose, the dynamic and geometric constraints, to optimize the objective function of the trajectories while satisfying the dynamic and geometric constraints and vehicle operating constraints, for each time interval in the sequence of time intervals, so that the trajectories move the vehicle from the starting pose to the ending pose; and the computing system is further configured to: control the motor of the vehicle and the vehicle steering drive system to move the vehicle based upon the optimized trajectories obtained from the NLO program, wherein the geometric constraints are determined by a parameterization of the vehicle by end-points of the vehicle, wherein the end-points of the vehicle are defined so that any point on the vehicle can be obtained by using a convex combination of the end-points, and then finding a minimum of a distance of a parameterized position on the vehicle from each of the at least one obstacle in the environment and requiring that the minimum distance is above the radius of the smallest sphere circumscribing the obstacle, wherein the distance is calculated from the center of the smallest sphere circumscribing each obstacle. receive inputs including sensor data, vehicle operation and dynamics, VOD, data, a vehicle dynamic, VD, model, a nonlinear optimization, NLO, program, and an objective function; determine trajectories including a path with a starting pose and an ending pose over a sequence of time intervals while satisfying dynamic constraints and geometric constraints on a vehicle, the vehicle having a motor and a steering drive system for directing the vehicle along the path, wherein the determination of trajectories includes: determining the dynamic constraints based on dynamics of the vehicle operation and dynamics, VOD, from the VOD data, and determine the geometric constraints as a first-order differentiable function by generating a coordinate grid of an environment from the sensor data, to determine Cartesian coordinates of at least one obstacle and end-points of the vehicle; and using the VD model to formulate the NLO program for the vehicle, the NLO program uses as inputs the starting and ending pose, the dynamic and geometric constraints, to optimize the objective function of the trajectories while satisfying the dynamic and geometric constraints and vehicle operating constraints, for each time interval in the sequence of time intervals, so that the trajectories move the vehicle from the starting pose to the ending pose; determining the dynamic constraints based on dynamics of the vehicle operation and dynamics, VOD, from the VOD data, and determine the geometric constraints as a first-order differentiable function by generating a coordinate grid of an environment from the sensor data, to determine Cartesian coordinates of at least one obstacle and end-points of the vehicle; and using the VD model to formulate the NLO program for the vehicle, the NLO program uses as inputs the starting and ending pose, the dynamic and geometric constraints, to optimize the objective function of the trajectories while satisfying the dynamic and geometric constraints and vehicle operating constraints, for each time interval in the sequence of time intervals, so that the trajectories move the vehicle from the starting pose to the ending pose; and the computing system is further configured to: control the motor of the vehicle and the vehicle steering drive system to move the vehicle based upon the optimized trajectories obtained from the NLO program, wherein the geometric constraints are determined by a parameterization of the vehicle by end-points of the vehicle, wherein the end-points of the vehicle are defined so that any point on the vehicle can be obtained by using a convex combination of the end-points, and then finding a minimum of a distance of a parameterized position on the vehicle from each of the at least one obstacle in the environment and requiring that the minimum distance is above the radius of the smallest sphere circumscribing the obstacle, wherein the distance is calculated from the center of the smallest sphere circumscribing each obstacle.