/// <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>
/// 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));
}
/// <summary>
/// Reorders the values to match the order of the given <c>JointSet</c>.
/// </summary>
/// <param name="newOrder">A <c>JointSet</c> with the same name of joints, but in a different order than the current <c>JointSet</c>.</param>
/// <returns>A new instance of <c>JointValues</c>.</returns>
/// <exception cref="ArgumentException">Thrown when the given <paramref name="newOrder"/> is not a permutation of the names in the <c>JointSet</c>.</exception>
public JointValues Reorder(JointSet newOrder)
{
if (!newOrder.IsSimilar(this.JointSet))
{
throw new ArgumentException("The given joint set must be similar to the current joint set.", nameof(newOrder));
}
return(new JointValues(newOrder, newOrder.Select(x => this[x])));
}
/// <summary>
/// Creates a new <c>JointValues</c> instance that contains only values for joints of the given <c>JointSet</c>.
/// The <paramref name="subset"/> parameter defines the new order of the joints in the resulting <c>JointValues</c> object.
/// </summary>
/// <param name="subset">A <c>JointSet</c> with a subset of the joints.</param>
/// <returns>A new instance of <c>JointValues</c>.</returns>
/// <exception cref="ArgumentException">Thrown when the given <paramref name="subset"/> is not a subset of the names the <c>JointSet</c>.</exception>
public JointValues Select(JointSet subset)
{
if (!subset.IsSubset(this.JointSet))
{
throw new ArgumentException("The given joint set needs to be a subset of the current joint set.", nameof(subset));
}
return(new JointValues(subset, subset.Select(x => this[x])));
}
/// <summary>
/// Creates a new instance of <c>JointValues</c> with the given joint set and the collection of values. The first value is assigned the first name in the joint set, the second value is assigned the second name and so on.
/// </summary>
/// <param name="jointSet">The names of joint that get values assigned.</param>
/// <param name="values">The values (in radians) that should be assigned to the <c>JointSet</c>.</param>
/// <exception cref="Exception">Thrown when the amount of joint names in <paramref name="jointSet"/> does not match the length of values in <paramref name="values"/>.</exception>
public JointValues(JointSet jointSet, IEnumerable <double> values)
{
double[] array = values.ToArray();
if (jointSet.Count != array.Length)
{
throw new Exception($"The length of the values sequence needs to match the amount of joint names in the joint set.");
}
this.values = Array.AsReadOnly(array);
this.JointSet = jointSet;
}
public static JointStates ToJointStates(this JointStatesModel model)
{
var jointSet = new JointSet(model.JointNames);
return(new JointStates(
new JointValues(jointSet, model.Positions),
model.Velocities != null ? new JointValues(jointSet, model.Velocities) : null,
model.Efforts != null ? new JointValues(jointSet, model.Efforts) : null
));
}
public static JointSet Combine(JointSet left, JointSet right)
{
if (left == null)
{
return(right);
}
if (right == null)
{
return(left);
}
return(left.Combine(right));
}
/// <summary>
/// Creates a new <c>PlanParameters</c> object in Joint Space for a MoveGroup
/// </summary>
/// <param name="moveGroupName">The name of the planning group to control and plan for.</param>
/// <param name="joints">Collection of The Move Group Joints set in a certain order.</param>
/// <param name="maxVelocity">Array of maximum velocities for each joint in the <c>JointSet</c> in the Move Group [in m/s].</param>
/// <param name="maxAcceleration">Array of maximum accelerations for each joint in the <c>JointSet</c> in the Move Group [in m/s^2].</param>
/// <param name="sampleResolution">The time resolution for trajectory generation (dt) [in Second].</param>
/// <param name="collisionCheck">Indicates whether generated trajectories should be tested for collision.</param>
/// <param name="maxDeviation">In case waypoints are used, this parameter will allow blending between the segments.</param>
public PlanParameters(string moveGroupName, JointSet joints, double[] maxVelocity, double[] maxAcceleration, double sampleResolution = 0.008, bool collisionCheck = true, double maxDeviation = 0.2)
: this(new Builder() {
MoveGroupName = moveGroupName,
JointSet = joints,
MaxVelocity = maxVelocity,
MaxAcceleration = maxAcceleration,
SampleResolution = sampleResolution,
CollisionCheck = collisionCheck,
MaxDeviation = maxDeviation
})
{
}
/// <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 <c>JointValues</c> with the given joint set and the collection of values. The first value is assigned the first name in the joint set, the second value is assigned the second name and so on.
/// </summary>
/// <param name="jointSet">The names of joint that get values assigned.</param>
/// <param name="values">The values (in radians) that should be assigned to the <c>JointSet</c>.</param>
/// <exception cref="Exception">Thrown when the amount of joint names in <paramref name="jointSet"/> does not match the length of values in <paramref name="values"/>.</exception>
public JointValues(JointSet jointSet, IReadOnlyList <double> values)
{
double[] array = new double[values.Count];
for (int i = 0; i < array.Length; i++)
{
array[i] = values[i];
}
if (jointSet.Count != array.Length)
{
throw new Exception($"The length of the values list needs to match the amount of joint names in the joint set.");
}
this.values = Array.AsReadOnly(array);
this.JointSet = jointSet;
}
/// <summary>
/// Creates a new instance of <c>JointLimits</c>.
/// </summary>
/// <param name="jointSet"></param>
/// <param name="maxVelocity"></param>
/// <param name="maxAcceleration"></param>
/// <param name="minPosition"></param>
/// <param name="maxPosition"></param>
/// <exception cref="ArgumentNullException">Thrown when any of the given parameters is null</exception>
/// <exception cref="ArgumentException">Thrown when there is a mismatch in the amount of joints in the joint set and the length of the given limit arrays.</exception>
public JointLimits(JointSet jointSet, double?[] maxVelocity, double?[] maxAcceleration, double?[] minPosition, double?[] maxPosition)
{
if (jointSet == null)
{
throw new ArgumentNullException(nameof(jointSet));
}
if (maxVelocity == null)
{
throw new ArgumentNullException(nameof(maxVelocity));
}
if (maxAcceleration == null)
{
throw new ArgumentNullException(nameof(maxAcceleration));
}
if (minPosition == null)
{
throw new ArgumentNullException(nameof(MinPosition));
}
if (maxPosition == null)
{
throw new ArgumentNullException(nameof(MaxPosition));
}
if (jointSet.Count != maxVelocity.Length)
{
throw new ArgumentException("Number of joints does not match length of velocity limits array.", nameof(maxVelocity));
}
if (jointSet.Count != maxAcceleration.Length)
{
throw new ArgumentException("Acceleration limits array length does not match number of joints.", nameof(maxAcceleration));
}
if (jointSet.Count != minPosition.Length)
{
throw new ArgumentException("Minimum position limits array length does not match number of joints.", nameof(minPosition));
}
if (jointSet.Count != maxPosition.Length)
{
throw new ArgumentException("Maximum position limits array length does not match number of joints.", nameof(maxPosition));
}
this.JointSet = jointSet;
this.MaxAcceleration = maxAcceleration;
this.MaxVelocity = maxVelocity;
this.MinPosition = minPosition;
this.MaxPosition = maxPosition;
}
/// <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="jointSet">The set of joints whose values should be changed.</param>
/// <param name="values">The new values for the given joint set.</param>
/// <returns>A new instance of <c>JointValues</c>.</returns>
/// <exception cref="ArgumentException">Thrown when the given <paramref name="jointSet"/> is not a subset of the names the <c>JointSet</c>.</exception>
/// <exception cref="ArgumentException">Thrown when the number of elements in the <paramref name="values"/> list does not match the number of joints in the <paramref name="jointSet"/> argument.</exception>
public JointValues SetValues(JointSet jointSet, IReadOnlyList <double> values)
{
if (!jointSet.IsSubset(this.JointSet))
{
throw new ArgumentException("The given joint set needs to be a subset of the current joint set.", nameof(jointSet));
}
if (jointSet.Count != values.Count)
{
throw new ArgumentException("Number of elements in values list does not match number of joints in joint set.", nameof(values));
}
double[] newValues = this.ToArray();
for (var i = 0; i < values.Count; i++)
{
string jointName = jointSet[i];
int index = this.JointSet.GetIndexOf(jointName);
newValues[index] = values[i];
}
return(new JointValues(this.JointSet, newValues));
}
/// <summary>
/// Creates a new <c>JointPath</c> from the given set of joints and the given collection of joint values.
/// </summary>
/// <param name="joints">The names of the joints used in the new <c>JointPath</c>.</param>
/// <param name="points">The joint values that should be added to the new <c>JointPath</c>.</param>
/// <exception cref="ArgumentNullException">Thrown when no <paramref name="joints"/> or no <paramref name="points"/> are given.</exception>
/// <exception cref="ArgumentException">Thrown when there is a mismatch between any <c>JointSet</c> in <paramref name="points"/> and the given <c>JointSet</c> <paramref name="joints"/>.</exception>
/// <seealso cref="JointSet"/>
public JointPath(JointSet joints, IEnumerable <JointValues> points)
{
this.joints = joints ?? throw new ArgumentNullException(nameof(joints));
if (points == null)
{
throw new ArgumentNullException(nameof(points));
}
this.points = points
.Where(x => x != null)
.Select(x =>
{
if (!x.JointSet.Equals(joints))
{
if (!x.JointSet.IsSimilar(joints))
{
throw new ArgumentException("Provided path points have joint values of incompatible joint sets.", nameof(points));
}
x = x.Reorder(joints);
}
return(x);
})
.ToList();
}
public static IJointTrajectory ToJointTrajectory(this JointTrajectoryModel model)
{
var jointSet = new JointSet(model.JointNames);
return(new JointTrajectory(jointSet, model.Points.Select(x => x.ToJointTrajectoryPoint(jointSet)), model.IsValid));
}
public static JointTrajectoryPoint ToJointTrajectoryPoint(this JointTrajectoryPointModel model, JointSet jointSet) => new JointTrajectoryPoint(TimeSpan.FromSeconds(model.TimeFromStart), GetJointValues(jointSet, model.Positions), GetJointValues(jointSet, model.Velocities), GetJointValues(jointSet, model.Accelerations), GetJointValues(jointSet, model.Efforts));
private static JointValues GetJointValues(JointSet jointSet, double[] values) => values != null ? new JointValues(jointSet, values) : null;
public static IJointPath ToJointPath(this JointPathModel model)
{
var jointSet = new JointSet(model.JointNames);
return(new JointPath(jointSet, model.Points.Select(x => new JointValues(jointSet, x))));
}
/// <summary> /// Tests whether the all joint names of the current <c>JointSet</c> are included in the given other <c>JointSet</c>. /// </summary> /// <param name="other">Another <c>JointSet</c> which should be tested for being a subset.</param> /// <returns>True when the given other <c>JointSet</c> is a subset of the current one; False otherwise.</returns> public bool IsSubset(JointSet other) => jointNames.All(other.Contains);
/// <summary> /// Appends the joint names of the given joint set to the current joint names, if they are not already in the set. /// </summary> /// <param name="other">The <c>JointSet</c> containing the names that should be added<param> /// <returns>A new <c>JointSet</c></returns> public JointSet Combine(JointSet other) => Combine(other.jointNames);
/// <summary>
/// Creates a new instance of <c>JointValues</c> with the given joint set and fills each value with the given fill value.
/// </summary>
/// <param name="jointSet">The names of joint that get values assigned.</param>
/// <param name="fillValue">The value that should be assigned to each name in the <c>JointSet</c>.</param>
public JointValues(JointSet jointSet, double fillValue)
: this(jointSet, Fill(jointSet.Count, fillValue))
{
}
/// <summary> /// Tests whether the given other <c>JointSet</c> is similar to the current one. Two <c>JointSet</c> instances are similar, when they contain the same number of joint names and are a subset of each other. The order in which they hold the joint names does not matter in this case. /// </summary> /// <param name="other">Another <c>JointSet</c> which should be tested for similarity.</param> /// <returns>True when the other <c>JointSet</c> is similar the current one; False otherwise.</returns> public bool IsSimilar(JointSet other) => other.Count == this.Count && this.IsSubset(other);
/// <summary> /// Tests whether the given other <c>JointSet</c> equals the current one. Two <c>JointSet</c> instances are equal, when they contain the same joint names in the same order. /// </summary> /// <param name="other">Another <c>JointSet</c> which should be tested for equality.</param> /// <returns>True when the other <c>JointSet</c> equals the current one; False otherwise.</returns> public bool Equals(JointSet other) => object.ReferenceEquals(this, other) || (other != null && this.Count == other.Count && this.jointNames.SequenceEqual(other.jointNames));
/// <summary>
/// Creates a new <c>JointPath</c> from the given set of joints and the given array of joint values.
/// </summary>
/// <param name="joints">The names of the joints used in the new <c>JointPath</c>.</param>
/// <param name="points">The joint values that should be added to the new <c>JointPath</c>.</param>
/// <exception cref="ArgumentNullException">Thrown when no <paramref name="joints"/> or no <paramref name="points"/> are given.</exception>
/// <exception cref="ArgumentException">Thrown when there is a mismatch between any <c>JointSet</c> in <paramref name="points"/> and the given <c>JointSet</c> <paramref name="joints"/>.</exception>
/// <seealso cref="JointSet"/>
public JointPath(JointSet joints, params JointValues[] points)
: this(joints, (IEnumerable <JointValues>)points)
{
}
/// <summary> /// Appends the joint names of the given joint set to the array of joint names already which form the current joint set. /// </summary> /// <param name="other">The names that should be appended.</param> /// <returns>A new <c>JointSet</c></returns> public JointSet Append(JointSet other) => Append(other.jointNames);
/// <summary>
/// Creates a new instance of <c>JointTrajectory</c> from the given <c>JointSet</c> and the collection of <c>JointTrajectoryPoint</c>
/// </summary>
/// <param name="joints">The set of joints that is used for the new trajectory.</param>
/// <param name="points">The collection of points which should form the trajectory. The points need to meed the following requirements:
/// <list type="number">
/// <item><para>The time from start values need to be ascending.</para></item>
/// <item><para>The position value needs to be set.</para></item>
/// <item><para>The joint sets used in the joint values, like positions, velocities etc, need to match the given <c>JointSet</c> <paramref name="joints"/>.</para></item>
/// </list>
/// </param>
/// <param name="valid">Indicates if the new trajectory is valid.</param>
/// <exception cref="ArgumentNullException">Thrown when <paramref name="joints"/> is null.</exception>
/// <exception cref="ArgumentException">Thrown when the criteria for the points defined in <paramref name="points"/> are not fulfilled.</exception>
public JointTrajectory(JointSet joints, IEnumerable <JointTrajectoryPoint> points, bool valid = true)
{
this.joints = joints ?? throw new ArgumentNullException(nameof(joints));
if (points == null)
{
throw new ArgumentNullException(nameof(points));
}
this.points = points.ToList();
JointTrajectoryFlags flags = JointTrajectoryFlags.HasEffort | JointTrajectoryFlags.HasVelocity | JointTrajectoryFlags.HasAcceleration;
if (valid)
{
flags |= JointTrajectoryFlags.IsValid;
}
// consistency checks for provided trajectory points
TimeSpan lastTime = TimeSpan.Zero;
foreach (var point in this.points)
{
if (point.TimeFromStart < lastTime)
{
throw new ArgumentException("TimeFromStart values of trajectory points must be ascending.", nameof(points));
}
lastTime = point.TimeFromStart;
if (point.Positions == null)
{
throw new ArgumentException("Position of trajectory point is null.", nameof(points));
}
if (!point.Positions.JointSet.Equals(joints))
{
throw new ArgumentException("Provided points have position values for different joint sets.", nameof(points));
}
if (point.Velocities != null && point.Velocities.Count > 0)
{
if (!point.Velocities.JointSet.Equals(joints))
{
throw new ArgumentException("Provided points have velocity values for different joint sets.", nameof(points));
}
}
else
{
flags &= ~JointTrajectoryFlags.HasVelocity;
}
if (point.Accelerations != null && point.Accelerations.Count > 0)
{
if (!point.Velocities.JointSet.Equals(joints))
{
throw new ArgumentException("Provided points have acceleration values for different joint sets.", nameof(points));
}
}
else
{
flags &= ~JointTrajectoryFlags.HasAcceleration;
}
if (point.Efforts != null && point.Efforts.Count > 0)
{
if (!point.Velocities.JointSet.Equals(joints))
{
throw new ArgumentException("Provided points have effort values for different joint sets.", nameof(points));
}
}
else
{
flags &= ~JointTrajectoryFlags.HasEffort;
}
}
this.flags = flags;
}
/// <summary> /// Creates a new instance of <c>JointValues</c> with each value set to zero. /// </summary> /// <param name="jointSet"></param> /// <returns></returns> public static JointValues Zero(JointSet jointSet) => new JointValues(jointSet, 0);
