Skip to content

Improve motor demo #144

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
i-make-robots wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Improve motor demo #144

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
36c8a80
improved forces and response from user input
i-make-robots Nov 4, 2024
4009d29
switch easier to read (formatting), nearcallback not static
i-make-robots Nov 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
147 changes: 68 additions & 79 deletions demo/src/main/java/org/ode4j/demo/DemoMotor.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,76 +45,67 @@


/**
*
* Motor demo.
* Motor demo. Creates two boxes.
* The first box is connected to the world with an angular motor (amotor).
* The second box is connected to the first box with a linear motor (lmotor).
*/
public class DemoMotor extends dsFunctions {

// some constants
//#define SIDE (0.5f) // side length of a box
//#define MASS (1.0) // mass of a box
private static final float SIDE = 0.5f;
private static final float MASS = 1.0f;

// maximum force that can be applied in a given frame.
private final double FORCE_MAX_ANGULAR = 0.01;
private final double FORCE_MAX_LINEAR = 0.1;
// force applied by the user each frame when keys are pressed. (not held, not released.)
private final int ACCELERATION = 1;

// dynamics and collision objects
private static DWorld world;
private static DBody[] body = new DBody[2];
private static DBox[] geom = new DBox[2];
private static DLMotorJoint[] lmotor = new DLMotorJoint[2];
private static DAMotorJoint[] amotor = new DAMotorJoint[2];
private static final DBody[] body = new DBody[2];
private static final DBox[] geom = new DBox[2];
private static final DLMotorJoint[] lmotor = new DLMotorJoint[2];
private static final DAMotorJoint[] amotor = new DAMotorJoint[2];
private static DSpace space;
private static DJointGroup contactgroup;


// start simulation - set viewpoint

// viewpoint at start
private static float[] xyz = {1.0382f,-1.0811f,1.4700f};
private static float[] hpr = {135.0000f,-19.5000f,0.0000f};

@Override
public void start()
{
public void start() {
dsSetViewpoint (xyz,hpr);
System.out.println ("Press 'q,a,z' to control one axis of lmotor connectiong two bodies. (q is +,a is 0, z is -)");
System.out.println ("Press 'w,e,r' to control one axis of lmotor connectiong first body with world. (w is +,e is 0, r is -)");
System.out.println ("Press 'w,e,r' to control one axis of amotor connectiong first body with world. (w is +,e is 0, r is -)");
}


// called when a key pressed
@Override
public void command (char cmd)
{
if (cmd == 'q' || cmd == 'Q') {
lmotor[0].setParamVel(0);
lmotor[0].setParamVel2(0);
lmotor[0].setParamVel3(0.1);
} else if (cmd == 'a' || cmd == 'A') {
lmotor[0].setParamVel(0);
lmotor[0].setParamVel2(0);
lmotor[0].setParamVel3(0);
} else if (cmd == 'z' || cmd == 'Z') {
lmotor[0].setParamVel(0);
lmotor[0].setParamVel2(0);
lmotor[0].setParamVel3(-10.1);
} else if (cmd == 'w' || cmd == 'W') {
lmotor[1].setParamVel(10.1);
lmotor[1].setParamVel2(0);
lmotor[1].setParamVel3(0);
} else if (cmd == 'e' || cmd == 'E') {
lmotor[1].setParamVel(0);
lmotor[1].setParamVel2(0);
lmotor[1].setParamVel3(0);
} else if (cmd == 'r' || cmd == 'R') {
lmotor[1].setParamVel(-10.1);
lmotor[1].setParamVel2(0);
lmotor[1].setParamVel3(0);
public void command (char cmd) {
// get the forces
double linear = lmotor[0].getParam(DJoint.PARAM_N.dParamVel3);
double angular = amotor[1].getParam(DJoint.PARAM_N.dParamVel1);
// change the forces based on the key
switch(cmd) {
case 'q': case 'Q': linear += ACCELERATION; break;
case 'a': case 'A': linear =0; break;
case 'z': case 'Z': linear -= ACCELERATION; break;
case 'w': case 'W': angular += ACCELERATION; break;
case 'e': case 'E': angular =0; break;
case 'r': case 'R': angular -= ACCELERATION; break;
default: break;
}
// report the new forces
System.out.println("linear="+linear+" angular="+angular);
// apply the new forces
lmotor[0].setParamVel3(linear);
amotor[1].setParamVel(angular);
}



private static void nearCallback (Object data, DGeom o1, DGeom o2)
{
private void nearCallback (Object data, DGeom o1, DGeom o2) {
// exit without doing anything if the two bodies are connected by a joint
Copy link
Copy Markdown
Author

@i-make-robots i-make-robots Nov 4, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This comment is misleading. Was it supposed to be a TODO?

Copy link
Copy Markdown
Owner

@tzaeschke tzaeschke Nov 5, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The comment is not from me. I will have a look.

DBody b1 = o1.getBody();
DBody b2 = o2.getBody();
Expand All @@ -129,24 +120,15 @@ private static void nearCallback (Object data, DGeom o1, DGeom o2)
}
}

// simulation loop
private static DNearCallback nearCallback = new DNearCallback() {

@Override
public void call(Object data, DGeom o1, DGeom o2) {
nearCallback(data, o1, o2);

}

};
private static void simLoop (boolean pause)
{
@Override
public void step(boolean pause) {
if (!pause) {
OdeHelper.spaceCollide(space,null,nearCallback);
OdeHelper.spaceCollide(space,null, this::nearCallback);
world.quickStep (0.05);
contactgroup.empty ();
}

// draw the two boxes
DVector3C sides1 = geom[0].getLengths();
DVector3C sides2 = geom[1].getLengths();
dsSetTexture (DS_TEXTURE_NUMBER.DS_WOOD);
Expand All @@ -156,56 +138,66 @@ private static void simLoop (boolean pause)
dsDrawBox (body[1].getPosition(), body[1].getRotation(), sides2);
}


/**
* @param args args
*/
public static void main (String[] args)
{
public static void main (String[] args) {
new DemoMotor().demo(args);
}


private void demo(String[] args) {

// create world
OdeHelper.initODE2(0);
contactgroup = OdeHelper.createJointGroup();
world = OdeHelper.createWorld();
space = OdeHelper.createSimpleSpace (null);

// create a mass descriptor
DMass m = OdeHelper.createMass();
m.setBox (1,SIDE,SIDE,SIDE);
m.adjust (MASS);

// box 0
body[0] = OdeHelper.createBody (world);
body[0].setMass (m);
body[0].setPosition (0,0,1);
geom[0] = OdeHelper.createBox(space,SIDE,SIDE,SIDE);
geom[0].setBody (body[0]);

// box 1
body[1] = OdeHelper.createBody (world);
body[1].setMass (m);
body[1].setPosition (0,0,2);
geom[1] = OdeHelper.createBox(space,SIDE,SIDE,SIDE);

geom[0].setBody (body[0]);
geom[1] = OdeHelper.createBox(space,SIDE,SIDE,SIDE);
geom[1].setBody (body[1]);

// linear motor 0 connects two boxes together
lmotor[0] = OdeHelper.createLMotorJoint (world,null);
lmotor[0].attach (body[0],body[1]);

// linear motor 1 connects first box with world
lmotor[1] = OdeHelper.createLMotorJoint (world,null);
lmotor[1].attach (body[0],null);

// angular motor 0 connects two boxes together
amotor[0] = OdeHelper.createAMotorJoint (world,null);
amotor[0].attach (body[0],body[1]);

// angular motor 1 connects first box with world
amotor[1] = OdeHelper.createAMotorJoint (world,null);
amotor[1].attach (body[0], null);

// set the axies and forces for each motor.
// axies are in world space, regardless of the body they are attached to.

for (int i=0; i<2; i++) {
amotor[i].setNumAxes( 3);
amotor[i].setNumAxes(3);
amotor[i].setAxis(0,1,1,0,0);
amotor[i].setAxis(1,1,0,1,0);
amotor[i].setAxis(2,1,0,0,1);
amotor[i].setParamFMax(0.00001);
amotor[i].setParamFMax2(0.00001);
amotor[i].setParamFMax3(0.00001);
amotor[i].setParamFMax (FORCE_MAX_ANGULAR);
amotor[i].setParamFMax2(FORCE_MAX_ANGULAR);
amotor[i].setParamFMax3(FORCE_MAX_ANGULAR);

amotor[i].setParamVel(0);
amotor[i].setParamVel2(0);
Expand All @@ -216,9 +208,13 @@ private void demo(String[] args) {
lmotor[i].setAxis(1,1,0,1,0);
lmotor[i].setAxis(2,1,0,0,1);

lmotor[i].setParamFMax(0.0001);
lmotor[i].setParamFMax2(0.0001);
lmotor[i].setParamFMax3(0.0001);
lmotor[i].setParamFMax (FORCE_MAX_LINEAR);
lmotor[i].setParamFMax2(FORCE_MAX_LINEAR);
lmotor[i].setParamFMax3(FORCE_MAX_LINEAR);

lmotor[i].setParamVel(0);
lmotor[i].setParamVel2(0);
lmotor[i].setParamVel3(0);
}

// run simulation
Expand All @@ -230,13 +226,6 @@ private void demo(String[] args) {
OdeHelper.closeODE();
}


@Override
public void step(boolean pause) {
simLoop(pause);
}


@Override
public void stop() {
//Nothing
Expand Down