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