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