Commit 093d42c5 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

test r2 calibration, add first parse of real plan for first print (#19)

parent 2be137b1
...@@ -55,7 +55,7 @@ class KeyboardController(Controller): ...@@ -55,7 +55,7 @@ class KeyboardController(Controller):
""" """
def __init__(self, def __init__(self,
homeConf_deg=np.array([-90,-90,45,-45,-90,-45]), homeConf_deg=np.array([0,-90,45,-45,-90,-45]),
waitForExtruder=True, waitForExtruder=True,
timeScale=1.0, timeScale=1.0,
disableExtruder=False, disableExtruder=False,
...@@ -595,7 +595,7 @@ class KeyboardController(Controller): ...@@ -595,7 +595,7 @@ class KeyboardController(Controller):
if priors: if priors:
# calculate IK for next point and move there # calculate IK for next point and move there
mat = np.identity(4) mat = np.identity(4)
p[2] += 0.01 # go 1cm above bed for safety #p[2] += 0.01 # go 1cm above bed for safety
mat[:3,3] = p mat[:3,3] = p
mat = world2armT.dot(mat).dot(_vert_or[arm]).dot(tip2armT) mat = world2armT.dot(mat).dot(_vert_or[arm]).dot(tip2armT)
q = Rotation.from_dcm(mat[:3,:3]).as_quat() q = Rotation.from_dcm(mat[:3,:3]).as_quat()
...@@ -662,14 +662,14 @@ class KeyboardController(Controller): ...@@ -662,14 +662,14 @@ class KeyboardController(Controller):
if __name__=="__main__": if __name__=="__main__":
con = KeyboardController(disableExtruder=True) #con = KeyboardController(disableExtruder=True)
#con = KeyboardController() #con = KeyboardController()
#con1 = KeyboardController(robot=1) #con1 = KeyboardController(robot=1)
#con2 = KeyboardController(robot=2, disableExtruder=True) con2 = KeyboardController(robot=2, disableExtruder=True)
print('Collecting configrations...') print('Collecting configrations...')
configuations = con.prompt_configurations() #configuations = con.prompt_configurations()
#configuations = con1.prompt_configurations() #configuations = con1.prompt_configurations()
#configuations = con2.prompt_configurations() configuations = con2.prompt_configurations()
print('Your configurations:') print('Your configurations:')
print(configuations) print(configuations)
...@@ -83,7 +83,7 @@ def execute_plan(plan, ...@@ -83,7 +83,7 @@ def execute_plan(plan,
if __name__ == '__main__': if __name__ == '__main__':
plan1 = load_plan('origin') plan1 = load_plan('origin')
calibration_test = load_plan('r1_cal_test') calibration_test = load_plan('r2_cal_test')
plan2, flexirex_contours = load_plan('flexirex', contours=True) plan2, flexirex_contours = load_plan('flexirex', contours=True)
# Printing # Printing
......
...@@ -22,7 +22,6 @@ def execute_plan(plan, ...@@ -22,7 +22,6 @@ def execute_plan(plan,
r1_con, r1_con,
r2_con, r2_con,
contours=None, contours=None,
contour_speed=0.2,
move_speed=1, move_speed=1,
retry_time=0.1, retry_time=0.1,
max_traj_retries=20): max_traj_retries=20):
...@@ -84,7 +83,7 @@ def execute_plan(plan, ...@@ -84,7 +83,7 @@ def execute_plan(plan,
# Extruding with trajectory # Extruding with trajectory
if t.contour is not None and contours is not None: if t.contour is not None and contours is not None:
traj2send = speed_multiplier(t, 1/contour_speed) traj2send = speed_multiplier(t, 1/move_speed)
while not controllers[arm].exec_ctraj( while not controllers[arm].exec_ctraj(
traj2send, traj2send,
contour=contours[t.contour], contour=contours[t.contour],
...@@ -117,7 +116,7 @@ def execute_plan(plan, ...@@ -117,7 +116,7 @@ def execute_plan(plan,
if __name__ == '__main__': if __name__ == '__main__':
plan2arms = load_plan('test_rotation2') plan2arms = load_plan('octox2')
r1_con = Controller(disable_extruder=True, robot=1) r1_con = Controller(disable_extruder=True, robot=1)
r2_con = Controller(disable_extruder=True, robot=2) r2_con = Controller(disable_extruder=True, robot=2)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment