public override void Execute(PropertyCollection args)
        {
            ms.AppendMessage("Executing RegisterRobot...", MessageLevel.Warning);

            //TODO: Fix the hard index access or at least print out a message if input was wrong
            String robotName = (String)args.GetByIndex(0).Value;
            IRobot robot     = app.Value.World.FindComponent(robotName).GetRobot();

            if (robot != null)
            {
                if (RobotController.getInstance().RegisterRobot(robot))
                {
                    ms.AppendMessage("Robot registered at RobotController instance.", MessageLevel.Warning);
                }
                else
                {
                    ms.AppendMessage("Robot Registration failed!", MessageLevel.Warning);
                }
            }
        }
示例#2
0
        /// <summary>
        /// Calculates safety separation distance and allowed velocity, when SSM_Data object is received.
        /// </summary>
        /// <param name="data"> Information regarding the robot and a sensed human </param>
        public void Handle(SSM_Data data)
        {
            double separationDistance = separationCalculator
                                        .GetSeparationDistance(data.GetHumanVelocity(), data.GetRobotVelocity());
            double allowedVelocity = speedCalculator
                                     .GetAllowedVelocity(data.GetBodyPart(), data.GetHumanVelocity(), data.GetContactArea());

            string message = "Distance: " + separationDistance + "| Velocity: " + allowedVelocity;

            IMessageService ms = IoC.Get <IMessageService>();

            ms.AppendMessage(message, MessageLevel.Warning);
        }
        /// <summary>
        /// Plugin Initialization. Only called once at program start.
        /// </summary>
        public void Initialize()
        {
            if (!(IoC.Get <IPluginManager>().queryPlugin(PluginCategory.Controller)))
            {
                return;
            }
            ConfigReader.init();


            this.app.Simulation.SimulationStarted += SimulationStarted;
            this.app.Simulation.SimulationStopped += SimulationStopped;
            this.app.Simulation.SimulationReset   += SimulationReset;
            //IoC.Get<ISimulationService>().PropertyChanged += JointConfigurationChanged;
            ms = IoC.Get <IMessageService>();
            statisticsManager                    = this.app.StatisticsManager;
            statisticsManager.IsEnabled          = true;
            statisticsManager.StatisticsInterval = TICK_INTERVAL;
            // Must be at least Warning, Info level is not printed
            ms.AppendMessage("DynamicRobotControl Plugin started initialization...", MessageLevel.Warning);

            app.World.ComponentAdded    += World_ComponentAdded;
            app.World.ComponentRemoving += World_ComponentRemoving;
            //IoC.Get<ISimulationService>().PropertyChanged += SimulationPropertyChanged;
        }
