/// <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 void SendData(long IPOC)
{
RobotVector correction;
RobotVector currentVelocity;
RobotVector currentAcceleration;
RobotVector targetPosition;
RobotVector targetVelocity;
double targetDuration;
lock (generatorSyncLock) {
// GetNextCorrection() updates theoretical values
correction = generator.GetNextCorrection();
currentVelocity = generator.Velocity;
currentAcceleration = generator.Acceleration;
targetPosition = generator.TargetPosition;
targetVelocity = generator.TargetVelocity;
targetDuration = generator.TargetDuration;
}
if (!Limits.CheckCorrection(correction))
{
throw new InvalidOperationException("Correction limit has been exceeded:" +
$"{Environment.NewLine}{correction}");
}
if (!Limits.CheckVelocity(currentVelocity))
{
throw new InvalidOperationException("Velocity limit has been exceeded:" +
$"{Environment.NewLine}{currentVelocity}");
}
if (!Limits.CheckAcceleration(currentAcceleration))
{
throw new InvalidOperationException("Acceleration limit has been exceeded:" +
$"{Environment.NewLine}{currentAcceleration}");
}
OutputFrame outputFrame = new OutputFrame()
{
Correction = correction,
IPOC = IPOC
};
rsiAdapter.SendData(outputFrame);
FrameSent?.Invoke(this, new FrameSentEventArgs {
FrameSent = outputFrame,
Position = position,
TargetPosition = targetPosition,
TargetVelocity = targetVelocity,
TargetDuration = targetDuration
});
}
public void MoveTo(RobotMovement[] movementsStack)
{
lock (forceMoveSyncLock) {
if (isForceMoveModeEnabled)
{
return;
}
}
if (!isInitialized)
{
throw new InvalidOperationException("Robot is not initialized");
}
for (int i = 0; i < movementsStack.Length; i++)
{
RobotMovement movement = movementsStack[i];
if (!Limits.CheckPosition(movement.TargetPosition))
{
throw new ArgumentException("Target position is outside the available workspace:" +
$"{Environment.NewLine}{movement.TargetPosition}");
}
if (!Limits.CheckVelocity(movement.TargetVelocity))
{
throw new ArgumentException("Target velocity exceeding max value:" +
$"{Environment.NewLine}{movement.TargetVelocity}");
}
}
RobotVector currentVelocity;
RobotVector currentAcceleration;
lock (generatorSyncLock) {
generator.SetMovements(movementsStack);
currentVelocity = generator.Velocity;
currentAcceleration = generator.Acceleration;
}
MovementChanged?.Invoke(this, new MovementChangedEventArgs {
Position = Position,
Velocity = currentVelocity,
Acceleration = currentAcceleration,
MovementsStack = movementsStack
});
}
