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)