def run(self): while (not self.attitude_command_received_flag) and ( not rospy.is_shutdown()): print "Waiting for attitude controller to start" rospy.sleep(0.5) print "Attitude control started." while (not self.mot_vel_ref_received_flag) and ( not rospy.is_shutdown()): print "Waiting for height controller to start" rospy.sleep(0.5) print "Height control started." while not rospy.is_shutdown(): self.ros_rate.sleep() # Compute motor velocities, + configuration mot1 = self.mot_vel_ref - self.vpc_pitch_command + self.yaw_command mot2 = self.mot_vel_ref + self.vpc_roll_command - self.yaw_command mot3 = self.mot_vel_ref + self.vpc_pitch_command + self.yaw_command mot4 = self.mot_vel_ref - self.vpc_roll_command - self.yaw_command mot_speed_msg = Actuators() mot_speed_msg.header.stamp = rospy.Time.now() mot_speed_msg.angular_velocities = [mot1, mot2, mot3, mot4] self.mot_vel_pub.publish(mot_speed_msg) self.q1_left_pub.publish(Float64(self.q1_left)) self.q2_left_pub.publish(Float64(self.q2_left)) self.q3_left_pub.publish(Float64(self.q3_left)) self.q1_right_pub.publish(Float64(self.q1_right)) self.q2_right_pub.publish(Float64(self.q2_right)) self.q3_right_pub.publish(Float64(self.q3_right))
def step1(self, thrust_cmds, dt): # thrust_cmds = np.array([0., 0., 0., 0.]) # print("DYN: step1: wait_for_message: odometry") # print('DYN: thrust: ', thrust_cmds) # time_start = current_time_ms() # Publish the action actuator_msg = Actuators() ## Direct angular velocity control # angular_velocities = (self.max_angular_val*np.array(thrust_cmds)).astype(dtype=np.int) ## Approximate torque control (converting torque to angular velocity for quad input) angular_velocities = np.clip(np.sqrt( (thrust_cmds * self.thrust) / 8.54858e-06), a_min=0., a_max=self.max_angular_val) # print('Rotor commands: ', angular_velocities) actuator_msg.angular_velocities = angular_velocities self.action_publisher.publish(actuator_msg) ## Delay monitoring self.step_delay = (current_time_ms() - self.time_last) / 1000. self.time_last = current_time_ms()
def send_motor_command(self): actuator = Actuators() actuator.header.stamp = rospy.Time.now() # print ("motor_speed: ", self.motor_speed) actuator.angular_velocities = self.motor_speed self.pub_actuator.publish(actuator) float64 = Float64() float64.data = self.motor_speed[0, 0]/1000.0 self.pub_motor_speed_des.publish(float64)
def run(self): ''' Runs ROS node - computes PID algorithms for z and vz control. ''' while not self.start_flag: print 'Waiting for velocity measurements.' rospy.sleep(0.5) print "Starting height control." while not rospy.is_shutdown(): self.ros_rate.sleep() ######################################################## ######################################################## # Implement cascade PID control here. # Reference for z is stored in self.z_sp. # Measured z-position is stored in self.z_mv. # If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp. # Measured vz-velocity is stored in self.vz_mv # Resultant reference values for motor velocity should be stored in variable self.mot_speed. hover_0 = 650.95 Ts = datetime.now() - self.t_old self.t_old = datetime.now() #print( Ts.total_seconds()) print(self.z_sp) print(self.z_mv) self.vz_sp = self.pid_z.compute(self.z_sp, self.z_mv) self.mot_speed = self.pid_vz.compute(self.vz_sp, self.vz_mv) + hover_0 #self.mot_speed = hover_0 print("mot_speed ", self.mot_speed) ######################################################## ######################################################## if self.attitude_ctl == 0: # Publish motor velocities mot_speed_msg = Actuators() mot_speed_msg.angular_velocities = [ self.mot_speed, self.mot_speed, self.mot_speed, self.mot_speed ] self.pub_mot.publish(mot_speed_msg) else: # publish reference motor velocity to attitude controller mot_speed_msg = Float32(self.mot_speed) self.mot_ref_pub.publish(mot_speed_msg) # Publish PID data - could be usefule for tuning self.pub_pid_z.publish(self.pid_z.create_msg()) self.pub_pid_vz.publish(self.pid_vz.create_msg())
def rotor_s_message(self,U,PsiStar): # creating actuators message actuators_message = Actuators() # this is just for testing # actuators_message.angular_velocities = np.array([100,100,100,100,100,100]) # copy motor speeds into message previously created # actuators_message.angular_velocities = n # just for debug pruposes # actuators_message.angular_velocities = np.array([200,200,200,200,200,200]) actuators_message.angular_velocities = self.rotor_s_standard_converter(U,PsiStar) return actuators_message
def run(self): """Run ros node, compute PID algorithms for z and vz control""" # init publisher while not self.first_measurement: print("Waiting for pose measurement") rospy.sleep(0.5) print("Start height control.") self.t_old = rospy.Time.now() while not rospy.is_shutdown(): self.ros_rate.sleep() t = rospy.Time.now() dt = (t - self.t_old).to_sec() if dt > 0.105 or dt < 0.095: print(dt) self.t_old = t # publish angles for tilt (in order to have rotors in normal position) self.tilt_0_pub.publish(0.0) self.tilt_1_pub.publish(0.0) self.tilt_2_pub.publish(0.0) self.tilt_3_pub.publish(0.0) self.motor_hover_speed = math.sqrt(293 / 0.000456874 / 4) # prefilter for reference (Why we use prefilter for reference) a = 0.1 self.z_ref_filt = (1 - a) * self.z_ref_filt + a * self.z_sp vz_ref = self.pid_z.compute(self.z_ref_filt, self.z_mv, dt) self.motor_speed = self.motor_hover_speed + \ self.pid_vz.compute(vz_ref, self.vz_mv, dt) motor_speed_msg = Actuators() motor_speed_msg.angular_velocities = [ self.motor_speed, self.motor_speed, self.motor_speed, self.motor_speed ] self.pub_mot.publish(motor_speed_msg) # Publish PID data - could be useful for tuning self.pub_pid_z.publish(self.pid_z.create_msg()) self.pub_pid_vz.publish(self.pid_vz.create_msg()) self.z_sp = 1
def pub_llc(self): pos_err = self.pos_des - self.pos vel_err = self.vel_des - self.vel # acc_des : feedforward, error : feedback g = 9.81 e3 = np.array([0, 0, 1]) t = self.kx * pos_err + self.kv * vel_err + self.m * g * e3 - self.m * self.acc_des # Desired Thrust F = t * self.R * e3 if F < 0: F = 0 elif F > self.max_thrust: F = self.max_thrust rot_err = 1 / 2 * self.R_des.T * self.R - self.R.T * self.R_des # v map from 3x3 skew-symmetric to 3x1 vector eR = np.array([0, 0, 0]) eR[0] = -rot_err[1][2] # x eR[1] = rot_err[0][2] # y eR[2] = -rot_err[0][1] # z ew = self.w - self.R.T * self.R_des * self.w_des # h map from 3x1 vector to 3x3 skew-symmetric w_h = np.array([[0, -self.w[2], self.w[1]], [self.w[2], 0, -self.w[0]], [-self.w[1], self.w[0], 0]]) M = -self.kR * eR - self.kw * eR + np.cross( self.w, self.Inertia * self.w) - self.Inertia * (w_h * self.R.T * self.R_des * self.w_des - self.R.T * self.R_des * self.w_dot_des) U = np.array([F, M[0], M[1], M[2]]) R2 = self.Kinv * U R = np.array( [np.sqrt(R2[0]), np.sqrt(R2[1]), np.sqrt(R2[2]), np.sqrt(R2[3])]) actuators = Actuators() actuators.angular_velocities = R actuators.header.stamp = rospy.Time.now() self.actuators_pub.publish(actuators)
def __init__(self): """ Quadrotor position resetter """ quadrotor = "hummingbird" # That is for us to command a new trajectory trajectory_topic = "command_trajectory" # Topic to get feedback from the quadrotor odometry_topic = "odometry_sensor1/odometry" # Topic to send commands to quadrotor actuators_topic = "command/motor_speed" # Resettting quadrotor reset_topic = "/gazebo/set_model_state" # Sync publisher (send syncing messages) sync_topic = "/world_control" self.start_time = time.time() self.odo_msg_count = 0 #Initializing the node rospy.init_node('quadrotor_env', anonymous=True) action_publisher = rospy.Publisher(quadrotor + "/" + actuators_topic, Actuators, queue_size=1) # Waiting for reset service to appear rospy.wait_for_service(reset_topic) reset_service = rospy.ServiceProxy(reset_topic, SetModelState) # Resetting self.reset(reset_service) for i in range(10): actuator_msg = Actuators() actuator_msg.angular_velocities = 0. * np.array([1., 1., 1., 1.]) action_publisher.publish(actuator_msg) rospy.sleep(.1) # print('Motors are reset: ', actuator_msg) print("Motors are reset")