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