public void setHome(Vector3f newhome) {
HOME_X = newhome.x;
HOME_Y = newhome.y;
HOME_Z = newhome.z;
motion_future.rotateBase(0f, 0f);
motion_future.moveBase(new Vector3f(0, 0, 0));
moveIfAble();
}
public RotaryStewartPlatform2() {
super();
setDisplayName("Rotary Stewart Platform 2");
/*
// set up bounding volumes
for(int i=0;i<volumes.length;++i) {
volumes[i] = new Cylinder();
}
volumes[0].radius=3.2f;
volumes[1].radius=3.0f*0.575f;
volumes[2].radius=2.2f;
volumes[3].radius=1.15f;
volumes[4].radius=1.2f;
volumes[5].radius=1.0f*0.575f;*/
motion_now.rotateBase(0, 0);
motion_now.updateIKEndEffector();
motion_now.rebuildShoulders();
motion_now.updateIKWrists();
motion_future.set(motion_now);
setupModels();
// find the starting height of the end effector at home position
// @TODO: project wrist-on-bicep to get more accurate distance
float aa = (motion_now.arms[0].elbow.y - motion_now.arms[0].wrist.y);
float cc = FOREARM_LENGTH;
float bb = (float) Math.sqrt((cc * cc) - (aa * aa));
aa = motion_now.arms[0].elbow.x - motion_now.arms[0].wrist.x;
cc = bb;
bb = (float) Math.sqrt((cc * cc) - (aa * aa));
motion_now.finger_tip.set(0, 0, bb + BASE_TO_SHOULDER_Z - WRIST_TO_FINGER_Z);
motion_future.finger_tip.set(motion_now.finger_tip);
moveIfAble();
}