/// <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) { }