def allocate(self):
''' Solve for thrust vectors after creating trans matrix '''
# create thrust matrix
A = self.thrust_matrix()
b = self.des_force
# solve Ax = b
# solutions are given respectively by thruster pose
bl, br, fl, fr = np.linalg.lstsq(A, b)[0]
# Temporarily sending the left thruster command to the motor driver
BL_msg, BR_msg, FL_msg, FR_msg = Command(), Command(), Command(), Command()
# convert thrusts (in newtons) to effort values (in firmware units)
# clip them to firmware limits
# assign to ROS messages
BL_msg.setpoint = np.clip(bl * self.effort_ratio, -self.effort_limit, self.effort_limit)
BR_msg.setpoint = np.clip(br * self.effort_ratio, -self.effort_limit, self.effort_limit)
FL_msg.setpoint = np.clip(fl * self.effort_ratio, -self.effort_limit, self.effort_limit)
FR_msg.setpoint = np.clip(fr * self.effort_ratio, -self.effort_limit, self.effort_limit)
# publish ROS messages
if self.kill is True:
self.BL_pub.publish(Command(setpoint=0))
self.BR_pub.publish(Command(setpoint=0))
self.FL_pub.publish(Command(setpoint=0))
self.FR_pub.publish(Command(setpoint=0))
else:
self.BL_pub.publish(BL_msg)
self.BR_pub.publish(BR_msg)
self.FL_pub.publish(FL_msg)
self.FR_pub.publish(FR_msg)
def callback(self, twist):
FLW_cmd = Command()
FRW_cmd = Command()
RLW_cmd = Command()
RRW_cmd = Command()
# linear velocity
vLinear = sqrt(twist.linear.x ** 2 + twist.linear.y ** 2)
if vLinear > self.maxLinearVelocity:
vLinear = self.maxLinearVelocity
# movement orientation
Heading = atan2(twist.linear.y, twist.linear.x)
# x axis linear velocity
xVel = vLinear * cos(Heading)
# y axis linear velocity
yVel = vLinear * sin(Heading)
# YAW axis rotational velocity
yawVel = twist.angular.z
if yawVel ** 2 > self.maxAngularVelocity ** 2:
yawVel = self.maxAngularVelocity * yawVel / abs(yawVel)
w = self.InverseKinematics(xVel, yVel, yawVel)
FLW_cmd.mode = 0
FLW_cmd.setpoint = w[0]
FRW_cmd.mode = 0
FRW_cmd.setpoint = w[1]
RLW_cmd.mode = 0
RLW_cmd.setpoint = w[2]
RRW_cmd.mode = 0
RRW_cmd.setpoint = w[3]
self.pubFLW.publish(FLW_cmd)
self.pubFRW.publish(FRW_cmd)
self.pubRLW.publish(RLW_cmd)
self.pubRRW.publish(RRW_cmd)
def callback(self, twist):
FLW_cmd = Command()
FRW_cmd = Command()
RLW_cmd = Command()
RRW_cmd = Command()
# linear velocity
vLinear = sqrt(twist.linear.x**2 + twist.linear.y**2)
if vLinear > self.maxLinearVelocity:
vLinear = self.maxLinearVelocity
# movement orientation
Heading = atan2(twist.linear.y, twist.linear.x)
# x axis linear velocity
xVel = vLinear * cos(Heading)
# y axis linear velocity
yVel = vLinear * sin(Heading)
# YAW axis rotational velocity
yawVel = twist.angular.z
if yawVel**2 > self.maxAngularVelocity**2:
yawVel = self.maxAngularVelocity * yawVel / abs(yawVel)
w = self.InverseKinematics(xVel, yVel, yawVel)
FLW_cmd.mode = 0
FLW_cmd.setpoint = w[0]
FRW_cmd.mode = 0
FRW_cmd.setpoint = w[1]
RLW_cmd.mode = 0
RLW_cmd.setpoint = w[2]
RRW_cmd.mode = 0
RRW_cmd.setpoint = w[3]
self.pubFLW.publish(FLW_cmd)
self.pubFRW.publish(FRW_cmd)
self.pubRLW.publish(RLW_cmd)
self.pubRRW.publish(RRW_cmd)
def publish_thrusts(self):
'''
Use the mapper to find the individual thrusts needed to acheive the current body wrench.
If killed, publish 0 to all thrusters just to be safe.
'''
BL, BR, FL, FR = Command(), Command(), Command(), Command()
if not self.kill: # Only get thrust of not killed, otherwise publish default 0 thrust
BL.setpoint, BR.setpoint, FL.setpoint, FR.setpoint = self.thruster_map.wrench_to_thrusts(self.wrench)
self.BL_pub.publish(BL)
self.BR_pub.publish(BR)
self.FL_pub.publish(FL)
self.FR_pub.publish(FR)
def publish_thrusts(self):
'''
Use the mapper to find the individual thrusts needed to acheive the current body wrench.
If killed, publish 0 to all thrusters just to be safe.
'''
BL, BR, FL, FR = Command(), Command(), Command(), Command()
if not self.kill: # Only get thrust of not killed, otherwise publish default 0 thrust
BL.setpoint, BR.setpoint, FL.setpoint, FR.setpoint = self.thruster_map.wrench_to_thrusts(
self.wrench)
self.BL_pub.publish(BL)
self.BR_pub.publish(BR)
self.FL_pub.publish(FL)
self.FR_pub.publish(FR)
def allocate(self):
''' Solve for thrust vectors after creating trans matrix '''
# create thrust matrix
A = self.thrust_matrix()
b = self.des_force
# solve Ax = b
# solutions are given respectively by thruster pose
bl, br, fl, fr = np.linalg.lstsq(A, b)[0]
# Temporarily sending the left thruster command to the motor driver
BL_msg, BR_msg, FL_msg, FR_msg = Command(), Command(), Command(
), Command()
# convert thrusts (in newtons) to effort values (in firmware units)
# clip them to firmware limits
# assign to ROS messages
BL_msg.setpoint = np.clip(bl * self.effort_ratio, -self.effort_limit,
self.effort_limit)
BR_msg.setpoint = np.clip(br * self.effort_ratio, -self.effort_limit,
self.effort_limit)
FL_msg.setpoint = np.clip(fl * self.effort_ratio, -self.effort_limit,
self.effort_limit)
FR_msg.setpoint = np.clip(fr * self.effort_ratio, -self.effort_limit,
self.effort_limit)
# publish ROS messages
if self.kill is True:
self.BL_pub.publish(Command(setpoint=0))
self.BR_pub.publish(Command(setpoint=0))
self.FL_pub.publish(Command(setpoint=0))
self.FR_pub.publish(Command(setpoint=0))
else:
self.BL_pub.publish(BL_msg)
self.BR_pub.publish(BR_msg)
self.FL_pub.publish(FL_msg)
self.FR_pub.publish(FR_msg)
def send_command(self, setpoint):
command = Command()
command.mode = self.get_mode()
command.setpoint = setpoint
def send_command(self, setpoint):
command = Command()
command.mode = self.get_mode()
command.setpoint = setpoint
def send_command(self, setpoint):
command = Command()
command.setpoint = setpoint
self.cmd_publisher.publish(command)