Skip to content

Instantly share code, notes, and snippets.

@flixr
Created May 5, 2015 16:34
Show Gist options
  • Save flixr/107f31b2a973e17e8728 to your computer and use it in GitHub Desktop.
Save flixr/107f31b2a973e17e8728 to your computer and use it in GitHub Desktop.
IPython notebook v4: Paparazzi RotorcaftMixing example
Display the source blob
Display the rendered blob
Raw
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"import numpy as np"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"# The first two lines of the A matrix represent the coordinates of each rotor in the X,Y plane,\n",
"# and the third line the direction in which they spin.\n",
"# Note that in this example the X axis is vertical, and the Y coordinates are in the top row of A.\n",
"A = np.array([[-0.17, 0.17, -0.25, 0.25, -0.33, 0.33],\n",
" [-0.35, -0.35, 0., 0., 0.35, 0.35],\n",
" [-0.1, 0.1, 0.1, -0.1, -0.1, 0.1 ]])"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"collapsed": false
},
"outputs": [],
"source": [
"# Moore-Penrose pseudoinverse of A\n",
"B = np.linalg.pinv(A)\n",
"# normalize roll/pitch to the largest of both\n",
"# normalize yaw to 0.5\n",
"# and transpose\n",
"rp_max = B[:,0:1].max()\n",
"n = np.array([rp_max, rp_max, 2*B[:,2].max()])\n",
"B_nt = (B / n).T"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[[ -67 67 -256 256 -189 189]\n",
" [-197 -197 0 0 197 197]\n",
" [ -77 77 128 -128 -57 57]]\n"
]
}
],
"source": [
"# scale and round to 256 to return final coefficients\n",
"scale = 256\n",
"coeffs = np.around(scale * B_nt).astype(int)\n",
"print(coeffs)"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"<define name=\"ROLL_COEF\" value=\"{ -67, 67, -256, 256, -189, 189}\"/>\n",
"<define name=\"PITCH_COEF\" value=\"{-197, -197, 0, 0, 197, 197}\"/>\n",
"<define name=\"YAW_COEF\" value=\"{ -77, 77, 128, -128, -57, 57}\"/>\n"
]
}
],
"source": [
"# output defines\n",
"import string\n",
"rows = ['ROLL_COEF\" ', 'PITCH_COEF\" ', 'YAW_COEF\" ']\n",
"for i, r in enumerate(rows):\n",
" print('<define name=\"' + r + 'value=\"{' + string.join(['{:>4d}'.format(c) for c in coeffs[i]], ', ') + '}\"/>')"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"collapsed": true
},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 2",
"language": "python",
"name": "python2"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment