private void stapleConfig(String stapleComponentName)
{
StapleSection parameterStaple = ConfigReader.readStapleConfig();
ISimComponent stapleComponent = app.Value.World.FindComponent(stapleComponentName);
if (stapleComponentName != "" && stapleComponent == null)
{
ms.AppendMessage("Failed to find staple component with name\"" + stapleComponentName + "\"! Planning of motion aborted...", MessageLevel.Warning);
return;
}
String stapleFileFolder = parameterStaple.staplePath.Path;
if (stapleComponent.GetProperty("stlID") == null)
{
ms.AppendMessage("Component with name \"" + stapleComponentName + "\" has no property \"stlID\"! Planning of motion aborted...", MessageLevel.Warning);
return;
}
String stlID = (String)stapleComponent.GetProperty("stlID").Value;
String obstacleFilePath = stapleFileFolder + stlID + ".stl";
if (!File.Exists(obstacleFilePath))
{
ms.AppendMessage("A path planning request requested stlID with \"" + stlID + "\" failed, because file \"" + obstacleFilePath + "\" does not exist...", MessageLevel.Warning);
return;
}
;
if (stapleComponent.GetProperty("StackHeight") == null)
{
ms.AppendMessage("Failed to find StackHeight property in component with name \"" + stapleComponentName + "\"! Planning of motion aborted!", MessageLevel.Warning);
return;
}
Vector3 staplePosition = stapleComponent.TransformationInWorld.GetP();
IDoubleProperty stackwidth = (IDoubleProperty)stapleComponent.GetProperty("StackWidth");
IDoubleProperty stacklength = (IDoubleProperty)stapleComponent.GetProperty("StackLength");
float translate_x = (float)staplePosition.X - (float)(stacklength.Value / 2.0);
float translate_y = (float)staplePosition.Y - (float)(stackwidth.Value / 2.0);
IDoubleProperty stackheight = (IDoubleProperty)stapleComponent.GetProperty("StackHeight");
float translate_z = (float)stackheight.Value;
// setup staple obstacle
int obstacleId = motionPlan.addObstacle(obstacleFilePath, translate_x / 1000.0f, translate_y / 1000.0f, translate_z / 1000.0f, 0.0f, 0.0f, 0.0f);
}
/// <summary>
/// Initialize a motionPlanner instance for a specific robot.
/// This instance is stored in the dictonary of the class which manages and maintains the instances of all motionPlanners of all robots.
/// </summary>
/// <param name="robot"></param>The VC robot instance for which a motionPlanner instance should be created
/// <param name="pathToRobotUrdfDescriptionFile"></param>The URDF description of the robot
/// <param name="kinChainStart"></param>The name of the first part in the kinematic chain.
/// <param name="kinChainEnd"></param>The name of the last part in the kinematic chain.
/// <param name="pathToObstacleFile"></param>The path to an obstacle model file (e.g. stl format).
public MotionPlan InitializeMotionPlanner(IRobot robot, String pathToRobotUrdfDescriptionFile, String kinChainStart, String kinChainEnd, String pathToObstacleFile)
{
MotionPlan motionPlan = new MotionPlan(pathToRobotUrdfDescriptionFile, kinChainStart, kinChainEnd);
if (!motionPlan.isValid())
{
Console.WriteLine("Failed to initialize motionPlan, unable to load robot urdf file \"" + pathToRobotUrdfDescriptionFile + "\" from " + kinChainStart + " to " + kinChainEnd + "!");
return(null);
}
MotionPlanRobotDescription description = motionPlan.getMotionPlanRobotDescription();
Vector3 wpr = robot.Component.TransformationInWorld.GetWPR();
description.setRobotPosition(robot.Component.TransformationInWorld.GetP().X / 1000,
robot.Component.TransformationInWorld.GetP().Y / 1000,
robot.Component.TransformationInWorld.GetP().Z / 1000);
description.setRobotRotation(wpr.X, wpr.Y, wpr.Z);
motionPlan.addObstacle(pathToObstacleFile);
return(motionPlan);
}