Esempio n. 1
0
        /// <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;
        }
Esempio n. 2
0
        /// <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);
        }
Esempio n. 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)
                       ));
        }
Esempio n. 4
0
        /// <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));
        }
Esempio n. 5
0
        /// <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.");
            }
        }
Esempio n. 6
0
        /// <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])));
            }
        }
Esempio n. 7
0
        /// <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));
            }
        }
Esempio n. 8
0
 public static JointValuesModel ToModel(this JointValues values) =>
 new JointValuesModel
 {
     JointNames = values.JointSet.ToArray(), Values = values.ToArray()
 };
Esempio n. 9
0
 public JointTrajectoryPoint(double timeFromStartSeconds, JointValues positions, JointValues velocities = null, JointValues accelerations = null, JointValues efforts = null)
     : this(TimeSpan.FromSeconds(timeFromStartSeconds), positions, velocities, accelerations, efforts)
 {
 }
Esempio n. 10
0
 public static JointValues Interpolate(JointValues a, JointValues b, double t = 0.5) =>
 (1 - t) * a + t * b;
Esempio n. 11
0
 /// <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);
Esempio n. 12
0
 /// <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);
Esempio n. 13
0
 public bool Equals(JointValues other) =>
 object.ReferenceEquals(this, other) ||
 (other != null &&
  other.JointSet.Equals(this.JointSet) &&
  this.Values.SequenceEqual(other.Values));
Esempio n. 14
0
 /// <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);
Esempio n. 15
0
 /// <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)
 {
 }
Esempio n. 16
0
 /// <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)
 {
 }