public static RobotSection readSection(String name)
        {
            RobotSection sec = config.Sections[name] as RobotSection;

            if (sec == null)
            {
                sec = new RobotSection();
                sec.obsFile.Path  = "C:\\defaultPath";
                sec.urdfFile.Path = "C:\\defaultPath2";
                config.Sections.Add(name, sec);
                config.Save();
            }
            return(sec);
        }
        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();
            }
        }