diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
index f16debe88377c08adde32dd68a6a0aafb6b8ad24..a9ef8b4996e0bd1281f5d0bd4956cb4b044d3265 100644
Binary files a/python/examples/.drawing_from_input_drawing.py.swp and b/python/examples/.drawing_from_input_drawing.py.swp differ
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index bfa0b35bbd9bb523912534841b0b45c8be3a6293..d8760ef2902274655759a56484d1a4c854556aa7 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -59,8 +59,8 @@ def getArgs():
TODO: check what this means", default=0.3)
parser.add_argument('--speed-slider', type=float,\
help="cap robot's speed with the speed slider \
- to something between 0 and 1, 0.5 by default \
- BE CAREFUL WITH THIS.", default=0.4)
+ to something between 0 and 1, 1.0 by default because for dmp. \
+ BE CAREFUL WITH THIS.", default=1.0)
parser.add_argument('--max-iterations', type=int, \
help="maximum allowable iteration number (it runs at 500Hz)", default=50000)
#######################################################################
@@ -166,8 +166,8 @@ Obviously it's just a hacked solution, but it works so who cares.
"""
def getMarkerOffset(rotation_matrix, translation_vector, q_init):
# put this in a correct place
- old_speed_slider = robot.speed_slider
- robot.setSpeedSlider(0.3)
+# old_speed_slider = robot.speed_slider
+# robot.setSpeedSlider(0.3)
# TODO: this isn't gonna work because it's not going
# orthogonaly to the board
# so TODO: calculate TCP speed based on the rotation matrix
@@ -191,7 +191,7 @@ def getMarkerOffset(rotation_matrix, translation_vector, q_init):
print("translation_vector - current_translation", \
translation_vector - current_translation)
marker_offset = np.linalg.norm(translation_vector - current_translation)
- robot.setSpeedSlider(old_speed_slider)
+# robot.setSpeedSlider(old_speed_slider)
return marker_offset
#######################################################################
@@ -299,7 +299,7 @@ if __name__ == "__main__":
# do calibration if specified
if args.calibration:
rotation_matrix, translation_vector, q_init = \
- calibratePlane(robot, args.board_width, args.board_height, \
+ calibratePlane(args, robot, args.board_width, args.board_height, \
args.n_calibration_tests)
else:
# TODO: save this somewhere obviously
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 7714927a78d98221b2209eaed8d32f2703856d96..6f2f56163bc2f98ba3e080c746ef686d05b2c35f 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,124 +1,49 @@
-0.27578,0.76759
-0.27727,0.76608
-0.28025,0.76458
-0.31307,0.73749
-0.33843,0.71793
-0.36976,0.69686
-0.38169,0.68933
-0.39064,0.68332
-0.41749,0.66676
-0.42346,0.66074
-0.42793,0.65623
-0.45478,0.63967
-0.45926,0.63817
-0.46522,0.63215
-0.49208,0.61560
-0.49655,0.61259
-0.50401,0.60657
-0.52191,0.59754
-0.52638,0.59603
-0.52788,0.59453
-0.53533,0.59152
-0.55920,0.58249
-0.56368,0.57948
-0.59053,0.56293
-0.59948,0.55992
-0.60992,0.55691
-0.62782,0.54938
-0.63826,0.54637
-0.63975,0.54487
-0.64572,0.53885
-0.66213,0.52982
-0.66362,0.52982
-0.66810,0.52681
-0.67257,0.52380
-0.67406,0.52229
-0.67555,0.52229
-0.68003,0.51628
-0.68152,0.51477
-0.68301,0.51327
-0.68749,0.50725
-0.68749,0.50574
-0.68898,0.50424
-0.68898,0.50273
-0.68898,0.50123
-0.68749,0.50123
-0.68301,0.49822
-0.65616,0.48919
-0.64721,0.48919
-0.61290,0.47865
-0.60544,0.47564
-0.59649,0.47564
-0.56219,0.46511
-0.52489,0.45909
-0.48313,0.45909
-0.47417,0.45608
-0.45777,0.45608
-0.44882,0.45307
-0.41152,0.44856
-0.39511,0.44856
-0.38169,0.44856
-0.34738,0.44254
-0.33694,0.44254
+0.18180,0.64870
+0.19075,0.65171
+0.24744,0.65171
+0.26534,0.64870
+0.29517,0.64268
+0.30263,0.64118
+0.30710,0.63817
+0.33395,0.62312
+0.37274,0.58249
+0.37423,0.57797
+0.37871,0.56744
+0.38169,0.55841
+0.38169,0.54788
+0.38169,0.54336
+0.38169,0.53885
+0.38169,0.51477
+0.38169,0.50574
+0.37871,0.49822
+0.37274,0.49220
+0.35782,0.46661
+0.35186,0.46210
+0.32500,0.44555
0.32053,0.44254
-0.29219,0.44254
-0.27578,0.44254
-0.26534,0.44254
-0.21611,0.44254
-0.20567,0.44254
-0.19821,0.44254
-0.16390,0.44856
-0.16241,0.44856
-0.15943,0.45006
-0.14898,0.45006
-0.14749,0.45006
-0.14600,0.45157
-0.14451,0.45157
-0.14451,0.45307
-0.14451,0.45458
-0.14451,0.45608
-0.15048,0.45608
-0.15197,0.45759
-0.15793,0.46360
-0.16241,0.46511
-0.17434,0.47263
-0.18180,0.47564
-0.20269,0.48919
-0.21313,0.49220
-0.22208,0.49521
-0.26832,0.52229
-0.27876,0.52530
-0.28771,0.52831
-0.29965,0.53584
-0.32202,0.54637
-0.32948,0.54788
-0.33694,0.55390
-0.34589,0.55691
-0.37125,0.57196
-0.37572,0.57497
-0.38467,0.57647
-0.41003,0.59302
-0.41451,0.59453
-0.42495,0.60205
-0.44136,0.60958
-0.45180,0.61710
-0.45627,0.62011
-0.46522,0.62162
-0.49208,0.63817
-0.49804,0.64118
-0.50998,0.64870
-0.55025,0.67278
-0.56815,0.68181
-0.57710,0.68482
-0.61290,0.70137
-0.61738,0.70739
-0.62633,0.71040
-0.65318,0.72696
-0.66064,0.72846
-0.66511,0.73147
-0.67555,0.73448
-0.67705,0.73599
-0.67854,0.73599
-0.67854,0.73749
-0.68003,0.73749
-0.68003,0.73749
+0.31158,0.43652
+0.28771,0.43200
+0.28622,0.43200
+0.28473,0.43200
+0.28025,0.43351
+0.28025,0.43501
+0.27727,0.43953
+0.27727,0.44856
+0.27727,0.46511
+0.27727,0.47263
+0.27727,0.48919
+0.29666,0.54788
+0.30561,0.56744
+0.32202,0.60356
+0.39362,0.72094
+0.42495,0.75856
+0.45478,0.79919
+0.54727,0.91356
+0.58158,0.94065
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..ff3d6e01f31f9e881f271a98f3a6be33a1e8c8a4
Binary files /dev/null and b/python/ur_simple_control/.managers.py.swp differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index ccb51da838093a6f4a35c6013d4b2b91b781502e..096da1369580f1e89df4205b2cd3de66a679d0b5 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/.clik_point_to_point.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..50eb85a9a671af30316c6c890d7473e7b1ab7fcf
Binary files /dev/null and b/python/ur_simple_control/clik/.clik_point_to_point.py.swp differ
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc
index 35180bad01703e847b6908f23e4ac77c45afb34b..742b227418d085d2c798b868d28a01f1ebb18069 100644
Binary files a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc and b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc differ
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index a85c17654827d05f1528c7c78cfcedb1d9ad324c..adf18cf033ce705b1a220395e561f36a2b2c29a6 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -4,6 +4,7 @@ import copy
import argparse
from functools import partial
from ur_simple_control.managers import ControlLoopManager, RobotManager
+import time
@@ -141,32 +142,97 @@ def controlLoopClik(robot, clik_controller, i, past_data):
SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.goal_error:
- print("Convergence achieved, reached destionation!")
- breakFlag = True
+# print("Convergence achieved, reached destionation!")
+ breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
# compute the joint velocities.
# just plug and play different ones
qd = clik_controller(J, err_vector)
robot.sendQd(qd)
+ # TODO: only print under a debug flag, it's just clutter otherwise
# we do the printing here because controlLoopManager should be general.
# and these prints are click specific (although i'm not 100% happy with the arrangement)
- if not i % 1000:
- print("pos", robot.data.oMi[robot.JOINT_ID])
- print("linear error = ", pin.log6(SEerror).linear)
- print("angular error = ", pin.log6(SEerror).angular)
- print(" error = ", err_vector.transpose())
+# if not i % 1000:
+# print("pos", robot.data.oMi[robot.JOINT_ID])
+# print("linear error = ", pin.log6(SEerror).linear)
+# print("angular error = ", pin.log6(SEerror).angular)
+# print(" error = ", err_vector.transpose())
# we're not saving or loggin here, but need to respect the API,
# hence the empty dicts
return breakFlag, {}, {}
-if __name__ == "__main__":
- args = get_args()
- robot = RobotManager(args)
- Mgoal = robot.defineGoalPoint()
+"""
+moveUntilContactControlLoop
+---------------
+generic control loop for clik (handling error to final point etc).
+in some version of the universe this could be extended to a generic
+point-to-point motion control loop.
+"""
+def moveUntilContactControlLoop(speed, robot, clik_controller, i, past_data):
+ breakFlag = False
+ save_past_dict = {}
+ # know where you are, i.e. do forward kinematics
+ q = robot.getQ()
+ pin.forwardKinematics(robot.model, robot.data, q)
+ # break if wrench is nonzero basically
+ wrench = robot.getWrench()
+# print(np.linalg.norm(wrench))
+ # TODO: remove magick number
+ if np.linalg.norm(wrench) > 4:
+ breakFlag = True
+ # pin.computeJointJacobian is much different than the C++ version lel
+ J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ # compute the joint velocities.
+ qd = clik_controller(J, speed)
+ robot.sendQd(qd)
+ return breakFlag, {}, {}
+
+"""
+moveUntilContact
+-----
+does clik until it feels something with the f/t sensor
+"""
+def moveUntilContact(args, robot, speed):
+ assert type(speed) == np.ndarray
+ clik_controller = getClikController(args)
+ # TODO: just make the manager pass the robot or something, this is weird man
+ controlLoop = partial(moveUntilContactControlLoop, speed, robot, clik_controller)
+ # we're not using any past data or logging, hence the empty arguments
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
+ loop_manager.run()
+ # TODO: remove, this isn't doing anything
+ time.sleep(0.01)
+ print("Colision detected!!")
+
+"""
+moveL
+-----
+does moveL.
+send a SE3 object as goal point.
+if you don't care about rotation, make it np.zeros((3,3))
+"""
+def moveL(args, robot, goal_point):
+ assert type(goal_point) == pin.pinocchio_pywrap.SE3
+ robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
loop_manager.run()
+ # TODO: remove, this isn't doing anything
+ time.sleep(0.01)
+ print("MoveL done: convergence achieved, reached destionation!")
+
+
+# TODO: remove once you know shit works (you might be importing incorectly)
+#if __name__ == "__main__":
+# args = get_args()
+# robot = RobotManager(args)
+# Mgoal = robot.defineGoalPoint()
+# clik_controller = getClikController(args)
+# controlLoop = partial(controlLoopClik, robot, clik_controller)
+# # we're not using any past data or logging, hence the empty arguments
+# loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
+# loop_manager.run()
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 0e39dc8dcc907ad874cdcb891d00ae6b7d50dab0..af7c8db4ba3aab9fa1ab51fcdfa5d24530ed913a 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,5 +1,9 @@
# TODO clear out unnecessary imports
# TODO rename all private variables to start with '_'
+# TODO: just read the q and update everything every timestep, don't deepcopy,
+# --> just create a RobotManager.step() function, update everything there
+# don't do forwardKinematics 2 extra times for no good reason. make that the libraries
+# responsibility, not the users
import pinocchio as pin
import numpy as np
import time
@@ -13,7 +17,6 @@ import copy
import signal
# TODO make the package work
from ur_simple_control.util.get_model import get_model
-from ur_simple_control.util.robotiq_gripper import RobotiqGripper
from collections import deque
"""
@@ -63,6 +66,7 @@ class ControlLoopManager:
def __init__(self, robot_manager, controlLoop, args, save_past_dict, log_dict):
self.max_iterations = args.max_iterations
# i need rtde_control do to rtde_control
+ # TODO this thing might be re-initializing the robot
self.robot_manager = robot_manager
self.controlLoop = controlLoop
self.args = args
@@ -108,28 +112,6 @@ class ControlLoopManager:
# but i want to do it here. this is a big ick
self.log_dict = log_dict
- # if you just stop it the program with Ctrl-C, it will continue running
- # the last speedj lmao.
- # there's also an actual stop command, but sending 0s works so i'm not changing it
- signal.signal(signal.SIGINT, self.stopHandler)
-
-
- """
- stopHandler
- -----------
- # can't have self as first argument, because the
- # signal handler has to have these 2 arguments (even though they're not used, but
- # let's be correct if we can)
- # so it's static, problem solved
- """
-# @staticmethod
- def stopHandler(self, signum, frame):
- print('sending 100 speedjs full of zeros and exiting')
- for i in range(100):
- vel_cmd = np.zeros(6)
- self.robot_manager.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
- #exit()
-
"""
run
---
@@ -167,12 +149,14 @@ class ControlLoopManager:
continue
else:
time.sleep(self.dt - diff)
- if i < self.max_iterations -1:
- print("success in", i, "iterations!")
- else:
- print("FAIL: did not succed in", max_iterations, "iterations")
- if not self.args.pinocchio_only:
- self.stopHandler(None, None)
+# TODO: provide a debug flag for this
+# if i < self.max_iterations -1:
+# print("success in", i, "iterations!")
+# else:
+# print("FAIL: did not succed in", max_iterations, "iterations")
+# TODO: reintroduce maybe
+# if not self.args.pinocchio_only:
+# self.robot_manager.stopHandler(None, None)
"""
robotmanager:
@@ -199,6 +183,11 @@ class RobotManager:
# like freedrive or forcemode or whatever.
# you shouldn't care about previous states
def __init__(self, args):
+ # if you just stop it the program with Ctrl-C, it will continue running
+ # the last speedj lmao.
+ # there's also an actual stop command, but sending 0s works so i'm not changing it
+ signal.signal(signal.SIGINT, self.stopHandler)
+
self.args = args
self.pinocchio_only = args.pinocchio_only
self.simulation = args.simulation
@@ -235,6 +224,8 @@ class RobotManager:
if self.pinocchio_only:
self.q = pin.neutral(self.model)
elif not args.simulation:
+ # NOTE: you can't connect twice, dw 'bout that
+ print("CONNECTING TO UR5e!")
self.rtde_control = RTDEControlInterface("192.168.1.102")
self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
self.rtde_io = RTDEIOInterface("192.168.1.102")
@@ -279,6 +270,22 @@ class RobotManager:
# TODO this is clik specific, put it in a better place
self.goal_error = args.goal_error
+ """
+ stopHandler
+ -----------
+ TODO: make ifs for simulation too
+ can have self as first argument apparently.
+ upon receiving SIGINT it sends zeros for speed commands to
+ stop the robot
+ """
+ def stopHandler(self, signum, frame):
+ print('sending 100 speedjs full of zeros and exiting')
+ for i in range(100):
+ vel_cmd = np.zeros(6)
+ self.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
+ exit()
+
+
"""
setSpeedSlider
---------------
@@ -324,6 +331,41 @@ class RobotManager:
return self.q
return q
+ """
+ getMtool
+ -----
+ urdf treats gripper as two prismatic joints,
+ but they do not affect the overall movement
+ of the robot, so we add or remove 2 items to the joint list.
+ also, the gripper is controlled separately so we'd need to do this somehow anyway
+ NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
+ """
+ def getMtool(self):
+ if not self.pinocchio_only:
+ q = self.rtde_receive.getActualQ()
+ if self.args.gripper:
+ # TODO: make it work or remove it
+ #self.gripper_past_pos = self.gripper_pos
+ # this is pointless by itself
+ self.gripper_pos = self.gripper.get_current_position()
+ # the /255 is to get it dimensionless.
+ # the gap is 5cm,
+ # thus half the gap is 0.025m (and we only do si units here).
+ q.append((self.gripper_pos / 255) * 0.025)
+ q.append((self.gripper_pos / 255) * 0.025)
+ else:
+ # just fill it with zeros otherwise
+ q.append(0.0)
+ q.append(0.0)
+ else:
+ q = self.q
+ q = np.array(q)
+ self.q = q
+ pin.forwardKinematics(self.model, self.data, q)
+ # TODO probably remove deepcopy
+ self.Mtool = self.data.oMi[self.JOINT_ID]
+ return copy.deepcopy(self.Mtool)
+
"""
getQd
-----
diff --git a/python/ur_simple_control/util/.calib_board_hacks.py.swp b/python/ur_simple_control/util/.calib_board_hacks.py.swp
index 0c65b52814ff442ca7d522cc8b831f8b20b5c347..c991d6f05588066e2000883dccfddc9f00ddec33 100644
Binary files a/python/ur_simple_control/util/.calib_board_hacks.py.swp and b/python/ur_simple_control/util/.calib_board_hacks.py.swp differ
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc
index 18ef112aef1717a31bc5af78a2f9a5da0aa64059..9fbe6dde187a2c199737f2ed5ab83fdad5896ef4 100644
Binary files a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index c1db22c8d4dc7bbea96b28358e309068b79642b3..1d66a7abb6cc624ca99bb777841320578bf73529 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -7,6 +7,7 @@ import time
import copy
from ur_simple_control.managers import RobotManager
from ur_simple_control.util.freedrive import freedrive
+from ur_simple_control.clik.clik_point_to_point import moveL, moveUntilContact
# used to deal with freedrive's infinite while loop
import threading
@@ -35,26 +36,47 @@ you should do a weighting of these points in the least squares
def fitNormalVector(positions):
positions = np.array(positions)
# non-weighted least square
-# n = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
+ # none
+ n_non_weighted = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
+ n_non_weighted = n_non_weighted / np.linalg.norm(n_non_weighted)
+ print("n_non_weighted", n_non_weighted)
+ for p in positions:
+ print("cdot none", p @ n_non_weighted)
# bs weights, just to weight the new things more and linear is probably good
# (altough it could/should be even more agressive probably)
# +1 to start with 1 (0 both doesn't make sense and is makes W singular)
- W = np.diag(np.arange(1, len(positions) + 1))
# print("W", W)
# print("positions", positions)
# print("positions.T", positions.T)
# print("positions.T @ W @ positions", positions.T @ W @ positions)
# print("np.linalg.inv(positions.T @ W @ positions)", np.linalg.inv(positions.T @ W @ positions))
- n = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
+ # weak
+ try:
+ W = np.diag(np.linspace(1, 1.2, len(positions)))
+ n_slightly_weighted = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
+ n_slightly_weighted = n_slightly_weighted / np.linalg.norm(n_slightly_weighted)
+ print("n_slightly_weighted", n_slightly_weighted)
+ for p in positions:
+ print("cdot weak", p @ n_slightly_weighted)
+ except numpy.linalg.LinAlgError:
+ print("n_slightly_weighted is singular bruh")
+
+ # strong
+ W = np.diag(np.arange(1, len(positions) + 1))
+ n_strongly_weighted = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
#print("n", *n, sep=',')
# normalize
# TODO why am i not doing this again?
#n = n - 1
- n = n / np.linalg.norm(n)
- print("if the following give you roughly the same number, it worked")
- for p in positions:
- print("cdot", p @ n)
- return n
+ try:
+ n_strongly_weighted = n_strongly_weighted / np.linalg.norm(n_strongly_weighted)
+ print("n_strongly_weighed", n_strongly_weighted)
+ print("if the following give you roughly the same number, it worked")
+ for p in positions:
+ print("cdot strong", p @ n_strongly_weighted)
+ except numpy.linalg.LinAlgError:
+ print("n_strongly_weighted is singular bruh")
+ return n_non_weighted
"""
constructFrameFromNormalVector
@@ -207,7 +229,9 @@ def getSpeedInDirectionOfN(R):
# make speed small no matter what
speed = speed / np.linalg.norm(speed)
# nice 'n' slow
- speed = speed / 10
+ # TODO: remove magic number
+ speed = speed / 40
+ speed[2] = -1 * speed[2]
return speed
# TODO
@@ -218,21 +242,31 @@ def getSpeedInDirectionOfN(R):
# --> then you don't even need to use the stupid rpy,
# but instead rock only rotation matrices the way it should be.
# TODO: replace all moveUntilContacts with your own implementation
-def calibratePlane(robot, plane_width, plane_height, n_tests):
+# and of course replace the stupid speeds
+# TODO move width and height, rock just args
+def calibratePlane(args, robot, plane_width, plane_height, n_tests):
# i don't care which speed slider you have,
# because 0.4 is the only reasonable one here
- old_speed_slider = robot.speed_slider
- robot.setSpeedSlider(0.4)
+# old_speed_slider = robot.speed_slider
+# robot.setSpeedSlider(0.7)
handleUserToHandleTCPPose(robot)
q_init = copy.deepcopy(robot.getQ())
+ # GET TCP
+ Mtool = robot.getMtool()
- init_pose = copy.deepcopy(robot.rtde_receive.getActualTCPPose())
+ init_pose = copy.deepcopy(Mtool)
new_pose = copy.deepcopy(init_pose)
- q = robot.getQ()
- pin.forwardKinematics(robot.model, robot.data, q)
+ # you just did it dawg.
+ # i mean i know it's in the robot
+ # TODO: don't manage forward kinematics yourself,
+ # just create a RobotManager.step() function, update everything there
+ #q = robot.getQ()
+ #pin.forwardKinematics(robot.model, robot.data, q)
# this apsolutely has to be deepcopied aka copy-constructed
- R_initial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+ #R_initial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+ R_initial_estimate = copy.deepcopy(Mtool.rotation)
+ R = copy.deepcopy(R_initial_estimate)
# go in the TCP z direction
speed = getSpeedInDirectionOfN(R_initial_estimate)
@@ -240,35 +274,37 @@ def calibratePlane(robot, plane_width, plane_height, n_tests):
positions = []
for i in range(n_tests):
- robot.rtde_control.moveUntilContact(speed)
+ time.sleep(0.01)
+ print("iteration number:", i)
+ #robot.rtde_control.moveUntilContact(speed)
+ moveUntilContact(args, robot, speed)
+ # TODO: replace forwardkinematics call with robot.step()
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, np.array(q))
- print("pin:", *robot.data.oMi[6].translation.round(4), \
- *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4))
+ print("pin:", *robot.data.oMi[robot.JOINT_ID].translation.round(4), \
+ *pin.rpy.matrixToRpy(robot.data.oMi[robot.JOINT_ID].rotation).round(4))
print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
- positions.append(copy.deepcopy(robot.data.oMi[6].translation))
+ positions.append(copy.deepcopy(robot.data.oMi[robot.JOINT_ID].translation))
if i < n_tests -1:
- current_pose = robot.rtde_receive.getActualTCPPose()
+ current_pose = robot.getMtool()
new_pose = copy.deepcopy(current_pose)
# go back up (assuming top-left is highest incline)
# TODO: make this assumption an argument, or print it at least
- new_pose[2] = init_pose[2]
+ new_pose.translation[2] = init_pose.translation[2]
# TODO make these moveLs slower, they're way too fast
- robot.rtde_control.moveL(new_pose)
+ #robot.rtde_control.moveL(new_pose)
+ moveL(args, robot, new_pose)
+ q = robot.getQ()
+ pin.forwardKinematics(robot.model, robot.data, np.array(q))
# TODO: make this not base-orientation dependent,
# this is also an ugly ugly hack
- new_pose[0] = init_pose[0] + np.random.random() * plane_width
- new_pose[1] = init_pose[1] - np.random.random() * plane_height
- robot.rtde_control.moveL(new_pose)
+ new_pose.translation[0] = init_pose.translation[0] + np.random.random() * plane_width
+ new_pose.translation[1] = init_pose.translation[1] - np.random.random() * plane_height
+ moveL(args, robot, new_pose)
# fix orientation
- rpy = pin.rpy.matrixToRpy(R_initial_estimate)
- rpy = fixPinsRPY(rpy)
- # TODO: ensure these rpy's make sense (no weird trig messing it up)
- new_pose[3] = rpy[0]
- new_pose[4] = rpy[1]
- new_pose[5] = rpy[2]
- robot.rtde_control.moveL(new_pose)
+ new_pose.rotation = R
+ moveL(args, robot, new_pose)
# skip the first one
if i > 0:
n = fitNormalVector(positions)
@@ -277,27 +313,26 @@ def calibratePlane(robot, plane_width, plane_height, n_tests):
print("finished estimating R")
- current_pose = robot.rtde_receive.getActualTCPPose()
+ current_pose = robot.getMtool()
new_pose = copy.deepcopy(current_pose)
# go back up
- new_pose[2] = init_pose[2]
- robot.rtde_control.moveL(new_pose)
+ new_pose.translation[2] = init_pose.translation[2]
+ moveL(args, robot, new_pose)
# go back to the same spot
- new_pose[0] = init_pose[0]
- new_pose[1] = init_pose[1]
- new_pose[2] = init_pose[2]
+ new_pose.translation[0] = init_pose.translation[0]
+ new_pose.translation[1] = init_pose.translation[1]
+ new_pose.translation[2] = init_pose.translation[2]
# but in new orientation
- rpy = pin.rpy.matrixToRpy(R)
- rpy = fixPinsRPY(rpy)
- new_pose[3] = rpy[0]
- new_pose[4] = rpy[1]
- new_pose[5] = rpy[2]
- robot.rtde_control.moveL(new_pose)
+ new_pose.rotation = R
+ moveL(args, robot, new_pose)
+
+ # TODO there's certainly no need for all of these moves bro
# --> now you're ready to measure the translation vector correctly
# for this we want to go directly into the board
print("i'll estimate the translation vector now")
speed = getSpeedInDirectionOfN(R)
- robot.rtde_control.moveUntilContact(speed)
+
+ moveUntilContact(args, robot, speed)
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, np.array(q))
@@ -307,27 +342,28 @@ def calibratePlane(robot, plane_width, plane_height, n_tests):
# TODO: get rid of all movels, just your own stuff,
# or at least shove them avait to the RobotManager
# and now go back up
- current_pose = robot.rtde_receive.getActualTCPPose()
+ current_pose = robot.getMtool()
new_pose = copy.deepcopy(current_pose)
- new_pose[2] = init_pose[2]
- robot.rtde_control.moveL(new_pose)
+ new_pose.translation[2] = init_pose.translation[2]
+ moveL(args, robot, new_pose)
q = robot.getQ()
init_q = copy.deepcopy(q)
print("went back up, saved this q as initial q")
# put the speed slider back to its previous value
- robot.setSpeedSlider(old_speed_slider)
+# robot.setSpeedSlider(old_speed_slider)
print('also, the translation vector is:', translation)
return R, translation, q_init
-if __name__ == "__main__":
- robot = RobotManager()
- # TODO make this an argument
- n_tests = 10
- # TODO:
- # - tell the user what to do with prints, namely where to put the end-effector
- # to both not break things and also actually succeed
- # - start freedrive
- # - use some keyboard input [Y/n] as a blocking call,
- # release the freedrive and then start doing the calibration process
- calibratePlane(robot, n_tests)
+# TODO: remove once you know shit works (you might be importing incorectly)
+#if __name__ == "__main__":
+# robot = RobotManager()
+# # TODO make this an argument
+# n_tests = 10
+# # TODO:
+# # - tell the user what to do with prints, namely where to put the end-effector
+# # to both not break things and also actually succeed
+# # - start freedrive
+# # - use some keyboard input [Y/n] as a blocking call,
+# # release the freedrive and then start doing the calibration process
+# calibratePlane(robot, n_tests)