def set_ang_vel_thrust(self, ang_vel, thrust, freq):
"""
Offboard method that sends angular velocity and thrust references to the PX4 autopilot of the the drone.
Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS.
Converts the thrust from newtons to a normalized value between 0 and 1 through the mathematical expression of the thrust curve of the vehicle.
Parameters
----------
ang_vel : np.array of floats with shape (3,1)
Desired angular velocity for the drone.
thrust : float
Desired thrust value in newtons.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
msg = AttitudeTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.type_mask = 128
msg.body_rate.x, msg.body_rate.y, msg.body_rate.z = ned_to_enu(ang_vel)
msg.thrust = normalize_thrust(thrust, self.info.thrust_curve,
norm(self.ekf.vel))
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def update_attitude(self):
# Create message
msg = AttitudeTarget(header=Header(stamp=rospy.Time.now()))
# Ignore all but thrust
msg.type_mask = AttitudeTarget.IGNORE_ROLL_RATE | AttitudeTarget.IGNORE_PITCH_RATE | AttitudeTarget.IGNORE_YAW_RATE | AttitudeTarget.IGNORE_ATTITUDE
# # Set attitude
# msg.orientation.x = 0
# msg.orientation.y = 0
# msg.orientation.z = 0
# msg.orientation.w = 0
# # Set rates
# msg.body_rate.x = 0.1
# msg.body_rate.y = 0.1
# msg.body_rate.z = 0.1
# Set velocity
msg.thrust = self._thrust
# Navigate until location is reached
while not self._done.is_set() and not rospy.is_shutdown():
# Publish
self._rawatt_pub.publish(msg)
# Ensure proper communication rate
self._rate.sleep()
def callback(self, rc):
#rospy.loginfo('{} {} {} {}'.format(rc.channels[0], rc.channels[1], rc.channels[2], rc.channels[3]))
cmd = AttitudeTarget()
Ts = (rospy.Time.now() - self.previous_time).to_sec()
self.previous_time = rospy.Time.now()
cmd.header.stamp = rospy.Time.now()
cmd.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE
#ROS use ENU convention
roll = .6 * (rc.channels[1] - 1492.) / 500.
pitch = -.6 * (rc.channels[2] - 1493.) / 500.
yawrate = -3. * (rc.channels[3] - 1498.) / 500.
self.yaw = self.yaw + yawrate * Ts
thrust = (rc.channels[0] - 1000.) / 1200.
rospy.loginfo(
'Ts = {Ts:.3f} r = {r:.2f} p = {p:.2f} y = {y:.2f} t = {t:.2f}'.
format(Ts=Ts, r=roll, p=pitch, y=self.yaw, t=thrust))
q = quaternion_from_euler(roll, pitch, self.yaw)
cmd.orientation.x = q[0]
cmd.orientation.y = q[1]
cmd.orientation.z = q[2]
cmd.orientation.w = q[3]
cmd.body_rate.x = 0.
cmd.body_rate.y = 0.
cmd.body_rate.z = yawrate
cmd.thrust = thrust
self.pub.publish(cmd)
def main1():
global setpoint
# thrust_pub = rospy.Publisher('/mavros/setpoint_attitude/thrust',Thrust,queue_size=10)
# thrust = Thrust()
attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=10)
point_pub = rospy.Publisher('/waypoint',PoseStamped,queue_size=10)
attitude = AttitudeTarget()
attitude.type_mask = 3
attitude.header = msg.header
# attitude.header.frame_id = 'FRAME_LOCAL_NED'
# quaternion = tf.transformations.quaternion_from_euler(msg.roll,msg.pitch, 0)
# attitude.orientation.x = quaternion[0]
# attitude.orientation.y = quaternion[1]
# attitude.orientation.z = quaternion[2]
# attitude.orientation.w = quaternion[3]
attitude.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(msg.roll, msg.pitch, 0))
attitude.body_rate.z = msg.yaw_rate
t = msg.thrust.z/100
if t>1:
t=1
elif t<-1:
t=-1
attitude.thrust = (t+1)/2
attitude_pub.publish(attitude)
point_pub.publish(setpoint)
def test_control(self):
self.toggle_arm(True)
#self.takeoff(1.0)
self.set_offboard_mode()
self.move_to(0,0,5)
#sleep(2)
'''
self.move_to(-2,2,3.3)
sleep(2)
self.move_to(0,4,3.3)
sleep(2)
self.move_to(2,2,3.3)
sleep(2)'''
############
print('Starting')
sleep(5)
rate = rospy.Rate(20)
sr = AttitudeTarget()
sr.type_mask = 135
sr.thrust = 0.9
for i in range(20):
print('stg 1')
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Stage 1 done')
sr.type_mask = 134
sr.body_rate.x = 1500.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.5
for i in range(20):
print('stg 2')
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Stage 2 done')
sr.type_mask = 134
sr.body_rate.x = -1500.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.5
for i in range(5):
print('final')
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Roll Complete!!')
def thrust_recursion(self, thrust):
if self.status_thrust == "up":
if (thrust >= 1):
print ("the thrust has reached its desired point")
print ("the motors should start slowing down...")
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = 1
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
self.status_thrust = "slow down"
return self.thrust_recursion(target_raw_attitude.thrust)
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = thrust + 0.003
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.thrust_recursion(target_raw_attitude.thrust)
elif self.status_thrust == "slow down":
if (thrust <= 0):
print ("the motors should stop")
self.status_thrust = "stop"
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = thrust - 0.003
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.thrust_recursion(target_raw_attitude.thrust)
def send_commands(self, event):
# Create the AttitudeTarget command message.
command_msg = AttitudeTarget()
# Fill out the message.
command_msg.header.stamp = rospy.get_rostime()
command_msg.type_mask = self.ignore_mask
command_msg.thrust = self.thrust_val
# Publish the message.
self.command_pub.publish(command_msg)
def construct_target_attitude(self, body_x = 0, body_y = 0, body_z = 0, thrust = 0.2):
target_raw_attitude = AttitudeTarget() # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html
target_raw_attitude.header.stamp = rospy.Time.now()
#target_raw_attitude.orientation. = self.imu.orientation
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
#target_raw_attitude.body_rate.x = body_x # ROLL_RATE
#target_raw_attitude.body_rate.y = body_y # PITCH_RATE
#target_raw_attitude.body_rate.z = body_z # YAW_RATE
target_raw_attitude.thrust = thrust
return target_raw_attitude
def send_attitude_command(self):
command_msg = AttitudeTarget()
command_msg.header.stamp = rospy.get_rostime()
command_msg.type_mask = self.attitude_mask
command_msg.body_rate.x = 0.0
command_msg.body_rate.y = 0.0
command_msg.body_rate.z = 0.0
command_msg.thrust = self.thrust_val
# Publish.
self.attitude_pub_mavros.publish(command_msg)
def get_message(sp_att):
msg = AttitudeTarget()
msg.header.stamp = rospy.get_rostime()
msg.type_mask = AT.IGNORE_ROLL_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_YAW_RATE
q = quaternion_from_euler(sp_att[0], sp_att[1], sp_att[2])
msg.orientation.x = q[0]
msg.orientation.y = q[1]
msg.orientation.z = q[2]
msg.orientation.w = q[3]
msg.thrust = sp_att[3]
return msg
def thrust_recursion(self, thrust):
if (thrust >= 1):
print("the thrust has reached its desired point")
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \
+ AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = thrust + 0.003
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.thrust_recursion(target_raw_attitude.thrust)
def send_commands(self, event):
command_msg = AttitudeTarget()
command_msg.header.stamp = rospy.get_rostime()
command_msg.type_mask = self.ignore_mask
command_msg.body_rate.x = 0.0
command_msg.body_rate.y = 0.0
command_msg.body_rate.z = 0.0
command_msg.thrust = self.thrust_val
print self.thrust_val
print "here"
# Publish.
self.land_pub.publish(command_msg)
def body_recursion(self, thrust):
if self.status_thrust == "up":
if (thrust <= -1):
return True
else:
target_raw_attitude = AttitudeTarget()
target_raw_attitude.header.stamp = rospy.Time.now()
target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ATTITUDE
target_raw_attitude.thrust = 0.5
target_raw_attitude.body_rate.x = 0 # ROLL_RATE
target_raw_attitude.body_rate.y = thrust - 0.003 # PITCH_RATE
target_raw_attitude.body_rate.z = 0 # YAW_RATE
self.attitude_target_pub.publish(target_raw_attitude)
time.sleep(0.1)
return self.body_recursion(target_raw_attitude.body_rate.y)
def pathplanning():
global depth, net_plane_parameter, depth_des, distance_to_net_des, publisher_marker, publisher_waypoint
# transform into real_world_coordinates
f = 476.0
c = -net_plane_parameter[2]
a = net_plane_parameter[1]
x_real = 400.0 / f * np.abs(c)
a = a / abs(x_real) * 0.5
d = np.sqrt((x_real * a)**2 + x_real**2)
e = np.sqrt(d**2 + distance_to_net_des**2)
alpha = np.pi / 2 + np.arctan(a) # np.pi - np.pi / 2 - np.arctan(abs(a))
# if a > 0:
# alpha = alpha + np.pi / 2
gamma = np.arcsin(distance_to_net_des * np.sin(np.pi / 2) / e)
betta = np.pi - gamma - alpha
y_max = c + e * np.cos(betta)
x_max = np.sin(betta) * e
d_cubic = 0
c_cubic = a
b_cubic = (y_max - a * x_max) / (-2.0 / 3.0 * x_max**2 + x_max**2)
a_cubic = -2.0 * b_cubic / x_max / 3.0
stammfunktion = a_cubic * x_max**3 + b_cubic * x_max**2 + c_cubic * x_max + d_cubic
pitch_gain = 3
pitch = pitch_gain * np.arctan(
(depth - depth_des) / np.sqrt(stammfunktion**2 + x_max**2))
ableitung = 3.0 * a_cubic * (x_max / 2.0)**2 + 2.0 * b_cubic * (
x_max / 2.0) + c_cubic
yaw = np.arctan(ableitung)
# pitch = np.pi / 4
print("pitch:", pitch * 180 / np.pi)
# yaw=-np.pi/2
print("yaw:", yaw * 180 / np.pi)
roll = 0
qz_90n = Quaternion(axis=[0, 0, 1], angle=-(yaw - np.pi / 2)) * Quaternion(
axis=[0, 1, 0], angle=-pitch) * Quaternion(axis=[1, 0, 0], angle=roll)
thrust = 0.0
send_waypoint = AttitudeTarget()
send_waypoint.type_mask = 0
send_waypoint.orientation.x = qz_90n.x
send_waypoint.orientation.y = qz_90n.y
send_waypoint.orientation.z = qz_90n.z
send_waypoint.orientation.w = qz_90n.w
# print(qz_90n.x,qz_90n.y,qz_90n.z,qz_90n.w)
# 0.2 works
send_waypoint.thrust = thrust
publisher_waypoint.publish(send_waypoint)
# print("pitch:")
# print(-np.arctan((wanted_depth - depth) / 1)*180/np.pi)
# print("yaw:")
# print(1*(wanted_distance_to_net-distance_to_net))
# Visualisierung Rviz:
rviz = True
if rviz:
markerArray = MarkerArray()
i = 1
for x in np.linspace(0, x_max, 20):
r = 0.1
marker = Marker()
marker.header.frame_id = "local_boat"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = r # r*2 of distance to camera from tag_14
marker.scale.y = r
marker.scale.z = r
marker.color.r = 1
marker.color.a = 1 # transparency
marker.pose.orientation.w = 1.0
marker.pose.position.x = x # x
marker.pose.position.y = -(
a_cubic * x**3 + b_cubic * x**2 + c_cubic * x + d_cubic) # y
marker.pose.position.z = x / x_max * (depth - depth_des) # z
markerArray.markers.append(marker)
i = i + 1
publisher_marker.publish(markerArray)
def callback(msg):
""""""
global current_pos_number, N, R, p, rate, thrust, carrot, just_changed, do_roll
current_pos = p[1:4, current_pos_number]
send_waypoint = AttitudeTarget()
# look if next waypoint should be loaded
if np.sqrt((msg.pose.position.x - current_pos[0])**2 +
(msg.pose.position.y - current_pos[1])**2) < R: # define R
current_pos_number = current_pos_number + 1
if current_pos_number > N - 1:
current_pos_number = 0
current_pos = p[1:4, current_pos_number]
if current_pos_number == 18 and not just_changed: # every time the the 24 waypoint is seen, then parameter can change
change_parameter()
just_changed = True
if current_pos_number == 19:
just_changed = False
current_waypoint = pathplanning(
current_pos, np.asarray([msg.pose.position.x, msg.pose.position.y]))
#rotation_body_frame = Quaternion(w=msg.pose.orientation.w,
# x=msg.pose.orientation.x,
# y=msg.pose.orientation.y,
# z=msg.pose.orientation.z)
yaw_current, pitch_current, roll_current = yaw_pitch_roll([
msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z
])
# calculates with triangles the correct orientation for the vehicle
yaw_des = np.arctan2((current_waypoint[1] - msg.pose.position.y),
(current_waypoint[0] - msg.pose.position.x))
pitch_des = -np.arctan(
(wanted_z_position - msg.pose.position.z) / distance_to_point)
roll_des = 0.0 / 180.0 * np.pi
if auftauchen:
pitch_des = np.pi / 2 - 0.1
yaw_des = 0
roll_des = 0
send_waypoint.thrust = thrust * np.cos(yaw_current - yaw_des) * np.cos(
roll_current - roll_des) * np.cos(pitch_current - pitch_des)
if abs(yaw_current - yaw_des) > np.pi / 2 or abs(
roll_current - roll_des) > np.pi / 2 or abs(pitch_current -
pitch_des) > np.pi / 2:
send_waypoint.thrust = 0
if do_roll and current_pos_number > 85 and current_pos_number < 99:
current_pos_number = 98
roll_des = roll_current + np.pi / 2
print("roll_des", roll_des)
if roll_des > np.pi:
roll_des = roll_des - np.pi * 2
if roll_des > -np.pi / 3 and roll_des < 0:
roll_des = 0
do_roll = False
send_waypoint.thrust = thrust * 0.1
# yaw_des = 0.0 / 180.0 * np.pi
# pitch_des = 0.0 / 180.0 * np.pi
qz_90n = Quaternion(
axis=[0, 0, 1], angle=-(yaw_des - np.pi / 2)) * Quaternion(
axis=[0, 1, 0], angle=-pitch_des) * Quaternion(axis=[1, 0, 0],
angle=roll_des)
send_waypoint.type_mask = 0
send_waypoint.orientation.x = qz_90n.x
send_waypoint.orientation.y = qz_90n.y
send_waypoint.orientation.z = qz_90n.z
send_waypoint.orientation.w = qz_90n.w
# print(qz_90n.x,qz_90n.y,qz_90n.z,qz_90n.w)
# 0.2 works
if not do_roll:
send_waypoint.thrust = thrust * np.cos(yaw_current - yaw_des) * np.cos(
roll_current - roll_des) * np.cos(pitch_current - pitch_des)
if abs(yaw_current - yaw_des) > np.pi / 2 or abs(
roll_current -
roll_des) > np.pi / 2 or abs(pitch_current -
pitch_des) > np.pi / 2:
send_waypoint.thrust = 0.0
publisher_waypoint.publish(send_waypoint)
rate.sleep()
def euler_angle_controller(self, state, trajectory):
"""
@description This controller uses an euler angle representation of orientation
in order to control it.
@state quadrotor state
@trajectory reference trajectory
"""
# Extract orientation reference
ori_quat_ref = [
trajectory.pose.orientation.x, trajectory.pose.orientation.y,
trajectory.pose.orientation.z, trajectory.pose.orientation.w
]
psi_ref, theta_ref, phi_ref = tf.transformations.euler_from_quaternion(
ori_quat_ref, axes='rzyx')
Rbw_ref = np.array([
[trajectory.rot[0], trajectory.rot[1],
trajectory.rot[2]], # world to body reference transformation
[trajectory.rot[3], trajectory.rot[4], trajectory.rot[5]],
[trajectory.rot[6], trajectory.rot[7], trajectory.rot[8]]
])
# Extract real orientation
ori_quat = [
state.pose.pose.orientation.x, state.pose.pose.orientation.y,
state.pose.pose.orientation.z, state.pose.pose.orientation.w
]
psi, theta, phi = tf.transformations.euler_from_quaternion(ori_quat,
axes='rzyx')
Rwb = tf.transformations.quaternion_matrix(
ori_quat) # this is an homogenous world to body transformation
Rbw = Rwb[0:3,
0:3].T # body to world transformation, only rotation part
angular_velocity = np.array([[state.twist.twist.angular.x],
[state.twist.twist.angular.y],
[state.twist.twist.angular.z]])
# extract reference values
p_ref = np.array([[trajectory.pose.position.x],
[trajectory.pose.position.y],
[trajectory.pose.position.z]])
v_ref = np.array([[trajectory.twist.linear.x],
[trajectory.twist.linear.y],
[trajectory.twist.linear.z]])
a_ref = np.array(
[trajectory.acc.x, trajectory.acc.y,
trajectory.acc.z]).reshape(3, 1)
euler_dot_ref = np.array([[trajectory.uc.x], [trajectory.uc.y],
[trajectory.uc.z]])
# extract real values
p = np.array([[state.pose.pose.position.x],
[state.pose.pose.position.y],
[state.pose.pose.position.z]])
v = np.array([[state.twist.twist.linear.x],
[state.twist.twist.linear.y],
[state.twist.twist.linear.z]])
v = np.dot(Rbw, v)
state_ = [
p, v,
np.zeros((3, 1)),
np.zeros((3, 1)),
np.zeros((3, 1)), Rbw
]
ref_state = [
p_ref, v_ref, a_ref,
np.zeros((3, 1)),
np.zeros((3, 1)), Rbw_ref, trajectory.yaw, trajectory.yawdot,
trajectory.yawddot, euler_dot_ref
]
#self.T, self.Rbw_des, w_des = self.flc.position_controller(state_, ref_state)
self.T, self.Rbw_des, w_des = self.gc.position_controller(
state_, ref_state)
#w_des = attitude_controller(Rbw, self.Rbw_des)
# Fill out message
hlc_msg = riseq_high_level_control()
hlc_msg.header.stamp = rospy.Time.now()
hlc_msg.header.frame_id = 'riseq/uav'
#hlc_msg.thrust.x = self.Traw2 #np.linalg.norm(self.a_e) #for debugging purposes
#hlc_msg.thrust.y = self.Traw #np.linalg.norm(self.a_e2) #for debugging purposes
hlc_msg.thrust.z = self.T
hlc_msg.rot = self.Rbw_des.flatten().tolist()
hlc_msg.angular_velocity.x = angular_velocity[0][0]
hlc_msg.angular_velocity.y = angular_velocity[1][0]
hlc_msg.angular_velocity.z = angular_velocity[2][0]
hlc_msg.angular_velocity_des.x = 0.1 * w_des[0][0]
hlc_msg.angular_velocity_des.y = 0.1 * w_des[1][0]
hlc_msg.angular_velocity_des.z = 0.1 * w_des[2][0]
hlc_msg.angular_velocity_dot_ref.x = trajectory.ub.x
hlc_msg.angular_velocity_dot_ref.y = trajectory.ub.y
hlc_msg.angular_velocity_dot_ref.z = trajectory.ub.z
#self.hlc_pub.publish(hlc_msg)
#rospy.loginfo(hlc_msg)
px4_msg = AttitudeTarget()
px4_msg.header.stamp = rospy.Time.now()
px4_msg.header.frame_id = 'map'
px4_msg.type_mask = 7 #px4_msg.IGNORE_ATTITUDE
q = tf.transformations.quaternion_from_matrix(
utils.to_homogeneous_transform(self.Rbw_des))
px4_msg.orientation.x = q[0]
px4_msg.orientation.y = q[1]
px4_msg.orientation.z = q[2]
px4_msg.orientation.w = q[3]
px4_msg.body_rate.x = 0.01 * w_des[0][0]
px4_msg.body_rate.y = 0.01 * w_des[1][0]
px4_msg.body_rate.z = 0.01 * w_des[2][0]
px4_msg.thrust = np.min([1.0, 0.0381 * self.T]) #0.56
self.px4_pub.publish(px4_msg)
def actuate(x, y, z, v_test, v_min, v_max):
#plt.figure(figsize=(10,5))
#ax = plt.axes(projection ='3d')
ms = min_snap(x, y, z, v_test, v_min, v_max)
#ms.plot_test_case('r','Test Case Trajectory')
ms.optimize()
x_path, x_dot_path, x_dot_dot_path, y_path, y_dot_path, y_dot_dot_path, z_path, z_dot_path, z_dot_dot_path, psi_path = ms.get_trajectory_var(
)
#ms.plot('g','Time Optimized Trajectory')
#plt.legend()
#plt.show()
drone = DroneIn3D()
#sleep(2)
control_system = Controller(z_k_p=20.0,
z_k_d=1.5,
x_k_p=0.8,
x_k_d=0.0,
y_k_p=0.0,
y_k_d=0.0,
k_p_roll=0.0,
k_p_pitch=20,
k_p_yaw=0.0)
iter = 0
rate = rospy.Rate(20)
print(np.shape(z_path)[0])
#print(z_path)
#for i in range(0,z_path.shape[0]):
while (iter < np.shape(z_path)[0]):
rot_mat = drone.R()
c = control_system.altitude_controller(z_path[iter], z_dot_path[iter],
z_dot_dot_path[iter],
drone.X[2], drone.X[8], rot_mat)
b_x_c, b_y_c = control_system.lateral_controller(
x_path[iter], x_dot_path[iter], x_dot_dot_path[iter], drone.X[0],
drone.X[6], y_path[iter], y_dot_path[iter], y_dot_dot_path[iter],
drone.X[1], drone.X[7], c)
#for j in range(5):
rot_mat = drone.R()
p_c, q_c = control_system.roll_pitch_controller(b_x_c, b_y_c, rot_mat)
r_c = control_system.yaw_controller(psi_path[iter], drone.X[5])
#print(drone.X[0],drone.X[1],drone.X[2],drone.X[3],drone.X[4],drone.X[5],drone.X[6],drone.X[7],drone.X[8],'\n')
ATT = AttitudeTarget()
ATT.header.stamp = rospy.Time.now()
ATT.header.frame_id = 'map'
ATT.type_mask = 134
ATT.body_rate.x = p_c
ATT.body_rate.y = q_c
ATT.body_rate.z = r_c
if c < 0:
ATT.thrust = 0
else:
scaled_thrust = ((c - 9.8) / (2 * 9.8)) + 0.5
ATT.thrust = scaled_thrust
print('Calculated thrust :', c, ' | Scaled Thrust : ', ATT.thrust,
'| Body rates :', p_c, q_c, r_c, '\n')
drone.publish_attitude_thrust.publish(ATT)
iter += 1
rate.sleep()
print(' STARTING !!')
drone.gotopose(0, 0, 12)
def odom_callback(self, odom):
"""send command to px4"""
self.odom_time = odom.header.stamp.to_sec()
t = float(self.odom_time - self.traj_start_time)
if t <= self.traj_end_time[self.number_of_segments]:
index = bisect.bisect(self.traj_end_time, t)-1
else:
index = bisect.bisect(self.traj_end_time, t)-2
#fff = open('traj_time_in_odom.txt', 'a')
#fff.write('%s, %s, %s, %s, %s\n' %(index, self.odom_time, self.traj_start_time, t, self.traj_end_time[self.number_of_segments]))
if index == -1:
pass
else:
xx = np.poly1d(self.p1[index])
vx = np.polyder(xx, 1); aax = np.polyder(xx, 2)
jx = np.polyder(xx, 3)
yy = np.poly1d(self.p2[index])
vy = np.polyder(yy, 1); aay = np.polyder(yy, 2)
jy = np.polyder(yy, 3)
zz = np.poly1d(self.p3[index])
vz = np.polyder(zz, 1); aaz = np.polyder(zz, 2)
jz = np.polyder(zz, 3)
if t <= self.traj_end_time[self.number_of_segments]:
#print ar([[xx(t)], [yy(t)], [zz(t)]])
self.pdes = ar([[xx(t)], [yy(t)], [zz(t)]])
self.vdes = ar([[vx(t)], [vy(t)], [vz(t)]])
self.ades = ar([[aax(t)], [aay(t)], [aaz(t)]])
self.jdes = ar([[jx(t)], [jy(t)], [jz(t)]])
self.ddes = self.ddir#d/np.linalg.norm(d)
else:
self.pdes = ar([[xx(self.traj_end_time[-1])], [yy(self.traj_end_time[-1])], [zz(self.traj_end_time[-1])]])
self.vdes = ar([[vx(self.traj_end_time[-1])], [vy(self.traj_end_time[-1])], [vz(self.traj_end_time[-1])]])
self.ades = ar([[aax(self.traj_end_time[-1])], [aay(self.traj_end_time[-1])], [aaz(self.traj_end_time[-1])]])
self.jdes = ar([[jx(self.traj_end_time[-1])], [jy(self.traj_end_time[-1])], [jz(self.traj_end_time[-1])]])
self.ddes = self.ddir
if self.mode == 'hover':
self.ddes = self.ddir
current_time = time.time()-self.start_time
if self.counter == 0:
self.initial_odom_time = current_time
X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]])
q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\
odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
R = q.rotation_matrix
# ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix
V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]])
# CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW?
Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]])
Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities
Xd = self.pdes; Vd = self.vdes; ad = self.ades; b1d = self.ddes
b1d_dot = ar([[0],[0],[0]]); ad_dot = self.jdes
if self.controller == 1: # velocity controller
ex = ar([[0],[0],[0]])
else:
ex = X-Xd
ev = V-Vd
#_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+ mult(self.ki, ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction
_b3c = -(mult(self.kx, ex) + mult(self.kv, ev))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction
b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector
b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d
b1c = tp(np.cross(b2c.T, b3c.T))
Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix
#qc = self.rotmat2quat(Rc)
#qc = rowan.to_matrix(Rc)
qc = self.rotmat2quat(Rc)
#print 'qc', qc
omega_des = q.inverse*qc
self.f = self.m*np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.norm_thrust_constant # normalized thrust
#self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega))))
msg = AttitudeTarget()
msg.header.stamp = rospy.Time.now()#odom.header.stamp
msg.type_mask = 128
msg.body_rate.x = self.factor*np.sign(omega_des[0])*omega_des[1]
msg.body_rate.y = self.factor*np.sign(omega_des[0])*omega_des[2]
msg.body_rate.z = self.factor*np.sign(omega_des[0])*omega_des[3]
msg.thrust = min(1.0, self.f[0][0])
print 'thrust:', self.f*self.norm_thrust_constant, 'thrust_factor:', min(1.0, self.f[0][0])
print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z
print 'zheight', Xd[0][2]
self.counter = self.counter+1
self.pub.publish(msg)
def flip(self):
'''print('Starting')
rospy.sleep(5)
rate = rospy.Rate(20)
sr = AttitudeTarget()
#sr.type_mask = 135
sr.type_mask = 128
sr.header.frame_id="map"
sr.body_rate.x = 0.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.9
for i in range(20):
sr.header.stamp=rospy.Time.now()
#print('stg 1\n')
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Stage 1 done')
#sr.header.frame_id="map"
sr.type_mask = 128
sr.body_rate.x = 1500.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.5
for i in range(20):
#print('stg 2\n')
sr.header.stamp=rospy.Time.now()
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Stage 2 done')
sr.header.frame_id="map"
sr.type_mask = 128
sr.body_rate.x = -1500.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.5
for i in range(5):
#print('final')
sr.header.stamp=rospy.Time.now()
self.publish_attitude_thrust.publish(sr)
rate.sleep()
print('Roll Complete!!')'''
#self.setmode("GUIDED_NOGPS")
print('Starting')
rospy.sleep(5)
rate = rospy.Rate(20)
sr = AttitudeTarget()
sr.type_mask = 134
sr.thrust = 0.0
sr.body_rate.x = 0.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
for i in range(40):
#print('descend - 0.0',self.acc_x,self.acc_y,self.acc_z)
self.publish_attitude_thrust.publish(sr)
print('descend - 0.0 | Z acceleration', self.acc_z)
rate.sleep()
print('Stage 1 done')
rospy.sleep(5)
print('descend - 0.0 | Z acceleration', self.acc_z)
sr.type_mask = 134
#sr.body_rate.x = 1500.0
sr.body_rate.x = 0.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 0.5
for i in range(40):
#print('Hover - 0.5',self.acc_x,self.acc_y,self.acc_z)
self.publish_attitude_thrust.publish(sr)
print('Hover - 0.5 | Z acceleration', self.acc_z)
rate.sleep()
print('Stage 2 done')
rospy.sleep(5)
print('Hover - 0.5 | Z acceleration', self.acc_z)
sr.type_mask = 134
# sr.body_rate.x = -1500.0
sr.body_rate.x = 0.0
sr.body_rate.y = 0.0
sr.body_rate.z = 0.0
sr.thrust = 1
for i in range(40):
self.publish_attitude_thrust.publish(sr)
print('Ascend - 1 | Z acceleration', self.acc_z)
rate.sleep()
print('Roll Complete!!')
rospy.sleep(5)
print('Ascend - 1 | Z acceleration', self.acc_z)
def odom_callback(self, odom):
"""send commands to px4"""
current_time = odom.header.stamp.to_sec()
self.ct = current_time
if self.counter == 0:
self.initial_odom_time = current_time
X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]])
q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\
odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
R = q.rotation_matrix
V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]])
# CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME.
Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]])
#Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities
try:
index, value = min(enumerate(self.trajectory_time), key=lambda x: abs(x[1]-current_time))
Xd = self.despos[index]; Vd = self.desvel[index]; ad = self.desacc[index]; b1d = self.desdirection[index]
#print index, Xd, Vd, ad
except:
rospy.loginfo('Trajectory has not yet been received.')
if self.controller == 1: # velocity controller
ex = ar([[0],[0],[0]])
else:
ex = X-Xd
ev = V-Vd
_b3c = -(mult(self.kx, ex) + mult(self.kv, ev))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction
b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector
b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d
b1c = tp(np.cross(b2c.T, b3c.T))
Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix`
qc = self.rotmat2quat(Rc)
omega_des = q.inverse*qc
thrust = self.m*np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.norm_thrust_constant # normalized thrust
msg = AttitudeTarget()
msg.header.stamp = odom.header.stamp
msg.type_mask = 128
msg.body_rate.x = self.factor_rp*np.sign(omega_des[0])*omega_des[1]
msg.body_rate.y = self.factor_rp*np.sign(omega_des[0])*omega_des[2]
msg.body_rate.z = self.factor_yaw*np.sign(omega_des[0])*omega_des[3]
msg.thrust = min(1.0, thrust[0][0])
#print 'thrust:', thrust*self.norm_thrust_constant, 'thrust_factor:', min(1.0, thrust[0][0])
#print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z
#print 'zheight', Xd[2][0]
f1 = open(self.path+'position_trajectory_RN{}.txt'.format(self.RN), 'a')
f1.write("%s, %s, %s, %s, %s, %s\n" % (Xd[0][0], Xd[1][0], Xd[2][0], X[0][0], X[1][0], X[2][0]))
f2 = open(self.path+'velocity_trajectory_RN{}.txt'.format(self.RN), 'a')
f2.write("%s, %s, %s, %s, %s, %s\n" % (Vd[0][0], Vd[1][0], Vd[2][0], V[0][0], V[1][0], V[2][0]))
f3 = open(self.path + 'commands_generated_RN{}.txt'.format(self.RN), 'a')
f3.write("%s, %s, %s, %s\n" % (msg.thrust, msg.body_rate.x, msg.body_rate.y, msg.body_rate.z))
self.counter = self.counter+1
self.pub.publish(msg)
def main():
# INITIALIZE ADAPTIVE NETWORK PARAMETERS:
N_INPUT = 3 # Number of input network
N_OUTPUT = 1 # Number of output network
INIT_NODE = 1 # Number of node(s) for initial structure
INIT_LAYER = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
N_WINDOW = 500 # Sliding Window Size
EVAL_WINDOW = 3 # Evaluation Window for layer Adaptation
DELTA = 0.05 # Confidence Level for layer Adaptation (0.05 = 95%)
ETA = 0.0001 # Minimum allowable value if divided by zero
FORGET_FACTOR = 0.98 # Forgetting Factor of Recursive Calculation
#EXP_DECAY = 1 # Learning Rate decay factor
#LEARNING_RATE = 0.00005 # Network optimization learning rate
LEARNING_RATE = 0.00002 / 2 # Network optimization learning rate
WEIGHT_DECAY = 0.000125 / 5 # Regularization weight decay factor
#WEIGHT_DECAY = 0.0 # Regularization weight decay factor
# INITIALIZE SYSTEM AND SIMULATION PARAMETERS:
IRIS_THRUST = 0.5629 # Nominal thrust for IRIS Quadrotor
RATE = 30.0 # Sampling Frequency (Hz)
# PID_GAIN_X = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-X
# PID_GAIN_Y = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-Y
PID_GAIN_X = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-X
PID_GAIN_Y = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-Y
#PID_GAIN_Z = [0.013,0.0004,0.2] # PID Gain Parameter [KP,KI,KD] axis-Z
PID_GAIN_Z = [0.013, 0.0, 0.2] # PID Gain Parameter [KP,KI,KD] axis-Z
SIM_TIME = 200 # Simulation time duration (s)
# Initial conditions of UAV system
xref = 0.0
yref = 0.0
zref = 2.0
interrx = 0.0
interry = 0.0
interrz = 0.0
errlastx = 0.0
errlasty = 0.0
errlastz = 0.0
runtime = 0.0
# Ignite the Evolving NN Controller
Xcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
Ycon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
Zcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
if INIT_LAYER > 2:
pdb.set_trace()
for i in range(INIT_LAYER - 2):
Xcon.addhiddenlayer()
Ycon.addhiddenlayer()
Zcon.addhiddenlayer()
dt = 1 / RATE
Xcon.dt = dt
Ycon.dt = dt
Zcon.dt = dt
Xcon.smcpar = PID_GAIN_X
Ycon.smcpar = PID_GAIN_Y
Zcon.smcpar = PID_GAIN_Z
# PX4 API object
modes = fcuModes() # Flight mode object
uav = uavCommand() # UAV command object
# Data Logger object
logData = dataLogger()
xconLog = dataLogger()
yconLog = dataLogger()
zconLog = dataLogger()
# Initiate node and subscriber
rospy.init_node('setpoint_node', anonymous=True)
rate = rospy.Rate(RATE) # ROS loop rate, [Hz]
# Subscribe to drone state
rospy.Subscriber('mavros/state', State, uav.stateCb)
# Subscribe to drone's local position
#rospy.Subscriber('mavros/local_position/pose', PoseStamped, uav.posCb)
rospy.Subscriber('mavros/mocap/pose', PoseStamped, uav.posCb)
# Setpoint publisher
att_sp_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
# Initiate Attitude messages
att = AttitudeTarget()
att.body_rate = Vector3()
att.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
att.thrust = IRIS_THRUST
att.type_mask = 7
# Arming the UAV --> no auto arming
text = colored("Ready for Arming..Press Enter to continue...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
while not uav.state.armed:
modes.setArm()
rate.sleep()
text = colored('Vehicle is arming..', 'yellow')
print(text)
# Switch to OFFBOARD after send few setpoint messages
text = colored("Vehicle Armed!! Press Enter to switch OFFBOARD...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
while not uav.state.armed:
modes.setArm()
rate.sleep()
text = colored('Vehicle is arming..', 'yellow')
print(text)
rate.sleep()
k = 0
while k < 10:
att_sp_pub.publish(att)
rate.sleep()
k = k + 1
modes.setOffboardMode()
text = colored('Now in OFFBOARD Mode..',
'blue',
attrs=['reverse', 'blink'])
print(text)
# ROS Main loop::
k = 0
while not rospy.is_shutdown():
start = time.time()
rate.sleep()
# Setpoint generator
if runtime <= 20:
xref = 0.0
yref = 0.0
zref = runtime * 1.5 / 20.0
if runtime > 20 and runtime <= 30:
xref = (runtime - 20) * 1.0 / 10.0
#xref = (runtime-20)* 5.0/10.0
yref = 0.0
zref = 1.5
if runtime > 30:
xref = 0.0 + 1.0 * math.cos(math.pi * (runtime - 30) / 20.0)
yref = 0.0 + 1.0 * math.sin(math.pi * (runtime - 30) / 20.0)
#xref = 0.0+5.0*math.cos(math.pi*(runtime-30)/60.0)
#yref = 0.0+5.0*math.sin(math.pi*(runtime-30)/60.0)
zref = 1.5
# Adjust the number of nodes in the latest layer (Network Width)
Xcon.adjustWidth(FORGET_FACTOR, N_WINDOW)
Ycon.adjustWidth(FORGET_FACTOR, N_WINDOW)
#Zcon.adjustWidth(FORGET_FACTOR,N_WINDOW)
# Adjust the number of layer (Network Depth)
Xcon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
Ycon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
#Zcon.adjustDepth(ETA,DELTA,N_WINDOW,EVAL_WINDOW)
# update current position
xpos = uav.local_pos.x
ypos = uav.local_pos.y
zpos = uav.local_pos.z
# calculate errors,delta errors, and integral errors
errx, derrx, interrx = Xcon.calculateError(xref, xpos)
erry, derry, interry = Ycon.calculateError(yref, ypos)
errz, derrz, interrz = Zcon.calculateError(zref, zpos)
# PID Controller equations
#theta_ref = 0.04*errx+0.0005*interrx+0.01*derrx
#phi_ref = 0.04*erry+0.0005*interry+0.01*derry
# vx = PID_GAIN_X[0]*errx+PID_GAIN_X[1]*interrx+PID_GAIN_X[2]*derrx
# vy = PID_GAIN_Y[0]*erry+PID_GAIN_Y[1]*interry+PID_GAIN_Y[2]*derry
#thrust_ref = IRIS_THRUST+PID_GAIN_Z[0]*errz+PID_GAIN_Z[1]*interrz+PID_GAIN_Z[2]*derrz
# SMC + NN controller
vx = Xcon.controlUpdate(xref) # Velocity X
vy = Ycon.controlUpdate(yref) # Velocity Y
thz = Zcon.controlUpdate(zref) # Additional Thrust Z
euler = euler_from_quaternion(uav.q)
psi = euler[2]
phi_ref, theta_ref = transform_control_signal(vx, vy, psi)
thrust_ref = IRIS_THRUST + thz
# control signal limiter
#phi_ref = limiter(-1*phi_ref,0.2)
phi_ref = limiter(-1 * phi_ref, 0.25)
theta_ref = limiter(-1 * theta_ref, 0.25)
att.thrust = limiter(thrust_ref, 1)
# Phi, Theta and Psi quaternion transformation
att.orientation = Quaternion(
*quaternion_from_euler(phi_ref, theta_ref, 0.0))
# Publish the control signal
att_sp_pub.publish(att)
# NN Learning stage
Xcon.optimize(LEARNING_RATE, WEIGHT_DECAY)
Ycon.optimize(LEARNING_RATE, WEIGHT_DECAY)
Zcon.optimize(LEARNING_RATE, WEIGHT_DECAY)
# Print all states
print('Xr,Yr,Zr = ', xref, yref, zref)
print('X, Y, Z = ', uav.local_pos.x, uav.local_pos.y,
uav.local_pos.z)
print('phi, theta = ', phi_ref, theta_ref)
print('att angles = ', euler)
print('Thrust = ', att.thrust)
print('Nodes X Y Z = ', Xcon.nNodes, Ycon.nNodes, Zcon.nNodes)
print('Layer X Y Z = ', Xcon.nLayer, Ycon.nLayer, Zcon.nLayer)
k += 1
runtime = k * dt
# Append Data Logger
logData.appendStateData(runtime, xref, yref, zref, xpos, ypos, zpos,
phi_ref, theta_ref, thrust_ref)
xconLog.appendControlData(runtime, Xcon.us, Xcon.un, Xcon.u,
Xcon.nNodes)
yconLog.appendControlData(runtime, Ycon.us, Ycon.un, Ycon.u,
Ycon.nNodes)
zconLog.appendControlData(runtime, Zcon.us, Zcon.un, Zcon.u,
Zcon.nNodes)
print('Runtime = ', runtime)
print('Exec time = ', time.time() - start)
print('')
# Break condition
if runtime > SIM_TIME:
modes.setRTLMode()
text = colored('Auto landing mode now..',
'blue',
attrs=['reverse', 'blink'])
print(text)
text = colored('Now saving all logged data..',
'yellow',
attrs=['reverse', 'blink'])
print(text)
logData.plotFigure()
xconLog.plotControlData()
yconLog.plotControlData()
zconLog.plotControlData()
text = colored('Mission Complete!!',
'green',
attrs=['reverse', 'blink'])
print(text)
break
def send_commands(self, odom, traj):
"""send command to px4"""
X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]])
q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\
odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
R = q.rotation_matrix
# ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix
V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]])
# suppressed for now as Patrick's code doesnt give angular velocities, not needed also with Pixhawk and approximate
# controller
# CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW?
#Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]])
#Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities
# Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction
Xd = ar([[traj.pdes.x], [traj.pdes.y], [traj.pdes.z]])
Vd = ar([[traj.vdes.x], [traj.vdes.y], [traj.vdes.z]])
ad = ar([[traj.ades.x], [traj.ades.y], [traj.ades.z]])
b1d = ar([[traj.ddes.x], [traj.ddes.y], [traj.ddes.z]])
ex = ar([[0],[0],[0]]) if traj.controller == 1 else X-Xd
ev = V-Vd
#if self.counter==0:
# ei = 0; self.ei = ei
#else:
# ei = (ex+ev)*self.dt; ei = self.ei+ei; self.ei = ei
#_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)+ mult(self.ki, ei))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction
_b3c = -(mult(self.kx, ex) + mult(self.kv, ev))/self.m + self.g*self.e[:,2][np.newaxis].T + ad # desired direction
b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector
b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d
b1c = tp(np.cross(b2c.T, b3c.T))
Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix
#qc = self.rotmat2quat(Rc)
#qc = rowan.to_matrix(Rc)
qc = self.rotmat2quat(Rc)
#print 'qc', qc
omega_des = q.inverse*qc
phi = np.arctan2(b2c[1][0], b2c[0][0])
if phi < 0:
phi = phi + 2 * np.pi
#error = 0.5 * np.trace(self.e-np.dot(Rc.T, R))
if self.counter != 0:
phi_dot = (phi-self.phi_)/self.dt; self.phi_ = phi
else:
phi_dot = 0; self.phi_ = phi
Omega_c = ar([[0], [0], [phi_dot]])
#eR_hat = 0.5*(np.dot(Rc.T, R) - np.dot(R.T, Rc)) # eR skew symmetric matrix
#
#eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation
#eOmega = Omega - np.dot(np.dot(R.T, Rc), Omega_c) # vector that gives error in angular velocity
self.f = self.m*np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.norm_thrust_constant # normalized thrust
#self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega))))
msg = AttitudeTarget()
msg.header.stamp = odom.header.stamp
msg.type_mask = 128
msg.body_rate.x = self.factor*np.sign(omega_des[0])*omega_des[1]
msg.body_rate.y = self.factor*np.sign(omega_des[0])*omega_des[2]
msg.body_rate.z = self.factor*np.sign(omega_des[0])*omega_des[3]
msg.thrust = min(1.0, self.f[0][0])
print 'thrust:', self.f*self.norm_thrust_constant, 'thrust_factor:', min(1.0, self.f[0][0])
print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z
print 'zheight', traj.pdes.z
f0 = open(self.path + 'commands.txt', 'a')
f0.write("%s, %s, %s, %s\n" % (msg.thrust, msg.body_rate.x, msg.body_rate.y, msg.body_rate.z))
self.counter = self.counter+1
self.pub.publish(msg)
def __init__(self):
self.att_sp_cb_flag = False
self.thrust_sp_cb_flag = False
self.rc_cb_flag = False
self.yaw_sp_cb_flag = False
# Rate init
# DECIDE ON PUBLISHING RATE
self.rate = rospy.Rate(20.0) # MUST be more then 2Hz
self.attitude_thrust_pub = rospy.Publisher(
"/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10)
attitude_target_sub = rospy.Subscriber(
"/px4_quad_controllers/rpy_setpoint", PoseStamped,
self.attitude_setpoint_sub_callback)
thrust_target_sub = rospy.Subscriber(
"/px4_quad_controllers/thrust_setpoint", PoseStamped,
self.thrust_setpoint_sub_callback)
yaw_target_sub = rospy.Subscriber("/px4_quad_controllers/yaw_setpoint",
PoseStamped,
self.yaw_setpoint_sub_callback)
# Manual control sub
thrust_target_sub = rospy.Subscriber("/mavros/rc/in", RCIn,
self.rc_in_sub_callback)
self.attitude_threshold = rospy.get_param(
'/attitude_thrust_publisher/attitude_threshold')
while not rospy.is_shutdown():
# if(self.att_sp_cb_flag==True and self.thrust_sp_cb_flag==True):
if (self.rc_cb_flag and self.att_sp_cb_flag
and self.yaw_sp_cb_flag):
if (not self.thrust_sp_cb_flag):
self.thrust_sp = rospy.get_param(
'/attitude_thrust_publisher/thrust_sp')
# USE THE NEXT 8 LINES ONLY FOR INITIAL TESTING
# self.att_r = \
# rospy.get_param('/attitude_thrust_publisher/att_r')
# self.att_p = \
# rospy.get_param('/attitude_thrust_publisher/att_p')
# self.att_y = \
# rospy.get_param('/attitude_thrust_publisher/att_y')
# self.thrust_sp = \
# rospy.get_param('/attitude_thrust_publisher/thrust_sp')
# print 'att_r'+str(att_r)
# print 'att_p'+str(att_p)
# print 'att_y'+str(att_y)
# print 'thrust_sp'+str(thrust_sp)
# print('\n')
#temp_att_r = self.rc_roll
#temp_att_p = -self.rc_pitch
temp_att_r = self.att_r
temp_att_p = self.att_p
if temp_att_p > self.attitude_threshold:
temp_att_p = self.attitude_threshold
if temp_att_r > self.attitude_threshold:
temp_att_r = self.attitude_threshold
# Manual control
att_quat_w, att_quat_x, att_quat_y, att_quat_z = \
tf.transformations.quaternion_from_euler(
self.att_y, temp_att_p,
temp_att_r, axes='sxyz')
target_attitude_thrust = AttitudeTarget()
target_attitude_thrust.header.frame_id = "home"
target_attitude_thrust.header.stamp = rospy.Time.now()
target_attitude_thrust.type_mask = 7
target_attitude_thrust.orientation.x = att_quat_x
target_attitude_thrust.orientation.y = att_quat_y
target_attitude_thrust.orientation.z = att_quat_z
target_attitude_thrust.orientation.w = att_quat_w
target_attitude_thrust.thrust = self.thrust_sp
self.attitude_thrust_pub.publish(target_attitude_thrust)
self.rate.sleep()
def callback(msg):
""""""
global current_pos_number, N, R, p, rate, thrust, carrot, yaw
# msg=PoseStamped()# SPAETER ENTFERNEN@@@@@@@@@@@@@@@@@@@@@@@@@@@
# msg.pose.position.x
# print(p.shape)
# z 90 erst dann x 180
current_pos = p[1:4, current_pos_number]
if np.sqrt((msg.pose.position.x - current_pos[0])**2 +
(msg.pose.position.y - current_pos[1])**2) < R: # define R
current_pos_number = current_pos_number + 1
if current_pos_number > N - 1:
current_pos_number = 0
current_pos = p[1:4, current_pos_number]
rotation_body_frame = Quaternion(w=msg.pose.orientation.w,
x=msg.pose.orientation.x,
y=msg.pose.orientation.y,
z=msg.pose.orientation.z)
yaw, pitch, roll = rotation_body_frame.inverse.yaw_pitch_roll
#print("first:",yaw* 180.0 / np.pi, pitch* 180.0 / np.pi, roll* 180.0 / np.pi)
#yaw = (-yaw - 90 / 180.0 * np.pi + 360 / 180.0 * np.pi) % (np.pi * 2) - 180 / 180.0 * np.pi
yaw_current = -yaw
pitch_current = -pitch
roll_current = -((roll + 360 / 180.0 * np.pi) %
(np.pi * 2) - 180 / 180.0 * np.pi)
# print(roll_current * 180.0 / np.pi, pitch_current * 180.0 / np.pi, yaw_current * 180.0 / np.pi)
# print(np.sqrt((msg.pose.position.x - current_pos[0]) ** 2 + (msg.pose.position.y - current_pos[1]) ** 2))
# print("wanted_pos:", current_pos)
# print("current_pos:", msg.pose.position.x, msg.pose.position.y)
# print(msg.pose.position)
# send_waypoint = PoseStamped()
# send_waypoint.header.stamp = rospy.Time.now()
# send_waypoint.header.frame_id = "global_tank"
# send_waypoint.pose.position.x = 0.1
# send_waypoint.pose.position.y = 0.2
# send_waypoint.pose.position.z = 0.3
# send_waypoint.pose.orientation.x = 0
# send_waypoint.pose.orientation.y = 0
# send_waypoint.pose.orientation.z = 0
# send_waypoint.pose.orientation.w = 1
####TEST ANDERES POSE SENDEN############
# send_waypoint = PoseStamped()
# send_waypoint.header.stamp = rospy.Time.now()
# send_waypoint.header.frame_id = "LOCAL_NED"
# send_waypoint.pose.position.x = current_pos[0]
# send_waypoint.pose.position.y = current_pos[1]
# send_waypoint.pose.position.z = 0
# 2D controller
# yaw = np.arctan2((current_pos[1] - msg.pose.position.y), (current_pos[0] - msg.pose.position.x))
# qz_90n = Quaternion(axis=[0, 0, 1], angle=-(yaw - np.pi / 2))
# 3d controller CARROT CONTROL
if carrot == 1:
yaw2 = np.arctan2((current_pos[1] - msg.pose.position.y),
(current_pos[0] - msg.pose.position.x))
# yaw = 0 / 180.0 * np.pi
pitch = -np.arctan(
(wanted_z_position - msg.pose.position.z) / distance_to_point)
# print(yaw2)
# print(yaw2)
# yaw2=yaw2+np.pi
# print("current Depth:",msg.pose.position.z)
# print("Pitch:",pitch*180.0/np.pi)
# print(msg.pose.position.z , wanted_z_position)
# print("Yaw:",yaw*180.0/np.pi)
# pitch = 0
# print(yaw2)
#yaw2 = 0.0 / 180.0 * np.pi
#pitch = 0.0 / 180.0 * np.pi
roll_old = 0.0 / 180.0 * np.pi
# pitch=-pitch_old
roll = roll_old
# pitch = -pitch_old*np.cos(yaw2) + roll_old * np.sin(yaw2)
# roll = pitch_old * np.sin(yaw2) + roll_old * np.cos(yaw2)
# print(pitch)
# qz_90n = Quaternion(axis=[0, 1, 0], angle=roll) * Quaternion(axis=[1, 0, 0], angle=pitch) * Quaternion(
# axis=[0, 0, 1], angle=-( yaw2 - np.pi / 2))
# qz_90n = Quaternion(axis=[1, 0, 0], angle=roll) * Quaternion(axis=[0, 1, 0], angle=pitch) * Quaternion(
# axis=[0, 0, 1], angle=yaw2)
qz_90n = Quaternion(
axis=[0, 0, 1], angle=-(yaw2 - np.pi / 2)) * Quaternion(
axis=[0, 1, 0], angle=-pitch) * Quaternion(axis=[1, 0, 0],
angle=roll)
# qz_90n = qz_90n*Quaternion(w=msg.pose.orientation.w,x=msg.pose.orientation.x,y=msg.pose.orientation.y,z=msg.pose.orientation.z)
else:
# 3d controller DARPA Control
# error noch ausrechnen
yaw = current_pos[2] + 0.5 * np.arctan2(
(current_pos[1] - msg.pose.position.y),
(current_pos[0] - msg.pose.position.x))
print(
"alpha_k:", current_pos[2], "error:",
np.arctan2((current_pos[1] - msg.pose.position.y),
(current_pos[0] - msg.pose.position.x)), "yaw:", yaw)
# yaw = 0 / 180.0 * np.pi
pitch = np.arctan(
(msg.pose.position.z - wanted_z_position) / distance_to_point)
# pitch = 0
# pitch = 20 / 180.0 * np.pi
roll = 0 / 180.0 * np.pi
qz_90n = Quaternion(axis=[0, 1, 0], angle=roll) * Quaternion(
axis=[1, 0, 0], angle=pitch) * Quaternion(axis=[0, 0, 1],
angle=-(yaw - np.pi / 2))
# roll= 100/180.0 * np.pi
# qz_90n = Quaternion(axis=[1, 0, 0], angle=roll)
# send_waypoint.pose.orientation.x = 0
# send_waypoint.pose.orientation.y = 0
# send_waypoint.pose.orientation.z = 0
# send_waypoint.pose.orientation.w = 1
# send_waypoint.pose.orientation.x = qz_90n.x
# send_waypoint.pose.orientation.y = qz_90n.y
# send_waypoint.pose.orientation.z = qz_90n.z
# send_waypoint.pose.orientation.w = qz_90n.w
# print(qz_90n)
# send_waypoint = PositionTarget()
# send_waypoint.coordinate_frame = send_waypoint.FRAME_LOCAL_NED
# send_waypoint.header.stamp = rospy.Time.now()
# send_waypoint.header.frame_id = "global_tank"
# # send_waypoint.type_mask = 4 + 8 + 16 + 32 + 64 + 128 + 256 + 512 + 2048
# send_waypoint.position.x = current_pos[0]
# send_waypoint.position.y = current_pos[1]
# send_waypoint.yaw = np.pi / 4
send_waypoint = AttitudeTarget()
send_waypoint.type_mask = 0
send_waypoint.orientation.x = qz_90n.x
send_waypoint.orientation.y = qz_90n.y
send_waypoint.orientation.z = qz_90n.z
send_waypoint.orientation.w = qz_90n.w
# print(qz_90n.x,qz_90n.y,qz_90n.z,qz_90n.w)
# 0.2 works
send_waypoint.thrust = thrust * np.cos(yaw_current - yaw2)
if abs(yaw_current - yaw2) > np.pi / 2:
send_waypoint.thrust = 0
publisher_waypoint.publish(send_waypoint)
rate.sleep()
rviz = True
if rviz:
r = 0.05
markerArray = MarkerArray()
for i in range(p.shape[1]):
if i == current_pos_number:
marker = Marker()
marker.header.frame_id = "global_tank"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = r * 2 # r*2 of distance to camera from tag_14
marker.scale.y = r * 2
marker.scale.z = r * 2
marker.color.r = 1
marker.color.a = 1 # transparency
marker.pose.orientation.w = 1.0
marker.pose.position.x = p[1, i] # x
marker.pose.position.y = p[2, i] # y
marker.pose.position.z = wanted_z_position # z
markerArray.markers.append(marker)
else:
marker = Marker()
marker.header.frame_id = "global_tank"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = r # r*2 of distance to camera from tag_14
marker.scale.y = r
marker.scale.z = r
marker.color.g = 1
marker.color.a = 1 # transparency
marker.pose.orientation.w = 1.0
marker.pose.position.x = p[1, i] # x
marker.pose.position.y = p[2, i] # y
marker.pose.position.z = wanted_z_position # z
markerArray.markers.append(marker)
publisher_marker.publish(markerArray)
def main(args):
global list_current_position, list_current_velocity, current_position, usingvelocitycontrol, usingpositioncontrol, xvel, yvel, state, cur_pose, list_error, target
global global_obs_detected, list_velocity_angle, vController
par.CurrentIteration = 1
if (len(args) > 1):
uav_ID = str(args[1])
else:
uav_ID = "1"
idx = int(uav_ID) - 1
rospy.init_node("control_uav_" + uav_ID)
print "UAV" + uav_ID
vController = VelocityController()
try:
rospy.wait_for_service("/uav" + uav_ID + "/mavros/cmd/arming")
rospy.wait_for_service("/uav" + uav_ID + "/mavros/set_mode")
except rospy.ROSException:
fail("failed to connect to service")
state_sub = rospy.Subscriber("/uav" + uav_ID + "/mavros/state",
State,
queue_size=10,
callback=state_cb)
local_vel_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=10)
local_pos_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
local_pos_target = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/local",
PositionTarget,
queue_size=10)
atittude_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/attitude",
AttitudeTarget,
queue_size=10)
thr_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_attitude/att_throttle",
Float64,
queue_size=10)
Astar_grid_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_grid",
GridCells,
queue_size=10)
Astar_path_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_path",
GridCells,
queue_size=10)
arming_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/cmd/arming",
CommandBool)
set_mode_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/set_mode",
SetMode)
for i in range(4):
#velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i)
position_sub = rospy.Subscriber("/uav" + repr(i + 1) +
"/mavros/local_position/pose",
PoseStamped,
queue_size=10,
callback=position_cb,
callback_args=(i, int(uav_ID) - 1))
scan_lidar_sub = rospy.Subscriber("/scan",
LaserScan,
queue_size=10,
callback=scan_lidar_cb,
callback_args=uav_ID)
br = tf.TransformBroadcaster()
r = rospy.Rate(10)
print "TRY TO CONNECT"
while ((not rospy.is_shutdown()) and (not current_state.connected)):
#rospy.spinOnce()
r.sleep()
#print(current_state.connected.__class__)
rospy.loginfo("CURRENT STATE CONNECTED")
poses = PoseStamped()
#poses.pose = Pose()
#poses.pose.position = Point()
target = Pose()
if (idx == 0):
target.position.x = 0
target.position.y = 0
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 0
target.position.y = 0
if (idx == 3):
target.position.x = 4
target.position.y = 4
target.position.z = 10
poses.pose.position.x = target.position.x
poses.pose.position.y = target.position.y
poses.pose.position.z = target.position.z
q = quaternion_from_euler(0, 0, 45 * np.pi / 180.0)
poses.pose.orientation.x = q[0]
poses.pose.orientation.y = q[1]
poses.pose.orientation.z = q[2]
poses.pose.orientation.w = q[3]
i = 100
#while((not rospy.is_shutdown()) and (i>0)):
# local_pos_pub.publish(poses)
# i = i-1
rviz_visualization_start = False
last_request = rospy.Time.now()
count = 0
if (idx == 1):
target.position.x = 28
target.position.y = 28
if (idx == 2):
target.position.x = 0
target.position.y = 17
#thread1 = Thread(target = key_function)
#thread1.start()
style.use('fivethirtyeight')
thread2 = Thread(target=plot_error_live)
thread2.start()
thread3 = Thread(target=calculate_and_publish_wall,
args=(Astar_grid_pub, Astar_path_pub, idx))
thread3.start()
#file_error_pos_x = open(dir_path+'/txt/error_pos_x.txt','w')
#file_error_pos_y = open(dir_path+'/txt/error_pos_y.txt','w')
#file_error_pos_z = open(dir_path+'/txt/error_pos_z.txt','w')
error_time = 0
print poses
uav_color = [255, 0, 0, 255]
topic = 'uav_marker_' + uav_ID
uav_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_marker = Marker()
uav_marker.header.frame_id = "world"
uav_marker.type = uav_marker.SPHERE
uav_marker.action = uav_marker.ADD
uav_marker.scale.x = par.ws_model['robot_radius'] * 3
uav_marker.scale.y = par.ws_model['robot_radius'] * 3
uav_marker.scale.z = 0.005 * par.RealScale
uav_marker.color.r = float(uav_color[0]) / 255
uav_marker.color.g = float(uav_color[1]) / 255
uav_marker.color.b = float(uav_color[2]) / 255
uav_marker.color.a = float(uav_color[3]) / 255
uav_marker.pose.orientation.w = 1.0
uav_marker.pose.position.x = 0 #init_pos[0]
uav_marker.pose.position.y = 0 #init_pos[1]
uav_marker.pose.position.z = 0 #init_pos[2]
uav_marker_publisher.publish(uav_marker)
uav_color = [255, 255, 255, 255]
topic = 'uav_goal_marker_' + uav_ID
uav_goal_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_goal_marker = Marker()
uav_goal_marker.header.frame_id = "world"
uav_goal_marker.type = uav_goal_marker.SPHERE
uav_goal_marker.action = uav_goal_marker.ADD
uav_goal_marker.scale.x = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.y = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.z = 0.005 * par.RealScale
uav_goal_marker.color.r = float(uav_color[0]) / 255
uav_goal_marker.color.g = float(uav_color[1]) / 255
uav_goal_marker.color.b = float(uav_color[2]) / 255
uav_goal_marker.color.a = float(uav_color[3]) / 255
uav_goal_marker.pose.orientation.w = 1.0
uav_goal_marker.pose.position.x = 0 #init_pos[0]
uav_goal_marker.pose.position.y = 0 #init_pos[1]
uav_goal_marker.pose.position.z = 0 #init_pos[2]
uav_goal_marker_publisher.publish(uav_goal_marker)
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
last_request_rviz = rospy.Time.now()
last_HRVO_request = rospy.Time.now()
while (not rospy.is_shutdown()):
if (rviz_visualization_start and
(rospy.Time.now() - last_request_rviz > rospy.Duration(0.2))):
publish_uav_position_rviz(
br, list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, uav_ID)
uav_marker = update_uav_marker(
uav_marker, (list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, 1.0))
uav_marker_publisher.publish(uav_marker)
last_request_rviz = rospy.Time.now()
if ((not rviz_visualization_start)
and (current_state.mode != 'OFFBOARD')
and (rospy.Time.now() - last_request > rospy.Duration(1.0))):
offb_set_mode = set_mode_client(0, 'OFFBOARD')
if (offb_set_mode.mode_sent):
rospy.loginfo("OFFBOARD ENABLED")
last_request = rospy.Time.now()
else:
if ((not current_state.armed) and
(rospy.Time.now() - last_request > rospy.Duration(1.0))):
arm_cmd = arming_client(True)
if (arm_cmd.success):
rospy.loginfo("VEHICLE ARMED")
rviz_visualization_start = True
last_request = rospy.Time.now()
#print distance3D(cur_pose.pose,target)
if (distance3D(cur_pose.pose, target) <
0.5) and (not usingvelocitycontrol):
print "sampai"
usingvelocitycontrol = True
usingpositioncontrol = False
target = Pose()
if (idx == 0):
target.position.x = 28
target.position.y = 28
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 21
target.position.y = 14
if (idx == 3):
target.position.x = 0
target.position.y = 0
target.position.z = 10
print target
psi_target = 45 * np.pi / 180.0 #np.pi/180.0 #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
diagram4 = GridWithWeights(par.NumberOfPoints, par.NumberOfPoints)
diagram4.walls = []
for i in range(par.NumberOfPoints
): # berikan wall pada algoritma pathfinding A*
for j in range(par.NumberOfPoints):
if (par.ObstacleRegion[j, i] == 0):
diagram4.walls.append((i, j))
for i in range(par.NumberOfPoints):
for j in range(par.NumberOfPoints):
if (par.ObstacleRegionWithClearence[j][i] == 0):
diagram4.weights.update({(i, j): par.UniversalCost})
goal_pos = (target.position.x / par.RealScale,
target.position.y / par.RealScale)
start_pos = (cur_pose.pose.position.x / par.RealScale,
cur_pose.pose.position.y / par.RealScale)
pathx, pathy = Astar_version1(par, start_pos, goal_pos, diagram4)
print pathx
print pathy
vController.setHeadingTarget(deg2rad(90.0)) #45.0))
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
vController.setTarget(target)
vController.setPIDGainX(1, 0.0, 0.0)
vController.setPIDGainY(1, 0.0, 0.0)
vController.setPIDGainZ(1, 0, 0)
vController.setPIDGainPHI(1, 0.0, 0.0)
vController.setPIDGainTHETA(1, 0.0, 0.0)
vController.setPIDGainPSI(1, 0.0, 0.0)
if (usingpositioncontrol):
print "kontrol posisi"
poses.pose.position.x = 29
poses.pose.position.y = 29
poses.pose.position.z = 10
local_pos_pub.publish(poses)
elif (usingvelocitycontrol):
if (distance2D(cur_pose.pose, target) < 1.5):
if (len(pathx) > 1):
del pathx[0]
del pathy[0]
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker, (target.position.x, target.position.y,
target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
print target
vController.setTarget(target)
if (rospy.Time.now() - last_HRVO_request > rospy.Duration(0.05)):
last_HRVO_request = rospy.Time.now()
header.stamp = rospy.Time.now()
des_vel = vController.update(cur_pose)
current_agent_pose = (
list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z)
current_agent_vel = (list_current_velocity[idx].twist.linear.x,
list_current_velocity[idx].twist.linear.y)
current_neighbor_pose = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_pose.append(
(list_current_position[i].pose.position.x,
list_current_position[i].pose.position.y))
current_neighbor_vel = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_vel.append(
(list_current_velocity[i].twist.linear.x,
list_current_velocity[i].twist.linear.y))
V_des = (des_vel.twist.linear.x, des_vel.twist.linear.y)
quat_arr = np.array([
list_current_position[idx].pose.orientation.x,
list_current_position[idx].pose.orientation.y,
list_current_position[idx].pose.orientation.z,
list_current_position[idx].pose.orientation.w
])
att = euler_from_quaternion(quat_arr, 'sxyz')
#V = locplan.RVO_update_single(current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par,list_velocity_angle,list_distance_obs,att[2])
V, RVO_BA_all = locplan.RVO_update_single_static(
current_agent_pose, current_neighbor_pose,
current_agent_vel, current_neighbor_vel, V_des, par)
V_msg = des_vel
V_msg.twist.linear.x = V[0]
V_msg.twist.linear.y = V[1]
local_vel_pub.publish(V_msg)
goal = (target.position.x, target.position.y,
target.position.z)
x = current_position
list_error[0].append(error_time)
list_error[1].append(goal[0] - x[0])
list_error[2].append(goal[1] - x[1])
list_error[3].append(goal[2] - x[2])
error_time = error_time + 1
k = 0.1
vx = k * (goal[0] - x[0])
vy = k * (goal[1] - x[1])
vz = k * (goal[2] - x[2])
postar = PositionTarget()
postar.header = header
postar.coordinate_frame = PositionTarget().FRAME_LOCAL_NED
p = PositionTarget().IGNORE_PX | PositionTarget(
).IGNORE_PY | PositionTarget().IGNORE_PZ
a = PositionTarget().IGNORE_AFX | PositionTarget(
).IGNORE_AFY | PositionTarget().IGNORE_AFZ
v = PositionTarget().IGNORE_VX | PositionTarget(
).IGNORE_VY | PositionTarget().IGNORE_VZ
y = PositionTarget().IGNORE_YAW | PositionTarget(
).IGNORE_YAW_RATE
f = PositionTarget().FORCE
postar.type_mask = a | y | p | f #| PositionTarget().IGNORE_YAW | PositionTarget().IGNORE_YAW_RATE | PositionTarget().FORCE
postar.velocity.x = vx
postar.velocity.y = vy
postar.velocity.z = vz
postar.position.x = goal[0]
postar.position.y = goal[1]
postar.position.z = goal[2]
vel_msg = TwistStamped()
vel_msg.header = header
vel_msg.twist.linear.x = xvel
vel_msg.twist.linear.y = yvel
vel_msg.twist.linear.z = 0.0
vel_msg.twist.angular.x = 0.0
vel_msg.twist.angular.y = 0.0
vel_msg.twist.angular.z = 0.0
q = quaternion_from_euler(0, 0, 0.2)
att = PoseStamped()
att.pose.orientation.x = q[0]
att.pose.orientation.y = q[1]
att.pose.orientation.z = q[2]
att.pose.orientation.w = q[3]
att.pose.position.x = 0.0
att.pose.position.y = 0.0
att.pose.position.z = 2.0
cmd_thr = Float64()
cmd_thr.data = 0.3
att_raw = AttitudeTarget()
att_raw.header = Header()
att_raw.body_rate = Vector3()
att_raw.thrust = 0.7 #Float64()
att_raw.header.stamp = rospy.Time.now()
att_raw.header.frame_id = "fcu"
att_raw.type_mask = 71
att_raw.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0.2))
else:
local_pos_pub.publish(poses)
count = count + 1
r.sleep()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
def command_callback(self, odom):
"""send command to px4"""
X = ar([[odom.pose.pose.position.x], [odom.pose.pose.position.y], [odom.pose.pose.position.z]])
q = Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,\
odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
R = q.rotation_matrix
# ensure that the linear velocity is in inertial frame, ETHZ code multiply linear vel to rotation matrix
V = ar([[odom.twist.twist.linear.x], [odom.twist.twist.linear.y], [odom.twist.twist.linear.z]])
# CROSSCHECK AND MAKE SURE THAT THE ANGULAR VELOCITY IS IN INERTIAL FRAME...HOW?
Omega = self.angular_velocity
#Omega = ar([[odom.twist.twist.angular.x], [odom.twist.twist.angular.y], [odom.twist.twist.angular.z]])
#Omega = np.dot(R.T, Omega) # this is needed because "vicon" package from Upenn publishes spatial angular velocities
# Xd = desired position, Vd = desired velocity, ad = desired acceleration b1d: desired heading direction
Xd = self.pdes; Vd = self.vdes; ad = self.ades; b1d = self.ddes
ex = ar([[0],[0],[0]]) if traj.controller == 1 else X-Xd
ev = V-Vd
_b3c = -(mult(self.kx, ex) + mult(self.kv, ev)) + self.g*self.e[:,2][np.newaxis].T + ad # desired direction
b3c = ar(_b3c/np.linalg.norm(_b3c)) # get a normalized vector
self.f = self.m* np.dot(_b3c.T, tp(R[:,2][np.newaxis]))/self.max_possible_thrust # normalized thrust
yaw = np.arctan2(b2c[1][0], b2c[0][0])
yaw = yaw + 2 * np.pi if yaw < 0 else yaw # vary yaw between 0 and 360 degree
# find rate of change of yaw and force vector
if self.counter != 0:
yaw_dot = (yaw-self.yaw_)/self.dt; self.yaw_ = yaw
_b3c_dot = (_b3c-self._b3c_previous)/self.dt; self._b3c_previous = _b3c
else:
yaw_dot = 0; self.yaw_ = yaw
_b3c_dot = ar([[0], [0], [0]]); self._b3c_previous = _b3c
b1d = ar([[np.cos(yaw)], [np.sin(yaw)], [0]])
b1d_dot = ar([[-np.sin(yaw)*yaw_dot], [np.cos(yaw)*yaw_dot], [0]])
b2c = tp(np.cross(b3c.T,b1d.T)/np.linalg.norm(np.cross(b3c.T, b1d.T))) # vector b2d
b1c = tp(np.cross(b2c.T, b3c.T))
Rc = np.column_stack((b1c, b2c, b3c)) # desired rotation matrix
# take the derivatives
middle1 = _b3c_dot/np.linalg.norm(_b3c)
last_two = np.cross(middle1.T, b3c.T)
b3c_dot = np.cross(b3c.T, last_two)
middle2 = (np.cross(b3c_dot, b1d.T) + np.cross(b3c.T, b1d_dot.T))/np.linalg.norm(np.cross(b3c.T, b1d.T))
last_two = np.cross(middle2, b2c.T)
b2c_dot = np.cross(b2c.T, last_two)
b1c_dot = np.cross(b2c_dot, b3c.T) + np.cross(b2c.T, b3c_dot)
Rc_dot = np.column_stack((b1c_dot.T, b2c_dot.T, b3c_dot.T)) # desired rotation matrix
Omega_des = np.dot(Rc.T, Rc_dot)
"""
qc = self.rotmat2quat(Rc)
omega_des = q.inverse*qc
phi = np.arctan2(b2c[1][0], b2c[0][0])
if phi < 0:
phi = phi + 2 * np.pi
#error = 0.5 * np.trace(self.e-np.dot(Rc.T, R))
if self.counter != 0:
phi_dot = (phi-self.phi_)/self.dt; self.phi_ = phi
else:
phi_dot = 0; self.phi_ = phi
Omega_c = ar([[0], [0], [phi_dot]])
"""
#eR_hat = 0.5*(np.dot(Rc.T, R) - np.dot(R.T, Rc)) # eR skew symmetric matrix
#
#eR = ar([[-eR_hat[1][2]], [eR_hat[0][2]], [-eR_hat[0][1]]]) # vector that gives error in rotation
#eOmega = Omega - np.dot(np.dot(R.T, Rc), Omega_c) # vector that gives error in angular velocity
#self.M = -(mult(self.kR, eR) + mult(self.kOmega, eOmega)) + tp(np.cross(Omega.T, tp(np.dot(self.J,Omega))))
msg = AttitudeTarget()
msg.header.stamp = odom.header.stamp
msg.type_mask = 128
msg.body_rate.x = Omega_des[1][2]
msg.body_rate.y = -Omega_des[0][2]
msg.body_rate.z = -Omega_des[0][1]
#msg.body_rate.x = self.factor*np.sign(omega_des[0])*omega_des[1]
#msg.body_rate.y = -self.factor*np.sign(omega_des[0])*omega_des[2]
#msg.body_rate.z = -self.factor*np.sign(omega_des[0])*omega_des[3]
msg.thrust = min(1.0, self.f[0][0])
print 'thrust', min(1.0, self.f[0][0])
print 'omega_des',msg.body_rate.x, msg.body_rate.y, msg.body_rate.z
print 'zheight', traj.desired_position.z
#f0 = open('commands.txt', 'a')
#f0.write("%s, %s, %s, %s\n" % (msg.thrust, msg.body_rate.x, msg.body_rate.y, msg.body_rate.z))
self.counter = self.counter+1
self.pub.publish(msg)
