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 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)