Commit de1ef531 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

add third vert_or for 3 arm simulation, fix greedy n arm (#43)

parent 5fe53fd1
......@@ -513,10 +513,16 @@ if __name__ == "__main__":
model = "flexirex_big"
tracker = tu.get_tracker(model)
# 3arms simulation
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
[[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
[[0,0,0,1]]*3,
[[0.15,0,0]]*3) # WARNING - also edit urdf
# 2arms real calibration
env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
'calibrations/r2_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc2, gui=False, home=home)
env = tu.SimEnv(env_desc=env_desc3, gui=False, home=home)
# decmcts plan first n contours
c_traj_file = "gcode/" + model + ".c_traj"
......
......@@ -155,8 +155,8 @@ if __name__ == "__main__":
tracker = tu.get_tracker(model)
# 3arms simulation
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,0,1],[0,0,0,1]],
[[0.9, -0.25, 0],[-0.5, -0.25, 0.001],[0, 0.5, 0]],
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
[[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
[[0,0,0,1]]*3,
[[0.15,0,0]]*3) # WARNING - also edit urdf
......@@ -165,4 +165,4 @@ if __name__ == "__main__":
'calibrations/r2_tforms.yaml')
# Greedily plan first n contours
plan = greedy(tracker, env_desc2, n_contours=10, gui=True)
plan = greedy(tracker, env_desc3, n_contours=10, gui=True)
......@@ -46,6 +46,12 @@ if __name__ == "__main__":
model = "flexirex_big"
tracker = tu.get_tracker(model)
# 3arms simulation
env_desc3 = tu.PlanningEnv([[0,0,1,0],[0,0,1,0],[0,0,0,1]],
[[0.9, -0.35, 0],[-0.8, -0.35, 0.001],[0, 0.5, 0]],
[[0,0,0,1]]*3,
[[0.15,0,0]]*3) # WARNING - also edit urdf
# 2arms real calibration
env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
'calibrations/r2_tforms.yaml')
......
......@@ -61,7 +61,11 @@ _vert_or = [np.array([[-6.74517010e-04, 8.33315658e-04, -9.99999404e-01, -2.905
[ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, -8.34294187e-04, 6.73821778e-04, 2.74792069e-06],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]),
np.array([[-6.74517010e-04, -8.33315658e-04, 9.99999404e-01, -2.90597873e-05],
np.array([[-6.74517010e-04, -8.33315658e-04, 9.99999404e-01, -2.90597873e-05], # 2nd arm
[ 8.33732192e-04, 9.99999285e-01, 8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, 8.34294187e-04, -6.73821778e-04, 2.74792069e-06],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]),
np.array([[-6.74517010e-04, -8.33315658e-04, 9.99999404e-01, -2.90597873e-05], # 3rd arm
[ 8.33732192e-04, 9.99999285e-01, 8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, 8.34294187e-04, -6.73821778e-04, 2.74792069e-06],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])]
......
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