public static IJointTrajectory PlanMoveJoints( [InputPin(PropertyMode = PropertyMode.Allow)] IJointPath path, [InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters ) { return(MotionService.PlanMoveJoints(path, parameters)); }
public static IJointPath PlanCollisionFreeJointPath( [InputPin(PropertyMode = PropertyMode.Allow)] IJointPath waypoints, [InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters ) { return(MotionService.PlanCollisionFreeJointPath(waypoints, parameters)); }
public static ICartesianPath PlanMoveCartesian( [InputPin(PropertyMode = PropertyMode.Allow)] ICartesianPath path, [InputPin(PropertyMode = PropertyMode.Default)] int numSteps, [InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters ) { return(MotionService.PlanMoveCartesian(path, numSteps, parameters)); }
public void Init(PlanParameters planParams) { if (planParams != null && planParams.Set) { StartPoint = DatabaseService.GetInstance().GetPoint(planParams.StartId); _startText = StartPoint.Label; } }
public void TestWithMaxDeviation() { var builder = new PlanParameters.Builder(); var parameters = new PlanParameters(builder); Assert.Equal(5, parameters.WithMaxDeviation(5).MaxDeviation); Assert.Equal(-3, parameters.WithMaxDeviation(-3).MaxDeviation); }
public void TestWithSampleResolution() { var builder = new PlanParameters.Builder(); var parameters = new PlanParameters(builder); Assert.Equal(5, parameters.WithSampleResolution(5).SampleResolution); Assert.Equal(-3, parameters.WithSampleResolution(-3).SampleResolution); }
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)); }
/// <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 TestWithCollision() { var builder = new PlanParameters.Builder(); var parameters = new PlanParameters(builder); Assert.True(parameters.WithCollisionCheck(true).CollisionCheck); Assert.False(parameters.WithCollisionCheck(false).CollisionCheck); }
public static IJointPath PlanCartesianPath( [InputPin(PropertyMode = PropertyMode.Allow)] ICartesianPath waypoints, [InputPin(PropertyMode = PropertyMode.Default)] int numSteps, [InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters ) { return(MotionService.PlanCartesianPath(waypoints, numSteps, parameters)); }
public MoveJointsOperationBase(MoveJointsArgs args) { this.Start = args.Start; this.Target = args.Target; this.MoveGroup = args.MoveGroup; this.VelocityScaling = args.VelocityScaling; this.AccelerationScaling = args.AccelerationScaling; this.Parameters = this.MoveGroup.BuildPlanParameters(VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.SampleResolution); }
public MoveJointPathOperation(MoveJointPathArgs args) { this.Start = args.Start; this.Waypoints = args.Waypoints; this.MoveGroup = args.MoveGroup; this.VelocityScaling = args.VelocityScaling; this.AccelerationScaling = args.AccelerationScaling; this.Parameters = this.MoveGroup.BuildPlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.SampleResolution); }
public static IJointPath PlanCartesianPathSegment( [InputPin(PropertyMode = PropertyMode.Allow)] Pose start, [InputPin(PropertyMode = PropertyMode.Allow)] Pose goal, [InputPin(PropertyMode = PropertyMode.Default)] int numSteps, [InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters ) { return(MotionService.PlanCartesianPath(start, goal, numSteps, parameters)); }
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)); }
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> /// Create a instance of <c>MoveGroup</c> /// </summary> /// <param name="motionService">An object implementing <c>IMotionService</c> which is used to communicate with the motion server.</param> /// <param name="moveGroupName">The name of the move group represented by this instance</param> /// <param name="defaultEndEffectorName">Name of the default end effector</param> /// <exception cref="Exception">Thrown when current name does not exist. </exception> /// <exception cref="ArgumentNullException">Thrown when <paramref name="motionService"/> is null.</exception> public MoveGroup(IMotionService motionService, string moveGroupName = null, string defaultEndEffectorName = null) { this.motionService = motionService ?? throw new ArgumentNullException(nameof(motionService)); var groups = motionService.QueryAvailableMoveGroups(); var groupDetails = groups.ToDictionary(x => x.Name, x => x); this.name = moveGroupName ?? groups.First().Name; if (!groupDetails.TryGetValue(this.name, out this.details)) { throw new Exception($"Move group with name '{this.name}' not found."); } this.jointSet = this.details.JointSet; this.defaultPlanParameters = motionService.CreatePlanParameters(this.name, this.jointSet, null, null, 0.05, false, 1); this.velocityScaling = 1; this.SetDefaultEndEffector(defaultEndEffectorName, groups); }
public static PyObject ToPython(this PlanParameters obj) { var nullObject = new object(); nullObject = null; var none = nullObject.ToPython(); var moveGroupName = PyConvert.ToPyObject(obj.MoveGroupName); var jointSet = PyConvert.ToPyObject(obj.JointSet); var maxVelocity = PyConvert.ToPyObject(obj.MaxVelocity); var maxAcceleration = PyConvert.ToPyObject(obj.MaxAcceleration); var sampleResolution = PyConvert.ToPyObject(obj.SampleResolution); var collisionCheck = PyConvert.ToPyObject(obj.CollisionCheck); var maxDeviation = PyConvert.ToPyObject(obj.MaxDeviation); using (Py.GIL()) { return(pyXamlaMotionTypes.PlanParameters.from_arguments(moveGroupName, jointSet, maxVelocity, maxAcceleration, none, none, sample_resolution: sampleResolution, collisionCheck: collisionCheck, max_devitation: maxDeviation)); } }
public Plan(IMoveGroup moveGroup, IJointTrajectory trajectory, PlanParameters parameters) { this.MoveGroup = moveGroup; this.Trajectory = trajectory; this.Parameters = parameters; }