private ITrajectory copyToTrajectoryImpl(IActuator actuator, ITrajectory trajectory) {
ITrajectory trajectoryImpl = null;
// System.out.println("copyToTrajectoryImpl= " + name + " trajectory= " + trajectory);
if (trajectory != null && actuator != null) {
if (trajectory instanceof ITrajectoryK) {
ITrajectoryK trajectoryK = (ITrajectoryK) trajectory;
TrajectoryKImpl trajectoryKImpl = new TrajectoryKImpl();
trajectoryKImpl.setRelative(false);
trajectoryKImpl.setE0(trajectoryK.getE0());
trajectoryKImpl.setE1(trajectoryK.getE1());
trajectoryKImpl.setE2(trajectoryK.getE2());
trajectoryKImpl.setEDeltaEdge(trajectoryK.getEDeltaEdge());
trajectoryKImpl.setEDeltaPreEdge(trajectoryK.getEDeltaPreEdge());
trajectoryKImpl.setEMin(trajectoryK.getEMin());
trajectoryKImpl.setKDelta(trajectoryK.getKDelta());
trajectoryKImpl.setKMax(trajectoryK.getKMax());
trajectoryKImpl.setKMin(trajectoryK.getKMin());
trajectoryKImpl.setM(trajectoryK.getM());
trajectoryKImpl.setN(trajectoryK.getN());
trajectoryImpl = trajectoryKImpl;
}
else {
trajectoryImpl = new TrajectoryImpl();
Double beginPosition = trajectory.getBeginPosition();