def set_motion_profile_position(self, pos):
_, self.mp_x, self.mp_v, _ = (frccnt.generate_trapezoid_profile(
self.max_v, self.time_max_v, self.dt,
pos) if self.s_profile else frccnt.generate_s_curve_profile(
self.max_v, self.max_a, self.time_max_a, self.dt, pos))
self.motion_profile = True
self.index = 0
def set_position(self, pos):
_, self.x, self.v, self.a = (frccnt.generate_trapezoid_profile(
self.max_v, self.time_max_v, self.dt,
pos) if self.s_profile else frccnt.generate_s_curve_profile(
self.max_v, self.max_a, self.time_max_a, self.dt, pos))
self.index = 0
self.error_sum = 0
def main():
t, x, v, a = fct.generate_trapezoid_profile(max_v=7.0,
time_to_max_v=2.0,
dt=0.05,
goal=50.0)
plt.figure(1)
plt.subplot(3, 1, 1)
plt.ylabel("Position (m)")
plt.plot(t, x, label="Position")
plt.subplot(3, 1, 2)
plt.ylabel("Velocity (m/s)")
plt.plot(t, v, label="Velocity")
plt.subplot(3, 1, 3)
plt.xlabel("Time (s)")
plt.ylabel("Acceleration ($m/s^2$)")
plt.plot(t, a, label="Acceleration")
if "--noninteractive" in sys.argv:
latex.savefig("trapezoid_profile")
t, x, v, a = fct.generate_s_curve_profile(max_v=7.0,
max_a=3.5,
time_to_max_a=1.0,
dt=0.05,
goal=50.0)
plt.figure(2)
plt.subplot(3, 1, 1)
plt.ylabel("Position (m)")
plt.plot(t, x, label="Position")
plt.subplot(3, 1, 2)
plt.ylabel("Velocity (m/s)")
plt.plot(t, v, label="Velocity")
plt.subplot(3, 1, 3)
plt.xlabel("Time (s)")
plt.ylabel("Acceleration ($m/s^2$)")
plt.plot(t, a, label="Acceleration")
if "--noninteractive" in sys.argv:
latex.savefig("s_curve_profile")
else:
plt.show()
def main():
dt = 0.00505
single_jointed_arm = SingleJointedArm(dt)
single_jointed_arm.export_cpp_coeffs("SingleJointedArm", "Subsystems/")
if "--save-plots" in sys.argv or "--noninteractive" not in sys.argv:
try:
import slycot
plt.figure(1)
single_jointed_arm.plot_pzmaps()
except ImportError: # Slycot unavailable. Can't show pzmaps.
pass
if "--save-plots" in sys.argv:
plt.savefig("single_jointed_arm_pzmaps.svg")
t, xprof, vprof, aprof = frccnt.generate_s_curve_profile(max_v=0.5,
max_a=1,
time_to_max_a=0.5,
dt=dt,
goal=1.04)
# Generate references for simulation
refs = []
for i in range(len(t)):
r = np.array([[xprof[i]], [vprof[i]]])
refs.append(r)
if "--save-plots" in sys.argv or "--noninteractive" not in sys.argv:
plt.figure(2)
state_rec, ref_rec, u_rec = single_jointed_arm.generate_time_responses(
t, refs)
single_jointed_arm.plot_time_responses(t, state_rec, ref_rec, u_rec)
if "--save-plots" in sys.argv:
plt.savefig("single_jointed_arm_response.svg")
if "--noninteractive" not in sys.argv:
plt.show()
def main():
dt = 0.00505
single_jointed_arm = SingleJointedArm(dt)
single_jointed_arm.export_cpp_coeffs("SingleJointedArm", "subsystems/")
single_jointed_arm.export_java_coeffs("SingleJointedArm")
try:
import slycot
single_jointed_arm.plot_pzmaps()
except ImportError: # Slycot unavailable. Can't show pzmaps.
pass
if "--noninteractive" in sys.argv:
names = ["open-loop", "closed-loop", "observer"]
for i in range(3):
plt.figure(i + 1)
plt.savefig(f"single_jointed_arm_pzmap_{names[i]}.svg")
t, xprof, vprof, aprof = fct.generate_s_curve_profile(max_v=0.5,
max_a=1,
time_to_max_a=0.5,
dt=dt,
goal=1.04)
# Generate references for simulation
refs = []
for i in range(len(t)):
r = np.array([[xprof[i]], [vprof[i]]])
refs.append(r)
x_rec, ref_rec, u_rec, y_rec = single_jointed_arm.generate_time_responses(
t, refs)
single_jointed_arm.plot_time_responses(t, x_rec, ref_rec, u_rec)
if "--noninteractive" in sys.argv:
plt.savefig("single_jointed_arm_response.svg")
else:
plt.show()
def main():
dt = 0.00505
four_bar_lift = FourBarLift(dt)
four_bar_lift.export_cpp_coeffs("FourBarLift", "control/")
if "--save-plots" in sys.argv or "--noninteractive" not in sys.argv:
try:
import slycot
plt.figure(1)
four_bar_lift.plot_pzmaps()
except ImportError: # Slycot unavailable. Can't show pzmaps.
pass
if "--save-plots" in sys.argv:
plt.savefig("four_bar_lift_pzmaps.svg")
t, xprof, vprof, aprof = frccnt.generate_s_curve_profile(max_v=0.5,
max_a=1,
time_to_max_a=0.5,
dt=dt,
goal=1.04)
# Generate references for simulation
refs = []
for i in range(len(t)):
r = np.array([[xprof[i]], [vprof[i]]])
refs.append(r)
if "--save-plots" in sys.argv or "--noninteractive" not in sys.argv:
plt.figure(2)
state_rec, ref_rec, u_rec = four_bar_lift.generate_time_responses(
t, refs)
four_bar_lift.plot_time_responses(t, state_rec, ref_rec, u_rec)
if "--save-plots" in sys.argv:
plt.savefig("four_bar_lift_response.svg")
if "--noninteractive" not in sys.argv:
plt.show()
def main():
dt = 0.00505
drivetrain = Drivetrain(dt)
drivetrain.export_cpp_coeffs("Drivetrain", "subsystems/")
if "--save-plots" in sys.argv or "--noninteractive" not in sys.argv:
try:
import slycot
plt.figure(1)
drivetrain.plot_pzmaps()
except ImportError: # Slycot unavailable. Can't show pzmaps.
pass
if "--save-plots" in sys.argv:
plt.savefig("drivetrain_pzmaps.svg")
t, xprof, vprof, aprof = frccnt.generate_s_curve_profile(max_v=4.0,
max_a=3.5,
time_to_max_a=1.0,
dt=dt,
goal=50.0)
# Generate references for simulation
refs = []
for i in range(len(t)):
r = np.array([[xprof[i]], [vprof[i]], [xprof[i]], [vprof[i]]])
refs.append(r)
if "--save-plots" in sys.argv or "--noninteractive" not in sys.argv:
plt.figure(2)
x_rec, ref_rec, u_rec = drivetrain.generate_time_responses(t, refs)
drivetrain.plot_time_responses(t, x_rec, ref_rec, u_rec)
if "--save-plots" in sys.argv:
plt.savefig("drivetrain_response.svg")
if "--noninteractive" not in sys.argv:
plt.show()
def main():
dt = 0.00505
diff_drive = DifferentialDrive(dt)
diff_drive.export_cpp_coeffs("DifferentialDrive", "subsystems/")
diff_drive.export_java_coeffs("DifferentialDrive")
try:
import slycot
diff_drive.plot_pzmaps()
except ImportError: # Slycot unavailable. Can't show pzmaps.
pass
if "--noninteractive" in sys.argv:
names = ["open-loop", "closed-loop", "observer"]
for i in range(3):
plt.figure(i + 1)
plt.savefig(f"differential_drive_pzmap_{names[i]}.svg")
t, xprof, vprof, aprof = fct.generate_s_curve_profile(max_v=4.0,
max_a=3.5,
time_to_max_a=1.0,
dt=dt,
goal=50.0)
# Generate references for simulation
refs = []
for i in range(len(t)):
r = np.array([[xprof[i]], [vprof[i]], [xprof[i]], [vprof[i]]])
refs.append(r)
x_rec, ref_rec, u_rec, y_rec = diff_drive.generate_time_responses(t, refs)
diff_drive.plot_time_responses(t, x_rec, ref_rec, u_rec)
if "--noninteractive" in sys.argv:
plt.savefig("differential_drive_response.svg")
else:
plt.show()
def main():
dt = 0.00505
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = fct.generate_s_curve_profile(
max_v=4.0, max_a=3.5, time_to_max_a=1.0, dt=dt, goal=10.0
)
# Generate references for LQR
refs = []
for i in range(len(t)):
r = np.array([[vprof[i]], [vprof[i]]])
refs.append(r)
# Run LQR
state_rec, ref_rec, u_rec, y_rec = drivetrain.generate_time_responses(t, refs)
plt.figure(1)
drivetrain.plot_time_responses(t, state_rec, ref_rec, u_rec)
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_decoupled_vel_lqr_profile")
# Initial robot pose
pose = Pose2d(2, 0, np.pi / 2.0)
desired_pose = Pose2d()
# Ramsete tuning constants
b = 2
zeta = 0.7
vl = float("inf")
vr = float("inf")
x_rec = []
y_rec = []
vref_rec = []
omegaref_rec = []
v_rec = []
omega_rec = []
ul_rec = []
ur_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(0)
omega_rec.append(0)
# Run Ramsete
i = 0
while i < len(t) - 1:
desired_pose.x = 0
desired_pose.y = xprof[i]
desired_pose.theta = np.pi / 2.0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], 0, pose, b, zeta)
vl, vr = get_diff_vels(vref, omegaref, drivetrain.rb * 2.0)
next_r = np.array([[vl], [vr]])
drivetrain.update(next_r)
vc = (drivetrain.x[0, 0] + drivetrain.x[1, 0]) / 2.0
omega = (drivetrain.x[1, 0] - drivetrain.x[0, 0]) / (2.0 * drivetrain.rb)
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(vc)
omega_rec.append(omega)
# Update nonlinear observer
pose.x += vc * math.cos(pose.theta) * dt
pose.y += vc * math.sin(pose.theta) * dt
pose.theta += omega * dt
if i < len(t) - 1:
i += 1
plt.figure(2)
plt.plot([0] * len(t), xprof, label="Reference trajectory")
plt.plot(x_rec, y_rec, label="Ramsete controller")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
# Equalize aspect ratio
xlim = plt.xlim()
width = abs(xlim[0]) + abs(xlim[1])
ylim = plt.ylim()
height = abs(ylim[0]) + abs(ylim[1])
if width > height:
plt.ylim([-width / 2, width / 2])
else:
plt.xlim([-height / 2, height / 2])
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_decoupled_response")
plt.figure(3)
num_plots = 4
plt.subplot(num_plots, 1, 1)
plt.title("Time domain responses")
plt.ylabel(
"Velocity (m/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, vref_rec, label="Reference")
plt.plot(t, v_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 2)
plt.ylabel(
"Angular rate (rad/s)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, omegaref_rec, label="Reference")
plt.plot(t, omega_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 3)
plt.ylabel(
"Left voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ul_rec, label="Control effort")
plt.legend()
plt.subplot(num_plots, 1, 4)
plt.ylabel(
"Right voltage (V)",
horizontalalignment="right",
verticalalignment="center",
rotation=45,
)
plt.plot(t, ur_rec, label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_decoupled_vel_lqr_response")
else:
plt.show()
def main():
dt = 0.05
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = fct.generate_s_curve_profile(max_v=4.0,
max_a=3.5,
time_to_max_a=1.0,
dt=dt,
goal=10.0)
# Initial robot pose
pose = Pose2d(2, 0, np.pi / 2.0)
desired_pose = Pose2d()
twist_pose = Pose2d(2, 0, np.pi / 2.0)
# Ramsete tuning constants
b = 2
zeta = 0.7
vl = float("inf")
vr = float("inf")
x_rec = []
y_rec = []
twist_x_rec = []
twist_y_rec = []
vref_rec = []
omegaref_rec = []
v_rec = []
omega_rec = []
ul_rec = []
ur_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
twist_x_rec.append(twist_pose.x)
twist_y_rec.append(twist_pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(0)
omega_rec.append(0)
# Run Ramsete
i = 0
while i < len(t) - 1:
desired_pose.x = 0
desired_pose.y = xprof[i]
desired_pose.theta = np.pi / 2.0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], 0, pose, b, zeta)
vl, vr = get_diff_vels(vref, omegaref, drivetrain.rb * 2.0)
next_r = np.array([[vl], [vr]])
drivetrain.update(next_r)
vc = (drivetrain.x[0, 0] + drivetrain.x[1, 0]) / 2.0
omega = (drivetrain.x[1, 0] - drivetrain.x[0, 0]) / (2.0 *
drivetrain.rb)
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
twist_x_rec.append(twist_pose.x)
twist_y_rec.append(twist_pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(vc)
omega_rec.append(omega)
# Update nonlinear observer
pose.x += vc * math.cos(pose.theta) * dt
pose.y += vc * math.sin(pose.theta) * dt
pose.theta += omega * dt
twist_pose.exp(Twist2d(vc, 0, omega), dt)
if i < len(t) - 1:
i += 1
plt.figure(1)
plt.ylabel("Odometry error (m)")
plt.plot(t, np.subtract(twist_x_rec, x_rec), label="Error in x")
plt.plot(t, np.subtract(twist_y_rec, y_rec), label="Error in y")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latex.savefig("ramsete_twist_odometry_error")
else:
plt.show()
def main():
dt = 0.00505
drivetrain = Drivetrain(dt)
t, xprof, vprof, aprof = frccnt.generate_s_curve_profile(max_v=4.0,
max_a=3.5,
time_to_max_a=1.0,
dt=dt,
goal=10.0)
# Generate references for LQR
refs = []
for i in range(len(t)):
r = np.matrix([[vprof[i]], [0]])
refs.append(r)
# Run LQR
state_rec, ref_rec, u_rec = drivetrain.generate_time_responses(t, refs)
plt.figure(1)
subplot_max = drivetrain.sysd.states + drivetrain.sysd.inputs
for i in range(drivetrain.sysd.states):
plt.subplot(subplot_max, 1, i + 1)
plt.ylabel(drivetrain.state_labels[i])
if i == 0:
plt.title("Time-domain responses")
if i == 1:
plt.ylim([-3, 3])
plt.plot(t,
drivetrain.extract_row(state_rec, i),
label="Estimated state")
plt.plot(t, drivetrain.extract_row(ref_rec, i), label="Reference")
plt.legend()
for i in range(drivetrain.sysd.inputs):
plt.subplot(subplot_max, 1, drivetrain.sysd.states + i + 1)
plt.ylabel(drivetrain.u_labels[i])
plt.plot(t, drivetrain.extract_row(u_rec, i), label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_coupled_vel_lqr_profile")
# Initial robot pose
pose = Pose2d(2, 0, np.pi / 2.0)
desired_pose = Pose2d()
# Ramsete tuning constants
b = 2
zeta = 0.7
vref = float("inf")
omegaref = float("inf")
x_rec = []
y_rec = []
vref_rec = []
omegaref_rec = []
v_rec = []
omega_rec = []
ul_rec = []
ur_rec = []
# Log initial data for plots
vref_rec.append(0)
omegaref_rec.append(0)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(0)
omega_rec.append(0)
# Run Ramsete
drivetrain.reset()
i = 0
while i < len(t) - 1:
desired_pose.x = 0
desired_pose.y = xprof[i]
desired_pose.theta = np.pi / 2.0
# pose_desired, v_desired, omega_desired, pose, b, zeta
vref, omegaref = ramsete(desired_pose, vprof[i], 0, pose, b, zeta)
next_r = np.matrix([[vref], [omegaref]])
drivetrain.update(next_r)
# Log data for plots
vref_rec.append(vref)
omegaref_rec.append(omegaref)
x_rec.append(pose.x)
y_rec.append(pose.y)
ul_rec.append(drivetrain.u[0, 0])
ur_rec.append(drivetrain.u[1, 0])
v_rec.append(drivetrain.x[0, 0])
omega_rec.append(drivetrain.x[1, 0])
# Update nonlinear observer
pose.x += drivetrain.x[0, 0] * math.cos(pose.theta) * dt
pose.y += drivetrain.x[0, 0] * math.sin(pose.theta) * dt
pose.theta += drivetrain.x[1, 0] * dt
if i < len(t) - 1:
i += 1
plt.figure(2)
plt.plot([0] * len(t), xprof, label="Reference trajectory")
plt.plot(x_rec, y_rec, label="Ramsete controller")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
# Equalize aspect ratio
ylim = plt.ylim()
width = abs(ylim[0]) + abs(ylim[1])
plt.xlim([-width / 2, width / 2])
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_coupled_response")
plt.figure(3)
num_plots = 4
plt.subplot(num_plots, 1, 1)
plt.title("Time-domain responses")
plt.ylabel("Velocity (m/s)")
plt.plot(t, vref_rec, label="Reference")
plt.plot(t, v_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 2)
plt.ylabel("Angular rate (rad/s)")
plt.plot(t, omegaref_rec, label="Reference")
plt.plot(t, omega_rec, label="Estimated state")
plt.legend()
plt.subplot(num_plots, 1, 3)
plt.ylabel("Left voltage (V)")
plt.plot(t, ul_rec, label="Control effort")
plt.legend()
plt.subplot(num_plots, 1, 4)
plt.ylabel("Right voltage (V)")
plt.plot(t, ur_rec, label="Control effort")
plt.legend()
plt.xlabel("Time (s)")
if "--noninteractive" in sys.argv:
latexutils.savefig("ramsete_coupled_vel_lqr_response")
else:
plt.show()
