def position_and_velocity_from_odometry(odometry):
x = np.array([odometry.pose.pose.position.x,\
odometry.pose.pose.position.y,\
odometry.pose.pose.position.z])
# TODO: naming of child_frame_id
if odometry.child_frame_id == 'firefly/base_link':
# velocity is in the body reference frame
v_body = np.array([odometry.twist.twist.linear.x,\
odometry.twist.twist.linear.y,\
odometry.twist.twist.linear.z])
quaternion = np.array([odometry.pose.pose.orientation.x,\
odometry.pose.pose.orientation.y,\
odometry.pose.pose.orientation.z,\
odometry.pose.pose.orientation.w])
# TODO
rotation_matrix = utility_functions.rot_from_quaternion(quaternion)
v = np.dot(rotation_matrix,v_body)
else:
# velocity is in the body reference frame
v = np.array([odometry.twist.twist.linear.x,\
odometry.twist.twist.linear.y,\
odometry.twist.twist.linear.z])
return x,v
def get_quad_state(self,data_odometry): #---------------------------------------------------------------# quaternion_quad = np.array([data_odometry.pose.pose.orientation.x,\ data_odometry.pose.pose.orientation.y,\ data_odometry.pose.pose.orientation.z,\ data_odometry.pose.pose.orientation.w]) # attitude: euler in DEGREES # ee = np.array([roll,pitch,yaw]) rotation_matrix = uts.rot_from_quaternion(quaternion_quad) ee = uts.euler_deg_from_rot(rotation_matrix) # omega_body = np.array([data_odometry.twist.twist.angular.x,\ # data_odometry.twist.twist.angular.y,\ # data_odometry.twist.twist.angular.z]) #---------------------------------------------------------------# p = np.array([data_odometry.pose.pose.position.x,\ data_odometry.pose.pose.position.y,\ data_odometry.pose.pose.position.z]) # velocity is in the body reference frame v_body = np.array([data_odometry.twist.twist.linear.x,\ data_odometry.twist.twist.linear.y,\ data_odometry.twist.twist.linear.z]) v_inertial = np.dot(rotation_matrix,v_body) # current_time = data_odometry.header.stamp.secs + data_odometry.header.stamp.nsecs/1e9 # print current_time # print self.QuadVelocityEstimator.out(position_quad,current_time) # print velocity_quad #---------------------------------------------------------------# # collect all components of state return np.concatenate([p,v_inertial,ee])
def rotor_s_attitude_for_control(self, data_odometry):
#---------------------------------------------------------------#
# rotation matrix
quaternion_quad = np.array([data_odometry.pose.pose.orientation.x,\
data_odometry.pose.pose.orientation.y,\
data_odometry.pose.pose.orientation.z,\
data_odometry.pose.pose.orientation.w])
rotation_matrix = uts.rot_from_quaternion(quaternion_quad)
#---------------------------------------------------------------#
# angular velocity in body reference frame
omega_body = np.array([data_odometry.twist.twist.angular.x,\
data_odometry.twist.twist.angular.y,\
data_odometry.twist.twist.angular.z])
#---------------------------------------------------------------#
# saving as states
self.rotation_matrix = rotation_matrix
self.omega_body = omega_body
def rotor_s_attitude_for_control(self,data_odometry): #---------------------------------------------------------------# # rotation matrix quaternion_quad = np.array([data_odometry.pose.pose.orientation.x,\ data_odometry.pose.pose.orientation.y,\ data_odometry.pose.pose.orientation.z,\ data_odometry.pose.pose.orientation.w]) rotation_matrix = uts.rot_from_quaternion(quaternion_quad) #---------------------------------------------------------------# # angular velocity in body reference frame omega_body = np.array([data_odometry.twist.twist.angular.x,\ data_odometry.twist.twist.angular.y,\ data_odometry.twist.twist.angular.z]) #---------------------------------------------------------------# # saving as states self.rotation_matrix = rotation_matrix self.omega_body = omega_body
def get_quad_state(self, data_odometry):
#---------------------------------------------------------------#
quaternion_quad = np.array([data_odometry.pose.pose.orientation.x,\
data_odometry.pose.pose.orientation.y,\
data_odometry.pose.pose.orientation.z,\
data_odometry.pose.pose.orientation.w])
# attitude: euler in DEGREES
# ee = np.array([roll,pitch,yaw])
rotation_matrix = uts.rot_from_quaternion(quaternion_quad)
ee = uts.euler_deg_from_rot(rotation_matrix)
# omega_body = np.array([data_odometry.twist.twist.angular.x,\
# data_odometry.twist.twist.angular.y,\
# data_odometry.twist.twist.angular.z])
#---------------------------------------------------------------#
p = np.array([data_odometry.pose.pose.position.x,\
data_odometry.pose.pose.position.y,\
data_odometry.pose.pose.position.z])
# velocity is in the body reference frame
v_body = np.array([data_odometry.twist.twist.linear.x,\
data_odometry.twist.twist.linear.y,\
data_odometry.twist.twist.linear.z])
v_inertial = np.dot(rotation_matrix, v_body)
# current_time = data_odometry.header.stamp.secs + data_odometry.header.stamp.nsecs/1e9
# print current_time
# print self.QuadVelocityEstimator.out(position_quad,current_time)
# print velocity_quad
#---------------------------------------------------------------#
# collect all components of state
return np.concatenate([p, v_inertial, ee])