Fix staubli coord origin

visose · Jan 9, 2023 · cc04df4 · cc04df4
1 parent da3fcef
commit cc04df4
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/src/Robots/PostProcessors/VAL3PostProcessor.cs b/src/Robots/PostProcessors/VAL3PostProcessor.cs
@@ -309,7 +309,10 @@ string Frame(Frame frame)
         }
 
         Plane plane = frame.Plane;
-        plane.InverseOrient(ref _system.BasePlane);
+        var basePlane = _system.BasePlane;
+        var offset = _system.MechanicalGroups[0].Joints[0].D;
+        plane.Origin += basePlane.Normal * offset;
+        plane.InverseOrient(ref basePlane);
         var values = _system.PlaneToNumbers(plane);
 
         string frameName = frame.Name.NotNull();