Skip to content

Instantly share code, notes, and snippets.

@cdsousa
Last active August 29, 2015 14:02
Show Gist options
  • Save cdsousa/f760ef46d73980a19d18 to your computer and use it in GitHub Desktop.
Save cdsousa/f760ef46d73980a19d18 to your computer and use it in GitHub Desktop.
SymPyBotics - Coriolis matrix
Display the source blob
Display the rendered blob
Raw
{
"metadata": {
"name": "",
"signature": "sha256:88622d1d8a89808cba9b9b0be646204c86509a691507964b7c2e19cd428bc075"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "heading",
"level": 1,
"metadata": {},
"source": [
"SymPyBotics - Coriolis matrix"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"--------"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import sympy\n",
"import numpy\n",
"import sympybotics\n",
"\n",
"rbtdef = sympybotics.RobotDef('Example Robot', # robot name\n",
" [('-pi/2', 0, 0, 'q+pi/2'), # list of tuples with Denavit-Hartenberg parameters\n",
" ( 'pi/2', 0, 0, 'q-pi/2')], # (alpha, a, d, theta)\n",
" dh_convention='standard' # either 'standard' or 'modified'\n",
" )\n",
"rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'\n",
"\n",
"rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating geometric model\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating kinematic model\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating inverse dynamics code\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating gravity term code\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating coriolis term code\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating coriolis matrix code\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating inertia matrix code\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating regressor matrix code\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"generating friction term code\n"
]
},
{
"output_type": "stream",
"stream": "stdout",
"text": [
"done\n"
]
}
],
"prompt_number": 1
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from numpy import cos, sin, random\n",
"rand_dynp = random.rand(rbt.dyn.n_dynparms)\n",
"rand_q = random.rand(rbt.dof)\n",
"rand_dq = random.rand(rbt.dof)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 2
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Coriolis and centripetal forces vector $c(q, \\dot{q})$"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"exec(sympybotics.robotcodegen.robot_code_to_func('python', rbt.c_code, 'c', 'coriolis_vector', rbtdef))"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 3
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"c = coriolis_vector(rand_dynp, rand_q, rand_dq)\n",
"c = numpy.matrix(c).T\n",
"c"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 4,
"text": [
"matrix([[ 0.06027864],\n",
" [-0.01747512]])"
]
}
],
"prompt_number": 4
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Coriolis and centripetal forces matrix $C(q, \\dot{q})$"
]
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"$C(q, \\dot{q})\\,\\dot{q} = c(q, \\dot{q})$"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"exec(sympybotics.robotcodegen.robot_code_to_func('python', rbt.C_code, 'C', 'coriolis_matrix', rbtdef))"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 5
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"C = coriolis_matrix(rand_dynp, rand_q, rand_dq)\n",
"C = numpy.matrix(C).reshape((rbt.dof, rbt.dof))\n",
"C"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 6,
"text": [
"matrix([[ 0. , 0.19423486],\n",
" [-0.07206067, 0. ]])"
]
}
],
"prompt_number": 6
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"C * numpy.matrix(rand_dq).T"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 7,
"text": [
"matrix([[ 0.06027864],\n",
" [-0.01747512]])"
]
}
],
"prompt_number": 7
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"c - C * numpy.matrix(rand_dq).T"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 8,
"text": [
"matrix([[ 6.93889390e-18],\n",
" [ -3.46944695e-18]])"
]
}
],
"prompt_number": 8
}
],
"metadata": {}
}
]
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment