Commit 1e41aa27 authored by Jayant Khatkar's avatar Jayant Khatkar

Merge branch 'feature/first-print'

parents bfddec85 a9faff9a
......@@ -298,7 +298,7 @@
<visual>
<origin rpy="0 0 0" xyz="0.074 0 0"/>
<geometry>
<box size="0.148 0.23 0.13"/>
<box size="0.145 0.23 0.13"/>
</geometry>
<material name="pt_material">
<color rgba="0.3 0.8 0.3 0.95"/>
......@@ -307,7 +307,7 @@
<collision>
<origin rpy="0 0 0" xyz="0.074 0 0"/>
<geometry>
<box size="0.148 0.23 0.13"/>
<box size="0.145 0.23 0.13"/>
</geometry>
</collision>
</link>
......
......@@ -4,7 +4,7 @@ certifi==2020.11.8
cycler==0.10.0
Cython==0.29.21
decorator==4.4.2
gcode2contour @ git+https://code.research.uts.edu.au/bigprint/gcode2contour.git@01eb49aee19574572fafdb7d8134c3638783fc75
gcode2contour @ git+https://code.research.uts.edu.au/bigprint/gcode2contour.git@defe7cf5eefe80e4dda02a7d051d22245402aa2a
ipython==7.19.0
ipython-genutils==0.2.0
jedi==0.17.2
......
calibration:
pos_ET: [0.15007592984204163, -0.04830698520623148, -0.031668261140098686]
pos_WR: [0.03655708997718542, -0.575405839173846, -0.13224784915908203]
pos_ET: [0.1502296461658024, -0.04698558961114961, -0.031116243011024135]
pos_WR: [0.03844669361396861, -0.572941079182837, -0.13003114911618197]
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [-0.004907671722772035, 0.0026006325187357495, -0.9999828094804524, 0.0018794180869107977]
quat_WR: [-0.005519573998493153, 0.0023551564258786424, -0.999981578967214, 0.0009106411601250241]
measurements:
errors:
- [-0.00035093605232278113, 0.0015154904646795808, 0.0005004698912217387]
- [0.0008203797060748544, 0.0008922491648686859, 0.0002722545866968018]
- [0.001289894680197884, 0.0009346809299097258, -0.0011471590539748888]
- [-0.00026782915015455266, -0.00019411428454588426, -0.00017438502446384674]
- [-0.000711953482581712, -0.00017432903503347696, 0.0007754958989695804]
- [-0.0009272510092923555, -0.001473229551918892, 0.0009148858434974705]
- [-0.0007779454099607619, -0.0013991536560543083, -9.067649884858531e-05]
- [-0.0006907081723367681, -0.00013629415861853315, 0.0005679499481896777]
- [0.0014934971392888685, -0.000434524141417425, -0.001318136805991993]
- [0.00012285175106990787, 0.0004692242681248698, -0.0003006987853121923]
- [0.00015395547326951808, 0.002142423630859619, 0.0003355815224046488]
- [-1.721708528695104e-05, 5.051743681194765e-05, 0.00041670383526773436]
- [0.00014560316238965143, -1.839592857558192e-05, -0.0009316653575424105]
- [3.161792856745671e-05, -2.53463553584113e-05, -9.16948482303781e-05]
- [-0.0010221720460194261, -1.0771535927212028e-05, 0.0012171946733552097]
- [-0.0010681151514920634, -0.0009799175199797783, 2.801905010083039e-05]
- [-6.682373151054022e-05, -0.0008270223124680898, -0.0008691193706457379]
- [2.541162594885127e-05, 0.0001367769605153022, 0.0006730588199809484]
- [0.001417878916551707, -0.0008426181698256152, -0.00037090151330573806]
- [0.0003998609358597527, 0.00037435379811547335, -0.0004071767733215548]
pos_RE:
- [0.28238526739189035, -0.743445281602937, 0.2753943928759848]
- [0.06856808024088912, -0.7433152737798132, 0.27773399071687555]
- [-0.14592886427691276, -0.7424717736062426, 0.28125724809030916]
- [-0.14669659051873055, -0.528554844189385, 0.2813981571448582]
- [-0.1463260383308909, -0.3135266064730779, 0.28156287698542204]
- [0.06844080035242153, -0.3156302723662834, 0.27930760468097077]
- [0.283587823473315, -0.31634541818249157, 0.2781937485538753]
- [0.28285396776950816, -0.530129589269635, 0.2764449159253464]
- [0.5066506093478649, -0.20096898804832836, 0.16677352259446088]
- [-0.369184926735048, -0.5275417892542257, 0.2618225946251175]
- [0.28619790934684075, -0.7402544719962719, 0.2742202952337487]
- [0.07105893398903566, -0.7419692559154405, 0.2765033963380906]
- [-0.1437662590736294, -0.7416598402185833, 0.2802276770777924]
- [-0.1434836763099208, -0.5266801743305487, 0.2804022408548177]
- [-0.14417271043384325, -0.3116608075668326, 0.2801197811161653]
- [0.0708014310233577, -0.31297986484632256, 0.2789127806741849]
- [0.2867904938256065, -0.31325436323976485, 0.2774381453273922]
- [0.28647132057396424, -0.5272750808751241, 0.2748870851772862]
- [0.49198953051379024, -0.1614223801212086, 0.15202211073521693]
- [-0.36583396130433044, -0.5260828760890328, 0.26097777977314823]
quat_RE:
- [0.004515506751761712, 0.7093312647790816, 0.0005263393718474751, 0.7048606174078313]
- [0.004551772876819091, 0.7093577385565347, 0.000732948249208785, 0.7048335568774383]
- [0.004513807736845369, 0.7093838031536429, 0.0007461847780781305, 0.7048075542807879]
- [0.00442392266478542, 0.709359777178923, 0.0006503084317854549, 0.7048324003107893]
- [0.004471491373419877, 0.7093586207919866, 0.0005390274408393596, 0.7048333578388627]
- [0.004466760991158438, 0.7093646956760709, 0.0005683729336144619, 0.7048272508401273]
- [0.004391686236732675, 0.7093756214129284, 0.0005693173620683987, 0.704816725620754]
- [0.00448742433570489, 0.7093749968660662, 0.000748052655762252, 0.7048165841276184]
- [-0.00045970685855608695, 0.6540762178342059, -0.653219981430286, 0.38142855922639246]
- [0.003642363656463826, 0.38561399879651764, 0.002617478574255283, 0.922649297363341]
- [0.0010413200769373307, 0.7105862039337016, 0.0006649326483706677, 0.7036090678041056]
- [0.001084891693411131, 0.7106329642035853, 0.0006550901734732243, 0.7035617841057772]
- [0.0010554592940128175, 0.7106074401362242, 0.0007063304987061758, 0.7035875589618877]
- [0.0011041161037969283, 0.7106222933336699, 0.0007199736181426762, 0.7035724687498894]
- [0.0011314856150625339, 0.7105696678299577, 0.0006810590537584474, 0.7036256128502517]
- [0.00101736222811157, 0.7106283885489839, 0.0006081311970953219, 0.7035665487633834]
- [0.0010867007796504266, 0.7106128191421314, 0.0007128943418593383, 0.7035820720668952]
- [0.0010999875504787095, 0.7106047226468447, 0.0006587354591232912, 0.7035902815182887]
- [-0.14338537701650084, 0.8552601557202018, -0.3542228559665395, 0.34999552569454434]
- [0.00045600965998949725, 0.3870842766393148, 0.0012555108399978506, 0.922043371282685]
targets:
pos_W:
- [-0.215, 0.215, 0]
......
......@@ -511,7 +511,7 @@ def decmcts(contour_tracker,
if __name__ == "__main__":
# Read gcode file
model = "flexirex_big"
model = "CuteOcto_x2"
tracker = tu.get_tracker(model)
# 3arms simulation
......@@ -523,13 +523,12 @@ if __name__ == "__main__":
# 2arms real calibration
env_desc2 = tu.read_env('calibrations/r1_tforms.yaml',
'calibrations/r2_tforms.yaml')
env = tu.SimEnv(env_desc=env_desc3, gui=False, home=home)
env = tu.SimEnv(env_desc=env_desc2, gui=True, home=home)
# decmcts plan first n contours
c_traj_file = "gcode/" + model + ".c_traj"
cs, plan = decmcts(tracker,
env,
n_layers=2,
n_layers=1,
mcts_nodes=1000
)
visualiseCombinedSchedule(cs)
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
......@@ -13,7 +13,8 @@ import pickle
import copy
home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
home = [0, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
home2 = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def plan(points, env, gui=False):
......@@ -27,23 +28,26 @@ def plan(points, env, gui=False):
# Got to each point
for p in points:
# Go to 5cm above the point
p_above = copy.deepcopy(p)
p_above[2] = p[2] + 0.05
jabove = tu.get_closest(env.ik_pos(p_above,0),home, max_diff=100)
go2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([go2above], arm)
# Go to the point
jpoint = tu.get_closest(env.ik_pos(p,0),home, max_diff=100)
go2point = plan.plan_to(jpoint, arm)
plan.appendTrajectory([go2point], arm)
# go to 5cm above point again
back2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([back2above], arm)
print("-----")
try:
# Go to 5cm above the point
p_above = copy.deepcopy(p)
p_above[2] = p[2] + 0.05
jabove = tu.get_closest(env.ik_pos(p_above,0), home2, max_diff=100)
go2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([go2above], arm)
# Go to the point
jpoint = tu.get_closest(env.ik_pos(p,0), home2, max_diff=100)
go2point = plan.plan_to(jpoint, arm)
plan.appendTrajectory([go2point], arm)
# go to 5cm above point again
back2above = plan.plan_to(jabove, arm)
plan.appendTrajectory([back2above], arm)
except:
print("Could not do point {}".format(p))
# try going to home pos
gohome = plan.plan_to(env.home, arm)
......
......@@ -7,7 +7,7 @@ import warnings
import pickle
home = [-pi/2, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
home = [0, -pi/2, pi/4, -pi/4, -pi/2, -pi/4]
def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
......@@ -15,9 +15,9 @@ def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
Greedily create a plan on for n arms printer setup
"""
if n_contours is None:
#n_contours = len(contour_tracker.all_contours)
n_contours = len(contour_tracker.available_contours)
if n_contours is None or \
n_contours > len(contour_tracker.available_contours):
n_contours = len(contour_tracker.all_contours)
l_height = tu.get_layer_height(contour_tracker)
bound_mins, bound_maxs = tu.get_bounding_box(contour_tracker)
......@@ -109,7 +109,8 @@ def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
# stay at home for another 10s
stayhome = tu.JTrajectory([home, home],[0,10], arm)
if plan.appendTrajectory([stayhome], arm)!=0:
raise Exception("cannot stay at home - this should never happen, choose a better home position")
return plan
#raise Exception("cannot stay at home - this should never happen, choose a better home position")
else:
# try going to home pos
......@@ -151,7 +152,7 @@ def greedy(contour_tracker, env_desc, n_contours=None, gui=False):
if __name__ == "__main__":
# Read gcode file
model = "flexirex_big"
model = "CuteOcto_x2"
tracker = tu.get_tracker(model)
# 3arms simulation
......@@ -165,4 +166,4 @@ if __name__ == "__main__":
'calibrations/r2_tforms.yaml')
# Greedily plan first n contours
plan = greedy(tracker, env_desc3, n_contours=10, gui=True)
plan = greedy(tracker, env_desc2, n_contours=150, gui=True)
import gcode2contour as gc
from pydecmcts import Tree
import utils as tu
from collections import deque
import matplotlib.pyplot as plt
from math import pi
import time
import numpy as np
import pickle
import copy
import warnings
import random
import argparse
if __name__ == "__main__":
# load plan address from args
parser = argparse.ArgumentParser(description='to watch, visulise and inspect loaded plans')
parser.add_argument('plan',
metavar='plan',
help='plan to read')
args = parser.parse_args()
# load plan
plan = tu.load_plan(args.plan, gui=True)
print("plan loaded, it is {}s long".format(plan.len()))
print("use plan.watch(x) to watch the plan in 3D")
print("use plan.visualise(x) to plot plan")
......@@ -428,7 +428,7 @@ def get_tracker(model):
# 5mm safety, max 5 layers ahead
print("\nCalculating dependency graph - this could take some time...")
tracker = gc.DependencyGraph(contours, 5, 5)
tracker = gc.DependencyGraph(contours, 5, 1)
with open(pkl, 'wb') as f:
pickle.dump(tracker, f)
......
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