How to make robot evaluate gestures

I tried to make a simulation, when I choose endpoints of edges, and generate Line statement, and robot will also move to those endpoints. But the gesture of robot is wrong, collision happened. So how do I evaluate gesture of robot by c#.

here is my pseudo code to generate statement:

    var geo=Node.GeometrySets.First();
     var cur=geo.TopologyCurves.First();
    var edge = cur.Edge;
     var eps = edge.Endpoints;
     var state=  MainRoutine.AddStatement<LinStatement>();
                            var pe = state.Positions[0];
                            pe.TransformationInWorld = rel;
                            var pq = pe.TransformationInWorld;
                            pq.Px += eps.Item2.X;
                            pq.Py += eps.Item2.Y;
                            pq.Pz += eps.Item2.Z;
                            pe.TransformationInWorld = pq;

I provide my current practices,it works for almost all situations:

var myposMatrix=new Matrix();
//type your own operation on myposMatrix
var posfram=statement.Position[0];
posfram.TransformationInWorld = myposMatrix;
int configNum=8;
for(int i=0;i<configNum;i++)
var rbtcont = robot.RobotController;
var jtdouble= robot.RobotController.Joints.Select(c => c.Value).ToArray();
var ok= CheckingConfig(rbt);
if (ok)


private bool CheckingConfig(IRobot rbt)
var jts = rbt.Controller.Joints;
var maxs = jts.Select(c => c.MaxValue).ToArray();
var mins = jts.Select(c => c.MinValue).ToArray();
var jtdouble = rbt.RobotController.Joints.Select(c => c.Value).ToArray();
for (int i = 0; i < jtdouble.Length; i++)
var val = jtdouble[i];
if (maxs[i] <val || mins[i] > val)
return false;

        return true;