public static CollisionObject FromPyCollisionObject(PyObject obj)
{
var frame = (string)PyConvert.ToClrObject(obj.GetAttr("frame_id"), typeof(string));
var primitives = (List <CollisionPrimitive>)PyConvert.ToClrObject(obj.GetAttr("primitives"), typeof(List <CollisionPrimitive>));
return(new CollisionObject(frame, primitives));
}
public static JointPath FromPyJointPath(PyObject obj)
{
var jointSet = (JointSet)PyConvert.ToClrObject(obj.GetAttr("joint_set"), typeof(JointSet));
var points = (List <JointValues>)PyConvert.ToClrObject(obj.GetAttr("points"), typeof(List <JointValues>));
return(new JointPath(jointSet, points));
}
public static JointValues FromPyJointValues(PyObject obj)
{
var jointSet = FromPyJointSet(obj.GetAttr("joint_set"));
var values = (A <double>)PyConvert.ToClrObject(obj.GetAttr("values"), typeof(A <double>));
return(new JointValues(jointSet, values));
}
public static JointStates FromPyJointStates(PyObject obj)
{
var positions = (JointValues)PyConvert.ToClrObject(obj.GetAttr("positions"), typeof(JointValues));
var velocities = (JointValues)PyConvert.ToClrObject(obj.GetAttr("velocities"), typeof(JointValues));
var efforts = (JointValues)PyConvert.ToClrObject(obj.GetAttr("efforts"), typeof(JointValues));
return(new JointStates(positions, velocities, efforts));
}
public static JointTrajectory FromPyJointTrajectory(PyObject obj)
{
var jointSet = (JointSet)PyConvert.ToClrObject(obj.GetAttr("joint_set"), typeof(JointSet));
var points = (List <JointTrajectoryPoint>)PyConvert.ToClrObject(obj.GetAttr("points"), typeof(List <JointTrajectoryPoint>));
var valid = (bool)PyConvert.ToClrObject(obj.GetAttr("is_valid"), typeof(bool));
return(new JointTrajectory(jointSet, points, valid));
}
public static TimeSpan FromPyTimeDelta(PyObject obj)
{
var days = (int)PyConvert.ToClrObject(obj.GetAttr("days"), typeof(int));
var seconds = (int)PyConvert.ToClrObject(obj.GetAttr("seconds"), typeof(int));
var microseconds = (int)PyConvert.ToClrObject(obj.GetAttr("microseconds"), typeof(int));
return(new TimeSpan((long)days * 864000000000 + (long)seconds * 10000000 + (long)microseconds * 10));
}
public static CollisionPrimitive FromPyCollisionPrimitive(PyObject obj)
{
var pyKind = obj.GetAttr("kind");
var kind = (CollisionPrimitiveKind)PyConvert.ToClrObject(pyKind.GetAttr("value"), typeof(int));
var parameters = (A <double>)PyConvert.ToClrObject(obj.GetAttr("parameters"), typeof(A <double>));
var pose = (Pose)PyConvert.ToClrObject(obj.GetAttr("pose"), typeof(Pose));
return(new CollisionPrimitive(kind, parameters.ToArray(), pose));
}
public static JointTrajectoryPoint FromPyJoinTrajectoryPoint(PyObject obj)
{
var timeFromStart = (TimeSpan)PyConvert.ToClrObject(obj.GetAttr("time_from_start"), typeof(TimeSpan));
var positions = (JointValues)PyConvert.ToClrObject(obj.GetAttr("positions"), typeof(JointValues));
var velocities = (JointValues)PyConvert.ToClrObject(obj.GetAttr("velocities"), typeof(JointValues));
var accelerations = (JointValues)PyConvert.ToClrObject(obj.GetAttr("accelerations"), typeof(JointValues));
var efforts = (JointValues)PyConvert.ToClrObject(obj.GetAttr("efforts"), typeof(JointValues));
return(new JointTrajectoryPoint(timeFromStart, positions, velocities, accelerations, efforts));
}
public static Pose FromPyPose(PyObject obj)
{
var t = (A <double>)PyConvert.ToClrObject(obj.GetAttr("translation"), typeof(A <double>));
var translationVector = new Vector3((float)t[0], (float)t[1], (float)t[2]);
var pyQuaternion = obj.GetAttr("quaternion");
var q = (A <double>)PyConvert.ToClrObject(pyQuaternion.GetAttr("elements"), typeof(A <double>));
var quaternion = new Quaternion((float)q[1], (float)q[2], (float)q[3], (float)q[0]);
var frame = (string)PyConvert.ToClrObject(obj.GetAttr("frame_id"), typeof(string));
return(new Pose(translationVector, quaternion, frame));
}
public static PlanParameters FromPyPlanParameters(PyObject obj)
{
var moveGroupName = (string)PyConvert.ToClrObject(obj.GetAttr("move_group_name"), typeof(string));
var jointSet = (JointSet)PyConvert.ToClrObject(obj.GetAttr("joint_set"), typeof(JointSet));
var maxVelocity = (A <double>)PyConvert.ToClrObject(obj.GetAttr("max_velocity"), typeof(A <double>));
var maxAcceleration = (A <double>)PyConvert.ToClrObject(obj.GetAttr("max_acceleration"), typeof(A <double>));
var sampleResolution = (double)PyConvert.ToClrObject(obj.GetAttr("sample_resolution"), typeof(double));
var collisionCheck = (bool)PyConvert.ToClrObject(obj.GetAttr("collision_check"), typeof(bool));
var maxDeviation = (double)PyConvert.ToClrObject(obj.GetAttr("max_deviation"), typeof(double));
return(new PlanParameters(moveGroupName, jointSet, maxVelocity.ToArray(), maxAcceleration.ToArray(),
sampleResolution, collisionCheck, maxDeviation));
}
public object Run(
[InputPin(Name = "caption", Description = "The caption of this module", PropertyMode = PropertyMode.Always, Editor = WellKnownEditors.SingleLineText)] string caption,
[InputPin(Name = "expression", Description = "The Python expression", PropertyMode = PropertyMode.Always, Editor = WellKnownEditors.SingleLineText)] string expression,
[InputPin(Name = "useMainThread", Description = "Whether to run the code on the Python main thread", PropertyMode = PropertyMode.Always, Editor = WellKnownEditors.CheckBox, DefaultValue = "false")] bool useMainThread,
[InputPin(Name = "input", Description = "These are the inputs for the expression", PropertyMode = PropertyMode.Never)] params object[] inputs
)
{
object evalBlock()
{
using (PyScope ps = Py.CreateScope())
{
int i = 0;
foreach (var inputPin in this.dynamicInputPin.Pins)
{
if (inputPin.Connections.Any())
{
ps.Set(inputPin.Id, PyConvert.ToPyObject(inputs[i]));
}
i++;
}
ps.Set(dynamicInputPin.Alias + "s", inputs);
PyObject evalResult = ps.Eval(expression);
return(PyConvert.ToClrObject(evalResult, typeof(object)));
}
}
if (useMainThread)
{
return(mainThread.EvalSync(evalBlock));
}
else
{
using (Py.GIL())
{
return(evalBlock());
}
}
}
public XamlaPythonException(PythonException e) : base(e.Message, e)
{
IntPtr pyTBPython = e.PyTB;
using (Py.GIL())
{
if (pyTBPython != IntPtr.Zero)
{
PyObject tb_module = PythonEngine.ImportModule("traceback");
using (var pyTB = new PyObject(pyTBPython))
{
var traceRaw = (List <string>)PyConvert.ToClrObject(tb_module.InvokeMethod("format_tb", pyTB),
typeof(List <string>));
stackTraceElements = traceRaw.Select(i => Regex.Replace(i, @"\r\n?|\n", "")).ToList();
}
stackTrace = String.Join(System.Environment.NewLine, stackTraceElements);
}
}
message = String.Join(System.Environment.NewLine, new List <string> {
e.Message, "Stack trace: ", stackTrace
});
}
public static CartesianPath FromPyCartesianPath(PyObject obj)
{
var poses = (List <Pose>)PyConvert.ToClrObject(obj.GetAttr("points"), typeof(List <Pose>));
return(new CartesianPath(poses));
}
// methods to convert Python Xamla.Robotics.Types to C# Xamla.Robotics.Types
public static JointSet FromPyJointSet(PyObject obj)
{
var names = (List <string>)PyConvert.ToClrObject(obj.GetAttr("names"), typeof(List <string>));
return(new JointSet(names));
}
protected override async Task <object[]> EvaluateInternal(object[] inputs, CancellationToken cancel)
{
if (parserError != null)
{
throw parserError;
}
var script = this.Script;
if (script == null)
{
return(new object[0]); // no script available
}
object[] returnValue = null;
void evalAction()
{
// create new python scope
using (PyScope ps = Py.CreateScope())
{
if (!string.IsNullOrEmpty(filename))
{
ps.Set("__file__", filename);
}
// load script into scope
ps.Execute(script);
ps.Set("context", this.Runtime.ScriptContext.ToPython());
ps.Set("store", this.Graph.ValueStore.ToPython());
// process module inputs and convert them to python objects
int firstArgIndex = inputs.Length - argumentPins.Count;
var args = CreateArguments(inputs, firstArgIndex);
// call python method
string functionName = (string)inputs[FUNCTION_NAME_PIN_INDEX] ?? DEFAULT_FUNCTION_NAME;
PyObject fn = ps.Get(functionName);
PyObject pyResult = null;
try
{
pyResult = fn.Invoke(args);
}
catch (PythonException e)
{
throw new XamlaPythonException(e);
}
// convert result back to clr object
var result = PyConvert.ToClrObject(pyResult, signature.ReturnType);
if (result == null)
{
returnValue = new object[] { null }
}
;
else if (!result.GetType().IsTuple())
{
returnValue = new object[] { result }
}
;
else
{
returnValue = TypeHelpers.GetTupleValues(result);
}
}
}
int useMainThreadPinIndex = 2;
if (this.flowMode != FlowMode.NoFlow)
{
useMainThreadPinIndex += 1;
}
bool useMainThread = (bool)inputs[useMainThreadPinIndex];
if (useMainThread)
{
await mainThread.Enqueue(evalAction, cancel);
}
else
{
using (Py.GIL())
{
evalAction();
}
}
if (this.flowMode != FlowMode.NoFlow)
{
return((new object[] { Flow.Default }.Concat(returnValue)).ToArray());
}
else
{
return(returnValue);
}
}
