successful force control on yumi

arduino/loadcell/loadcell.ino

@@ -6,9 +6,11 @@
 const int LOADCELL_DOUT_PIN = 2;
 const int LOADCELL_SCK_PIN = 3;
 
-const float calibration_weight = 500.0;
+const float calibration_weight = 4.9;
 float zero_reading, load_reading;
 
+const float calibration_factor = -479404.84;
+
 
 HX711 scale;
 
@@ -28,7 +30,7 @@ void setup() {
   //Serial.print("\t scale: ");
   //Serial.println(scale.get_scale(), DEC);
 
-  Serial.println("tare the scale. in 3 sec. Make sure it is unloaded");
+  //Serial.println("tare the scale. in 3 sec. Make sure it is unloaded");
 
   delay(3);
 
@@ -46,20 +48,24 @@ void setup() {
 
 
   //delay(2);
-  Serial.println("mount the known weight to the load cell and press ENTER to proceed with calibration");
-  int incomingByte = 0;
+  //Serial.println("mount the known weight to the load cell and press ENTER to proceed with calibration");
+  //int incomingByte = 0;
   // Calibration of the load cell
-  while(Serial.available() == 0) {
-    }
+  //while(Serial.available() == 0) {
+    //}
     
-  load_reading = scale.read_average(10);
-  Serial.print("zero and load reading: ");
+  //load_reading = scale.read_average(10);
+  /*Serial.print("zero and load reading: ");
   Serial.print(zero_reading, DEC);
   Serial.print("  ");
-  Serial.println(load_reading, DEC);
-  scale.set_scale((load_reading - zero_reading)/calibration_weight);  // this value is obtained by calibrating the scale with known weights; see the README for details
-  incomingByte = Serial.read();   // clear the receive buffer by assigning value to var
+  Serial.println(load_reading, DEC);*/
+  //scale.set_scale((load_reading - zero_reading)/calibration_weight);  // this value is obtained by calibrating the scale with known weights; see the README for details
+  //incomingByte = Serial.read();   // clear the receive buffer by assigning value to var
+
+  //Serial.print("the calibration factor is:  ");
+  //Serial.println(load_reading - zero_reading/calibration_weight);
   
+/*
   Serial.println("set scale....");
   Serial.print("offset: ");
   Serial.print(scale.get_offset(), DEC);
@@ -70,10 +76,10 @@ void setup() {
   Serial.print(scale.get_value());
   Serial.print("\t get_units: ");
   Serial.println(scale.get_units());
+*/
+  scale.set_scale(calibration_factor);
   
-
-  
-  Serial.println("load cell is setup. Start reading now...");
+  //Serial.println("load cell is setup. Start reading now...");
   delay(1); // delay that python code sees the correct starting byte
 }
 
@@ -82,6 +88,6 @@ void loop() {
   Serial.println(scale.get_units());
 
   //scale.power_down();			        // put the ADC in sleep mode
-  //delay(5);
+  delay(1/80);
   //scale.power_up();
 }

c++/include/yumi.hpp

@@ -22,6 +22,9 @@ class Yumi {
     void set_desPoseVel(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity);
     void set_driftCompGain(double gain);
     void set_sampleTime(double sampleTime);
+    void set_force(double force);
+    void set_kp(double kp);
+    void set_operationPoint(double op);
 
     void process();
 
@@ -34,20 +37,25 @@ class Yumi {
     private:
     // vars for rl library
     std::shared_ptr<rl::mdl::Model> m_model;
-    //rl::mdl::Model m_modelObj;
-    //rl::mdl::Kinematic* m_kinematic;
-    //rl::mdl::Kinematic m_kin_model;
 
     // vars for tuning
     double m_driftCompGain = 1.0;
     double m_sampleTime = 0.0125;
     double m_activationFactorTaskSpace = 1.0;
 
+    // var saving force
+    double m_force = 0;
+    double m_forceOP = 0; // operation around operating point of 2 newton
+
+    // gain for control
+    double m_kp = 1.0;
+
     // vars to store configuration
     rl::math::Vector3 m_desPosition = rl::math::Vector3::Zero();
     rl::math::Vector3 m_desPositionDot = rl::math::Vector3::Zero();
 	rl::math::Vector3 m_desOrientation = rl::math::Vector3::Zero();
     rl::math::Vector3 m_desOrientationDot = rl::math::Vector3::Zero();
+    Eigen::Matrix<double, 3, 3> m_rotationMatrix = Eigen::Matrix<double, 3, 3>::Zero();
 
     Eigen::Matrix<double, 7, 1> m_jointAngles = Eigen::Matrix<double, 7, 1>::Zero();
     Eigen::Matrix<double, 7, 1> m_jointVelocity = Eigen::Matrix<double, 7, 1>::Zero();
@@ -58,6 +66,7 @@ class Yumi {
     Eigen::Matrix<double, 6, 7> m_jacobian = Eigen::Matrix<double, 6, 7>::Zero();
 
     Eigen::Matrix<double, 6, 1> m_effectiveTaskSpaceInput = Eigen::Matrix<double, 6, 1>::Zero();
+    Eigen::Matrix<double, 3, 1> m_forceTaskSpaceInput = Eigen::Matrix<double, 3, 1>::Zero(); // only apply translational changes
 
     Eigen::Matrix<double, 7, 7>  m_inverseWeighing = Eigen::Matrix<double, 7, 7> ::Identity();
     Eigen::Matrix<double, 7, 1> m_nullSpaceGradient = Eigen::Matrix<double, 7, 1>::Zero();
@@ -68,6 +77,7 @@ class Yumi {
     void doForwardKinematics();
     Eigen::Matrix3d euler2rotMatrix(rl::math::Vector3 orientation);
     void compTaskSpaceInput();
+    void compForce2VelocityController();
 
     
  };
\ No newline at end of file

c++/src/py2gpmbind.cpp

@@ -17,5 +17,8 @@ PYBIND11_MODULE(invKin, m) {
        .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("printPose", &Yumi::print_pose); 
+       .def("printPose", &Yumi::print_pose)
+       .def("set_kp", &Yumi::set_kp)
+       .def("set_operationPoint", &Yumi::set_operationPoint)
+       .def("set_force", &Yumi::set_force);
 	}
\ No newline at end of file

c++/src/yumi.cpp

@@ -30,6 +30,7 @@ void Yumi::doForwardKinematics(){
     rl::math::Transform t = kinematic->getOperationalPosition(0);
 	m_position = t.translation();
 	m_orientation = t.rotation().eulerAngles(2, 1, 0).reverse();
+    m_rotationMatrix = t.rotation(); // rotation from world frame to ee frame
     m_jacobian = kinematic->getJacobian();
 }
 
@@ -77,7 +78,7 @@ void Yumi::compTaskSpaceInput(){
 	Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
 
     m_effectiveTaskSpaceInput.head(3) = m_driftCompGain/m_sampleTime * (m_desPosition - m_position)
-										+ m_desPositionDot;
+										+ m_desPositionDot + m_forceTaskSpaceInput;
 	m_effectiveTaskSpaceInput.tail(3) = m_driftCompGain/m_sampleTime * errorRotationInWorldFrame + m_desOrientationDot;
 
 }
@@ -85,6 +86,7 @@ void Yumi::compTaskSpaceInput(){
 void Yumi::process(){
 
     doForwardKinematics();
+    compForce2VelocityController();
     compTaskSpaceInput();
 
     Eigen::Matrix<double, 7, 1> jointVelocities;
@@ -108,3 +110,25 @@ Eigen::Matrix<double, 6, 1> Yumi::get_newPose(){
     pose << m_position, m_orientation;
     return pose;
 }
+
+void Yumi::set_force(double force){
+    m_force = force;
+}
+
+void Yumi::compForce2VelocityController(){
+    Eigen::Vector3d velocityEE;
+    // for a postive force outcome the ee of the right arm should move in postive y direction, 
+    // have a look at chosen ee frame in RS
+    velocityEE << 0, m_force-m_forceOP, 0;
+    velocityEE *= m_kp;
+    // transform the velocities computed in the ee frame to the world frame
+    m_forceTaskSpaceInput = m_rotationMatrix.transpose() * velocityEE;
+}
+
+void Yumi::set_kp(double kp){
+    m_kp = kp;
+}
+
+void Yumi::set_operationPoint(double op){
+    m_forceOP = op;
+}
\ No newline at end of file

python/abb_egm_pyclient/abb_egm_pyclient/feedback/control.py

-# 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 
-

python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_realSensor.py

+# 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 
+
+#!/usr/bin/env python
+from typing import Sequence
+import time
+import copy
+import numpy as np
+from abb_egm_pyclient import EGMClient
+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
+
+'''
+Before running this script make sure that the starting pose of the robot (either real one or in RS) match the
+starting pose of the computed joint angles. This script sends desired joint position to the robot. It takes to real
+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_RIGHT = 6511
+rate = 80
+
+# take the first desired taskSpaceInput for the right arm and simulate force sensor data 
+trans_const = copy.copy(p2[0, :])
+#rot_const = copy.copy(phi_delta[0, :])
+rot_const = np.array([0, 0, 0])
+desPose_R_const = np.concatenate((trans_const, rot_const), axis=0) 
+
+egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
+
+yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
+yumi_right.set_operationPoint(0.5)
+yumi_right.set_kp(3.65)
+
+desVelocities_R_const = np.array([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])
+
+# SETUP SERIAL INTERFACE
+# find active ports
+ports = serial.tools.list_ports.comports()
+arduino = serial.Serial()
+
+myPort = '/dev/ttyACM0'
+myBaudRate = 38400
+portList = []
+
+print("AVAILABLE PORTS \n")
+for onePort in ports:
+    portList.append(str(onePort))
+    print(str(onePort))
+
+
+# setup configuration for serial interface
+arduino.baudrate = myBaudRate
+arduino.port = myPort
+arduino.open()
+
+time.sleep(2)
+arduino.flushInput()
+
+# wait until first serial data from arduino is available
+while not arduino.in_waiting:
+    pass
+    
+i = 0
+force = 0.0 # 0.5 [N] seems to be a good value for the control
+timestamp = time.time()
+
+while True and arduino.isOpen():
+
+    # check for new force data
+    if arduino.in_waiting: # get the number of bytes in the input buffer
+        packet = arduino.readline() # type: bytes  
+        str_receive = packet.decode('utf-8').rstrip('\n')
+        force = float(str_receive)
+
+    if (time.time() - timestamp) >= (1.0/rate):
+        # 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)
+        jointAngles_R = np.radians(np.array(conf_R))
+
+        # compute the resulting jointVelocities
+        if i > 0:
+            jointVelocities_R = (jointAngles_R - jointAngles_R_old)/rate
+
+        yumi_right.set_jointValues(jointAngles_R, jointVelocities_R)
+        yumi_right.set_desPoseVel(desPose_R_const, desVelocities_R_const)
+        yumi_right.set_force(force)
+        yumi_right.process()
+
+        print(force)
+
+        ik_jointAngles_R = yumi_right.get_newJointValues() # computed joint values from IK
+
+        #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_R = np.degrees(ik_jointAngles_R)
+
+        egm_client_R.send_planned_configuration(logic2abb(des_conf_R))
+
+
+        # save joint values to compute joint velocities in the next iteration
+        jointAngles_R_old = copy.copy(jointAngles_R)
+
+        i = i+1
+        timestamp = time.time()
+        ##time.sleep(1/rate) 
+
+
+
+# check if poses have been reached
+n = 0
+while n < 10:
+    robot_msg = egm_client_R.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.")

python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_virtualSensor.py

+# 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 
+
+#!/usr/bin/env python
+from typing import Sequence
+import time
+import copy
+import numpy as np
+from abb_egm_pyclient import EGMClient
+import libs.invKin as invKin
+from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, logic2abb
+from matplotlib import pyplot as plt
+
+'''
+Before running this script make sure that the starting pose of the robot (either real one or in RS) match the
+starting pose of the computed joint angles. This script sends desired joint position to the robot. It takes to real
+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_RIGHT = 6511
+rate = 80
+
+# take the first desired taskSpaceInput for the right arm and simulate force sensor data 
+trans_const = copy.copy(p2[0, :])
+#rot_const = copy.copy(phi_delta[0, :])
+rot_const = np.array([np.pi/2, 0, 0])
+desPose_R_const = np.concatenate((trans_const, rot_const), axis=0) 
+
+
+# generate sinusoidal test data for the virtual force sensor
+# sin(t/pi *4) + 2, test function, test for 5 seconds
+numDataPoints = round(5/(1/rate))
+forceInput = np.zeros((numDataPoints))
+for i in range(numDataPoints):
+    forceInput[i] = np.sin(i/np.pi * 1/rate * 4)+2
+
+m_time = np.linspace(0, 5, num=numDataPoints)
+
+""" fig = plt.figure()
+plt.plot(m_time, forceInput, label='desired test signal') #
+fig.get_axes()[0].set_xlabel('time')
+fig.get_axes()[0].set_ylabel('virtual force signal')
+plt.legend()
+plt.title('test signal')
+plt.show() """
+
+egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
+
+yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
+
+desVelocities_R_const = np.array([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 index, force in enumerate(forceInput):
+
+    # 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)
+    jointAngles_R = np.radians(np.array(conf_R))
+
+    # compute the resulting jointVelocities
+    if index > 0:
+        jointVelocities_R = (jointAngles_R - jointAngles_R_old)/rate
+
+
+    yumi_right.set_jointValues(jointAngles_R, jointVelocities_R)
+    yumi_right.set_desPoseVel(desPose_R_const, desVelocities_R_const)
+    yumi_right.set_force(force)
+    yumi_right.process()
+
+
+    ik_jointAngles_R = yumi_right.get_newJointValues() # computed joint values from IK
+
+
+    #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_R = np.degrees(ik_jointAngles_R)
+
+    egm_client_R.send_planned_configuration(logic2abb(des_conf_R))
+
+
+    # save joint values to compute joint velocities in the next iteration
+    jointAngles_R_old = copy.copy(jointAngles_R)
+
+
+    time.sleep(1/rate) 
+
+
+
+# check if poses have been reached
+n = 0
+while n < 10:
+    robot_msg = egm_client_R.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.")

python/serial_interface/readSerial.py

@@ -12,7 +12,7 @@ Instructions how to use this:
 
 # find active ports
 ports = serial.tools.list_ports.comports()
-serialInst = serial.Serial()
+arduino = serial.Serial()
 
 myPort = '/dev/ttyACM0'
 myBaudRate = 38400
@@ -25,32 +25,41 @@ for onePort in ports:
 
 
 # setup configuration for serial interface
-serialInst.baudrate = myBaudRate
-serialInst.port = myPort
-serialInst.open()
+arduino.baudrate = myBaudRate
+arduino.port = myPort
+arduino.open()
 
 sleep(1)
-serialInst.flushInput()
+arduino.flushInput()
 
 # wait until first serial data from arduino is available
-while not serialInst.in_waiting:
+while not arduino.in_waiting:
     pass
     
 #print("Mount the calibration weight to the load cell and then hit ENTER")
-print("message from arduino: " + serialInst.readline().decode('utf-8').rstrip('\n'))
-print("message from arduino: " + serialInst.readline().decode('utf-8').rstrip('\n'))
-input()
-serialInst.write(bytes('0', 'utf-8'))
+#print("message from arduino: " + arduino.readline().decode('utf-8').rstrip('\n'))
+#print("message from arduino: " + arduino.readline().decode('utf-8').rstrip('\n'))
+#input()
+#arduino.write(bytes('0', 'utf-8'))
 
 #sleep(1)
 time_prev = time()
+timestamp = time()
 
 while True:
-    if serialInst.in_waiting: # get the number of bytes in the input buffer
-        packet = serialInst.readline() # type: bytes  
+    if arduino.in_waiting: # get the number of bytes in the input buffer
+        packet = arduino.readline() # type: bytes  
         time_diff = time() - time_prev  
         str_receive = packet.decode('utf-8').rstrip('\n')
         print(str_receive)
+        force = float(str_receive)
+        #print(f"force as float is {force}")
         #print("serial freq: " + str(1/time_diff))
         time_prev = time()
+        #arduino.flushInput()
+        print(time())
+        sleep(1/80)
     
+    if (time() - timestamp) >= (1.0/80.0):
+        print(time()-timestamp)
+        timestamp = time()