From 5f55cecba66169089afbd5001da70e253726767f Mon Sep 17 00:00:00 2001
From: Joschua Gosda <joschua.gosda@control.lth.se>
Date: Tue, 17 May 2022 11:34:37 +0200
Subject: [PATCH] repo cleanup

---
 c++/.gitignore                                |   6 -
 c++/gpm.cpp                                   | 187 ------------------
 c++/gpm.hpp                                   |   5 -
 c++/main.cpp                                  |  36 ----
 c++/pybind11_example.cpp                      |  17 --
 .../feedforward/main_program.py               | 110 -----------
 6 files changed, 361 deletions(-)
 delete mode 100644 c++/.gitignore
 delete mode 100644 c++/gpm.cpp
 delete mode 100644 c++/gpm.hpp
 delete mode 100644 c++/main.cpp
 delete mode 100644 c++/pybind11_example.cpp
 delete mode 100644 python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py

diff --git a/c++/.gitignore b/c++/.gitignore
deleted file mode 100644
index 6d5a8f2..0000000
--- a/c++/.gitignore
+++ /dev/null
@@ -1,6 +0,0 @@
-#Ignore build folder
-build/
-
-#Files
-*.pdf
-*.gv
diff --git a/c++/gpm.cpp b/c++/gpm.cpp
deleted file mode 100644
index 70312d7..0000000
--- a/c++/gpm.cpp
+++ /dev/null
@@ -1,187 +0,0 @@
-#include <iostream>
-#include <string>
-
-#include <broccoli/control/kinematics/ComfortPoseGradient.hpp>
-#include <broccoli/core/math.hpp>
-
-#include <rl/math/Transform.h>
-#include <rl/mdl/Kinematic.h>
-#include <rl/mdl/Model.h>
-#include <rl/mdl/UrdfFactory.h>
-
-
-std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> gpm(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity, 
-Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, const bool left_arm) {
-
-	// FORWARD KINEMATICS
-	rl::mdl::UrdfFactory factory;
-	std::string path2urdf;
-
-	if(left_arm){
-		path2urdf = "/home/joschua/Coding/forceControl/master-project/c++/src/urdf/yumi_left.urdf";
-	} else{
-		path2urdf = "/home/joschua/Coding/forceControl/master-project/c++/src/urdf/yumi_right.urdf";
-	}
-
-	std::shared_ptr<rl::mdl::Model> model(factory.create(path2urdf));
-
-	rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(model.get());
-
-	// forward kinematics for the right arm
-	kinematic->setPosition(jointAngles);
-	kinematic->forwardPosition();
-	kinematic->calculateJacobian();
-
-	Eigen::Matrix<double, 6, 7> J;
-	// copy entries from RL jacobian to Eigen::jacobian in order to use it in brocolli function
-	for (int i = 0; i < 7; i++){
-		for (int j = 0; j < 6; j++){
-			J(j, i) = kinematic->getJacobian()(j, i);
-		}
-	}
-
-	// check if matrices are the same
-	//std::cout << "RLJacobian \n" << kinematic->getJacobian() << std::endl;
-	//std::cout << "myJacobian \n" << J << std::endl;
-	//std::cout << "Manipulability meassure \n" << kinematic->calculateManipulabilityMeasure() << std::endl;
-	
-	// extract orientation and position for the right arm
-	rl::math::Transform t = kinematic->getOperationalPosition(0);
-	rl::math::Vector3 position = t.translation();
-	rl::math::Vector3 orientation = t.rotation().eulerAngles(2, 1, 0).reverse();
-	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;
-	
-
-	// INVERSE KINEMATICS
-	// compute translation and orientation error
-	Eigen::Matrix3d desOrientation;
-	// go from Euler ZYX to rotation matrix
-	desOrientation = Eigen::AngleAxisd(desPose(5), Eigen::Vector3d::UnitZ())
-					*Eigen::AngleAxisd(desPose(4), Eigen::Vector3d::UnitY())
-					*Eigen::AngleAxisd(desPose(3), Eigen::Vector3d::UnitX());
-					
-	//std::cout << "reverse euler angles" << desOrientation.eulerAngles(2, 1, 0).reverse();
-
-	//  check if these two rotation matrices match!
-	//std::cout << "desOrientation \n" << desOrientation << std::endl;
-	//std::cout << "RL Orientation \n" << t.rotation() << std::endl;
-
-	//Eigen::Vector3d einheitsvektor;
-	//einheitsvektor << 1.0, 0.0, 0.0;
-	//std::cout << "desOrientation x einheitsvektor \n" << desOrientation* einheitsvektor << std::endl;
-	//std::cout << "RL Orientation x einheitsvektor\n" << t.rotation()*einheitsvektor << std::endl;
-
-
-	// define Quaternion with coefficients in the order [x, y, z, w] 
-	Eigen::Vector3d desiredTranslation = desPose.head(3);
-	//std::cout << "desiredTranslation" << desiredTranslation << std::endl;
-	Eigen::Quaterniond desiredOrientation(desOrientation);
-
-	// set the current positions
-	Eigen::Matrix<double, 3, 1> currentTranslation;
-	currentTranslation << position.x(), position.y(), position.z();
-	Eigen::Quaterniond currentOrientation(t.rotation());
-	
-	// chose the correct quaternion such that the distance between desired and current
-	// quaternion is the shortest
-	if (desiredOrientation.dot(currentOrientation) < 0.0) {
-		currentOrientation.coeffs() *= -1.0;
-	}
-	// calculate delta between quaternions
-	Eigen::Quaterniond errorQuaternion = currentOrientation.inverse() * desiredOrientation;
-	Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
-
-	// compute task space velocity with drift compensation
-	const double gainDriftCompensation = 0.1;
-	const double dt = 0.0125; // refers to 80 Hz
-    Eigen::Matrix<double, 6, 1> effectiveTaskSpaceInput = Eigen::Matrix<double, 6, 1>::Zero();
-	effectiveTaskSpaceInput.head(3) = gainDriftCompensation/dt * (desiredTranslation - currentTranslation)
-										+ desVelocity.head(3);
-	effectiveTaskSpaceInput.tail(3) = gainDriftCompensation/dt * errorRotationInWorldFrame + desVelocity.tail(3);
-	std::cout << "effectiveTaskSpaceInput: " << effectiveTaskSpaceInput << std::endl;
-
-
-	// COMPUTE CPG GRADIENT
-	// define min and max values for the joints of Yumi
-	Eigen::Matrix< double, 7, 1> q_min;
-	Eigen::Matrix< double, 7, 1> q_max;
-	q_min << -168.5, -143.5, -168.5, -123.5, -290, -88, -229;
-	q_min *= rl::math::DEG2RAD;
-	q_max << 168.5, 43.5, 168.5, 80, 290, 138, 229;
-	q_max *= rl::math::DEG2RAD;
-	// create CompfortPoseGradient object
-	broccoli::control::ComfortPoseGradient<7> cpg;
-	// compute CPG gradient
-	cpg.useRangeBasedScaling(q_min, q_max);
-	//cpg.setWeight() by default it is 1
-
-	Eigen::Matrix<double, 7, 1> cpgGradient = Eigen::Matrix<double, 7, 1>::Zero();
-	cpgGradient = cpg.process(jointAngles, jointVelocity);	// if gradient is zero then the ASC is just a resolved motion ik method
-
-	// COMPUTE MANIPULABILTY GRADIENT
-	// compute Jacobian derivative - code snippet from Jonas Wittmann
-	std::array<Eigen::Matrix<double, 6, 7>, 7> dJ; // NOLINT
-    for (auto& matrix : dJ) {
-        matrix = Eigen::Matrix<double, 6, 7>::Zero();
-    }
-    Eigen::Matrix<double, 3, 7> transJ = Eigen::Matrix<double, 3, 7>::Zero();
-    transJ = J.block<3, 7>(0, 0);
-    Eigen::Matrix<double, 3, 7> rotJ = Eigen::Matrix<double, 3, 7>::Zero();
-    rotJ = J.block<3, 7>(3, 0);
-    const int numOfJoints = 7;
-    for (int jj = 0; jj < numOfJoints; ++jj) {
-        for (int ii = jj; ii < numOfJoints; ++ii) {
-            dJ.at(jj).block<3, 1>(0, ii) = rotJ.col(jj).cross(transJ.col(ii));
-            dJ.at(jj).block<3, 1>(3, ii) = rotJ.col(jj).cross(rotJ.col(ii));
-            if (ii != jj) {
-                dJ.at(ii).block<3, 1>(0, jj) = dJ.at(jj).block<3, 1>(0, ii);
-            }
-        }
-    }
-
-	Eigen::Matrix<double, 7, 1> manipGradient = Eigen::Matrix<double, 7, 1>::Zero();
-    double cost = sqrt((J * J.transpose()).determinant());
-    // Compute the manipulability gradient.
-    for (int jj = 0; jj < 7; ++jj) {
-        manipGradient[jj] = cost * ((J * J.transpose()).inverse() * dJ.at(jj) * J.transpose()).trace();
-    }
-	// add both gradients
-	Eigen::Matrix<double, 7, 1> nullSpaceGradient = Eigen::Matrix<double, 7, 1>::Zero();
-	nullSpaceGradient = 0*manipGradient + 0*cpgGradient;
-	//std::cout << "gradient \n" << nullSpaceGradient << std::endl;
-
-
-	// ASC desired effective velocity does not work -> implement myself
-	Eigen::Matrix<double, 7, 7>  m_inverseWeighing = Eigen::Matrix<double, 7, 7> ::Identity();
-	double m_activationFactorTaskSpace = 1.0;
-	Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * nullSpaceGradient;
-	Eigen::Matrix<double, 7, 1> jointVelocities;
-	jointVelocities = broccoli::core::math::solvePseudoInverseEquation(J, m_inverseWeighing, effectiveTaskSpaceInput, nullSpaceVelocity, m_activationFactorTaskSpace);
-	std::cout << "resulting jointVelocities: \n" << jointVelocities << std::endl;
-
-	// perform integration over one timestep to obtain positions that can be send to robot
-	Eigen::Matrix<double, 7, 1> jointAnglesDelta;
-	jointAnglesDelta << jointVelocities * dt;
-
-	//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;
-
-	// forward kinematics with the new joints values from IK
-	kinematic->setPosition(jointAngles+jointAnglesDelta);
-	kinematic->forwardPosition();
-
-	rl::math::Transform dest = kinematic->getOperationalPosition(0);
-	rl::math::Vector3 dposition = dest.translation();
-	rl::math::Vector3 dorientation = dest.rotation().eulerAngles(2, 1, 0).reverse();
-	std::cout << "IK joint configuration in degrees: " << (jointAngles+jointAnglesDelta).transpose() * rl::math::RAD2DEG << std::endl;
-	std::cout << "IK end-effector position: [m] " << dposition.transpose() << " orientation [deg] " << dorientation.transpose() * rl::math::RAD2DEG << std::endl;
-	
-	Eigen::Matrix<double, 6, 1> resPose;
-	resPose << dposition.transpose()(0), dposition.transpose()(1), dposition.transpose()(2), dorientation.transpose()(0), dorientation.transpose()(1), dorientation.transpose()(2);
-
-	// return desired joint angles for the next step and pose for computing the IK accuracy
-	return std::make_pair((jointAngles+jointAnglesDelta), resPose);
-}
-
diff --git a/c++/gpm.hpp b/c++/gpm.hpp
deleted file mode 100644
index cbe1e7a..0000000
--- a/c++/gpm.hpp
+++ /dev/null
@@ -1,5 +0,0 @@
-#include <Eigen/Eigen>
-
-
-std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> gpm(Eigen::Matrix<double, 6, 1> &desPose, Eigen::Matrix<double, 6, 1> &desVelocity, 
-Eigen::Matrix<double, 7, 1> &jointAngles, Eigen::Matrix<double, 7, 1> &jointVelocity, const bool left_arm);
\ No newline at end of file
diff --git a/c++/main.cpp b/c++/main.cpp
deleted file mode 100644
index 2d93361..0000000
--- a/c++/main.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#include <iostream>
-#include "gpm.hpp"
-
-#include <Eigen/Eigen>
-#include <rl/math/Unit.h>
-
-
-int main(int, char**) {
-
-	enum yumi_arm{YUMI_RIGHT, YUMI_LEFT};
-
-	// Is Values
-	Eigen::Matrix<double, 6, 1> actualPosition;
-	Eigen::Matrix<double, 7, 1> jointAngles;
-	//jointAngles << 90.48, 17.87, -25.0, 48.0, -137.0, 122.0, -74.21; // 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;
-	jointVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
-	
-	// Desired Values
-	Eigen::Matrix<double, 6, 1> desPose;
-	desPose << 0.300, 0.200, 0.200, 33.4*rl::math::DEG2RAD, 157.0*rl::math::DEG2RAD, 39.4*rl::math::DEG2RAD; // obtained from RS with stated joint values
-	Eigen::Matrix<double, 6, 1> desVelocity;
-	desVelocity << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
-
-	std::pair<Eigen::Matrix<double, 7, 1>, Eigen::Matrix<double, 6, 1>> result;
-	result = gpm(desPose, desVelocity, jointAngles, jointVelocity, YUMI_RIGHT);
-	
-	std::cout << "desired joint values: \n" << result.first << std::endl;
-	std::cout << "current pose: \n" << result.second << std::endl;
-	
-	return 0;
-}
\ No newline at end of file
diff --git a/c++/pybind11_example.cpp b/c++/pybind11_example.cpp
deleted file mode 100644
index 0cee6f9..0000000
--- a/c++/pybind11_example.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include <pybind11/pybind11.h>
-#include <pybind11/eigen.h>
-#include "gpm.hpp"
-
-namespace py = pybind11;
-
-int add(int i, int j) {
-    return i + j;
-}
-
-
-PYBIND11_MODULE(invKin, m) {
-    m.doc() = "pybind11 binding to C++ function that computes IK based on GPM"; // optional module docstring
-    m.def("gpm", &gpm, "A function to compute the invserse kinematics for a 7 DOF serial manipulator on vecocity level with comfort pose and manipulabilty gradient",
-    py::arg("desired pose"),py::arg("desired Velocities"), py::arg("joint angles"), py::arg("joint velocities"), py::arg("left arm -> 1, right arm -> 0"), py::return_value_policy::copy);
-	
-	}
\ No newline at end of file
diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py b/python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py
deleted file mode 100644
index 88c8edd..0000000
--- a/python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py
+++ /dev/null
@@ -1,110 +0,0 @@
-#!/usr/bin/env python
-import sys
-from typing import Sequence
-import time
-
-import numpy as np
-
-
-from abb_egm_pyclient import EGMClient
-
-'''
-The example of the repo https://gitlab.control.lth.se/tetov/abb_egm_pyclient is a good starting point but
-code needs to be adapted to projects goal of foam cutting. Structure of functions needs to changed since
-further steps like computing the IK needs to be performed between receiving and sending positions and 
-reading force sensor data.
-
-'''
-
-
-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]]
-
-
-egm_client_left = EGMClient(port=UDP_PORT_LEFT)
-egm_client_right = EGMClient(port=UDP_PORT_RIGHT)
-
-print("waiting to receive msg")
-# get the current values of joints
-robot_msg_L = egm_client_left.receive_msg()
-robot_msg_R = egm_client_right.receive_msg()
-
-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 
-#conf_diff = np.abs(logic2abb(conf_des) - conf_yumi)
-#condition = np.array([0.03, 0.03, 0.03, 0.03, 0.03, 0.03, 0.03]) # equals 2 degrees
-#assert (conf_diff-condition).all(), 'Starting configurations are not synchronized'
-
-
-for n in range(len(comp_conf_left[:,0])):
-    sTime = time.time()
-
-    # 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}")
-
-    # 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
-    sleeping_time = 1/rate - (time.time()-sTime)
-    time.sleep(1/rate) 
-
-# after all messages are sent out, wait for 10 sec and check if positions converged
-n = 0
-while n < 10:
-    robot_msg = egm_client_left.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.")
-
-
-
-
-
-
-# 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 
-
-# TODO: read the data from g-code and transform it into the global frame of yumi
-# make sure to set the first configuration as the fine point in robot studio, so that they
-# virtual and real configuration are synchronized
-
-# TODO: loop over the g-code positions (check g-code of its based on constant speed)
-# instructions in G code are G1 instruction meaning moving at a constant feed (velocity in axis direction)
-# https://www.seniorcare2share.com/how-does-g-code-work/
-
-    # TODO: read current configuration via egm client
-
-    # TODO: use RL to compute the current jacobian
-
-    # TODO: call the C++ function to get the joint values
-    #   input:  current jacobian, nullspace gradient, desired position in task space, sample time 
-
-    # TODO: send to obtained joint values via egm client
\ No newline at end of file
-- 
GitLab