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)