From b60fcca264301d726fa0a26a24eef28a413f7f6c Mon Sep 17 00:00:00 2001
From: Jayant Khatkar <jayant.khatkar@student.uts.edu.au>
Date: Wed, 10 Feb 2021 16:02:40 +1100
Subject: [PATCH] new calib positions for r1, safe fast calib for angles (#19)
---
calibrations/r1_tforms_alt.yaml | 26 ++++++--------------------
src/KeyboardController.py | 33 ++++++++++++++-------------------
2 files changed, 20 insertions(+), 39 deletions(-)
diff --git a/calibrations/r1_tforms_alt.yaml b/calibrations/r1_tforms_alt.yaml
index fa877d6..5ab6b14 100644
--- a/calibrations/r1_tforms_alt.yaml
+++ b/calibrations/r1_tforms_alt.yaml
@@ -3,29 +3,15 @@ calibration:
pos_WR: [0.033410320051684865, -0.5779185680332759, -0.129]
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [-0.0035406624036226648, 0.00401943913741378, -0.9999807593358486, 0.003128702105328147]
-measurements:
- errors:
- - [0.0007463740373759575, 0.0007358258676100349, 2.272630091060268e-08]
- - [-0.0009020387968710353, 0.00016398070836140533, 1.617623791638767e-05]
- - [-0.00032259677224483196, -0.000623335143616649, 4.8670205514383946e-05]
- - [3.681804322736015e-05, 3.621990973879134e-05, -2.1586398841921817e-05]
- - [0.00044144348854441295, -0.00031269134204364724, -4.3282771052338687e-05]
- pos_RE:
- - [0.08910742634272081, -0.7837172401184285, 0.27405864289606996]
- - [-0.12486057160685166, -0.3528862439702894, 0.2790308995256403]
- - [0.3056457370236582, -0.3563077645137278, 0.27594833824244247]
- - [-0.3553141725527921, -0.56732851836896, 0.27599090687109595]
- - [0.47642188171860617, -0.18642048697664007, 0.20344840058382788]
- quat_RE:
- - [0.2692242146471514, 0.6541983200550241, -0.27068386305481473, 0.6528959538598255]
- - [0.26903425766037653, 0.6541193957881474, -0.2707136444182161, 0.6530409688409196]
- - [0.2689585934995034, 0.6539863896443541, -0.2707389095513149, 0.653194856071048]
- - [0.3517200864442189, 0.3547622789699167, -0.14691067603123373, 0.8537294416147239]
- - [0.019597120228666234, 0.6766897609973048, -0.526359272046243, 0.5144442019996951]
targets:
pos_W:
+ - [-0.215, 0.215, 0]
- [0, 0.215, 0]
+ - [0.215, 0.215, 0]
+ - [0.215, 0, 0]
- [0.215, -0.215, 0]
+ - [0, -0.215, 0]
- [-0.215, -0.215, 0]
- - [0.322, 0, 0]
+ - [-0.215, 0, 0]
- [-0.322, -0.322, 0]
+ - [0.322, 0, 0]
diff --git a/src/KeyboardController.py b/src/KeyboardController.py
index 7c76337..1867ce3 100644
--- a/src/KeyboardController.py
+++ b/src/KeyboardController.py
@@ -602,31 +602,26 @@ class KeyboardController(Controller):
gpose.orientation.x, gpose.orientation.y, \
gpose.orientation.z, gpose.orientation.w = q
+ # check angle diff from current pos
+ cur_pose = self.group.get_current_pose().pose
+ orientation_diff = sum([
+ (cur_pose.orientation.x - gpose.orientation.x)**2,
+ (cur_pose.orientation.y - gpose.orientation.y)**2,
+ (cur_pose.orientation.z - gpose.orientation.z)**2,
+ (cur_pose.orientation.w - gpose.orientation.w)**2])**0.5
+
+ # go home in between if significant angle diff from vertical
+ if orientation_diff > 0.1:
+ self.move_home()
+
# compute path to next config
waypoints = [self.group.get_current_pose().pose, gpose]
plan, fraction = self.group.compute_cartesian_path(
waypoints,
0.01, # ee_step
0)
-
- speed_multiplier(plan.joint_trajectory, 4) # 4x slowers
- # display path on screen and confirm execution
- try:
- print("Displaying Trajectory to estimated point.")
- disp_traj = DisplayTrajectory()
- disp_traj.trajectory_start = self.robot.get_current_state()
- disp_traj.trajectory.append(plan)
- self.disp_traj_pub.publish(disp_traj)
-
- a = raw_input("Execute displayed Trajectory (y/n) ? ")
- if a == 'y':
- self.group.execute(plan, wait=True)
- else:
- print("OK, move manually to next pos.")
-
- except:
- Exception("Only calibrate one arm at a time")
- time.sleep(1)
+ speed_multiplier(plan.joint_trajectory, 4) # 4x slower
+ self.group.execute(plan, wait=True)
with keyboard.Listener(
on_release = lambda key:self._move_callback(key,configurations,params),
--
GitLab