public bool Is_Valid_Target(CartesianPosition Position)
{
var Current_Position = robot.GetCurrentPosition();
CartesianPosition New_Position = new CartesianPosition();
if (Position.X + Current_Position.X >= MAX_X)
{
return false;
} else if (Position.X + Current_Position.X <= MIN_X) {
return false;
} else if (Position.Y + Current_Position.Y >= MAX_Y)
{
return false;
}
else if (Position.Y + Current_Position.Y <= MIN_Y)
{
return false;
}
else if (Position.Z + Current_Position.Z >= MAX_Z)
{
return false;
}
else if (Position.Z + Current_Position.Z <= MIN_Z)
{
return false;
}
else
{
return true;
}
}
//bool cartesianPositionUpdated = false;
//CartesianPosition lastKnownCartesianPositionOfArm = new CartesianPosition();
private void OnReceivedFreezeArm(NetworkMessage m)
{
// Called after the DeadTrigger is released
Debug.Log("Receive FREEZE pos callback!");
// After we receive the current arm cartesian position from the server,
// Send a final signal to the server to move the arm to its current position,
// resulting in a freeze at that position until further commands are sent.
// So the arm won't move again until DeadTrigger is pulled.
var msg = m.ReadMessage <FreezeArmPositionMessage>();
//cartesianPositionUpdated = true;
CartesianPosition frozenPosition = new CartesianPosition(
msg.x,
msg.y,
msg.z,
msg.thetaX,
msg.thetaY,
msg.thetaZ,
msg.fp1,
msg.fp2,
msg.fp3
);
FindObjectOfType <Tracker_Handler>().FreezeArmPositionCallback(frozenPosition);
}
public override Transform Transform()
{
double tol = mDatabase == null ? 1e-5 : mDatabase.Tolerance;
IfcDistanceExpression distanceExpression = Distance;
IfcCurve curve = PlacementMeasuredAlong;
Plane plane = Plane.Unset;
plane = curve.planeAt(distanceExpression, false, tol);
if (plane.IsValid)
{
Vector3d xAxis = Vector3d.CrossProduct(plane.ZAxis, Vector3d.ZAxis);
plane = new Plane(plane.Origin, xAxis, plane.ZAxis);
if (plane.IsValid)
{
IfcOrientationExpression orientationExpression = Orientation;
if (orientationExpression != null)
{
Vector3d x = orientationExpression.LateralAxisDirection.Vector3d, z = orientationExpression.VerticalAxisDirection.Vector3d;
Vector3d y = Vector3d.CrossProduct(z, x);
plane = new Plane(plane.Origin, x, y);
}
return(Rhino.Geometry.Transform.ChangeBasis(plane, Plane.WorldXY));
}
}
if (CartesianPosition != null)
{
return(CartesianPosition.Transform());
}
return(Rhino.Geometry.Transform.Unset);
}
public void FreezeArmPositionCallback(CartesianPosition frozenPosition)
{
armFrozen = true;
Debug.Log("Callback with frozen pos :" + frozenPosition.x.ToString("0:00") + "," + frozenPosition.y.ToString("0:00") + "," + frozenPosition.z.ToString("0:00"));
cartesianCommandToSend = frozenPosition;
SendCurrentCommand();
}
public bool Is_Valid_Target(CartesianPosition Position)
{
var Current_Position = robot.GetCurrentPosition();
CartesianPosition New_Position = new CartesianPosition();
if (Position.X + Current_Position.X >= MAX_X)
{
return(false);
}
else if (Position.X + Current_Position.X <= MIN_X)
{
return(false);
}
else if (Position.Y + Current_Position.Y >= MAX_Y)
{
return(false);
}
else if (Position.Y + Current_Position.Y <= MIN_Y)
{
return(false);
}
else if (Position.Z + Current_Position.Z >= MAX_Z)
{
return(false);
}
else if (Position.Z + Current_Position.Z <= MIN_Z)
{
return(false);
}
else
{
return(true);
}
}
private void init()
{
// Parcours du plateau
for (int i = 0; i < NBLIGNESPLATEAU; i++)
{
for (int j = 0; j < NBCOLONNESPLATEAU; j++)
{
// Création des coordonées X et Y
double posx = XB + j * PASX * Math.Cos(THETA) + i * PASY * Math.Sin(THETA);
double posy = YB - i * PASY * Math.Cos(THETA) + j * PASX * Math.Sin(THETA);
// Création d'un nouveau point en conservant le Z et les angles pour avoir une approche identique du plateau
point = new NLX.Robot.Kuka.Controller.CartesianPosition();
point.X = posx;
point.Y = posy;
point.Z = 144.49; //391.337982;
point.A = 54;
point.B = -0.54;
point.C = 91.58;
// On marque l'emplacement comme étant libre
bool busy = false;
Robot.RobotActions.Emplacement temp = new Robot.RobotActions.Emplacement();
temp.point = point;
temp.isBusy = busy;
// Ajout de l'emplacement
this.plateau.Add(temp);
Console.WriteLine("Emplacement(" + i + "," + j + ") => X: " + posx + " Y: " + posy);
}
}
this.plateau.Reverse();
}
public override void OnInspectorGUI()
{
CartesianPosition thing = (CartesianPosition)target;
DrawDefaultInspector();
thing.X = EditorGUILayout.IntField("X", thing.X);
thing.Y = EditorGUILayout.IntField("Y", thing.Y);
}
/// <summary>
/// Construit une instance de RobotPosition à partir de la position actuelle du robot
/// </summary>
/// <param name="robot"></param>
public RobotPosition(RobotController robot)
{
try
{
robot.StopRelativeMovement();
Position = robot.GetCurrentPosition();
robot.StartRelativeMovement();
}
catch (Exception ex)
{
Position = new CartesianPosition();
}
}
public RobotActions()
{
liste_temp = new List <CartesianPosition>();
/*init*/
vecteur = new TDx.TDxInput.Vector3D();
point_relatif = new CartesianPosition();
liste_aller_magasin = new List <CartesianPosition>();
liste_aller_plateau = new List <CartesianPosition>();
liste_placer_piece = new List <CartesianPosition>();
liste_retour_magasin = new List <CartesianPosition>();
plateau = new Plateau().GetPlateau();
}
void SetArmPose(CartesianPosition pose)
{
// Make sure to call this so that the FIRST command that is sent the robot is already a pose (not 0,0,0,0,0)
cartesianCommandToSend = new CartesianPosition(
pose.x,
pose.y,
pose.z,
pose.thetaX,
pose.thetaY,
pose.thetaZ,
pose.fp1,
pose.fp2,
pose.fp3
);
SendCurrentCommand();
}
CartesianPosition GetCurrentArmCartesianPosition(string cartPosAsStr, NetworkMessageInfo info)
{
string[] ps = cartPosAsStr.Split(',');
CartesianPosition cartPos = new CartesianPosition(
int.Parse(ps[0]),
int.Parse(ps[1]),
int.Parse(ps[2]),
int.Parse(ps[3]),
int.Parse(ps[4]),
int.Parse(ps[5]),
int.Parse(ps[6]),
int.Parse(ps[7]),
int.Parse(ps[8])
);
Debug.Log("cartpos:" + cartPos.x);
return(cartPos);
}
static void Main(string[] args)
{
RobotController Robot = new RobotController();
Robot.Connect("192.168.1.1");
Console.WriteLine("Robot connecté ... ");
Console.WriteLine("Robot position : x:" + Robot.GetCurrentPosition().X + "; y:" + Robot.GetCurrentPosition().Y + "; z: " + Robot.GetCurrentPosition().Z);
List <CartesianPosition> Trajectoire = new List <CartesianPosition>();
for (int i = 1; i <= 100; i++)
{
Trajectoire.Add(new CartesianPosition
{
X = i + Robot.GetCurrentPosition().X,
Y = i + Robot.GetCurrentPosition().Y,
Z = i + Robot.GetCurrentPosition().Z,
A = 0 + Robot.GetCurrentPosition().A,
B = 0 + Robot.GetCurrentPosition().B,
C = 0 + Robot.GetCurrentPosition().C
});
}
//Robot.PlayTrajectory(Trajectoire);
Robot.StartRelativeMovement();
CartesianPosition Test = new CartesianPosition
{
X = -1,
Y = 0,
Z = 0,
A = 0,
B = 0,
C = 0
};
Robot.SetRelativeMovement(Test);
System.Threading.Thread.Sleep(2000);
Robot.StopRelativeMovement();
Device Mouse = new Device();
}
static void Main(string[] args)
{
RobotController Robot = new RobotController();
Robot.Connect("192.168.1.1");
Console.WriteLine("Robot connecté ... ");
Console.WriteLine("Robot position : x:" + Robot.GetCurrentPosition().X + "; y:" + Robot.GetCurrentPosition().Y + "; z: " + Robot.GetCurrentPosition().Z);
List<CartesianPosition> Trajectoire = new List<CartesianPosition>();
for (int i = 1; i <= 100; i++)
{
Trajectoire.Add(new CartesianPosition
{
X = i + Robot.GetCurrentPosition().X,
Y = i + Robot.GetCurrentPosition().Y,
Z = i + Robot.GetCurrentPosition().Z,
A = 0 + Robot.GetCurrentPosition().A,
B = 0 + Robot.GetCurrentPosition().B,
C = 0 + Robot.GetCurrentPosition().C
});
}
//Robot.PlayTrajectory(Trajectoire);
Robot.StartRelativeMovement();
CartesianPosition Test = new CartesianPosition
{
X = -1,
Y = 0,
Z = 0,
A = 0,
B = 0,
C = 0
};
Robot.SetRelativeMovement(Test);
System.Threading.Thread.Sleep(2000);
Robot.StopRelativeMovement();
Device Mouse = new Device();
}
private void OnReceivedCartesianPosition(NetworkMessage m)
{
// Called after the DeadTrigger is released
Debug.Log("Receive cart pos callback!");
// After we receive the current arm cartesian position from the server,
// Send a final signal to the server to move the arm to its current position,
// resulting in a freeze at that position until further commands are sent.
// So the arm won't move again until DeadTrigger is pulled.
var msg = m.ReadMessage <RequestCartesianPositionMessage>();
//cartesianPositionUpdated = true;
CartesianPosition currentPosition = new CartesianPosition(
msg.x,
msg.y,
msg.z,
msg.thetaX,
msg.thetaY,
msg.thetaZ,
msg.fp1,
msg.fp2,
msg.fp3
);
FindObjectOfType <HUD>().currentPosition.text = currentPosition.x.ToString("0.00") + "," +
currentPosition.y.ToString("0.00") + ", " +
currentPosition.z.ToString("0.00") + ", " +
currentPosition.thetaX.ToString("0.00") + ", " +
currentPosition.thetaY.ToString("0.00") + ", " +
currentPosition.thetaZ.ToString("0.00") + ", " +
currentPosition.fp1.ToString("0") + ", " +
currentPosition.fp2.ToString("0") + ", " +
currentPosition.fp3.ToString("0");
}
/// <summary>
///
/// </summary>
public void Loop()
{
#region Variables
Vector3D VectorNorm = new Vector3D();
Vector3D Translation;
AngleAxis Rotation;
var Norm = Math.Sqrt(Math.Pow(MaxTransX, 2) + Math.Pow(MaxTransY, 2) + Math.Pow(MaxTransZ, 2));
#endregion
#region Loop which get the information from the mouse and convert to the movement of the robot
while (!_shouldStop)
{
Translation = GetTranslationVector();
Rotation = GetRotationAxis();
#region Normalization of the vector of the mouse
VectorNorm.X = Translation.X / Norm;
VectorNorm.Y = Translation.Y / Norm;
VectorNorm.Z = Translation.Z / Norm;
#endregion
#region Error due to a vector's component upper than 1.0
if (VectorNorm.X > 1.0 || VectorNorm.Y > 1.0 || VectorNorm.Z > 1.0)
{
Logs.AddLog("Error", "In mouse loop: vector > 1");
_shouldStop = true;
}
#endregion
#region Movement vector send to the robot
// Translation alone
if (Math.Abs(VectorNorm.X) > Treshold || Math.Abs(VectorNorm.Y) > Treshold || Math.Abs(VectorNorm.Z) > Treshold)
{
MoveByVector.X = VectorNorm.X * VitesseTranslation;
MoveByVector.Y = VectorNorm.Y * VitesseTranslation;
MoveByVector.Z = VectorNorm.Z * VitesseTranslation;
RotateByVector.X = 0.0;
RotateByVector.Y = 0.0;
RotateByVector.Z = 0.0;
}
// Rotation alone
else if (Math.Abs(Rotation.X) > Treshold || Math.Abs(Rotation.Y) > Treshold || Math.Abs(Rotation.Z) > Treshold)
{
MoveByVector.X = 0.0;
MoveByVector.Y = 0.0;
MoveByVector.Z = 0.0;
#region Rotation X first
if (Math.Abs(Rotation.X) > Math.Abs(Rotation.Y) && Math.Abs(Rotation.X) > Math.Abs(Rotation.Z))
{
if (Rotation.X > 0)
{
RotateByVector.X = Rotation.Angle * VitesseRotation;
}
else
{
RotateByVector.X = -Rotation.Angle * VitesseRotation;
}
RotateByVector.Y = 0.0;
RotateByVector.Z = 0.0;
}
#endregion
#region Rotation Y first
if (Math.Abs(Rotation.Y) > Math.Abs(Rotation.X) && Math.Abs(Rotation.Y) > Math.Abs(Rotation.Z))
{
RotateByVector.X = 0.0;
if (Rotation.Y > 0)
{
RotateByVector.Y = Rotation.Angle * VitesseRotation;
}
else
{
RotateByVector.Y = -Rotation.Angle * VitesseRotation;
}
RotateByVector.Z = 0.0;
}
#endregion
#region Rotation Z first
if (Math.Abs(Rotation.Z) > Math.Abs(Rotation.Y) && Math.Abs(Rotation.Z) > Math.Abs(Rotation.X))
{
RotateByVector.X = 0.0;
RotateByVector.Y = 0.0;
if (Rotation.Z > 0)
{
RotateByVector.Z = Rotation.Angle * VitesseRotation;
}
else
{
RotateByVector.Z = -Rotation.Angle * VitesseRotation;
}
}
#endregion
}
else
{
MoveByVector.X = 0.0;
MoveByVector.Y = 0.0;
MoveByVector.Z = 0.0;
RotateByVector.X = 0.0;
RotateByVector.Y = 0.0;
RotateByVector.Z = 0.0;
}
#endregion
//Logs.AddLog("info", string.Format("Mouse rotation: X: {0} | Y: {1} | Z: {2}", RotateByVector.X, RotateByVector.Y, RotateByVector.Z));
CartPosition = new CartesianPosition()
{
X = -MoveByVector.Z,
Y = -MoveByVector.X,
Z = MoveByVector.Y,
A = -RotateByVector.Z,
B = -RotateByVector.X,
C = RotateByVector.Y
};
}
#endregion
}
static void Main(string[] args)
{
Console.WriteLine("Start program...");
Mouse MyMouse = new Mouse();
RobotController MyRobot = new RobotController();
// Calibre la souris, valider la calibration avec la touche 4
MyMouse.Calibrate();
// Connexion au robot
MyRobot.Connect("192.168.1.1");
// Ouvre ou ferme la pince
//if (MyRobot.IsGripperOpen()) MyRobot.CloseGripper();
//else MyRobot.OpenGripper();
// Envoi la commande au robot
MyRobot.StartRelativeMovement();
// Créer un objet thread de l'objet Mouse, fonction Loop
Thread MouseThread = new Thread(MyMouse.Loop);
// Demarre le thread.
MouseThread.Start();
Console.WriteLine("main thread: Starting mouse thread...");
// Attend que le thread soit lancé et activé
while (!MouseThread.IsAlive)
{
;
}
Console.WriteLine("main thread: Mouse alive");
// Boucle tant qu'on utilise la souris
bool endMouse = true;
while (endMouse)
{
// Met le thread principale (ici) en attente d'une millisecond pour autoriser le thread secondaire à faire quelque chose
Thread.Sleep(1);
// Convertion des donnees de la souris pour le robot
CartesianPosition CartPositionMouse = new CartesianPosition();
CartPositionMouse.X = -MyMouse.MoveByVector.Z;
CartPositionMouse.Y = -MyMouse.MoveByVector.X;
CartPositionMouse.Z = MyMouse.MoveByVector.Y;
CartPositionMouse.A = -MyMouse.RotateByVector.Z;
CartPositionMouse.B = -MyMouse.RotateByVector.X;
CartPositionMouse.C = MyMouse.RotateByVector.Y;
Console.WriteLine("cmd robot: X : {0} | Y : {1} | Z : {2} | A : {3} | B : {4} | C : {5}", CartPositionMouse.X, CartPositionMouse.Y, CartPositionMouse.Z, CartPositionMouse.A, CartPositionMouse.B, CartPositionMouse.C);
// Envoi les commandes de deplacement au robot
MyRobot.SetRelativeMovement(CartPositionMouse);
}
// Demande l'arret du thread de la souris
MyMouse.RequestStop();
// Bloque le thread principale tant que le thread Mouse n'est pas terminé
MouseThread.Join();
// Arret le mouvement
MyRobot.StopRelativeMovement();
Console.WriteLine("main thread: mouse thread has terminated.");
Console.WriteLine("End program... Press a key to quit...");
Console.ReadKey();
}
