#, this is the main file for the drawing your drawings with a dmp with force feedback
# TODO:
# 2. delete the unnecessary comments
# 8. add some code to pick up the marker from a prespecified location
# 10. write documentation as you go along
# BIG TODO:
# DOES NOT WORK WITH SPEED SLIDER =/= 1.0!!!!!!!!!!!!!!!!!!!!!!!!!!!
# realistic solution: keep it at 1.0 at all times. make acceleration and/or
# speeds small
import pinocchio as pin
import numpy as np
import matplotlib.pyplot as plt
import matplotlib
import copy
import argparse
import time
from functools import partial
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
# TODO merge these clik files as well, they don't deserve to be separate
# TODO but first you need to clean up clik.py as specified there
from ur_simple_control.clik.clik_point_to_point import getClikController, moveL, moveUntilContact, controlLoopClik, compliantMoveL
from ur_simple_control.clik.clik_trajectory_following import map2DPathTo3DPlane, clikCartesianPathIntoJointPath
from ur_simple_control.managers import ControlLoopManager, RobotManager
from ur_simple_control.util.calib_board_hacks import calibratePlane, getSpeedInDirectionOfN
from ur_simple_control.basics.basics import moveJ
#######################################################################
# arguments #
#######################################################################
def getArgs():
#######################################################################
# generic arguments #
#######################################################################
parser = argparse.ArgumentParser(description='Make a drawing on screen,\
watch the robot do it on the whiteboard.',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
# TODO this one won't really work but let's leave it here for the future
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
help="whether you are running the UR simulator. \
NOTE: doesn't actually work because it's not a physics simulator", \
default=False)
parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
help="whether you want to just integrate with pinocchio.\
NOTE: doesn't actually work because it's not a physics simulator", \
default=False)
parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction,
help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction,
help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
help="whether you're using the gripper", default=False)
parser.add_argument('--acceleration', type=float, \
help="robot's joints acceleration. scalar positive constant, \
max 1.7, and default 0.4. \
BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
TODO: check what this means", default=0.3)
parser.add_argument('--speed-slider', type=float,\
help="cap robot's speed with the speed slider \
to something between 0 and 1, 1.0 by default because for dmp. \
BE CAREFUL WITH THIS.", default=1.0)
# TODO: make this optional
parser.add_argument('--max-iterations', type=int, \
help="maximum allowable iteration number (it runs at 500Hz)", default=50000)
parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
help="print some debug info", default=False)
#######################################################################
# your controller specific arguments #
#######################################################################
# not applicable here, but leaving it in the case it becomes applicable
# it's also in the robot manager even though it shouldn't be
parser.add_argument('--past-window-size', type=int, \
help="how many timesteps of past data you want to save", default=5)
parser.add_argument('--goal-error', type=float, \
help="the final position error you are happy with. NOTE: not used here", \
default=1e-3)
# TODO: test the interaction of this and the overall demo
parser.add_argument('--tikhonov-damp', type=float, \
help="damping scalar in tikhonov regularization.\
This is used when generating the joint trajectory from the drawing.", \
default=1e-2)
# TODO add the rest
parser.add_argument('--clik-controller', type=str, \
help="select which click algorithm you want", \
default='dampedPseudoinverse', \
choices=['dampedPseudoinverse', 'jacobianTranspose'])
# maybe you want to scale the control signal
parser.add_argument('--controller-speed-scaling', type=float, \
default='1.0', help='not actually_used atm')
parser.add_argument('--contact-detecting-force', type=float, \
default='1.3', help='the force used to detect contact (collision) in the moveUntilContact function')
#############################
# dmp specific arguments #
#############################
parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
help="whether you want to use temporal coupling", default=True)
parser.add_argument('--kp', type=float, \
help="proportial control constant for position errors", \
default=1.0)
parser.add_argument('--tau0', type=float, \
help="total time needed for trajectory. if you use temporal coupling,\
you can still follow the path even if it's too fast", \
default=5)
parser.add_argument('--gamma-nominal', type=float, \
help="positive constant for tuning temporal coupling: the higher,\
the fast the return rate to nominal tau", \
default=1.0)
parser.add_argument('--gamma-a', type=float, \
help="positive constant for tuning temporal coupling, potential term", \
default=0.5)
parser.add_argument('--eps-tc', type=float, \
help="temporal coupling term, should be small", \
default=0.001)
parser.add_argument('--alpha', type=float, \
help="force feedback proportional coefficient", \
default=0.007)
#default=0.05)
parser.add_argument('--beta', type=float, \
help="low-pass filter beta parameter", \
default=0.01)
# TODO add low pass filtering and make it's parameters arguments too
#######################################################################
# task specific arguments #
#######################################################################
# TODO measure this for the new board
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)
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)
parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \
help="are you wiping the board (default is no because you're writing)", \
default=False)
# 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('--draw-new', action=argparse.BooleanOptionalAction, \
help="whether draw a new picture, or use the saved path path_in_pixels.csv", default=True)
parser.add_argument('--pick-up-marker', action=argparse.BooleanOptionalAction, \
help="""
whether the robot should pick up the marker.
NOTE: THIS IS FROM A PREDEFINED LOCATION.
""", default=False)
parser.add_argument('--find-marker-offset', action=argparse.BooleanOptionalAction, \
help="""
whether you want to do find marker offset (recalculate TCP
based on the marker""", default=False)
parser.add_argument('--n-calibration-tests', type=int, \
help="number of calibration tests you want to run", default=10)
parser.add_argument('--clik-goal-error', type=float, \
help="the clik error you are happy with", default=1e-2)
parser.add_argument('--max-init-clik-iterations', type=int, \
help="number of max clik iterations to get to the first point", default=10000)
parser.add_argument('--max-running-clik-iterations', type=int, \
help="number of max clik iterations between path points", default=1000)
args = parser.parse_args()
if args.gripper and args.simulation:
raise NotImplementedError('Did not figure out how to put the gripper in \
the simulation yet, sorry :/ . You can have only 1 these flags right now')
if args.calibration or args.pick_up_marker:
args.gripper = True
return args
# go and pick up the marker
# marker position:
"""
R =
-0.73032 -0.682357 0.0319574
-0.679774 0.730578 0.0645244
-0.067376 0.0253997 -0.997404
p = 0.574534 -0.508343 0.249325
pin: 0.5745 -0.5083 0.2493 3.1161 0.0674 -2.392
ur5: 0.5796 -0.4982 0.0927 -1.1314 2.8725 0.0747
q: -0.5586 -2.3103 -1.1638 -1.2468 1.6492 0.262 0.0 0.0
"""
def getMarker(args, robot, rotation_matrix, translation_vector):
# init position is the safe one above starting point of the board
above_starting_write_point = pin.SE3.Identity()
# 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("Mtool", robot.getMtool())
Tinit = robot.getMtool().copy()
q_init = robot.getQ()
#exit()
#moveJ(args, robot, q_init)
compliantMoveL(args, robot, above_starting_write_point)
# goal position, predefined
Tgoal = pin.SE3()
Tgoal.rotation = np.array([
[ -0.73032, -0.682357, 0.0319574],
[-0.679774, 0.730578, 0.0645244],
[-0.067376, 0.0253997, -0.997404]])
Tgoal.translation = np.array([0.574534, -0.508343, 0.249325])
# slighly up from goal to not hit marker weirdly
TgoalUP = Tgoal.copy()
TgoalUP.translation += np.array([0,0,0.2])
#TgoalUP.translation += np.array([0,0,0.3])
# instantiate controller
#clik_controller = getClikController(args)
#controlLoop = partial(controlLoopClik, robot, clik_controller)
#log_item = {
# 'qs' : np.zeros(robot.model.nq),
# 'dqs' : np.zeros(robot.model.nq),
# }
#loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
print("going to above marker")
robot.Mgoal = TgoalUP.copy()
compliantMoveL(args, robot, TgoalUP.copy())
#moveL(args, robot, TgoalUP.copy())
#log_dict, final_iteration = loop_manager.run()
print("going down to marker")
robot.Mgoal = Tgoal.copy()
compliantMoveL(args, robot, Tgoal.copy())
#moveL(args, robot, Tgoal.copy())
#log_dict, final_iteration = loop_manager.run()
print("picking up marker")
robot.gripper.move(255,255,255)
time.sleep(2)
print("going up")
robot.Mgoal = TgoalUP.copy()
compliantMoveL(args, robot, TgoalUP.copy())
#moveL(args, robot, TgoalUP.copy())
#log_dict, final_iteration = loop_manager.run()
print("going back")
# TODO: this HAS to be a movej
# in fact all of them should be
robot.Mgoal = Tinit.copy()
#compliantMoveL(args, robot, Tinit.copy())
moveJ(args, robot, q_init)
#moveL(args, robot, Tinit.copy())
#log_dict, final_iteration = loop_manager.run()
print("got back")
# robot.stopHandler(None, None)
# robot.stopHandler(None, None)
# robot.stopHandler(None, None)
# print("bye")
# exit()
"""
findMarkerOffset
---------------
This relies on having the correct orientation of the plane
and the correct translation vector for top-left corner.
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
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("Mtool", robot.getMtool())
#exit()
#moveJ(args, robot, q_init)
compliantMoveL(args, robot, above_starting_write_point)
#Tinit = robot.getMtool().copy()
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[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.getMtool().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
#######################################################################
# control loop #
#######################################################################
# feedforward velocity, feedback position and force for impedance
# TODO: actually write this out
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):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
log_item = {}
dmp.step(robot.dt) # dmp step
# temporal coupling step
tau_dmp = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
dmp.set_tau(tau_dmp)
q = robot.getQ()
Mtool = robot.getMtool()
# TODO look into UR code/api for estimating the same
# based on currents in the joints.
# it's probably worse, but maybe some sensor fusion-type thing
# is actually better, who knows.
# also you probably want to do the fusion of that onto tau (got from J.T @ wrench)
#Z = np.diag(np.array([0.6, 0.6, 1.0, 0.5, 0.5, 0.5]))
#Z = np.diag(np.array([0.6, 1.0, 0.6, 0.5, 0.5, 0.5]))
#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, 2.0, 0.0, 0.0, 0.0]))
# 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)
# rolling average
#wrench = np.average(np.array(past_data['wrench']), axis=0)
# 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]
# 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
mapping = np.zeros((6,6))
mapping[0:3, 0:3] = Mtool.rotation
mapping[3:6, 3:6] = Mtool.rotation
wrench = mapping.T @ wrench
wrench = Z @ wrench
# if i % 100 == 0:
# print(*wrench.round(1))
pin.forwardKinematics(robot.model, robot.data, q)
J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
dq = robot.getQd()[:6].reshape((6,1))
# get joitn
tau = J.T @ wrench
tau = tau[:6].reshape((6,1))
# compute control law:
# - feedforward the velocity and the force reading
# - feedback the position
# TODO: don't use vel for qd, it's confusion (yes, that means changing dmp code too)
# TODO: put this in a controller function for easy swapping (or don't if you won't swap)
# solve this q[:6] bs (clean it up)
vel_cmd = dmp.vel + args.kp * (dmp.pos - q[:6].reshape((6,1))) + args.alpha * tau
robot.sendQd(vel_cmd)
# tau0 is the minimum time needed for dmp
# 500 is the frequency
# so we need tau0 * 500 iterations minimum
if (np.linalg.norm(dmp.vel) < 0.01) and (i > int(args.tau0 * 500)):
breakFlag = True
# immediatelly stop if something weird happened (some non-convergence)
if np.isnan(vel_cmd[0]):
breakFlag = True
# log what you said you'd log
# TODO fix the q6 situation (hide this)
log_item['qs'] = q[:6].reshape((6,))
log_item['dmp_poss'] = dmp.pos.reshape((6,))
log_item['dqs'] = dq.reshape((6,))
log_item['dmp_vels'] = dmp.vel.reshape((6,))
log_item['wrench'] = wrench.reshape((6,))
log_item['tau'] = tau.reshape((6,))
return breakFlag, save_past_dict, log_item
if __name__ == "__main__":
#######################################################################
# software setup #
#######################################################################
args = getArgs()
print(args)
clikController = getClikController(args)
robot = RobotManager(args)
# calibrate FT first
# it's done by default now because it's basically always necessary
#######################################################################
# 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:
# pure evil way to solve a bug that was pure evil
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=',')
# do calibration if specified
if args.calibration:
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()
else:
# TODO: save this somewhere obviously
# 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 = robot.getQ()
#Mtool = robot.getMtool()
#rotation_matrix = np.array([
# [-1, 0, 0],
# [0, 0, -1],
# [0, -1, 0]])
#translation_vector = Mtool.translation.copy()
# 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
if args.pick_up_marker:
getMarker(args, robot, rotation_matrix, translation_vector)
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)
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])
else:
path = path + np.array([0.0, 0.0, -1 * marker_offset + 0.015])
else:
#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.
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
joint_trajectory = clikCartesianPathIntoJointPath(path, args, robot, \
clikController, q_init, rotation_matrix, translation_vector)
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)
# 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
current_pose = robot.getMtool()
# 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.getMtool(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.getMtool())
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)
#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
log_dict, final_iteration = loop_manager.run()
#loop_manager.stopHandler(None, None)
mtool = robot.getMtool()
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)
#plotFromDict(log_dict, args)
loop_manager.stopHandler(None, None)
#robot.stopHandler(None, None)
#robot.stopHandler(None, None)
# plot results
#plotFromDict(log_dict, args)
# TODO: add some math to analyze path precision