def new_csys_from_xpy(self):
"""
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
self.csys = m3d.Transform()
print(
"A new coordinate system will be defined from the next three points"
)
print("Firs point is X, second Origin, third Y")
print(
"Set it as a new reference by calling myrobot.set_csys(new_csys)")
input("Move to first point and click Enter")
# we do not use get_pose so we avoid rounding values
pose = URRobot.getl(self)
print("Introduced point defining X: {}".format(pose[:3]))
px = m3d.Vector(pose[:3])
input("Move to second point and click Enter")
pose = URRobot.getl(self)
print("Introduced point defining Origo: {}".format(pose[:3]))
p0 = m3d.Vector(pose[:3])
input("Move to third point and click Enter")
pose = URRobot.getl(self)
print("Introduced point defining Y: {}".format(pose[:3]))
py = m3d.Vector(pose[:3])
new_csys = m3d.Transform.new_from_xyp(px - p0, py - p0, p0)
self.set_csys(new_csys)
return new_csys
def new_csys_from_xpy(self):
"""
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
self.csys = m3d.Transform()
print("A new coordinate system will be defined from the next three points")
print("Firs point is X, second Origin, third Y")
print("Set it as a new reference by calling myrobot.set_csys(new_csys)")
input("Move to first point and click Enter")
# we do not use get_pose so we avoid rounding values
pose = URRobot.getl(self)
print("Introduced point defining X: {}".format(pose[:3]))
px = m3d.Vector(pose[:3])
input("Move to second point and click Enter")
pose = URRobot.getl(self)
print("Introduced point defining Origo: {}".format(pose[:3]))
p0 = m3d.Vector(pose[:3])
input("Move to third point and click Enter")
pose = URRobot.getl(self)
print("Introduced point defining Y: {}".format(pose[:3]))
py = m3d.Vector(pose[:3])
new_csys = m3d.Transform.new_from_xyp(px - p0, py - p0, p0)
self.set_csys(new_csys)
return new_csys
def get_pose(self, wait=False, _log=True):
"""
get current transform from base to to tcp
"""
pose = URRobot.getl(self, wait, _log)
trans = self.csys.inverse * m3d.Transform(pose)
if _log:
self.logger.debug("Returning pose to user: %s", trans.pose_vector)
return trans
def get_pose(self, wait=False, _log=True):
"""
get current transform from base to to tcp
"""
pose = URRobot.getl(self, wait, _log)
trans = self.csys.inverse * m3d.Transform(pose)
if _log:
self.logger.debug("Returning pose to user: %s", trans.pose_vector)
return trans
def new_csys_from_xpy(self):
"""
Return new coordinate system from three points: X, Origin, Y
based on math3d: Transform.new_from_xyp
"""
print("A new coordinate system will be defined from the next three points")
print("Firs point is X, second Origin, third Y")
print("Set it as a new reference by calling myrobot.set_csys(new_csys)")
input("Move to first point and click Enter")
# we do not use get_pose so we avoid rounding values
pose = URRobot.getl(self)
p0 = m3d.Vector(pose[:3])
input("Move to second point and click Enter")
pose = URRobot.getl(self)
p1 = m3d.Vector(pose[:3])
input("Move to second point and click Enter")
pose = URRobot.getl(self)
p2 = m3d.Vector(pose[:3])
return m3d.Transform.new_from_xyp(p1 - p0, p2 - p0, p0)
def new_csys_from_xpy(self):
"""
Return new coordinate system from three points: X, Origin, Y
based on math3d: Transform.new_from_xyp
"""
print(
"A new coordinate system will be defined from the next three points"
)
print("Firs point is X, second Origin, third Y")
print(
"Set it as a new reference by calling myrobot.set_csys(new_csys)")
input("Move to first point and click Enter")
# we do not use get_pose so we avoid rounding values
pose = URRobot.getl(self)
p0 = m3d.Vector(pose[:3])
input("Move to second point and click Enter")
pose = URRobot.getl(self)
p1 = m3d.Vector(pose[:3])
input("Move to second point and click Enter")
pose = URRobot.getl(self)
p2 = m3d.Vector(pose[:3])
return m3d.Transform.new_from_xyp(p1 - p0, p2 - p0, p0)
def _get_lin_dist(self, target):
pose = URRobot.getl(self, wait=True)
target = m3d.Transform(target)
pose = m3d.Transform(pose)
return pose.dist(target)
def _get_lin_dist(self, target):
pose = URRobot.getl(self, wait=True)
target = m3d.Transform(target)
pose = m3d.Transform(pose)
return pose.dist(target)
