xPendulumJoint.setInitialState(0.1, 0.6);
yPendulumJoint.setInitialState(0.4, 0.0);
// Visualizers:
final DynamicGraphicPosition penPositionViz = new DynamicGraphicPosition("penPosition", "", registry, 0.01, YoAppearance.Purple());
final DynamicGraphicPosition deskCenterViz = new DynamicGraphicPosition("deskCenter", "", registry, 0.01, YoAppearance.Orange());
final DynamicGraphicPosition penProjectionOntoDeskViz = new DynamicGraphicPosition("penProjectionOntoDesk", "", registry, 0.01, YoAppearance.DarkBlue());
// final BagOfBalls bagOfBalls = new BagOfBalls(registry, dynamicGraphicObjectsListRegistry);
DynamicGraphicObjectsList dynamicGraphicObjectsList = new DynamicGraphicObjectsList("PenAndDesk");
dynamicGraphicObjectsList.add(penPositionViz);
dynamicGraphicObjectsList.add(deskCenterViz);
dynamicGraphicObjectsList.add(penProjectionOntoDeskViz);
dynamicGraphicObjectsListRegistry.registerDynamicGraphicObjectsList(dynamicGraphicObjectsList);
rotationalKineticEnergy.set(this.computeRotationalKineticEnergy());
translationalKineticEnergy.set(this.computeTranslationalKineticEnergy());
FunctionToIntegrate weldAndPenFunctionToIntegrate = new FunctionToIntegrate()
{
private final Point3d xArmPosition = new Point3d();
private final Point3d yArmPosition = new Point3d();
private final Vector3d xArmVelocity = new Vector3d();
private final Vector3d yArmVelocity = new Vector3d();
private final Transform3D transformFromDeskToWorld = new Transform3D();
private final Transform3D transformFromWorldToDesk = new Transform3D();
private final Point3d penPosition = new Point3d();
private final Point3d penProjectionInDeskFrame = new Point3d();
private final Point3d deskCenterPosition = new Point3d();
private final Vector3d deskSurfaceNormal = new Vector3d();
private final Vector3d deskCenterToPen = new Vector3d();
private final Vector3d penProjectionOntoDeskNormal = new Vector3d();
private final Point3d penProjectionOntoDesk = new Point3d();
public int getVectorSize()
{
return 0;
}
public DoubleYoVariable[] getOutputVariables()
{
return new DoubleYoVariable[]{};
}
public double[] computeDerivativeVector()
{
xArmExternalForcePoint.getPosition(xArmPosition);
xArmExternalForcePoint.getVelocity(xArmVelocity);
yArmExternalForcePoint.getPosition(yArmPosition);
yArmExternalForcePoint.getVelocity(yArmVelocity);
Vector3d xArmToYArm = new Vector3d();
xArmToYArm.sub(yArmPosition, xArmPosition);
Vector3d xArmToYArmVelocity = new Vector3d();
xArmToYArmVelocity.sub(yArmVelocity, xArmVelocity);
Vector3d armForcesKp = new Vector3d();
Vector3d armForcesKd = new Vector3d();
armForcesKp.set(xArmToYArm);
armForcesKp.scale(kpArmWeld.getDoubleValue());
armForcesKd.set(xArmToYArmVelocity);
armForcesKd.scale(kdArmWeld.getDoubleValue());
Vector3d armForces = new Vector3d();
armForces.add(armForcesKp, armForcesKd);
xArmExternalForcePoint.setForce(armForces);
armForces.negate();
yArmExternalForcePoint.setForce(armForces);
// Pen onto desk:
deskPendulumYJoint.getTransformToWorld(transformFromDeskToWorld);
transformFromWorldToDesk.set(transformFromDeskToWorld);
transformFromWorldToDesk.invert();
deskSurfaceNormal.set(0.0, 0.0, 1.0);
transformFromDeskToWorld.transform(deskSurfaceNormal);
deskCenterExternalForcePoint.getPosition(deskCenterPosition);
penExternalForcePoint.getPosition(penPosition);
deskCenterToPen.set(penPosition);
deskCenterToPen.sub(deskCenterPosition);
penToDeskDistance.set(deskCenterToPen.dot(deskSurfaceNormal));
penIsAboveDesk.set(penToDeskDistance.getDoubleValue() > 0.0);
penProjectionOntoDeskNormal.set(deskSurfaceNormal);
penProjectionOntoDeskNormal.scale(penToDeskDistance.getDoubleValue());
penProjectionOntoDesk.set(penPosition);
penProjectionOntoDesk.sub(penProjectionOntoDeskNormal);
penProjectionInDeskFrame.set(penProjectionOntoDesk);
transformFromWorldToDesk.transform(penProjectionInDeskFrame);
// Only apply a force on the pen, not the desk. Doesn't follow Newton's third law, but the force is probably pretty small anyway.
// Maybe later we'll track the other force point, and perhaps even check if energy is conserved...
if (!penIsAboveDesk.getBooleanValue())
{
Vector3d forceOnPen = new Vector3d();
forceOnPen.set(penProjectionOntoDeskNormal);
forceOnPen.scale(-kpPenOnDesk.getDoubleValue());
double maxPenForce = 5.0;
if (forceOnPen.length() > maxPenForce)
{
forceOnPen.normalize();
forceOnPen.scale(maxPenForce);
}
penExternalForcePoint.setForce(forceOnPen);
if (getTime() > 10.0) harmonographPaperJPanel.addPoint(penProjectionInDeskFrame);
// bagOfBalls.setBall(new FramePoint(ReferenceFrame.getWorldFrame(), penPosition));
}
else
{
penExternalForcePoint.setForce(new Vector3d());
}
if (clearPoints.getBooleanValue())
{
harmonographPaperJPanel.clearPoints();
clearPoints.set(false);
}
// Visualizers:
penPositionViz.setPosition(penPosition.x, penPosition.y, penPosition.z);
deskCenterViz.setPosition(deskCenterPosition.x, deskCenterPosition.y, deskCenterPosition.z);
penProjectionOntoDeskViz.setPosition(penProjectionOntoDesk.x, penProjectionOntoDesk.y, penProjectionOntoDesk.z);
computeEnergies();
return new double[]{};
}