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