diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
deleted file mode 100644
index 6cb848bc0e1c7948199fc53aa4c7507705b8526d..0000000000000000000000000000000000000000
Binary files a/python/examples/.drawing_from_input_drawing.py.swp and /dev/null differ
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 31e7cb85677938ec2b636dc93331b7153c95c818..a25e7131a5c190773851ae3c427bd3ca16872e65 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -133,7 +133,10 @@ def getArgs():
parser.add_argument('--board-height', type=float, \
help="height of the board (in meters) the robot will write on", \
default=0.4)
-
+ parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
+ help="whether you want to do calibration", default=True)
+ parser.add_argument('--n-calibration-tests', type=int, \
+ help="number of calibration tests you want to run", default=10)
args = parser.parse_args()
if args.gripper and args.simulation:
@@ -240,8 +243,15 @@ if __name__ == "__main__":
# TODO: fix z axis in 2D to 3D path
# TODO: make this an argument once the rest is OK
path = path + np.array([0.0, 0.0, -0.0938])
+ # do calibration if specified
+ if args.calibration:
+ R, translation = calibratePlane(robot, args.n_calibrations_tests)
+ else:
+ # TODO: save this somewhere obviously
+ raise NotImplementedError("you gotta give me that R somehow, 'cos there is no default atm")
# create a joint space trajectory based on the path
- joint_trajectory = TODO
+ transf_body_to_board = pin.SE3(R, p)
+ joint_trajectory = clikCartesianPathIntoJointPath(path, robot, transf_body_to_board)
# create DMP based on the trajectory
dmp = DMP(joint_trajectory)
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
deleted file mode 100644
index f697a6cfa3b4672a101af84fd21aa323b24910de..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/.managers.py.swp and /dev/null 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
deleted file mode 100644
index 44e6abddd15938267cc7d739954e6a98cbf46ee0..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/clik/.clik_point_to_point.py.swp and /dev/null differ
diff --git a/python/ur_simple_control/clik/.clik_trajectory_following.py.swp b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp
deleted file mode 100644
index 2712234ef37d6e72b70c659ecaa1c192a007dda8..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/clik/.clik_trajectory_following.py.swp and /dev/null differ
diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
index dd2e244a7ade629dadb0cc54ebae1b34f8ee8cc5..f9469c1bb6524c33658bf814304f6977504aafbe 100644
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ b/python/ur_simple_control/clik/clik_trajectory_following.py
@@ -14,11 +14,20 @@ import sys
#######################################################################
# map the pixels to a 3D plane #
#######################################################################
-# x-y start in top-left corner (natual for western latin script writing)
-# and then it's natural to have the z axis pointing out of the board
-# TODO but that's not a right-handed frame lmao, change that where it should be
-# NOTE: this might as well be in the util file, but whatever for now, it will be done
-# once it will actually be needed elsewhere
+"""
+map2DPathTo3DPlane
+--------------------
+assumptions:
+- origin in top-left corner (natual for western latin script writing)
+- x goes right (from TCP)
+- z will go away from the board
+- y just completes the right-hand frame
+TODO: RIGHT NOW we don't have a right-handed frame lmao, change that where it should be
+NOTE: this function as well be in the util or drawing file, but whatever for now, it will be done
+ once it will actually be needed elsewhere
+Returns a 3D path appropriately scaled, and placed into the first quadrant
+of the x-y plane of the body-frame (TODO: what is the body frame if we're general?)
+"""
def map2DPathTo3DPlane(path_2D, width, height):
z = np.zeros((len(path),1))
path_3D = np.hstack((path,z))
@@ -32,118 +41,62 @@ def map2DPathTo3DPlane(path_2D, width, height):
return path
-# now the path is appropriately scaled and in the first quadrant
-# of the x-y plane of the body-frame
# we now need a transformation from the body frame to the board
# such that the first quadrant of the x-y plane is
# in the bottom left corner of the board (obvious solution)
-
-# sample stuff
-#translation_body_to_board = np.array([0.2, 0.2, 0.3])
-## here let's assume it's just rotated around x (it's not ofc)
-#rot_mat_body_to_board = np.array([[1.0, 0.0, 0.0],
-# [0.0, 0.0, 1.0],
-# [0.0, 1.0, 0.0]])
-#transf_body_to_board = pin.SE3(rot_mat_body_to_board, translation_body_to_board)
-
-# real stuff
-# TODO measure AND AUTOMATICALLY FIND/CALIBRATE
-# TODO write some algorithm to do this automatically, this is asinine
-# (in pin coordinates, real ones are [-x, -y, z])
-# UPPER LEFT POINT
-# 0.1065, 0.7083, 0.6362, -2.6137, -0.0248, -0.0033
-#rpy = np.array([-2.6137, -0.0248, -0.0033])
-#R = pin.rpy.rpyToMatrix(rpy)
-#print(R)
-#R = np.array([[ 1. , 0. , 0.14570094],
-# [ 0. , -0.70647877, 0.69257417],
-# [ 0. , -0.69257417, -0.70647877]])
-#R = np.array([[ 1. , 0. , 0.1496001 ],
-# [ 0. , -0.71010996, 0.68801429],
-# [ 0. , -0.68801429, -0.71010996]])
-#R = np.array([[1., 0., 0.03236534],
-#[ 0., -0.82404727, 0.56559577],
-#[ 0. , -0.56559577, -0.82404727]])
-#R = np.array([[1., 0. , 0.02588808],
-#[ 0., -0.82340404 , 0.56686471],
-#[ 0. , -0.56686471 ,-0.82340404]])
-
-R = np.array([[1. , 0. , 0.02562061],
-[ 0. , -0.82215985 , 0.56867984],
-[ 0. , -0.56867984 ,-0.82215985]])
-
-
-
-print(R)
-p = np.array([0.1065, 0.7083, 0.6362])
-transf_body_to_board = pin.SE3(R, p)
-
-# offset in z
-# very close
-#path = path + np.array([0.0, 0.0, -0.0238])
-path = path + np.array([0.0, 0.0, -0.0938])
-#path = path + np.array([0.0, 0.0, -0.1248])
-
-#path = path + np.array([0.0, 0.2, -0.1438])
-for i in range(len(path)):
- path[i] = transf_body_to_board.act(path[i])
-print(path)
-#######################################################################
-# STEP 2: clik that path #
-#######################################################################
-
-urdf_path_relative = "../../../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
-urdf_path_absolute = os.path.abspath(urdf_path_relative)
-mesh_dir = "../../../robot_descriptions/"
-mesh_dir_absolute = os.path.abspath(mesh_dir)
-
-model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
-# clik it
-JOINT_ID = 6
-eps = 10**-2
-dt = 0.01
-# skip inital pos tho
-#q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
-#q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
-q = np.array([1.584, -1.859, -0.953, -1.309, 1.578, -0.006, 0.0, 0.0])
-INIT_ITER = 10000
-n_iter = INIT_ITER
-RUNNING_ITER = 1000
-qs = []
-for goal in path:
- # there's no orientation error (you do need it lmao)
- #Mgoal = pin.SE3(np.zeros((3,3)), goal)
- Mgoal = pin.SE3(R, goal)
- for i in range(n_iter):
- pin.forwardKinematics(model, data, q)
- SEerror = data.oMi[JOINT_ID].actInv(Mgoal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < eps:
- if not n_iter == INIT_ITER:
- print("converged in", i)
- break
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
- v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
- #v = J.T @ err_vector
- q = pin.integrate(model, q, v * dt)
- if (not n_iter == INIT_ITER) and (i % 10 == 0):
- qs.append(q[:6])
-
- # just skipping the first run with one stone
- if n_iter == INIT_ITER:
- n_iter = RUNNING_ITER
- else:
- if i == RUNNING_ITER-1:
- print("DID NOT CONVERGE")
-
-#######################################################################
-# STEP 3: save joint path #
-#######################################################################
-qs = np.array(qs)
-# let's assume it takes 10 seconds
-t = np.linspace(0, 10, len(qs)).reshape((len(qs),1))
-timed_qs = np.hstack((t, qs))
-np.savetxt("../new_traj.csv", timed_qs, delimiter=',', fmt='%.5f')
+def clikCartesianPathIntoJointPath(path, robot, transf_body_to_plane):
+ transf_body_to_plane = pin.SE3(R, p)
+
+ for i in range(len(path)):
+ path[i] = transf_body_to_plane.act(path[i])
+ print(path)
+
+ # TODO: finish this
+ # - pass clik alg as argument
+ # - remove magic numbers
+ # - give output in the right format
+ # skip inital pos tho
+ #q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
+ #q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
+ q = np.array([1.584, -1.859, -0.953, -1.309, 1.578, -0.006, 0.0, 0.0])
+ INIT_ITER = 10000
+ n_iter = INIT_ITER
+ RUNNING_ITER = 1000
+ qs = []
+ for goal in path:
+ # there's no orientation error (you do need it lmao)
+ #Mgoal = pin.SE3(np.zeros((3,3)), goal)
+ Mgoal = pin.SE3(R, goal)
+ for i in range(n_iter):
+ pin.forwardKinematics(model, data, q)
+ SEerror = data.oMi[JOINT_ID].actInv(Mgoal)
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < eps:
+ if not n_iter == INIT_ITER:
+ print("converged in", i)
+ break
+ J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+ v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
+ #v = J.T @ err_vector
+ q = pin.integrate(model, q, v * dt)
+ if (not n_iter == INIT_ITER) and (i % 10 == 0):
+ qs.append(q[:6])
+
+ # just skipping the first run with one stone
+ if n_iter == INIT_ITER:
+ n_iter = RUNNING_ITER
+ else:
+ if i == RUNNING_ITER-1:
+ print("DID NOT CONVERGE")
+
+ #######################################################################
+ # STEP 3: save joint path #
+ #######################################################################
+ qs = np.array(qs)
+ # let's assume it takes 10 seconds
+ t = np.linspace(0, 10, len(qs)).reshape((len(qs),1))
+ timed_qs = np.hstack((t, qs))
+ np.savetxt("../new_traj.csv", timed_qs, delimiter=',', fmt='%.5f')
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index e55168d9b946f72bfdb9b9aa02b409876cc90456..c7f17cfac856e0881e5fa5cbf8bbcf7073934819 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -193,6 +193,9 @@ class RobotManager:
# just pass all of the arguments here and store them as is
# so as to minimize the amount of lines.
# might be changed later if that seems more appropriate
+ # TODO: put the end to all modes not compatible with control
+ # like freedrive or forcemode or whatever.
+ # you shouldn't care about previous states
def __init__(self, args):
self.args = args
self.pinocchio_only = args.pinocchio_only
diff --git a/python/ur_simple_control/util/.calib_board_hacks.py.swp b/python/ur_simple_control/util/.calib_board_hacks.py.swp
deleted file mode 100644
index 654ccdea21df40efc4deb0f2c418c7e9e9736fc5..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/util/.calib_board_hacks.py.swp and /dev/null differ
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index 84539ec3a31e7f47b859b4a996507c5b35274d89..0d8acd5a5daacbf59c470dfb5fa698a953f28525 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -1,7 +1,5 @@
-# TODO:
-# ideally make this a continuous update process
-# where you use the previous estimate to try to hit the board at the
-# right orientation, thereby making the estimate more precise
+# TODO: make prints prettier (remove \ in code, and also
+# make it look good in the terminal)
import pinocchio as pin
import numpy as np
@@ -9,15 +7,24 @@ import sys
import time
import copy
from ur_simple_control.managers import RobotManager
+from ur_simple_control.util.freedrive import freedrive
"""
+general
+-----------
Estimate a plane by making multiple contacts with it.
You need to start with a top left corner of it,
and you thus don't need to find an offset (you have to know it in advance).
TODO: test and make sure the above statement is in fact correct.
Thus the offset does not matter, we only need the angle,
i.e. the normal vector to the plane.
-Returns R because that's what I wan a
+Returns R because that's what's needed to construct the hom transf. mat.
+"""
+
+"""
+fitNormalVector
+----------------
+classic least squares fit
"""
def fitNormalVector(positions):
positions = np.array(positions)
@@ -31,12 +38,16 @@ def fitNormalVector(positions):
for p in positions:
print("cdot", p @ n)
-# constuct a frame around the found normal vector
-# we just assume the x axis is parallel with the robot's x axis
-# this is of course completly arbitrary, so
-# TODO fix the fact that you just assume the x axis
-# or write down why you don't need it (i'm honestly not sure atm, but it is late)
-def constructFrameFromNormalVector(R_intial_estimate, n):
+"""
+constructFrameFromNormalVector
+----------------------------------
+constuct a frame around the found normal vector
+we just assume the x axis is parallel with the robot's x axis
+this is of course completly arbitrary, so
+TODO fix the fact that you just assume the x axis
+or write down why you don't need it (i'm honestly not sure atm, but it is late)
+"""
+def constructFrameFromNormalVector(R_initial_estimate, n):
z_new = n
x_new = np.array([1.0, 0.0, 0.0])
y_new = np.cross(x_new, z_new)
@@ -51,35 +62,71 @@ def constructFrameFromNormalVector(R_intial_estimate, n):
#z_new[0] = np.abs(z_new[0])
#z_new[1] = np.abs(z_new[1])
#z_new[2] = np.abs(z_new[2]) * -1
- y_new[0] = np.abs(y_new[0])
- y_new[1] = np.abs(y_new[1])
- y_new[2] = np.abs(y_new[2])
- z_new[0] = np.abs(z_new[0])
- z_new[1] = np.abs(z_new[1])
- z_new[2] = np.abs(z_new[2])
# y is good 'cos it was obtained with a cross
R = np.hstack((x_new.reshape((3,1)), y_new.reshape((3,1))))
R = np.hstack((R, z_new.reshape((3,1))))
# now ensure all the signs are the signs that you want,
# which we get from the initial estimate (which can not be that off)
- for i in range(R.shape[0]):
- for j in range(R.shape[1]):
- # TODO ensure all signs are the same
- continue
-
+ R = np.abs(R) * np.sign(R_initial_estimate)
print('rot mat to new frame:')
print(*R, sep=',\n')
return R
+"""
+handleUserToHandleTCPPose
+-----------------------------
+1. tell the user what to do with prints, namely where to put the end-effector
+ to both not break things and also actually succeed
+2. start freedrive
+3. use some keyboard input [Y/n] as a blocking call,
+4. release the freedrive and then start doing the calibration process
+"""
+def handleUserToHandleTCPPose(robot):
+ print("Whatever code you ran wants you to calibrate the plane on which you will be doing\
+ your things. Put the end-effector at the top left corner of the plane \
+ where you'll be doing said things. \n
+ Make sure the orientation is reasonably correct as that will be \
+ used as the initial estimate of the orientation, \
+ which is what you will get as an output from this calibration procedure.\
+ The end-effector will go down (it's TCP z pozitive direction) and touch the thing\
+ the number of times you specified (if you are not aware of this, check the\
+ arguments of the program you ran.\n \
+ The robot will now enter freedrive mode so that you can manually put\
+ the end-effector where it's supposed to be.\n \
+ When you did it, press 'Y', or press 'n' to exit.")
+ while True:
+ answer = input("Ready to calibrate or exit? [Y/n]")
+ if answer == 'n' or answer == 'N':
+ print("The whole program will exit. Change the argument to --no-calibrate or \
+ change code that lead you here.")
+ exit()
+ elif answer == 'y' or answer == 'Y':
+ print("The robot will now enter freedrive mode. Put the end-effector to the \
+ top left corner of your plane and mind the orientation.")
+ break
+ else:
+ print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
+ continue
+ freedrive()
+ while True:
+ answer = input("You got the end-effector in the correct pose and are ready \
+ to start calibrating or do you want to exit? [Y/n]")
+ if answer == 'n' or answer == 'N':
+ print("The whole program will exit. Goodbye!")
+ exit()
+ elif answer == 'y' or answer == 'Y':
+ print("Calibration about to start. Have your hand on the big red stop button!")
+ break
+ else:
+ print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
+ continue
+ robot.rtde_control.endFreedriveMode()
+
def calibratePlane(robot, n_tests):
- # 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
+ handleUserToHandleTCPPose(robot)
+
init_pose = robot.rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(init_pose)
@@ -99,47 +146,51 @@ def calibratePlane(robot, n_tests):
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, q)
# this apsolutely has to be deepcopied aka copy-constructed
- R_intial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+ R_initial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+ translation = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].translation)
positions = []
for i in range(n_tests):
- rtde_control.moveUntilContact(speed)
- q = rtde_receive.getActualQ()
+ robot.rtde_control.moveUntilContact(speed)
+ q = robot.rtde_receive.getActualQ()
q.append(0.0)
q.append(0.0)
- pin.forwardKinematics(model, data, np.array(q))
- print("pin:", *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
- print("ur5:", *np.array(rtde_receive.getActualTCPPose()).round(4))
- positions.append(copy.deepcopy(data.oMi[6].translation))
+ 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("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ positions.append(copy.deepcopy(robot.data.oMi[6].translation))
if i < n_tests -1:
- current_pose = rtde_receive.getActualTCPPose()
+ current_pose = robot.rtde_receive.getActualTCPPose()
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]
- rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(new_pose)
new_pose[0] = init_pose[0] + np.random.random() * 0.3
new_pose[1] = init_pose[1] - np.random.random() * 0.2
- rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(new_pose)
# fix orientation
- rpy = pin.rpy.matrixToRpy(R)
+ rpy = pin.rpy.matrixToRpy(R_initial_estimate)
print("rpy", rpy)
if rpy[0] > 0.0:
rpy[0] = rpy[0] - 2*np.pi
- # who knows if this is ok
+ # 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]
- rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(new_pose)
n = fitNormalVector(positions)
- R = constructFrameFromNormalVector(n)
-
+ R = constructFrameFromNormalVector(R_initial_estimate, n)
current_pose = rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(current_pose)
new_pose[2] = init_pose[2]
- rtde_control.moveL(new_pose)
- rtde_control.moveL(init_pose)
+ robot.rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(init_pose)
+
+ print('also, the translation vector is:', translation)
+ return R, translation
if __name__ == "__main__":
robot = RobotManager()
diff --git a/python/ur_simple_control/util/freedrive.py b/python/ur_simple_control/util/freedrive.py
new file mode 100644
index 0000000000000000000000000000000000000000..7c89d701c31e87193b01127d667a2529638170b5
--- /dev/null
+++ b/python/ur_simple_control/util/freedrive.py
@@ -0,0 +1,24 @@
+import pinocchio as pin
+import numpy as np
+import time
+from ur_simple_control.managers import RobotManager
+
+def freedrive(robot):
+ robot.rtde_control.freedriveMode()
+
+ while True:
+ q = robot.rtde_receive.getActualQ()
+ q.append(0.0)
+ q.append(0.0)
+ pin.forwardKinematics(robot.model, robot.data, np.array(q))
+ print(robot.data.oMi[6])
+ print("pin:", *robot.data.oMi[6].translation.round(4), \
+ *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4))
+ print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ time.sleep(0.005)
+
+if __name__ == "__main__":
+ robot = RobotManager()
+ freedrive()
+ # TODO possibly you want to end freedrive here as well.
+ # or end everything imaginable in the signal handler