public JointsResult(int moveId, double[] joints, double[] speeds, double[] accelerations, ErrorPathType error, double linearStep, double jointStep, double timeStep)
{
MoveId = moveId;
Joints = CopyArray(joints);
Speeds = CopyArray(speeds);
Accelerations = CopyArray(accelerations);
Error = error;
LinearStep = linearStep;
JointStep = jointStep;
TimeStep = timeStep;
}
/// <inheritdoc />
public InstructionListJointsResult GetInstructionListJoints(
double mmStep = 10.0,
double degStep = 5.0,
string saveToFile = "",
CollisionCheckOptions collisionCheck = CollisionCheckOptions.CollisionCheckOff,
ListJointsType flags = 0,
int timeoutSec = 3600,
double time_step = 0.2)
{
InstructionListJointsResult result =
new InstructionListJointsResult {
JointList = new List <InstructionListJointsResult.JointsResult>()
};
string errorMessage;
Mat jointList;
result.ErrorCode = InstructionListJoints(out errorMessage, out jointList, mmStep, degStep, saveToFile,
collisionCheck,
flags, timeoutSec, time_step);
result.ErrorMessage = errorMessage;
var numberOfJoints = GetLink(ItemType.Robot).Joints().Length;
for (var colId = 0; colId < jointList.Cols; colId++)
{
var joints = new double[numberOfJoints];
for (var rowId = 0; rowId < numberOfJoints; rowId++)
{
joints[rowId] = jointList[rowId, colId];
}
int jointError = (int)jointList[numberOfJoints, colId];
ErrorPathType errorType = (ErrorPathType)Convert.ToUInt32(jointError.ToString(), 2);
var maxLinearStep = jointList[numberOfJoints + 1, colId];
var maxJointStep = jointList[numberOfJoints + 2, colId];
var moveId = (int)jointList[numberOfJoints + 3, colId];
result.JointList.Add(
new InstructionListJointsResult.JointsResult
{
Joints = joints,
Error = errorType,
MaxLinearStep = maxLinearStep,
MaxJointStep = maxJointStep,
MoveId = moveId
}
);
}
return(result);
}
/// <summary>
/// Convert the error number returned by RoboDK to error flags
/// </summary>
/// <param name="evalue"></param>
/// <returns></returns>
public static ErrorPathType ConvertErrorCodeToJointErrorType(int evalue)
{
ErrorPathType flags = 0;
if (evalue % 10000000 > 999999)
{
// "The robot can't make a rotation so close to 180 deg. (the rotation axis is not properly defined
flags |= ErrorPathType.PathFlipAxis;
}
if (evalue % 1000000 > 99999)
{
// Collision detected.
flags |= ErrorPathType.Collision;
}
if (evalue % 1000 > 99)
{
// Joint 5 crosses 0 degrees. This is a singularity and it is not allowed for a linear move.
flags |= ErrorPathType.WristSingularity;
flags |= ErrorPathType.PathSingularity;
}
else if (evalue % 10000 > 999)
{
if (evalue % 10000 > 3999)
{
// The robot is too close to the front/back singularity (wrist close to axis 1).
flags |= ErrorPathType.ShoulderSingularity;
flags |= ErrorPathType.PathSingularity;
}
else if (evalue % 10000 > 1999)
{
flags |= ErrorPathType.ElbowSingularity;
flags |= ErrorPathType.PathSingularity;
// Joint 3 is too close the elbow singularity.
}
else
{
// Joint 5 is too close to a singularity (0 degrees).
flags |= ErrorPathType.WristSingularity;
flags |= ErrorPathType.PathSingularity;
flags |= ErrorPathType.PathNearSingularity;
}
}
if (evalue % 10 > 0)
{
// There is no solution available to complete the path.
flags |= ErrorPathType.PathLimit;
}
if (evalue % 100 > 9)
{
// The robot can't make a linear movement because of joint limits or the target is out of reach. Consider a Joint move instead.
flags |= ErrorPathType.PathLimit;
flags |= ErrorPathType.Kinematic;
}
return(flags);
}
