/// <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);