Skip to content
Permalink
master
Switch branches/tags

Name already in use

A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?

kineval-stencil/project_pendularm/pendularm1.html

Go to file
 
 
Cannot retrieve contributors at this time
350 lines (271 sloc) 11.7 KB
Raw Blame
Open in GitHub Desktop
<!--|\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/|
|\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/|
||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/
/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\
Pendularm 1 simulation
Simple 1 DOF pendulum dynamics and control in HTML5/JavaScript and threejs
@author ohseejay / https://github.com/ohseejay
/ https://bitbucket.org/ohseejay
Chad Jenkins
Laboratory for Perception RObotics and Grounded REasoning Systems
University of Michigan
License: Michigan Honor License
|\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/|
||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/
/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\
\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\/||\-->
<html>
<body>
<!-- //////////////////////////////////////////////////
///// JAVASCRIPT INCLUDES
////////////////////////////////////////////////// -->
<!-- threejs - https://github.com/mrdoob/three.js/ -->
<script src="js/three.min.js"></script>
<!-- threejs camera controls helpers -->
<script src="js/OrbitControls.js"></script>
<!-- threejs keyboard input helper -->
<script src="js/THREEx.KeyboardState.js"></script>
<!-- functions to be implemented -->
<script src="update_pendulum_state.js"></script>
<script>
//////////////////////////////////////////////////
///// MAIN FUNCTION CALLS
//////////////////////////////////////////////////
// initialize threejs scene, user input, and robot kinematics
init();
// main animation loop maintained by threejs
animate();
//////////////////////////////////////////////////
///// INITIALIZATION FUNCTION DEFINITONS
//////////////////////////////////////////////////
function init() {
// create pendulum object and its kinematic and dynamic parameters
pendulum = {length:2.0, mass:2.0, angle:Math.PI/2, angle_dot:0.0, angle_previous:0.0};
// initialize pendulum controls
pendulum.control = 0;
pendulum.desired = -Math.PI/2.5;
// initialize integral term accumulated error to zero
accumulated_error = 0;
// set gravity
gravity = 9.81; // Earth gravity
// initialize pendulum PID servo gains
pendulum = set_PID_parameters(pendulum)
// initialize time and set timestep
t = 0;
dt = 0.05; // default
// initialize method of numerical integration of dynamics
numerical_integrator = "none";
//numerical_integrator = "euler";
//numerical_integrator = "verlet";
//numerical_integrator = "velocity verlet";
//numerical_integrator = "runge-kutta";
// OPTIONAL servo controller additional features
steady_state_error_reset = false; // integral term resets after desired met
servo_error_threshold = 0.001; // threshold for achieving desired
servo_active_persist = false; // toggle to turn on servo controller
servo_active_state = {}; // string with current state of servo activation
//STENCIL: for verlet integration, a first step in time is needed
if (typeof numerical_integrator !== "undefined") {
if (numerical_integrator === "verlet") {
var result = init_verlet_integrator(pendulum, t, gravity);
pendulum = result[0]; t = result[1];
}
}
document.addEventListener('keydown', function(e) {
if (e.keyCode == 88) // 'x' == 88
servo_active_persist = !servo_active_persist;
}, true);
// initialize rendering scene and user interface
createScene();
}
//////////////////////////////////////////////////
///// ANIMATION AND INTERACTION LOOP
//////////////////////////////////////////////////
function animate() {
// note: three.js includes requestAnimationFrame shim
// alternative to using setInterval for updating in-browser drawing
// this effectively request that the animate function be called again for next draw
// http://learningwebgl.com/blog/?p=3189
requestAnimationFrame( animate );
// switch between numerical integrators based on user input
if (keyboard.pressed("0"))
numerical_integrator = "none";
if (keyboard.pressed("1"))
numerical_integrator = "euler";
if (keyboard.pressed("2"))
numerical_integrator = "verlet";
if (keyboard.pressed("3"))
numerical_integrator = "velocity verlet";
if (keyboard.pressed("4"))
numerical_integrator = "runge-kutta";
// zero the controls for the current time step
pendulum.control = 0;
// update servo desired state from user interaction
if ( keyboard.pressed("e") )
pendulum.desired += 0.05; // move the desired angle for the servo
if ( keyboard.pressed("q") )
pendulum.desired += -0.05; // move the desired angle for the servo
// add user force from user interaction
if ( keyboard.pressed("d") )
pendulum.control += 50.0; // add a motor force to the pendulum motor
else if ( keyboard.pressed("a") )
pendulum.control += -50.0; // add a motor force to the pendulum motor
// STENCIL: implement servo controller
var result = PID(pendulum, accumulated_error, dt);
pendulum = result[0]; accumulated_error = result[1];
// toggle activation of servo controller from user interaction
if (keyboard.pressed("c"))
servo_active_persist = !servo_active_persist;
// disable motor from user interaction
if (keyboard.pressed("s")||!servo_active_persist) {
pendulum.control = 0;
accumulated_error = 0;
servo_active_state = "disabled";
}
else
servo_active_state = "active";
pendulum.angle_dot_dot = pendulum_acceleration(pendulum, gravity);
pendulum = update_pendulum_state(numerical_integrator, pendulum, dt, gravity);
// set the angles of the pendulum
pendulum.geom.rotation.y = pendulum.angle; // threejs cylinders have their axes along the y-axis
// advance time
t = t + dt;
textbar.innerHTML =
"System <br> " +
" t = " + t.toFixed(2) +
" dt = " + dt.toFixed(2) +
"<br>" +
" integrator = " + numerical_integrator +
"<br>" +
" x = " + pendulum.angle.toFixed(2) +
"<br>" +
" x_dot = " + pendulum.angle_dot.toFixed(2) +
"<br>" +
" x_desired = " + pendulum.desired.toFixed(2) +
"<br><br> Servo: " + servo_active_state + " <br> " +
" u = " + pendulum.control.toFixed(2) +
"<br>" +
" kp = " + pendulum.servo.kp.toFixed(2) +
"<br>" +
" kd = " + pendulum.servo.kd.toFixed(2) +
"<br>" +
" ki = " + pendulum.servo.ki.toFixed(2) +
"<br><br> Pendulum <br> " +
" mass = " + pendulum.mass.toFixed(2) +
"<br>" +
" length = " + pendulum.length.toFixed(2) +
"<br>" +
" gravity = " + gravity.toFixed(2) +
"<br><br> Keys <br> " +
" [0-4] - select integrator " +
"<br>" +
" a/d - apply user force " +
"<br>" +
" q/e - adjust desired angle " +
"<br>" +
" c|x - toggle servo " +
"<br>" +
" s - disable servo "
;
// threejs rendering update
renderer.render( scene, camera );
}
function createScene() {
// instantiate threejs scene graph
scene = new THREE.Scene();
// instantiate threejs camera and set its position in the world
camera = new THREE.PerspectiveCamera( 75, window.innerWidth / window.innerHeight, 1, 10000 );
camera.position.y = 1;
camera.position.z = 4;
var light1 = new THREE.PointLight( 0xffffff, 0.3, 1000 );
light1.position.set( 10, 10, 10 );
scene.add( light1 );
var light2 = new THREE.PointLight( 0xffffff, 0.3, 1000 );
light2.position.set( 10, -10, 10 );
scene.add( light2 );
var light3 = new THREE.PointLight( 0xffffff, 0.3, 1000 );
light3.position.set( -10, -10, 10 );
scene.add( light3 );
var light4 = new THREE.PointLight( 0xffffff, 0.3, 1000 );
light4.position.set( -10, 10, 10 );
scene.add( light4 );
// instantiate threejs renderer and its dimensions
renderer = new THREE.WebGLRenderer();
renderer.setSize( window.innerWidth, window.innerHeight );
// attach threejs renderer to DOM
document.body.appendChild( renderer.domElement );
// instantiate threejs camera controls
camera_controls = new THREE.OrbitControls( camera );
camera_controls.addEventListener( 'change', renderer );
// instantiate threejs keyboard controls, for continuous interactive controls
keyboard = new THREEx.KeyboardState();
textbar = document.createElement('div');
textbar.style.position = 'absolute';
//textbar.style.zIndex = 1; // if you still don't see the label, try uncommenting this
textbar.style.width = window.width-10;
textbar.style["font-family"] = "Monospace";
textbar.style.height = 20;
//textbar.style.backgroundColor = "black";
textbar.style.color = "#000000";
textbar.innerHTML = "M4PRoGReS - pendularm!";
textbar.style.top = 10 + 'px';
textbar.style.left = 10 + 'px';
document.body.appendChild(textbar);
temp_geom = new THREE.CylinderGeometry(0.2, 0.2, 3.5, 20, 20, false);
temp_material = new THREE.MeshLambertMaterial( { } );
temp_material.color.r = 1;
temp_material.color.g = 1;
temp_material.color.b = 1;
temp_material.color.b = 1;
temp_material.transparent = true;
temp_material.opacity = 0.3;
leg1 = new THREE.Mesh(temp_geom, temp_material);
leg2 = new THREE.Mesh(temp_geom, temp_material);
leg3 = new THREE.Mesh(temp_geom, temp_material);
leg4 = new THREE.Mesh(temp_geom, temp_material);
leg1.position = {x:2,z:1,y:0};
leg2.position = {x:-2,z:1,y:0};
leg3.position = {x:-2,z:-1,y:0};
leg4.position = {x:2,z:-1,y:0};
scene.add(leg1);
scene.add(leg2);
scene.add(leg3);
scene.add(leg4);
temp_geom = new THREE.CylinderGeometry(0.2, 0.2, 4.0, 20, 20, false);
sidebar1 = new THREE.Mesh(temp_geom, temp_material);
sidebar1.rotateOnAxis(new THREE.Vector3(0,0,1),Math.PI/2);
sidebar1.position = {x:-2,z:0,y:1.5};
leg1.add(sidebar1);
sidebar2 = new THREE.Mesh(temp_geom, temp_material);
sidebar2.rotateOnAxis(new THREE.Vector3(0,0,1),Math.PI/2);
sidebar2.position = {x:2,z:0,y:1.5};
leg3.add(sidebar2);
temp_geom = new THREE.CylinderGeometry(0.2, 0.2, 2.0, 20, 20, false);
crossbar = new THREE.Mesh(temp_geom, temp_material);
crossbar.rotateOnAxis(new THREE.Vector3(1,0,0),Math.PI/2);
crossbar.position = {x:0,z:-1,y:0};
sidebar1.add(crossbar);
temp_geom = new THREE.CylinderGeometry(0.3, 0.3, 0.3, 20, 20, false);
temp_material = new THREE.MeshLambertMaterial( { } );
temp_material.color.r = 1;
temp_material.color.g = 0;
temp_material.color.b = 0;
temp_material.transparent = false;
pendulum.geom = new THREE.Mesh(temp_geom, temp_material);
pendulum.geom.rotateOnAxis(new THREE.Vector3(1,0,0),Math.PI/2);
//crossbar.add(pendulum.geom);
scene.add(pendulum.geom);
pendulum.geom.position = {x:0,y:1.5,z:0};
temp_geom = new THREE.CylinderGeometry(0.2, 0.2, pendulum.length, 20, 20, false);
pendulum_link = new THREE.Mesh(temp_geom, temp_material);
pendulum_link.rotateOnAxis(new THREE.Vector3(1,0,0),-Math.PI/2);
pendulum_link.position = {x:0,z:pendulum.length/2,y:0};
pendulum.geom.add(pendulum_link);
temp_geom = new THREE.SphereGeometry(Math.sqrt(pendulum.mass*0.1));
pendulum_mass = new THREE.Mesh(temp_geom, temp_material);
pendulum_mass.position = {x:0,y:-pendulum.length/2,z:0};
pendulum_link.add(pendulum_mass);
}
</script>
</body>
</html>