public void CalculateCurrentRobotSpeed(IRobot robot, ref Dictionary <IRobot, RobotParameters> robotList, double tickInterval, Vector3 humanWorldPosition)
{
RobotParameters param = robotList[robot];
if (robot.Component == null)
{
return;
}
Vector3 currentTcpWorldPosition = robot.Component.TransformationInWorld.GetP() + robot.RobotController.ToolCenterPoint.GetP(); //robot.RobotController.GetMotionTester().CurrentTarget.WorldTargetMatrix;
//Distance between last and current position
if (!param.lastTcpWorldPosition.Equals(Matrix.Zero))
{
double distance = (param.lastTcpWorldPosition - currentTcpWorldPosition).Length;
if (distance != 0.0)
{
//[mm/s]
double cartesianSpeed = distance / tickInterval;
double dx = currentTcpWorldPosition.X - param.lastTcpWorldPosition.X;
double dy = currentTcpWorldPosition.Y - param.lastTcpWorldPosition.Y;
double dz = currentTcpWorldPosition.Z - param.lastTcpWorldPosition.Z;
double d = Math.Sqrt((dx * dx) + (dy * dy) + (dz * dz));
double vx = dx / d * cartesianSpeed;
double vy = dy / d * cartesianSpeed;
double vz = dz / d * cartesianSpeed;
double vxy = Math.Sqrt((vx * vx) + (vy * vy));
double alpha = Math.Atan2((humanWorldPosition.Y - currentTcpWorldPosition.Y), (humanWorldPosition.X - currentTcpWorldPosition.X));
if (robot.Component.GetProperty("ArrowAngleZ") == null)
{
return;
}
//robot.Component.GetProperty("ArrowAngleZ").Value = alpha * (180 / Math.PI); //param.angleToHuman * (180 / Math.PI);
// Amount of speed of the robot that is directed towards the human
double vHuman = (vx * Math.Cos(alpha)) + (vy * Math.Sin(alpha));
//ms.AppendMessage("Vx: " + vx + ", Vy: " + vy + ", VHuman: " + vHuman + "Alpha: " + alpha, MessageLevel.Error);
if (Math.Abs(vHuman - param.currentCartesianSpeed) >= 100)
{
// Do nothing speed calculation seems to be wrong, we don't want spikes
}
else
{
param.currentCartesianSpeed = vHuman;
}
}
//ms.AppendMessage("Current Cartesian Speed measured: " + robotList[robot].currentCartesianSpeed, MessageLevel.Warning);
}
param.lastTcpWorldPosition = currentTcpWorldPosition;
}
public void setMaxAllowedCartesianSpeed(IRobot robot, int maxspeed)
{
RobotParameters param = robotList[robot];
param.allowedCartesianSpeed = maxspeed;
if (param.motionPlan != null)
{
param.motionPlan.getMotionInterpolator().setCartesianSpeedLimit(maxspeed);
}
}
//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();
}
}
