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() # odom_msg = rospy.wait_for_message(self.odometry_topic, Odometry) # self.odometry_callback(msg=odom_msg) # time_end = current_time_ms() # print('DYN: wait odometry delay: ', (time_end - time_start)) # 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) time_end = current_time_ms() self.step_delay = 2 * ( time_end - time_start ) # *2 because afterwards we will wait for the state again
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 = saturate(self.mot_vel_ref + self.yaw_command - self.pitch_command, 0.0, 1450.0) mot2 = saturate(self.mot_vel_ref - self.yaw_command + self.roll_command, 0.0, 1450.0) mot3 = saturate(self.mot_vel_ref + self.yaw_command + self.pitch_command, 0.0, 1450.0) mot4 = saturate(self.mot_vel_ref - self.yaw_command - self.roll_command, 0.0, 1450.0) # Publish payload payload_msg = Point() payload_msg.x = self.vpc_pitch_command payload_msg.y = self.vpc_roll_command self.payload_position_pub.publish(payload_msg) # Publish everything 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)
def _motor_speed_publish(self): """ type: mav_msgs/Actuators name: /blimp/command/motor_speed format: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' angles: - 0 angular_velocities: - 0 normalized: - 0 """ motor1_limit = +self._limit(self.motor1_speed, self.MOTOR_LIMIT) motor2_limit = -self._limit(self.motor2_speed, self.MOTOR_LIMIT) motor3_limit = self._limit(self.motor3_speed, self.MOTOR3_LIMIT) all_motor_speed = Actuators() all_motor_speed.angular_velocities = [motor1_limit, motor2_limit, motor3_limit] #publish and record the data self.pub_motor_speed.publish(all_motor_speed)
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 ctrl_update(self, state): """ Multiply state by Discrete LQR Gain Matrix and then formulate motor speeds""" currErr, errorAccum, integralErrFlag = self.calc_error(state) for i in range(5): state[i, 0] = currErr[i] state[8, 0] = currErr[6] state[11, 0] = currErr[7] desiredInput = (-1) * np.dot(self.dlqrGainInt, state) + self.equilibriumInput + np.dot( self.integralGain, errorAccum) # if integralErrFlag and (not self.waypointEndAchieved): # desiredInput = (-1)*np.dot(self.dlqrGainInt, state) + self.equilibriumInput + np.dot(self.integralGain, errorAccum) # print('dlqr with integrator') # else: # desiredInput = (-1)*np.dot(self.dlqrGain, state) + self.equilibriumInput # print('dlqr') # find the rotor speed for each rotor motorSpeeds = Actuators() motorSpeeds.angular_velocities = np.zeros((4, 1)) motorSpeedTransitionVec = np.dot( np.linalg.inv(self.speedAllocationMatrix), desiredInput) motorSpeeds.angular_velocities = np.sqrt( np.abs(motorSpeedTransitionVec)) self.dlqrPublisher.publish(motorSpeeds)
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 compute_stuff(self): pub = rospy.Publisher('ardrone/command/motor_speed', Actuators, queue_size=10) rate = rospy.Rate(200) global tho, pitch_imu, pitch, roll, yaw, front_left, front_right, back_left, back_right, pitch_gyro, roll_gyro, yaw_gyro global rr_pid, pr_pid, yr_pid, yaw_target, roll_imu, yaw_imu global rs_pid, ps_pid, ys_pid back_left, front_left, front_right, back_right = 0.0, 0.0, 0.0, 0.0 if (tho > 200): rol_stab_out = constrain(rs_pid.get_pid(roll - roll_imu, 1), -250, 250) pitch_stab_out = constrain(ps_pid.get_pid(pitch - pitch_imu, 1), -250, 250) yaw_stab_out = constrain( ys_pid.get_pid(wrap_180(yaw_target - yaw_imu), 1), -360, 360) if (abs(yaw) > 5): yaw_stab_out = yaw yaw_target = yaw_imu rol_out = constrain(rr_pid.get_pid(rol_stab_out - roll_gyro, 1), -500, 500) pit_out = constrain(pr_pid.get_pid(pitch_stab_out - pitch_gyro, 1), -500, 500) yaw_out = constrain(yr_pid.get_pid(yaw_stab_out - yaw_gyro, 1), -500, 500) front_right = tho - (pit_out) + (rol_out) + yaw_out front_left = tho - (pit_out) - (rol_out) - yaw_out back_right = tho + (pit_out) + (rol_out) - yaw_out back_left = tho + (pit_out) - (rol_out) + yaw_out """ hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output); hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output); hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output); hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output); """ else: front_right = tho front_left = tho back_right = tho back_left = tho yaw_target = yaw_imu rr_pid.reset_I() pr_pid.reset_I() yr_pid.reset_I() rs_pid.reset_I() ps_pid.reset_I() ys_pid.reset_I() act = Actuators() a = [back_left, front_right, back_right, front_left] act.angular_velocities = a rospy.loginfo(act) pub.publish(act) rate.sleep()
def callback(self, data): f = data.Total_thrust; M1 = data.Moment_x M2 = data.Moment_y; M3 = data.Moment_z T = tp(ar([[M1,M2,M3,f]])) Msg = Actuators() #Msg.header.stamp = rospy.Time.now() Msg.header.stamp = data.header.stamp if self.uav == 'hummingbird': #self.tau_fh = 8.54858e-6 c1 = tp(ar([[0, -self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c2 = tp(ar([[self.d*self.tau_f, 0, -self.tau_f*self.tau_m, self.tau_f]])) c3 = tp(ar([[0, self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c4 = tp(ar([[-self.d*self.tau_f, 0, -self.tau_f*self.tau_m, self.tau_f]])) C = np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C),T) w_initial = np.array([[self.w0_h**2], [self.w0_h**2], [self.w0_h**2], [self.w0_h**2]]) w = np.sqrt(np.abs(w_square+w_initial)) w[w>800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] #print w_square, w self.pub.publish(Msg) elif self.uav== 'pelican': #self.tau_fp = 9.986e-6 # for the time being using eth value, need to find a good solution c1 = tp(ar([[0, -self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c2 = tp(ar([[self.d*self.tau_f, 0, -self.tau_f*self.tau_m, self.tau_f]])) c3 = tp(ar([[0, self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c4 = tp(ar([[-self.d*self.tau_f, 0, -self.tau_f*self.tau_m, self.tau_f]])) C = np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C),T) #w_initial = np.array([[self.w0_p**2], [self.w0_p**2], [self.w0_p**2], [self.w0_p**2]]) #w = np.sqrt(np.abs(w_square+w_initial)) w = np.sqrt(np.abs(w_square)) w[w>800.0] = 800.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] f2 = open('motorspeeds.txt', 'a') f2.write("%s, %s, %s, %s\n" % (w[0][0], w[1][0], w[2][0], w[3][0])) self.pub.publish(Msg) else: c1 = tp(ar([[sin(pi/6)*self.d*self.tau_f, -cos(pi/6)*self.d*self.tau_f, -self.tau_f*self.tau_m, self.tau_f]])) c2 = tp(ar([[sin(pi/2)*self.d*self.tau_f, -cos(pi/2)*self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c3 = tp(ar([[sin(5*pi/6)*self.d*self.tau_f, -cos(5*pi/6)*self.d*self.tau_f, -self.tau_f*self.tau_m, self.tau_f]])) c4 = tp(ar([[sin(7*pi/6)*self.d*self.tau_f, -cos(7*pi/6)*self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c5 = tp(ar([[sin(3*pi/2)*self.d*self.tau_f, -cos(3*pi/2)*self.d*self.tau_f, -self.tau_f*self.tau_m, self.tau_f]])) c6 = tp(ar([[sin(11*pi/6)*self.d*self.tau_f, -cos(11*pi/6)*self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) C = np.column_stack((c1,c2,c3,c4,c5,c6)) # solving linear eq T = Cw^2 to get w^2 inverted_matrix = np.dot(C.T, np.linalg.inv(np.dot(C, C.T))) w_square = np.dot(inverted_matrix,T) #w_initial = np.array([[self.w0_f**2], [self.w0_f**2], [self.w0_f**2], [self.w0_f**2], [self.w0_f**2], [self.w0_f**2]]) #w = np.sqrt(np.abs(w_square+w_initial)) w = np.sqrt(np.abs(w_square)) #Msg = Actuators() #Msg.header.stamp = rospy.Time.now() w[w>900.0] = 900.0 Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0]] f2 = open('motorspeeds.txt', 'a') f2.write("%s, %s, %s, %s, %s, %s\n" % (w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0])) self.pub.publish(Msg)
def signalCallback(self, msg): actuators_msg = Actuators() actuators_msg.header = msg.header for i in range(self.num_rotors): s = msg.values[i] w = self.omega_polys[i][0] * s + self.omega_polys[i][ 1] # calculate motor speed actuators_msg.angular_velocities.append(w) self.speedPub.publish(actuators_msg)
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 timer_callback(self, event): self.altitude_setpoint_pub.publish( self.current_uav_setpoint.position.z) self.altitude_state_pub.publish(self.current_uav_pose.position.z) ac = Actuators() alt_ref = 400 output = alt_ref #+self.alt_effort ac.angular_velocities = [output, output, output, output] self.motor_speed_pub.publish(ac)
def run(self): while not rospy.is_shutdown(): newMsg = Actuators() newMsg.angular_velocities = Float64() newMsg.angular_velocities.data = [] self.pub_motor.publish(newMsg) rospy.spin()
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 set_rotor_vel(self, pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust): # change arg names? # get conversions rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie( pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust) rotor_velocities = rotorvel_converter.CalculateRotorVelocities() # publish rotor velocities to Actuator msg = Actuators() msg.angular_velocities = rotor_velocities msg.header.stamp = self._currpos_msg.header.stamp self.pub_rotor_vel.publish(msg)
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.yaw_command - self.vpc_pitch_command mot2 = self.mot_vel_ref - self.yaw_command + self.vpc_roll_command mot3 = self.mot_vel_ref + self.yaw_command + self.vpc_pitch_command mot4 = self.mot_vel_ref - self.yaw_command - self.vpc_roll_command mot1 = self.quantization( mot1, self.quantization_step) + self.motor_noise.data[0] mot2 = self.quantization( mot2, self.quantization_step) + self.motor_noise.data[1] mot3 = self.quantization( mot3, self.quantization_step) + self.motor_noise.data[2] mot4 = self.quantization( mot4, self.quantization_step) + self.motor_noise.data[3] moving_mass_front = self.pitch_command # mm1 moving_mass_back = -self.pitch_command # mm3 moving_mass_left = -self.roll_command # mm2 moving_mass_right = self.roll_command # mm4 # Publish everything 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.mass_front_pub.publish(Float64(moving_mass_front)) self.mass_back_pub.publish(Float64(moving_mass_back)) self.mass_left_pub.publish(Float64(moving_mass_left)) self.mass_right_pub.publish(Float64(moving_mass_right)) all_mass_msg = Float64MultiArray() all_mass_msg.data = [ moving_mass_front, moving_mass_left, moving_mass_back, moving_mass_right ] self.mass_all_pub.publish(all_mass_msg)
def _set_action(self, action): """ It sets the joints of pelican based on the action integer given based on the action number given. :param action: The action integer that sets what movement to do next. """ rospy.logdebug("Start Set Action ==>" + str(action)) motor_cmd = Actuators(angular_velocities=[0, 0, 0, 0]) for i in range(self.a_dim): motor_cmd.angular_velocities[i] = action[i] self._cmd_drive_pub.publish(motor_cmd) time.sleep(0.02) rospy.logdebug("END Set Action ==>" + str(action))
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 rotor_s_message(self,U,PsiStar): # creating actuators message actuators_message = Actuators() # this is just for testing # actuators_message.angular_velocities = numpy.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 = numpy.array([200,200,200,200,200,200]) actuators_message.angular_velocities = self.rotor_s_standard_converter(U,PsiStar) return actuators_message
def compute_stuff(self): pub = rospy.Publisher('ardrone/command/motor_speed', Actuators, queue_size=10) rate = rospy.Rate(200) global tho,pitch_imu,pitch,roll,front_left,front_right,back_left,back_right,pitch_gyro,roll_gyro,yaw_gyro global rr_pid,pr_pid,yr_pid back_left,front_left,front_right,back_right=0.0,0.0,0.0,0.0 if(tho > 200): """rol_out = max(min(rr_pid.get_pid(roll - roll_gyro, 1), 500), -500) pit_out = max(min(pr_pid.get_pid(pitch - pitch_gyro, 1), 500), -500) yaw_out = max(min(yr_pid.get_pid(yaw - yaw_gyro, 1), 500), -500)""" rol_out = rr_pid.get_pid(roll - roll_gyro, 1) pit_out = pr_pid.get_pid(pitch - pitch_gyro, 1) yaw_out = yr_pid.get_pid(yaw - yaw_gyro, 1) front_right = tho - (pit_out)+(rol_out) + yaw_out front_left = tho - (pit_out)-(rol_out) - yaw_out back_right = tho + (pit_out)+(rol_out) - yaw_out back_left = tho + (pit_out)-(rol_out) + yaw_out """ curr_time = datetime.now() formatted_time = curr_time.strftime('%M:%S.%f') f = open("/home/birhan/birhan/src/beginner_tutorials/src/scripts/selami2.txt","a") f.write(formatted_time +"\ntho: " + str(tho) + " gyro_roll: " + str(roll_gyro) + " pitch_gyro: " + str(pitch_gyro)+ " yaw_gyro: " + str(yaw_gyro)+"\n" +"roll_out "+str(rol_out)+" pit out"+str(pit_out)+ " yaw out"+str(yaw_out)+"\n FR:"+str(front_right)+" FL:"+str(front_left)+" BR:"+str(back_right)+ " BL:"+str(back_left)+"\n") f.write("---------------------------\n") f.close()""" else: front_right = tho front_left = tho back_right = tho back_left= tho rr_pid.reset_I() pr_pid.reset_I() yr_pid.reset_I() act = Actuators() a=[back_left,front_right,back_right,front_left] #[sol arka,sağ ön, sağ arka, sol ön ] act.angular_velocities=a rospy.loginfo(act) pub.publish(act) rate.sleep()
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 step_old(self, thrust_cmds, dt): """ This is how you can publish in the old version """ # 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)
def run(self): while not rospy.is_shutdown(): self.ros_rate.sleep() newMsg = Actuators() #newMsg.angular_velocities = Float64() speed1 = 800 speed2 = 800
def __init__(self, model_name='hummingbird', target_pos=np.zeros(3, np.float32), rate=100, state_space_names=[ 'rotation_matrix', 'position', 'linear_vel', 'angular_vel' ], **kwargs_init): super(WrapperROSQuad, self).__init__(rate) self.model_name = model_name self.state_space = StateSpaceRobots(state_space_names) shape_state_space = self.state_space.get_state_space_shape() robot_description = RobotDescription(self.model_name) max_velocity_rotor = robot_description._get_max_vel() rospy.loginfo('Found Rotor max speed {}'.format(max_velocity_rotor)) rospy.loginfo( 'State space shape: {} with following meassurements: \n{}'.format( shape_state_space, '-\t' + '\n-\t'.join(self.state_space.names))) self.last_observation = None self.action_space = spaces.Box(low=0.0, high=max_velocity_rotor, shape=(4, ), dtype=np.float32) self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=shape_state_space, dtype=np.float32) self.target_pos = target_pos #print(self.target_pos) self.actuator_msg = Actuators() self._sensors_topic_name = '/hummingbird/ground_truth/odometry' self._actuator_reader = '/hummingbird/motor_speed' self._imu_topic_name = '/hummingbird/imu' #TODO: Fill with correspondent ROS topics self._actuators_pub = rospy.Publisher( 'hummingbird/command/motor_speed', Actuators, queue_size=10) self._set_states_srv = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) #self._states_pub = None self._sensors_subs = rospy.Subscriber( '/hummingbird/ground_truth/odometry', Odometry, self._callback_sensor_meassurements) self._imu_subs = rospy.Subscriber(self._imu_topic_name, Imu, self._callback_imu_meassurements) self.max_radius = kwargs_init['max_radius'] self.max_ang_speed = kwargs_init['max_ang_speed'] self.max_radius_init = kwargs_init['max_radius_init'] self.angle_std = kwargs_init['angle_rad_std'] self.angular_speed_mean = kwargs_init['angular_speed_mean'] self.angular_speed_std = kwargs_init['angular_speed_std'] self.counter = 0 self._imu = None
def ctrl_update(self, state): """ Multiply state by Discrete LQR Gain Matrix and then formulate motor speeds""" currErr = self.calc_error(state) for i in range(5): state[i, 0] = currErr[i] state[8, 0] = currErr[6] state[11, 0] = currErr[7] desiredInput = (-1) * np.dot(self.dlqrGain, state) + self.equilibriumInput # find the rotor speed for each rotor motorSpeeds = Actuators() motorSpeeds.angular_velocities = np.zeros((4, 1)) motorSpeedTransitionVec = np.dot( np.linalg.inv(self.speedAllocationMatrix), desiredInput) motorSpeeds.angular_velocities = np.sqrt( np.abs(motorSpeedTransitionVec)) self.dlqrPublisher.publish(motorSpeeds)
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")
def update_rotor_vels(self, id): # this function takes in the UAV's id and computes+publishes the rotor velocities to that UAV # get z_oold, z_old, and desired position/yaw self.get_data(id) # get roll/pitch/yawrate/thrust commands from position controller roll_c, pitch_c, z_dot_c, yaw_dot_c = self.controller.get_command( self._pos[str(id)].x, self._pos[str(id)].y, self._pos[str(id)].z, # x,y,z self._euler_angles[str(id)][0], self._euler_angles[str(id)][1], self._euler_angles[str(id)][0][2], # change? roll, pitch, yaw self.initials[str(id)][0], self.initials[str(id)][1], self.initials[str(id)][2], # change? xd, yd, zd self.vx_d[int(id)], self.vy_d[int(id)], self.vz_d[int(id)], self.yaw_d[int(id)]) # obtain p,q,r/roll,pitch,yaw for UAV id from odometry subscription p = self._euler_angular_rates[str(id)][0] q = self._euler_angular_rates[str(id)][1] r = self._euler_angular_rates[str(id)][2] roll = self._euler_angles[str(id)][0] pitch = self._euler_angles[str(id)][1] yaw = self._euler_angles[str(id)][2] # convert above commands to rotor velocity commands rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie( pitch_c, roll_c, yaw_dot_c, p, q, r, roll, pitch, yaw, z_dot_c) rotor_velocities = rotorvel_converter.CalculateRotorVelocities( ) # this yields a 4-element list # publish rotor velocities to Actuator rotorvel_msg = Actuators() rotorvel_msg.angular_velocities = rotor_velocities #rotorvel_msg.header.stamp = self._currpos_msg.header.stamp self.cmdV_pubs[str(cf_id)].publish(rotorvel_msg)
def callback(self, data): f = data.Total_thrust; M1 = data.Moment_x M2 = data.Moment_y; M3 = data.Moment_z T = tp(ar([[M1,M2,M3,f]])) if self.uav == 'pelican'or self.uav == 'hummingbird': c1 = tp(ar([[0, -self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c2 = tp(ar([[self.d*self.tau_f, 0, -self.tau_f*self.tau_m, self.tau_f]])) c3 = tp(ar([[0, self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c4 = tp(ar([[-self.d*self.tau_f, 0, -self.tau_f*self.tau_m, self.tau_f]])) C = np.column_stack((c1,c2,c3,c4)) # solving linear eq T = Cw^2 to get w^2 w_square = np.dot(np.linalg.inv(C),T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() #Msg.header.stamp = rospy.Time.now() Msg.header.stamp = data.header.stamp Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0]] f2 = open('motorspeeds.txt', 'a') f2.write("%s, %s, %s, %s\n" % (w[0][0], w[1][0], w[2][0], w[3][0])) self.pub.publish(Msg) else: c1 = tp(ar([[sin(pi/6)*self.d*self.tau_f, -cos(pi/6)*self.d*self.tau_f, -self.tau_f*self.tau_m, self.tau_f]])) c2 = tp(ar([[sin(pi/2)*self.d*self.tau_f, -cos(pi/2)*self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c3 = tp(ar([[sin(5*pi/6)*self.d*self.tau_f, -cos(5*pi/6)*self.d*self.tau_f, -self.tau_f*self.tau_m, self.tau_f]])) c4 = tp(ar([[sin(7*pi/6)*self.d*self.tau_f, -cos(7*pi/6)*self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) c5 = tp(ar([[sin(3*pi/2)*self.d*self.tau_f, -cos(3*pi/2)*self.d*self.tau_f, -self.tau_f*self.tau_m, self.tau_f]])) c6 = tp(ar([[sin(11*pi/6)*self.d*self.tau_f, -cos(11*pi/6)*self.d*self.tau_f, self.tau_f*self.tau_m, self.tau_f]])) C = np.column_stack((c1,c2,c3,c4,c5,c6)) # solving linear eq T = Cw^2 to get w^2 inverted_matrix = np.dot(C.T, np.linalg.inv(np.dot(C, C.T))) w_square = np.dot(inverted_matrix,T) w = np.sqrt(np.abs(w_square)) Msg = Actuators() Msg.header.stamp = rospy.Time.now() Msg.angular_velocities = [w[0][0], w[1][0], w[2][0], w[3][0], w[4][0], w[5][0]] self.pub.publish(Msg)
def run(self): ''' Runs ROS node - computes PID algorithms for cascade attitude control. ''' while not self.start_flag: print "Waiting for the first measurement." rospy.sleep(0.5) print "Starting attitude control." while not rospy.is_shutdown(): self.ros_rate.sleep() #################################################################### #################################################################### # Add your code for cascade control for roll, pitch, yaw. # reference attitude values are stored in self.euler_sp # (self.euler_sp.x - roll, self.euler_sp.y - pitch, self.euler_sp.z - yaw) # Measured attitude values are stored in self.euler_mv (x,y,z - roll, pitch, yaw) # Measured attitude rate values are store in self.euler_rate_mv (self.euler_rate_mv.x, y, z) # Your result should be reference velocity value for each motor. # Store them in variables mot_sp1, mot_sp2, mot_sp3, mot_sp4 #################################################################### #################################################################### # Publish motor velocities mot_speed_msg = Actuators() mot_speed_msg.angular_velocities = [mot_sp1,mot_sp2,mot_sp3,mot_sp4] self.pub_mot.publish(mot_speed_msg) # Publish PID data - could be usefule for tuning self.pub_pid_roll.publish(self.pid_roll.create_msg()) self.pub_pid_roll_rate.publish(self.pid_roll_rate.create_msg()) self.pub_pid_pitch.publish(self.pid_pitch.create_msg()) self.pub_pid_pitch_rate.publish(self.pid_pitch_rate.create_msg()) self.pub_pid_yaw.publish(self.pid_yaw.create_msg()) self.pub_pid_yaw_rate.publish(self.pid_yaw_rate.create_msg())
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-0.1 mot1 = self.mot_vel_ref + self.yaw_command - self.vpc_pitch_command mot2 = self.mot_vel_ref - self.yaw_command + self.vpc_roll_command mot3 = self.mot_vel_ref + self.yaw_command + self.vpc_pitch_command mot4 = self.mot_vel_ref - self.yaw_command - self.vpc_roll_command wing_front = -0.25 # - self.pitch_command #-0.25 - self.pitch_command # mm1 wing_back = 0.25 # - self.pitch_command #-0.25 + self.pitch_command # mm3 wing_left = -0.25 # + self.roll_command #0.25 - self.roll_command # mm2 wing_right = 0.25 # + self.roll_command #0.25 + self.roll_command # mm4 # Publish everything 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.wing_front_pub.publish(Float32(wing_front)) self.wing_back_pub.publish(Float32(wing_back)) self.wing_left_pub.publish(Float32(wing_left)) self.wing_right_pub.publish(Float32(wing_right)) all_wing_msg = Float64MultiArray() all_wing_msg.data = [wing_front, wing_left, wing_back, wing_right] self.wing_all_pub.publish(all_wing_msg)
def ctrl_update(self, state): """ Perform optimization and then formulate motor speeds""" xInit = cv.Parameter(self.nx) xInit.value = state # calculate the time difference # time now subtracted by start time currTime = rospy.get_time() - self.startTime xr = self.calc_ref_state(currTime) prob = self.mpc_problem_def(state, xr) prob.solve(solver=cv.OSQP, warm_start=True, verbose=False) desiredInput = self.u[:, 0].value + self.equilibriumInput desiredInput = desiredInput.reshape(desiredInput.shape[0], -1) # find the rotor speed for each rotor motorSpeeds = Actuators() motorSpeeds.angular_velocities = np.zeros((4, 1)) motorSpeedTransitionVec = np.dot( np.linalg.inv(self.speedAllocationMatrix), desiredInput) motorSpeeds.angular_velocities = np.sqrt( np.abs(motorSpeedTransitionVec)) self.mpcPublisher.publish(motorSpeeds)