public void TestInit()
{
var joints = new JointSet("a", "b", "c");
var start = new JointValues(joints, new double[] { 1, 2, 3 });
var goal = new JointValues(joints, new double[] { 0, 0, 0 });
var badGoal = new JointValues(new JointSet("d", "b", "c"), new double[] { 0, 0, 0 });
var p1 = new JointPath(start);
Assert.Equal(start, p1[0]);
var p2 = new JointPath(start, goal);
Assert.Equal(start, p2[0]);
Assert.Equal(goal, p2[1]);
Assert.Throws <ArgumentException>(() => new JointPath(start, badGoal));
var p3 = new JointPath(joints, start, goal);
Assert.Equal(start, p3[0]);
Assert.Equal(goal, p3[1]);
var p4 = new JointPath(joints, new JointValues[] { start, goal });
Assert.Equal(start, p4[0]);
Assert.Equal(goal, p4[1]);
}
public void TestRandom()
{
var joints = new JointSet("a", "b");
var maxAcc = new double?[] { 10, 10 };
var maxVel = new double?[] { 50, 10 };
var minPos = new double?[] { -10, 0 };
var maxPos = new double?[] { 50, 10 };
JointLimits jl = new JointLimits(joints, maxVel, maxAcc, minPos, maxPos);
for (int i = 0; i < 100; ++i)
{
var a = JointValues.Random(jl);
Assert.True(a[0] >= minPos[0]);
Assert.True(a[1] >= minPos[1]);
Assert.True(a[0] <= maxPos[0]);
Assert.True(a[1] <= maxPos[1]);
}
Assert.Throws <ArgumentNullException>(() => JointValues.Random(null));
// assert that there must be a jointlimit for every name in jointset
maxAcc = new double?[] { 10, 10 };
maxVel = new double?[] { 50, 10 };
minPos = new double?[] { null, 0 };
maxPos = new double?[] { 50, 10 };
jl = new JointLimits(joints, maxVel, maxAcc, minPos, maxPos);
Assert.Throws <ArgumentException>(() => JointValues.Random(jl));
}
public void TestIndexAccess()
{
var joints = new JointSet("a", "c", "b");
Assert.Equal("b", joints[2]);
Assert.Throws <IndexOutOfRangeException>(() => joints[3]);
}
public void TestContains()
{
var joints = new JointSet("a", "c", "b");
Assert.True(joints.Contains("a"));
Assert.False(joints.Contains("d"));
}
/// <summary>
/// Creates a new <c>EndEffectorDescription</c>, from the given properties that define an end effector.
/// </summary>
/// <param name="name">The name of the end effector</param>
/// <param name="subMoveGroupIds">The IDs of the sub movegroups the end effector uses. (Not supported yet.)</param>
/// <param name="jointSet">The <see cref="JointSet"></see> holding the joints that are moved when the end effector is moved in Cartesian space./></param>
/// <param name="moveGroupName">The name of the movegroup that is used when the end effector is moved in Cartesian space.</param>
/// <param name="endEffectorLinkName">The name of the link that is located at the end of the kinematic chain of the end effector.</param>
public EndEffectorDescription(string name, string[] subMoveGroupIds, JointSet jointSet, string moveGroupName, string endEffectorLinkName)
{
this.Name = name;
this.SubMoveGroupIds = subMoveGroupIds;
this.JointSet = jointSet;
this.MoveGroupName = moveGroupName;
this.LinkName = endEffectorLinkName;
}
/// <summary>
/// Extends type <c>sensor_msgs.JointState</c> with a method <c>ToJointState</c>, which creates corresponding jointstates
/// </summary>
/// <param name="jointState">A time joint states message</param>
/// <returns>Returns an instance of <c>JointStates</c>.</returns>
public static JointStates ToJointState(this sensor_msgs.JointState jointState)
{
var joints = new JointSet(jointState.name);
JointValues getJointValues(double[] v) => v != null && v.Length > 0 ? new JointValues(joints, v) : null;
return(new JointStates(getJointValues(jointState.position), getJointValues(jointState.velocity), getJointValues(jointState.effort)));
}
/// <summary>
/// Creates a <c>MoveGroup</c> for manually specified joint set. Since the mapping between ROS move-groups
/// and the specified joint set is not know nor analysed it is not possible to execute end-effector
/// related functions (e.g. inverse kinematics).
/// </summary>
/// <param name="motionService">An object implementing <c>IMotionService</c> which is used to communicate with the motion server.</param>
/// <param name="jointSet">A joint set</param>
/// <exception cref="ArgumentNullException">Thrown when <paramref name="motionService"/> is null.</exception>
public MoveGroup(IMotionService motionService, JointSet jointSet)
{
this.motionService = motionService ?? throw new ArgumentNullException(nameof(motionService));
this.jointSet = jointSet;
this.defaultPlanParameters = motionService.CreatePlanParameters(null, this.jointSet, null, null, 0.05, false, 1);
this.velocityScaling = 1;
this.name = this.defaultPlanParameters.MoveGroupName;
}
public void TestAddPrefix()
{
var jointsA = new JointSet("a", "b", "c");
var jointsB = jointsA.AddPrefix("test_");
Assert.True(jointsB.ToArray().SequenceEqual(new string[] { "test_a", "test_b", "test_c" }));
Assert.False(jointsB.ToArray().SequenceEqual(new string[] { "Test_a", "Test_b", "Test_c" }));
}
public MoveGroupDescription(string name, string[] subMoveGroupIds, JointSet jointSet, string[] endEffectorNames, string[] endEffectorLinkNames)
{
this.Name = name;
this.SubMoveGroupIds = subMoveGroupIds;
this.JointSet = jointSet;
this.EndEffectorNames = endEffectorNames;
this.EndEffectorLinkNames = endEffectorLinkNames;
}
// extenstions to convert C# Xamla.Robotics.Types to Python Xamla.Robotics.Types
public static PyObject ToPython(this JointSet obj)
{
PyObject jointNames = PyConvert.ToPyObject(obj.ToArray());
using (Py.GIL())
{
return(pyXamlaMotionTypes.JointSet(jointNames));
}
}
public void TestIsSimilar()
{
var jointsA = new JointSet("a", "b", "e");
var jointsB = new JointSet("e", "b", "a");
var jointsC = new JointSet("e", "b", "d");
Assert.True(jointsB.IsSimilar(jointsA));
Assert.False(jointsA.IsSimilar(jointsC));
}
public void TestIsSubset()
{
var jointsA = new JointSet("a", "b", "e");
var jointsB = new JointSet("b", "a");
Assert.True(jointsB.IsSubset(jointsA));
Assert.True(jointsA.IsSubset(jointsA));
Assert.True(jointsB.IsSubset(jointsB));
Assert.False(jointsA.IsSubset(jointsB));
}
public void TestToArray()
{
var jointsA = new JointSet("a", "b", "c");
var array = new double[] { 7, 6, 5 };
// test value clone in ctor
var a = new JointValues(jointsA, array);
Assert.Equal(array, a.ToArray());
}
public void TestGetIndex()
{
var joints = new JointSet("a", "c", "b");
Assert.Equal(2, joints.GetIndexOf("b"));
Assert.Equal(0, joints.GetIndexOf("a"));
Assert.Throws <Exception>(() => joints.GetIndexOf("d"));
Assert.Throws <ArgumentNullException>(() => joints.GetIndexOf(null));
}
public void TestTryGetIndex()
{
var joints = new JointSet("a", "c", "b");
var success = joints.TryGetIndexOf("b", out int val1);
Assert.True(success);
Assert.Equal(2, val1);
success = joints.TryGetIndexOf("d", out int val2);
Assert.False(success);
Assert.Equal(-1, val2);
}
public void TestEquals()
{
var jointsA = new JointSet("a", "b", "e");
var jointsB = new JointSet("a", "b", "e");
var jointsC = new JointSet("e", "b", "a");
Assert.True(jointsB.Equals(jointsA));
Assert.True(jointsA.Equals(jointsB));
Assert.True(jointsA.Equals(jointsA));
Assert.False(jointsA.Equals(jointsC));
}
private static JointTrajectoryPoint GetPoint()
{
var timeSpan = new TimeSpan(100);
var jointSet = new JointSet("a", "b", "c");
var pos = new JointValues(jointSet, new double[] { 0, 1, 2 });
var vel = new JointValues(jointSet, new double[] { 3, 4, 5 });
var acc = new JointValues(jointSet, new double[] { 6, 7, 8 });
var eff = new JointValues(jointSet, new double[] { 9, 10, 11 });
var p = new JointTrajectoryPoint(timeSpan, pos, vel, acc, eff);
return(p);
}
public void TestPrepend()
{
var joints = new JointSet("a", "b", "c");
var val1 = new JointValues(joints, new double[] { 1, 2, 3 });
var val2 = new JointValues(joints, new double[] { 5, 5, 5 });
var val3 = new JointValues(joints, new double[] { 0, 0, 0 });
var p = new JointPath(val1).Prepend(new JointValues[] { val2, val3 });
Assert.Equal(val2, p[0]);
Assert.Equal(val3, p[1]);
Assert.Equal(val1, p[2]);
}
public void TestGetValue()
{
var jointsA = new JointSet("a", "b", "c");
var array = new double[] { 7, 6, 5 };
var a = new JointValues(jointsA, array);
Assert.Equal(6, a.GetValue("b"));
Assert.Equal(7, a.GetValue("a"));
Assert.Throws <Exception>(() => a.GetValue("d"));
Assert.Throws <ArgumentNullException>(() => a.GetValue(null));
}
public void TestCombine()
{
var jointsA = new JointSet("a", "b", "e");
// using array
var jointsB = jointsA.Combine(new string[] { "c", "f", "e" });
Assert.True(jointsB.ToArray().SequenceEqual(new string[] { "a", "b", "e", "c", "f" }));
Assert.False(jointsB.ToArray().SequenceEqual(new string[] { "a", "b", "c", "d", "e" }));
// using JointSet
jointsB = jointsA.Combine(new JointSet("c", "f", "e"));
Assert.True(jointsB.ToArray().SequenceEqual(new string[] { "a", "b", "e", "c", "f" }));
Assert.False(jointsB.ToArray().SequenceEqual(new string[] { "a", "b", "c", "d", "e" }));
}
public static Tuple <PlanParameters, JointSet, double[], double[]> CreatePlanParameters(
[InputPin(PropertyMode = PropertyMode.Default)] string moveGroupName = null,
[InputPin(PropertyMode = PropertyMode.Default)] JointSet joints = null,
[InputPin(PropertyMode = PropertyMode.Default)] double[] maxVelocity = null,
[InputPin(PropertyMode = PropertyMode.Default)] double[] maxAcceleration = null,
[InputPin(PropertyMode = PropertyMode.Default)] double sampleResolution = 0.008,
[InputPin(PropertyMode = PropertyMode.Default)] bool checkCollision = true,
[InputPin(PropertyMode = PropertyMode.Default)] double velocityScaling = 1
)
{
var parameters = MotionService.CreatePlanParameters(moveGroupName, joints, maxVelocity, maxAcceleration, sampleResolution, checkCollision, velocityScaling);
return(Tuple.Create(parameters, parameters.JointSet, parameters.MaxVelocity, parameters.MaxAcceleration));
}
public void TestInit()
{
var joints = new JointSet("a", "b", "c");
var p1 = GetPoint(100, joints);
var p2 = GetPoint(200, joints);
var p3 = GetPoint(300, joints);
var t = new JointTrajectory(joints, new JointTrajectoryPoint[] { p1, p2, p3 });
Assert.Equal(3, t.Count);
Assert.Equal(p1, t[0]);
Assert.Equal(p2, t[1]);
Assert.Equal(p3, t[2]);
Assert.Throws <ArgumentException>(() => new JointTrajectory(joints, new JointTrajectoryPoint[] { p1, p3, p2 }));
}
public void TestConcat()
{
var joints = new JointSet("a", "b", "c");
var val1 = new JointValues(joints, new double[] { 1, 2, 3 });
var val2 = new JointValues(joints, new double[] { 5, 5, 5 });
var val3 = new JointValues(joints, new double[] { 0, 0, 0 });
var p1 = new JointPath(val1);
var p2 = new JointPath(val2, val3);
var p = p1.Concat(p2);
Assert.Equal(val1, p[0]);
Assert.Equal(val2, p[1]);
Assert.Equal(val3, p[2]);
}
public void TestInterpolation()
{
void AssertEqualPoints(JointTrajectoryPoint a, JointTrajectoryPoint b)
{
bool Compare(JointValues aa, JointValues bb)
{
double delta = Math.Abs(aa.MaxNorm() - bb.MaxNorm());
return(delta < 1E-6);
}
Assert.Equal(a.TimeFromStart, b.TimeFromStart);
Assert.True(a.JointSet == b.JointSet);
Assert.True(Compare(a.Positions, b.Positions));
Assert.True(Compare(a.Velocities, b.Velocities));
}
var joints = new JointSet("a", "b", "c");
var velocity = new JointValues(joints, new double[] { 1, 1, 1 });
var acceleration = new JointValues(joints, new double[] { 0, 0, 0 });
var positionA = new JointValues(joints, new double[] { 0, 0, 0 });
var positionB = new JointValues(joints, new double[] { 1, 1, 1 });
var positionC = new JointValues(joints, new double[] { 2, 2, 2 });
var timeA = TimeSpan.FromSeconds(1);
var timeB = TimeSpan.FromSeconds(2);
var timeC = TimeSpan.FromSeconds(3);
var pointA = new JointTrajectoryPoint(timeA, positionA, velocity, acceleration);
var pointB = new JointTrajectoryPoint(timeB, positionB, velocity, acceleration);
var pointC = new JointTrajectoryPoint(timeC, positionC, velocity, acceleration);
JointTrajectoryPoint evalA = pointA.InterpolateCubic(pointC, timeA);
AssertEqualPoints(evalA, pointA);
JointTrajectoryPoint evalC = pointA.InterpolateCubic(pointC, timeC);
AssertEqualPoints(evalC, pointC);
// This one is interpolated between A and C
JointTrajectoryPoint evalB = pointA.InterpolateCubic(pointC, timeB);
AssertEqualPoints(evalB, pointB);
// Assert that value out of boundaries are clipped
var timeAOut = new TimeSpan(0);
var timeCOut = new TimeSpan(400000);
JointTrajectoryPoint evalAOut = pointA.InterpolateCubic(pointC, timeAOut);
AssertEqualPoints(evalAOut, pointA.WithTimeFromStart(timeAOut));
JointTrajectoryPoint evalCOut = pointA.InterpolateCubic(pointC, timeCOut);
AssertEqualPoints(evalCOut, pointA.WithTimeFromStart(timeCOut));
}
public void TestTryGetValue()
{
var jointsA = new JointSet("a", "b", "c");
var array = new double[] { 7, 6, 5 };
var a = new JointValues(jointsA, array);
var success = a.TryGetValue("b", out double val1);
Assert.True(success);
Assert.Equal(6.0, val1);
success = a.TryGetValue("d", out double val2);
Assert.False(success);
}
public void TestInit()
{
var joints = new JointSet("a", "b");
var tooManyJoints = new JointSet("a", "b", "c");
var notEnoughJoints = new JointSet("a");
var maxAcc = new double?[] { 10, 10 };
var maxVel = new double?[] { 50, 10 };
var minPos = new double?[] { -10, 0 };
var maxPos = new double?[] { 50, 10 };
JointLimits jl = new JointLimits(joints, maxVel, maxAcc, minPos, maxPos);
Assert.Throws <ArgumentNullException>(() => new JointLimits(joints, null, maxAcc, minPos, maxPos));
Assert.Throws <ArgumentException>(() => new JointLimits(tooManyJoints, maxVel, maxAcc, minPos, maxPos));
Assert.Throws <ArgumentException>(() => new JointLimits(notEnoughJoints, maxVel, maxAcc, minPos, maxPos));
}
public void TestTransform()
{
var joints = new JointSet("a", "b", "c");
var val1 = new JointValues(joints, new double[] { 1, 2, 3 });
var val2 = new JointValues(joints, new double[] { 5, 5, 5 });
var val3 = new JointValues(joints, new double[] { 0, 0, 0 });
var p1 = new JointPath(joints, val1, val2, val3);
Func <JointValues, int, JointValues> func = (jVals, i) => jVals.Add(i);
var p = p1.Transform(func);
Assert.Equal(val1, p[0]);
Assert.Equal(new JointValues(joints, new double[] { 6, 6, 6 }), p[1]);
Assert.Equal(new JointValues(joints, new double[] { 2, 2, 2 }), p[2]);
}
public void TestAppend()
{
var jointsA = new JointSet("a", "b", "c");
// using array
var jointsB = jointsA.Append(new string[] { "d", "e" });
Assert.True(jointsB.ToArray().SequenceEqual(new string[] { "a", "b", "c", "d", "e" }));
// using JointSet
jointsB = jointsA.Append(new JointSet("d", "e"));
Assert.True(jointsB.ToArray().SequenceEqual(new string[] { "a", "b", "c", "d", "e" }));
// Check that exception is thrown when duplicate entry is given
Assert.Throws <Xamla.Utilities.DuplicateElementException>(() => jointsA.Append(new JointSet("d", "e", "a")));
}
public void TestEvaluateAt()
{
void AssertEqualPoints(JointTrajectoryPoint a, JointTrajectoryPoint b)
{
bool Compare(JointValues aa, JointValues bb)
{
double delta = Math.Abs(aa.MaxNorm() - bb.MaxNorm());
return(delta < 1E-6);
}
Assert.Equal(a.TimeFromStart, b.TimeFromStart);
Assert.True(a.JointSet == b.JointSet);
Assert.True(Compare(a.Positions, b.Positions));
Assert.True(Compare(a.Velocities, b.Velocities));
}
var joints = new JointSet("a", "b", "c");
JointTrajectoryPoint[] points = new JointTrajectoryPoint[10];
for (int i = 0; i < 10; ++i)
{
points[i] = GetPoint(i, joints);
}
Assert.Equal(points.Count(), 10);
var traj = new JointTrajectory(joints, points);
for (int i = 0; i < 50; ++i)
{
// Create negative and out of bound time values, to assure exceptions are thrown accordingly
double time = rng.NextDouble() * 12 - 2;
var index = (int)time;
var k = Math.Min(index + 1, 9);
var timeSpan = TimeSpan.FromSeconds(time);
if (time < 0 || time > 9)
{
Assert.Throws <ArgumentOutOfRangeException>(() => traj.EvaluateAt(timeSpan));
}
else
{
var pointGT = traj[index].InterpolateCubic(traj[k], TimeSpan.FromSeconds(time));
// time = Math.Min(time, 9);
// time = Math.Max(time, 0);
var pointEval = traj.EvaluateAt(timeSpan);
AssertEqualPoints(pointGT, pointEval);
}
}
}
public void TestSub()
{
var joints = new JointSet("a", "b", "c");
var val1 = new JointValues(joints, new double[] { 1, 2, 3 });
var val2 = new JointValues(joints, new double[] { 5, 5, 5 });
var val3 = new JointValues(joints, new double[] { 0, 0, 0 });
var val4 = new JointValues(joints, new double[] { 2, 2, 2 });
var pBig = new JointPath(joints, val1, val2, val3, val4);
var p = pBig.Sub(1, 3);
Assert.Equal(val2, p[0]);
Assert.Equal(val3, p[1]);
Assert.Throws <ArgumentOutOfRangeException>(() => p[2]);
Assert.Throws <ArgumentOutOfRangeException>(() => pBig.Sub(1, 5));
}
