Skip to content
Snippets Groups Projects
Commit 06493a2b authored by Joschua Gosda's avatar Joschua Gosda
Browse files

outsourced code that is reused to function within /data/get_data.py

parent e6bbac10
Branches
No related tags found
1 merge request!2Cleanup
...@@ -14,8 +14,8 @@ the IK does not take the real joint angles and velocities into account but suppo ...@@ -14,8 +14,8 @@ the IK does not take the real joint angles and velocities into account but suppo
UDP_PORT_LEFT = 6510 UDP_PORT_LEFT = 6510
UDP_PORT_RIGHT = 6511 UDP_PORT_RIGHT = 6511
comp_conf_left = get_desJoints_L comp_conf_left = get_desJoints_L()
comp_conf_right = get_desJoints_R comp_conf_right = get_desJoints_R()
rate = 80 rate = 80
......
...@@ -4,6 +4,7 @@ import time ...@@ -4,6 +4,7 @@ import time
import numpy as np import numpy as np
from abb_egm_pyclient import EGMClient from abb_egm_pyclient import EGMClient
from data.get_data import get_desJoints_L, get_desJoints_R from data.get_data import get_desJoints_L, get_desJoints_R
from libs.invKin import gpm
''' '''
Before running this script make sure that the starting pose of the robot (either real one or in RS) match the Before running this script make sure that the starting pose of the robot (either real one or in RS) match the
...@@ -14,8 +15,8 @@ joint positions at the robot into account ...@@ -14,8 +15,8 @@ joint positions at the robot into account
UDP_PORT_LEFT = 6510 UDP_PORT_LEFT = 6510
UDP_PORT_RIGHT = 6511 UDP_PORT_RIGHT = 6511
comp_conf_left = get_desJoints_L comp_conf_left = get_desJoints_L()
comp_conf_right = get_desJoints_R comp_conf_right = get_desJoints_R()
rate = 80 rate = 80
# rearrange the logic joint values to the strange convention abb has # rearrange the logic joint values to the strange convention abb has
......
import numpy as np import numpy as np
import copy
from enum import Enum
def get_trajectory(): def get_trajectory():
try: try:
...@@ -27,3 +29,25 @@ def get_desJoints_R(): ...@@ -27,3 +29,25 @@ def get_desJoints_R():
except: except:
print('It seems like you need to run the script first that generate the requested data') print('It seems like you need to run the script first that generate the requested data')
return comp_conf_right return comp_conf_right
def transform2yumi_workspace(p1, v1, p2, v2, phi_delta, dphi):
for m in [p1, v1, p2, v2, phi_delta, dphi]:
copy_col = copy.copy(m[:, 2]) # copy z
m[:, 2] = m[:, 1] # shift y to z
m[:, 1] = -copy_col # copy z to y
return p1, v1, p2, v2, phi_delta, dphi
def place_trajectory(start_des, p1, p2):
p_start = p1[0, :]
offset = start_des - p_start
# apply offset to all position coordinates
for i in range(len(p1[:,0])):
p1[i,:] = p1[i,:] + offset
p2[i,:] = p2[i,:] + offset
return p1, p2
class Yumi(Enum):
RIGHT = False
LEFT = True
\ No newline at end of file
from enum import Enum
from matplotlib import pyplot as plt from matplotlib import pyplot as plt
import numpy as np import numpy as np
import copy import copy
from libs.invKin import gpm from libs.invKin import gpm
from data.get_data import get_trajectory from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, Yumi
class Yumi(Enum):
RIGHT = False
LEFT = True
# READ IN THE TRAJECTORY # READ IN THE TRAJECTORY
# define staring postition in workspace for left arm - found by try and error in RS # get the data in the yumi workspace
desp_start = np.array([0.3, 0.2, 0.2])
# import the preprocessing data
#data = np.load('./taskspace_placement/traj_data.npy')
# for each var x | y | z
#p1 = data[:, 0:3]
#v1 = data[:, 3:6]
#p2 = data[:, 6:9]
#v2 = data[:, 9:12]
#phi_delta = data[:, 12:15]
#dphi = data[:, 15:18]
p1, v1, p2, v2, phi_delta, dphi = get_trajectory() p1, v1, p2, v2, phi_delta, dphi = get_trajectory()
p1, v1, p2, v2, phi_delta, dphi = transform2yumi_workspace(p1, v1, p2, v2, phi_delta, dphi)
# coordinates system differ and need to be synchronized - y -> z, x -> x, z -> -y # define staring postition in workspace for left arm - found by try and error in RS
for m in [p1, v1, p2, v2, phi_delta, dphi]: p1_start_des = np.array([0.3, 0.2, 0.2])
copy_col = copy.copy(m[:, 2]) # copy z p1, p2 = place_trajectory(p1_start_des, p1, p2)
m[:, 2] = m[:, 1] # shift y to z
m[:, 1] = -copy_col # copy z to y
# place the trajectories within the workspace of the robot
# read the coordinates of p1 (that refers to the left arm) and modify it to match to desired
# starting postion
p_start = p1[0, :]
offset = desp_start - p_start
# apply offset to all position coordinates
for i in range(len(p1[:,0])):
p1[i,:] = p1[i,:] + offset
p2[i,:] = p2[i,:] + offset
# START CONFIGURATION FOR THE LEFT ARM # START CONFIGURATION FOR THE LEFT ARM
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment