public RobotController([Import(typeof(IApplication))] IApplication app) { this.app = app; instance = this; MotionPlanningManagerInstance = new MotionPlanningManager(); MotionInterpolationInstance = new MotionInterpolation(); }
//double lastTime = 0.0; public void RegularTick(object sender, EventArgs e) { //double deltaTime = app.Simulation.Elapsed - lastTime; //lastTime = app.Simulation.Elapsed; foreach (ILaserScanner laserScanner in laser_scanners) { laserScanner.Scan(); } double appElapsed = app.Simulation.Elapsed; try { robotListLock.EnterReadLock(); foreach (IRobot robot in robotList.Keys) { if (!robot.IsValid) { continue; } UpdateVisualizationDistance(robot); double deltaTime = appElapsed - robotList[robot].LastTimeElapsed; if (human != null) { MotionInterpolationInstance.CalculateCurrentRobotSpeed(robot, ref robotList, TICK_INTERVAL, human.TransformationInWorld.GetP()); //robotList[robot].closestHumanWorldPosition } RobotParameters param = robotList[robot]; if (param.motionPlan == null) { continue; } MotionInterpolator mp = param.motionPlan.getMotionInterpolator(); if (param.motionPlan != null && !param.motionPlan.getMotionInterpolator().motionDone()) { VectorOfDouble result = param.motionPlan.getMotionInterpolator().interpolate_tick(deltaTime); robot.RobotController.InvalidateKinChains(); if (robot.RobotController.Joints.Count == 7) { robot.RobotController.SetJointValues(MotionInterpolation.KukaSorted(result)); } else { double[] firstJointAngleCollectionSorted = new double[result.Count]; firstJointAngleCollectionSorted[0] = result.ElementAt(0); //A1 firstJointAngleCollectionSorted[1] = result.ElementAt(1); //A2 firstJointAngleCollectionSorted[2] = result.ElementAt(2); //A3 firstJointAngleCollectionSorted[3] = result.ElementAt(3); //A4 firstJointAngleCollectionSorted[4] = result.ElementAt(4); //A5 firstJointAngleCollectionSorted[5] = result.ElementAt(5); //A6 robot.RobotController.SetJointValues(firstJointAngleCollectionSorted); } } else { //ms.AppendMessage("!!MovementFinished!!", MessageLevel.Warning); // After this command, the video recording breaks... // set movement done! IBehavior movementFinished = (IBehavior)robot.Component.FindBehavior("MovementFinished"); if (movementFinished is IStringSignal) { IStringSignal movementFinishedStringSignal = (IStringSignal)movementFinished; if (!String.Equals(movementFinishedStringSignal.Value, robotList[robot].payloadOnFinishMovement)) { movementFinishedStringSignal.Value = robotList[robot].payloadOnFinishMovement; } } else { ms.AppendMessage("\"MovementFinished\" behavior was null or not of type IStringSignal. Abort!", MessageLevel.Warning); } } robotList[robot].LastTimeElapsed = appElapsed; } } finally { robotListLock.ExitReadLock(); } }