import os
import sys
import casadi as cd
import numpy as np
def rob_navi_dynamics_casadi(x, u, p):
x_r = x[0]
y_r = x[1]
theta_r = x[2]
v_r = x[3]
d_theta_r = x[4]
# control
acc_r = u[0] # 0
acc_theta_r = u[1] # 1
# for the robot
d_x_r = cd.cos(theta_r)*v_r
d_y_r = cd.sin(theta_r)*v_r
d_rob_state = cd.vertcat(d_x_r, \
d_y_r, \
d_theta_r, \
acc_r, \
acc_theta_r
)
return d_rob_state