diff --git a/TODOS b/TODOS
index 973694db546a4deb03c53fa2f4fa7eb2e1dcc4b6..2b541ce555222c6d85e2c8f383c171388d8918fc 100644
--- a/TODOS
+++ b/TODOS
@@ -150,9 +150,17 @@ goal 4: more controllers
(fix the problem of writing on a non-flat surface/whatever) --> publishable side-project
--> give it 2-3 days before robotweek
+goal 5: more convenience - set gripper payload etc automatically
+------------------------
+ # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+ # you already found an API in rtde_control for this, just put it in initialization
+ # under using/not-using gripper parameters
+ # problem: bindings weren't exported to python xd
+ # ALSO NOTE: to use this you need to change the version inclusions in
+ # ur_rtde due to a bug there in the current ur_rtde + robot firmware version
+ # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
-
-goal 5: panda/yumi
+goal 6: panda/yumi
----------------
finally, do what you promised and put this on another robot,
thereby rendering this something publishable in joss
diff --git a/python/convenience_tool_box/freedrive_v2.0.py b/python/convenience_tool_box/freedrive_v2.0.py
new file mode 100644
index 0000000000000000000000000000000000000000..fd41882d663579b5ec8b633a58b2b87cd3090bf9
--- /dev/null
+++ b/python/convenience_tool_box/freedrive_v2.0.py
@@ -0,0 +1,35 @@
+import pinocchio as pin
+import numpy as np
+import time
+import argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.basics.basics import freedriveUntilKeyboard
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'nothing'
+ # add more arguments here from different Simple Manipulator Control modules
+ args = parser.parse_args()
+ return args
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
+
+ pose_n_q_dict = freedriveUntilKeyboard(args, robot)
+ print(pose_n_q_dict)
+
+ # get expected behaviour here (library can't know what the end is - you have to do this here)
+ if not args.pinocchio_only:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot.log_manager.plotAllControlLoops()
+
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
+ #loop_manager.stopHandler(None, None)
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index fa8caa18c3b1e0fc08b4d435b656d67e4ad1113c..dc88f72e57f0d6b6cb08a550b17468eeef8b3904 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -5,6 +5,7 @@ import matplotlib
import copy
import argparse
import time
+import pickle
from functools import partial
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
@@ -12,7 +13,7 @@ from ur_simple_control.dmp.dmp import getDMPArgs, DMP, NoTC,TCVelAccConstrained
from ur_simple_control.clik.clik import getClikArgs, getClikController, moveL, moveUntilContact, controlLoopClik, compliantMoveL, clikCartesianPathIntoJointPath
from ur_simple_control.util.map2DPathTo3DPlane import map2DPathTo3DPlane
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.util.calib_board_hacks import getBoardCalibrationArgs, calibratePlane, getSpeedInDirectionOfN
+from ur_simple_control.util.calib_board_hacks import getBoardCalibrationArgs, calibratePlane
from ur_simple_control.basics.basics import moveJPI
#######################################################################
@@ -133,61 +134,28 @@ Idea is you pick up the marker, go to the top corner,
touch it, and see the difference between that and the translation vector.
Obviously it's just a hacked solution, but it works so who cares.
"""
-def findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init):
- # TODO make this more general
- # so TODO: calculate TCP speed based on the rotation matrix
- # and then go
+def findMarkerOffset(args, robot, plane_pose):
above_starting_write_point = pin.SE3.Identity()
- #above_starting_write_point.translation[0] = args.board_width / 2
- # this one might be with a minus sign
- #above_starting_write_point.translation[1] = -1* args.board_height / 2
- # start 20cm away from the board
above_starting_write_point.translation[2] = -0.2
- transf_bord_to_board = pin.SE3(rotation_matrix, translation_vector)
- above_starting_write_point = transf_bord_to_board.act(above_starting_write_point)
- print("above_starting_write_point", above_starting_write_point)
- print("current T_w_e", robot.getT_w_e())
- #exit()
- #moveJ(args, robot, q_init)
+ plane_pose = pin.SE3(rotation_matrix, translation_vector)
+ above_starting_write_point = plane_pose.act(above_starting_write_point)
+ print("going to above plane pose point", above_starting_write_point)
compliantMoveL(args, robot, above_starting_write_point)
- if args.board_axis == 'z':
- axis_of_rot = rotation_matrix[:,2]
- elif args.board_axis == 'y':
- axis_of_rot = rotation_matrix[:,1]
- else:
- print("you passed", board_axis, ", but it has to be 'z' or 'y'")
- exit()
- # it's going out of the board, and we want to go into the board, right????
- # TODO test this
- #z_of_rot = z_of_rot
- print("vector i'm following:", axis_of_rot)
- speed = getSpeedInDirectionOfN(rotation_matrix, args.board_axis)
- speed = np.zeros(6)
+
# this is in the end-effector frame, so this means going straight down
# because we are using the body jacobians in our clik
- # TODO: make this both more transparent AND provide the option to do clik
- # with a spatial jacobian
+ speed = np.zeros(6)
speed[2] = 0.02
- #speed[2] = speed[2] * -1
- #robot.rtde_control.moveUntilContact(speed)
moveUntilContact(args, robot, speed)
# we use the pin coordinate system because that's what's
# the correct thing long term accross different robots etc
current_translation = robot.getT_w_e().translation
# i only care about the z because i'm fixing the path atm
# but, let's account for the possible milimiter offset 'cos why not
- #print("translation_vector", translation_vector)
- #print("current_translation", current_translation)
- #print("translation_vector - current_translation", \
- # translation_vector - current_translation)
marker_offset = np.linalg.norm(translation_vector - current_translation)
print("going back")
- # TODO: this HAS to be a movej
- # in fact all of them should be
- #robot.Mgoal = Tinit.copy()
compliantMoveL(args, robot, above_starting_write_point)
-# robot.setSpeedSlider(old_speed_slider)
return marker_offset
#######################################################################
@@ -199,47 +167,25 @@ def findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init):
def controller():
pass
-# TODO:
-# regarding saving data you have 2 options:
-# 1) explicitely return what you want to save - you can't magically read local variables
-# 2) make controlLoop a class and then save handle the saving behind the scenes -
-# now you these variables are saved in a class so they're not local variables
-# option 1 is clearly more hands-on and thus worse
-# option 2 introduces a third big class and makes everything convoluted.
-# for now, we go for option 1) because it's simpler to implement and deal with.
-# but in the future, implementing 2) should be tried. you really have to try
-# to do it cleanly to see how good/bad it is.
-# in that case you'd most likely want to inherit ControlLoopManager and go from there.
-# you'd still need to specify what you're saving with self.that_variable so no matter
-# there's no running away from that in any case.
-# it's 1) for now, it's the only non-idealy-clean part of this solution, and it's ok really.
-# TODO but also look into something fancy like some decorator or something and try
-# to find option 3)
-
-# control loop to be passed to ControlLoopManager
-def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
+def controlLoopWriting(args, robot : RobotManager, dmp, tc, controller, i, past_data):
+ """
+ controlLoopWriting
+ -----------------------
+ dmp reference on joint path + compliance
+ """
breakFlag = False
- # TODO rename this into something less confusing
save_past_dict = {}
log_item = {}
- dmp.step(robot.dt) # dmp step
+ dmp.step(robot.dt)
# temporal coupling step
tau_dmp = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
dmp.set_tau(tau_dmp)
q = robot.getQ()
T_w_e = robot.getT_w_e()
-# if args.board_axis == 'z':
-# Z = np.diag(np.array([0.0, 0.0, 2.0, 1.0, 1.0, 1.0]))
-# if args.board_axis == 'y':
-# Z = np.diag(np.array([0.0, 1.0, 0.0, 1.0, 1.0, 1.0]))
Z = np.diag(np.array([0.0, 0.0, 2.0, 1.0, 1.0, 1.0]))
- #Z = np.diag(np.ones(6))
- #Z = np.diag(np.array([0.1, 0.1, 1.0, 0.1, 0.1, 0.1]))
- #Z = np.diag(np.array([1.0, 0.6, 1.0, 0.5, 0.5, 0.5]))
wrench = robot.getWrench()
- # deepcopy for good coding practise (and correctness here)
- save_past_dict['wrench'] = copy.deepcopy(wrench)
+ save_past_dict['wrench'] = wrench.copy()
# rolling average
#wrench = np.average(np.array(past_data['wrench']), axis=0)
@@ -247,12 +193,9 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
# beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- # it's just coordinate change from base to end-effector,
- # they're NOT the same rigid body,
- # the vector itself is not changing, only the coordinate representation
-
wrench = Z @ wrench
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ #J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
dq = robot.getQd()[:6].reshape((6,1))
# get joint
tau = J.T @ wrench
@@ -286,6 +229,61 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
return breakFlag, save_past_dict, log_item
+def write(args, robot : RobotManager, joint_trajectory):
+ # create DMP based on the trajectory
+ dmp = DMP(joint_trajectory)
+ if not args.temporal_coupling:
+ tc = NoTC()
+ else:
+ v_max_ndarray = np.ones(robot.n_arm_joints) * robot.max_qd
+ a_max_ndarray = np.ones(robot.n_arm_joints) * args.acceleration
+ tc = TCVelAccConstrained(args.gamma_nominal, args.gamma_a, v_max_ndarray, a_max_ndarray, args.eps_tc)
+
+ print('going to starting write position')
+ dmp.step(1/500)
+ first_q = dmp.pos.reshape((6,))
+ first_q = list(first_q)
+ first_q.append(0.0)
+ first_q.append(0.0)
+ first_q = np.array(first_q)
+ # move to initial pose
+ mtool = robot.getT_w_e(q_given=first_q)
+ #mtool.translation[1] = mtool.translation[1] - 0.0035
+ #mtool.translation[1] = mtool.translation[1] - 0.012
+ # NOTE: magic number for how close to the board to get
+ # it should be fine without it too,
+ # this might just be an atavism which fixed something broken before
+ mtool.translation[1] = mtool.translation[1] - 0.006
+ if not args.board_wiping:
+ compliantMoveL(args, robot, mtool)
+ else:
+ moveL(args, robot, mtool)
+
+ save_past_dict = {
+ 'wrench' : np.zeros(6),
+ }
+ # here you give it it's initial value
+ log_item = {
+ 'qs' : np.zeros(robot.n_arm_joints),
+ 'dmp_poss' : np.zeros(robot.n_arm_joints),
+ 'dqs' : np.zeros(robot.n_arm_joints),
+ 'dmp_vels' : np.zeros(robot.n_arm_joints),
+ 'wrench' : np.zeros(6),
+ 'tau' : np.zeros(robot.n_arm_joints),
+ }
+ #moveJ(args, robot, dmp.pos.reshape((6,)))
+ controlLoop = partial(controlLoopWriting, dmp, tc, controller, robot)
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ # and now we can actually run
+ loop_manager.run()
+
+ print("move a bit back")
+ T_w_e = robot.getT_w_e()
+ go_away_from_plane_transf = pin.SE3.Identity()
+ go_away_from_plane_transf.translation[2] = -0.1
+ goal = T_w_e.act(go_away_from_plane_transf)
+ compliantMoveL(args, robot, goal)
+
if __name__ == "__main__":
#######################################################################
@@ -296,16 +294,11 @@ if __name__ == "__main__":
clikController = getClikController(args)
robot = RobotManager(args)
if args.pinocchio_only:
- robot.q = np.array([ 1.32022870e+00, -1.40114772e+00, -1.27237797e+00, -1.15715911e+00,
- 1.76543856e+00, -2.38652054e-01, 2.94117647e-04, 2.94117647e-04]) + np.random.random(8) * 0.1
-
- # calibrate FT first
- # it's done by default now because it's basically always necessary
+ robot.q = np.array([ 1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0 ]) + np.random.random(8) * 0.1
#######################################################################
# drawing a path, making a joint trajectory for it #
#######################################################################
- # TODO make these ifs make more sense
# draw the path on the screen
if args.draw_new:
@@ -313,7 +306,6 @@ if __name__ == "__main__":
matplotlib.use('tkagg')
pixel_path = drawPath(args)
matplotlib.use('qtagg')
- # make it 3D
else:
pixel_path_file_path = './path_in_pixels.csv'
pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=',')
@@ -322,179 +314,68 @@ if __name__ == "__main__":
rotation_matrix, translation_vector, q_init = \
calibratePlane(args, robot, args.board_width, args.board_height, \
args.n_calibration_tests)
- print(q_init)
- print(rotation_matrix)
- print(translation_vector)
- exit()
+ print("finished calibration")
else:
- # TODO: save this somewhere obviously (side file)
- # also make it prettier if possible
print("using predefined values")
- #q_init = np.array([1.4545, -1.7905, -1.1806, -1.0959, 1.6858, -0.1259, 0.0, 0.0])
- #translation_vector = np.array([0.10125722 ,0.43077874 ,0.9110792 ])
- #rotation_matrix = np.array([[1. , 0. ,0.00336406],
- # [-0. , -0.00294646, 0.99999 ],
- # [ 0. , -0.99999 , -0.00294646]])
- #translation_vector = np.array([0.21997482, 0.41345861, 0.7314353])
- #rotation_matrix = np.array([
- # [ 1., 0., 0.01792578],
- # [ 0., -0.58973106, 0.80740073],
- # [ 0., -0.80740073, -0.58973106]])
- #q_init = np.array([ 1.33085752e+00, -1.44578363e+00, -1.21776414e+00, -1.17214762e+00, 1.75202715e+00, -1.94359605e-01, 2.94117647e-04, 2.94117647e-04])
- translation_vector = np.array([0.21754368, 0.42021616, 0.74162252])
- rotation_matrix = np.array([[ 1. , 0. ,0.0139409 ],
- [ 0., -0.61730976 , 0.78659666],
- [ 0., -0.78659666 ,-0.61730976]])
- #q_init = np.array([ 1.32022870e+00, -1.40114772e+00, -1.27237797e+00, -1.15715911e+00, \
- # 1.76543856e+00, -2.38652054e-01, 2.94117647e-04, 2.94117647e-04])
- q_init = np.array([1.175, -1.44, -1.283, -1.079, 2.035, -1.887, 0.0, 0.0])
- #q_init = robot.getQ()
- #T_w_e = robot.getT_w_e()
- #rotation_matrix = np.array([
- # [-1, 0, 0],
- # [0, 0, -1],
- # [0, -1, 0]])
- #translation_vector = T_w_e.translation.copy()
+ file = open('./plane_pose.pickle', 'rb')
+ plane_calib_dict = pickle.load(file)
+ file.close()
+ plane_pose = plane_calib_dict['plane_top_left_pose']
+ translation_vector = plane_calib_dict['plane_top_left_pose'].translation
+ rotation_matrix = plane_calib_dict['plane_top_left_pose'].rotation
+ q_init = plane_calib_dict['q_init']
# make the path 3D
path = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
- # TODO: fix and trust z axis in 2D to 3D path
- # TODO: add an offset of the marker (this is of course approximate)
- # TODO: make this an argument once the rest is OK
- # ---> just go to the board while you have the marker ready to find this
- # ---> do that right here
+ # TODO: fix thi function
if args.pick_up_marker:
+ raise NotImplementedError("sorry")
getMarker(args, robot, rotation_matrix, translation_vector)
+ # marker moves between runs, i change markers etc,
+ # this goes to start, goes down to touch the board - that's the marker
+ # length aka offset (also handles incorrect frames)
if args.find_marker_offset:
# find the marker offset
- # TODO find a better q init (just moveL away from the board)
- # THIS Q_INIT IS NOT SUITED FOR THIS PURPOSE!!!!!
- #marker_offset = findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init)
- marker_offset = findMarkerOffset(args, robot, rotation_matrix, translation_vector, q_init)
+ marker_offset = findMarkerOffset(args, robot, plane_pose)
print('marker_offset', marker_offset)
# we're going in a bit deeper
- #path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.015])
if not args.board_wiping:
- #path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.003])
- path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.005])
+ path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.003])
+ #path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.005])
else:
# old robotweek
path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.015])
path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.025])
else:
+ print("i hope you know the magic number of marker length + going into board")
#path = path + np.array([0.0, 0.0, -0.1503])
path = path + np.array([0.0, 0.0, - 0.092792+ 0.003])
- # and if you don't want to draw new nor calibrate, but you want the same path
- # with a different clik, i'm sorry, i can't put that if here.
- # atm running the same joint trajectory on the same thing makes for easier testing
- # of the final system.
+ # create a joint space trajectory based on the 3D path
if args.draw_new or args.calibration or args.find_marker_offset:
-
- #path = path + np.array([0.0, 0.0, -0.0938])
- # create a joint space trajectory based on the 3D path
# TODO: add flag of success (now it's your eyeballs and printing)
# and immediatelly exit if it didn't work
+ TODO: rewrite this function to comply with controlLoop standars
+ do that by copying the arguments, setting them to quickly integrate
+ and get extract the qs from the result
joint_trajectory = clikCartesianPathIntoJointPath(path, args, robot, \
- clikController, q_init, rotation_matrix, translation_vector)
+ clikController, q_init, plane_pose)
else:
joint_trajectory_file_path = './joint_trajectory.csv'
joint_trajectory = np.genfromtxt(joint_trajectory_file_path, delimiter=',')
- # create DMP based on the trajectory
- dmp = DMP(joint_trajectory)
- if not args.temporal_coupling:
- tc = NoTC()
- else:
- # TODO test whether this works (it should, but test it)
- # test the interplay between this and the speed slider
- # ---> SPEED SLIDER HAS TO BE AT 1.0
- v_max_ndarray = np.ones(robot.n_arm_joints) * robot.max_qd #* args.speed_slider
- # test the interplay between this and the speed slider
- # args.acceleration is the actual maximum you're using
- a_max_ndarray = np.ones(robot.n_arm_joints) * args.acceleration #* args.speed_slider
- tc = TCVelAccConstrained(args.gamma_nominal, args.gamma_a, v_max_ndarray, a_max_ndarray, args.eps_tc)
+ write(args, robot, joint_trajectory)
- # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
- # you already found an API in rtde_control for this, just put it in initialization
- # under using/not-using gripper parameters
- # ALSO NOTE: to use this you need to change the version inclusions in
- # ur_rtde due to a bug there in the current ur_rtde + robot firmware version
- # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
- # here you give what you're saving in the rolling past window
- # it's initial value.
- # controlLoopManager will populate the queue with these initial values
- save_past_dict = {
- 'wrench' : np.zeros(6),
- }
- # here you give it it's initial value
- log_item = {
- 'qs' : np.zeros(robot.n_arm_joints),
- 'dmp_poss' : np.zeros(robot.n_arm_joints),
- 'dqs' : np.zeros(robot.n_arm_joints),
- 'dmp_vels' : np.zeros(robot.n_arm_joints),
- 'wrench' : np.zeros(6),
- 'tau' : np.zeros(robot.n_arm_joints),
- }
- #######################################################################
- # physical setup #
- #######################################################################
- # TODO: add marker picking
- # get up from the board
- robot._getT_w_e()
- current_pose = robot.getT_w_e()
- # Z
- #current_pose.translation[2] = current_pose.translation[2] + 0.03
- # Y
- #current_pose.translation[1] = current_pose.translation[1] + 0.03
- #moveL(args, robot, current_pose)
- # move to initial pose
- dmp.step(1/500)
- first_q = dmp.pos.reshape((6,))
- first_q = list(first_q)
- first_q.append(0.0)
- first_q.append(0.0)
- first_q = np.array(first_q)
- #pin.forwardKinematics(robot.model, robot.data, first_q)
- mtool = robot.getT_w_e(q_given=first_q)
- #mtool.translation[1] = mtool.translation[1] - 0.0035
- #mtool.translation[1] = mtool.translation[1] - 0.012
- mtool.translation[1] = mtool.translation[1] - 0.006
- if args.debug_prints:
- print("im at:", robot.getT_w_e())
- print("going to:", mtool)
- print('going to starting write position')
- # TODO: write a compliantMoveL - be careful that everything is in the body frame
- # since your jacobian is the body jacobian!!!
- #moveL(args, robot, mtool)
- if not args.board_wiping:
- compliantMoveL(args, robot, mtool)
- else:
- moveL(args, robot, mtool)
+ if not args.pinocchio_only:
+ robot.stopRobot()
- #moveJ(args, robot, dmp.pos.reshape((6,)))
- controlLoop = partial(controlLoopWriting, dmp, tc, controller, robot)
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- # and now we can actually run
- loop_manager.run()
- #loop_manager.stopHandler(None, None)
- mtool = robot.getT_w_e()
- print("move a bit back")
- if args.board_axis == 'z' or args.board_axis == 'y':
- mtool.translation[1] = mtool.translation[1] - 0.1
-# if args.board_axis == 'z':
-# mtool.translation[2] = mtool.translation[2] - 0.1
- compliantMoveL(args, robot, mtool)
+ if args.save_log:
+ robot.log_manager.plotAllControlLoops()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
- loop_manager.stopHandler(None, None)
- #robot.stopHandler(None, None)
- #robot.stopHandler(None, None)
- # TODO: add some math to analyze path precision
-
-
-
-
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index cfeb519c5eb52749fc2063c08f8e0d6af7931a98..e9587b3173118e4a5a7de50cb9b4b3f3b1bcaf8f 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,63 +1,33 @@
-0.53498,0.68831
-0.53498,0.68698
-0.53498,0.68565
-0.53498,0.68432
-0.53350,0.67766
-0.52165,0.65634
-0.50091,0.63370
-0.47720,0.61505
-0.45202,0.58308
-0.40906,0.52581
-0.39721,0.51382
-0.35573,0.44056
-0.35128,0.43523
-0.32758,0.40193
-0.32314,0.39394
-0.32017,0.38994
-0.31869,0.38728
-0.31721,0.38595
-0.31721,0.38462
-0.31721,0.38328
-0.33351,0.38728
-0.37499,0.39660
-0.41647,0.39927
-0.46239,0.40193
-0.52017,0.40326
-0.55868,0.40326
-0.58831,0.40326
-0.61201,0.40193
-0.65201,0.40060
-0.66090,0.40060
-0.67572,0.39794
-0.69201,0.39794
-0.70534,0.39660
-0.71571,0.39527
-0.71868,0.39527
-0.71868,0.39527
-0.72016,0.39527
-0.72460,0.39927
-0.72757,0.41392
-0.71720,0.43656
-0.70090,0.45654
-0.68609,0.47386
-0.65942,0.49917
-0.62535,0.53114
-0.60609,0.55511
-0.59127,0.57642
-0.56461,0.61905
-0.55572,0.63104
-0.54387,0.64302
-0.54091,0.64569
-0.54091,0.64702
-0.53942,0.64835
-0.53794,0.64968
-0.53646,0.65102
-0.53054,0.66167
-0.52757,0.67499
-0.52461,0.68565
-0.52165,0.69364
-0.52017,0.69897
-0.51868,0.70163
-0.51868,0.70296
-0.51868,0.70430
-0.51868,0.70430
+0.24118,0.49034
+0.24118,0.49163
+0.25736,0.49809
+0.27960,0.50326
+0.31196,0.50585
+0.34028,0.50714
+0.35848,0.50714
+0.38882,0.50714
+0.42118,0.50714
+0.44140,0.50585
+0.45960,0.50585
+0.48589,0.50585
+0.51623,0.50714
+0.53443,0.50714
+0.54657,0.50714
+0.56477,0.50714
+0.57893,0.50585
+0.60117,0.50456
+0.61533,0.50456
+0.63151,0.50456
+0.64162,0.50456
+0.65578,0.50326
+0.66387,0.50326
+0.67196,0.50197
+0.67803,0.50197
+0.68005,0.50197
+0.68005,0.50197
+0.68207,0.50197
+0.68409,0.50197
+0.68612,0.50197
+0.69016,0.50197
+0.69218,0.50197
+0.69218,0.50197
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index b4ad43001e06ef47c426e5cdff3e4b32059ac2e3..953b41a1335fcf9032911fdc8dbcb7fb162834f0 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -43,7 +43,7 @@ def controller():
pass
# control loop to be passed to ControlLoopManager
-def controlLoopPointImpedance(q_init, controller, robot : RobotManager, i, past_data):
+def controlLoopPointImpedance(args, q_init, controller, robot : RobotManager, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -59,23 +59,14 @@ def controlLoopPointImpedance(q_init, controller, robot : RobotManager, i, past_
# first-order low pass filtering instead
# beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
+ Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
+ #Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
#Z = np.diag(np.ones(6))
wrench = Z @ wrench
- # this jacobian starts at the end of the end-effector.
- # but the wrench is probably modified wrt tip of the end effector
- # because we defined the load in the ur inteface thing.
- # so that might cause some offset.
- # test for this:
- # when i touch the f/t sensor directly, it gives on linear force,
- # when i touch it at the tip of the end-effector, then it give both linear force
- # and torque. so that means there's no memeing going on with internally transforming the wrench.
- # plus the f/t sensor has no way of knowing where the contact is anyway, so you can't account
- # for this even if you wanted to.
# this jacobian might be wrong
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
dq = robot.getQd()[:6].reshape((6,1))
# get joint
tau = J.T @ wrench
@@ -103,7 +94,7 @@ def controlLoopPointImpedance(q_init, controller, robot : RobotManager, i, past_
-def controlLoopCartesianPointImpedance(Mtool_init, clik_controller, robot, i, past_data):
+def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot : RobotManager, i, past_data):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -117,28 +108,19 @@ def controlLoopCartesianPointImpedance(Mtool_init, clik_controller, robot, i, pa
# good generic values
#Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
# but let's stick to the default for now
- #Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
- Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
+ Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
+ #Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
+ #Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
wrench = Z @ wrench
- # this jacobian starts at the end of the end-effector.
- # but the wrench is probably modified wrt tip of the end effector
- # because we defined the load in the ur inteface thing.
- # so that might cause some offset.
- # test for this:
- # when i touch the f/t sensor directly, it gives on linear force,
- # when i touch it at the tip of the end-effector, then it give both linear force
- # and torque. so that means there's no memeing going on with internally transforming the wrench.
- # plus the f/t sensor has no way of knowing where the contact is anyway, so you can't account
- # for this even if you wanted to.
- J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
SEerror = Mtool.actInv(Mtool_init)
err_vector = pin.log6(SEerror).vector
- v_body_cmd = args.kp * err_vector + args.alpha * wrench
- # now this v_spatial_cmd can be passed to any inverse kinematics algorithm
+ v_cartesian_body_cmd = args.kp * err_vector + args.alpha * wrench
- vel_cmd = clik_controller(J, v_body_cmd)
+ vel_cmd = clik_controller(J, v_cartesian_body_cmd)
robot.sendQd(vel_cmd)
# immediatelly stop if something weird happened (some non-convergence)
@@ -189,9 +171,9 @@ if __name__ == "__main__":
Mtool_init = robot.getT_w_e()
if not args.cartesian_space_impedance:
- controlLoop = partial(controlLoopPointImpedance, q_init, controller, robot)
+ controlLoop = partial(controlLoopPointImpedance, args, q_init, controller, robot)
else:
- controlLoop = partial(controlLoopCartesianPointImpedance, Mtool_init, clikController, robot)
+ controlLoop = partial(controlLoopCartesianPointImpedance, args, Mtool_init, clikController, robot)
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index cd749a0e949021d4abe04b5ee549106e839b4e09..0970b3c2cb851b07f89c1ef1a7f933c437f1d1eb 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 0e87b163ebbfe0936a10a8fdbb724fb20f9fdd74..84399f29e64a0083682af835a08e95933ecd14d4 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index ed1d53be8dc25f800093d5ad2d4c3e92c12e98c2..17e58a4680408ae132ee4980a8d1be04afc787f5 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -9,6 +9,8 @@ import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
import time
+import threading
+import queue
def moveJControlLoop(q_desired, robot : RobotManager, i, past_data):
"""
@@ -274,6 +276,102 @@ def moveJPI(args, robot, q_desired):
if args.debug_prints:
print("MoveJPI done: convergence achieved, reached destination!")
+
+def freedriveControlLoop(args, robot : RobotManager, com_queue, pose_n_q_dict, i, past_data):
+ """
+ controlLoopFreedrive
+ -----------------------------
+ while in freedrive, collect qs.
+ this can be used to visualize and plot while in freedrive,
+ collect some points or a trajectory etc.
+ this function does not have those features,
+ but you can use this function as template to make them
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.getQ()
+ wrench = robot.getWrench()
+ T_w_e = robot.getT_w_e()
+
+ if not com_queue.empty():
+ msg = com_queue.get()
+ if msg == 'q':
+ breakFlag = True
+ if msg == 's':
+ pose_n_q_dict['T_w_es'].append(T_w_e.copy())
+ pose_n_q_dict['qs'].append(q.copy())
+
+ if args.debug_prints:
+ print("===========================================")
+ print(T_w_e)
+ print("q:", *np.array(q).round(4))
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+def freedriveUntilKeyboard(args, robot : RobotManager):
+ """
+ controlLoopFreedrive
+ -----------------------------
+ while in freedrive, collect qs.
+ this can be used to visualize and plot while in freedrive,
+ collect some points or a trajectory etc.
+ you can save the log from this function and use that,
+ or type on the keyboard to save specific points (q + T_w_e)
+ """
+ if args.pinocchio_only:
+ print("""
+ ideally now you would use some sliders or something,
+ but i don't have time to implement that. just run some movement
+ to get it where you want pls. freedrive will just exit now
+ """)
+ return {}
+ robot.setFreedrive()
+ # set up freedrive controlloop (does nothing, just accesses
+ # all controlLoopManager goodies)
+ log_item = {'qs' : np.zeros((robot.model.nq,)),
+ 'dqs' : np.zeros((robot.model.nv,))
+ }
+ save_past_dict = {}
+ # use queue.queue because we're doing this on a
+ # threading level, not multiprocess level
+ # infinite size (won't need more than 1 but who cares)
+ com_queue = queue.Queue()
+ # we're passing pose_n_q_list by reference
+ # (python default for mutables)
+ pose_n_q_dict = {'T_w_es' : [], 'qs': []}
+ controlLoop = ControlLoopManager(robot, partial(freedriveControlLoop, args, robot, com_queue, pose_n_q_dict), args, save_past_dict, log_item)
+
+ # wait for keyboard input in a different thread
+ # (obviously necessary because otherwise literally nothing else
+ # can happen)
+ def waitKeyboardFunction(com_queue):
+ cmd = ""
+ # empty string is cast to false
+ while True:
+ cmd = input("Press q to stop and exit, s to save joint angle and T_w_e: ")
+ if (cmd != "q") and (cmd != "s"):
+ print("invalid input, only s or q (then Enter) allowed")
+ else:
+ com_queue.put(cmd)
+ if cmd == "q":
+ break
+
+ # we definitely want a thread and leverage GIL,
+ # because the thread is literally just something to sit
+ # on a blocking call from keyboard input
+ # (it would almost certainly be the same without the GIL,
+ # but would maybe require stdin sharing or something)
+ waitKeyboardThread = threading.Thread(target=waitKeyboardFunction, args=(com_queue,))
+ waitKeyboardThread.start()
+ controlLoop.run()
+ waitKeyboardThread.join()
+ robot.unSetFreedrive()
+ return pose_n_q_dict
+
if __name__ == "__main__":
parser = getMinimalArgParser()
args = parser.parse_args()
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 3b6a437805cde5d30016b886469e36a23fe45abe..897d046b5378c19b84cf0dd3c40afdfabe0f0cdd 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -183,7 +183,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
return breakFlag, {}, log_item
-def moveUntilContactControlLoop(contact_detecting_force, speed, robot : RobotManager, clik_controller, i, past_data):
+def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controller, i, past_data):
"""
moveUntilContactControlLoop
---------------
@@ -208,9 +208,12 @@ def moveUntilContactControlLoop(contact_detecting_force, speed, robot : RobotMan
# NOTE: contact getting force is a magic number
# it is a 100% empirical, with the goal being that it's just above noise.
# so far it's worked fine, and it's pretty soft too.
- if np.linalg.norm(wrench[mask]) > contact_detecting_force:
+ if np.linalg.norm(wrench[mask]) > args.contact_detecting_force:
print("hit with", np.linalg.norm(wrench[mask]))
breakFlag = True
+ if (args.pinocchio_only) and (i > 500):
+ print("let's say you hit something lule")
+ 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.
@@ -227,8 +230,7 @@ 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, args.contact_detecting_force, speed, robot, clik_controller)
+ controlLoop = partial(moveUntilContactControlLoop, args, robot, speed, clik_controller)
# we're not using any past data or logging, hence the empty arguments
log_item = {'wrench' : np.zeros(6)}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
@@ -260,6 +262,20 @@ def moveL(args, robot, goal_point):
time.sleep(0.01)
print("MoveL done: convergence achieved, reached destionation!")
+# TODO: implement
+# the idea is to make a path from current to goal position
+# which is just a straight line between them.
+# the question is what to do with orientations.
+# i suppose it makes sense to have one function that enforces/assumes
+# that the start and end positions have the same orientation.
+# then another version goes in a line and linearly updates the orientation
+# as it goes
+def moveLFollowingLine(args, robot, goal_point):
+ pass
+
+# TODO: implement
+def cartesianPathFollowing(args, robot, path):
+ pass
def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
"""
@@ -281,10 +297,10 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
wrench = np.zeros(6)
save_past_dict['wrench'] = copy.deepcopy(wrench)
wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- mapping = np.zeros((6,6))
- mapping[0:3, 0:3] = T_w_e.rotation
- mapping[3:6, 3:6] = T_w_e.rotation
- wrench = mapping.T @ wrench
+ #mapping = np.zeros((6,6))
+ #mapping[0:3, 0:3] = T_w_e.rotation
+ #mapping[3:6, 3:6] = T_w_e.rotation
+ #wrench = mapping.T @ wrench
#Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
wrench = Z @ wrench
@@ -346,8 +362,11 @@ def compliantMoveL(args, robot, goal_point):
time.sleep(0.01)
print("compliantMoveL done: convergence achieved, reached destionation!")
+ TODO: rewrite this function to comply with controlLoop standars
+ do that by copying the arguments, setting them to quickly integrate
+ and get extract the qs from the result
def clikCartesianPathIntoJointPath(path, args, robot, \
- clikController, q_init, R, p):
+ clikController, q_init, plane_pose):
"""
clikCartesianPathIntoJointPath
------------------------------
@@ -362,61 +381,27 @@ def clikCartesianPathIntoJointPath(path, args, robot, \
arguments
----------
- - path:
- --> cartesian path given in task frame
- TODO: write asserts for these arguments
- - args:
- --> all the magic numbers used here better be here
- - clikController:
- --> clik controller you're using
- - robot:
- --> RobotManager instance (needed for forward kinematics etc)
- TODO: maybe it's better design to expect path in body frame idk
- the good thing here is you're forced to think about frames.
- TODO: make this a kwarg with a neural transform as default.
- - transf_body_task_frame:
- --> A transformation from the body frame to task frame.
- --> It is assumed that the path was provided in the task frame
- because 9/10 times that's more convenient,
- and you can just pass a neural transform if you're not using it.
- - q_init:
- --> starting point.
- --> you can movej to it before executing the trajectory,
- so this makes perfect sense to ensure correctness
+ - path: cartesian path given in task frame
"""
- transf_body_to_task_frame = pin.SE3(R, p)
- q = copy.deepcopy(q_init)
+ q = q_init.copy()
pin.forwardKinematics(robot.model, robot.data, q)
for i in range(len(path)):
- path[i] = transf_body_to_task_frame.act(path[i])
+ path[i] = plane_pose.act(path[i])
# TODO remove print, write a test for this instead
if args.debug_prints:
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])
- # this is just init_q right now
- # TODO: make this a flag or something for readability's sake
n_iter = args.max_init_clik_iterations
# we don't know how many there will be, so a linked list is
# clearly the best data structure here (instert is o(1) still,
# and we aren't pressed on time when turning it into an array later)
qs = []
for goal in path:
- #print("goal", goal)
- #print("T_w_e", robot.data.oMi[robot.JOINT_ID])
- Mgoal = pin.SE3(R, goal)
for i in range(n_iter):
- # TODO maybe hide pin call with RobotManager call
pin.forwardKinematics(robot.model, robot.data, q)
- SEerror = robot.data.oMi[robot.JOINT_ID].actInv(Mgoal)
+ SEerror = robot.data.oMi[robot.JOINT_ID].actInv(plane_pose)
err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < args.goal_error:
if not n_iter == args.max_init_clik_iterations:
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 49525cbe3832b0c5e43a750c88e054f08712579e..3e3bae24c3cb5bee5621942984671a0ef360ea30 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -980,3 +980,15 @@ class RobotManager:
self.rtde_control.freedriveMode()
time.sleep(0.5)
self.rtde_control.endFreedriveMode()
+
+ def setFreedrive(self):
+ if self.robot_name in ["ur5e", "gripperlessur5e"]:
+ self.rtde_control.freedriveMode()
+ else:
+ raise NotImplementedError("freedrive function only written for ur5e")
+
+ def unSetFreedrive(self):
+ if self.robot_name in ["ur5e", "gripperlessur5e"]:
+ self.rtde_control.endFreedriveMode()
+ else:
+ raise NotImplementedError("freedrive function only written for ur5e")
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc
index 16df281eec0a6cab5a3f34dfbf701488a8b725fe..642386db4332d0cfa0a90decd04b966746363e01 100644
Binary files a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
index f87a89b70ea42b66252ea0a118e7eb21c5ad2837..b2dc997ac21e0a3d24f05b3c9b4ebdbd2abbcbe3 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.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 92bd1773ca839c3e1618d6ac572431284646e5b6..df859fad9f9e9e72077884c55a4b6e41c9c6760b 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -6,34 +6,12 @@ import numpy as np
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 import moveL, moveUntilContact
+from ur_simple_control.basics.basics import freedriveUntilKeyboard
# used to deal with freedrive's infinite while loop
import threading
import argparse
-
-def getBoardCalibrationArgs(parser):
- parser.add_argument('--board-width', type=float, \
- help="width of the board (in meters) the robot will write on", \
- #default=0.5)
- #default=0.25)
- default=0.20)
- parser.add_argument('--board-height', type=float, \
- help="height of the board (in meters) the robot will write on", \
- #default=0.35)
- #default=0.25)
- default=0.20)
- # TODO: add axis direction too!!!!!!!!!!!!!!!!!
- # and change the various offsets accordingly
- parser.add_argument('--board-axis', type=str, \
- choices=['z', 'y'], \
- help="(world) axis (direction) in which you want to search for the board", \
- default="z")
- parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
- help="whether you want to do calibration", default=False)
- parser.add_argument('--n-calibration-tests', type=int, \
- help="number of calibration tests you want to run", default=10)
- return parser
+import pickle
"""
general
@@ -47,16 +25,30 @@ i.e. the normal vector to the plane.
Returns R because that's what's needed to construct the hom transf. mat.
"""
-"""
-fitNormalVector
-----------------
-classic least squares fit.
-there's also weighting to make new measurements more important,
-beucase we change the orientation of the end-effector as we go.
-the change in orientation is done so that the end-effector hits the
-board at the angle of the board, and thus have consistent measurements.
-"""
+def getBoardCalibrationArgs(parser):
+ parser.add_argument('--board-width', type=float, \
+ help="width of the board (in meters) the robot will write on", \
+ default=0.30)
+ parser.add_argument('--board-height', type=float, \
+ help="height of the board (in meters) the robot will write on", \
+ default=0.30)
+ parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
+ help="whether you want to do calibration", default=False)
+ parser.add_argument('--n-calibration-tests', type=int, \
+ help="number of calibration tests you want to run", default=10)
+ return parser
+
+
def fitNormalVector(positions):
+ """
+ fitNormalVector
+ ----------------
+ classic least squares fit.
+ there's also weighting to make new measurements more important,
+ beucase we change the orientation of the end-effector as we go.
+ the change in orientation is done so that the end-effector hits the
+ board at the angle of the board, and thus have consistent measurements.
+ """
positions = np.array(positions)
# non-weighted least squares as fallback (numerical properties i guess)
n_non_weighted = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
@@ -78,16 +70,16 @@ def fitNormalVector(positions):
print("n_linearly_weighted is singular bruh")
return n_non_weighted
-"""
-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):
+ """
+ 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)
+ """
z_new = n
x_new = np.array([1.0, 0.0, 0.0])
y_new = np.cross(x_new, z_new)
@@ -104,24 +96,28 @@ def constructFrameFromNormalVector(R_initial_estimate, n):
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):
+def handleUserToHandleTCPPose(args, robot):
+ """
+ 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
+ 5. MAKE SURE THE END-EFFECTOR FRAME IS ALIGNED BY LOOKING AT THE MANIPULATOR VISUALIZER
+ """
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 SOMEWHAT ABOVE of the plane
where you'll be doing said things. \n
- Make sure the orientation is reasonably correct as that will be
+ MAKE SURE THE END-EFFECTOR FRAME IS ALIGNED BY LOOKING AT THE MANIPULATOR VISUALIZER.
+ This means x-axis is pointing to the right, and y-axis is pointing down.
+ The movement will be defined based on the end-effector's frame so this is crucial.
+ Also, 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 end-effector will go down (it's TCP z pozitive direction) and touch the plane
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
@@ -151,36 +147,8 @@ def handleUserToHandleTCPPose(robot):
Press Enter to stop freedrive.
""")
time.sleep(2)
- # it is necessary both to have freedrive as an infinite loop,
- # and to run it in another thread to stop it.
- # TODO: make this pretty (redefine freedrive or something, idc,
- # just don't have it here)
- def freedriveThreadFunction(robot, stop_event):
- robot.rtde_control.freedriveMode()
- while not stop_event.is_set():
- 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))
- print("q:", *np.array(q).round(4))
- time.sleep(0.005)
-
- # Create a stop event
- stop_event = threading.Event()
-
- # Start freedrive in a separate thread
- freedrive_thread = threading.Thread(target=freedriveThreadFunction, args=(robot, stop_event,))
- freedrive_thread.start()
- input("Press Enter to stop...")
- stop_event.set()
-
- # join child thread (standard practise)
- freedrive_thread.join()
+ freedriveUntilKeyboard(args, robot)
while True:
answer = input("""
@@ -197,64 +165,40 @@ def handleUserToHandleTCPPose(robot):
else:
print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
continue
- robot.rtde_control.endFreedriveMode()
-
-def getSpeedInDirectionOfN(R, board_axis):
- # TODO: make this general
- # TODO: FIX WHAT YOU MEAN BY Y AND Z
- ############
- # this is for z
- if board_axis == 'y':
- z_of_rot = R[:,2]
- speed = np.array([z_of_rot[0], z_of_rot[1], z_of_rot[2], 0, 0, 0])
- ############
- # this is for y
- elif board_axis == 'z':
- y_of_rot = R[:,1]
- speed = np.array([y_of_rot[0], y_of_rot[1], y_of_rot[2], 0, 0, 0])
- else:
- print("you passed", board_axis, ", but it has to be 'z' or 'y'")
- exit()
- # make speed small no matter what
- speed = speed / np.linalg.norm(speed)
- # nice 'n' slow
- # TODO: remove magic number
- speed = speed / 40
- #speed[2] = -1 * speed[2]
- speed = -1 * speed
- print("going in", speed, "direction")
- return speed
def calibratePlane(args, robot : RobotManager, 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.7)
- handleUserToHandleTCPPose(robot)
- q_init = copy.deepcopy(robot.getQ())
- # GET TCP
+ """
+ calibratePlane
+ --------------
+ makes the user select the top-left corner of the plane in freedrive.
+ then we go in the gripper's frame z direction toward the plane.
+ we sam
+ """
+ handleUserToHandleTCPPose(args, robot)
+ q_init = robot.getQ()
Mtool = robot.getT_w_e()
init_pose = copy.deepcopy(Mtool)
new_pose = copy.deepcopy(init_pose)
- R_initial_estimate = copy.deepcopy(Mtool.rotation)
+ R_initial_estimate = Mtool.rotation.copy()
print("R_initial_estimate", R_initial_estimate)
- R = copy.deepcopy(R_initial_estimate)
+ R = R_initial_estimate.copy()
- go_up_transf = pin.SE3.Identity()
- go_up_transf.translation[2] = -0.1
+ go_away_from_plane_transf = pin.SE3.Identity()
+ go_away_from_plane_transf.translation[2] = -0.1
+ # used to define going to above new sample point on the board
+ go_above_new_sample_transf = pin.SE3.Identity()
- # go in the TCP z direction of the end-effector
+ # go in the end-effector's frame z direction
# our goal is to align that with board z
- #speed = getSpeedInDirectionOfN(R_initial_estimate, args.board_axis)
speed = np.zeros(6)
speed[2] = 0.02
print("speed", speed)
- # get q, do forward kinematics, get current TCP R
positions = []
for i in range(n_tests):
+ print("========================================")
time.sleep(0.01)
print("iteration number:", i)
#robot.rtde_control.moveUntilContact(speed)
@@ -270,27 +214,18 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
positions.append(copy.deepcopy(T_w_e.translation))
if i < n_tests -1:
current_pose = robot.getT_w_e()
- 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
- # THIS IS Z
- #if args.board_axis == 'z':
- # new_pose.translation[2] = init_pose.translation[2]
- ## THIS IS Y
- #if args.board_axis == 'y':
- # new_pose.translation[1] = init_pose.translation[1]
- new_pose = new_pose.act(go_up_transf)
+ # go back up
+ new_pose = current_pose.act(go_away_from_plane_transf)
moveL(args, robot, new_pose)
- # TODO: make this not base-orientation dependent,
- # this is also an ugly ugly hack
- new_pose.translation[0] = init_pose.translation[0] + np.random.random() * plane_width
- # THIS IS Z
- if args.board_axis == 'z':
- new_pose.translation[1] = init_pose.translation[1] - np.random.random() * plane_height
- # THIS IS Y
- if args.board_axis == 'y':
- new_pose.translation[2] = init_pose.translation[2] - np.random.random() * plane_height
- print("new pose for detection", new_pose)
+
+ # MAKE SURE THE END-EFFECTOR FRAME IS ALIGNED AS INSTRUCTED:
+ # positive x goes right, positive y goes down
+ # and you started in the top left corner
+ print("going to new pose for detection", new_pose)
+ new_pose = init_pose.copy()
+ go_above_new_sample_transf.translation[0] = np.random.random() * plane_width
+ go_above_new_sample_transf.translation[1] = np.random.random() * plane_height
+ new_pose = new_pose.act(go_above_new_sample_transf)
moveL(args, robot, new_pose)
# fix orientation
new_pose.rotation = R
@@ -300,22 +235,15 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
if i > 2:
n = fitNormalVector(positions)
R = constructFrameFromNormalVector(R_initial_estimate, n)
- #speed = getSpeedInDirectionOfN(R, args.board_axis)
- #speed = getSpeedInDirectionOfN(R_initial_estimate, args.board_axis)
speed = np.zeros(6)
speed[2] = 0.02
print("finished estimating R")
current_pose = robot.getT_w_e()
- new_pose = copy.deepcopy(current_pose)
+ new_pose = current_pose.copy()
# go back up
- # Z
- if args.board_axis == 'z':
- new_pose.translation[2] = init_pose.translation[2]
- # Y
- if args.board_axis == 'y':
- new_pose.translation[1] = init_pose.translation[1]
+ new_pose = new_pose.act(go_away_from_plane_transf)
moveL(args, robot, new_pose)
# go back to the same spot
new_pose.translation[0] = init_pose.translation[0]
@@ -323,13 +251,11 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
new_pose.translation[2] = init_pose.translation[2]
# but in new orientation
new_pose.rotation = R
+ print("going back to initial position with fitted 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_initial_estimate, args.board_axis)
+ print("i'll estimate the translation vector to board beginning now \
+ that we know we're going straight down")
speed = np.zeros(6)
speed[2] = 0.02
@@ -337,15 +263,11 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, np.array(q))
- translation = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].translation)
+ Mtool = robot.getT_w_e(q_given=q)
+ translation = Mtool.translation.copy()
print("got translation vector, it's:", translation)
- # 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.getT_w_e()
- new_pose = copy.deepcopy(current_pose)
- new_pose.translation[2] = init_pose.translation[2]
+
moveL(args, robot, new_pose)
q = robot.getQ()
init_q = copy.deepcopy(q)
@@ -354,9 +276,15 @@ def calibratePlane(args, robot : RobotManager, plane_width, plane_height, n_test
# put the speed slider back to its previous value
# robot.setSpeedSlider(old_speed_slider)
print('also, the translation vector is:', translation)
+ file_path = './plane_pose.pickle'
+ log_file = open(file_path, 'wb')
+ plane_pose = pin.SE3(R, translation)
+ log_item = {'plane_top_left_pose': plane_pose, 'q_init': q_init.copy()}
+ pickle.dump(log_item, log_file)
+ log_file.close()
return R, translation, q_init
-# TODO: remove once you know shit works (you might be importing incorectly)
+# TODO: update for the current year
#if __name__ == "__main__":
# robot = RobotManager()
# # TODO make this an argument