示例#4
0
 public void VcWriteWarningMsg(string message)
 {
     _ms.AppendMessage("[vc2opcua] " + message, MessageLevel.Warning);
 }
        /// <summary>
        /// Simple function to update the size of the cylinder which visualizes the current separation distance.
        /// </summary>
        /// <param name="robot"></param>The robot for which the update should be made.
        private void UpdateVisualizationDistance(IRobot robot)
        {
            if (robot != null && robot.IsValid && app.World.FindComponent("SeparationVisualization_" + robot.Name) != null)
            {
                ISimComponent comp = app.World.FindComponent("SeparationVisualization_" + robot.Name);
                ISimNode      node = robot.Component.FindNode("mountplate");

                Matrix matrix = comp.TransformationInReference;
                matrix.SetP(new Vector3(node.TransformationInWorld.Px, node.TransformationInWorld.Py, 201));

                comp.TransformationInReference = matrix;

                ICylinderFeature cylinder = (ICylinderFeature)app.World.FindComponent("SeparationVisualization_" + robot.Name).FindFeature("SeparationVisualization");
                if (robotList[robot].currentSeperationDistance <= 100)
                {
                    cylinder.GetProperty("Radius").Value         = "100";
                    comp.GetProperty("SeparationDistance").Value = "100";
                }
                else
                {
                    cylinder.GetProperty("Radius").Value =
                        robotList[robot].currentSeperationDistance.ToString();
                    comp.GetProperty("SeparationDistance").Value = robotList[robot].currentSeperationDistance;
                }
                cylinder.Rebuild();
            }
            else
            {
                ms.AppendMessage("UpdateVisualizationDistance: Failed to find robot component!", MessageLevel.Warning);
            }
        }
        public override void Execute(PropertyCollection args)
        {
            if (args.Count < 6)
            {
                ms.AppendMessage("Too few arguments were passed to StartMovementActionItem. [robotName, startFrameName, goalFrameName, maxAllowedCartesianSpeed, payload, stapleComponentName]", MessageLevel.Warning);
                return;
            }

            //TODO: Fix the hard index access or at least print out a message if input was wrong
            String        robotName   = (String)args.GetByIndex(0).Value;
            ISimComponent robotParent = app.Value.World.FindComponent(robotName);

            robot = robotParent.GetRobot();

            String startFrameName           = (String)args.GetByIndex(1).Value;
            String goalFrameName            = (String)args.GetByIndex(2).Value;
            int    maxAllowedCartesianSpeed = (int)args.GetByIndex(3).Value;
            String payload = (String)args.GetByIndex(4).Value;
            //String stapleComponentName = "StapleForRightIiwa"
            String stapleComponentName = (String)args.GetByIndex(5).Value;


            RobotSection parameter = ConfigReader.readSection(robotName);


            RobotController.getInstance().setMaxAllowedCartesianSpeed(robot, maxAllowedCartesianSpeed);
            try
            {
                motionPlanCollectionLock.EnterWriteLock();
                motionPlanCollection.Clear();
                if (!motionPlanCollection.TryGetValue(robotParent, out motionPlan))
                {
                    mpm        = new MotionPlanningManager();
                    motionPlan = mpm.InitializeMotionPlanner(robot,
                                                             parameter.urdfFile.Path,
                                                             RobotParameters.KinStart, RobotParameters.KinEnd,
                                                             parameter.obsFile.Path);
                    motionPlanCollection.Add(robotParent, motionPlan);
                    //ms.AppendMessage("Created new motionPlan for " + robotName, MessageLevel.Warning);
                }
            } finally {
                motionPlanCollectionLock.ExitWriteLock();
            }

            if (stapleComponentName != null && stapleComponentName != "")
            {
                stapleConfig(stapleComponentName);
            }

            ////motionPlan.showSetupInInspector();

            VectorOfDoubleVector resultMotion = mpm.planMotion(robot, motionPlan, startFrameName, goalFrameName);

            if (resultMotion != null)
            {
                IBehavior beh = robot.Component.FindBehavior("MovementFinished");
                if (beh != null && beh is IStringSignal)
                {
                    IStringSignal movementFinished = (IStringSignal)robot.Component.FindBehavior("MovementFinished");
                    movementFinished.Value = "";     // empty string means no payload contained yet

                    MotionInterpolator inp = motionPlan.getMotionInterpolator();
                    inp.setMaxJointAcceleration(7.5);
                    inp.setMaxJointVelocity(75.0);
                    RobotController.getInstance().AddMotionPlan(robot, payload, motionPlan);
                }
                else
                {
                    ms.AppendMessage("\"MovementFinished\" behavior was either null or not of type IStringSignal. Abort!", MessageLevel.Warning);
                }
            }
            else
            {
                motionPlan.showSetupInInspector();
            }
        }
 public StartMovementActionItem() : base("StartMovement")
 {
     ms = IoC.Get <IMessageService>();
     ms.AppendMessage("Constructor of StartMovement Action Item called", MessageLevel.Warning);
 }
 public RegisterRobotActionItem() : base("RegisterRobot")
 {
     ms = IoC.Get <IMessageService>();
     ms.AppendMessage("Constructor of RegisterRobot Action Item called", MessageLevel.Warning);
 }