Skip to content
Snippets Groups Projects
Commit 5f55cecb authored by Joschua Gosda's avatar Joschua Gosda
Browse files

repo cleanup

parent 0b544554
Branches
Tags
No related merge requests found
Show whitespace changes
Inline Side-by-side
c++/.gitignore deleted 100644 → 0
+ 0
− 6
View file @ 0b544554
#Ignore build folder
build/
#Files
*.pdf
*.gv
c++/gpm.cpp deleted 100644 → 0
+ 0
− 187
View file @ 0b544554
#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);
}
c++/gpm.hpp deleted 100644 → 0
+ 0
− 5
View file @ 0b544554
#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
c++/main.cpp deleted 100644 → 0
+ 0
− 36
View file @ 0b544554
#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
c++/pybind11_example.cpp deleted 100644 → 0
+ 0
− 17
View file @ 0b544554
#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
python/abb_egm_pyclient/abb_egm_pyclient/feedforward/main_program.py deleted 100644 → 0
+ 0
− 110
View file @ 0b544554
#!/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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment