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 void moveIfAble() {
rotateFinger();
if (motion_future.movePermitted()) {
arm_moved = true;
finalizeMove();
} else {
motion_future.set(motion_now);
}
}
protected void update_ik(float delta) {
boolean changed = false;
motion_future.set(motion_now);
// speeds
final float vtranslate = 10.0f * delta;
final float vrotate = 100.0f * delta;
// lateral moves
if (rDown) {
motion_future.finger_tip.x -= vtranslate;
}
if (fDown) {
motion_future.finger_tip.x += vtranslate;
}
if (tDown) {
motion_future.finger_tip.y += vtranslate;
}
if (gDown) {
motion_future.finger_tip.y -= vtranslate;
}
if (yDown) {
motion_future.finger_tip.z += vtranslate;
}
if (hDown) {
motion_future.finger_tip.z -= vtranslate;
}
if (!motion_now.finger_tip.epsilonEquals(motion_future.finger_tip, vtranslate / 2.0f)) {
changed = true;
}
// rotation
float ru = 0, rv = 0, rw = 0;
if (uDown) rw = 0.1f;
if (jDown) rw = -0.1f;
if (iDown) rv = 0.1f;
if (kDown) rv = -0.1f;
if (oDown) ru = 0.1f;
if (lDown) ru = -0.1f;
if (rw != 0 || rv != 0 || ru != 0) {
motion_future.iku += ru * vrotate;
motion_future.ikv += rv * vrotate;
motion_future.ikw += rw * vrotate;
changed = true;
}
if (changed == true) {
moveIfAble();
}
}
@Override
public void prepareMove(float delta) {
if (pDown) pWasOn = true;
if (!pDown && pWasOn) {
pWasOn = false;
moveMode = !moveMode;
}
if (moveMode) update_ik(delta);
else update_fk(delta);
// before the robot is allowed to do anything it has to be homed
if (this.isReadyToReceive) {
if (!homed) {
if (!homing) {
// we are not homed and we have not begun to home
if (HOME_AUTOMATICALLY_ON_STARTUP == true) {
// this should be sent by a human when they are ready
this.sendLineToRobot("G28");
homing = true;
}
} else {
motion_future.finger_tip.set(
HOME_X, HOME_Y, HOME_Z); // HOME_* should match values in robot firmware.
motion_future.updateInverseKinematics();
finalizeMove();
this.sendLineToRobot("G92 X" + HOME_X + " Y" + HOME_Y + " Z" + HOME_Z);
homing = false;
homed = true;
follow_mode = true;
}
}
}
}
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();
}
@Override
public void finalizeMove() {
// copy motion_future to motion_now
motion_now.set(motion_future);
if (arm_moved) {
if (homed && follow_mode && this.isReadyToReceive) {
arm_moved = false;
this.sendLineToRobot(
"G0 X"
+ motion_now.finger_tip.x
+ " Y"
+ motion_now.finger_tip.y
+ " Z"
+ motion_now.finger_tip.z
+ " U"
+ motion_now.iku
+ " V"
+ motion_now.ikv
+ " W"
+ motion_now.ikw);
}
// TODO: update text fields when the cursor moves. right now this causes inter-thread damage
// and crashes the app
/*
if(view_px!=null) {
view_px.getField().setText(Float.toString(motion_now.finger_tip.x));
viewPy.getField().setText(Float.toString(motion_now.finger_tip.y));
viewPz.getField().setText(Float.toString(motion_now.finger_tip.z));
viewRx.getField().setText(Float.toString(motion_now.iku));
viewRy.getField().setText(Float.toString(motion_now.ikv));
viewRz.getField().setText(Float.toString(motion_now.ikw));
}*/
}
}
protected void update_fk(float delta) {
boolean changed = false;
final float vel = 10.0f;
int i;
for (i = 0; i < 6; ++i) {
motion_future.arms[i].angle = motion_now.arms[i].angle;
}
if (rDown) {
motion_future.arms[1].angle -= vel * delta;
changed = true;
}
if (fDown) {
motion_future.arms[1].angle += vel * delta;
changed = true;
}
if (tDown) {
motion_future.arms[2].angle += vel * delta;
changed = true;
}
if (gDown) {
motion_future.arms[2].angle -= vel * delta;
changed = true;
}
if (yDown) {
motion_future.arms[0].angle += vel * delta;
changed = true;
}
if (hDown) {
motion_future.arms[0].angle -= vel * delta;
changed = true;
}
if (uDown) {
motion_future.arms[3].angle += vel * delta;
changed = true;
}
if (jDown) {
motion_future.arms[3].angle -= vel * delta;
changed = true;
}
if (iDown) {
motion_future.arms[4].angle += vel * delta;
changed = true;
}
if (kDown) {
motion_future.arms[4].angle -= vel * delta;
changed = true;
}
if (oDown) {
motion_future.arms[5].angle += vel * delta;
changed = true;
}
if (lDown) {
motion_future.arms[5].angle -= vel * delta;
changed = true;
}
if (changed == true) {
if (motion_future.checkAngleLimits()) {
motion_future.updateForwardKinematics();
arm_moved = true;
}
}
}
