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()