diff --git a/c++/main.cpp b/c++/main.cpp
index 76f27358d9224f7b9088683bd651419e3d90df95..31c5cfdc0f53377e1c3eba0a4c37873fbd87a5c1 100644
--- a/c++/main.cpp
+++ b/c++/main.cpp
@@ -7,12 +7,14 @@
int main(int, char**) {
- enum yumi_arm{YUMI_LEFT, YUMI_RIGHT};
+ enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
// Is Values
Eigen::Matrix<double, 6, 1> actualPosition;
Eigen::Matrix<double, 7, 1> jointAngles;
- jointAngles << 90.0, 20.0, -25.0, 48.0, -137.0, 70.0, -25.0; // start position left arm, angles from RS
+ //jointAngles << 90.0, 20.0, -25.0, 48.0, -137.0, 70.0, -25.0; // start position left arm, angles from RS
+ jointAngles << -110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0; // start position right arm, angles from RS
+ //jointAngles << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
jointAngles *= rl::math::DEG2RAD;
Eigen::Matrix<double, 7, 1> jointVelocity;
diff --git a/c++/src/urdf/yumi_right.urdf b/c++/src/urdf/yumi_right.urdf
index cd1bc662ea5040569cf419ded476980d0cd47160..1c87028f6d26ac89b34fc1d6c710f0051005b146 100644
--- a/c++/src/urdf/yumi_right.urdf
+++ b/c++/src/urdf/yumi_right.urdf
@@ -118,7 +118,7 @@
<joint name="yumi_joint_6_r" type="revolute">
<parent link="yumi_link_6_r"/>
<child link="yumi_link_7_r"/>
- <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0283 0.0372 -0.0005"/>
+ <origin rpy="1.57079632679 0.0 3.14159" xyz="0.0273 0.1165 -0.0007"/>
<axis xyz="0 0 1"/>
<limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
<dynamics damping="0.5"/>
diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/main_program.py b/python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py
similarity index 64%
rename from python/abb_egm_pyclient/abb_egm_pyclient/main_program.py
rename to python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py
index efc332e860b882289fbaaab0d8fec554e70034de..64f02a4e9cf9b147afcc2d18c86a2199b2e47cbc 100644
--- a/python/abb_egm_pyclient/abb_egm_pyclient/main_program.py
+++ b/python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py
@@ -16,38 +16,28 @@ reading force sensor data.
'''
-DEFAULT_UDP_PORT = 6510
-comp_conf = np.load('./abb_egm_pyclient/abb_egm_pyclient/desJointAngles_left.npy')
+UDP_PORT_LEFT = 6510
+UDP_PORT_RIGHT = 6511
+comp_conf_left = np.load('./abb_egm_pyclient/abb_egm_pyclient/feedforward/desJointAngles_left.npy')
+comp_conf_right = np.load('./abb_egm_pyclient/abb_egm_pyclient/feedforward/desJointAngles_right.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]]
+egm_client_left = EGMClient(port=UDP_PORT_LEFT)
+egm_client_right = EGMClient(port=UDP_PORT_RIGHT)
-
-#mylist = [1, 2, 3, 4, 5, 6]
-#mylist.insert(2, 7)
-#print("mylist", mylist)
-
-egm_client = EGMClient(port=DEFAULT_UDP_PORT)
-
+print("waiting to receive msg")
# 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)
+robot_msg_L = egm_client_left.receive_msg()
+robot_msg_R = egm_client_right.receive_msg()
-
-conf_des = comp_conf[0, :]
-print("des conf", conf_des)
-print("arranged", logic2abb(conf_des))
-
-print(conf_yumi)
-print(conf_des)
+conf_L: Sequence[float] = robot_msg_L.feedBack.joints.joints # get ABB's standard six joint values
+joint7 = robot_msg_L.feedBack.externalJoints.joints[0]
+conf_L.insert(2, joint7)
+conf_yumi_L = np.array(conf_L)
# check if real joint value match the computed starting joint positions
@@ -56,18 +46,23 @@ print(conf_des)
#assert (conf_diff-condition).all(), 'Starting configurations are not synchronized'
-for n in range(len(comp_conf[:,0])):
+for n in range(len(comp_conf_left[:,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}")
+ # do stuff for the left arm
+ robot_msg_L = egm_client_left.receive_msg()
+ conf_L: Sequence[float] = robot_msg_L.feedBack.joints.joints
+ joint7 = robot_msg_L.feedBack.externalJoints.joints[0]
+ conf_L.insert(2, joint7)
+ conf_yumi_L = np.array(conf_L)
+
+ print(f"Current configuration of left arm {conf_yumi_L}")
- des_conf = np.degrees(comp_conf[n, :])
- egm_client.send_planned_configuration(logic2abb(des_conf))
+ # send out
+ des_conf_L = np.degrees(comp_conf_left[n, :])
+ des_conf_R = np.degrees(comp_conf_right[n, :])
+ egm_client_left.send_planned_configuration(logic2abb(des_conf_L))
+ egm_client_right.send_planned_configuration(logic2abb(des_conf_R))
# take the execution time into account that loops stays iterating with the rate frequency
# get more important the higher rate becomes like 100-250
@@ -77,7 +72,7 @@ for n in range(len(comp_conf[:,0])):
# 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()
+ robot_msg = egm_client_left.receive_msg()
if robot_msg.mciConvergenceMet:
print("Joint positions converged.")
break
diff --git a/python/preprocessing/genPaths.py b/python/preprocessing/genPaths.py
index f843b4022ee2bd7005e261643bf56c44e24f3fe5..348f4ca70a0cc0c491f7631bae21df2f0d16bb1f 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('./taskspace_placement/traj_data', traj_data)
\ No newline at end of file
+np.save('traj_data', traj_data)
\ No newline at end of file
diff --git a/python/taskspace_placement/computeJoints.py b/python/taskspace_placement/computeJoints.py
new file mode 100644
index 0000000000000000000000000000000000000000..2fcb9f69edb0f32db103e5e914599b792a82921e
--- /dev/null
+++ b/python/taskspace_placement/computeJoints.py
@@ -0,0 +1,180 @@
+from enum import Enum
+from matplotlib import pyplot as plt
+
+import numpy as np
+import copy
+import invKin
+
+class Yumi(Enum):
+ RIGHT = False
+ LEFT = True
+
+# READ IN THE TRAJECTORY
+# define staring postition in workspace for left arm - found by try and error in RS
+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]
+
+# coordinates system differ and need to be synchronized - y -> z, x -> x, z -> -y
+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
+
+# 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
+# set the joint angles that map to the desired start position - read from RobotStudio
+jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, -74.21]) * np.pi/180.0 #show good manipulability index in RS
+#jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, -74.21]) * np.pi/180.0 #show good manipulability index in RS
+#jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, 15.79]) * np.pi/180.0
+# initial jointVelocites are zero
+jointVelocities = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+dt = 0.0125
+
+
+phi_const_left = np.array([90.0, 180.0, 90.0]) * np.pi/180.0 # values that FK computed, keep angle the same for now, just care about positions
+dphi_const = np.array([0.0, 0.0, 0.0]) * np.pi/180.0
+
+# build the final angles for the orientation of end effector - don't use it for now
+phi_base_left = np.zeros((len(p1[:,0]),3)) + phi_const_left
+phi_total_left = phi_base_left + phi_delta
+
+# create arrays to store values of loop
+desJointAngles_left = np.zeros((len(p1[:,0]),7))
+computedPose_left = np.zeros((len(p1[:,0]),6))
+error_left = np.zeros((len(p1[:,0]),6))
+
+
+
+# loop for the left arm
+for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_total_left, dphi)): # loop through all the desired position of left arm
+ desPose = np.concatenate((pos, phi), axis=0)
+ desVelocities = np.concatenate((vel, phi_dot), axis=0)
+ # call the c++ egm function, return joint values and resulting pose
+ result = invKin.gpm(desPose, desVelocities, jointAngles, jointVelocities, Yumi.LEFT.value)
+ desJointAngles_left[index,:] = result[0] # computed joint values from IK
+ computedPose_left[index, :] = result[1] # resulting pose with joint values from IK
+ if index > 0:
+ jointVelocities = (desJointAngles_left[index, :] - desJointAngles_left[index-1, :])/dt # only true in the ideal case where result of ik matches the desired pose
+ #print('IK joints:', result[0])
+ #print('IK resulting pose', result[1])
+ print('\n error', desPose - result[1])
+ error_left[index, :] = desPose - result[1]
+ jointAngles = result[0]
+
+
+phi_const_right = np.array([90.0, 0.0, 90.0]) * np.pi/180.0 # values that FK computed, keep angle the same for now, just care about positions
+dphi_const = np.array([0.0, 0.0, 0.0]) * np.pi/180.0
+
+# build the final angles for the orientation of end effector - don't use it for now
+phi_base_right = np.zeros((len(p1[:,0]),3)) + phi_const_right
+phi_total_right = phi_base_right + phi_delta
+
+desJointAngles_right = np.zeros((len(p1[:,0]),7))
+computedPose_right = np.zeros((len(p1[:,0]),6))
+error_right = np.zeros((len(p1[:,0]),6))
+
+jointAngles = np.array([-110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0]) * np.pi/180.0
+
+
+# loop for the right arm
+for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_total_right, dphi)): # loop through all the desired position of left arm
+ desPose = np.concatenate((pos, phi), axis=0)
+ desVelocities = np.concatenate((vel, phi_dot), axis=0)
+ result = invKin.gpm(desPose, desVelocities, jointAngles, jointVelocities, Yumi.RIGHT.value)
+ desJointAngles_right[index,:] = result[0]
+ computedPose_right[index, :] = result[1]
+ if index > 0:
+ jointVelocities = (desJointAngles_left[index, :] - desJointAngles_left[index-1, :])/dt
+ print('\n error', desPose - result[1])
+ error_right[index, :] = desPose - result[1]
+ jointAngles = result[0]
+
+# see development of joint values
+fig = plt.figure()
+plt.plot(desJointAngles_left[:,0], label='joint1')
+plt.plot(desJointAngles_left[:,1], label='joint2')
+plt.plot(desJointAngles_left[:,2], label='joint3')
+plt.plot(desJointAngles_left[:,3], label='joint4')
+plt.plot(desJointAngles_left[:,4], label='joint5')
+plt.plot(desJointAngles_left[:,5], label='joint6')
+plt.plot(desJointAngles_left[:,6], label='joint7')
+plt.title('joint values over samples, left arm')
+plt.legend()
+plt.show()
+
+# error
+fig = plt.figure()
+plt.plot(error_left[:,0], label='x')
+plt.plot(error_left[:,1], label='y')
+plt.plot(error_left[:,2], label='z')
+plt.plot(error_left[:,3], label='rx')
+plt.plot(error_left[:,4], label='ry')
+plt.plot(error_left[:,5], label='rz')
+plt.legend()
+plt.title('errors left')
+plt.show()
+
+# show trajectory in workspace of yumi
+fig = plt.figure()
+plt.plot(p1[:,0], p1[:,2], label='desired profile') # plot z over x
+plt.scatter(computedPose_left[:,0], computedPose_left[:,2], label='resulting pose from IK')
+fig.get_axes()[0].set_xlabel('x axis of yumi')
+fig.get_axes()[0].set_ylabel('z axis of yumi')
+plt.legend()
+plt.title('view on the x-z plane from the right arm side of yumi')
+plt.show()
+
+fig = plt.figure()
+plt.plot(computedPose_left[:,3], label='rx')
+plt.plot(computedPose_left[:,4], label='ry')
+plt.plot(computedPose_left[:,5], label='rz')
+plt.legend()
+plt.title('euler angles over trajectories')
+plt.show()
+
+fig = plt.figure()
+plt.plot(desJointAngles_right[:,0], label='joint1')
+plt.plot(desJointAngles_right[:,1], label='joint2')
+plt.plot(desJointAngles_right[:,2], label='joint3')
+plt.plot(desJointAngles_right[:,3], label='joint4')
+plt.plot(desJointAngles_right[:,4], label='joint5')
+plt.plot(desJointAngles_right[:,5], label='joint6')
+plt.plot(desJointAngles_right[:,6], label='joint7')
+plt.title('joint values over samples, right arm')
+plt.legend()
+plt.show()
+
+fig = plt.figure()
+plt.plot(error_right[:,0], label='x')
+plt.plot(error_right[:,1], label='y')
+plt.plot(error_right[:,2], label='z')
+plt.plot(error_right[:,3], label='rx')
+plt.plot(error_right[:,4], label='ry')
+plt.plot(error_right[:,5], label='rz')
+plt.legend()
+plt.title('errors right')
+plt.show()
+
+np.save('desJointAngles_left', desJointAngles_left)
+np.save('desJointAngles_right', desJointAngles_right)
+
diff --git a/python/taskspace_placement/computeJoints_left.py b/python/taskspace_placement/computeJoints_left.py
deleted file mode 100644
index 8d539fd23f0ff2529c5b59ebf179f1ddf7650801..0000000000000000000000000000000000000000
--- a/python/taskspace_placement/computeJoints_left.py
+++ /dev/null
@@ -1,125 +0,0 @@
-from enum import Enum
-from matplotlib import pyplot as plt
-
-import numpy as np
-import copy
-import invKin
-
-class Yumi(Enum):
- RIGHT = False
- LEFT = True
-
-# READ IN THE TRAJECTORY
-# define staring postition in workspace for left arm - found by try and error in RS
-desp_start = np.array([0.3, 0.2, 0.2])
-
-# import the preprocessing data
-data = np.load('./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]
-
-# coordinates system differ and need to be synchronized - y -> z, x -> x, z -> -y
-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
-
-# 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
-# set the joint angles that map to the desired start position - read from RobotStudio
-jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, -74.21]) * np.pi/180.0 #show good manipulability index in RS
-#jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, -74.21]) * np.pi/180.0 #show good manipulability index in RS
-#jointAngles = np.array([90.48, 17.87, -25.09, 48.0, -137.0, 122.0, 15.79]) * np.pi/180.0
-# initial jointVelocites are zero
-jointVelocities = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
-dt = 0.0125
-
-
-phi_const = np.array([90.0, 180.0, 90.0]) * np.pi/180.0 # values that FK computed, keep angle the same for now, just care about positions
-dphi_const = np.array([0.0, 0.0, 0.0]) * np.pi/180.0
-
-# build the final angles for the orientation of end effector - don't use it for now
-phi_base = np.zeros((len(p1[:,0]),3)) + phi_const
-phi_total = phi_base + phi_delta
-
-# create arrays to store values of loop
-desJointAngles = np.zeros((len(p1[:,0]),7))
-computedPose = np.zeros((len(p1[:,0]),6))
-error = np.zeros((len(p1[:,0]),6))
-
-for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_total, dphi)): # loop through all the desired position of left arm
- desPose = np.concatenate((pos, phi), axis=0) # note that phi_const is used -> same orientation throughout the movement
- desVelocities = np.concatenate((vel, phi_dot), axis=0) # same here
- # call the c++ egm function, return joint values and resulting pose
- result = invKin.gpm(desPose, desVelocities, jointAngles, jointVelocities, Yumi.LEFT.value)
- desJointAngles[index,:] = result[0] # computed joint values from IK
- computedPose[index, :] = result[1] # resulting pose with joint values from IK
- if index > 0:
- jointVelocities = (desJointAngles[index, :] - desJointAngles[index-1, :])/dt # only true in the ideal case where result of ik matches the desired pose
- #print('IK joints:', result[0])
- #print('IK resulting pose', result[1])
- print('\n error', desPose - result[1])
- error[index, :] = desPose - result[1]
- jointAngles = result[0]
-
-# see development of joint values
-fig = plt.figure()
-plt.plot(desJointAngles[:,0], label='joint1')
-plt.plot(desJointAngles[:,1], label='joint2')
-plt.plot(desJointAngles[:,2], label='joint3')
-plt.plot(desJointAngles[:,3], label='joint4')
-plt.plot(desJointAngles[:,4], label='joint5')
-plt.plot(desJointAngles[:,5], label='joint6')
-plt.plot(desJointAngles[:,6], label='joint7')
-plt.title('joint values over samples, left arm')
-plt.legend()
-plt.show()
-
-# error
-fig = plt.figure()
-plt.plot(error[:,0], label='x')
-plt.plot(error[:,1], label='y')
-plt.plot(error[:,2], label='z')
-plt.plot(error[:,3], label='rx')
-plt.plot(error[:,4], label='ry')
-plt.plot(error[:,5], label='rz')
-plt.legend()
-plt.title('errors')
-plt.show()
-
-# show trajectory in workspace of yumi
-fig = plt.figure()
-plt.plot(p1[:,0], p1[:,2], label='desired profile') # plot z over x
-plt.scatter(computedPose[:,0], computedPose[:,2], label='resulting pose from IK')
-fig.get_axes()[0].set_xlabel('x axis of yumi')
-fig.get_axes()[0].set_ylabel('z axis of yumi')
-plt.legend()
-plt.title('view on the x-z plane from the right arm side of yumi')
-plt.show()
-
-fig = plt.figure()
-plt.plot(computedPose[:,3], label='rx')
-plt.plot(computedPose[:,4], label='ry')
-plt.plot(computedPose[:,5], label='rz')
-plt.legend()
-plt.title('euler angles over trajectories')
-plt.show()
-
-np.save('./abb_egm_pyclient/abb_egm_pyclient/desJointAngles_left', desJointAngles)
-