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