first commit
This commit is contained in:
414
thirdparty/ode-0.16.5/ode/demo/demo_transmission.cpp
vendored
Normal file
414
thirdparty/ode-0.16.5/ode/demo/demo_transmission.cpp
vendored
Normal file
@@ -0,0 +1,414 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/ode.h>
|
||||
#include <drawstuff/drawstuff.h>
|
||||
#include "texturepath.h"
|
||||
|
||||
#ifdef dDOUBLE
|
||||
#define dsDrawCylinder dsDrawCylinderD
|
||||
#define dsDrawLine dsDrawLineD
|
||||
#define dsDrawSphere dsDrawSphereD
|
||||
#endif
|
||||
|
||||
dReal theta = M_PI / 4;
|
||||
dReal ratio = 1, speed = 5, rho_1 = 1, rho_2 = 1, backlash = 0.1;
|
||||
int mode = 0;
|
||||
|
||||
dWorldID world;
|
||||
dSpaceID space;
|
||||
dBodyID body1, body2;
|
||||
dGeomID geom1, geom2;
|
||||
dJointID hinge1, hinge2, transmission;
|
||||
dJointFeedback feedback;
|
||||
|
||||
void setup() {
|
||||
dMatrix3 R;
|
||||
|
||||
switch (mode) {
|
||||
case 0:
|
||||
// Parallel axes.
|
||||
|
||||
dBodySetPosition(body1, 1, 0, 1);
|
||||
dBodySetPosition(body2, -1, 0, 1);
|
||||
|
||||
dRSetIdentity (R);
|
||||
dBodySetRotation (body1, R);
|
||||
dBodySetRotation (body2, R);
|
||||
|
||||
dJointSetHingeAnchor(hinge2, -1, 0, 1);
|
||||
dJointSetHingeAxis(hinge2, 0, 0, 1);
|
||||
|
||||
dJointSetHingeAnchor(hinge1, 1, 0, 1);
|
||||
dJointSetHingeAxis(hinge1, 0, 0, 1);
|
||||
|
||||
dJointSetTransmissionMode(transmission, dTransmissionParallelAxes);
|
||||
dJointSetTransmissionRatio(transmission, ratio);
|
||||
dJointSetTransmissionAnchor1(transmission, 1, 0, 1);
|
||||
dJointSetTransmissionAnchor2(transmission, -1, 0, 1);
|
||||
dJointSetTransmissionAxis(transmission, 0, 0, 1);
|
||||
|
||||
break;
|
||||
case 1:
|
||||
// Intersecting axes.
|
||||
|
||||
dBodySetPosition(body1, 1, 0, 1);
|
||||
dBodySetPosition(body2, -1, 0, 2);
|
||||
|
||||
dRSetIdentity (R);
|
||||
dBodySetRotation (body1, R);
|
||||
|
||||
dRFromZAxis (R, cos(theta), 0, sin(theta));
|
||||
dBodySetRotation (body2, R);
|
||||
|
||||
dJointSetHingeAnchor(hinge2, -1, 0, 2);
|
||||
dJointSetHingeAxis(hinge2, cos(theta), 0, sin(theta));
|
||||
|
||||
dJointSetHingeAnchor(hinge1, 1, 0, 1);
|
||||
dJointSetHingeAxis(hinge1, 0, 0, 1);
|
||||
|
||||
dJointSetTransmissionMode(transmission, dTransmissionIntersectingAxes);
|
||||
dJointSetTransmissionAnchor1(transmission, 1, 0, 1);
|
||||
dJointSetTransmissionAnchor2(transmission, -1, 0, 2);
|
||||
dJointSetTransmissionAxis1(transmission, 0, 0, -1);
|
||||
dJointSetTransmissionAxis2(transmission, cos(theta), 0, sin(theta));
|
||||
|
||||
break;
|
||||
case 2:
|
||||
// Chain.
|
||||
|
||||
dBodySetPosition(body1, 2, 0, 1);
|
||||
dBodySetPosition(body2, -2, 0, 1);
|
||||
|
||||
dRSetIdentity (R);
|
||||
dBodySetRotation (body1, R);
|
||||
dBodySetRotation (body2, R);
|
||||
|
||||
dJointSetHingeAnchor(hinge2, -2, 0, 1);
|
||||
dJointSetHingeAxis(hinge2, 0, 0, 1);
|
||||
|
||||
dJointSetHingeAnchor(hinge1, 2, 0, 1);
|
||||
dJointSetHingeAxis(hinge1, 0, 0, 1);
|
||||
|
||||
dJointSetTransmissionMode(transmission, dTransmissionChainDrive);
|
||||
dJointSetTransmissionAnchor1(transmission, 2, 0, 1);
|
||||
dJointSetTransmissionAnchor2(transmission, -2, 0, 1);
|
||||
dJointSetTransmissionRadius1(transmission, rho_1);
|
||||
dJointSetTransmissionRadius2(transmission, rho_2);
|
||||
dJointSetTransmissionAxis(transmission, 0, 0, 1);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
dJointSetTransmissionBacklash(transmission, backlash);
|
||||
|
||||
dJointSetHingeParam(hinge2, dParamVel, speed);
|
||||
dJointSetHingeParam(hinge2, dParamFMax, 50);
|
||||
|
||||
dJointSetHingeParam(hinge1, dParamVel, 0);
|
||||
dJointSetHingeParam(hinge1, dParamFMax, 2);
|
||||
|
||||
dBodySetLinearVel(body1, 0, 0, 0);
|
||||
dBodySetLinearVel(body2, 0, 0, 0);
|
||||
dBodySetAngularVel(body1, 0, 0, 0);
|
||||
dBodySetAngularVel(body2, 0, 0, 0);
|
||||
}
|
||||
|
||||
void start()
|
||||
{
|
||||
dMass mass;
|
||||
|
||||
world = dWorldCreate();
|
||||
dWorldSetGravity (world,0,0,-9.8);
|
||||
|
||||
dWorldSetERP(world, 0.2);
|
||||
|
||||
space = dSimpleSpaceCreate (0);
|
||||
|
||||
body1 = dBodyCreate(world);
|
||||
body2 = dBodyCreate(world);
|
||||
|
||||
dBodySetFiniteRotationMode(body1, 1);
|
||||
dBodySetFiniteRotationMode(body2, 1);
|
||||
|
||||
geom1 = dCreateCylinder(space, 0.2, 0.5);
|
||||
dGeomSetBody(geom1, body1);
|
||||
dMassSetCylinder(&mass, 100, 3, 0.2, 0.5);
|
||||
dBodySetMass(body1, &mass);
|
||||
|
||||
geom2 = dCreateCylinder(space, 0.2, 0.5);
|
||||
dGeomSetBody(geom2, body2);
|
||||
dMassSetCylinder(&mass, 100, 3, 0.2, 0.5);
|
||||
dBodySetMass(body2, &mass);
|
||||
|
||||
hinge1 = dJointCreateHinge(world, 0);
|
||||
dJointAttach(hinge1, body1, 0);
|
||||
|
||||
hinge2 = dJointCreateHinge(world, 0);
|
||||
dJointAttach(hinge2, body2, 0);
|
||||
|
||||
transmission = dJointCreateTransmission(world, 0);
|
||||
dJointAttach(transmission, body1, body2);
|
||||
dJointSetFeedback(transmission, &feedback);
|
||||
|
||||
setup();
|
||||
|
||||
// initial camera position
|
||||
static float xyz[3] = {1.15,-2.78,4.1};
|
||||
static float hpr[3] = {105,-45.5,0};
|
||||
dsSetViewpoint (xyz,hpr);
|
||||
|
||||
fprintf (stderr,
|
||||
"The green wheel is driving the red one. To control it use the following:\n"
|
||||
" '[' : decrease wheel ratio\n"
|
||||
" ']' : increase wheel ratio\n"
|
||||
" ',' : decrease driving wheel speed\n"
|
||||
" '.' : increase driving wheel speed\n"
|
||||
" '-' : decrease backlash\n"
|
||||
" '=' : increase backlash\n"
|
||||
" '1' : switch to parallel axes gears mode\n"
|
||||
" '2' : switch to intersecting axes gears mode\n"
|
||||
" '3' : switch to chain (or belt) mode\n"
|
||||
);
|
||||
}
|
||||
|
||||
void stop()
|
||||
{
|
||||
dSpaceDestroy(space);
|
||||
|
||||
dWorldDestroy(world);
|
||||
}
|
||||
|
||||
void drawGeom(dGeomID g)
|
||||
{
|
||||
int gclass = dGeomGetClass(g);
|
||||
const dReal *pos = dGeomGetPosition(g);
|
||||
const dReal *rot = dGeomGetRotation(g);
|
||||
|
||||
switch (gclass) {
|
||||
case dCylinderClass:
|
||||
{
|
||||
dReal length, radius;
|
||||
|
||||
if (g == geom1) {
|
||||
dsSetColorAlpha(1, 0, 0, 1);
|
||||
} else {
|
||||
dsSetColorAlpha(0, 1, 0, 1);
|
||||
}
|
||||
|
||||
dsSetTexture (DS_WOOD);
|
||||
dGeomCylinderGetParams(g, &radius, &length);
|
||||
dsDrawCylinder(pos, rot, length, radius);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
abort();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void simLoop(int pause)
|
||||
{
|
||||
if (!pause) {
|
||||
|
||||
const dReal step = 0.003;
|
||||
const unsigned nsteps = 4;
|
||||
|
||||
for (unsigned i=0; i<nsteps; ++i) {
|
||||
dWorldQuickStep(world, step);
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
{
|
||||
const dReal *omega_1, *omega_2;
|
||||
|
||||
omega_1 = dBodyGetAngularVel(body1);
|
||||
omega_2 = dBodyGetAngularVel(body2);
|
||||
|
||||
printf ("T1: %f, %f, %f\n",
|
||||
feedback.t1[0], feedback.t1[1], feedback.t1[2]);
|
||||
|
||||
printf ("T2: %f, %f, %f\n",
|
||||
feedback.t2[0], feedback.t2[1], feedback.t2[2]);
|
||||
|
||||
printf ("F1: %f, %f, %f\n",
|
||||
feedback.f1[0], feedback.f1[1], feedback.f1[2]);
|
||||
|
||||
printf ("F2: %f, %f, %f\n",
|
||||
feedback.f2[0], feedback.f2[1], feedback.f2[2]);
|
||||
}
|
||||
#endif
|
||||
|
||||
// now we draw everything
|
||||
unsigned ngeoms = dSpaceGetNumGeoms(space);
|
||||
for (unsigned i=0; i<ngeoms; ++i) {
|
||||
dGeomID g = dSpaceGetGeom(space, i);
|
||||
|
||||
drawGeom(g);
|
||||
}
|
||||
|
||||
const dReal *R_1 = dGeomGetRotation(geom1);
|
||||
const dReal *R_2 = dGeomGetRotation(geom2);
|
||||
dVector3 c_1, c_2, a_1, a_2;
|
||||
|
||||
dJointGetTransmissionContactPoint1(transmission, c_1);
|
||||
dJointGetTransmissionContactPoint2(transmission, c_2);
|
||||
dJointGetTransmissionAnchor1(transmission, a_1);
|
||||
dJointGetTransmissionAnchor2(transmission, a_2);
|
||||
|
||||
dsSetColorAlpha(1, 0, 0, 0.5);
|
||||
dsDrawCylinder(a_1, R_1, 0.05, dCalcPointsDistance3(c_1, a_1));
|
||||
dsSetColorAlpha(0, 1, 0, 0.5);
|
||||
dsDrawCylinder(a_2, R_2, 0.05, dCalcPointsDistance3(c_2, a_2));
|
||||
|
||||
dsSetColorAlpha(1, 0, 0, 0.5);
|
||||
dsDrawSphere (c_1, R_1, 0.05);
|
||||
dsDrawSphere (c_2, R_1, 0.05);
|
||||
|
||||
dsSetColorAlpha(1, 1, 0, 0.5);
|
||||
if (mode == dTransmissionChainDrive) {
|
||||
dsDrawLine(c_1, c_2);
|
||||
}
|
||||
}
|
||||
|
||||
static void command (int cmd)
|
||||
{
|
||||
if (cmd == '[') {
|
||||
switch(mode) {
|
||||
case dTransmissionParallelAxes:
|
||||
if (ratio > 0.125) {
|
||||
ratio *= 0.5;
|
||||
|
||||
fprintf (stderr, "Gear ratio set to %.3f.\n", ratio);
|
||||
}
|
||||
break;
|
||||
case dTransmissionIntersectingAxes:
|
||||
if (theta > 0.1) {
|
||||
theta -= 0.1;
|
||||
|
||||
fprintf (stderr, "Gear angle set to %.3f deg.\n",
|
||||
theta / M_PI * 180);
|
||||
}
|
||||
break;
|
||||
case dTransmissionChainDrive:
|
||||
if (rho_2 > 0.125) {
|
||||
rho_2 /= 2;
|
||||
|
||||
fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
setup();
|
||||
} else if (cmd == ']') {
|
||||
switch(mode) {
|
||||
case dTransmissionParallelAxes:
|
||||
if (ratio < 8) {
|
||||
ratio *= 2;
|
||||
|
||||
fprintf (stderr, "Gear ratio set to %.3f.\n", ratio);
|
||||
}
|
||||
break;
|
||||
case dTransmissionIntersectingAxes:
|
||||
if (theta < 0.9) {
|
||||
theta += 0.1;
|
||||
|
||||
fprintf (stderr, "Gear angle set to %.3f deg.\n",
|
||||
theta / M_PI * 180);
|
||||
}
|
||||
break;
|
||||
case dTransmissionChainDrive:
|
||||
if (rho_2 < 2) {
|
||||
rho_2 *= 2;
|
||||
|
||||
fprintf (stderr, "Sprocket ratio set to %.3f.\n", rho_2 / rho_1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
setup();
|
||||
} else if (cmd == '.') {
|
||||
speed += 5;
|
||||
|
||||
fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed);
|
||||
|
||||
dJointSetHingeParam(hinge2, dParamVel, speed);
|
||||
} else if (cmd == ',') {
|
||||
speed -= 5;
|
||||
|
||||
fprintf (stderr, "Driving wheel speed set to %g rad/s.\n", speed);
|
||||
|
||||
dJointSetHingeParam(hinge2, dParamVel, speed);
|
||||
} else if (cmd == '/') {
|
||||
if (dJointGetHingeParam(hinge2, dParamFMax) > 0) {
|
||||
dJointSetHingeParam(hinge2, dParamFMax, 0);
|
||||
} else {
|
||||
dJointSetHingeParam(hinge2, dParamFMax, 50);
|
||||
}
|
||||
|
||||
} else if (cmd == '-') {
|
||||
backlash -= 0.1;
|
||||
|
||||
fprintf (stderr, "Backlash set to %g m.\n", backlash);
|
||||
|
||||
dJointSetTransmissionBacklash(transmission, backlash);
|
||||
} else if (cmd == '=') {
|
||||
backlash += 0.1;
|
||||
|
||||
fprintf (stderr, "Backlash set to %g m.\n", backlash);
|
||||
|
||||
dJointSetTransmissionBacklash(transmission, backlash);
|
||||
} else if (cmd == '1') {
|
||||
mode = dTransmissionParallelAxes;
|
||||
setup();
|
||||
} else if (cmd == '2') {
|
||||
mode = dTransmissionIntersectingAxes;
|
||||
setup();
|
||||
} else if (cmd == '3') {
|
||||
mode = dTransmissionChainDrive;
|
||||
setup();
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
// setup pointers to drawstuff callback functions
|
||||
dsFunctions fn;
|
||||
fn.version = DS_VERSION;
|
||||
fn.start = &start;
|
||||
fn.step = &simLoop;
|
||||
fn.command = &command;
|
||||
fn.stop = stop;
|
||||
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
|
||||
|
||||
// create world
|
||||
dInitODE();
|
||||
|
||||
// run demo
|
||||
dsSimulationLoop (argc, argv, 800, 600, &fn);
|
||||
|
||||
dCloseODE();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user