def stop_robots(self): com = RobotCommands() com.robot_index = self.robot_index com.type = ord('q') com.header.stamp = rospy.get_rostime() self.comm_pub.publish(com) return
def send_start_command(self): com = RobotCommands() com.robot_index = self.robot_index com.type = ord('m') com.header.stamp = rospy.get_rostime() self.comm_pub.publish(com) return
def send_initial_config(self): com = RobotCommands() com.robot_index = self.robot_index com.type = ord('a') com.header.stamp = rospy.get_rostime() com.header.frame_id = "/optimization_frame" com.div = 4 com.x = self.Qref[0][2] com.y = 0 com.th = 0 com.height_left = self.Qref[0][3] com.height_right = 1 self.comm_pub.publish(com) return
def send_initial_config(self): com = RobotCommands() com.robot_index = self.robot_index com.type = ord('a') com.header.stamp = rospy.get_rostime() com.header.frame_id = "/optimization_frame" com.div = 4 com.x = self.Q0[self.system.get_config('xr').index] com.y = 0 com.th = 0 com.height_left = self.Q0[self.system.get_config('r').index] com.height_right = 1 self.comm_pub.publish(com) return
def calc_send_controls(self): self.u1 = self.Xest2[2:4] self.u2 = self.Uref[self.k] + \ matmult(self.Kproj[self.k], self.Xref[self.k]-self.Xest2) # now convert to a velocity and send out: ucom = (self.u2-self.u1)/self.dt com = RobotCommands() com.robot_index = self.robot_index com.type = ord('i') com.v_robot = ucom[0] com.w_robot = 0 com.rdot = 0 com.rdot_left = ucom[1] com.rdot_right = 0 com.div = 3 self.comm_pub.publish(com) return
def convert_and_send_input(self, u1, u2): """ This function takes in two sets of kinematic inputs, processes them, and then sends them to the robot. u1 ~ where we think the kin configs are now u2 ~ where we want the kin configs to be in +dt seconds """ # clamp controls if we need to: if self.limit_bool: u2 = self.clamp_controls(u2) ucom = (u2-u1)/self.dt com = RobotCommands() com.robot_index = self.robot_index com.type = ord('i') com.v_robot = ucom[0] com.w_robot = 0 com.rdot = 0 com.rdot_left = ucom[1] com.rdot_right = 0 com.div = 3 self.comm_pub.publish(com) return
def convert_and_send_input(self, u1, u2): """ This function takes in two sets of kinematic inputs, processes them, and then sends them to the robot. u1 ~ where we think the kin configs are now u2 ~ where we want the kin configs to be in +dt seconds """ # clamp controls if we need to: if self.limit_bool: u2 = self.clamp_controls(u2) ucom = (u2 - u1) / self.dt com = RobotCommands() com.robot_index = self.robot_index com.type = ord('i') com.v_robot = ucom[0] com.w_robot = 0 com.rdot = 0 com.rdot_left = ucom[1] com.rdot_right = 0 com.div = 3 self.comm_pub.publish(com) return