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)); }