public void AddittionOperatorVectorPose_RotationShouldAdd(
double x, double y, double z, // initial rotation
double dx, double dy, double dz, // rotation vector
double expectedX, double expectedY, double expectedZ) // expected rotation
{
Vector3D poseVector = new Vector3D(0, 0, 0);
Vector3D rotationVector = new Vector3D(dx, dy, dz);
URVector vector = new URVector(poseVector, rotationVector);
Point3D position = new Point3D(0, 0, 0);
Point3D rotation = new Point3D(x, y, z);
URPose pose = new URPose(position, rotation);
var finalPose = vector + pose;
var actualX = finalPose.Rotation.X;
var actualY = finalPose.Rotation.Y;
var actualZ = finalPose.Rotation.Z;
Assert.Equal(expectedX, actualX);
Assert.Equal(expectedY, actualY);
Assert.Equal(expectedZ, actualZ);
}
public void SubtractionOperatorPoseVector_PoseShouldSubtract(
double x, double y, double z, // initial position
double dx, double dy, double dz, // vector
double expectedX, double expectedY, double expectedZ) // expected position
{
Point3D position = new Point3D(x, y, z);
Point3D rotation = new Point3D(0, 0, 0);
URPose pose = new URPose(position, rotation);
Vector3D poseVector = new Vector3D(dx, dy, dz);
Vector3D rotationVector = new Vector3D(0, 0, 0);
URVector vector = new URVector(poseVector, rotationVector);
var finalPose = pose - vector;
var actualX = finalPose.Position.X;
var actualY = finalPose.Position.Y;
var actualZ = finalPose.Position.Z;
Assert.Equal(expectedX, actualX);
Assert.Equal(expectedY, actualY);
Assert.Equal(expectedZ, actualZ);
}
public bool IsCloseEnough(URPose target, URPose actual)
{
if (actual == null) //You're not connected to UR10
{
return(false);
}
decimal xDistance = Math.Abs((decimal)target.Xpose - (decimal)actual.Xpose);
decimal yDistance = Math.Abs((decimal)target.Ypose - (decimal)actual.Ypose);
decimal zDistance = Math.Abs((decimal)target.Zpose - (decimal)actual.Zpose);
bool PositionClose =
(xDistance <= (decimal)MinimumProximityPosition) &&
(yDistance <= (decimal)MinimumProximityPosition) &&
(zDistance <= (decimal)MinimumProximityPosition);
decimal xRotDis = Math.Abs((decimal)target.RXpose - (decimal)actual.RXpose);
decimal yRotDis = Math.Abs((decimal)target.RYpose - (decimal)actual.RYpose);
decimal zRotDis = Math.Abs((decimal)target.RZpose - (decimal)actual.RZpose);
bool RotationClose =
(xRotDis <= (decimal)MinimumProximityPosition) &&
(yRotDis <= (decimal)MinimumProximityPosition) &&
(zRotDis <= (decimal)MinimumProximityPosition);
return(PositionClose || RotationClose);
}
public void SubtractMethodPosePose_RotationShouldSubtract(
double x1, double y1, double z1, // first rotation
double x2, double y2, double z2, // second rotation
double expectedX, double expectedY, double expectedZ) // expected vector
{
Point3D position1 = new Point3D(0, 0, 0);
Point3D rotation1 = new Point3D(x1, y1, z1);
URPose pose1 = new URPose(position1, rotation1);
Point3D position2 = new Point3D(0, 0, 0);
Point3D rotation2 = new Point3D(x2, y2, z2);
URPose pose2 = new URPose(position2, rotation2);
var finalVector = URPose.Subtract(pose1, pose2);
var actualX = finalVector.RotationVector.X;
var actualY = finalVector.RotationVector.Y;
var actualZ = finalVector.RotationVector.Z;
Assert.Equal(expectedX, actualX);
Assert.Equal(expectedY, actualY);
Assert.Equal(expectedZ, actualZ);
}
public void SubtractionOperatorPosePose_PoseShouldSubtract(
double x1, double y1, double z1, // first position
double x2, double y2, double z2, // second position
double expectedX, double expectedY, double expectedZ) // expected vector
{
Point3D position1 = new Point3D(x1, y1, z1);
Point3D rotation1 = new Point3D(0, 0, 0);
URPose pose1 = new URPose(position1, rotation1);
Point3D position2 = new Point3D(x2, y2, z2);
Point3D rotation2 = new Point3D(0, 0, 0);
URPose pose2 = new URPose(position2, rotation2);
var finalVector = pose1 - pose2;
var actualX = finalVector.PoseVector.X;
var actualY = finalVector.PoseVector.Y;
var actualZ = finalVector.PoseVector.Z;
Assert.Equal(expectedX, actualX);
Assert.Equal(expectedY, actualY);
Assert.Equal(expectedZ, actualZ);
}
public void SubtractMethodVectorPose_RotationShouldSubtract(
double x, double y, double z, // initial rotation
double dx, double dy, double dz, // ratation vector
double expectedX, double expectedY, double expectedZ) // expected position
{
Vector3D poseVector = new Vector3D(0, 0, 0);
Vector3D rotationVector = new Vector3D(dx, dy, dz);
URVector vector = new URVector(poseVector, rotationVector);
Point3D position = new Point3D(0, 0, 0);
Point3D rotation = new Point3D(x, y, z);
URPose pose = new URPose(position, rotation);
var finalPose = URVector.Subtract(vector, pose);
var actualX = finalPose.Rotation.X;
var actualY = finalPose.Rotation.Y;
var actualZ = finalPose.Rotation.Z;
Assert.Equal(expectedX, actualX);
Assert.Equal(expectedY, actualY);
Assert.Equal(expectedZ, actualZ);
}
/// <summary>Sends pose to the modbus general purpose register.</summary>
/// <param name="pose">New position for the universal robot.</param>
public void SendPose(URPose pose)
{
byte[] data = new byte[12];
ushort ID = 2;
byte unit = 0;
ushort StartAddress = 128; // StartAddress in the general purpose register
// Generates a byte array with data in the specific order based on the documentation, to be send to the modbus register
// Every position value is multiplied by 1000 to secure data precision and then converted into 16-bit signed integer
// Byte is reversed to NetwotkOrder and then converted to a word
Array.Copy(BitConverter.GetBytes((short)IPAddress.HostToNetworkOrder((short)Convert.ToInt16(pose.Xpose * 1000))), 0, data, 0, 2);
Array.Copy(BitConverter.GetBytes((short)IPAddress.HostToNetworkOrder((short)Convert.ToInt16(pose.Ypose * 1000))), 0, data, 2, 2);
Array.Copy(BitConverter.GetBytes((short)IPAddress.HostToNetworkOrder((short)Convert.ToInt16(pose.Zpose * 1000))), 0, data, 4, 2);
Array.Copy(BitConverter.GetBytes((short)IPAddress.HostToNetworkOrder((short)Convert.ToInt16(pose.RXpose * 1000))), 0, data, 6, 2);
Array.Copy(BitConverter.GetBytes((short)IPAddress.HostToNetworkOrder((short)Convert.ToInt16(pose.RYpose * 1000))), 0, data, 8, 2);
Array.Copy(BitConverter.GetBytes((short)IPAddress.HostToNetworkOrder((short)Convert.ToInt16(pose.RZpose * 1000))), 0, data, 10, 2);
byte[] result = new byte[100];
if (MBmaster != null)
{
MBmaster.WriteMultipleRegister(ID, unit, StartAddress, data, ref result);
}
}
public void CtorSetWithScanMovement_SetCornerPositionsCorretly()
{
var scanMove = new URMovement(lt, rt);
var point = new Point3D(-0.5, -1, 0);
var pose = new URPose(point, rotation);
var boundary = new RectangularBoundary(scanMove, pose);
Assert.True(Vector3D.Equals(boundary.LeftTop.Position, lt.Position));
Assert.True(Vector3D.Equals(boundary.LeftTop.Rotation, lt.Rotation));
Assert.True(Vector3D.Equals(boundary.RightTop.Position, rt.Position));
Assert.True(Vector3D.Equals(boundary.RightTop.Rotation, rt.Rotation));
Assert.Equal(rb.Position.X, boundary.RightBottom.Position.X, precision);
Assert.Equal(rb.Position.Y, boundary.RightBottom.Position.Y, precision);
Assert.Equal(rb.Position.Z, boundary.RightBottom.Position.Z, precision);
Assert.True(Vector3D.Equals(boundary.RightBottom.Rotation, rb.Rotation));
Assert.Equal(lb.Position.X, boundary.LeftBottom.Position.X, precision);
Assert.Equal(lb.Position.Y, boundary.LeftBottom.Position.Y, precision);
Assert.Equal(lb.Position.Z, boundary.LeftBottom.Position.Z, precision);
Assert.True(Vector3D.Equals(boundary.LeftBottom.Rotation, lb.Rotation));
}
public static float UR_PROBE_OFFSET = 0.10f; //The length of whatever is attached to the tool center point in meters.
/// <summary>
/// Finds robot arm tool center point poses for a list of vertices
/// </summary>
/// <param name="vectorPath">A list of vertices in a mesh. Vertices must exist in mesh and be part of at least one face each</param>
/// <param name="mesh">The mesh that the vectorPath belongs to</param>
/// <returns>A list of URPoses used to feed the robot arm later</returns>
public List <URPose> ToURPath(List <VertexIndex> vectorPath, CVMesh mesh)
{
List <URPose> poses = new List <URPose>();
foreach (var v in vectorPath)
{
Vector3 vertexNormal = FindVertexNormal(v, mesh);
//vertexNormal.X = 0.00001f;
vertexNormal = Extensions.Normalize(vertexNormal);
Vector3 urPos = FindURPosition(v.Vector, vertexNormal);
//Vector3 smoothedNormal = Extensions.Normalize(Extensions.AvgVector(new List<Vector3> {vertexNormal, new Vector3 {X=0, Y=0, Z=1}}));
Vector3 rotationNormal = Extensions.Multiply(vertexNormal, -1);
//Vector3 urRot = ToRotationVector(rotationNormal);
URPose pose = new URPose(urPos.X, urPos.Y, urPos.Z, rotationNormal.X, rotationNormal.Y, rotationNormal.Z);
poses.Add(pose);
}
int percent = (int)Math.Floor(poses.Count * 0.12);
var snippet = poses.GetRange(percent, poses.Count - percent * 2);
PLYHandler.Output(snippet, Environment.CurrentDirectory);
ConvertDirectionVectors(snippet);
return(snippet);
}
public URMovement(URPose start, URPose end)
{
Start = start;
End = end;
Movement = End - Start;
}
/// <summary>Sets the instance of the acutal position of the universal robot.</summary>
/// <param name="pose">New position from the universal robot.</param>
private void SetURPose(URPose pose)
{
actualURPose = pose;
}
public void SetStandardPose()
{
URPose pose = new URPose(standard_pose_x, standard_pose_y, standard_pose_z, standard_pose_rx, standard_pose_ry, standard_pose_rz);
SendEntirePose(pose);
}
public void GetCurrentURPose(URPose pose)
{
currentPose = pose;
}
private void setURPose(URPose a)
{
actualURPose = a;
}
private void SendEntirePose(URPose pose)
{
data.SendNewPose(pose);
}
private bool IsValid(URPose p)
{
return(!double.IsNaN(p.Xpose) && !double.IsNaN(p.Ypose) && !double.IsNaN(p.Zpose) && !double.IsNaN(p.RXpose) &&
!double.IsNaN(p.RYpose) && !double.IsNaN(p.RZpose));
}
private void setURPose(URPose a)
{
currentPose = a;
}
/// <summary>Analyses the cartesian info when robot message type is CartesianInfo.
/// Subpackage containing different informations about the CartesianInfo, such as TCP position and offset.
/// <para>Value for CartesianInfo is 4.</para>
/// <para>Package length is 101 bytes.</para>
/// </summary>
/// <param name="data">Byte array received on the socket stream.</param>
private void CartesianInfo(byte[] data)
{
//Console.WriteLine("Package type: {0} - CARTESIAN INFO", data[count + 1]);
count++;
//Console.WriteLine("X: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double xPoseDouble = GetDoubleFromByteArray(data, count);
//Console.WriteLine("X: " + xPoseDouble);
count = count + 8;
//Console.WriteLine("Y: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double yPoseDouble = GetDoubleFromByteArray(data, count);
//Console.WriteLine("Y: " + yPoseDouble);
count = count + 8;
//Console.WriteLine("Z: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double zPoseDouble = GetDoubleFromByteArray(data, count);
//Console.WriteLine("Z: " + zPoseDouble);
count = count + 8;
//Console.WriteLine("Rx: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double RxPoseDouble = GetDoubleFromByteArray(data, count);
//Console.WriteLine("Rx: " + RxPoseDouble);
count = count + 8;
//Console.WriteLine("Ry: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double RyPoseDouble = GetDoubleFromByteArray(data, count);
//Console.WriteLine("Ry: " + RyPoseDouble);
count = count + 8;
//Console.WriteLine("Rz: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
double RzPoseDouble = GetDoubleFromByteArray(data, count);
//Console.WriteLine("Rz: " + RzPoseDouble);
count = count + 8;
//Console.WriteLine("TCPOffsetX: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Console.WriteLine("TCPOffsetY: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Console.WriteLine("TCPOffsetZ: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Console.WriteLine("TCPOffsetRx: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Console.WriteLine("TCPOffsetRy: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
//Console.WriteLine("TCPOffsetRz: [{0}] [{1}] [{2}] [{3}] [{4}] [{5}] [{6}] [{7}]", data[count + 1], data[count + 2], data[count + 3], data[count + 4], data[count + 5], data[count + 6], data[count + 7], data[count + 8]);
count = count + 8;
URPose pose = new URPose(xPoseDouble, yPoseDouble, zPoseDouble, RxPoseDouble, RyPoseDouble, RzPoseDouble);
//events.UpdateActualURPose(pose);
if (OnActualURPose != null)
{
OnActualURPose(pose);
}
}
public void testURPose(double x, double y, double z, double rx, double ry, double rz)
{
URPose p = new URPose(x, y, z, rx, ry, rz);
m.SendPose(p);
}
public void SendURPose(URPose pose)
{
throw new NotImplementedException();
}
