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))
# Minimize magnitude of angular velocity to <= 5deg/s
ANGULAR_VEL_CONSTRAINT = stl.Predicate(drone_av.norm() <= np.deg2rad(2.5))
# SPEC = stl.G( # Always do the following:
# stl.F(POSITION_SPEC) # Head towards goal position
# & ANGLE_CONSTRAINT # Keep the angles constrained
# & ANGULAR_VEL_CONSTRAINT # COnstrain the angular velocity
# & GOAL_VELOCITY_SPEC # Minimize drift once you reach goal
# & (1000 * collision < 1000)
# )
PHI = stl.F(POSITION_SPEC,
(0, int(5 // dt))) # Reach goal position within 5 sec
PHI_SAFE = stl.G(ANGULAR_VEL_CONSTRAINT)
SPEC = PHI & PHI_SAFE
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))
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
stl.G(leg_2_ground_contact_flag <= 0, (1, 8)),
)),
stl.Implies((leg_2_ground_contact_flag > 0),
stl.And(
# Set foot in 10-16 timesteps
stl.F(leg_1_ground_contact_flag > 0, (10, 16)),
# Make sure to actually lift leg
stl.G(leg_1_ground_contact_flag <= 0, (1, 8)),
)),
)
)
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()
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(
stl.F(leg_2_ground_contact_flag > 0,
(10, 16)), # Set foot in 10-16 timesteps
stl.G(leg_2_ground_contact_flag <= 0,
(1, 8)), # Make sure to actually lift leg
)),
stl.Implies(
(leg_2_ground_contact_flag > 0),
stl.And(
stl.F(leg_1_ground_contact_flag > 0,
(10, 16)), # Set foot in 10-16 timesteps
stl.G(leg_1_ground_contact_flag <= 0,
(1, 8)), # Make sure to actually lift leg
)),
))
AGENT_CONFIG = dict(env_id='BipedalWalker-v2',
name='default',
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 = (
stl.Predicate(abs(roll_d) <= PI/6)
& stl.Predicate(abs(pitch_d) <= PI/6)
)
# Minimize magnitude of angular velocity to <= 5deg/s
ANGULAR_VEL_CONSTRAINT = stl.Predicate(drone_av.norm() <= np.deg2rad(5))
# SPEC = stl.G( # Always do the following:
# stl.F(POSITION_SPEC) # Head towards goal position
# & ANGLE_CONSTRAINT # Keep the angles constrained
# & ANGULAR_VEL_CONSTRAINT # COnstrain the angular velocity
# & GOAL_VELOCITY_SPEC # Minimize drift once you reach goal
# & (1000 * collision < 1000)
# )
PHI = stl.F(POSITION_SPEC, (0, 5)) # Reach goal position within 5 sec
PHI_SAFE = stl.G(PHI & ANGULAR_VEL_CONSTRAINT)
SPEC = PHI_SAFE
SPEC1 = PHI