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