pshell_config/bernina_robot
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added method to simulate stored motions

Browse Source
This commit is contained in:
gac-bernina
2024-01-15 15:17:55 +01:00
parent cb4b71fff3
commit 6a060751fb
1 changed files with 26 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
script/devices/RobotBernina.py
+26 -3
View File
@@ -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):