From 58be13fdd3a0e72e39352d63471b2e2c617f5ba8 Mon Sep 17 00:00:00 2001
From: Alvaro Capellan <alvaro.capellan@sintef.no>
Date: Tue, 5 Jul 2016 16:47:32 +0200
Subject: [PATCH] new_csys_from_xpy: Restore coord. system inside method

---
 urx/robot.py | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/urx/robot.py b/urx/robot.py
index bed5130..51ad313 100644
--- a/urx/robot.py
+++ b/urx/robot.py
@@ -217,7 +217,8 @@ class Robot(URRobot):
 
     def new_csys_from_xpy(self):
         """
-        Return new coordinate system from three points: X, Origin, Y
+        Restores and returns new coordinate system calculated from three points: X, Origin, Y
+
         based on math3d: Transform.new_from_xyp
         """
         # Set coord. sys. to 0
@@ -239,7 +240,11 @@ class Robot(URRobot):
         pose = URRobot.getl(self)
         print("Introduced point defining Y: {}".format(pose[:3]))
         py = m3d.Vector(pose[:3])
-        return m3d.Transform.new_from_xyp(px - p0, py - p0, p0)
+
+        new_csys = m3d.Transform.new_from_xyp(px - p0, py - p0, p0)
+        self.set_csys(new_csys)
+
+        return new_csys
 
     @property
     def x(self):