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 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 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 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 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 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));
}
// extenstions to convert C# Xamla.Robotics.Types to Python Xamla.Robotics.Types
public static PyObject ToPython(this JointSet obj)
{
PyObject jointNames = PyConvert.ToPyObject(obj.ToArray());
using (Py.GIL())
{
return(pyXamlaMotionTypes.JointSet(jointNames));
}
}
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 PyObject ToPython(this JointValues obj)
{
PyObject jointSet = obj.JointSet.ToPython();
PyObject values = PyConvert.ToPyObject(obj.Values);
using (Py.GIL())
{
return(pyXamlaMotionTypes.JointValues(jointSet, values));
}
}
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 PyObject ToPython(this JointStates obj)
{
var positions = PyConvert.ToPyObject(obj.Positions);
var velocities = PyConvert.ToPyObject(obj.Velocities);
var efforts = PyConvert.ToPyObject(obj.Efforts);
using (Py.GIL())
{
return(pyXamlaMotionTypes.JointStates(positions, velocities, efforts));
}
}
public static PyObject ToPython(this TimeSpan obj)
{
var days = PyConvert.ToPyObject(obj.Days);
var seconds = PyConvert.ToPyObject(obj.Hours * 3600 + obj.Minutes * 60 + obj.Seconds);
var microseconds = PyConvert.ToPyObject(obj.Milliseconds * 1000);
using (Py.GIL())
{
return(pyDateTime.timedelta(days: days, seconds: seconds, microseconds: microseconds));
}
}
public static PyObject ToPython(this CollisionPrimitive obj)
{
var kindInt = PyConvert.ToPyObject((int)obj.Kind);
var parameters = PyConvert.ToPyObject(obj.Parameters);
var pose = PyConvert.ToPyObject(obj.Pose);
using (Py.GIL())
{
var kind = pyXamlaMotionTypes.CollisionPrimitiveKind(kindInt);
return(pyXamlaMotionTypes.CollisionPrimitive(kind, parameters, pose));
}
}
public static PyObject ToPython(this CartesianPath obj)
{
using (Py.GIL())
{
var poses = new PyList();
foreach (var p in obj)
{
poses.Append(PyConvert.ToPyObject(p));
}
return(pyXamlaMotionTypes.CartesianPath(poses));
}
}
public static PyObject ToPython(this JointTrajectoryPoint obj)
{
var timeFromStart = PyConvert.ToPyObject(obj.TimeFromStart);
var positions = PyConvert.ToPyObject(obj.Positions);
var velocities = PyConvert.ToPyObject(obj.Velocities);
var accelerations = PyConvert.ToPyObject(obj.Accelerations);
var efforts = PyConvert.ToPyObject(obj.Effort);
using (Py.GIL())
{
return(pyXamlaMotionTypes.JointTrajectoryPoint(timeFromStart, positions, velocities, accelerations, efforts));
}
}
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 static PyObject ToPython(this CollisionObject obj)
{
var frame = PyConvert.ToPyObject(obj.Frame);
using (Py.GIL())
{
var primitives = new PyList();
foreach (var p in obj.Primitives)
{
primitives.Append(PyConvert.ToPyObject(p));
}
return(pyXamlaMotionTypes.CollisionObject(primitives, frame));
}
}
public static PyObject ToPython(this JointPath obj)
{
var jointSet = PyConvert.ToPyObject(obj.JointSet);
using (Py.GIL())
{
var jointValues = new PyList();
foreach (var v in obj)
{
jointValues.Append(PyConvert.ToPyObject(v));
}
return(pyXamlaMotionTypes.JointPath(jointSet, jointValues));
}
}
public static PyObject ToPython(this JointTrajectory obj)
{
var jointSet = PyConvert.ToPyObject(obj.JointSet);
using (Py.GIL())
{
var points = new PyList();
foreach (var p in obj)
{
points.Append(PyConvert.ToPyObject(p));
}
return(pyXamlaMotionTypes.JointTrajectory(jointSet, points));
}
}
private PyObject[] CreateArguments(object[] inputs, int firstArgIndex)
{
var args = new PyObject[argumentPins.Count];
if (firstArgIndex + args.Length != inputs.Length)
{
throw new Exception("Invalid argument count");
}
for (int i = 0; i < args.Length; i++)
{
var x = inputs[firstArgIndex + i];
args[i] = PyConvert.ToPyObject(x);
}
return(args);
}
public static PyObject ToPython(this PlanParameters obj)
{
var nullObject = new object();
nullObject = null;
var none = nullObject.ToPython();
var moveGroupName = PyConvert.ToPyObject(obj.MoveGroupName);
var jointSet = PyConvert.ToPyObject(obj.JointSet);
var maxVelocity = PyConvert.ToPyObject(obj.MaxVelocity);
var maxAcceleration = PyConvert.ToPyObject(obj.MaxAcceleration);
var sampleResolution = PyConvert.ToPyObject(obj.SampleResolution);
var collisionCheck = PyConvert.ToPyObject(obj.CollisionCheck);
var maxDeviation = PyConvert.ToPyObject(obj.MaxDeviation);
using (Py.GIL())
{
return(pyXamlaMotionTypes.PlanParameters.from_arguments(moveGroupName, jointSet, maxVelocity, maxAcceleration, none, none,
sample_resolution: sampleResolution, collisionCheck: collisionCheck,
max_devitation: 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
});
}
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);
}
}
// 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));
}
public static CartesianPath FromPyCartesianPath(PyObject obj)
{
var poses = (List <Pose>)PyConvert.ToClrObject(obj.GetAttr("points"), typeof(List <Pose>));
return(new CartesianPath(poses));
}
