/// <summary>
/// Creates a new instance of <c>JointTrajectoryPoint</c>.
/// </summary>
/// <param name="timeFromStart">Time at which the robot is required to reach the new point.</param>
/// <param name="positions">The target joint positions of the new point.</param>
/// <param name="velocities">Optional: The target velocity that each joint is required to have when the robot reaches the new point. Default: null.</param>
/// <param name="accelerations">Optional: The target accelerations that each joint is required to have when the robot reaches the new point. Default: null.</param>
/// <param name="efforts">Optional: The target effort that each joint is required to have when the robot reaches the new point. Default: null.</param>
/// <exception cref="ArgumentException">Thrown when <paramref name="positions"/>, <paramref name="velocities"/>, <paramref name="accelerations"/> or <paramref name="efforts"/> have incompatible <c>JointSet</c>.</exception>
public JointTrajectoryPoint(TimeSpan timeFromStart, JointValues positions, JointValues velocities = null, JointValues accelerations = null, JointValues efforts = null)
{
this.TimeFromStart = timeFromStart;
this.Positions = positions ?? throw new ArgumentNullException(nameof(positions));
// ensure alle components use the same joint set
JointSet jointSet = positions.JointSet;
if (velocities != null && velocities != JointValues.Empty && !velocities.JointSet.Equals(jointSet))
{
throw new ArgumentException($"JointValues '{nameof(velocities)}' have incompatible JointSet.", nameof(Velocities));
}
if (accelerations != null && accelerations != JointValues.Empty && !accelerations.JointSet.Equals(jointSet))
{
throw new ArgumentException($"JointValues '{nameof(accelerations)}' have incompatible JointSet.", nameof(accelerations));
}
if (efforts != null && efforts != JointValues.Empty && !efforts.JointSet.Equals(jointSet))
{
throw new ArgumentException($"JointValues '{nameof(efforts)}' have incompatible JointSet.", nameof(efforts));
}
this.Velocities = velocities;
this.Accelerations = accelerations;
this.Efforts = efforts;
}
/// <summary>
/// Merge JointValues instance with others <c>JointValues</c>.
/// </summary>
/// <param name="other">JointValues which are merge with current instance</param>
/// <returns>A new instance of <c>JointValues</c> with contains values for
/// all Joints defined in this and the other JointValues instance.</returns>
public JointValues Merge(JointValues other)
{
var fullJointSet = this.JointSet.Append(other.JointSet);
var merge = new JointValues(fullJointSet, 0);
merge = merge.SetValues(this);
merge = merge.SetValues(other);
return(merge);
}
/// <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>
/// Computes max{ |j_0|, |j_1|, ... , |j_n| }, the maximum (or uniform , L-inifinity) norm of the joint values.
/// </summary>
/// <returns>Returns the maximum value of the absolute joint values.</returns>
public static double MaxNorm(JointValues j)
{
if (j == null)
{
throw new ArgumentNullException(nameof(j));
}
if (j.Count == 0)
{
return(0); // special handling for empty joint values
}
return(j.Max((Func <double, double>)Math.Abs));
}
/// <summary>
/// Creates a new instance of <c>JointValues</c> from the given joint values. All joint values are required to have a joint set that matches the other given joint values.
/// </summary>
/// <param name="positions">The position values of the joints.</param>
/// <param name="velocities">The velocity values of the joints.</param>
/// <param name="efforts">The effort values of the joints.</param>
/// <exception cref="Exception">Thrown when the jointsets of the non-null jointstates differ.</exception>
public JointStates(JointValues positions, JointValues velocities = null, JointValues efforts = null)
{
this.Positions = positions;
this.Velocities = velocities;
this.Efforts = efforts;
var jointSet = this.JointSet;
var values = new JointValues[] { positions, velocities, efforts };
if (jointSet != null && !values.All(x => x == null || x.JointSet.Equals(jointSet)))
{
throw new Exception("JointSet values do not match.");
}
}
/// <summary>
/// Returns a new <c>JointValues</c> object whose values are the difference of the specified
/// <c>JointValues</c> object and this instance.
/// </summary>
/// <param name="jv">The joint values to subtract.</param>
/// <returns>A new object that represents the value of this instance minus the value of js.</returns>
/// <exception cref="ArgumentException">Thrown when the <c>JointSet</c> of <paramref name="jv"/> is not similar to the current <c>JointSet</c>.</exception>
/// <exception cref="ArgumentNullException">Thrown when <paramref name="jv"/> is null.</exception>
public JointValues Subtract(JointValues jv)
{
if (jv == null)
{
throw new ArgumentNullException(nameof(jv));
}
if (!jv.JointSet.IsSimilar(this.JointSet))
{
throw new ArgumentException("The given joint set must be similar to the current joint set.", nameof(jv));
}
if (jv.JointSet == this.JointSet)
{
return(this.Transform((x, i) => x - jv[i]));
}
else
{
return(this.Transform((x, i) => x - jv.GetValue(this.JointSet[i])));
}
}
/// <summary>
/// Computes the L^p-norm of a JointValues object.
/// </summary>
/// <param name="j">JointValues instance</param>
/// <param name="p">p-parameter</param>
/// <returns>Returns the p-norm of the JointValues.</returns>
public static double Norm(JointValues j, double p = 2.0)
{
if (j == null)
{
throw new ArgumentNullException(nameof(j));
}
if (j.Count == 0)
{
return(0); // special handling for empty joint values
}
if (p == 2.0)
{
return(Math.Sqrt(j.Sum(x => x * x)));
}
else if (p == 1.0)
{
return(j.Sum((Func <double, double>)Math.Abs));
}
else
{
return(Math.Pow(j.Sum(x => Math.Pow(Math.Abs(x), p)), 1.0 / p));
}
}
public static JointValuesModel ToModel(this JointValues values) =>
new JointValuesModel
{
JointNames = values.JointSet.ToArray(), Values = values.ToArray()
};
public JointTrajectoryPoint(double timeFromStartSeconds, JointValues positions, JointValues velocities = null, JointValues accelerations = null, JointValues efforts = null)
: this(TimeSpan.FromSeconds(timeFromStartSeconds), positions, velocities, accelerations, efforts)
{
}
public static JointValues Interpolate(JointValues a, JointValues b, double t = 0.5) => (1 - t) * a + t * b;
/// <summary>
/// Computes max{ |j_0|, |j_1|, ... , |j_n| }, the maximum (or uniform , L-inifinity) norm of the joint values.
/// </summary>
/// <returns>The maximum norm</returns>
public double MaxNorm() =>
JointValues.MaxNorm(this);
/// <summary> /// Computes the p-norm of this instance. /// </summary> /// <param name="p">p-parameter</param> /// <returns>Returns the p-norm of the JointValues.</returns> public double Norm(double p = 2.0) => JointValues.Norm(this, p);
public bool Equals(JointValues other) => object.ReferenceEquals(this, other) || (other != null && other.JointSet.Equals(this.JointSet) && this.Values.SequenceEqual(other.Values));
/// <summary> /// Updates the joint values of the given joint set. Values which are not in the given joint set are not changed. /// </summary> /// <param name="source">The new values for the given joint set.</param> /// <returns>A new instance of <c>JointValues</c>.</returns> public JointValues SetValues(JointValues source) => SetValues(source.JointSet, source.Values);
/// <summary>
/// Creates a new <c>JointPath</c> containing the given start and end point in Joint space.
/// </summary>
/// <param name="start">Start point of the new <c>JointPath</c>.</param>
/// <param name="goal">End point of the new <c>JointPath</c>.</param>
/// <exception cref="ArgumentException">Thrown when there is a mismatch between the <c>JointSet</c> in <paramref name="start"/> and <paramref name="goal"/>.</exception>
public JointPath(JointValues start, JointValues goal)
: this(start?.JointSet, start, goal)
{
}
/// <summary>
/// Creates a new <c>JointPath</c> containing only the given point in Joint space.
/// </summary>
/// <param name="point"></param>
public JointPath(JointValues point)
: this(point?.JointSet, point)
{
}
