diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/main_program.py b/python/abb_egm_pyclient/abb_egm_pyclient/main_program.py index 3a76d5caa82509bf096690b1972cae5a4f978a7e..efc332e860b882289fbaaab0d8fec554e70034de 100644 --- a/python/abb_egm_pyclient/abb_egm_pyclient/main_program.py +++ b/python/abb_egm_pyclient/abb_egm_pyclient/main_program.py @@ -15,8 +15,86 @@ reading force sensor data. ''' + DEFAULT_UDP_PORT = 6510 +comp_conf = np.load('./abb_egm_pyclient/abb_egm_pyclient/desJointAngles_left.npy') +rate = 80 + +def logic2abb(joint_values): + return joint_values[[0, 1, 3, 4, 5, 6, 2]] + +def abb2logic(joint_values): + return joint_values[[0, 1, 5, 2, 3, 4]] + + + +#mylist = [1, 2, 3, 4, 5, 6] +#mylist.insert(2, 7) +#print("mylist", mylist) + +egm_client = EGMClient(port=DEFAULT_UDP_PORT) + +# get the current values of joints +robot_msg = egm_client.receive_msg() +conf: Sequence[float] = robot_msg.feedBack.joints.joints # get ABB's standard six joint values +joint7 = robot_msg.feedBack.externalJoints.joints[0] +conf.insert(2, joint7) +conf_yumi = np.array(conf) + + +conf_des = comp_conf[0, :] +print("des conf", conf_des) +print("arranged", logic2abb(conf_des)) + +print(conf_yumi) +print(conf_des) + + +# check if real joint value match the computed starting joint positions +#conf_diff = np.abs(logic2abb(conf_des) - conf_yumi) +#condition = np.array([0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) # equals 2 degrees +#assert (conf_diff-condition).all(), 'Starting configurations are not synchronized' + + +for n in range(len(comp_conf[:,0])): + sTime = time.time() + robot_msg = egm_client.receive_msg() + conf: Sequence[float] = robot_msg.feedBack.joints.joints + joint7 = robot_msg.feedBack.externalJoints.joints[0] + conf.insert(2, joint7) + conf_yumi = np.array(conf) + + print(f"Current configuration {conf_yumi}") + + des_conf = np.degrees(comp_conf[n, :]) + egm_client.send_planned_configuration(logic2abb(des_conf)) + + # take the execution time into account that loops stays iterating with the rate frequency + # get more important the higher rate becomes like 100-250 + sleeping_time = 1/rate - (time.time()-sTime) + time.sleep(sleeping_time) + +# after all messages are sent out, wait for 10 sec and check if positions converged +n = 0 +while n < 10: + robot_msg = egm_client.receive_msg() + if robot_msg.mciConvergenceMet: + print("Joint positions converged.") + break + else: + print("Waiting for convergence.") + n += 1 + time.sleep(1) +else: + raise TimeoutError(f"Joint positions did not converge after {n} s timeout.") + + + + + +# TODO: check if all devices and neccessary files are online/in the right place +# show if thats the case and ask user if he wants to start control # TODO: read the data from g-code and transform it into the global frame of yumi # make sure to set the first configuration as the fine point in robot studio, so that they diff --git a/python/preprocessing/genPaths.py b/python/preprocessing/genPaths.py index 348f4ca70a0cc0c491f7631bae21df2f0d16bb1f..f843b4022ee2bd7005e261643bf56c44e24f3fe5 100644 --- a/python/preprocessing/genPaths.py +++ b/python/preprocessing/genPaths.py @@ -211,4 +211,4 @@ plt.show() # make data ready for export traj_data = np.hstack((p1m, v1, p2m, v2, ang, odot)) -np.save('traj_data', traj_data) \ No newline at end of file +np.save('./taskspace_placement/traj_data', traj_data) \ No newline at end of file diff --git a/python/taskspace_placement/computeJoints_left.py b/python/taskspace_placement/computeJoints_left.py index 9798f2be91f01d2cda0e534f84ee05116f66ae2b..8d539fd23f0ff2529c5b59ebf179f1ddf7650801 100644 --- a/python/taskspace_placement/computeJoints_left.py +++ b/python/taskspace_placement/computeJoints_left.py @@ -14,7 +14,7 @@ class Yumi(Enum): desp_start = np.array([0.3, 0.2, 0.2]) # import the preprocessing data -data = np.load('/home/joschua/Coding/forceControl/master-project/python/taskspace_placement/traj_data.npy') +data = np.load('./traj_data.npy') # for each var x | y | z p1 = data[:, 0:3] v1 = data[:, 3:6] @@ -121,5 +121,5 @@ plt.legend() plt.title('euler angles over trajectories') plt.show() -np.save('desJointAngles_left', desJointAngles) +np.save('./abb_egm_pyclient/abb_egm_pyclient/desJointAngles_left', desJointAngles)