def write_quad_state_msg(self, pos, rot, vel): """Converts the local state of the quad into a 'QuadPositionDerived' message from the mocap package. Warning. 'QuadPositionDerived' was maybe a poorly chosen name, since that type of message contains: position as x,y,z attitude as euler angles in degrees velocity angular velocity acceleration angular acceleration However, I decided to stick with the original mocap settings inherited by Matteo, so I did not change the name or format of that message. Some field of the message will be left blank, since our simulators only use position, attitude and velocity. It does not matter what will be put in those fileds, since we will not use them. """ msg = mocap.msg.QuadPositionDerived() msg.found_body = True msg.x, msg.y, msg.z = pos msg.roll, msg.pitch, msg.yaw = uf.rot_to_ea_deg(rot) msg.x_vel, msg.y_vel, msg.z_vel = vel return msg
def print_time_state(self): print 'time: ' + str(self._time) print 'pos: ' + str(tuple(self._pos)) print 'vel: ' + str(tuple(self._vel)) print 'roll, pitch, yaw: ' + str(tuple(uf.rot_to_ea_deg(self._rot)))