public void SetTargetPosition(RobotVector currentPosition, RobotVector targetPosition, RobotVector targetVelocity, double targetDuration) { if (targetDuration <= 0.0) { throw new ArgumentException($"Duration value must be greater than 0, get {targetDuration}"); } bool targetPositionChanged = !targetPosition.Compare(this.targetPosition, 0.1, 1); bool targetVelocityChanged = !targetVelocity.Compare(this.targetVelocity, 0.1, 1); bool targetDurationChanged = targetDuration != this.targetDuration; if (targetDurationChanged || targetPositionChanged || targetVelocityChanged) { lock (syncLock) { targetPositionReached = false; this.targetPosition = targetPosition; this.targetVelocity = targetVelocity; this.targetDuration = targetDuration; elapsedTime = 0.0; } polyX.UpdateCoefficients(currentPosition.X, this.targetPosition.X, this.targetVelocity.X, this.targetDuration); polyY.UpdateCoefficients(currentPosition.Y, this.targetPosition.Y, this.targetVelocity.Y, this.targetDuration); polyZ.UpdateCoefficients(currentPosition.Z, this.targetPosition.Z, this.targetVelocity.Z, this.targetDuration); polyA.UpdateCoefficients(currentPosition.A, this.targetPosition.A, this.targetVelocity.A, this.targetDuration); polyB.UpdateCoefficients(currentPosition.B, this.targetPosition.B, this.targetVelocity.B, this.targetDuration); polyC.UpdateCoefficients(currentPosition.C, this.targetPosition.C, this.targetVelocity.C, this.targetDuration); } }
/// <summary> /// Sends data (IPOC, correction) to the robot, raises <see cref="KUKARobot.FrameSent">FrameSent</see> event /// </summary> private void SendData() { RobotVector correction; lock (forceMoveSyncLock) { correction = generator.GetNextCorrection(position); } correction = new RobotVector(correction.X, correction.Y, correction.Z, 0, 0, 0); // CAŁY IF DO ZAKOMENTOWANIA DLA POZYCJI ABSOLUTNEJ if (!Limits.CheckCorrection(correction)) { Uninitialize(); throw new InvalidOperationException("Correction limit has been exceeded:" + $"{Environment.NewLine}{correction}"); } OutputFrame outputFrame = new OutputFrame() { Correction = correction, IPOC = IPOC }; rsiAdapter.SendData(outputFrame); FrameSent?.Invoke(outputFrame); }
/// <summary> /// Moves robot to specified position (sets target position). /// </summary> /// <param name="targetPosition">target position</param> /// <param name="targetVelocity">target velocity (velocity after targetDuration)</param> /// <param name="targetDuration">desired movement duration in seconds</param> public void MoveTo(RobotVector targetPosition, RobotVector targetVelocity, double targetDuration) { if (!isInitialized) { throw new InvalidOperationException("Robot is not initialized"); } if (!Limits.CheckPosition(targetPosition)) { throw new ArgumentException("Target position is outside the available workspace:" + $"{Environment.NewLine}{targetPosition}"); } if (!Limits.CheckVelocity(targetVelocity)) { throw new ArgumentException("target velocity exceeding max value " + $"({Limits.MaxVelocity.XYZ} [mm/s], {Limits.MaxVelocity.ABC} [deg/s]):" + $"{Environment.NewLine}{targetVelocity}"); } lock (forceMoveSyncLock) { if (forceMoveMode) { return; } } generator.SetTargetPosition(targetPosition, targetVelocity, targetDuration); }
private async Task <long> ReceiveDataAsync() { InputFrame receivedFrame = await rsiAdapter.ReceiveDataAsync(); if (!Limits.CheckAxisPosition(receivedFrame.AxisPosition)) { Uninitialize(); throw new InvalidOperationException("Axis position limit has been exceeded:" + $"{Environment.NewLine}{receivedFrame.AxisPosition}"); } if (!Limits.CheckPosition(receivedFrame.Position)) { Uninitialize(); throw new InvalidOperationException("Available workspace limit has been exceeded:" + $"{Environment.NewLine}{receivedFrame.Position}"); } lock (receivedDataSyncLock) { position = receivedFrame.Position; axisPosition = receivedFrame.AxisPosition; } FrameReceived?.Invoke(this, new FrameReceivedEventArgs { ReceivedFrame = receivedFrame }); return(receivedFrame.IPOC); }
public RobotVector GetNextValue(RobotVector currentPosition) { lock (syncLock) { if (!targetPositionReached && timeLeft >= Ts && !currentPosition.Compare(targetPosition, 0.02, 0.004)) { double nx = polyX.GetNextValue(currentPosition.X, targetPosition.X, targetVelocity.X, timeLeft, Ts); double ny = polyY.GetNextValue(currentPosition.Y, targetPosition.Y, targetVelocity.Y, timeLeft, Ts); double nz = polyZ.GetNextValue(currentPosition.Z, targetPosition.Z, targetVelocity.Z, timeLeft, Ts); double na = polyA.GetNextValue(currentPosition.A, targetPosition.A, targetVelocity.A, timeLeft, Ts); double nb = polyB.GetNextValue(currentPosition.B, targetPosition.B, targetVelocity.B, timeLeft, Ts); double nc = polyC.GetNextValue(currentPosition.C, targetPosition.C, targetVelocity.C, timeLeft, Ts); timeLeft -= Ts; return(new RobotVector(nx, ny, nz, na, nb, nc) - homePosition); } else { if (!targetPositionReached) { reachedPosition = currentPosition - homePosition; } targetPositionReached = true; polyX.Reset(targetVelocity.X); polyY.Reset(targetVelocity.Y); polyZ.Reset(targetVelocity.Z); polyA.Reset(targetVelocity.A); polyB.Reset(targetVelocity.B); polyC.Reset(targetVelocity.C); return(reachedPosition); } } }
/// <summary> /// Moves robot to the specified position and blocks current thread until position is reached /// Enables force move mode during the movement. /// </summary> /// <param name="targetPosition">target position</param> /// <param name="targetVelocity">target velocity (velocity after targetDuration)</param> /// <param name="targetDuration">desired movement duration in seconds</param> public void ForceMoveTo(RobotVector targetPosition, RobotVector targetVelocity, double targetDuration) { MoveTo(targetPosition, targetVelocity, targetDuration); lock (forceMoveSyncLock) { forceMoveMode = true; } ManualResetEvent targetPositionReached = new ManualResetEvent(false); void checkPosition(InputFrame frameReceived) { if (generator.IsTargetPositionReached) { targetPositionReached.Set(); } }; FrameReceived += checkPosition; targetPositionReached.WaitOne(); FrameReceived -= checkPosition; lock (forceMoveSyncLock) { forceMoveMode = false; } }
public RobotVector GetNextCorrection(RobotVector currentPosition) { lock (syncLock) { if (time2Dest >= 0.004) { UpdateCoefficients(currentPosition, targetPosition, time2Dest); ComputeNextPoint(); time2Dest -= period; UpdateVelocity(); return(new RobotVector( X.GetNextValue(), Y.GetNextValue(), Z.GetNextValue(), A.GetNextValue(), B.GetNextValue(), C.GetNextValue() )); } else { targetPositionReached = true; totalTime2Dest = 0.0; ResetVelocity(); return(new RobotVector()); } } }
private void UpdateCoefficients(RobotVector currentPosition, RobotVector targetPosition, double time2Dest, double targetZLevel = 0.0) { // guessing targetVelocity == 0.0 X.UpdateCoefficients(currentPosition.X, targetPosition.X, 0.0, time2Dest); Y.UpdateCoefficients(currentPosition.Y, targetPosition.Y, 0.0, time2Dest); Z.UpdateCoefficients(currentPosition.Z, targetPosition.Z, targetZLevel, time2Dest); /* Vector<double> currentABC = currentPosition.ABC; * Vector<double> targetABC = targetPosition.ABC; * * // handling passing through +-180 * if (targetABC[0] - currentABC[0] > 180.0 || targetABC[0] - currentABC[0] < -180.0) { * currentABC[0] = (currentABC[0] + 360.0) % 360 - currentABC[0]; * targetABC[0] = (targetABC[0] + 360.0) % 360 - targetABC[0]; * } * * if (targetABC[1] - currentABC[1] > 180.0 || targetABC[1] - currentABC[1] < -180.0) { * currentABC[1] = (currentABC[1] + 360.0) % 360 - currentABC[1]; * targetABC[1] = (targetABC[1] + 360.0) % 360 - targetABC[1]; * } * * if (targetABC[2] - currentABC[2] > 180.0 || targetABC[2] - currentABC[2] < -180.0) { * currentABC[2] = (currentABC[2] + 360.0) % 360 - currentABC[2]; * targetABC[2] = (targetABC[2] + 360.0) % 360 - targetABC[2]; * } * * currentPosition += new E6POS(0.0, 0.0, 0.0, currentABC[0], currentABC[1], currentABC[2]); * targetPosition += new E6POS(0.0, 0.0, 0.0, targetABC[0], targetABC[1], targetABC[2]);*/ A.UpdateCoefficients(currentPosition.A, targetPosition.A, 0.0, time2Dest); B.UpdateCoefficients(currentPosition.B, targetPosition.B, 0.0, time2Dest); C.UpdateCoefficients(currentPosition.C, targetPosition.C, 0.0, time2Dest); }
/// <summary> /// Receives data (IPOC, cartesian and axis position) from the robot asynchronously, /// raises <see cref="KUKARobot.FrameRecived">FrameReceived</see> event /// </summary> private async Task ReceiveDataAsync() { InputFrame receivedFrame = await rsiAdapter.ReceiveDataAsync(); RobotVector correction = receivedFrame.Position - position; if (!Limits.CheckCorrection(correction)) { Uninitialize(); throw new InvalidOperationException("Correction limit has been exceeded:" + $"{Environment.NewLine}{correction}"); } if (!Limits.CheckAxisPosition(receivedFrame.AxisPosition)) { Uninitialize(); throw new InvalidOperationException("Axis position limit has been exceeded:" + $"{Environment.NewLine}{receivedFrame.AxisPosition}"); } if (!Limits.CheckPosition(receivedFrame.Position)) { Uninitialize(); throw new InvalidOperationException("Available workspace limit has been exceeded:" + $"{Environment.NewLine}{receivedFrame.Position}"); } lock (receivedDataSyncLock) { IPOC = receivedFrame.IPOC; position = receivedFrame.Position; axisPosition = receivedFrame.AxisPosition; } FrameReceived?.Invoke(receivedFrame); }
public RobotVector GetNextValue(RobotVector currentPosition) { lock (syncLock) { if (!targetPositionReached && elapsedTime < targetDuration) { elapsedTime += Ts; double nx = polyX.GetValueAt(elapsedTime); double ny = polyY.GetValueAt(elapsedTime); double nz = polyZ.GetValueAt(elapsedTime); double na = polyA.GetValueAt(elapsedTime); double nb = polyB.GetValueAt(elapsedTime); double nc = polyC.GetValueAt(elapsedTime); return(new RobotVector(nx, ny, nz, na, nb, nc) - homePosition); } else { if (!targetPositionReached) { reachedPosition = currentPosition - homePosition; } targetPositionReached = true; polyX.Reset(targetVelocity.X); polyY.Reset(targetVelocity.Y); polyZ.Reset(targetVelocity.Z); polyA.Reset(targetVelocity.A); polyB.Reset(targetVelocity.B); polyC.Reset(targetVelocity.C); return(reachedPosition); } } }
public Robot() { rsiAdapter = new RSIAdapter(); generator = new TrajectoryGenerator(); position = RobotVector.Zero; axisPosition = RobotAxisVector.Zero; HomePosition = RobotVector.Zero; }
public TrajectoryGenerator5(RobotVector homePosition) { targetPositionReached = true; targetPosition = homePosition; targetVelocity = RobotVector.Zero; targetDuration = 0.0; timeLeft = 0.0; }
public RobotEmulator(RobotVector homePosition) { HomePosition = homePosition; generator = new TrajectoryGenerator(); position = RobotVector.Zero; axisPosition = RobotAxisVector.Zero; correctionBuffor = new List <RobotVector>(); correction = RobotVector.Zero; }
public TrajectoryGenerator5v2(RobotVector homePosition) { this.homePosition = homePosition; targetPositionReached = true; targetPosition = homePosition; targetVelocity = RobotVector.Zero; targetDuration = 0.0; elapsedTime = 0.0; }
public bool Compare(RobotVector vectorToCompare, double xyzTolerance, double abcTolerance) { return (Math.Abs(X - vectorToCompare.X) <= xyzTolerance && Math.Abs(Y - vectorToCompare.Y) <= xyzTolerance && Math.Abs(Z - vectorToCompare.Z) <= xyzTolerance && Math.Abs(A - vectorToCompare.A) <= abcTolerance && Math.Abs(B - vectorToCompare.B) <= abcTolerance && Math.Abs(C - vectorToCompare.C) <= abcTolerance); }
// UWAGA: TOTALNIE NIE WIADOMO CZY DZIAŁA // targetZVelocity [mm / s] public RobotVector Hit(RobotVector currentPosition, double targetZVelocity) { lock (syncLock) { if (totalTime2Dest > 0 && time2Dest - 50 / targetZVelocity > 0.004) { // ruch do punktu pod pilka RobotVector underTargetPosition = targetPosition + new RobotVector(0, 0, -50, 0, 0, 0); UpdateCoefficients(currentPosition, underTargetPosition, time2Dest - 50 / targetZVelocity, targetZVelocity); ComputeNextPoint(); time2Dest -= period; UpdateVelocity(); return(new RobotVector( X.GetNextValue(), Y.GetNextValue(), Z.GetNextValue(), A.GetNextValue(), B.GetNextValue(), C.GetNextValue() )); } else if (time2Dest + 50 / targetZVelocity > 0.004) { // ruch v = const time2Dest -= period; return(new RobotVector(0, 0, targetZVelocity * period, 0, 0, 0)); } else if (time2Dest + 50 / targetZVelocity + 3 > 0.004) { // wytracenie predkosci w czasie np: 3s, do pozycji odbicia UpdateCoefficients(currentPosition, targetPosition, time2Dest + 50 / targetZVelocity + 3); ComputeNextPoint(); time2Dest -= period; UpdateVelocity(); return(new RobotVector( X.GetNextValue(), Y.GetNextValue(), Z.GetNextValue(), A.GetNextValue(), B.GetNextValue(), C.GetNextValue() )); } else { // zwroc 0, reset targetPositionReached = true; totalTime2Dest = 0.0; ResetVelocity(); return(new RobotVector()); } } }
public void Initialize(RobotVector actualRobotPosition) { elapsedTime = 0.0; IsTargetPositionReached = true; currentMovement = new RobotMovement(actualRobotPosition, RobotVector.Zero, -1.0); polyX.Initialize(actualRobotPosition.X); polyY.Initialize(actualRobotPosition.Y); polyZ.Initialize(actualRobotPosition.Z); polyA.Initialize(actualRobotPosition.A); polyB.Initialize(actualRobotPosition.B); polyC.Initialize(actualRobotPosition.C); }
public void SetTargetPosition(RobotVector targetPosition, double time) { if (time <= 0.0) { throw new ArgumentException($"Duration value must be greater than 0, get {time}"); } if (totalTime2Dest != time || !targetPosition.Compare(this.targetPosition, 0.1, 1)) { lock (syncLock) { this.targetPosition = targetPosition.Clone() as RobotVector; totalTime2Dest = time; time2Dest = time; targetPositionReached = false; } } }
private async Task Connect() { cts = new CancellationTokenSource(); IsCancellationRequested = false; InputFrame receivedFrame = null; isInitialized = true; // Connect with the robot try { Task.Run(async() => { receivedFrame = await rsiAdapter.Connect(Config.Port); }).Wait(cts.Token); } catch (OperationCanceledException) { rsiAdapter.Disconnect(); return; } lock (generatorSyncLock) { generator.Initialize(receivedFrame.Position); } lock (receivedDataSyncLock) { position = receivedFrame.Position; HomePosition = receivedFrame.Position; } // Send first response OutputFrame response = new OutputFrame() { Correction = RobotVector.Zero, IPOC = receivedFrame.IPOC }; rsiAdapter.SendData(response); Initialized?.Invoke(this, EventArgs.Empty); // Start loop for receiving and sending data while (!IsCancellationRequested) { long IPOC = await ReceiveDataAsync(); SendData(IPOC); } }
/// <param name="port">Port defined in RSI_EthernetConfig.xml</param> /// <param name="limits">robot limits</param> public KUKARobot(int port, RobotLimits limits) { rsiAdapter = new RSIAdapter(port); Limits = limits; worker = new BackgroundWorker() { WorkerSupportsCancellation = true }; worker.DoWork += async(sender, args) => { // Connect with the robot InputFrame receivedFrame = await rsiAdapter.Connect(); generator = new TrajectoryGenerator5(receivedFrame.Position); lock (receivedDataSyncLock) { IPOC = receivedFrame.IPOC; position = receivedFrame.Position; HomePosition = receivedFrame.Position; } // Send first response (prevent connection timeout) rsiAdapter.SendData(new OutputFrame() { Correction = new RobotVector(), IPOC = IPOC }); isInitialized = true; Initialized?.Invoke(); // Start loop for receiving and sending data while (!worker.CancellationPending) { await ReceiveDataAsync(); SendData(); } isInitialized = false; rsiAdapter.Disconnect(); }; }
private long ReceiveDataAsync() { correctionBuffor.Add(correction); RobotVector currentCorrection = RobotVector.Zero; if (correctionBuffor.Count == 8) { currentCorrection = correctionBuffor[0]; correctionBuffor.RemoveAt(0); } InputFrame receivedFrame = new InputFrame { Position = position + currentCorrection, AxisPosition = RobotAxisVector.Zero, IPOC = 0 }; if (!Limits.CheckAxisPosition(receivedFrame.AxisPosition)) { Uninitialize(); throw new InvalidOperationException("Axis position limit has been exceeded:" + $"{Environment.NewLine}{receivedFrame.AxisPosition}"); } if (!Limits.CheckPosition(receivedFrame.Position)) { Uninitialize(); throw new InvalidOperationException("Available workspace limit has been exceeded:" + $"{Environment.NewLine}{receivedFrame.Position}"); } lock (receivedDataSyncLock) { position = receivedFrame.Position; axisPosition = receivedFrame.AxisPosition; } FrameReceived?.Invoke(this, new FrameReceivedEventArgs { ReceivedFrame = receivedFrame }); return(receivedFrame.IPOC); }
private void Connect() { IsCancellationRequested = false; InputFrame receivedFrame = new InputFrame { Position = HomePosition, AxisPosition = RobotAxisVector.Zero, IPOC = 0 }; generator.Initialize(receivedFrame.Position); lock (receivedDataSyncLock) { position = receivedFrame.Position; HomePosition = receivedFrame.Position; } isInitialized = true; Initialized?.Invoke(this, EventArgs.Empty); // Start loop for receiving and sending data try { while (!IsCancellationRequested) { long IPOC = ReceiveDataAsync(); SendData(IPOC); Thread.Sleep(4); } } catch (Exception e) { var args = new ErrorOccuredEventArgs { RobotIp = ToString(), Exception = e }; ErrorOccured?.Invoke(this, args); } isInitialized = false; Uninitialized?.Invoke(this, EventArgs.Empty); }
public void SetTargetPosition(RobotVector targetPosition, RobotVector targetVelocity, double targetDuration) { if (targetDuration <= 0.0) { throw new ArgumentException($"Duration value must be greater than 0, get {targetDuration}"); } bool targetPositionChanged = !targetPosition.Compare(this.targetPosition, 0.1, 0.1); bool targetVelocityChanged = !targetVelocity.Compare(this.targetVelocity, 0.1, 0.1); bool targetDurationChanged = targetDuration != this.targetDuration; if (targetDurationChanged || targetPositionChanged || targetVelocityChanged) { lock (syncLock) { targetPositionReached = false; this.targetPosition = targetPosition; this.targetVelocity = targetVelocity; this.targetDuration = targetDuration; timeLeft = targetDuration; } } }
private void SendData(long IPOC) { correction = generator.GetNextCorrection(); if (!Limits.CheckCorrection(correction)) { Uninitialize(); throw new InvalidOperationException("Correction limit has been exceeded:" + $"{Environment.NewLine}{correction}"); } OutputFrame outputFrame = new OutputFrame() { Correction = correction, IPOC = IPOC }; FrameSent?.Invoke(this, new FrameSentEventArgs { FrameSent = outputFrame, Position = position }); }
private void UpdateCurrentMovement(RobotMovement movement) { movement = new RobotMovement( movement.TargetPosition, movement.TargetVelocity, movement.TargetDuration - 0.032 ); RobotVector targetPosition = movement.TargetPosition; RobotVector targetVelocity = movement.TargetVelocity; double targetDuration = movement.TargetDuration; if (movement.TargetDuration <= 0.0) { throw new ArgumentException($"Duration value must be greater than 0.032s"); } bool targetPositionChanged = !targetPosition.Compare(currentMovement.TargetPosition, 1, 0.1); bool targetVelocityChanged = !targetVelocity.Compare(currentMovement.TargetVelocity, 1, 0.1); bool targetDurationChanged = Math.Abs(targetDuration - currentMovement.TargetDuration) >= 0.001; if (targetDurationChanged || targetPositionChanged || targetVelocityChanged) { double tmpElapsedTime = elapsedTime; currentMovement = movement; elapsedTime = 0.0; IsTargetPositionReached = false; polyX.UpdateCoefficients(targetPosition.X, targetVelocity.X, targetDuration, tmpElapsedTime); polyY.UpdateCoefficients(targetPosition.Y, targetVelocity.Y, targetDuration, tmpElapsedTime); polyZ.UpdateCoefficients(targetPosition.Z, targetVelocity.Z, targetDuration, tmpElapsedTime); polyA.UpdateCoefficients(targetPosition.A, targetVelocity.A, targetDuration, tmpElapsedTime); polyB.UpdateCoefficients(targetPosition.B, targetVelocity.B, targetDuration, tmpElapsedTime); polyC.UpdateCoefficients(targetPosition.C, targetVelocity.C, targetDuration, tmpElapsedTime); } }
public TrajectoryGenerator3(RobotVector currentPosition) { targetPosition = currentPosition; }
public RobotEmulator(RobotConfig config, RobotVector homePosition) : this(homePosition) { Config = config; }
/// <summary> /// Shifts robot by the specified delta position and blocks current thread until new position is reached. /// Enables force move mode during the movement. /// </summary> /// <param name="deltaPosition">desired position change</param> /// <param name="targetVelocity">target velocity (velocity after targetDuration)</param> /// <param name="targetDuration">desired movement duration in seconds</param> public void ForceShift(RobotVector deltaPosition, RobotVector targetVelocity, double targetDuration) { ForceMoveTo(Position + deltaPosition, targetVelocity, targetDuration); }
public void ForceMoveTo(RobotVector targetPosition, RobotVector targetVelocity, double targetDuration) { ForceMoveTo(new RobotMovement(targetPosition, targetVelocity, targetDuration)); }