private Vector3D exponentialCutForceModel(ExternalForcePoint forcePoint) { tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); if(tangentVector.length() != 0.0) { tangentionalVelocity.setX(0.0); tangentVector.setX(0.0); tangentVector.normalize(); tangentVector.scale(-1.0); tangentVector.scale(90.0 * (Math.exp(1.0 *tangentionalVelocity.length()) - 1.0)); efpHandControlFrameVelocity.set(tangentionalVelocity.length()); return tangentVector; } else { return tangentVector; } } }
private Vector3d exponentialCutForceModel(ExternalForcePoint forcePoint) { tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); if(tangentVector.length() != 0.0) { tangentionalVelocity.setX(0.0); tangentVector.setX(0.0); tangentVector.normalize(); tangentVector.scale(-1.0); tangentVector.scale(90.0 * (Math.exp(1.0 *tangentionalVelocity.length()) - 1.0)); efpHandControlFrameVelocity.set(tangentionalVelocity.length()); return tangentVector; } else { return tangentVector; } } }
private Vector3d exponentialCutForceModel(ExternalForcePoint forcePoint) { tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); if(tangentVector.length() != 0.0) { tangentionalVelocity.setX(0.0); tangentVector.setX(0.0); tangentVector.normalize(); tangentVector.scale(-1.0); tangentVector.scale(90.0 * (Math.exp(1.0 *tangentionalVelocity.length()) - 1.0)); efpHandControlFrameVelocity.set(tangentionalVelocity.length()); return tangentVector; } else { return tangentVector; } } }
private Vector3d quadraticCutForceModel(ExternalForcePoint forcePoint) tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); tangentVector.scale(quadraticForceCoeff.getDoubleValue() * Math.pow(forcePoint.getVelocityVector().length(), 2)); climbingForceVector.scale(tangentionalVelocity.length() * 100.0);
private Vector3D quadraticCutForceModel(ExternalForcePoint forcePoint) tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); tangentVector.scale(quadraticForceCoeff.getDoubleValue() * Math.pow(forcePoint.getVelocityVector().length(), 2)); climbingForceVector.scale(tangentionalVelocity.length() * 100.0);
private Vector3d quadraticCutForceModel(ExternalForcePoint forcePoint) tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); tangentVector.scale(quadraticForceCoeff.getDoubleValue() * Math.pow(forcePoint.getVelocityVector().length(), 2)); climbingForceVector.scale(tangentionalVelocity.length() * 100.0);