def iterate(self):
if self.enable:
# Main loop
rospy.loginfo("desired_velocity: %s", str(self.desired_velocity))
rospy.loginfo("current velocity: %s", str(self.v))
# Apply PID to obtain tau
# Compute real period
now = rospy.Time.now()
real_period_ = (now - self.past_time) #nano seconds to seconds
self.past_time = now
rospy.loginfo("real_period: %s", str(real_period_.to_sec()))
# Compute TAU using a PID
pid_tau = zeros(6)
[pid_tau,
self.ek_velocity_1,
self.eik_velocity_1] = self.pid.computePid( self.desired_velocity,
self.v,
self.ek_velocity_1,
self.eik_velocity_1,
real_period_.to_sec())
# Compute TAU using an Open Loop Controller
open_loop_tau = zeros(6)
for i in range(6):
open_loop_tau[i] = cola2_lib.polyval(self.adjust_poly[i], self.desired_velocity[i])
# Combine open loop TAU and PID TAU
tau = cola2_lib.saturateValue(pid_tau + open_loop_tau, 1.0) * self.force_max
rospy.loginfo("Tau: %s", str(tau))
data = BodyForceReq()
data.header.stamp = now
data.header.frame_id = "vehicle_frame"
data.goal.requester = "velocity_controller"
data.goal.id = 0
data.goal.priority = GoalDescriptor.PRIORITY_NORMAL
data.wrench.force.x = tau[0]
data.wrench.force.y = tau[1]
data.wrench.force.z = tau[2]
data.wrench.torque.x = tau[3]
data.wrench.torque.y = tau[4]
data.wrench.torque.z = tau[5]
data.disable_axis.x = self.resp.disable_axis.x
data.disable_axis.y = self.resp.disable_axis.y
data.disable_axis.z = self.resp.disable_axis.z
data.disable_axis.roll = self.resp.disable_axis.roll
data.disable_axis.pitch = self.resp.disable_axis.pitch
data.disable_axis.yaw = self.resp.disable_axis.yaw
self.pub_tau.publish(data)
def updateBodyForceReq(self, w) :
# Input data
body_force_req = zeros(6)
if not w.disable_axis.x :
body_force_req[0] = w.wrench.force.x
if not w.disable_axis.y :
body_force_req[1] = w.wrench.force.y
if not w.disable_axis.z :
body_force_req[2] = w.wrench.force.z
if not w.disable_axis.roll :
body_force_req[3] = w.wrench.torque.x
if not w.disable_axis.pitch :
body_force_req[4] = w.wrench.torque.y
if not w.disable_axis.yaw :
body_force_req[5] = w.wrench.torque.z
rospy.loginfo("Body Force Request: %s", str(body_force_req))
#Merge Surge and Yaw DoF giving priority to YAW
body_force_req = self.mergeSurgeYaw(body_force_req)
rospy.loginfo("Surge, Yaw merged: %s", str(body_force_req))
# Computes the force to be done for each actuator
f = self.acm_inv * matrix(body_force_req).T
f = squeeze(asarray(f)) #matrix to array
rospy.loginfo("force per thruster: %s", str(f))
#Linearize compute thruster setpoints
setpoint = self.computeThrusterSetpoint(f, self.apl)
rospy.loginfo("thrusters linearized: %s", str(setpoint))
#Saturate setpoint to [-1, 1]
setpoint = cola2_lib.saturateValue(setpoint, 1.0)
rospy.loginfo("thrusters saturated: %s", str(setpoint))
# Log and send computed data
thrusters = ThrustersData()
thrusters.header.stamp = rospy.Time.now()
thrusters.header.frame_id = "vehicle_frame"
thrusters.setpoints = setpoint
#publish
self.pub.publish(thrusters)