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();
}
}