from math import pi as PI import numpy as np import sympy as sp import temporal_logic.signal_tl as stl SIGNALS = ('x_d', 'y_d', 'z_d', 'roll', 'pitch', 'yaw', 'dot_x', 'dot_y', 'dot_z', 'dot_roll', 'dot_pitch', 'dot_yaw', 'x_g', 'y_g', 'z_g', 'collision') dt = 50e-3 # s (x_d, y_d, z_d, roll_d, pitch_d, yaw_d, dotx, doty, dotz, dotax, dotay, dotaz, x_g, y_g, z_g, collision) = stl.signals(SIGNALS) drone_pos = sp.Matrix([x_d, y_d, z_d]) drone_ori = sp.Matrix([roll_d, pitch_d, yaw_d]) drone_lv = sp.Matrix([dotx, doty, dotz]) drone_av = sp.Matrix([dotax, dotay, dotaz]) goal_pos = sp.Matrix([x_g, y_g, z_g]) POSITION_SPEC = stl.Predicate((goal_pos - drone_pos).norm() <= 0.01) GOAL_VELOCITY_SPEC = POSITION_SPEC >> stl.Predicate( drone_lv.norm() <= 0.001) # Reached goal => stay still # Keep roll and pitch within 30deg ANGLE_CONSTRAINT = (stl.Predicate(abs(roll_d) <= PI / 6) & stl.Predicate(abs(pitch_d) <= PI / 6))
import temporal_logic.signal_tl as stl SIGNALS = ('x', 'x_dot', 'theta', 'theta_dot') x, x_dot, theta, theta_dot = stl.signals(SIGNALS) SPEC = stl.G( stl.F(x_dot < abs(0.01)) & (abs(theta) < 5) & (abs(x) < 0.5) & stl.F(abs(theta) < 1))
import temporal_logic.signal_tl as stl SIGNALS = ('p_x', 'p_y', 'v_x', 'v_y', 'theta', 'avel', 'contact_l', 'contact_r') p_x, p_y, v_x, v_y, theta, avel, contact_l, contact_r = stl.signals(SIGNALS) POSITION_SPEC = stl.F()
'lidar0', 'lidar1', 'lidar2', 'lidar3', 'lidar4', 'lidar5', 'lidar6', 'lidar7', 'lidar8', 'lidar9', ) (hull_angle, hull_angularVelocity, vel_x, vel_y, hip_joint_1_angle, hip_joint_1_speed, knee_joint_1_angle, knee_joint_1_speed, leg_1_ground_contact_flag, hip_joint_2_angle, hip_joint_2_speed, knee_joint_2_angle, knee_joint_2_speed, leg_2_ground_contact_flag, lidar0, lidar1, lidar2, lidar3, lidar4, lidar5, lidar6, lidar7, lidar8, lidar9) = stl.signals( SIGNALS) SPEC = stl.G( stl.And( # Be moving forward always (vel_x > 1), # Reduce hull tilt to ~ +- 12deg (abs(hull_angle) <= 0.2), # Reduce hull angular velocity (abs(hull_angularVelocity) < 2), # Prevents running stl.Implies((leg_1_ground_contact_flag > 0), stl.And( # Set foot in 10-16 timesteps stl.F(leg_2_ground_contact_flag > 0, (10, 16)), # Make sure to actually lift leg
'x_d', 'y_d', 'z_d', 'roll', 'pitch', 'yaw', 'dot_x', 'dot_y', 'dot_z', 'dot_roll', 'dot_pitch', 'dot_yaw', 'x_g', 'y_g', 'z_g', 'collision' ) dt = 50e-2 # s (x_d, y_d, z_d, roll_d, pitch_d, yaw_d, dotx, doty, dotz, dotax, dotay, dotaz, x_g, y_g, z_g, collision) = stl.signals(SIGNALS) drone_pos = sp.Matrix([x_d, y_d, z_d]) drone_ori = sp.Matrix([roll_d, pitch_d, yaw_d]) drone_lv = sp.Matrix([dotx, doty, dotz]) drone_av = sp.Matrix([dotax, dotay, dotaz]) goal_pos = sp.Matrix([x_g, y_g, z_g]) POSITION_SPEC = stl.Predicate((goal_pos - drone_pos).norm() <= 0.05) GOAL_VELOCITY_SPEC = POSITION_SPEC >> stl.Predicate( drone_lv.norm() <= 0.001) # Reached goal => stay still # Keep roll and pitch within 30deg ANGLE_CONSTRAINT = (