def main(nh, point, direction, offset = 0.0, flip = False): boat = yield boat_scripting.get_boat(nh) print 'Direction:', direction * 180 / np.pi yaw = direction[2] print 'Yaw:', yaw * 180 / np.pi print 'dx:', np.cos(yaw) * offset print 'dy:', np.sin(yaw) * offset print 'Point:', point point = point + [np.cos(yaw) * offset, np.sin(yaw) * offset, 0] if flip: direction[2] = (direction[2] + np.pi) % (2*np.pi) yield boat.move.set_position(point).set_orientation(rotvec_to_quat(direction)).go()
def quaternion_from_rotvec(rot): return quaternion_from_xyzw_array(rotvec_to_quat(rot))