/*
* File name: SnakeAgent.java (package eas.users.fabian.snake)
* Author(s): Fabian
* Java version: 6.0
* Generation date: 02.02.2012 (13:28:02)
*
* Adapted work from Dr. Juan Gonzalez-Gomez *
*
* (c) This file and the EAS (Easy Agent Simulation) framework containing it
* is protected by Creative Commons by-nc-sa license. Any altered or
* further developed versions of this file have to meet the agreements
* stated by the license conditions.
*
* In a nutshell
* -------------
* You are free:
* - to Share -- to copy, distribute and transmit the work
* - to Remix -- to adapt the work
*
* Under the following conditions:
* - Attribution -- You must attribute the work in the manner specified by the
* author or licensor (but not in any way that suggests that they endorse
* you or your use of the work).
* - Noncommercial -- You may not use this work for commercial purposes.
* - Share Alike -- If you alter, transform, or build upon this work, you may
* distribute the resulting work only under the same or a similar license to
* this one.
*
* + Detailed license conditions (Germany):
* http://creativecommons.org/licenses/by-nc-sa/3.0/de/
* + Detailed license conditions (unported):
* http://creativecommons.org/licenses/by-nc-sa/3.0/deed.en
*
* This header must be placed in the beginning of any version of this file.
*/
package eas.users.students.fabian.snake;
import org.ode4j.math.DVector3;
import org.ode4j.ode.DBody;
import org.ode4j.ode.DGeom;
import org.ode4j.ode.DHingeJoint;
import org.ode4j.ode.DMass;
import org.ode4j.ode.OdeHelper;
import eas.plugins.standard.visualization.visualization3D.VideoPluginLWJGL;
import eas.plugins.standard.visualization.visualization3D.VideoPluginLWJGL.DS_TEXTURE_NUMBER;
import eas.simulation.spatial.sim3D.physicalSimulation.standardAgents.AbstractAgentODE3D;
import eas.startSetup.ParCollection;
/**
* @author Fabian
*
*/
public class Snake extends AbstractAgentODE3D<SnakeEnv> {
/**
*
*/
private static final long serialVersionUID = -7439434446851645912L;
private DVector3 lastPos;
private final double TORQUE = 2000;
private final double L = .72;
private final double W = .52;
private final double H = W;
private final double MASS = 0.5;
private final double WMAX = 13. * Math.PI / 9.;// 13. * Math.PI / 9.;
private final double ERROR_THRESHOLD = Math.toRadians(7);
private final int N = 32;
// Servo properties
private final double KP = 10;
// Horizontal/Vertical amplitudes
private double Av, Ah;
// Phase differences
private double PDv, PDh, PDvh;
// Initial Phase
private double F0;
// Horizontal Offset
private double Oh;
// Vertical Offset
private double Ov;
// Snake's direction
private int direction = -1;
// Number to sample sin function
private int n = 0;
private final int numberOfModules;
private DHingeJoint joints[];
private double servoRefPos[];
private int lastChoice = 0;
public Snake(int id, SnakeEnv env, ParCollection params,
int numberOfModules) {
super(id, env, params);
// initialize necessary variables
this.numberOfModules = numberOfModules;
servoRefPos = new double[numberOfModules];
// masses = new DMass[1];
DMass mass = OdeHelper.createMass();
masses.add(mass);
// bodies = new DBody[numberOfModules + 1];
// geoms = new DGeom[numberOfModules * 2];
joints = new DHingeJoint[numberOfModules];
// create bodies and geoms for the modules
{
// create left body
DBody body = OdeHelper.createBody(env.getWorld());
body.setPosition(0, -L / 4, H / 2);
mass.setBoxTotal(MASS / 2, W, L / 2, H);
body.setMass(mass);
DGeom geom = OdeHelper.createBox(env.getSpace(), W, L / 2, H);
geom.setBody(body);
bodies.add(body);
geoms.add(geom);
// create central bodies
for (int i = 1; i < numberOfModules; i++) {
body = OdeHelper.createBody(env.getWorld());
body.setPosition(0, -L * (i), H / 2);
mass.setBoxTotal(MASS, W, L / 2, H);
body.setMass(mass);
geom = OdeHelper.createBox(env.getSpace(), W, L / 2, H);
DGeom geom2 = OdeHelper.createBox(
env.getSpace(), W, L / 2, H);
geom.setBody(body);
geom2.setBody(body);
geom.setOffsetPosition(0, L / 4, 0);
geom2.setOffsetPosition(0, -L / 4, 0);
bodies.add(body);
geoms.add(geom);
geoms.add(geom2);
}
// create right body
body = OdeHelper.createBody(env.getWorld());
body.setPosition(0,
(-(numberOfModules) * L + L / 4), H / 2);
mass.setBoxTotal(MASS / 2, W, L / 2, H);
body.setMass(mass);
geom = OdeHelper.createBox(
env.getSpace(), W, L / 2, H);
geom.setBody(body);
bodies.add(body);
geoms.add(geom);
}
// create hinges
for (int i = 0; i < numberOfModules; i++) {
joints[i] = OdeHelper.createHingeJoint(env.getWorld());
joints[i].attach(bodies.get(i), bodies.get(i+1));
if (i % 2 == 0) {
joints[i].setAxis(1, 0, 0);
} else {
joints[i].setAxis(0, 0, 1);
}
joints[i].setAnchor(0, (-i * L) - (L / 2), H / 2);
joints[i].setParamFMax(TORQUE);
joints[i].setParamVel(0);
}
lastPos = new DVector3(getPosition());
}
/*-------------------------------------------------------------------------*/
/* Servos simulation */
/* This function performs a simulation step in the servos. */
/* A proportional controller is used, with a KP gain. */
/* The current servo angle is read and compared to the reference position */
/* This difference is the error. Then the servo angular velocity is set. */
/* It is proportional to the error, but it has a maximum value (WMAX). */
/* */
/* If all the servos have reached their reference positions, the */
/* sequence_generation() function is called for calculating the next */
/* reference angles */
/* */
/* For obtaining a smoother simulation the servos are not allowed to reach */
/* their reference position. When the servo angle is near the reference */
/* position (and therefore the servo velocity is not 0) the new position is */
/* calculated. This way, the servos never stop and the movement is */
/* smoother. */
/*-------------------------------------------------------------------------*/
public void handleServos() {
int stable = 0;
for (int i = 0; i < numberOfModules; i++) {
double pos = joints[i].getAngle();
double error = pos - Math.toRadians(servoRefPos[i]);
double velocity = -error * KP;
if (velocity > WMAX)
velocity = WMAX;
if (velocity < -WMAX)
velocity = -WMAX;
joints[i].setParamVel(velocity);
if (Math.abs(error) < ERROR_THRESHOLD)
stable++;
if (stable == numberOfModules) {
sequenceGeneration();
}
}
}
/*---------------------------------------------------------------------------*/
/* Simulation of the sinusoidal oscillators. This function is called */
/* every time all the servos have reached their reference positions. The */
/* next servo positions are calculated by means of sampling sinusoidal */
/* functions */
/* The n variable is the discrete time */
/* The function for calculating the servo position depends on the type of */
/* module: horizontal (yawing) or vertical (pitching) */
/* The equations are: */
/* -Vertical servos: */
/* pos(i) = Av*sin(2*PI*n/N + PDv*(i/2) + F0); */
/* -Horizontal servos: */
/* pos(i) = Ah*sin(2*PI*n/N + PDh(i-1)/2 + PDvh + F0) + Oh */
/* Where N is the total number of samples. The default is 32 */
/*----------------------------------------------------------------------------*/
private void sequenceGeneration() {
double phase;
for (int i = 0; i < numberOfModules; i++) {
if (i % 2 == 0) {
phase = PDv * (i / 2) + F0;
servoRefPos[i] = Av
* Math.sin(2 * Math.PI * n / N + Math.toRadians(phase))
+ Ov;
} else {
phase = PDh * (i - 1) / 2 + PDvh + F0;
servoRefPos[i] = Ah
* Math.sin(2 * Math.PI * n / N + Math.toRadians(phase))
+ Oh;
}
}
n = (n + 1) % N;
}
/**
* Lets the snake move in a straight line.
*
* @param alpha
* winding angle (0-120 degrees)
* @param kv
* number of vertical undulations
*/
public void setMotionStraight(double alpha, int kv) {
if (lastChoice != 1) {
System.out
.println("Moving straight with a winding angle of " + alpha
+ " degrees and " + kv + " vertical undulation(s).");
}
lastChoice = 1;
direction *= -1;
// No horizontal wave
Ah = 0;
Oh = 0;
F0 = 0;
// Vertical wave
Ov = 0;
Av = 2 * alpha * Math.sin(2 * Math.PI * kv / numberOfModules);
PDv = direction * (4 * Math.PI * kv / numberOfModules) * 180 / Math.PI;
}
/**
* Lets the snake move in a circular arc.
*
* @param alphah
* arc's angle (0-360 degrees)
* @param kv
* number of vertical undulations
*/
public void setMotionTurning(double alpha, double alphah, int kv) {
if (lastChoice != 2) {
System.out.println("Turning with an arc angle of " + alphah
+ " degrees, a winding angle of " + alpha + " degrees and "
+ kv + " vertical undulation(s).");
}
lastChoice = 2;
direction *= -1;
// no horizontal wave
Ah = 0;
F0 = 0;
// set yawing points to a fixed value
Oh = 2 * alphah / numberOfModules;
// vertical wave
Ov = 0;
Av = 2 * alpha * Math.sin(2 * Math.PI * kv / numberOfModules);
PDv = direction * (4 * Math.PI * kv / numberOfModules) * 180 / Math.PI;
}
/**
* Lets the snake move in a side-winding manner.
*
* @param alphah
* horizontal windig angle (0-120 degrees)
* @param kh
* number of horizontal undulations
*/
public void setMotionSideWinding(double alphah, int kh) {
if (lastChoice != 3) {
System.out
.println("Side winding with a horizontal winding angle of "
+ alphah + " degrees and " + kh
+ " horizontal undulation(s).");
}
lastChoice = 3;
direction *= -1;
// number of undulations is equal both horizontally and vertically
double kv = kh;
// there is a low amplitude vertical wave
Ov = 0;
Av = 2 * 5 * Math.sin(2 * Math.PI * kv / numberOfModules);
PDv = direction * (4 * Math.PI * kv / numberOfModules) * 180 / Math.PI;
// there is a horizontal wave. Both the vertical and horizontal waves
// have the same undulations
Oh = 0;
Ah = 2 * alphah * Math.sin(2 * Math.PI * kh / numberOfModules);
PDh = PDv;
PDvh = 0;
}
/**
* Lets the snake move in an inclined side-winding manner.
*
* @param alpha
* winding angle (0-120 degrees)
* @param k
* number of undulations
*/
public void setMotionInclinedSideWinding(double alpha, int k) {
if (lastChoice != 4) {
System.out.println("Inclined side winding with a winding angle of "
+ alpha + " degrees and " + k + " undulation(s).");
}
lastChoice = 4;
direction *= -1;
// tilt the snake
double alphav = alpha * Math.sin(45);
double alphah = alphav;
// same number of undulations both vertically and horizontally
double kv = k;
double kh = k;
// vertical wave
Ov = 0;
Av = 2 * alphav * Math.sin(2 * Math.PI * kv / numberOfModules);
PDv = direction * (4 * Math.PI * kv / numberOfModules) * 180 / Math.PI;
Oh = 0;
Ah = 2 * alphah * Math.sin(2 * Math.PI * kh / numberOfModules);
PDh = PDv;
PDvh = 0;
}
/**
* Lets the snake flap.
*/
public void setMotionFlapping() {
if (lastChoice != 5) {
System.out.println("Flapping.");
}
lastChoice = 5;
Av = 4.5;
PDv = 9.0;
Ah = 4.5;
PDh = 9.0;
Ov = 0;
Oh = 0;
PDvh = 0;
}
/**
* Lets the snake move in an s-shaped rotation.
*
* @param alphah
* winding angle (0-120 degrees)
* @param kh
* number of horizontal undulations
*/
public void setMotionRotatingS(double alphah, int kh) {
if (lastChoice != 6) {
System.out
.println("S-shaped rotation with a horizontal winding angle of "
+ alphah
+ " degrees and "
+ kh
+ " horizontal undulation(s).");
}
lastChoice = 6;
direction *= -1;
double kv = 2 * kh;
// vertical wave with low amplitude and double the number of
Ov = 0;
// horizontal undulations
Av = 2 * 5 * Math.sin(2 * Math.PI * kv / numberOfModules);
PDv = direction * (4 * Math.PI * kv / numberOfModules) * 180 / Math.PI;
// horizontal wave with bigger amplitude
Oh = 0;
Ah = 2 * alphah * Math.sin(2 * Math.PI * kh / numberOfModules);
PDh = direction * (4 * Math.PI * kh / numberOfModules) * 180 / Math.PI;
PDvh = 0;
}
/**
* Lets the snake move in a u-shaped rotation.
*
* @param alpha
* horizontal arc angle (0-360 degrees)
*/
public void setMotionRotatingU(double alpha) {
if (lastChoice != 7) {
System.out.println("U-Shaped rotation with an arc angle of "
+ alpha + " degrees.");
}
lastChoice = 7;
direction *= -1;
double kv = 2;
// low vertical amplitude
Ov = 0;
Av = 2 * 5 * Math.sin(2 * Math.PI * kv / numberOfModules);
PDv = direction * (4 * Math.PI * kv / numberOfModules) * 180 / Math.PI;
// horizontal joints are in phase
Oh = 0;
Ah = 2 * alpha / numberOfModules;
PDh = 0;
PDvh = direction * 90;
}
/**
* Lets the snake roll.
*
* @param alpha
* arc angle (0-360 degrees)
*/
public void setMotionRolling(double alpha) {
if (lastChoice != 8) {
System.out.println("Rolling with an arc angle of " + alpha
+ " degrees.");
}
lastChoice = 8;
direction *= -1;
// All the vertical joints are in phase
// All the horizontal joints are in phase
// The phase difference between vertical and horizontal is 90 degrees
// The amplitudes should not be so small. If they are, the snake will
// perform the flapping gait.
Ov = 0;
Av = 2 * alpha / numberOfModules;
Ah = Av;
PDv = 0;
PDh = 0;
PDvh = direction * 90;
Oh = 0;
F0 = 90;
}
public void setMotionTest() {
if (lastChoice != 9) {
System.out
.println("Test motion.");
}
lastChoice = 9;
direction *= -1;
Av = 20;
PDv = 90;
Ah = 20;
PDh = 90;
Oh = 0;
Ov = 0;
PDvh = direction*90;
}
public void setMotionTest2() {
if (lastChoice != 10) {
System.out
.println("Test motion.");
}
lastChoice = 10;
direction *= -1;
F0 = 0;
Av = -10;
PDv = 00;
Ov = 10;
Ah = 10;
PDh = 0;
Oh = 0;
PDvh = direction*90;
}
public void setMotionAwesomeSideWinding() {
if (lastChoice != 0) {
System.out
.println("Awesome side-winding.");
}
lastChoice = 0;
direction *= -1;
Av = 45;
PDv = 45;
Ah = 20;
PDh = 45;
Oh = 0;
Ov = 0;
PDvh = direction*90;
}
@Override
public void drawInLWJGL() {
float green = 0;
VideoPluginLWJGL.setTexture(DS_TEXTURE_NUMBER.DS_CHECKERED);
VideoPluginLWJGL.dsSetColor(1, green, 0);
// draw left
VideoPluginLWJGL.drawGeom(geoms.get(0));
// draw all elements in between
for (int i = 1; i < numberOfModules*2-2; i++) {
VideoPluginLWJGL.dsSetColor(1, green, 0);
VideoPluginLWJGL.drawGeom(geoms.get(i));
if (green == 1) {
green = 0;
}
else {
green = 1;
}
i+=1;
VideoPluginLWJGL.dsSetColor(1, green, 0);
VideoPluginLWJGL.drawGeom(geoms.get(i));
}
// draw right
VideoPluginLWJGL.dsSetColor(1, green, 0);
VideoPluginLWJGL.drawGeom(geoms.getLast());//get(numberOfModules * 2 - 1));
}
/*
* (non-Javadoc)
*
* @see eas.simulation.spatial.sim3D.physicalSimulation.standardAgents.
* AbstractAgentODE3D#isDrawableInODE()
*/
@Override
public boolean isDrawableInLWJGL() {
return true;
}
// @Override
// public DVector3 getPosition() {
// double x = 0;
// double y = 0;
// double z = 0;
// int len = bodies.length;
//
// for (int i = 0; i < len; i++) {
// x += bodies[i].getPosition().get0();
// y += bodies[i].getPosition().get1();
// z += bodies[i].getPosition().get2();
// }
// DVector3 vec = new DVector3(x/len, y/len, z/len);
// return vec;
// }
/**
* Return a vector that indicates the change in the agent's position since this method had last been called.
* @return Change in agent's position since last method call.
*/
public DVector3 getChangeInPosition() {
double delta[] = new double[3];
for (int i = 0; i < 3; i ++) {
delta[i] = getPosition().get(i) - lastPos.get(i);
}
lastPos.set(getPosition());
return new DVector3(delta[0],delta[1],delta[2]);
}
}