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