def cmd(self, u1, u2):
"""
BicycleCommandMsg: The commands to a bicycle model robot (v, phi)
float64 linear_velocity
float64 steering_rate
"""
self.pub.publish(BicycleCommandMsg(u1, u2))
print(BicycleCommandMsg(u1, u2))
print('################')
def execute(self, plan):
"""
Executes a plan made by the planner
Parameters
----------
plan : :obj:`list` of (time, BicycleCommandMsg, BicycleStateMsg)
"""
print('Exectutor starting to exectute plan')
if len(plan) == 0:
return
self.start_time = rospy.get_time()
for (t, cmd, state) in plan:
self.times.append(rospy.get_time() - self.start_time)
self.actual_states.append([self.state.x,
self.state.y,
self.state.theta,
self.state.phi])
self.plan_states.append([state.x,
state.y,
state.theta,
state.phi])
self.cmd(cmd)
self.rate.sleep()
if rospy.is_shutdown():
break
self.cmd(BicycleCommandMsg())
print('Exectutor done exectuting')
self.plot()
def __init__(self):
self._name = rospy.get_name()
self.last_time = rospy.Time.now()
self.rate_hz = 200
self.rate = rospy.Rate(self.rate_hz)
self.get_params()
self.state = BicycleStateMsg()
self.command = BicycleCommandMsg()
if self.turtlesim:
self.turtlesim_subscriber = rospy.Subscriber(self.turtlesim_pose_topic, Pose, self.update_turtlesim_pose)
self.unicycle_command_topic = self.turtlesim_command_topic
# resetting stuff
print 'Waiting for turtlesim/reset service ...',
rospy.wait_for_service('reset')
print 'found!'
self.reset_turtlesim_service = rospy.ServiceProxy('reset', EmptySrv)
else:
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.unicycle_command_topic = self.turtlebot_command_topic
# resetting stuff
self.reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry', EmptyMsg, queue_size=10)
self.command_publisher = rospy.Publisher(self.unicycle_command_topic, Twist, queue_size = 1)
self.state_publisher = rospy.Publisher(self.state_topic, BicycleStateMsg, queue_size = 1)
self.subscriber = rospy.Subscriber(self.bicycle_command_topic, BicycleCommandMsg, self.command_listener)
rospy.Service('converter/reset', EmptySrv, self.reset)
rospy.on_shutdown(self.shutdown)
def executeGain(self, plan, gains):
for i in range(len(plan)):
newcmd = plan[i][1] - np.matmul(gains[i],
(self.state - plan[i][2]))
self.cmd(newcmd)
self.rate.sleep()
self.state_record.append((state, self.state))
if rospy.is_shutdown():
break
self.cmd(BicycleCommandMsg())
def execute(self, plan):
"""
Executes a plan made by the planner
Parameters
----------
plan : :obj:`list` of (time, BicycleCommandMsg, BicycleStateMsg)
"""
for t, cmd, state in plan:
self.cmd(cmd)
self.rate.sleep()
if rospy.is_shutdown():
break
self.cmd(BicycleCommandMsg())
def execute(self, plan):
"""
Executes a plan made by the planner
Parameters
----------
plan : :obj:`list` of (time, BicycleCommandMsg, BicycleStateMsg)
self.state -> x(t)
state -> x0(t)
cmd -> u0(t)
"""
if len(plan) == 0:
return
for (t, cmd, state) in plan:
self.cmd(cmd)
self.rate.sleep()
self.state_record.append((state, self.state))
if rospy.is_shutdown():
break
self.cmd(BicycleCommandMsg())
def cmd(self, u1, u2, dt):
"""
BicycleCommandMsg: The commands to a bicycle model robot (v, phi)
float64 linear_velocity
float64 steering_rate
"""
# self.path.append((self.t, u1, u2))
curr_state = self.state
# for i, (self.t, u1, u2) in enumerate(path):
cmd_u = BicycleCommandMsg(u1, u2)
self.path.append([self.t, cmd_u, curr_state])
print([self.t, cmd_u, curr_state])
self.state = BicycleStateMsg(
curr_state.x + np.cos(curr_state.theta) * cmd_u.linear_velocity*dt,
curr_state.y + np.sin(curr_state.theta) * cmd_u.linear_velocity*dt,
curr_state.theta + np.tan(curr_state.phi) / float(self.l) * cmd_u.linear_velocity*dt,
curr_state.phi + cmd_u.steering_rate*dt
)
# print(self.state)
self.t += dt
def shutdown(self):
rospy.loginfo("Shutting Down")
self.cmd(BicycleCommandMsg())
def v2cmd(v1, v2, state):
u1 = v1 / np.cos(state.theta)
u2 = v2
return BicycleCommandMsg(u1, u2)
def execute_closed_loop(self, plan, l, dt=0.01):
"""
Executes a plan made by the planner with the closed loop controller
Parameters
----------
plan : :obj:`list` of (time, BicycleCommandMsg, BicycleStateMsg)
"""
def compute_A(t_index):
t = plan[t_index][0]
u1 = plan[t_index][1].linear_velocity
u2 = plan[t_index][1].steering_rate
theta = plan[t_index][2].theta
phi = plan[t_index][2].phi
A00 = 0
A01 = 0
A02 = np.sin(theta) * u1
A03 = 0
A10 = 0
A11 = 0
A12 = -np.cos(theta) * u1
A13 = 0
A20 = 0
A21 = 0
A22 = 0
A23 = (1/l) * (1/np.cos(phi))**2 * u1
A30 = 0
A31 = 0
A32 = 0
A33 = 0
A = np.array([[A00, A01, A02, A03],
[A10, A11, A12, A13],
[A20, A21, A22, A23],
[A30, A31, A32, A33],
])
return A
def compute_B(t_index):
t = plan[t_index][0]
u1 = plan[t_index][1].linear_velocity
u2 = plan[t_index][1].steering_rate
theta = plan[t_index][2].theta
phi = plan[t_index][2].phi
B00 = np.cos(theta)
B01 = 0
B10 = np.sin(theta)
B11 = 0
B20 = (1/l) * np.tan(theta)
B21 = 0
B30 = 0
B31 = 1
B = np.array([[B00, B01],
[B10, B11],
[B20, B21],
[B30, B31],
])
return B
def integrand_00(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[0,0]
def integrand_01(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[0,1]
def integrand_02(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[0,2]
def integrand_03(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[0,3]
def integrand_10(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[1,0]
def integrand_11(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[1,1]
def integrand_12(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[1,2]
def integrand_13(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot( BT, exp_A_tauT))[1,3]
def integrand_20(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[2,0]
def integrand_21(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[2,1]
def integrand_22(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[2,2]
def integrand_23(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[2,3]
def integrand_30(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[3,0]
def integrand_31(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[3,1]
def integrand_32(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[3,2]
def integrand_33(tau, t, t_index):
exp_A_tau = expm(-A*tau)
exp_A_tauT = np.transpose(exp_A_tau)
return np.exp(6*0.1*(tau-t)) * np.dot( np.dot(exp_A_tau, B), np.dot(BT, exp_A_tauT))[3,3]
print("COMPUTING THE INTERESTING MATRICES")
Pcs = []
Bs = []
nb_steps_dt = 5
for t_index in range(50):#range(np.asarray(plan).shape[0]-1):
A = compute_A(t_index)
B = compute_B(t_index)
AT = np.transpose(A)
BT = np.transpose(B)
t = plan[t_index][0]
t_plus_dt = plan[t_index+1][0]
Hc = np.zeros((4, 4))
Hc[0,0] = quad(integrand_00, t, t_plus_dt, args=(t, t_index))[0]
Hc[0,1] = quad(integrand_01, t, t_plus_dt, args=(t, t_index))[0]
Hc[0,2] = quad(integrand_02, t, t_plus_dt, args=(t, t_index))[0]
Hc[0,3] = quad(integrand_03, t, t_plus_dt, args=(t, t_index))[0]
Hc[1,0] = quad(integrand_10, t, t_plus_dt, args=(t, t_index))[0]
Hc[1,1] = quad(integrand_11, t, t_plus_dt, args=(t, t_index))[0]
Hc[1,2] = quad(integrand_12, t, t_plus_dt, args=(t, t_index))[0]
Hc[1,3] = quad(integrand_13, t, t_plus_dt, args=(t, t_index))[0]
Hc[2,0] = quad(integrand_20, t, t_plus_dt, args=(t, t_index))[0]
Hc[2,1] = quad(integrand_21, t, t_plus_dt, args=(t, t_index))[0]
Hc[2,2] = quad(integrand_22, t, t_plus_dt, args=(t, t_index))[0]
Hc[2,3] = quad(integrand_23, t, t_plus_dt, args=(t, t_index))[0]
Hc[3,0] = quad(integrand_30, t, t_plus_dt, args=(t, t_index))[0]
Hc[3,1] = quad(integrand_31, t, t_plus_dt, args=(t, t_index))[0]
Hc[3,2] = quad(integrand_32, t, t_plus_dt, args=(t, t_index))[0]
Hc[3,3] = quad(integrand_33, t, t_plus_dt, args=(t, t_index))[0]
# t = plan[t_index][0]
# A_t = compute_A(t_index)
# B_t = compute_B(t_index)
# A_t_T = np.transpose(A_t)
# B_t_T = np.transpose(B_t)
# t_dt_ = plan[t_index + nb_steps_dt][0]
# A_t_dt_ = compute_A(t_index + nb_steps_dt)
# B_t_dt_ = compute_B(t_index + nb_steps_dt)
# A_t_dt__T = np.transpose(A_t_dt)
# B_t_dt__T = np.transpose(B_t_dt)
# left_value = np.dot(np.dot(np.expm(-A_t*t), B_t), np.dot(B_t_T, np.transpose(np.expm(-A_t*t))))
# right_value = np.exp(6*0.1*t_dt-t)
Pc = np.linalg.inv(Hc)
Pcs.append(Pc)
B = compute_B(t_index)
Bs.append(B)
print('Exectutor starting to exectute plan')
print(len(Pcs))
print(len(Bs))
GAMMA = 0.1
if len(plan) == 0:
return
self.start_time = rospy.get_time()
for i, (t, cmd, state) in enumerate(plan):
self.times.append(rospy.get_time() - self.start_time)
self.actual_states.append([self.state.x,
self.state.y,
self.state.theta,
self.state.phi])
self.plan_states.append([state.x,
state.y,
state.theta,
state.phi])
curr_state = np.array([self.state.x, self.state.y, self.state.theta, self.state.phi]).reshape((4,1))
plan_state = np.array([state.x, state.y, state.theta, state.phi]).reshape((4,1))
feedback_term = GAMMA * np.dot(np.dot(np.transpose(Bs[i]), Pcs[i]), curr_state - plan_state)
u1_feedback = cmd.linear_velocity - feedback_term[0]
u2_feedback = cmd.steering_rate - feedback_term[1]
self.cmd(BicycleCommandMsg(u1_feedback, u2_feedback))
self.rate.sleep()
if rospy.is_shutdown():
break
self.cmd(BicycleCommandMsg())
print('Exectutor done exectuting')
self.plot()
def v2cmd(v1, v2, state):
u1 = v1
u2 = v2
return BicycleCommandMsg(u1, u2)
def cmd(self, u1, u2):
self.state_record.append(self.state)
self.pub.publish(BicycleCommandMsg(u1, u2))
def cmd(self, u1, u2):
self.pub.publish(BicycleCommandMsg(u1, u2))
#print('u1 = {}, u2 = {}'.format(u1, u2))
self.u1s.append(u1)
self.u2s.append(u2)
self.time_u.append(rospy.get_time() - self.time_initial)
def cmd(self, u1, u2):
self.pub.publish(BicycleCommandMsg(u1, u2))
