//
// Inverse Kinematics
//
// Fred Chen & Loren Tsai
// CS184 Fall 98
import vrml.*;
import vrml.node.*;
import vrml.field.*;
import java.io.*;
import java.util.*;
public class arm2InvKin extends Script {
// fields
private SFNode Arm;
private SFFloat UpperLength;
private SFFloat LowerLength;
// eventOuts
private SFVec3f position_changed;
private SFRotation Up;
private SFRotation Side;
private SFRotation Out;
private SFRotation LowerArm;
// internal variables
private SFRotation init_up;
private SFRotation init_side;
private SFRotation init_out;
private SFRotation init_arm;
private boolean first_val = true;
private float init_position[] = new float[3];
private float last_position[] = new float[3];
private float next_position[] = new float[3];
private float l1;
private float l2;
private float l3;
private double alpha;
private double beta;
private double phi;
private double uprange[] = new double[2];
private double siderange[] = new double[2];
private double outrange[] = new double[2];
private double armrange[] = new double[2];
private SFVec3f Path[];
private int steps = 20;
private int iter = 0;
private float x;
private float y;
private float z;
// This method is called when the script is loaded
public void initialize()
{
// Get handles to fields from the VRML environment
Arm = (SFNode) getField("Arm");
UpperLength = (SFFloat) getField("UpperLength");
LowerLength = (SFFloat) getField("LowerLength");
// Get handles to eventOuts from the VRML environment
position_changed = (SFVec3f) getEventOut("position_changed");
Up = (SFRotation) getEventOut("Up");
Side = (SFRotation) getEventOut("Side");
Out = (SFRotation) getEventOut("Out");
LowerArm = (SFRotation) getEventOut("LowerArm");
// Initialize internal variables
init_up = (SFRotation) ((Node) Arm.getValue()).getExposedField("UpperarmUp");
init_side = (SFRotation) ((Node) Arm.getValue()).getExposedField("ShoulderSide");
init_out = (SFRotation) ((Node) Arm.getValue()).getExposedField("ShoulderOut");
init_arm = (SFRotation) ((Node) Arm.getValue()).getExposedField("LowerarmUp");
l1 = UpperLength.getValue();
l2 = LowerLength.getValue();
l3 = l1+l2;
for(int i=0;i<3;i++) {
init_position[i] = 0;
}
for(int i=0;i<2;i++) {
if (i==0) {
uprange[i] = -1.57;
siderange[i] = -1.6;
outrange[i] = 0;
armrange[i] = 0;
}
else if (i==1) {
uprange[i] = 2.3;
siderange[i] = 0.1;
outrange[i] = 1.57;
armrange[i] = 2.6;
}
}
// Initialize Path to walk
/*Path = new SFVec3f[steps];
float temp[] = new float[3];
double angle = 2;
double netHeight = 3;
for(int i=0;i<steps;i++) {
Path[i] = new SFVec3f();
if (i<steps/4) {
temp[0] = 0;
temp[1] = (float) (i*netHeight*4/steps);
temp[2] = (float) (-(l1+l2)*Math.sin(4*angle*i/steps));
}
else if (i< (3*steps/4)) {
temp[0] = 0;
temp[1] = (float) netHeight;
temp[2] = (float) (4*i*(l1+l2)*Math.sin(angle)/steps);
}
else if (i<steps) {
temp[0] =0;
temp[1] = (float) netHeight*(1 - ((4*i-3*steps)/steps));
temp[2] = (float) (-(l1+l2)*Math.sin(4*angle*(1 - (4*i-3*steps)/steps)));
}
Path[i].setValue(temp);
}*/
}
//eventIns
private void set_position(ConstSFVec3f newpos) {
if(first_val == true) {
newpos.getValue(init_position);
newpos.getValue(last_position);
first_val = false;
}
else {
newpos.getValue(next_position);
for(int i=0;i<3;i++)
last_position[i] = last_position[i] + next_position[i];
x = next_position[0] - init_position[0];
y = next_position[1] - init_position[1];
z = next_position[2] - init_position[2];
x = 3*x/10; // scale factor of Hand object
y = 4*y/10;
z = 2*z/10;
l3 = (float) Math.sqrt((l1+l2-y)*(l1+l2-y) + x*x);
//l3 = (float) Math.sqrt(y*y + z*z);
if ( (Math.abs(y) > l1+l2) ) {
}
else {
beta = Math.acos((l3*l3 - l1*l1 - l2*l2)/(2*l1*l2));
alpha = Math.atan((z+l2*Math.sin(beta))/(l1+l2*Math.cos(beta)));
//if (z < 0)
//alpha = -alpha;
position_changed.setValue(next_position);
}
}
}
private void set_upperarmup(ConstSFVec3f newpos) {
float temp[] = new float[4];
if (alpha < uprange[0])
alpha = uprange[0];
else if (alpha > uprange[1])
alpha = uprange[1];
init_up.getValue(temp);
temp[3] = (float) alpha;
Up.setValue(temp);
}
private void set_shoulderout(ConstSFVec3f newpos) {
l3 = (float) Math.sqrt(l1*l1 + l2*l2 + 2*l1*l2*Math.cos(beta));
}
private void set_shoulderside(ConstSFVec3f newpos) {
float temp[] = new float[4];
l3 = (float) Math.sqrt(l1*l1 + l2*l2 + 2*l1*l2*Math.cos(beta));
phi = Math.atan(x/(l1+l2-y));
if (phi < siderange[0])
phi = siderange[0];
else if (phi > siderange[1])
phi = siderange[1];
init_side.getValue(temp);
temp[3] = (float) phi;
Side.setValue(temp);
}
private void set_arm(ConstSFVec3f newpos) {
float temp[] = new float[4];
//float x = next_position[0] - init_position[0];
//float y = next_position[1] - init_position[1];
//float z = next_position[2] - init_position[2];
//x = x/5; // scale factor of Foot object
//y = y/5;
//z = z/5;
//beta = 3.14-Math.acos((y*y + z*z - l1*l1 - l2*l2)/(2*l1*l2));
if (beta < armrange[0])
beta = armrange[0];
else if (beta > armrange[1])
beta = armrange[1];
init_arm.getValue(temp);
temp[3] = (float) beta;
LowerArm.setValue(temp);
}
private void nextStep(float frac) {
float temp[] = new float[3];
Path[iter].getValue(next_position);
iter = iter+1;
if (iter >= steps)
iter = 0;
position_changed.setValue(temp);
}
public void processEvent (Event e) {
String EventName = e.getName();
if (EventName.equals("set_position"))
set_position((ConstSFVec3f)e.getValue());
else if (EventName.equals("set_upperarmup"))
set_upperarmup((ConstSFVec3f)e.getValue());
else if (EventName.equals("set_shoulderout"))
set_shoulderout((ConstSFVec3f)e.getValue());
else if (EventName.equals("set_shoulderside"))
set_shoulderside((ConstSFVec3f)e.getValue());
else if (EventName.equals("set_arm"))
set_arm((ConstSFVec3f)e.getValue());
else if (EventName.equals("nextStep"))
nextStep(((ConstSFFloat)e.getValue()).getValue());
}
}