From 401521b72299442a9cf7d546c01c1c8ce779b0cd Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 14 Nov 2024 22:13:04 +0100
Subject: [PATCH] in the process of cleaning up drawing demo. significant
progress has been made, but it's not done yet
---
TODOS | 12 +-
python/convenience_tool_box/freedrive_v2.0.py | 35 ++
python/examples/drawing_from_input_drawing.py | 327 ++++++------------
python/examples/path_in_pixels.csv | 96 ++---
python/examples/point_impedance_control.py | 46 +--
.../__pycache__/managers.cpython-312.pyc | Bin 45765 -> 46407 bytes
.../basics/__pycache__/basics.cpython-312.pyc | Bin 8798 -> 12728 bytes
python/ur_simple_control/basics/basics.py | 98 ++++++
python/ur_simple_control/clik/clik.py | 79 ++---
python/ur_simple_control/managers.py | 12 +
.../calib_board_hacks.cpython-312.pyc | Bin 15476 -> 13360 bytes
.../__pycache__/logging_utils.cpython-312.pyc | Bin 6766 -> 6766 bytes
.../util/calib_board_hacks.py | 260 +++++---------
13 files changed, 432 insertions(+), 533 deletions(-)
create mode 100644 python/convenience_tool_box/freedrive_v2.0.py
diff --git a/TODOS b/TODOS
index 973694d..2b541ce 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 0000000..fd41882
--- /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 fa8caa1..dc88f72 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 cfeb519..e9587b3 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 b4ad430..953b41a 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
GIT binary patch
delta 584
zcmX^5l<D{@Cce|Wyj%<n3=D_)&C(n9Z{*{3W1Ka)K|+}+MRKyEo2s;A3Qr4T6mJS|
z3qurN3SSFD6n_eT3qzDZiqzyrH!Xn_@fL<Ci4=(zhA7EOR!!;6>)j4#OkTWJj46v@
z^1^9SY$=Ro3`NqD8K;X+=1-TM{C}oceG}tKW<O1)A~6OAh9U_D28Jq)w4&71l%mYC
zRE4zCyyTM1{5*yHyqrpf@}kU=lGHqfwEQB4(jwDTO_rjmAYIiUq7Fnn01=xR7#NBf
z85kH8igXzm7;f<tr<S<EjI5Fj%1_EK@lDK2OiwM+D|SSZyTQ!BPz=)2z;J_`zu&&o
z{(_w4MQ+O*JObBwWH0i_Ugy!i$fJFgNB4=4>IJoc%R+${cmgNSTPuy%e?@$Z3=Al4
zZ2~zHt6TX>^Ma9FR3(k#T2#q<gq^E*QGrMADvtrOdyDizwr^r!V9;cA)0CQAv%Zpz
zpOJx~NO1DU^@@xwlZ7{w34YRIWi|Z7$HE%K_?ZVpZtmN#pOH(3k%0jevc(yb!#BkX
PhI29oF@9nIQ($8OVkoUi
delta 147
zcmX^9is|T6Cce|Wyj%<n3=HA7jng;m-N?u5#@IKxK|+}+MSQZOo2s;U3Qr4T6mJS|
z3qurN3SSFD6n_eT3qzDZip1nbH!YS*R!zyxyWI|Fa20K0U|`T>bkmfW{A^?8<jhU7
zj1`mHH<ht|)?#Hf+|0LmKO>hoBLf2?0|P^G+T`n7;`zNf84W)%fXE_!1_lNIz=<kP
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
GIT binary patch
delta 3764
zcmccTvLl)AG%qg~0|NuYc{j84MDvM!5{w@vs{7S5q%h|2M)9VAXuc@E6qZ!(G^P~R
z7M3Xf6t)(ID1j9AR6(#POA1E|OO#LwXA47=Fj#~sg{y@nN+gB5g&|55D#Fvk5+w#@
z^R}==iKj}W@MUW-Fci&UVo2p&4RQhl14EQ#s$`Za1H<G^ViICe(o77g+^N#586mP!
zGO03I){{4gNpUeUFfpXcrOIU4O;!++a$sagWzKSds6r;Q9Fchtb_zetycB^dRtAtx
zr^)}B#9UIDvz#DGkjW}828Jjlux&~(jbPgaQl(R6Qk7OSLsT;|q{?KugB-`f$WX~x
z$*3u~`3vJWCPx0vuUQ^2F&b??!>+{6=)3tnUlbc-?`AJiJ66U;n>(fUurPj_>?Z$F
zBbg0i5|{+BK_-9R!NkBYogsy>mMxQ^mOYc9mSb`Ohdd+a<Ov+|tXwsWS*##C*=o6K
zn6lWx%;JO^rYuen)2f!Ih9`vy9L!89%(ZMaJSi+-o=XjL3acao149-!*g6!VhC7QF
zq+JW)-x}r=Hk`^iA<CInGfp;OlXr0`5ri3+!j3QkAzH(YO*Dlgov}n1s&jI@i0I^Z
zTwGdN;!qJJDuolKe-Yzawmt?%h8oT+$;orLHMFv1kkmn$X^b%CHS8&DHVh05HH=yE
zAP+J@bR}mgK$Rj<D7v`7y4Y$sYS`15gBdir6RjB;7`PM^6cm#4^Gb^HbA0mi3*3rQ
zQ&WmE%TmD-x+q|BBC|++c}8YVszPR-LK;k^jzV&NPEKlai9%tq9#~sRMrN@>a$=rB
zQmR5}acYV}Nxni^W^rj^PG(iALSkNuLP1V`i2@d*z}jKP73b%sDiq{r=9LsH<QFL<
zDwGr@)@P+Am*f{!Dx{VqBUzSKnwMOXnV+YSlAl_vke6SgkdauHs!)=VU!1CtmYP^n
zT9jI>12#RWv_zpYzZC4g(qfRB%wh$&{={O1lGNOSoWzn;kgIbOvq36SbHQ5Oip1*~
z7#KosF&5uq3yCj}Pc1IeW?*2b(zYcYR52A=y<}lvU?_@aU|`S`EaG8cV7SGZo>~%K
zl$w{Eaf>58wFGSVE!NcBf|ANx%;~8mx7ZR33R3e@>Tj_m=ND8KDT5490TH|)A_imz
zYf*k_Udk=@qSWGy#Ddf!JCIBeh%f^Y!655+i&Q~uc943`<ow+D!qU{z)LXm-`NgU6
zdGUquDVfP7MW7^HBmz<>3L-#7N|6|dB@RlPx7f-dE@g!{lR39Iy%^+-$s75E>qX!R
z!x@pRiYgfx7$g`O7>Yqe1H%Uq20^g~&l~)L4IVc{#T)z{aPanXb#is{G&tVi73z=g
zjK9n)*Wh-8N3h?o)9*5mOoQtKZjpZbPWvm|GB^0d`*S;UyYoAkZ*cQXQ0}t2AtBS@
z31-)KS>51~oS|`%NBKIB)<qtz6{hR0R$8sLxx!-sl2EzGBY&Mo?IMrbg2Lrx3(FQ)
zT;VZzz|MW0UE&hE#2pUa3w+jBIBf24@OGs3@PMe43mjTgf~H4IjF^!)Ir_S&+C@>d
z1%;PIH7|2$JrEU}9yu{`0qb(^h1|>e7V=%Fmp8a9YS`e>5z^>)Lri*l_QdSTx!1+C
zFN$fe2wfk&GJ1X7%D4+UwwJ~18az5e8~tx^i%iH{ka2-q?*fZnkt_oPLoyRm9tKrx
z;6hXnT!_{%WFbkym_@uLAT=PaC_@UotYBowWC#Y86yWleiIHLQD?tyYG{(tBLIGlk
z!Xbre4pS{tCPOWAiQwdYTr!jEnK<iN(izejQkds3)v}g=5&_6;h8o5acv)5=0Tu!k
zMGQ5JH7qr(t63o;H4HV(HB2yGEn5mp3qvh?i8Mr(fdN|qSHoVzlE%1(bv4xQY_%LU
z9FP)@jgg^-ElUwp*nyZFS;{abW0oq2Y01b?!y3$>$(HCfEJ`$3=~N6U!crA-^3xU4
zit=+6zy&L!tjbFP6|V{<8Hpv3vMs+zp`@}PRUto5p(G<!Av?7)DL=6YT>gR$D=tV)
z&P>ZphLx%sg$mmB3gBF<2`=HPPJ^k;l+?tWoJxhf{Bm$vS)N~-1F;KSCKu;qrlb~u
z3uO?$BqK8~T}J_|6I8lpDx~D+sl$rk%v^9unweXWlbV~FR|0mfo<de>afw1vX`Vto
z$Owhp{4$6v*p!ldh4j=Cg-lQ#ky?}raYJHWi9$h6u^yu4Q7F&M$$^-ZT9H`-axPdq
zs7?c6O(t;h3@K=tia<FBERYOsjDZtmJt*gZ3cSx%4A5o@BPT-%TuBNOtm>_0tYJ)H
z&NgRYC=#q;Okn}Dc;GpYA%zvDl93@#yoMnQnjgT{K#3Zp%n3G4qn?4Gh_RBP$5NBc
z?-oOqbwE*Saj`-nsP-u?$u9s`mmuHiC=`RmK#3v?R4FPX=B4LAs#9<bS}7EPGG|dH
zs7BJy%qvUG$xKnm%qu7@(NW0H%c)cV$5^3)MoC6$o`P#$NotX%LSlVRPJVf6iYD_d
zR<PP4P_1%{xuCQ}lc}f#R8KG`=cW{c@}+`85hxSi5-Cs2Eb)dWE;ne^TV(<-_(Stb
zGIL<^dNw)v$%#3|c6!)!#WU73FciBnFfcSQ+~5=FV1B?SbVo@1f<)+Lp|B3;8v?=;
ziYIX`5MIH&Na~J&=mjzNs{$Sm1SF^PPUM}Dd__R%hOpQS<w;gIL?l1(Gw=(83s6m_
zTWlqXMd_&};Mz`8_!eJ5W?p`Baz<u;JSZd}e&i`mEpaQVhc<YMKs8vAHK>+DRGgqX
z^A;<lj${o4Rh31cNG$@T8E~C;i?bx7C^a!9GcWxXTL_qWi?z5Uv8bd-2^4lLps2dV
zSDF_LvI}aJCMVckMW9-^s2)+ff<n8f6yyRm5YYfKKnOY1AQlvJGB7ZJ(l;1GqC^aP
z;1~5UFff4HdBtpupdkJr!ob0Eon7h@yVM;H-VY2c`~vk~7+5$3KX5S!2z7AYVBx&Z
zB6g8Q><Wv-7d8e->G>QpITolc*IuZ-TyLS?1x2IF62^BVq_7EpWM&c&YVi2L#vmrq
z;P-`tK~QWu_eAc=ybYco*ccR*S14T+(QWX(Bc!q*bdCEJ6OR)r7Xm^r)Q2ZrNXoyg
zQ*fcE>;nTsC8Imj2L=XDra-383=G0SOby<jxVTuQz9=zBDPNb=yeO%8!O-<Y<O!cE
zl93l!#BQ*NU1yQG$RcxvMeYU*f4hI9{}mQdu&DG!7U?T2vS2?+Tx5~B!XkBpTjBzX
z1UQl0;)svWP0Y;Wi;ph?1?Wpq3fEMb?5iNdT2#ouz%V&q!H2UA6cH^T-Yx|dc5svz
z^-R94(8N8Tfq?;3NEGukPR>wNWV_8EaGQZ=a;KuRv=5^qqyB=L&kTx;`XAgFtr#U&
R$b4q7VwC()G5M_GL;wwd*%1H$
delta 462
zcmdmye9wjNG%qg~0|Nttm6KMwmgYo03C0r>)%`+QQn}NZQdnD9qIgr-S{S1EQrJ`Z
zQ$V6DDI6^<Q35HPEeuhDU=gMit`?Rkp%m^GhA81wkrbY64F-myIZO<xoU1`bGcYhj
ziKdEX=}tZ%Ate?i&cu+)ohrVX5h52Qkt&g843gqvWME=Ql}eS!GMjuxR%&trBP*lD
z<VGc7bxVj+WHN;pW<UyG6)OWnDsz_A<OPh9j#kL3AnYnG28Jj(u-S4jU0}QTQpHmx
zQsq`NL)0-cq)KGjgPg*_$WX~x$*9S{S%&Ew6C>|tVb%vsj0T&(uq&}MdT$ozk78r&
z-drSV$I7^1^G2yXEQ}u}=gEJxxWy44pPQJO7aw25$-uzyl8J$VK~v!tXF+COesXd~
zW`2<k0|Ub?*1Xc(g32OZ5St}Azo2sRRDGYxul1EDOBmGaR5LIzfC8uZJp%*72WCb_
r#?M>~j1spQgl;o%-)7*y%OHE7A?6}O%xwnl$tMk*8NDW}8%_iOt(<4V
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index ed1d53b..17e58a4 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 3b6a437..897d046 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 49525cb..3e3bae2 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
GIT binary patch
delta 6201
zcmexTu_1%+G%qg~0|NttZ;)Afnf^pR3C6C8>ek%3Tv1$%3@HpLEIAW%CA1k*7<2fe
z_&FI;SX&sP1X9>q7@`DI*jpH)gi<(K7@~wzI8)ivm{Pb}SSGHM^=G=pnwwvi>T^o~
zM1|&+Wac>M=anQTmuNEH5=$#eO-(7vEK3DTc&An-<tG-UBr}4X0mW<#3=GT+3=E$i
zF)%PppIpc&F<F%{fASF?5w;YL6wWnVlQS3<6}d||pz0Y?cv5)Ru&ic*@EI9u7=jrz
z`6g>Hd9oXW!R5&nOi7b{m?tu_PQJ%1Hu)#>2|p1A1_l`h28QBx1_p)(h8rwg*IC3b
zvWU;%Uz2)6(DR0f#0OReR*5eROsrBL*ce#3A<`ElG}e^f5cEQmUu?<BD9X5oX*I~<
z3=9k@%#sWY47H3kjFTPM<tOK|=`pfQp2Mcd$U1o+Tc|i&2{**C3=B04tD&AxVcV?5
zuE;3OUc<7Q2_ntNP{I#kF)*ZXtY)0NfK8T>b8-tud_5OT8zVy`Lk(jvLpehQb0kAK
zqb7G^HX{QAmx6+VLRw~tUw%<;Voq3Ua!Gy>SWFiUfCZCt5{rv7lNEAO6N^g}iVI5<
zi&Be0+V#NFB^jwjsp`cFi8;mj3gxMp=^6DUnR)37CHV@uiP@<Nd8y?Jxv7c8rA4W^
zsd**E3c2}3sS2671^GoKiFqYDU_+8pOOq3eQx(cn6_PU&^U_llN-|Ov@{2N4^GXs+
zGV}8k^3%XVsd*{7scC6oCo3ctgEXY)>nVT@0cip0R!C1($jn30oms4qlAo8VP@Jz&
zl94#MP)Il`BeSF!Y$RAOIH@TlmViYP^U`xtVK#zgbQBWvQWQ!uN{baT63bE*lJoP5
zGmA@7^GZ<s1__bP|2QNW86_r5aA|4@GcYjR;ztYXA{_<>1}#tmi(v#MurDGEg2J0K
zxm20C5^G`cGufL@t{&_*v`E3i1uFx&siZWyL?KZjttc@!RUxq`zZC51RE0D!D-Rss
z3T5EX1M4YIRmdtWE>TD<E-uZ51bT%+Vnt@LLT0f-L1KMTVopwKjzW26Nd`D%it>~4
zOVo=MAd0|7lw?3u=cg$o=a&{0g90bFASbmXr&1xYC@Hg~D6yzgN1-?$tUAQs#a|&U
zvjS{dT4FLNz!OUpD)URRIV7ecvly%`zeu6HD6=G0Atk>&PoX@c609R7KTo|xAulyG
zMIp09K_gQ=S0N)mFSQtCbzXjnLUCzPszPE(u8u-dX$eS4X0bv}Vo9nd*v!fOyrR5%
z3=9mKOfNxMvq*CCMqV{GX$A&{BALl|dDR(}CbRN6FbYq0<5Ll_VPIhJ1V!0GaGCHy
zgn^fDat+^XB~b<jhGb}-1m{OkSpzcf^EE~WhUp9?jFSx{L?-+4i}NzoFr_eOYcMbr
zv(_+8F61}mW=dg$^EdKW*&yfr8b(A`=VT}W83{H&g$I_sYtao9X5^i0DX1uqWNZpM
z$burp5@<=nz>vZ>xl+)Csgi&4MSk)6<ep?rfyCpmRF{#Mmy(kjTAW%GlJ5a#hd2iW
z<QJ!cgA7a3LkNLY80smMq~_#+qqnp;wMd~n1C*)q6;kpcsi>eRGq0prM<FjUH#MhH
zp}rhce1Hmzf>KagNL4`2;b0>bK(a~sB^e5!I87=_P0Us($;ix0F9v5yaJiG1TvD2t
zlT)csT$-GmnwkREXr!l5T#{H+q5!X0z{19Q3Z=!Mq98X_A-f)0%P3^#fgBJWsh<}M
zDpwK}l5+Bsvq1$}a$-&nq?9tzQz%LW6@!qR0yh@yj*^VjJg9vs`5=AZ;wUjEGpPtt
zf)y0yC#M!e(zB_ag0G{ut3q&iXppNyh=;3!tDlRmtDBpvbBKSCf?JTIud9M*u!5tH
zr@No4i-Lkvq=JvXzqhBKyMki~Sc9*lpJza*k0VHZm}hXPqmO5lYY^Cni5e`ES%n_f
zgKZ$r0%FaFlyD)SJfEAIm{+V&p$jiA@-y>bB|pgTMfI8K86}XCvl3GkC?~^AOH}~1
z2SC+>LV0FRjzUtZLP}~{W?pKFLQ-OJYKlUB9ymoIrw#RDXfX#a(;;P0a#3k=W?~M+
zSVy*;;(Q&2+|AE~gc(^2auV}WH~$k(XJj;=oF>}Be2cMi^Lx==#>o|Oe?9y(*@~1v
zMKvop9o%9`t;j67#Zr=)n|h12I43o=pa|6T&}6>Fl30{pe2W!a5Z+=-%quQWEh+}(
zO$7yoBH_ur3i1k|Vj>w-RQv%K6<@>{Sh+qh2yk<JVGv;D{Gc@1OQD+U35(PWlLewz
zSkxygDyFa(fr^nL_sI`cB$Z^iA>{@*w=;m60mwx`4PzGb<cES%Otq|&{iNlXYS|_;
z3dl{KrOG>*NkEpbh8<oyaMZ9(7T^_`{DM_vvVl71WL`B%mKx41c93qCTCNhl$yo+6
zOj*JpzEUlB3R4Yt7O3e1vlmLIFoViI6xDpSJSE~#X@)F`NzyWDwY({;HM~%@yfC>G
zwi;eUDVEMq%U1#_U%}EfEHIb4Fic>IO<-WC<u5_#g!_Y$p+pv}fq{Wxa)Y*r6R5QV
z5nyCM(!g26kFcsl5uySTdNsVT#up<)4P%z_<cESXEQo*<00pE*tss(NT<MH8g4jYM
zOLcO9zK96S>5CXk_+U0o{-7=*mZb^fBIp{y$sbf@m>^p15L(x=^)aA$fhV02DO^D<
z1cV`ARt@81Hw}%+2I?H}HXg)ORdiEp__7SaT9AoJ0`e>%Z%uwEC_I^8id_n(i;<y*
zzlJ4+w~C8_A)ASTq3AIvNYrYDYJ{?kkxhWGYq((=YdK0xC!f=mkU<JNz8VfhMAWcD
z5*&9eM~N6jH^bzMTB1x@=Ad+^UBgtx%D_;=gDtF6_`xX;i#E3!W<1(>LE36Ku(}*k
zc|u$+IeEV>A0zK%eL+!1kWVDt`BRv)T^JaOnI<sx7?fB--ONzK2lumZi8Wl5vql)^
z*;+27$OB~xLF7y^fvG2%Wpbc_I4ES<z#$9HiE6bRIGl<e$ddK!*h2}_pQz!$E(Z<@
z?D}}|=z|0ynm#{>uT#Js7#tRY3yT`A8jg5Yh-wDpJSqeWLq-Njx|w{zNSqtBDA7U=
zHN5#4Tn^UP3LrW{Ss?Gi?1j=bj9GS2CK6S{nB{=Pg))&M5L@mOfH@zjIGOA(Eh<za
zum)Zv)UwsEWr<9_#;OJ?b)`j+a)xM)2rRLH10c^xni1>?rdk0|DIQh>x1m-PNuL<Z
zUX;>89NDxKiCVD|XQ($AYQ$0`VK&u@PYzI%6RF`sD6f^Mk*HyXWK6RZsX0uwk|nM%
zy;*KB7J{yktPzKq$H-74p5+NDLhC_h*082A2Qz3&R~-hoJ(8g<p40$Pg$ou!G!a28
zuuyJdb}FRz1#To3r{<)By8@{SCHV!qIjLzS3d#9Jd8tLPUKqIg2KDOTjV?Vdg?a@A
zP_rNp(hCI1g4!MFMVSQ!sYUSiL6t&EW>IP~xEEQHU!GV5>d2)kfK7z76v|T-iW76e
z4l6QaU|>j}H4_AO?k{p=U|@I&s`=BaMb^1LteRqfi?z5QH8rJ5!`9YT)s`4AS;JVy
zuZpE0GtcTJsBcikQd(qcRRqd>Rr2YOreeMVsGD4nUkvK16)B{omOva_#KgeBP$g1Y
zkdg>)T0winldFwo^mQP5lM<7&K`rFWyv&l!#2kf!{NhZoU%{QWw9Jx{)D(rFs@%yl
zj8%h6@)f}SS%sw3^vt|GP%||zzZ^6i0K+Aq_Ii1$LN-XeJXO6YRRLm8aY<1ks6hwr
zf@w0|VlRwO&n!#Ln|#1HtG-HDPahnh@gP6w6=Wu7=cL|ZDo@g6x+M&eD9JB~2SqGc
z#VxkNc#wlNRf|BarCTiNsU?B8IMP!~LgLHgQ*W^(=NDAo;z&tNEr76!@*(2@kX9OV
zL1x}9=3rOjTO6J#sd*)qnI)CCxJ!x>^NMr8jqM^(E8`YxRccW_sIdkbQ7Cc&b?iW?
zya-e>-C~U|E=etzY#<})Sp*vFxy4gdl9C!<l$xBHS(bWBAU(Cj5!|bQc7SfNB^DH<
z=B3<XPE0N-0!8F4wxYzml>A(9cfJTz@=Sg&t)Oy?CBGmw?-m<05Li-5a|>><Cg<cA
zr)o+SfihQ-`{cJK>a0b9pcajcscbzc(G`K}lv|SN`SFS6iIwqbMfth$(2xW9EA5sP
zNGd76EHyqawLHE!5!A_!2m3j#$Oq&%4NwD?G4mE<;Vo83F!Cgq78RxDm4L&%$Q-07
z3Z#e)6!!6jw>Z->b5i3A>Jv*cZgJ$~r^kc%x40nIf?^UPnpu*XTP(%EzyM1mm~;_%
zpo0%J_ZQVMFfgnHwS+rZz~dYO46Jf9QWm&gVbQw5E7Tv=8Py%r;C5s31yl932a<C0
z<z~uVm(;l^sdHIU?*fb19Tv_DTyhr_jIOX4-{6qC&LMx1L;f;{VuRxo1?A=J3)!zL
z7+h2^xU67wfkpO)zVQVX+3PHt7g;n{C|+dId7`MYTzsMUbw$IAiiVdJjW4jsec_m_
zX{I3lKtQO$^A4}j1oQs54#qniychT^u5egRjx<wA=iu$<?&R*_y}={c@73wm?bE?{
zLqK#o-$cI40ty|hcQ|-E(mpUSa@tQQy)LeCQC#D?xZXu^y%mL*#SOvYcLYVIi%%54
zE~s)*P-Q{r^0<X@mjz9(3tC+iwAx{KS<v<~Gb5+{WJz<!`s?Zz7u78;OIlvxu)4v`
z(@{Qyv9qen?uN9&e7l);m!-8kd>)7^Ul&)uD6W1%!|}4XQ-{YL9>EDJGgxO-T;b99
z!o|QVbV0~?L&OyxyBp#P)9WVIT^3jG@VH}PwV!h*=XDE@ixwW2Exb-dU$lt0z@v7Z
z$LJ!D(FVhdJf?Rntb9<_#9Xw9L{)P`Tz-1Z#F`5#c9+HNQLTQUpxEJYg-7-dkKhF%
zjVnBwlfPSN$=wkVo9;2uV@BX)?+(`+;?f<i6BN5WKKL^-@X34$n(SyPCVPWl`Z~YD
zMSg|L{7MZT5BSCVYdUK#$e3Q{H*4^iywx&8{sNc96&A}oESw!G6IdryTwzgo!ovB1
zfsxzz3j-so$z($-e-lvb_;va%FzoQV%%j@idP7M3x{&fkA?52r8W)8$E(>Wjc!N`q
z<_ha8EaqR>7+AR`zp}Ed7f@~Ryu&X%!E;8*Wqy?g4`^)75S-zCSx}?F>xR1Fj<EgF
zJEM2UUgY;~@VLXm)$ZNsJ%M9}#uXOj8!SBSevN)tSVTb4l|94k0=MD?7DaI9O;hw1
zXF+COesXd~W`0pPD4Vh7mF5;yg3BUE=@T+J(pq)$Qft|IQ27BVhvZ9(;)^rEnK?N>
zucRnHM=vR{I5W8zN`X6`w>V2Oic%9(GV{`lK%>4zS)jZO>Z~COA8<(u&2nHDLGmPr
zO>TZlX-=wLQ5ypT185Ye*pDHUf#Cx)BO~Ky4hBYv+YCat8MtpVaNlJxzRzHPm%-^V
zgTQSD{@V<^A6U2;Sw2Z{GYWl@k!6(nWGBJM`^m(Gkx}%64^s&vqtXJIFAN|WY#{(l
C<38sA
delta 6954
zcmdmx@uh<AG%qg~0|NuYntzJv-)txHNicRyRJZ14WJqC1Vaegj<&K(|E1}8S!VtyJ
z$-v2w!q&nNCBVs$!rsCVCCJH;!qLJIC6vP1!Vo2#!qvhsaf2)m<1Nm#qSVxsqRg_?
ziD#sxlNmu4KruT50|PSy1H)%2Mh1rIlN%T%CYvzk^QCa4aHcb*aHVjp;a<%;c|W5(
zGfxTI<ai|+wiMnJzBT-l_cAIf3Y37|3Nk81Fhyt$%W4)7n}LCmp@t!tK~s40e@0LC
zSu;W4<Ya%Qq<S7*-K6})q7>c4ip=6$j8(T7E32F|%JYkIQZyAnVhS25nMJ9|C7Jno
znhKeD3gsD@$r%cj`K1cwiFqXoCHV@)sfk6&8479nMG7SusS04@G+Bx`7#J9Cu_tHb
zXC|i>7jZH$Fcg6tSH-KVo1B=FnN*Zml9@kwA5)7o+by2Nl$7|yqV&?-)Vz{gtZ6y<
zi6ys~i%W_oM=+nPmtbIE;Adc9DBj4xz|g>OgN5rli}*zr@frMUQf~-)eqdt|mHfcT
zz$)>DfrVA-0~Z4;H$=e&35_+SHw3-VRq(-8%-~xSd_z=XhVlfj8-k%XM8rR^gUw-P
zmHfZ~RW-Sc<tH!08phS2XklPrNMV{>!y3xUT*J7UY4RL)Sw@!0UsyvKStkpyi!i0I
zZcboRWaMY7VOh-t3;PoO$%YCdlb5iIGIC7b#U3xsxf&Yfj0}+sHH^Uw<qQ?fkqqUG
znp~Tga7r>V%1qwJrKuqf3PS$0%o4x+qTIxsu+-#|{GuWQ1_lOeP;f*sf`a3YfY4?^
zZdGPwKTYOzh>aiYpF<c73`Ir^3=En~FF|fElAU~uUyV&36cq}S*#y)X)h6o+I53J&
z&KFP-a$sO!hyocq9}>zuypxv;%$AX4U|>K=pscV2dWvasoS-2uV+~^pbG8NpLosU&
z<K#JlrreAvY;gV^!77dvo)(5$rV{SSH+W?x$I5f^GM8|}+?T~Sd7+%JI8zNn4RadP
z8eUkS)w0yEWC>2zmy?oVWGDf-9;_pU4-s-T%vr)91#FBAH7r@8Am-#UL4Iw1n37r)
zr=_rf991M%0u6cwh7^Ix8wBOW&1;YxoWc%PrC7o^`J;-+WI+wN$$Y{*lYa?HvM2W>
z*Dz#>O`a$(%qR)6jf;_?l0j3*FHwbgGP~%*&2Pkn8Cgp*GV{_mvq+>fGFng0k!;~A
z;sM1XV`SCr$<-1%n^mM@7!{JCNdxRBP>vS{<@g883=Gp5Y8bK@Kz2;llarpjUx9UU
zmb&odSa}ZC35>;uYbG=DiZW$!PS%%`W@V~j%Hp1^$S8swC{Q6WSRgPml<<PBVPIf@
z1U556mH;Tw6&bRGU@{20L=?u(5(BGZf|&$M|BMVJphN*yR>P1b1ad5+)MQ0QVMQj8
z1WY}$x)kPgMpU)zV6|B?VBPYQ530!UEn+MIg%#M~ECsNHGRO(cD3(Aqs!Xm|l48mN
z>r`6AxR$Mt0mVYrbVhWqK*COa@_$8TMx6TCF!gc6g8*V37u-6{$)^=%CR-}-Oiou9
z14UAOG1CObo<o(Ogu&ql&U`P~7#J9Su@q$HS-k|6#=lrfi%hMmm<p}Zt3}qiKddUT
z*A%<OQ&f_Y8lRk>S5lOpbBh;VKltXSq~2o7ERHWuErBX3N=;79EK9w`m7ZGSm|Rkt
zm=k!5EwP{=H815B3v)qc-sJB(3NA&EY!1rnYM=zjobQ`?i@T&KF|RlWRPyHCVlB!q
z%}cq(TvSkbi#s>5q$sl@BtNL2@)k!?ehFB~<byiG^(-Zsxv95Ui*r&_3pANQx{~rs
zZgCZt<QK%JmVwG^#=>Gy231f{C<2#Q;&9i8WE7<)rnr^nf$NVdtBl0Fl$_Mi;?$y$
zd=D@?#5o`!zc^LTCMQ2RF{jv04_$9PRP#Jg(G&(Q@osSNUgway$RTx|L*pWc#uW~&
z8$vSIg_JJ}DPI**y&)iaT|nugfYNmV-HQUcmj(1Y9B=UO_Iq@CObG7rMPlCJ;lIwK
zc#%hOf$DPYh1x4h*H^8q+GDieVyDFc$KBRfc--&s2-Z(fovt-eYr5V<y#<07g_N)G
zsNCTZoS^oBfsxmrafa!9tC?2w?Pl7oP`oIub6wivqO`?zX}gQkb~_3$OFKeT-4T_T
z9zHSrx~RrQQH>R%>!nsot(RLVw}bJbrqy*#=Zl)omqlH!i+WuY^*UjAS=9G4Gsx^G
zAZsqv^MuSXnr|`FVu9;*6|;*fX4h5RFRHj-R`I+n?R8x`;G%TEg}|W8(!rN`LO>2v
zxxk}0M`*spOo;`Gv!$;~YF(7nT48uuQui{C-W?u+ey>ii38hzfWIu>92+KkO?YfZa
z1tHZUP~O#Ky2VzKSd^Yxa*HLgD80B!Oe~-%wYXTpHLoPK2wdBj<QM4a=}m5s{#&of
zTLey9teJTQr6spmQY$h`ilRYDE(Vn3I7`6MpP83_i`5mJQrI8~;udRhNn#PGh6mY`
zm7kec<OR|okeZj`25T9CQobf@Q7lM;EitdSJhiAO0c3U@NJJp5C{+^Dgo=k)Qw*w@
z6hNRzeDYBldAU3W28I$)s!0H+nma6<9~gMKIleIPvT}Zqnam<v9rKBchgI&29D|Vb
zboq($mj#s?yuPq8NGM(xSHCE(enH>uMCygm_zS*?SHzPpu!!7X5xLGHb&*Bt3X9AW
z7S0P?N>^BvZ?N#S`!)JqVG;Sj#=t6iokj8@i{up+>B*h4DgGbi7+ATkvxr?}5xdT!
zevw7}DvKsKpiyc7P*Vw%gFY95+I$Q(j46!ZT1c-3)XHRNWlCX60~eQ*=kO?a)G(zm
zgW{@&DTM_}gC$d#K`@<dHMD}OWzO@eVN78~Gl~UbR4vQoc|3A)H7roWSfGZnq~J14
zyaW`&;L<sTt%fm$9jQV9)nQ<n)nF3T;H+mZVF0yHK^4+MIpKO{n7gXD85nArYnW3w
zAgmP58s;iikQkB$Ts6!o+z1P5n1dNKdHle2X^{&k6@Y5cB3Td%)Rw5?0<}mB5{rvd
zQ>tWj6p~6y6f#Q`G7^hHX*MZULA}08T_L|nLA_G_r3M27!^{8w|NpPzNYBsAOIOIu
zyTyrU<+7m_okb}iTNFWrDyT8VT4WDOfn4B*Zak=se2b$hK0htKD8J+uYjHtpYRWB+
zN~i!lRn|ibN3rzOl3<V;&pa1c!`eU1uc(}Xf#DP=wVnYtzdmp=a0pEZzQitZLs(=|
z;Do{n!4u6oIPM4tP3M@%F+*d4;Y{rX6&n~AR&Vgw5xCL&fZzeg1BDkPJueG*b+Ue7
zW8e~*P<)9~3a-1J1no~acsp`tcwNwNzrx{hhl8hw?E#PA6sPH)6FsN<P4rt}dRavC
zB9CT+>jyRlUZDw2on8&D4|w<{h=Le*SU5YhCZx`YzQUq;gIl1%2a?{6;faTVfdQW2
zx%fa$9z>I@h7p!ZK&`&X=Xhip87D7Pmw~r%YFTR-vp6UF%gHh_gQO>S$?-r~yzDTo
zwHzf7xdU>dlY138CV!NZVuA?q!du?8TqXRI`PgJ8>+o^~)pDmW*Knt>*no-*cylR*
zwT2tk9%W=mXQ<^V0kzY>DZGXSW}6Gc1m@WD3=FlrDD4eJMqzt+Rzz-urLfoVB1}bY
zO4TsdASZc{$7Ol>N~FOSf|_qK;MNdhmK=yyWXw{4$sp(wP)7;O%~ArXQiX>cA~PWx
zgee^9j5UnVww4M=y)<JMs7ME!fFLA6wrhY}WZIyHo;W-h7crJd!PI5xfK*KWrzp&V
zYN#YmLnj}U6Petr$jJycmXUMvYDH;&u5?CpZ!j}v8GsFEQV|BVv$$ZPp9R*aiR2zg
zOR$C~%M|7=1Pw_9%*6Y+UP)Dyw}vH!yNZi}A)ASTVFF{(ZEy<Imy>4ZhnUBM=y7n=
zOb$>M<w@ZI7rHVv>{%L<52}c8fC8|F1Hxeir81~GUL<vjjKZAUaD|GD!tC52l_i#w
z6ICR^*?{8IAdV$9fZeFbC@jbavb%&27Ft<Wpa5sI2K99KYM4MJbQJ>w1K1aejKXYu
zAjd(y0r9RK+$gX&a2W+I>_OoPHwx@es8Ns*ff#jAMMMCfqfkvcs3IZ<auZVW=C9#E
z%4*<%oNS}Q#|UQG@}{t4yD%^mGfiObnO|ZH^LPypEOFHelz@tQutW`KjR4Fn<g!Bt
zx$Kz0+;fQqsl$~b05gh_VIWcsGbsIm(iJ#vVd2dQi8`1ZII`e!khlQJp{H9BP~g^Z
z)o{eai*j(<l$gw~%su&+3io6wKCa1PY$Bi{RRcL#XkEyOVoBDJtVvQ<my)X$L`pnD
zHG;4R$BaFG<UEfpwAetQ1r8r*-eu;4IH?})*jgbZ+k_D*1xZu{#SL6Ff;FsZpr(+h
zUy&84LE^^1zyR$=PO*nHj7kwgE%sG{nI);<k+wXAywcpH)FLbMB6pBZ4-nxABH)cE
zNb3sJu&NTvOD$I@$S+O>59_6*mZT<6-k>GJs4@AsmMU{ze);5L?W}rDg(6TR`xZ-b
zKB(K6lA2loX0fEFmIU77NKY*Zi7$^&Es6wL1FBZQEvs9sRjEb!#kW|C67$ki5&eZC
zP)E5a0Hih$L<E6|U=RVS=ZZo=jV=x^e^0-Vcu$w2PzDAUhN3W#bU28J01>_*A__$K
zgNR}f0ZOd5*oqSKQu1@bV`oL6kuFW)q9l-P>||?Qb&gwXh4GnrnI)6cb!8pE0Sn6a
zMWCQ8(gE4am{}ALqIr@_i;7b7O5#CbQv@1?1384@78^)+d|@%DeK6TjPuK<06Xt`B
zbfyO6B<7_Sbu%z9yaP4z8dyO?cVD;|SmkD<USUzW!z<Vy+ZhY#PR_Qu%&XDh_F!_8
zp1RQ&5e6x_`LZ)*7nClqTUd8l((D3@*d0*kL+*lt(G?csI~+Xyyq&xk#568*Xf`;0
z;hM~^uORS&jX_Yj!SfEU&;;}TxDLk2Zu%<qJc6L^U$;*O;~fs}>m0HdIb>&~&d;8i
zy~1?8)k>=^McXSjRvcj5Tz!SZ`3?tfN7@GlM$QPv38mAkCRR<an^?CXb$Q;xyo=(d
z*To$#iaTBxcfTm^euD9`xYq};$~%H0)5Rx>Ul&xlD5$a^ba~XmsO51B<2JZ%_uJ@q
zQC8LYx~k7bRiDd(zSjkVFA4@<2noF`81|VNWXt4>`i}L>s}@#)ytyHDd)~&pi|Ve|
z)dMf82VRy8y225BgPW(Ld<J7@Rkz&@X$4TH`?9olhtC~x>FKo-YZt^^7B}kf_`ty+
zAktsoS-(K}62IyVE#vE2_7}D64=`TTa+<(#T|nicfXafvivsEo_=Pc))nicy?o`iU
zol$XxN8^To=ya}$T$6b^90?B|d|+cxH@L25eNoMNhvP*xyCXu!B@aqocMQDf7<kz+
z_(E9XMaRU8YKa{l*Lf5!@+d42yjahpjLlgPR~-pD9(geGx@+h~*U-zZ;TNLPF1n^&
z)Jh}hE8?~DsNdlcydb2t!s7~$>Er_j;*%d3sMmjCV-OXe?lIA0M&M-c4%Zvv(jBf7
z6uUh>Br-Da$$Uv+WMJjGz@>PFMd^XC#B{rfc9(^f8+`8Y3-{M{*3O8z%&*np@qvwj
zgQvZ|v3`c~C3e{xO4`?z%r7dL?_j*BWYxiOokQj#hs=z?iyZO~*tyY^iDFeZIl?eQ
zq(fx_>x7CcEDE5U#C3to<O+-F<okyHK6iw~rt41BUBJ4WYa!QizJ+`%O4rw|th=aS
zcU{5jqJr0DA@2rna4ynZVSR<g`~w$*h$IfDPp&tVX1>G1IXTJ5(u|v@BYTF~1#ZO)
zEQ;Wcji%@=&VtOm{N&_}%zSX^b&EBxG`FA<T-HHKx5~*<#;VE+rA6_@nYjfy@Nqu9
z(vr*^#E4fB%VcX~#d=VI1t|+tK_wKZNGSqMfglPmaH$LKF@j5!l?-5)K#Cj=o80`A
z(wtPgqDc&(Za4!&F{o(#z|6?V_?d%&QQ|g(&}|0p+YH=y8I12TnB8Zvf6O3on}Pom
zizuVe2R$xhMwSl}9E>cVghUvnKInj1{EWOGl)$VIMn<hqIZP6af}cGYU6~k_z*YkQ
DQ<vgF
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
GIT binary patch
delta 20
bcmaE7^3H_&G%qg~0|Ntt?=RDh+{sb^Md=0f
delta 20
bcmaE7^3H_&G%qg~0|NuYHb<?E+{sb^L^}nU
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index 92bd177..df859fa 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
--
GitLab