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