/// <summary>
/// Merges two joint trajectories.
/// </summary>
/// <param name="a">The first joint trajectory.</param>
/// <param name="b">The second joint trajectory.</param>
/// <param name="delayA">The delay of the first <c>JointTrajectory</c>.</param>
/// <param name="delayB">The delay of the second <c>JointTrajectory</c>.</param>
/// <returns>A new instance of <c>JointTrajectory</c>.</returns>
public static IJointTrajectory Merge(IJointTrajectory a, IJointTrajectory b, TimeSpan delayA, TimeSpan delayB)
{
JointSet unionJointSet = a.JointSet.Combine(b.JointSet);
int numPointsA = a.Count;
int numPointsB = b.Count;
TimeSpan durationA = a[numPointsA - 1].TimeFromStart + delayA;
TimeSpan durationB = b[numPointsB - 1].TimeFromStart + delayB;
TimeSpan duration = durationA > durationB ? durationA : durationB;
int maxNumberPoints = Math.Max(numPointsA, numPointsB);
var result = new List <JointTrajectoryPoint>();
for (int i = 0; i < maxNumberPoints; i++)
{
double t = i / (double)(maxNumberPoints - 1);
TimeSpan simulatedTime = duration * t;
JointTrajectoryPoint q_a = a.EvaluateAt(simulatedTime - delayA).WithTimeFromStart(simulatedTime);
JointTrajectoryPoint q_b = b.EvaluateAt(simulatedTime - delayB).WithTimeFromStart(simulatedTime);
JointTrajectoryPoint jtp = q_a.Merge(q_b);
result.Add(jtp);
}
return(new JointTrajectory(unionJointSet, result));
}
public static JointTrajectoryPointModel ToModel(this JointTrajectoryPoint point) =>
new JointTrajectoryPointModel
{
TimeFromStart = point.TimeFromStart.TotalSeconds,
Positions = point.Positions?.ToArray(),
Velocities = point.Velocities?.ToArray(),
Accelerations = point.Accelerations?.ToArray(),
Efforts = point.Efforts?.ToArray()
};
/// <summary>
/// Cubic Interpolation between two <c>JointTrajectoryPoints</c>
/// </summary>
/// <param name="time"></param>
/// <param name="point0">The first point to be interpolated.</param>
/// <param name="point1">The second point to be interpolated.</param>
/// <returns>The interpolated <c>JointTrajectoryPoint</c>.</returns>
/// <exception cref="ArgumentException">Thrown when the .</exception>
public static JointTrajectoryPoint InterpolateCubic(JointTrajectoryPoint point0, JointTrajectoryPoint point1, TimeSpan time)
{
if (point0.TimeFromStart > point1.TimeFromStart)
{
throw new ArgumentException("Point0 must occur before point1.");
}
TimeSpan t0 = point0.TimeFromStart;
TimeSpan t1 = point1.TimeFromStart;
TimeSpan deltaT = t1 - t0;
JointSet jointSet = point1.Positions.JointSet;
if (deltaT.TotalSeconds < 1e-6)
{
return(new JointTrajectoryPoint(
timeFromStart: t0 + deltaT,
positions: point1.Positions,
velocities: JointValues.Zero(jointSet)
));
}
double[] pos = point0.Positions.ToArray();
double[] vel = point0.Velocities.ToArray();
JointValues p0 = point0.Positions;
JointValues p1 = point1.Positions;
JointValues v0 = point0.Velocities;
JointValues v1 = point1.Velocities;
// clip t to be between 0 and t1 - t0
double t = Math.Max((time - t0).TotalSeconds, 0);
t = Math.Min((t1 - t0).TotalSeconds, t);
for (int i = 0; i < p0.Count; i++)
{
double dt = deltaT.TotalSeconds;
double a = p0[i];
double b = v0[i];
double c = (-3.0 * p0[i] + 3.0 * p1[i] - 2.0 * dt * v0[i] - dt * v1[i]) / Math.Pow(dt, 2);
double d = (2.0 * p0[i] - 2.0 * p1[i] + dt * v0[i] + dt * v1[i]) / Math.Pow(dt, 3);
pos[i] = a + b * t + c * Math.Pow(t, 2) + d * Math.Pow(t, 3);
vel[i] = b + 2.0 * c * t + 3.0 * d * Math.Pow(t, 2);
}
return(new JointTrajectoryPoint(
timeFromStart: time,
positions: new JointValues(jointSet, pos),
velocities: new JointValues(jointSet, vel)
));
}
/// <summary>
/// Creates a new instance of JointTrajectory as a result of the merge operation.
/// </summary>
/// <param name="other">TrajectoryPoints to merge with the current TrajectoryPoint</param>
/// <returns>New instance of JointTrajectoryPoint which contains the merged JointTrajectoryPoints</returns>
/// <exception cref="Exception">Thrown when TimeFromStart of the points do not match.</exception>
public JointTrajectoryPoint Merge(JointTrajectoryPoint other)
{
if (this.TimeFromStart != other.TimeFromStart)
{
throw new Exception("Merge conflict: TimeFromStart in other JointTrajectoryPoint is not equal to TimeFromStart of this instance.");
}
return(new JointTrajectoryPoint(
this.TimeFromStart,
this.Positions.Merge(other.Positions),
this.Velocities?.Merge(other.Velocities),
this.Accelerations?.Merge(other.Accelerations),
this.Efforts?.Merge(other.Efforts)
));
}
/// <summary>
/// Evaluates the trajectory at a given time.
/// </summary>
/// <param name="time">The simulated time</param>
/// <returns>An instance of <c>JointTrajectoryPoint</c> at the given time.</returns>
/// <exception cref="ArgumentOutOfRangeException">Thrown when <paramref name="time"/> is not represented by this trajectory.</exception>
public JointTrajectoryPoint EvaluateAt(TimeSpan time)
{
if (time < this[0].TimeFromStart || time > this[this.Count - 1].TimeFromStart)
{
throw new ArgumentOutOfRangeException("Time is out of bounds.");
}
else
{
int index = GetPointBefore(time);
int k = Math.Min(index + 1, this.Count - 1);
JointTrajectoryPoint p0 = this[index];
JointTrajectoryPoint p1 = this[k];
JointTrajectoryPoint q = p0.InterpolateCubic(p1, time);
return(q.WithTimeFromStart(time));
}
}
public JointTrajectoryPoint InterpolateCubic(JointTrajectoryPoint point1, TimeSpan time) => JointTrajectoryPoint.InterpolateCubic(this, point1, time);
