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