simplification of control with selection matrices and python progressbar

c++/include/yumi.hpp

@@ -30,7 +30,7 @@ class Yumi {
     void process();
 
     Eigen::Matrix<double, 7, 1> get_newJointValues();
-    Eigen::Matrix<double, 6, 1> get_newPose();
+    Eigen::Matrix<double, 6, 1> get_pose();
 
     // for debugging
     void print_pose();

c++/src/py2gpmbind.cpp

@@ -16,7 +16,7 @@ PYBIND11_MODULE(invKin, m) {
        .def("set_desPoseVel", &Yumi::set_desPoseVel, py::arg("desired pose"), py::arg("desired velocities"))
        .def("process", &Yumi::process)
        .def("get_newJointValues", &Yumi::get_newJointValues, py::return_value_policy::copy)
-       .def("get_newPose", &Yumi::get_newPose, py::return_value_policy::copy)
+       .def("get_pose", &Yumi::get_pose, py::return_value_policy::copy)
        .def("printPose", &Yumi::print_pose)
        .def("set_kp", &Yumi::set_kp)
        .def("set_operationPoint", &Yumi::set_operationPoint)

c++/src/yumi.cpp

@@ -77,11 +77,8 @@ void Yumi::compTaskSpaceInput(){
 	Eigen::Quaterniond errorQuaternion = currentOrientation.inverse() * desiredOrientation;
 	Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
 
-    m_desPosition = m_selectVelMatrix * m_desPosition + (Eigen::Matrix3d::Identity() - m_selectVelMatrix) * m_position;
-    m_desPositionDot = m_selectVelMatrix * m_desPositionDot;
-
-    m_effectiveTaskSpaceInput.head(3) = m_driftCompGain/m_sampleTime * (m_desPosition - m_position)
-										+ m_desPositionDot + m_forceTaskSpaceInput;
+    m_effectiveTaskSpaceInput.head(3) = m_selectVelMatrix * (m_driftCompGain/m_sampleTime * (m_desPosition - m_position) + m_desPositionDot) 
+										 + m_forceTaskSpaceInput;
 	m_effectiveTaskSpaceInput.tail(3) = m_driftCompGain/m_sampleTime * errorRotationInWorldFrame + m_desOrientationDot;
 
 }
@@ -107,7 +104,7 @@ Eigen::Matrix<double, 7, 1> Yumi::get_newJointValues(){
     return m_jointAngles;
 }
 
-Eigen::Matrix<double, 6, 1> Yumi::get_newPose(){
+Eigen::Matrix<double, 6, 1> Yumi::get_pose(){
     doForwardKinematics();
     Eigen::Matrix<double, 6, 1> pose;
     pose << m_position, m_orientation;

python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py

@@ -2,7 +2,7 @@
 # show if thats the case and ask user if he wants to start control 
 
 #!/usr/bin/env python
-from ssl import PROTOCOL_TLSv1_1
+import keyboard
 from typing import Sequence
 import time
 import copy
@@ -12,6 +12,7 @@ import libs.invKin as invKin
 from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, logic2abb
 from matplotlib import pyplot as plt
 import serial.tools.list_ports
+from tqdm import tqdm
 
 '''
 Before running this script make sure that the starting pose of the robot (either real one or in RS) match the
@@ -43,7 +44,7 @@ egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
 egm_client_L = EGMClient(port=UDP_PORT_LEFT)
 
 yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
-yumi_right.set_operationPoint(0.7)
+yumi_right.set_operationPoint(1.0)
 yumi_right.set_kp(0.2)
 yumi_right.set_hybridControl(True)
 
@@ -84,8 +85,11 @@ timestamp = time.time()
 cutting = False
 traj_samples = len(p1[:, 0]) 
 
+
 print("\n Force control only to tension the wire...")
 
+
+
 while True and arduino.isOpen():
 
     # check for new force data
@@ -97,6 +101,7 @@ while True and arduino.isOpen():
     # force control only until wire is tensioned
     if not cutting:
         if (time.time() - timestamp) >= (1.0/rate):
+            timestamp = time.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
@@ -126,17 +131,18 @@ while True and arduino.isOpen():
             jointAngles_R_old = copy.copy(jointAngles_R)
 
             i = i+1
-            timestamp = time.time()
-            if (force > 0.7):
+            if (force > 0.65) and keyboard.is_pressed('enter'):
                 cutting = True
                 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])
                 i = 0 # reset counter
                 print("Changing to hybrid control now...")
+                pbar = tqdm(total=traj_samples)
     
     # hybrid control - cutting is True
     else:
         if (time.time() - timestamp) >= (1.0/rate):
+            timestamp = time.time()
             # copy array entries to local variables
             pos1 = p1[i, :]
             vel1 = v1[i, :]
@@ -193,9 +199,12 @@ while True and arduino.isOpen():
             jointAngles_L_old = copy.copy(jointAngles_L)
             jointAngles_R_old = copy.copy(jointAngles_R)
             i = i + 1 
-            timestamp = time.time()
+            pbar.update(1)
+            #print("freq for one loop: ", 1/(time.time()- timestamp))
+            
 
             if (i >= (traj_samples-1)):
+                pbar.close()
                 break # break out of while loop
 
 

python/preprocessing/gcode.txt

+G1 X00.00 Y53.70 U00.00 V53.70
 G1 X10.00 Y53.70 U10.00 V53.70
 G1 X10.00 Y53.70 U10.00 V53.70
 G1 X10.14 Y54.86 U10.48 V54.56
@@ -73,3 +74,4 @@ G1 X11.50 Y51.00 U10.61 V51.07
 G1 X10.68 Y51.79 U10.07 V51.97
 G1 X10.18 Y52.68 U9.87 V52.84
 G1 X10.00 Y53.70 U10.00 V53.70
+G1 X00.00 Y53.70 U00.00 V53.70

python/preprocessing/genPaths.py

@@ -31,7 +31,7 @@ pos1, pos2 = pos_split[0]*0.001, pos_split[1]*0.001 # transform to SI-units - [m
 # TODO: compute trajectory based on path of g-code
 # set a cutting speed, sample rate and compute necessary
 # position at every time step
-c_speed = 3000 * 0.001 * 1/60 # define cutting speed as 300 mm/min [m/s]
+c_speed = 100 * 0.001 * 1/60 # define cutting speed as 300 mm/min [m/s]
 st = 1.0/80.0 # highest possible sample time - 250 Hz
 
 # modificatin to delete in order to check angles in RS