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):
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 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 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 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 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 rospy.is_shutdown():
self.ros_rate.sleep()
newMsg = Actuators()
#newMsg.angular_velocities = Float64()
speed1 = 800
speed2 = 800
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 __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 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 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 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 = 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 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 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 __init__(self):
# True if node received bebop trajectory, otherwise false
self.trajectory_received = False
self.first_measurement = True
self.controller_info = False
self.trajectory_subscriber = rospy.Subscriber("/multi_dof_trajectory",
MultiDOFJointTrajectory,
self.trajectory_cb)
# initialize publishers
self.pos_ref_pub = rospy.Publisher("/bebop/pos_ref",
Vector3,
queue_size=10)
self.ang_ref_pub = rospy.Publisher("/bebop/angle_ref",
Vector3,
queue_size=10)
self.actuator_msg = Actuators()
# define vector for measured and setpoint values
self.pose_sp = Vector3(0., 0., 0.)
self.euler_sp = Vector3(0., 0., 0.)
self.euler_mv = Vector3(0., 0., 0.)
self.euler_rate_mv = Vector3(0., 0., 0.)
self.p = 0
self.q = 0
self.r = 0
# Controller rate
# Trajectory points are being given with a frequency of 100Hz
self.controller_rate = 50
self.rate = rospy.Rate(self.controller_rate)
self.t_old = 0
# Initialize trajectory information
self.trajectory_index = 0
self.trajectory_point_count = -1
self.trajectory_points = []
self.sleep_sec = 2
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 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)
