diff --git a/c++/src/gpm.cpp b/c++/src/gpm.cpp
index 514920e97314256fe235ccd891fc32edeece71d7..d3c828c42c24cf0e49a3975f0475401795500cc6 100644
--- a/c++/src/gpm.cpp
+++ b/c++/src/gpm.cpp
@@ -17,6 +17,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
rl::mdl::UrdfFactory factory;
std::string path2urdf;
+ const clock_t t0 = clock();
+
+
if(left_arm){
path2urdf = "/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf";
} else{
@@ -27,6 +30,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
+ const clock_t t1 = clock();
+ std::cout << "time for parsing model: \t" << t1-t0 << std::endl;
+
// forward kinematics for the right arm
kinematic->setPosition(jointAngles);
kinematic->forwardPosition();
@@ -40,6 +46,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
}
}
+ const clock_t t2 = clock();
+ std::cout << "time for copy jacobi: \t" << t2-t1 << std::endl;
+
// check if matrices are the same
//std::cout << "RLJacobian \n" << kinematic->getJacobian() << std::endl;
//std::cout << "myJacobian \n" << J << std::endl;
@@ -52,7 +61,8 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
std::cout << "Joint configuration in degrees: " << jointAngles.transpose() * rl::math::RAD2DEG << std::endl;
std::cout << "FK end-effector position: [m] " << position.transpose() << " orientation [deg] " << orientation.transpose() * rl::math::RAD2DEG << std::endl;
-
+ const clock_t t3 = clock();
+ std::cout << "time for extracting & printing: \t" << t3-t2 << std::endl;
// INVERSE KINEMATICS
// compute translation and orientation error
Eigen::Matrix3d desOrientation;
@@ -102,6 +112,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
std::cout << "effectiveTaskSpaceInput: " << effectiveTaskSpaceInput << std::endl;
+ const clock_t t4 = clock();
+ std::cout << "time for taskspace input: \t" << t4-t3 << std::endl;
+
// COMPUTE CPG GRADIENT
// define min and max values for the joints of Yumi
Eigen::Matrix< double, 7, 1> q_min;
@@ -149,6 +162,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
// add both gradients
Eigen::Matrix<double, 7, 1> nullSpaceGradient = Eigen::Matrix<double, 7, 1>::Zero();
nullSpaceGradient = 0*manipGradient + 0*cpgGradient;
+
+ const clock_t t5 = clock();
+ std::cout << "time for gradients: \t" << t5-t4 << std::endl;
//std::cout << "gradient \n" << nullSpaceGradient << std::endl;
@@ -164,6 +180,9 @@ Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelo
Eigen::Matrix<double, 7, 1> jointAnglesDelta;
jointAnglesDelta << jointVelocities * dt;
+ const clock_t t6 = clock();
+ std::cout << "time for pseudo: \t" << t6-t5 << std::endl;
+
//std::cout << "current qs in DEG \n" << jointAngles* rl::math::RAD2DEG << std::endl;
//std::cout << "delta qs in DEG \n" << jointAnglesDelta * rl::math::RAD2DEG << std::endl;
//std::cout << "next qs in DEG \n" << (jointAngles+jointAnglesDelta)* rl::math::RAD2DEG << std::endl;
diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_offline/send2robot.py b/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_offline/send2robot.py
index eada6a4aadfc7a77f03a34f1a9051bdb6e4011eb..53c20b475149fd505db55b8c2c6eeaa49395595c 100644
--- a/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_offline/send2robot.py
+++ b/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_offline/send2robot.py
@@ -3,7 +3,7 @@ from typing import Sequence
import time
import numpy as np
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, logic2abb
'''
Before running this script make sure that the starting pose of the robot (either real one or in RS) match the
@@ -16,15 +16,8 @@ UDP_PORT_LEFT = 6510
UDP_PORT_RIGHT = 6511
comp_conf_left = get_desJoints_L()
comp_conf_right = get_desJoints_R()
-
-
rate = 80
-# rearrange the logic joint values to the strange convention abb has
-def logic2abb(joint_values):
- return joint_values[[0, 1, 3, 4, 5, 6, 2]]
-
-
egm_client_left = EGMClient(port=UDP_PORT_LEFT)
egm_client_right = EGMClient(port=UDP_PORT_RIGHT)
diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_online/send2robot.py b/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_online/send2robot.py
index 0fe1f46ec4b8a269ac5ffd5bd6d2434605bc8362..0e53abde30651134e04553c822916e5eea143490 100644
--- a/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_online/send2robot.py
+++ b/python/abb_egm_pyclient/abb_egm_pyclient/feedforward_online/send2robot.py
@@ -1,10 +1,12 @@
#!/usr/bin/env python
from typing import Sequence
import time
+import copy
import numpy as np
from abb_egm_pyclient import EGMClient
from data.get_data import get_desJoints_L, get_desJoints_R
from libs.invKin import gpm
+from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, Yumi, logic2abb
'''
Before running this script make sure that the starting pose of the robot (either real one or in RS) match the
@@ -12,52 +14,101 @@ starting pose of the computed joint angles. This script sends desired joint posi
joint positions at the robot into account
'''
+# READ IN THE TRAJECTORY
+# get the data in the yumi workspace
+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)
+# define staring postition in workspace for left arm - found by try and error in RS
+p1_start_des = np.array([0.3, 0.2, 0.2])
+p1, p2 = place_trajectory(p1_start_des, p1, p2)
+
+# setup for UDP connection
UDP_PORT_LEFT = 6510
UDP_PORT_RIGHT = 6511
-comp_conf_left = get_desJoints_L()
-comp_conf_right = get_desJoints_R()
rate = 80
-# rearrange the logic joint values to the strange convention abb has
-def logic2abb(joint_values):
- return joint_values[[0, 1, 3, 4, 5, 6, 2]]
-
+egm_client_L = EGMClient(port=UDP_PORT_LEFT)
+egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
-egm_client_left = EGMClient(port=UDP_PORT_LEFT)
-egm_client_right = EGMClient(port=UDP_PORT_RIGHT)
+jointVelocities_L = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+jointVelocities_R = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
-for n in range(len(comp_conf_left[:,0])):
+for index, (pos1, vel1, pos2, vel2, phi, phi_dot) in enumerate(zip(p1, v1, p2, v2, phi_delta, dphi)):
+
+ t0 = time.time()
- # do stuff for the left arm
- robot_msg_L = egm_client_left.receive_msg()
+ # get the current joints angles for the left arm
+ robot_msg_L = egm_client_L.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)
+ jointAngles_L = np.radians(np.array(conf_L))
- # do stuff for the right arm
- robot_msg_R = egm_client_right.receive_msg()
+ t1 = time.time()
+ ex_time = t1-t0
+ print(f'computation time for msg left: {ex_time}')
+
+ # get the current joints angles for the right arm
+ robot_msg_R = egm_client_R.receive_msg()
conf_R: Sequence[float] = robot_msg_R.feedBack.joints.joints
joint7 = robot_msg_R.feedBack.externalJoints.joints[0]
conf_R.insert(2, joint7)
- conf_yumi_R = np.array(conf_R)
+ jointAngles_R = np.radians(np.array(conf_R))
+
+ t2 = time.time()
+ ex_time = t2-t1
+ print(f'computation time for msg right: {ex_time}')
+
+ # compute the resulting jointVelocities
+ if index > 0:
+ jointVelocities_L = (jointAngles_L - jointAngles_L_old)/rate
+ jointVelocities_R = (jointAngles_R - jointAngles_R_old)/rate
+
+ desPose_L = np.concatenate((pos1, phi), axis=0)
+ desVelocities_L = np.concatenate((vel1, phi_dot), axis=0)
+
+ desPose_R = np.concatenate((pos2, phi), axis=0)
+ desVelocities_R = np.concatenate((vel2, phi_dot), axis=0)
+
+ t3 = time.time()
+ ex_time = t3-t2
+ print(f'computation time for assembling arrays: {ex_time}')
+
+ ik_jointAngles_L, ik_pose_L = gpm(desPose_L, desVelocities_L, jointAngles_L, jointVelocities_L, Yumi.LEFT.value)
+ ik_jointAngles_R, ik_pose_R = gpm(desPose_R, desVelocities_R, jointAngles_R, jointVelocities_R, Yumi.RIGHT.value)
+
+ t4 = time.time()
+ ex_time = t4-t3
+ print(f'computation time for ik: {ex_time}')
+
+ #print(f"Current configuration of left arm {conf_yumi_L}")
+ #print(f"Current configuration of right arm {conf_yumi_R}")
+
+ # transform to degrees as egm wants it
+ des_conf_L = np.degrees(ik_jointAngles_L)
+ des_conf_R = np.degrees(ik_jointAngles_R)
+
+ egm_client_L.send_planned_configuration(logic2abb(des_conf_L))
+ egm_client_R.send_planned_configuration(logic2abb(des_conf_R))
- print(f"Current configuration of left arm {conf_yumi_L}")
- print(f"Current configuration of right arm {conf_yumi_R}")
+ t5 = time.time()
+ ex_time = t5-t4
+ print(f'computation time for sending out joints: {ex_time}')
- # 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))
+ # save joint values to compute joint velocities in the next iteration
+ jointAngles_L_old = copy.copy(jointAngles_L)
+ jointAngles_R_old = copy.copy(jointAngles_R)
+ t6 = time.time()
+ ex_time = t6-t0
+ print(f'computation time whole loop: {ex_time}')
time.sleep(1/rate)
# check if poses have been reached
n = 0
while n < 10:
- robot_msg = egm_client_left.receive_msg()
+ robot_msg = egm_client_L.receive_msg()
if robot_msg.mciConvergenceMet:
print("Joint positions converged.")
break
diff --git a/python/data/get_data.py b/python/data/get_data.py
index bc85986422efe6cb1f8f79d8e3ba82a818622ccf..2ecbbba8c6782580e6cd127420afe5053904d926 100644
--- a/python/data/get_data.py
+++ b/python/data/get_data.py
@@ -48,6 +48,10 @@ def place_trajectory(start_des, p1, p2):
return p1, p2
+# rearrange the logic joint values to the strange convention abb has
+def logic2abb(joint_values):
+ return joint_values[[0, 1, 3, 4, 5, 6, 2]]
+
class Yumi(Enum):
RIGHT = False
LEFT = True
\ No newline at end of file