diff --git a/script/devices/RobotBernina.py b/script/devices/RobotBernina.py index 5c14f90..a359071 100644 --- a/script/devices/RobotBernina.py +++ b/script/devices/RobotBernina.py @@ -134,6 +134,20 @@ class RobotBernina(RobotTCP): print("Point is not in range") return reachable + def simulate_stored_commands(self): + if self.last_remote_motion == "None": + ret = None + elif self.last_remote_motion == "cartesian": + target = self.cartesian_motors["x"].target_pos + ret = move_cartesian(simulate = True, **target) + elif self.last_remote_motion == "spherical": + target = self.spherical_motors["r"].target_pos + ret = move_spherical(simulate = True, **target) + elif self.last_remote_motion == "joint": + target = self.joint_motors["j1"].target_pos + ret = self.move_joint(simulate = True, **target) + return ret + def move_cartesian(self, x=None, y=None, z=None, rx=None, ry=None, rz=None, tool=None, frame=None, desc=None, sync=False, simulate=False, coordinates="joint"): """ Motion in the cartesian coordinate system using a linear motion movel command to @@ -280,9 +294,18 @@ class RobotBernina(RobotTCP): for k, v in target_jnt.items(): if v == None: target_jnt[k] = current_jnt[k] - self.set_jnt([target_jnt[k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]], name="tcp_j") - self.set_motion_queue_empty(False) - self.movej("tcp_j", tool , desc) + if simulate: + sim = [] + cur_joint = [current_jnt[k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]] + delta_joint = [target_jnt[k]-current_jnt[k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]] + for i in range(11): + sim.append([j0+dj*i/10 for j0, dj in zip(cur_joint, delta_joint)]) + else: + self.set_jnt([target_jnt[k] for k in ["j1", "j2", "j3", "j4", "j5", "j6"]], name="tcp_j") + self.set_motion_queue_empty(False) + self.movej("tcp_j", tool , desc) + if simulate: + return sim def move_home(self): if not self.is_in_point(P_HOME):