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 CartesianPath obj) { using (Py.GIL()) { var poses = new PyList(); foreach (var p in obj) { poses.Append(PyConvert.ToPyObject(p)); } return(pyXamlaMotionTypes.CartesianPath(poses)); } }
public static async Task UpdateCartesianPath( [InputPin(PropertyMode = PropertyMode.Default)] string elementPath, [InputPin(PropertyMode = PropertyMode.Allow)] CartesianPath cartesianPath ) { var trace = await worldViewService.ResolvePath(elementPath, WorldViewElementType.JointValues); var element = await worldViewService.GetCartesianPathByTrace(trace); element.Value = cartesianPath.ToModel(); await worldViewService.UpdateCartesianPath(element); }
public void TestInit() { int count = 50; List <Pose> poses = poseHelper.RandomPoses(count).ToList(); var path = new CartesianPath(poses); int i = 0; foreach (var pose in poses) { Assert.Equal(pose, path[i]); ++i; } Assert.Equal(count, path.Count()); }
public static async Task AddCartesianPath( [InputPin(PropertyMode = PropertyMode.Default)] string elementPath, [InputPin(PropertyMode = PropertyMode.Allow)] CartesianPath cartesianPath, [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.AddCartesianPath(containerTrace, cartesianPath.ToModel(), displayName, transient); }
public void TestAppend() { int count = 50; List <Pose> poses1 = poseHelper.RandomPoses(count).ToList(); List <Pose> poses2 = poseHelper.RandomPoses(count).ToList(); List <Pose> poses = poses1.Concat(poses2).ToList(); ICartesianPath path = new CartesianPath(poses1); path = path.Append(poses2); for (int i = 0; i < 2 * count; ++i) { Assert.Equal(poses[i], path[i]); } Assert.Equal(2 * count, path.Count()); }
public async static Task MovePoseWaypointsSupervised( [InputPin(PropertyMode = PropertyMode.Never)] IEnumerable <Pose> waypoints = null, [InputPin(PropertyMode = PropertyMode.Default)] string endEffectorName = null, [InputPin(PropertyMode = PropertyMode.Default)] bool collisionChecking = false, [InputPin(PropertyMode = PropertyMode.Default)] double velocityScaling = 1, [InputPin(PropertyMode = PropertyMode.Default)] double sampleResolution = 0.05, [InputPin(PropertyMode = PropertyMode.Default, Editor = "JointValues")] JointValuesProperty seed = null, [InputPin(PropertyMode = PropertyMode.Default)] bool cacheResult = true, CancellationToken cancel = default(CancellationToken) ) { if (waypoints == null) { throw new ArgumentNullException("Required property 'waypoints' for MovePoseLinearWaypointsSupervised module was not specified.", nameof(waypoints)); } if (waypoints.ToList().Count == 0) { throw new ArgumentException("Required property 'waypoints' is empty.", nameof(waypoints)); } var seedValues = await ResolveProperty(seed); var endEffector = MotionService.QueryAvailableEndEffectors().FirstOrDefault(x => x.Name == endEffectorName); if (endEffector == null) { throw new Exception($"EndEffector '{endEffectorName}' not available."); } CartesianPath path = new CartesianPath(waypoints); using (var group = MotionService.CreateMoveGroup(endEffector.MoveGroupName, endEffector.Name)) { if (seed == null) { seedValues = group.CurrentJointPositions; } group.SampleResolution = sampleResolution; group.CollisionCheck = collisionChecking; group.VelocityScaling = velocityScaling; using (var client = group.GetEndEffector(endEffectorName).MoveCartesianPathSupervisedAsync(path, seedValues, cancel)) { await HandleStepwiseMotions(client, group); } } }
public void TestSub() { int count = 10; List <Pose> poses = poseHelper.RandomPoses(count).ToList(); ICartesianPath pathBig = new CartesianPath(poses); int beg = 2; int end = 5; ICartesianPath path = pathBig.Sub(beg, end); Assert.Equal(end - beg, path.Count()); for (int i = 0; i < end - beg; ++i) { Assert.Equal(pathBig[i + beg], path[i]); } Assert.Throws <ArgumentOutOfRangeException>(() => path.Sub(0, count + 1)); Assert.Throws <ArgumentOutOfRangeException>(() => path.Sub(-1, count - 1)); }
public async static Task MoveLWaypoints( [InputPin(PropertyMode = PropertyMode.Never)] IEnumerable <Pose> waypoints = null, [InputPin(PropertyMode = PropertyMode.Default)] string endEffectorName = null, [InputPin(PropertyMode = PropertyMode.Default)] bool collisionChecking = false, [InputPin(PropertyMode = PropertyMode.Default)] double velocityScaling = 1, [InputPin(PropertyMode = PropertyMode.Default)] double sampleResolution = 0.05, [InputPin(PropertyMode = PropertyMode.Default)] double ikJumpThreshold = 1.6, [InputPin(PropertyMode = PropertyMode.Default)] double maxDeviation = 0.0, [InputPin(PropertyMode = PropertyMode.Default)] bool cacheResult = true, CancellationToken cancel = default(CancellationToken) ) { if (waypoints == null) { throw new ArgumentNullException("Required property 'waypoints' for MoveLWayPoints module was not specified.", nameof(waypoints)); } if (waypoints.ToList().Count == 0) { throw new ArgumentException("Required property 'waypoints' is empty.", nameof(waypoints)); } var endEffector = MotionService.QueryAvailableEndEffectors().FirstOrDefault(x => x.Name == endEffectorName); if (endEffector == null) { throw new Exception($"EndEffector '{endEffectorName}' not available."); } CartesianPath path = new CartesianPath(waypoints); using (var group = MotionService.CreateMoveGroup(endEffector.MoveGroupName, endEffector.Name)) { group.SampleResolution = sampleResolution; group.IkJumpThreshold = ikJumpThreshold; var planParameters = group.BuildTaskSpacePlanParameters(velocityScaling, collisionChecking, maxDeviation, null, endEffector.Name); var(trajectory, parameters) = group.GetEndEffector(endEffector.Name).PlanMovePoseLinearWaypoints(path, planParameters); await MotionService.ExecuteJointTrajectory(trajectory, parameters.CollisionCheck, cancel); } }