Exemple #1
0
        /// <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));
        }
Exemple #2
0
 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()
 };
Exemple #3
0
        /// <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)
                       ));
        }
Exemple #4
0
        /// <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)
                       ));
        }
Exemple #5
0
        /// <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));
            }
        }
Exemple #6
0
 public JointTrajectoryPoint InterpolateCubic(JointTrajectoryPoint point1, TimeSpan time) =>
 JointTrajectoryPoint.InterpolateCubic(this, point1, time);