Trajectory planning with Timefold

[francesco-bufalini]

I am trying to use Timefold to assign a list of Input to a drone (a representation of the command given to the drone expressed as total thrust and torque).
To do this I am using a planning list located in the planning entity Drone. When an Input is assigned to index K of the list (which corresponds to the input that is given to the drone at time DTxK with constant DT) it triggers the calculation of the drone's state (position, speed, etc.) at time DT*K starting from the previous state and input (contained in the element at index K-1). This is possible thanks to a previous element shadow variable and several cascading update shadow variables that are located in the Input class.
Despite the convenience of these exclusive planning list mechanisms, I am forced to create N copies of each assignable Input because the best solution could be that the list, of maximum length N, is filled with the same Input. Needless to say, this causes the solution space to explode. However, reversing the problem and avoiding using a planning list is complicated because it is the only one that guarantees a sequential assignment and therefore allows the state to be calculated at the time of assignment. In the current way, the program manages to find a feasible solution in less than 100 seconds to go to any point I have tested within a radius of 15 meters (the travel time of the trajectories is always less than 1.5 seconds and I use a tolerance to the centimeter when considering distances). I am open to any type of advice and idea, especially to optimize the assignment.

@JsonIdentityInfo(scope = Drone.class, generator = ObjectIdGenerators.PropertyGenerator.class, property = "id")
@PlanningEntity
public class Drone {

@PlanningListVariable(allowsUnassignedValues = true)
@JsonIdentityReference(alwaysAsId = true)
private List<State> trajectory;

@PlanningId
private String id;

@PlanningPinToIndex
private int pinIndex = 0;

public Drone() {
}
//GETTERS/SETTERS...

.

@JsonIdentityInfo(scope = State.class, generator = ObjectIdGenerators.PropertyGenerator.class, property = "id")
@PlanningEntity
public class State {

private double tz;
private double tauX;
private double tauY;
private double tauZ;


private static final double g = 9.81; 
private static final double m = 1.0; 
private static final double dt = 0.01; 


private static final double[] J = { 0.01, 0.01, 0.02 };
private static final PersonalVector3D finalGoal = new PersonalVector3D(10, 10 ,10);
private static final double tolerance = 0.5;
private static List<PersonalVector3D> obstacles = new ArrayList<>();


@CascadingUpdateShadowVariable(targetMethodName = "updatePosition")
private PersonalVector3D position = null;
@CascadingUpdateShadowVariable(targetMethodName = "updatePosition")
private PersonalVector3D velocity = null;
@CascadingUpdateShadowVariable(targetMethodName = "updatePosition")
private PersonalRotation orientation = null;	
@CascadingUpdateShadowVariable(targetMethodName = "updatePosition")
private PersonalVector3D angularVelocity = null;
@CascadingUpdateShadowVariable(targetMethodName = "updatePosition")
private Double distanceFromFinalGoal = null;
@InverseRelationShadowVariable(sourceVariableName = "trajectory")
@JsonIdentityReference(alwaysAsId = true)
private Drone drone;

@PreviousElementShadowVariable(sourceVariableName = "trajectory")
@JsonIdentityReference(alwaysAsId = true)
private State previous;

@PlanningId
private String id;

@PlanningPin
private boolean pinned = false;
private boolean hasHitObstacle;

public State() {
}

public State(double tz, double tauX, double tauY, double tauZ, Integer i) {
	super();
	this.tz = tz;
	this.tauX = tauX;
	this.tauY = tauY;
	this.tauZ = tauZ;
	this.id = i.toString();
}

public void updatePosition() {
	if (drone == null) {
		this.position = null;
		this.velocity = null;
		this.orientation = null;
		this.angularVelocity = null;
		this.distanceFromFinalGoal = null;
		return;
	} else if (previous == null) {
		setAsInitial();
		return;
	}
	//CALCULATION OF NEW STATE FROM PREVIOUS[...]
	updateDistanceFromFinalGoal();
	this.hasHitObstacle =  position.getX()<0.0005 && position.getX()>-0.0005 && position.getY()<0.0005 && position.getY()>-0.0005 && position.getZ()>0.03 && position.getZ()<0.035;
}

@JsonIgnore
public void setAsInitial() {
	this.position = new PersonalVector3D(0, 0, 0);
	this.velocity = new PersonalVector3D(0, 0, 0);
	this.orientation = new PersonalRotation(1, 0, 0, 0); 
	this.angularVelocity = new PersonalVector3D(0, 0, 0);
	updateDistanceFromFinalGoal();
}

@JsonIgnore
public void updateDistanceFromFinalGoal() {
	this.distanceFromFinalGoal = Math.max(0, position.toVector3D().subtract(finalGoal.toVector3D()).getNorm()-tolerance);
}

//GETTERS/SETTERS...

.

@JsonCreator
public Drone(@JsonProperty("id") String id, @JsonProperty("trajectory") List<State> trajectory,
		@JsonProperty("pinIndex") int pinIndex) {
	super();
	this.id = id;
	this.trajectory = trajectory;
	this.pinIndex = pinIndex;
}

public Drone(String id) {
	super();
	this.id = id;
	this.trajectory = new ArrayList<>();
}

@JsonIgnore
public double getDistance() {
	if (trajectory.isEmpty())
		return State.getFinalgoal().toVector3D().getNorm();
	else
		return trajectory.stream().map(s -> s.getDistanceFromFinalGoal()).min(Double::compare).orElse(null);
}

@JsonIgnore
public int getOverMovement() {
	if (trajectory.isEmpty())
		return 0;
	int index = 0;
	for (int i = 1; i < trajectory.size(); i++)
		if (trajectory.get(i).getDistanceFromFinalGoal() < trajectory.get(index).getDistanceFromFinalGoal()) {
			index = i;
		}
	return trajectory.size() - 1 - index;
}
//GETTERS/SETTERS...

public class VehicleRoutingConstraintProvider implements ConstraintProvider {

public static final String VEHICLE_CAPACITY = "vehicleCapacity";
public static final String SERVICE_FINISHED_AFTER_MAX_END_TIME = "serviceFinishedAfterMaxEndTime";
public static final String MINIMIZE_TRAVEL_TIME = "minimizeTravelTime";

@Override
public Constraint[] defineConstraints(ConstraintFactory factory) {
	return new Constraint[] {
			minimizeTime(factory),
			justEnoughtMovement(factory),
			dashObstacles(factory),
			minimizeDistance(factory) };
}

// ************************************************************************
// Hard constraints
// ************************************************************************

protected Constraint minimizeDistance(ConstraintFactory factory) {
	return factory.forEach(Drone.class)
			.penalizeLong(HardMediumSoftLongScore.ONE_MEDIUM, drone -> (long) (drone.getDistance() * 1000))
			.asConstraint("Minimize distance");
}

protected Constraint minimizeTime(ConstraintFactory factory) {
	return factory.forEach(Drone.class)
			.penalizeLong(HardMediumSoftLongScore.ONE_SOFT, drone -> (long) (drone.getTrajectory().size()))
			.asConstraint("Minimize time");
}

protected Constraint justEnoughtMovement(ConstraintFactory factory) {
	return factory.forEach(Drone.class)
			.penalizeLong(HardMediumSoftLongScore.ONE_MEDIUM, drone -> drone.getOverMovement()*100)
			.asConstraint("Don't make useless movements");
}

protected Constraint dashObstacles(ConstraintFactory factory) {
	return factory.forEach(State.class).filter(s -> s.getDrone() != null && s.isHasHitObstacle() == true)
			.penalizeLong(HardMediumSoftLongScore.ONE_HARD).asConstraint("Don't hit obstacles");
}
}

[TomCools]

In your text you state use Timefold to assign a list of Input to a drone however in your code you seem to assign State to the Drone, which seems to be 2 distinct things. How do you intend to supply Timefold with the inputs, which calculate the state?

Some additional questions:

• What are you trying to achieve?
• What size of trajectories are you hoping to support? (now it's 15 meters, but I expect you'd want more)?
• What made you try Timefold for this use-case? There are very well known Path calculations available out there, so this use-case is something we haven't seen a lot. (usually, the paths between checkpoints are know before the solver is run and the solver just tries to find the most optimal one)