From a9faff9a833da80293b8d76c75b340b600db6518 Mon Sep 17 00:00:00 2001
From: Jayant Khatkar <jayant.khatkar@student.uts.edu.au>
Date: Fri, 12 Feb 2021 18:15:25 +1100
Subject: [PATCH] manually change calibration and urdf to get greedy to make a
sensible plan
(https://code.research.uts.edu.au/bigprint/twins-controller/-/issues/19)
---
models/urdf/ur5e_cap.urdf | 4 ++--
src/calibrations/r1_tforms.yaml | 2 +-
src/decmcts.py | 2 +-
src/greedy.py | 11 ++++++-----
4 files changed, 10 insertions(+), 9 deletions(-)
diff --git a/models/urdf/ur5e_cap.urdf b/models/urdf/ur5e_cap.urdf
index 7ccd5ab..59d2602 100644
--- a/models/urdf/ur5e_cap.urdf
+++ b/models/urdf/ur5e_cap.urdf
@@ -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>
diff --git a/src/calibrations/r1_tforms.yaml b/src/calibrations/r1_tforms.yaml
index d172742..bc7acc7 100644
--- a/src/calibrations/r1_tforms.yaml
+++ b/src/calibrations/r1_tforms.yaml
@@ -1,6 +1,6 @@
calibration:
pos_ET: [0.1502296461658024, -0.04698558961114961, -0.031116243011024135]
- pos_WR: [0.03844669361396861, -0.572941079182837, -0.13103114911618197]
+ pos_WR: [0.03844669361396861, -0.572941079182837, -0.13003114911618197]
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [-0.005519573998493153, 0.0023551564258786424, -0.999981578967214, 0.0009106411601250241]
measurements:
diff --git a/src/decmcts.py b/src/decmcts.py
index 546584b..a63aa79 100644
--- a/src/decmcts.py
+++ b/src/decmcts.py
@@ -511,7 +511,7 @@ def decmcts(contour_tracker,
if __name__ == "__main__":
# Read gcode file
- model = "Kitchen_sponge_holder_small-ascii"
+ model = "CuteOcto_x2"
tracker = tu.get_tracker(model)
# 3arms simulation
diff --git a/src/greedy.py b/src/greedy.py
index 705cebd..03d9fad 100644
--- a/src/greedy.py
+++ b/src/greedy.py
@@ -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
@@ -165,4 +166,4 @@ if __name__ == "__main__":
'calibrations/r2_tforms.yaml')
# Greedily plan first n contours
- plan = greedy(tracker, env_desc2, n_contours=50, gui=True)
+ plan = greedy(tracker, env_desc2, n_contours=150, gui=True)
--
GitLab