public virtual IPlan Plan()
{
if (this.Waypoints.Count == 0)
{
return(new Plan(this.MoveGroup, JointTrajectory.Empty, this.Parameters));
}
JointValues seed = this.Seed ?? this.MoveGroup.CurrentJointPositions;
IKResult targetJoints = this.EndEffector.InverseKinematicMany(this.Waypoints, this.Parameters.CollisionCheck, seed);
if (!targetJoints.Succeeded)
{
throw new Exception("No inverse kinematic solution found for at least one pose of cartesian path.");
}
IJointPath path = targetJoints.Path;
double ikJumpThreshold = this.TaskSpaceParameters.IkJumpThreshold;
for (int i = 0, j = 1; j < path.Count; i = j, j += 1)
{
JointValues delta = path[i] - path[j];
if (delta.MaxNorm() > ikJumpThreshold)
{
throw new DiscontinuityException($"The difference {delta.MaxNorm()} of two consecutive IK solutions for the given cartesian path at index {i} exceeded the IK jump threshold {ikJumpThreshold}.");
}
}
return(this.MoveGroup.MoveJointPath(path)
.With(a => this.ToArgs())
.Plan());
}
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 static IJointPath PlanCollisionFreeJointPathSegment(
[InputPin(PropertyMode = PropertyMode.Allow)] JointValues start,
[InputPin(PropertyMode = PropertyMode.Allow)] JointValues goal,
[InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters
)
{
return(MotionService.PlanCollisionFreeJointPath(start, goal, parameters));
}
public static Pose QueryPose(
[InputPin(PropertyMode = PropertyMode.Default)] string moveGroupName,
[InputPin(PropertyMode = PropertyMode.Allow)] JointValues jointPositions,
[InputPin(PropertyMode = PropertyMode.Default)] string endEffectorLink = ""
)
{
return(MotionService.QueryPose(moveGroupName, jointPositions, endEffectorLink));
}
/// <summary>
/// Computes the pose based on an instance of <c>JointValues</c>.
/// </summary>
/// <param name="jointValues">Joint configuration of the robot</param>
/// <returns>Returns a <c>Pose</c> instance.</returns>
/// <exception cref="ArgumentNullException">Thrown when <paramref name="jointValues"/> is null.</exception>
public Pose ComputePose(JointValues jointValues)
{
if (jointValues == null)
{
throw new ArgumentNullException(nameof(jointValues));
}
return(motionService.QueryPose(moveGroup.Name, jointValues, this.LinkName));
}
public override IPlan Plan()
{
JointValues start = this.Start ?? this.MoveGroup.CurrentJointPositions; // get start joint values
IJointPath jointPath = this.MoveGroup.MotionService.PlanCollisionFreeJointPath(start, this.Target, this.Parameters); // generate joint path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveJoints(jointPath, this.Parameters); // plan trajectory
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
public void TestInterpolate()
{
// first argument sets the order if the names in the jointset
var a = new JointValues(new JointSet("a", "b", "c"), new double[] { 2, 2, 2 });
var b = new JointValues(new JointSet("b", "a", "c"), new double[] { -2, 1, 3 });
Assert.Equal(new double[] { 1.5, 0, 2.5 }, JointValues.Interpolate(a, b).ToArray());
Assert.Equal(new double[] { 1.75, 1, 2.25 }, JointValues.Interpolate(a, b, 0.25).ToArray());
}
public override IPlan Plan()
{
JointValues seed = this.Seed ?? this.MoveGroup.CurrentJointPositions; // seed jointvalues
Pose startPose = this.StartPose ?? this.EndEffector.CurrentPose; // start pose
CartesianPath posePath = new CartesianPath(startPose, this.TargetPose); // generate taskspace path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveCartesianPathLinear(posePath, seed, this.TaskSpaceParameters);
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
public static PyObject ToPython(this JointValues obj)
{
PyObject jointSet = obj.JointSet.ToPython();
PyObject values = PyConvert.ToPyObject(obj.Values);
using (Py.GIL())
{
return(pyXamlaMotionTypes.JointValues(jointSet, values));
}
}
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 static JointValues InverseKinematic(
[InputPin(PropertyMode = PropertyMode.Allow)] Pose pose,
[InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters,
[InputPin(PropertyMode = PropertyMode.Allow)] JointValues jointPositionSeed = null,
[InputPin(PropertyMode = PropertyMode.Default)] string endEffectorLink = "",
[InputPin(PropertyMode = PropertyMode.Default)] TimeSpan?timeout = null,
[InputPin(PropertyMode = PropertyMode.Default)] int attempts = 1
)
{
return(MotionService.InverseKinematic(pose, parameters, jointPositionSeed, endEffectorLink, timeout, attempts));
}
/// <summary>
/// Get inverse kinematic solution for one pose.
/// </summary>
/// <param name="pose">The pose to be transformmed to joint space</param>
/// <param name="avoidCollision">If true the trajectory planing tries to plan collision free trajectory and before executing a trajectory a collision check is performed.</param>
/// <param name="jointPositionSeed">Optional seed joint configuration</param>
/// <param name="timeout">Timeout</param>
/// <param name="attempts">Attempts</param>
/// <returns>Returns the joint configuration as a <c>JointValues</c> instance.</returns>
public JointValues InverseKinematic(
Pose pose,
bool avoidCollision,
JointValues jointPositionSeed = null,
TimeSpan?timeout = null,
int attempts = 1
)
{
var parameters = moveGroup.DefaultPlanParameters.WithCollisionCheck(avoidCollision);
return(motionService.InverseKinematic(pose, parameters, jointPositionSeed, linkName, timeout, attempts));
}
public static async Task UpdateJointValues(
[InputPin(PropertyMode = PropertyMode.Default)] string elementPath,
[InputPin(PropertyMode = PropertyMode.Allow)] JointValues jointValues
)
{
var trace = await worldViewService.ResolvePath(elementPath, WorldViewElementType.JointValues);
var element = await worldViewService.GetJointValuesByTrace(trace);
element.Value = jointValues.ToModel();
await worldViewService.UpdateJointValues(element);
}
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 static IKResult InverseKinematicMany(
[InputPin(PropertyMode = PropertyMode.Allow)] IEnumerable <Pose> points,
[InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters,
[InputPin(PropertyMode = PropertyMode.Allow)] JointValues jointPositionSeed = null,
[InputPin(PropertyMode = PropertyMode.Default)] string endEffectorLink = "",
[InputPin(PropertyMode = PropertyMode.Default)] TimeSpan?timeout = null,
[InputPin(PropertyMode = PropertyMode.Default)] int attempts = 1,
[InputPin(PropertyMode = PropertyMode.Default)] bool constSeed = false
)
{
return(MotionService.InverseKinematicMany(points, parameters, jointPositionSeed, endEffectorLink, timeout, attempts, constSeed));
}
/// <summary>
/// Get inverse kinematic solutions for multiple poses.
/// </summary>
/// <param name="poses">The poses to be transformmed to joint space</param>
/// <param name="avoidCollision">If true the trajectory planing tries to plan collision free trajectory and before executing a trajectory a collision check is performed.</param>
/// <param name="jointPositionSeed">Optional seed joint configuration</param>
/// <param name="timeout">Timeout</param>
/// <param name="attempts">The amount of attempts</param>
/// <param name="constSeed">Determines if for each pose in poses the same seed should be used.</param>
/// <returns>Returns the results as an instance of <c>IKResult</c>.</returns>
public IKResult InverseKinematicMany(
IEnumerable <Pose> poses,
bool avoidCollision,
JointValues jointPositionSeed = null,
TimeSpan?timeout = null,
int attempts = 1,
bool constSeed = false
)
{
var parameters = moveGroup.DefaultPlanParameters.WithCollisionCheck(avoidCollision);
return(motionService.InverseKinematicMany(poses, parameters, jointPositionSeed, linkName, timeout, attempts, constSeed));
}
public void TestMath()
{
var a = new JointValues(new JointSet("a", "b", "c"), new double[] { 2, 2, 2 });
var b = new JointValues(new JointSet("b", "a", "c"), new double[] { 2, 1, 3 });
Assert.True(a.Add(b).Values.SequenceEqual(new double[] { 3, 4, 5 }));
Assert.True(b.Add(-1).Values.SequenceEqual(new double[] { 1, 0, 2 }));
Assert.True(a.Subtract(b).Values.SequenceEqual(new double[] { 1, 0, -1 }));
Assert.True(b.Subtract(1).Values.SequenceEqual(new double[] { 1, 0, 2 }));
Assert.True(b.Divide(2).Values.SequenceEqual(new double[] { 1, 0.5, 1.5 }));
Assert.True(b.Multiply(2).Values.SequenceEqual(new double[] { 4, 2, 6 }));
Assert.True(b.Negate().Values.SequenceEqual(new double[] { -2, -1, -3 }));
}
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 static async Task AddJointValues(
[InputPin(PropertyMode = PropertyMode.Default)] string elementPath,
[InputPin(PropertyMode = PropertyMode.Allow)] JointValues jointValues,
[InputPin(PropertyMode = PropertyMode.Default)] bool transient = false
)
{
var path = new PosixPath(elementPath);
string containerPath = path.RemoveLastElement().ToPosixPath();
string displayName = path.LastElement;
var containerTrace = await worldViewService.ResolvePath(containerPath);
await worldViewService.AddJointValues(containerTrace, jointValues.ToModel(), displayName, transient);
}
private MovePoseArgs CreateMovePoseArgs(Pose target, JointValues seed = null) =>
new MovePoseArgs
{
TargetPose = target,
Seed = seed,
EndEffector = this,
MoveGroup = this.MoveGroup,
IkJumpThreshold = this.MoveGroup.IkJumpThreshold,
VelocityScaling = this.MoveGroup.VelocityScaling,
AccelerationScaling = this.MoveGroup.VelocityScaling,
CollisionCheck = this.MoveGroup.CollisionCheck,
SampleResolution = this.MoveGroup.SampleResolution,
MaxDeviation = this.MoveGroup.MaxDeviation
};
public override IPlan Plan()
{
if (this.Waypoints.Count == 0)
{
return(new Plan(this.MoveGroup, JointTrajectory.Empty, this.Parameters));
}
JointValues seed = this.Seed ?? this.MoveGroup.CurrentJointPositions; // get current posture for seed jointvalues
Pose startPose = this.StartPose ?? this.EndEffector.CurrentPose; // get current pose
ICartesianPath path = this.Waypoints.Prepend(startPose); // generate joint path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveCartesianPathLinear(path, seed, this.TaskSpaceParameters); // plan trajectory
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
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 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 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 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 TestInit()
{
var pos = new JointValues(new JointSet("a", "b", "c"), new double[] { 2, 3, 4 });
var vel = new JointValues(new JointSet("a", "b", "c"), new double[] { 2, 3, 4 });
var eff = new JointValues(new JointSet("a", "b", "c"), new double[] { 2, 3, 4 });
var js = new JointStates(pos, vel, eff);
Assert.NotNull(js.JointSet);
Assert.Null(new JointStates(null, null, null).JointSet);
// all jointset must match
var badEff = new JointValues(new JointSet("f", "b", "c"), new double[] { 2, 3, 4 });
Assert.Throws <Exception>(() => new JointStates(pos, vel, badEff));
}
/// <summary>
/// Send velocities to service
/// </summary>
/// <param name="velocities">Velocities to sent [m/s]</param>
public void SendVelocities(JointValues velocities)
{
var point = new trajectory_msgs.JointTrajectoryPoint
{
time_from_start = TimeSpan.FromSeconds(0.008).ToDurationMessage(),
velocities = velocities.ToArray(),
};
var trajectory = new trajectory_msgs.JointTrajectory()
{
joint_names = velocities.JointSet.ToArray(),
points = new [] { point }
};
publisherJoggingCommand.Publish(trajectory);
}
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));
}
