public MoveCartesianPathOperation(MoveCartesianPathArgs args)
        {
            if (args == null)
            {
                throw new ArgumentNullException(nameof(args));
            }

            if (args.EndEffector == null)
            {
                throw new ArgumentException("EndEffector member of arguments must not be null.");
            }

            this.EndEffector         = args.EndEffector;
            this.Seed                = args.Seed;
            this.Waypoints           = args.Waypoints;
            this.StartPose           = args.StartPose;
            this.Start               = args.Start;
            this.MoveGroup           = this.EndEffector.MoveGroup;
            this.VelocityScaling     = args.VelocityScaling;
            this.AccelerationScaling = args.AccelerationScaling;

            this.Parameters          = this.MoveGroup.BuildPlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.SampleResolution);
            this.TaskSpaceParameters = this.EndEffector.BuildTaskSpacePlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.IkJumpThreshold);
            if (args.SampleResolution.HasValue)
            {
                this.TaskSpaceParameters = this.TaskSpaceParameters.WithSampleResolution(args.SampleResolution.Value);
            }
        }
예제 #2
0
        private void SetDefaultEndEffector(string endEffectorName = null, IList <MoveGroupDescription> groupDescription = null)
        {
            var groups       = groupDescription ?? this.motionService.QueryAvailableMoveGroups();
            var groupDetails = groups.ToDictionary(x => x.Name, x => x);

            if (!groupDetails.TryGetValue(this.name, out this.details))
            {
                throw new Exception($"Move group with name '{this.name}' not found.");
            }

            Dictionary <string, IEndEffector> tmp = new Dictionary <string, IEndEffector>();

            for (int i = 0; i < this.details.EndEffectorNames.Length; i++)
            {
                tmp.Add(this.details.EndEffectorNames[i], (IEndEffector) new EndEffector(this, this.details.EndEffectorNames[i], this.details.EndEffectorLinkNames[i]));
            }
            this.endEffectors = tmp;

            if (!this.endEffectors.ContainsKey(endEffectorName ?? ""))
            {
                endEffectorName = this.details.EndEffectorNames.FirstOrDefault(); // use first end effector as default
            }
            this.defaultEndEffectorName = endEffectorName;
            var limits = this.motionService.QueryEndEffectorLimits(endEffectorName);

            this.defaultTaskSpacePlanParameters = this.motionService.CreateTaskSpacePlanParameters(
                this.defaultEndEffectorName,
                limits.MaxXYZVelocity,
                limits.MaxXYZAcceleration,
                limits.MaxAngularVelocity,
                limits.MaxAngularAcceleration
                );
        }
예제 #3
0
        public void TestWithIkJumpThreshold()
        {
            var builder    = new TaskSpacePlanParameters.Builder();
            var parameters = new TaskSpacePlanParameters(builder);

            Assert.Equal(5, parameters.WithIkJumpThreshold(5).IkJumpThreshold);
            Assert.Equal(-3, parameters.WithIkJumpThreshold(-3).IkJumpThreshold);
        }
예제 #4
0
        public void TestWithMaxDeviation()
        {
            var builder    = new TaskSpacePlanParameters.Builder();
            var parameters = new TaskSpacePlanParameters(builder);

            Assert.Equal(5, parameters.WithMaxDeviation(5).MaxDeviation);
            Assert.Equal(-3, parameters.WithMaxDeviation(-3).MaxDeviation);
        }
예제 #5
0
        public void TestWithSampleResolution()
        {
            var builder    = new TaskSpacePlanParameters.Builder();
            var parameters = new TaskSpacePlanParameters(builder);

            Assert.Equal(5, parameters.WithSampleResolution(5).SampleResolution);
            Assert.Equal(-3, parameters.WithSampleResolution(-3).SampleResolution);
        }
예제 #6
0
        public void TestWithCollision()
        {
            var builder    = new TaskSpacePlanParameters.Builder();
            var parameters = new TaskSpacePlanParameters(builder);

            Assert.True(parameters.WithCollisionCheck(true).CollisionCheck);
            Assert.False(parameters.WithCollisionCheck(false).CollisionCheck);
        }
예제 #7
0
        public MovePoseOperationBase(MovePoseArgs args)
        {
            this.EndEffector         = args.EndEffector;
            this.Seed                = args.Seed;
            this.TargetPose          = args.TargetPose;
            this.StartPose           = args.StartPose;
            this.Start               = args.Start;
            this.MoveGroup           = this.EndEffector.MoveGroup;
            this.VelocityScaling     = args.VelocityScaling;
            this.AccelerationScaling = args.AccelerationScaling;

            this.Parameters          = this.MoveGroup.BuildPlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.SampleResolution);
            this.TaskSpaceParameters = this.EndEffector.BuildTaskSpacePlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.IkJumpThreshold);
            if (args.SampleResolution.HasValue)
            {
                this.TaskSpaceParameters = this.TaskSpaceParameters.WithSampleResolution(args.SampleResolution.Value);
            }
        }