Hello everyone,
I tested to create an Addon, which can automatically state, if a point in a program is reachable for a robot.
For this I use the method “Status” from “IPositionFrame”. Used on JointMotionStatements this works quite well. However, LinearMotionStatements or single frames of PathMotionStatements, the method returns always “valid”, even if the position clearly is not reachable.
Anyone here, who knows how I can check the reachability of points in a program without running the simulation?
Short remark: I am using Octopuz. The function itself should be from Visual Components. I am not an expert in C# programming and tried what can be possible with the api.
Kind regards,
Josh
Code Block of the function:
public override void Execute()
{
IMessageService messageService = IoC.Get<IMessageService>();
IApplication app = IoC.Get<IApplication > ();
ISimWorld world = app.World;
ISimulation simulation = app.Simulation;
IRobot robot = IoC.Get<ITeachContext>().ActiveRobot;
IRobotExecutor executor = robot.Executor;
IProgram robotProgram = executor.Program;
var mainroutine = robotProgram.MainRoutine;
IRobotController robCont = robot.RobotController;
// Test to check for point out of reach
var statements = mainroutine.Statements;
messageService.AppendMessage("Number of statements: " + statements.Count, MessageLevel.Warning);
foreach (var statement in statements)
{
messageService.AppendMessage(statement.GetType().Name,MessageLevel.Warning);
if (statement.GetType().Name.Contains("JointMotionStatement"))
{
IJointMotionStatement jointMotion = (IJointMotionStatement)statement;
messageService.AppendMessage(statement.GetType().Name + " ist ein JointMotionStatement", MessageLevel.Warning);
var statementPosition = jointMotion.Positions[0];
messageService.AppendMessage(statementPosition.Status.ToString(), MessageLevel.Warning);
if(statementPosition.Status.ToString() == "0")
{
messageService.AppendMessage("Point not reachable!", MessageLevel.Warning);
var jointValues = statementPosition.InternalJointValues;
foreach(double value in jointValues)
{
messageService.AppendMessage(value.ToString(), MessageLevel.Warning);
}
}
}
if (statement.GetType().Name.Contains("LinearMotionStatement"))
{
ILinearMotionStatement linMotion = (ILinearMotionStatement)statement;
messageService.AppendMessage(statement.GetType().Name + " ist ein LinearMotionStatement", MessageLevel.Warning);
var statementPosition = linMotion.Positions[0];
messageService.AppendMessage(statementPosition.Status.ToString(), MessageLevel.Warning);
if (statementPosition.Status.ToString() == "0")
{
messageService.AppendMessage("Point not reachable!", MessageLevel.Warning);
var jointValues = statementPosition.InternalJointValues;
foreach (double value in jointValues)
{
messageService.AppendMessage(value.ToString(), MessageLevel.Warning);
}
}
}
if (statement.GetType().Name.Contains("Path"))
{
IPathStatement pathMotion = (IPathStatement)statement;
messageService.AppendMessage(statement.GetType().Name + " is a PathStatement", MessageLevel.Warning);
var statementPosition = pathMotion.SchemaPositions;
int i = 0;
bool reachable = true;
foreach (var frame in statementPosition)
{
i += 1;
if (frame.Status.ToString() == "0")
{
reachable = false;
}
}
messageService.AppendMessage("Number of positions:" + i , MessageLevel.Warning);
messageService.AppendMessage("All Positions reachable: " + reachable, MessageLevel.Warning);
}
messageService.AppendMessage(statement.IsValid.ToString(), MessageLevel.Warning);
messageService.AppendMessage("###############", MessageLevel.Warning);
messageService.AppendMessage("Next statement", MessageLevel.Warning);
messageService.AppendMessage("###############", MessageLevel.Warning);
}
}