Skip to content

Instantly share code, notes, and snippets.

@domitry

domitry/hoge.ipynb

Created December 23, 2014 16:07
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save domitry/6ffaf80167b6035ca6af to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save domitry/6ffaf80167b6035ca6af to your computer and use it in GitHub Desktop.
Download ZIP
Display the source blob
Display the rendered blob
Raw
hoge.ipynb
{
"metadata": {
"name": "",
"signature": "sha256:64086f894a26edf85fda5efd069bebd27be567b124df07d000502ddf3fcce436"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"import link7\n",
"import numpy as np\n",
"import math"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 1
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from link7 import expected_dq_dt, expected_q"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 2
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"link7.init_ipynb()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"javascript": [
"if(window['d3'] === undefined){\n",
" require.config({paths: {\n",
"\t d3: 'http://cdnjs.cloudflare.com/ajax/libs/d3/3.4.4/d3.min'\n",
" }});\n",
"}\n"
],
"metadata": {},
"output_type": "display_data",
"text": [
"<IPython.core.display.Javascript at 0xb6f8fa4c>"
]
}
],
"prompt_number": 3
},
{
"cell_type": "heading",
"level": 1,
"metadata": {},
"source": [
"FixedJointsSimulator"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"init = np.array([4.8, 1.5, 1.040, -0.01, -0.889, 0.419])"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 4
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sim = link7.FixedJointsSimulator(initial_value=init, duration=30, monitor_height=50)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"javascript": [
"define(\"Viewer\", [\"d3\", \"underscore\"], function(d3, _){\n",
" var draw_monitor = function(g, val, options){\n",
" var data = g.select(\"path\").datum();\n",
"\n",
" //data.shift();\n",
" data.push(val);\n",
"\n",
" var scales = {\n",
" x: d3.scale.linear().domain([0, data.length]).range([50, options.width]),\n",
" y: d3.scale.linear().domain([_.min(data), _.max(data)]).range([options.monitor_height-5, 0])\n",
" };\n",
"\n",
" g.select(\"path\")\n",
" .datum(data)\n",
" .attr(\"d\", d3.svg.line()\n",
" .x(function(d, i){return scales.x(i);})\n",
" .y(function(d, i){return scales.y(d);})\n",
" );\n",
" g.select(\".g_axis\")\n",
" .call(d3.svg.axis().scale(scales.y).orient(\"left\").ticks(4).innerTickSize(-options.width+50).outerTickSize(0));\n",
" g.selectAll(\"path, line\").attr({\n",
" fill: 'none',\n",
" 'shape-rendering': 'crispEdges'\n",
" });\n",
" g.selectAll(\"text\").attr({\n",
" 'font-family': 'sans-serif',\n",
" 'font-size': 11\n",
" });\n",
" g.selectAll(\".tick line\").attr({\n",
" stroke: 'lightgrey',\n",
" 'stroke-width': 1,\n",
" opacity: 0.7\n",
" });\n",
" };\n",
"\n",
" // q: [theta, x, y, zeta1, zeta2, zeta3, zeta4, zeta5, zeta6]\n",
" var draw_viewer = function(svg, q){\n",
" var L1=0.122, L2=0.379, L3=0.420, L4=0.536, l4=0.332, l0=0.079;\n",
" var sin = Math.sin, cos = Math.cos, PI = Math.PI;\n",
" var height = 300, rate=150; // pixel\n",
" var s2px = function(x_val){return 200 + x_val*rate;}, //sim2pixel\n",
" s2py = function(y_val){return height - y_val*rate;};\n",
"\n",
" // HAT\n",
" var theta = q[0], x = q[1], y = q[2];\n",
"\n",
" var x4 = x - l4*cos(theta),\n",
" y4 = y - l4*sin(theta);\n",
"\n",
" var x3 = x + (L4-l4)*cos(theta),\n",
" y3 = y + (L4-l4)*sin(theta);\n",
"\n",
" svg.select(\".GND\").attr({\n",
" x1: 0,\n",
" y1: s2py(0),\n",
" x2: 1000,\n",
" y2: s2py(0)\n",
" });\n",
"\n",
" svg.select(\".L4\")\n",
" .attr({\n",
" x1: s2px(x4),\n",
" y1: s2py(y4),\n",
" x2: s2px(x3),\n",
" y2: s2py(y3)\n",
" });\n",
"\n",
" [['R', q.slice(2, 6)], ['L', q.slice(5, 9)]].forEach(function(arr){\n",
" var suffix = arr[0],\n",
" zeta = arr[1]; // R: q[3]->zeta[1], q[4]->zeta[2], q[5]->zeta[3]\n",
"\n",
" var x2 = x3 + L3*sin(zeta[3]),\n",
" y2 = y3 - L3*cos(zeta[3]);\n",
"\n",
" svg.select(\".L3\" + suffix)\n",
" .attr({\n",
" x1: s2px(x3),\n",
" y1: s2py(y3),\n",
" x2: s2px(x2),\n",
" y2: s2py(y2)\n",
" });\n",
"\n",
" var x1 = x2 + L2*sin(zeta[2]+zeta[3]),\n",
" y1 = y2 - L2*cos(zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L2\" + suffix)\n",
" .attr({\n",
" x1: s2px(x2),\n",
" y1: s2py(y2),\n",
" x2: s2px(x1),\n",
" y2: s2py(y1)\n",
" });\n",
"\n",
" var x0t = x1 + L1*sin(zeta[1]+zeta[2]+zeta[3]),\n",
" y0t = y1 - L1*cos(zeta[1]+zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L1\" + suffix)\n",
" .attr({\n",
" x1: s2px(x1),\n",
" y1: s2py(y1),\n",
" x2: s2px(x0t),\n",
" y2: s2py(y0t)\n",
" });\n",
"\n",
" var x0h = x1 + l0*cos(zeta[1]+zeta[2]+zeta[3]),\n",
" y0h = y1 + l0*sin(zeta[1]+zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L0\" + suffix)\n",
" .attr({\n",
" x1: s2px(x1),\n",
" y1: s2py(y1),\n",
" x2: s2px(x0h),\n",
" y2: s2py(y0h)\n",
" });\n",
" });\n",
" };\n",
"\n",
" return{\n",
" init_svg: function(svg, options){\n",
" svg.attr(\"class\", \"viz\").style({\n",
" position: 'relative',\n",
" width: options.width,\n",
" height: options.height\n",
" });\n",
"\n",
" for(var i=0;i<=3;i++){\n",
" [\"L\", \"R\"].forEach(function(lr){\n",
" svg.append(\"line\").attr({\"class\": \"L\" + i + lr});\n",
" });\n",
" }\n",
" svg.append(\"line\").attr({\"class\": \"L4\"});\n",
" svg.append(\"line\").attr({\"class\": \"GND\"});\n",
" svg.selectAll(\"line\").attr({\n",
" 'stroke': \"rgb(0,0,0)\",\n",
" 'stroke-width': 3\n",
" });\n",
"\n",
" _.each(options.monitors, function(name, i){\n",
" var arr = []; //_.map(_.range(options.buff_size*5), function(){return 0;});\n",
" var g = svg.append(\"g\").attr({\n",
" 'class': 'monitor'+i,\n",
" 'transform': \"translate(0,\" + (options.viewer_height + options.monitor_height*i) + \")\"\n",
" });\n",
" g.append(\"path\").attr({\n",
" 'fill': 'none',\n",
" 'stroke': '#000',\n",
" 'stroke-width': 2\n",
" }).datum(arr);\n",
"\n",
" g.append(\"g\").attr(\"class\", \"g_axis\").\n",
" attr(\"transform\", \"translate(50,0)\");\n",
" //text\n",
" g.append(\"text\").attr({\n",
" \"x\": -options.monitor_height/2,\n",
" \"y\": 10,\n",
" \"transform\": \"rotate(-90)\",\n",
" \"text-anchor\": \"middle\"\n",
" }).text(name);\n",
" });\n",
" },\n",
"\n",
" draw: function(svg, q, monitors, options){\n",
" draw_viewer(svg, q);\n",
" _.each(options.monitors, function(name, i){\n",
" var g = svg.select(\".monitor\"+i);\n",
" draw_monitor(g, monitors[i], options);\n",
" });\n",
" }\n",
" };\n",
"});\n",
"\n",
"require([\"d3\", \"jquery\", \"underscore\", \"widgets/js/widget\", \"Viewer\"], function(d3, $, _, WidgetManager, Viewer){\n",
" // Define the DatePickerView\n",
" var SimView = window.IPython.DOMWidgetView.extend({\n",
" render: function(){\n",
" this.$el.css('position', 'relative');\n",
" this.$el.css('width', this.options.width);\n",
" this.$el.css('height', this.options.height);\n",
" },\n",
"\n",
" update: function() {\n",
" return SimView.__super__.update.apply(this);\n",
" },\n",
"\n",
" initialize: function(parameters){\n",
" SimView.__super__.initialize.apply(this, [parameters]);\n",
" this.options = _.extend({\n",
" duration: 10,\n",
" buff_size: 50,\n",
" monitors: 0,\n",
" width: 700,\n",
" viewer_height: 400,\n",
" monitor_height: 200\n",
" }, parameters.model.attributes.options);\n",
"\n",
" this.options.height = (_.bind(function(){\n",
" return this.viewer_height + this.monitor_height * _.keys(this.monitors).length;\n",
" }, this.options))();\n",
"\n",
" //*** init view ***\n",
" this.$el.addClass('widget-hbox');\n",
" var dom = document.createElementNS(\"http://www.w3.org/2000/svg\", \"svg:svg\");\n",
" this.$el.append(dom);\n",
" var svg = d3.select(dom);\n",
" Viewer.init_svg(svg, this.options);\n",
"\n",
" // arr is ret value from p2\n",
" var main = _.bind(function(content){\n",
" var dfd = new $.Deferred();\n",
" var arr = content.new_arr;\n",
" var monitors = (this.options.monitors ? _.zip.apply(null, _.values(content.monitors)) : new Array(this.options.buff_size));\n",
"\n",
" var p2 = (_.bind(function(){\n",
" var deferred = new $.Deferred();\n",
"\n",
" if(arr.length == 0){\n",
" deferred.resolve();\n",
" }else{\n",
" var cnt = 0;\n",
" var loop = _.bind(function(){\n",
" Viewer.draw(svg, arr[cnt], monitors[cnt], this.options);\n",
"\n",
" if(cnt == this.options.buff_size - 1){\n",
" deferred.resolve();\n",
" }else{\n",
" cnt+=1;\n",
" window.setTimeout(loop, this.options.duration);\n",
" }\n",
" }, this);\n",
" loop();\n",
" }\n",
" return deferred.promise();\n",
" }, this))();\n",
"\n",
" var p1 = (_.bind(function(){\n",
" var deferred = new $.Deferred();\n",
"\n",
" this.model.send({\n",
" num: this.options.buff_size\n",
" });\n",
"\n",
" // send custom message through Comm. See the following article to learn more:\n",
" // http://ipython.org/ipython-doc/dev/development/messaging.html\n",
" this.model.on('msg:custom', $.proxy(function(content){\n",
" if(content.error != true)\n",
" deferred.resolve(content);\n",
" else\n",
" deferred.reject();\n",
" }), this);\n",
"\n",
" return deferred.promise();\n",
" }, this))();\n",
"\n",
" $.when(p1, p2).then(\n",
" main,\n",
" function(err1, err2){\n",
" console.log(\"Visualizer stopped.\");\n",
" }\n",
" );\n",
"\n",
" dfd.resolve();\n",
" return dfd.promise();\n",
" }, this);\n",
"\n",
" main({new_arr: [], monitors: {}});\n",
" }\n",
" });\n",
" \n",
" WidgetManager.register_widget_view('SimView', SimView);\n",
"});\n"
],
"metadata": {},
"output_type": "display_data",
"text": [
"<IPython.core.display.Javascript at 0xb6ee278c>"
]
}
],
"prompt_number": 5
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"#sim.add_monitor('F_y_r_t', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[4])\n",
"#sim.add_monitor('F_y_r_h', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[5])\n",
"#sim.add_monitor('F_y_l_t', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[6])\n",
"#sim.add_monitor('F_y_l_h', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[7])\n",
"\n",
"sim.add_monitor('dd_theta_dtdt', lambda q, dq_dt, t: (sim.solver.ddq_dtdt(t, q, dq_dt, np.zeros(9)))[0])\n",
"sim.add_monitor('dd_x_dtdt', lambda q, dq_dt, t: (sim.solver.ddq_dtdt(t, q, dq_dt, np.zeros(9)))[1])\n",
"sim.add_monitor('dd_y_dtdt', lambda q, dq_dt, t: (sim.solver.ddq_dtdt(t, q, dq_dt, np.zeros(9)))[2])"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 6
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sim.run()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"html": [
"<div\"></div>"
],
"metadata": {},
"output_type": "display_data",
"text": [
"<IPython.core.display.HTML at 0xb6ee28ac>"
]
}
],
"prompt_number": 7
},
{
"cell_type": "heading",
"level": 1,
"metadata": {},
"source": [
"InversedDynamicsController"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"q = np.concatenate((np.array([4.45, 1.5, 1.04]), link7.expected_q(0)[3:]))\n",
"dq_dt = np.concatenate((np.array([0.165, -1.189, 0.119]), link7.expected_dq_dt(0)[3:]))\n",
"init = np.concatenate((q, dq_dt))\n",
"print(init)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"[ 4.45 1.5 1.04 -1.04330218 1.15610381 -0.18525225\n",
" -1.25706716 0.34137921 -0.11993426 0.165 -1.189 0.119\n",
" -0.8653484 0.05947736 -2.66787512 -0.03768383 -1.60226281 1.68681628]\n"
]
}
],
"prompt_number": 37
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"controller = link7.InversedDynamicsController()\n",
"#controller = link7.ContinuousPDController()"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 38
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sim = link7.PDSimulator(initial_value=init, controller=controller, duration=10, monitor_height=50)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"javascript": [
"define(\"Viewer\", [\"d3\", \"underscore\"], function(d3, _){\n",
" var draw_monitor = function(g, val, options){\n",
" var data = g.select(\"path\").datum();\n",
"\n",
" //data.shift();\n",
" data.push(val);\n",
"\n",
" var scales = {\n",
" x: d3.scale.linear().domain([0, data.length]).range([50, options.width]),\n",
" y: d3.scale.linear().domain([_.min(data), _.max(data)]).range([options.monitor_height-5, 0])\n",
" };\n",
"\n",
" g.select(\"path\")\n",
" .datum(data)\n",
" .attr(\"d\", d3.svg.line()\n",
" .x(function(d, i){return scales.x(i);})\n",
" .y(function(d, i){return scales.y(d);})\n",
" );\n",
" g.select(\".g_axis\")\n",
" .call(d3.svg.axis().scale(scales.y).orient(\"left\").ticks(4).innerTickSize(-options.width+50).outerTickSize(0));\n",
" g.selectAll(\"path, line\").attr({\n",
" fill: 'none',\n",
" 'shape-rendering': 'crispEdges'\n",
" });\n",
" g.selectAll(\"text\").attr({\n",
" 'font-family': 'sans-serif',\n",
" 'font-size': 11\n",
" });\n",
" g.selectAll(\".tick line\").attr({\n",
" stroke: 'lightgrey',\n",
" 'stroke-width': 1,\n",
" opacity: 0.7\n",
" });\n",
" };\n",
"\n",
" // q: [theta, x, y, zeta1, zeta2, zeta3, zeta4, zeta5, zeta6]\n",
" var draw_viewer = function(svg, q){\n",
" var L1=0.122, L2=0.379, L3=0.420, L4=0.536, l4=0.332, l0=0.079;\n",
" var sin = Math.sin, cos = Math.cos, PI = Math.PI;\n",
" var height = 300, rate=150; // pixel\n",
" var s2px = function(x_val){return 200 + x_val*rate;}, //sim2pixel\n",
" s2py = function(y_val){return height - y_val*rate;};\n",
"\n",
" // HAT\n",
" var theta = q[0], x = q[1], y = q[2];\n",
"\n",
" var x4 = x - l4*cos(theta),\n",
" y4 = y - l4*sin(theta);\n",
"\n",
" var x3 = x + (L4-l4)*cos(theta),\n",
" y3 = y + (L4-l4)*sin(theta);\n",
"\n",
" svg.select(\".GND\").attr({\n",
" x1: 0,\n",
" y1: s2py(0),\n",
" x2: 1000,\n",
" y2: s2py(0)\n",
" });\n",
"\n",
" svg.select(\".L4\")\n",
" .attr({\n",
" x1: s2px(x4),\n",
" y1: s2py(y4),\n",
" x2: s2px(x3),\n",
" y2: s2py(y3)\n",
" });\n",
"\n",
" [['R', q.slice(2, 6)], ['L', q.slice(5, 9)]].forEach(function(arr){\n",
" var suffix = arr[0],\n",
" zeta = arr[1]; // R: q[3]->zeta[1], q[4]->zeta[2], q[5]->zeta[3]\n",
"\n",
" var x2 = x3 + L3*sin(zeta[3]),\n",
" y2 = y3 - L3*cos(zeta[3]);\n",
"\n",
" svg.select(\".L3\" + suffix)\n",
" .attr({\n",
" x1: s2px(x3),\n",
" y1: s2py(y3),\n",
" x2: s2px(x2),\n",
" y2: s2py(y2)\n",
" });\n",
"\n",
" var x1 = x2 + L2*sin(zeta[2]+zeta[3]),\n",
" y1 = y2 - L2*cos(zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L2\" + suffix)\n",
" .attr({\n",
" x1: s2px(x2),\n",
" y1: s2py(y2),\n",
" x2: s2px(x1),\n",
" y2: s2py(y1)\n",
" });\n",
"\n",
" var x0t = x1 + L1*sin(zeta[1]+zeta[2]+zeta[3]),\n",
" y0t = y1 - L1*cos(zeta[1]+zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L1\" + suffix)\n",
" .attr({\n",
" x1: s2px(x1),\n",
" y1: s2py(y1),\n",
" x2: s2px(x0t),\n",
" y2: s2py(y0t)\n",
" });\n",
"\n",
" var x0h = x1 + l0*cos(zeta[1]+zeta[2]+zeta[3]),\n",
" y0h = y1 + l0*sin(zeta[1]+zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L0\" + suffix)\n",
" .attr({\n",
" x1: s2px(x1),\n",
" y1: s2py(y1),\n",
" x2: s2px(x0h),\n",
" y2: s2py(y0h)\n",
" });\n",
" });\n",
" };\n",
"\n",
" return{\n",
" init_svg: function(svg, options){\n",
" svg.attr(\"class\", \"viz\").style({\n",
" position: 'relative',\n",
" width: options.width,\n",
" height: options.height\n",
" });\n",
"\n",
" for(var i=0;i<=3;i++){\n",
" [\"L\", \"R\"].forEach(function(lr){\n",
" svg.append(\"line\").attr({\"class\": \"L\" + i + lr});\n",
" });\n",
" }\n",
" svg.append(\"line\").attr({\"class\": \"L4\"});\n",
" svg.append(\"line\").attr({\"class\": \"GND\"});\n",
" svg.selectAll(\"line\").attr({\n",
" 'stroke': \"rgb(0,0,0)\",\n",
" 'stroke-width': 3\n",
" });\n",
"\n",
" _.each(options.monitors, function(name, i){\n",
" var arr = []; //_.map(_.range(options.buff_size*5), function(){return 0;});\n",
" var g = svg.append(\"g\").attr({\n",
" 'class': 'monitor'+i,\n",
" 'transform': \"translate(0,\" + (options.viewer_height + options.monitor_height*i) + \")\"\n",
" });\n",
" g.append(\"path\").attr({\n",
" 'fill': 'none',\n",
" 'stroke': '#000',\n",
" 'stroke-width': 2\n",
" }).datum(arr);\n",
"\n",
" g.append(\"g\").attr(\"class\", \"g_axis\").\n",
" attr(\"transform\", \"translate(50,0)\");\n",
" //text\n",
" g.append(\"text\").attr({\n",
" \"x\": -options.monitor_height/2,\n",
" \"y\": 10,\n",
" \"transform\": \"rotate(-90)\",\n",
" \"text-anchor\": \"middle\"\n",
" }).text(name);\n",
" });\n",
" },\n",
"\n",
" draw: function(svg, q, monitors, options){\n",
" draw_viewer(svg, q);\n",
" _.each(options.monitors, function(name, i){\n",
" var g = svg.select(\".monitor\"+i);\n",
" draw_monitor(g, monitors[i], options);\n",
" });\n",
" }\n",
" };\n",
"});\n",
"\n",
"require([\"d3\", \"jquery\", \"underscore\", \"widgets/js/widget\", \"Viewer\"], function(d3, $, _, WidgetManager, Viewer){\n",
" // Define the DatePickerView\n",
" var SimView = window.IPython.DOMWidgetView.extend({\n",
" render: function(){\n",
" this.$el.css('position', 'relative');\n",
" this.$el.css('width', this.options.width);\n",
" this.$el.css('height', this.options.height);\n",
" },\n",
"\n",
" update: function() {\n",
" return SimView.__super__.update.apply(this);\n",
" },\n",
"\n",
" initialize: function(parameters){\n",
" SimView.__super__.initialize.apply(this, [parameters]);\n",
" this.options = _.extend({\n",
" duration: 10,\n",
" buff_size: 50,\n",
" monitors: 0,\n",
" width: 700,\n",
" viewer_height: 400,\n",
" monitor_height: 200\n",
" }, parameters.model.attributes.options);\n",
"\n",
" this.options.height = (_.bind(function(){\n",
" return this.viewer_height + this.monitor_height * _.keys(this.monitors).length;\n",
" }, this.options))();\n",
"\n",
" //*** init view ***\n",
" this.$el.addClass('widget-hbox');\n",
" var dom = document.createElementNS(\"http://www.w3.org/2000/svg\", \"svg:svg\");\n",
" this.$el.append(dom);\n",
" var svg = d3.select(dom);\n",
" Viewer.init_svg(svg, this.options);\n",
"\n",
" // arr is ret value from p2\n",
" var main = _.bind(function(content){\n",
" var dfd = new $.Deferred();\n",
" var arr = content.new_arr;\n",
" var monitors = (this.options.monitors ? _.zip.apply(null, _.values(content.monitors)) : new Array(this.options.buff_size));\n",
"\n",
" var p2 = (_.bind(function(){\n",
" var deferred = new $.Deferred();\n",
"\n",
" if(arr.length == 0){\n",
" deferred.resolve();\n",
" }else{\n",
" var cnt = 0;\n",
" var loop = _.bind(function(){\n",
" Viewer.draw(svg, arr[cnt], monitors[cnt], this.options);\n",
"\n",
" if(cnt == this.options.buff_size - 1){\n",
" deferred.resolve();\n",
" }else{\n",
" cnt+=1;\n",
" window.setTimeout(loop, this.options.duration);\n",
" }\n",
" }, this);\n",
" loop();\n",
" }\n",
" return deferred.promise();\n",
" }, this))();\n",
"\n",
" var p1 = (_.bind(function(){\n",
" var deferred = new $.Deferred();\n",
"\n",
" this.model.send({\n",
" num: this.options.buff_size\n",
" });\n",
"\n",
" // send custom message through Comm. See the following article to learn more:\n",
" // http://ipython.org/ipython-doc/dev/development/messaging.html\n",
" this.model.on('msg:custom', $.proxy(function(content){\n",
" if(content.error != true)\n",
" deferred.resolve(content);\n",
" else\n",
" deferred.reject();\n",
" }), this);\n",
"\n",
" return deferred.promise();\n",
" }, this))();\n",
"\n",
" $.when(p1, p2).then(\n",
" main,\n",
" function(err1, err2){\n",
" console.log(\"Visualizer stopped.\");\n",
" }\n",
" );\n",
"\n",
" dfd.resolve();\n",
" return dfd.promise();\n",
" }, this);\n",
"\n",
" main({new_arr: [], monitors: {}});\n",
" }\n",
" });\n",
" \n",
" WidgetManager.register_widget_view('SimView', SimView);\n",
"});\n"
],
"metadata": {},
"output_type": "display_data",
"text": [
"<IPython.core.display.Javascript at 0xb1482b6c>"
]
}
],
"prompt_number": 39
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"# error value\n",
"sim.add_monitor('d_zeta1', lambda q, dq_dt, t: (q[3] - expected_q(t)[3]))\n",
"sim.add_monitor('d_zeta2', lambda q, dq_dt, t: (q[4] - expected_q(t)[4]))\n",
"sim.add_monitor('d_zeta3', lambda q, dq_dt, t: (q[5] - expected_q(t)[5]))\n",
"sim.add_monitor('d_zeta4', lambda q, dq_dt, t: (q[6] - expected_q(t)[6]))\n",
"sim.add_monitor('d_zeta5', lambda q, dq_dt, t: (q[7] - expected_q(t)[7]))\n",
"sim.add_monitor('d_zeta6', lambda q, dq_dt, t: (q[8] - expected_q(t)[8]))\n",
"\n",
"\n",
"#sim.add_monitor('zeta1', lambda q, dq_dt, t: q[3])\n",
"#sim.add_monitor('zeta2', lambda q, dq_dt, t: q[4])\n",
"#sim.add_monitor('zeta3', lambda q, dq_dt, t: q[5])\n",
"\n",
"# ground reaction force\n",
"#sim.add_monitor('F_y_r_t', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[4])\n",
"#sim.add_monitor('F_y_r_h', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[5])\n",
"#sim.add_monitor('F_y_l_t', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[6])\n",
"#sim.add_monitor('F_y_l_h', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[7])\n",
"\n",
"#sim.add_monitor('F_x_r_t', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[0])\n",
"#sim.add_monitor('F_x_r_h', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[1])\n",
"#sim.add_monitor('F_x_l_t', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[2])\n",
"#sim.add_monitor('F_x_l_h', lambda q, dq_dt, t: (sim.solver.GRF(q, dq_dt))[3])"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 40
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sim.run()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"html": [
"<div\"></div>"
],
"metadata": {},
"output_type": "display_data",
"text": [
"<IPython.core.display.HTML at 0xb14821ac>"
]
}
],
"prompt_number": 41
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"q = np.concatenate((np.array([4.45, 1.5, 0.95]), link7.expected_q(0)[3:]))\n",
"dq_dt = np.concatenate((np.array([0.195, -1.189, 0.219]), link7.expected_dq_dt(0)[3:]))\n",
"init = np.concatenate((q, dq_dt))\n",
"print(init)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"[ 4.45 1.5 0.95 -1.04330218 1.15610381 -0.18525225\n",
" -1.25706716 0.34137921 -0.11993426 0.195 -1.189 0.219\n",
" -0.8653484 0.05947736 -2.66787512 -0.03768383 -1.60226281 1.68681628]\n"
]
}
],
"prompt_number": 32
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"controller = link7.DoNothingController()"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 5
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sim = link7.PDSimulator(initial_value=init, controller=controller, duration=50, monitor_height=50)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"javascript": [
"define(\"Viewer\", [\"d3\", \"underscore\"], function(d3, _){\n",
" var draw_monitor = function(g, val, options){\n",
" var data = g.select(\"path\").datum();\n",
"\n",
" //data.shift();\n",
" data.push(val);\n",
"\n",
" var scales = {\n",
" x: d3.scale.linear().domain([0, data.length]).range([50, options.width]),\n",
" y: d3.scale.linear().domain([_.min(data), _.max(data)]).range([options.monitor_height-5, 0])\n",
" };\n",
"\n",
" g.select(\"path\")\n",
" .datum(data)\n",
" .attr(\"d\", d3.svg.line()\n",
" .x(function(d, i){return scales.x(i);})\n",
" .y(function(d, i){return scales.y(d);})\n",
" );\n",
" g.select(\".g_axis\")\n",
" .call(d3.svg.axis().scale(scales.y).orient(\"left\").ticks(4).innerTickSize(-options.width+50).outerTickSize(0));\n",
" g.selectAll(\"path, line\").attr({\n",
" fill: 'none',\n",
" 'shape-rendering': 'crispEdges'\n",
" });\n",
" g.selectAll(\"text\").attr({\n",
" 'font-family': 'sans-serif',\n",
" 'font-size': 11\n",
" });\n",
" g.selectAll(\".tick line\").attr({\n",
" stroke: 'lightgrey',\n",
" 'stroke-width': 1,\n",
" opacity: 0.7\n",
" });\n",
" };\n",
"\n",
" // q: [theta, x, y, zeta1, zeta2, zeta3, zeta4, zeta5, zeta6]\n",
" var draw_viewer = function(svg, q){\n",
" var L1=0.122, L2=0.379, L3=0.420, L4=0.536, l4=0.332, l0=0.079;\n",
" var sin = Math.sin, cos = Math.cos, PI = Math.PI;\n",
" var height = 300, rate=150; // pixel\n",
" var s2px = function(x_val){return 200 + x_val*rate;}, //sim2pixel\n",
" s2py = function(y_val){return height - y_val*rate;};\n",
"\n",
" // HAT\n",
" var theta = q[0], x = q[1], y = q[2];\n",
"\n",
" var x4 = x - l4*cos(theta),\n",
" y4 = y - l4*sin(theta);\n",
"\n",
" var x3 = x + (L4-l4)*cos(theta),\n",
" y3 = y + (L4-l4)*sin(theta);\n",
"\n",
" svg.select(\".GND\").attr({\n",
" x1: 0,\n",
" y1: s2py(0),\n",
" x2: 1000,\n",
" y2: s2py(0)\n",
" });\n",
"\n",
" svg.select(\".L4\")\n",
" .attr({\n",
" x1: s2px(x4),\n",
" y1: s2py(y4),\n",
" x2: s2px(x3),\n",
" y2: s2py(y3)\n",
" });\n",
"\n",
" [['R', q.slice(2, 6)], ['L', q.slice(5, 9)]].forEach(function(arr){\n",
" var suffix = arr[0],\n",
" zeta = arr[1]; // R: q[3]->zeta[1], q[4]->zeta[2], q[5]->zeta[3]\n",
"\n",
" var x2 = x3 + L3*sin(zeta[3]),\n",
" y2 = y3 - L3*cos(zeta[3]);\n",
"\n",
" svg.select(\".L3\" + suffix)\n",
" .attr({\n",
" x1: s2px(x3),\n",
" y1: s2py(y3),\n",
" x2: s2px(x2),\n",
" y2: s2py(y2)\n",
" });\n",
"\n",
" var x1 = x2 + L2*sin(zeta[2]+zeta[3]),\n",
" y1 = y2 - L2*cos(zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L2\" + suffix)\n",
" .attr({\n",
" x1: s2px(x2),\n",
" y1: s2py(y2),\n",
" x2: s2px(x1),\n",
" y2: s2py(y1)\n",
" });\n",
"\n",
" var x0t = x1 + L1*sin(zeta[1]+zeta[2]+zeta[3]),\n",
" y0t = y1 - L1*cos(zeta[1]+zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L1\" + suffix)\n",
" .attr({\n",
" x1: s2px(x1),\n",
" y1: s2py(y1),\n",
" x2: s2px(x0t),\n",
" y2: s2py(y0t)\n",
" });\n",
"\n",
" var x0h = x1 + l0*cos(zeta[1]+zeta[2]+zeta[3]),\n",
" y0h = y1 + l0*sin(zeta[1]+zeta[2]+zeta[3]);\n",
"\n",
" svg.select(\".L0\" + suffix)\n",
" .attr({\n",
" x1: s2px(x1),\n",
" y1: s2py(y1),\n",
" x2: s2px(x0h),\n",
" y2: s2py(y0h)\n",
" });\n",
" });\n",
" };\n",
"\n",
" return{\n",
" init_svg: function(svg, options){\n",
" svg.attr(\"class\", \"viz\").style({\n",
" position: 'relative',\n",
" width: options.width,\n",
" height: options.height\n",
" });\n",
"\n",
" for(var i=0;i<=3;i++){\n",
" [\"L\", \"R\"].forEach(function(lr){\n",
" svg.append(\"line\").attr({\"class\": \"L\" + i + lr});\n",
" });\n",
" }\n",
" svg.append(\"line\").attr({\"class\": \"L4\"});\n",
" svg.append(\"line\").attr({\"class\": \"GND\"});\n",
" svg.selectAll(\"line\").attr({\n",
" 'stroke': \"rgb(0,0,0)\",\n",
" 'stroke-width': 3\n",
" });\n",
"\n",
" _.each(options.monitors, function(name, i){\n",
" var arr = []; //_.map(_.range(options.buff_size*5), function(){return 0;});\n",
" var g = svg.append(\"g\").attr({\n",
" 'class': 'monitor'+i,\n",
" 'transform': \"translate(0,\" + (options.viewer_height + options.monitor_height*i) + \")\"\n",
" });\n",
" g.append(\"path\").attr({\n",
" 'fill': 'none',\n",
" 'stroke': '#000',\n",
" 'stroke-width': 2\n",
" }).datum(arr);\n",
"\n",
" g.append(\"g\").attr(\"class\", \"g_axis\").\n",
" attr(\"transform\", \"translate(50,0)\");\n",
" //text\n",
" g.append(\"text\").attr({\n",
" \"x\": -options.monitor_height/2,\n",
" \"y\": 10,\n",
" \"transform\": \"rotate(-90)\",\n",
" \"text-anchor\": \"middle\"\n",
" }).text(name);\n",
" });\n",
" },\n",
"\n",
" draw: function(svg, q, monitors, options){\n",
" draw_viewer(svg, q);\n",
" _.each(options.monitors, function(name, i){\n",
" var g = svg.select(\".monitor\"+i);\n",
" draw_monitor(g, monitors[i], options);\n",
" });\n",
" }\n",
" };\n",
"});\n",
"\n",
"require([\"d3\", \"jquery\", \"underscore\", \"widgets/js/widget\", \"Viewer\"], function(d3, $, _, WidgetManager, Viewer){\n",
" // Define the DatePickerView\n",
" var SimView = window.IPython.DOMWidgetView.extend({\n",
" render: function(){\n",
" this.$el.css('position', 'relative');\n",
" this.$el.css('width', this.options.width);\n",
" this.$el.css('height', this.options.height);\n",
" },\n",
"\n",
" update: function() {\n",
" return SimView.__super__.update.apply(this);\n",
" },\n",
"\n",
" initialize: function(parameters){\n",
" SimView.__super__.initialize.apply(this, [parameters]);\n",
" this.options = _.extend({\n",
" duration: 10,\n",
" buff_size: 50,\n",
" monitors: 0,\n",
" width: 700,\n",
" viewer_height: 400,\n",
" monitor_height: 200\n",
" }, parameters.model.attributes.options);\n",
"\n",
" this.options.height = (_.bind(function(){\n",
" return this.viewer_height + this.monitor_height * _.keys(this.monitors).length;\n",
" }, this.options))();\n",
"\n",
" //*** init view ***\n",
" this.$el.addClass('widget-hbox');\n",
" var dom = document.createElementNS(\"http://www.w3.org/2000/svg\", \"svg:svg\");\n",
" this.$el.append(dom);\n",
" var svg = d3.select(dom);\n",
" Viewer.init_svg(svg, this.options);\n",
"\n",
" // arr is ret value from p2\n",
" var main = _.bind(function(content){\n",
" var dfd = new $.Deferred();\n",
" var arr = content.new_arr;\n",
" var monitors = (this.options.monitors ? _.zip.apply(null, _.values(content.monitors)) : new Array(this.options.buff_size));\n",
"\n",
" var p2 = (_.bind(function(){\n",
" var deferred = new $.Deferred();\n",
"\n",
" if(arr.length == 0){\n",
" deferred.resolve();\n",
" }else{\n",
" var cnt = 0;\n",
" var loop = _.bind(function(){\n",
" Viewer.draw(svg, arr[cnt], monitors[cnt], this.options);\n",
"\n",
" if(cnt == this.options.buff_size - 1){\n",
" deferred.resolve();\n",
" }else{\n",
" cnt+=1;\n",
" window.setTimeout(loop, this.options.duration);\n",
" }\n",
" }, this);\n",
" loop();\n",
" }\n",
" return deferred.promise();\n",
" }, this))();\n",
"\n",
" var p1 = (_.bind(function(){\n",
" var deferred = new $.Deferred();\n",
"\n",
" this.model.send({\n",
" num: this.options.buff_size\n",
" });\n",
"\n",
" // send custom message through Comm. See the following article to learn more:\n",
" // http://ipython.org/ipython-doc/dev/development/messaging.html\n",
" this.model.on('msg:custom', $.proxy(function(content){\n",
" if(content.error != true)\n",
" deferred.resolve(content);\n",
" else\n",
" deferred.reject();\n",
" }), this);\n",
"\n",
" return deferred.promise();\n",
" }, this))();\n",
"\n",
" $.when(p1, p2).then(\n",
" main,\n",
" function(err1, err2){\n",
" console.log(\"Visualizer stopped.\");\n",
" }\n",
" );\n",
"\n",
" dfd.resolve();\n",
" return dfd.promise();\n",
" }, this);\n",
"\n",
" main({new_arr: [], monitors: {}});\n",
" }\n",
" });\n",
" \n",
" WidgetManager.register_widget_view('SimView', SimView);\n",
"});\n"
],
"metadata": {},
"output_type": "display_data",
"text": [
"<IPython.core.display.Javascript at 0xb6f7c64c>"
]
}
],
"prompt_number": 6
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"sim.run()"
],
"language": "python",
"metadata": {},
"outputs": [
{
"html": [
"<div\"></div>"
],
"metadata": {},
"output_type": "display_data",
"text": [
"<IPython.core.display.HTML at 0xb6f7c58c>"
]
}
],
"prompt_number": 7
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
}
],
"metadata": {}
}
]
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment