/// <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); }
/// <summary> /// Adds a new motion plan for a specific robot which would come from path planning. /// This motion plan contains at least two or more (the intermediate) joint angle configuration to reach the goal position. /// Furthermore motion interpolator and motion tester are initialized and the motion plan from path planning /// is subdived. /// </summary> /// <param name="robot"></param>The robot to which the motion plan belongs and for which it should be saved. /// <param name="motionPlan"></param> public void AddMotionPlan(IRobot robot, String payloadOnFinishMovement, MotionPlan motionPlan) { try { robotListLock.EnterWriteLock(); try { robotList[robot].motionPlan = motionPlan; robotList[robot].currentMotionStartTime = app.Simulation.Elapsed; robotList[robot].payloadOnFinishMovement = payloadOnFinishMovement; //ms.AppendMessage("New motion plan (" + motionPlan.getLastResult().Count + ") set for robot " + robot.Name + " starting at " + app.Simulation.Elapsed, MessageLevel.Warning); } catch (KeyNotFoundException e) { ms.AppendMessage("Motion plan could not be added, maybe robot was not registred?", MessageLevel.Warning); ms.AppendMessage(e.ToString(), MessageLevel.Error); } } finally { robotListLock.ExitWriteLock(); } }
public void Post([FromBody] MotionPlan value) { Response.StatusCode = 501; }
/// <summary> /// Trigger and receive a concrete motionPlan from startFrame to goalFrame. /// Obstacles must be added to the motionPlanner instance of the robot before. /// </summary> /// <param name="robot"></param>The robot for which a motion should be planned. /// <param name="motionPlan"></param>The preconfigured motionPlan which should be used. /// <param name="startFrame"></param>The frame where the motion should start. /// <param name="goalFrame"></param>The frame where the motion should end. /// <returns></returns> public VectorOfDoubleVector planMotion(IRobot robot, MotionPlan motionPlan, String startFrame, String goalFrame) { MotionPlanRobotDescription description = motionPlan.getMotionPlanRobotDescription(); // we need the current position of the robot to enhance the result of the inverse kinematics VectorOfDouble currentPositionJointAngles = new VectorOfDouble(robot.RobotController.Joints.Count); if (robot.RobotController.Joints.Count == 7) { currentPositionJointAngles.Add(robot.RobotController.Joints[0].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[1].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[6].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[2].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[3].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[4].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[5].Value); } else { currentPositionJointAngles.Add(robot.RobotController.Joints[0].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[1].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[2].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[3].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[4].Value); currentPositionJointAngles.Add(robot.RobotController.Joints[5].Value); } //TODO: Make those frames global? IFeature startNode = robot.Component.FindFeature(startFrame); if (startNode == null) { IoC.Get <IMessageService>().AppendMessage("Start Frame \"" + startFrame + "\" was not found.", MessageLevel.Error); return(null); } IFeature goalNode = robot.Component.FindFeature(goalFrame); if (goalNode == null) { IoC.Get <IMessageService>().AppendMessage("Goal Frame \"" + goalFrame + "\" was not found.", MessageLevel.Error); return(null); } Matrix startPosition = robot.Component.RootNode.GetFeatureTransformationInWorld(startNode); Matrix goalPosition = robot.Component.RootNode.GetFeatureTransformationInWorld(goalNode); Vector3 startRotation = startPosition.GetWPR(); Vector3 goalRotation = goalPosition.GetWPR(); Vector3 robotPosition = robot.Component.TransformationInWorld.GetP(); VectorOfDouble startJointAngles = currentPositionJointAngles; //IoC.Get<IMessageService>().AppendMessage("Goal Frame \"" + goalFrame + "\": [" + String.Format("{0:0.000}", (goalPosition.GetP().X / 1000)) + ", " + String.Format("{0:0.000}", (goalPosition.GetP().Y / 1000)) + ", " + String.Format("{0:0.000}", (goalPosition.GetP().Z / 1000)) + "] ["+ String.Format("{0:0.000}", goalRotation.X)+" "+ String.Format("{0:0.000}", goalRotation.Y)+" "+ String.Format("{0:0.000}", goalRotation.Z) + "]", MessageLevel.Warning); VectorOfDouble goalJointAngles = description.getIK(goalPosition.GetP().X / 1000, goalPosition.GetP().Y / 1000, goalPosition.GetP().Z / 1000, goalRotation.X, goalRotation.Y, goalRotation.Z, startJointAngles, 0.5, "Distance"); motionPlan.setStartPosition(startJointAngles); motionPlan.setGoalPosition(goalJointAngles); String startOut = "[", goalOut = "["; for (int i = 0; i < startJointAngles.Count; i++) { startOut += String.Format("{0:0.000}", startJointAngles[i]) + " "; } for (int i = 0; i < goalJointAngles.Count; i++) { goalOut += String.Format("{0:0.000}", goalJointAngles[i]) + " "; } startOut += "]"; goalOut += "]"; motionPlan.setSolveTime(10.0); motionPlan.setStateValidityCheckingResolution(0.01); //motionPlan.setReportFirstExactSolution(true); motionPlan.setPlannerByString("RRTConnect"); if (motionPlan.plan() > 0) { //IoC.Get<IMessageService>().AppendMessage("Found motion from " + startFrame + "@" + startOut + " to " + goalFrame + "@" + goalOut + ": ", MessageLevel.Warning); VectorOfDoubleVector plan = motionPlan.getLastResult(); //motionPlan.showSetupInInspector(); foreach (VectorOfDouble jointConfiguration in plan) { String motionBuf = "[", sep = ""; foreach (double jointAngle in jointConfiguration) { motionBuf += sep + String.Format("{0:0.000}", jointAngle); sep = ","; } //IoC.Get<IMessageService>().AppendMessage(motionBuf + "]", MessageLevel.Warning); } //IoC.Get<IMessageService>().AppendMessage("Found motion END", MessageLevel.Warning); return(plan); } else { //motionPlan.showSetupInInspector(); IoC.Get <IMessageService>().AppendMessage("Failed to find motion from " + startOut + " to " + goalOut + ": " + motionPlan.getLastPlanningError(), MessageLevel.Warning); } return(null); }
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(); } }