diff --git a/c++/include/yumi.hpp b/c++/include/yumi.hpp
index 3b4aa0ea09869892257158ee08285aafbfc13011..7a48c70fe01e307e68e81e612aa9458ebe1129cb 100644
--- a/c++/include/yumi.hpp
+++ b/c++/include/yumi.hpp
@@ -32,6 +32,7 @@ class Yumi {
Eigen::Matrix<double, 7, 1> get_newJointValues();
Eigen::Matrix<double, 6, 1> get_pose();
+ double get_manipulabilityMeasure();
// for debugging
void print_pose();
@@ -59,6 +60,9 @@ class Yumi {
double m_transitionTime = 0.0;
double m_deltaTime = 0.0;
+ // manipulability meassure
+ double m_manipulabilty;
+
// vars to store configuration
rl::math::Vector3 m_desPosition = rl::math::Vector3::Zero();
rl::math::Vector3 m_desPositionDot = rl::math::Vector3::Zero();
@@ -83,13 +87,16 @@ class Yumi {
Eigen::Matrix<double, 7, 1> m_jointAnglesDelta;
Eigen::Matrix3d m_selectVelMatrix = Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d m_modSelectVelMatrix = Eigen::Matrix3d::Identity();
// private functions
void doForwardKinematics();
Eigen::Matrix3d euler2rotMatrix(rl::math::Vector3 orientation);
void compTaskSpaceInput();
void compForce2VelocityController();
+ void compIK();
void modifySelectionMatrix();
+
};
\ No newline at end of file
diff --git a/c++/src/py2gpmbind.cpp b/c++/src/py2gpmbind.cpp
index e8f26bf3ba60c46a653f40d0341c296e9465ad1a..08efd535b5a5ec071ebe0d488acb7b8ee5221fe9 100644
--- a/c++/src/py2gpmbind.cpp
+++ b/c++/src/py2gpmbind.cpp
@@ -22,5 +22,6 @@ PYBIND11_MODULE(invKin, m) {
.def("set_operationPoint", &Yumi::set_operationPoint)
.def("set_hybridControl", &Yumi::set_hybridControl)
.def("set_transitionTime", &Yumi::set_transitionTime)
+ .def("get_manipulabilityMeasure", &Yumi::get_manipulabilityMeasure)
.def("set_force", &Yumi::set_force);
}
\ No newline at end of file
diff --git a/c++/src/yumi.cpp b/c++/src/yumi.cpp
index cbaddadf3ae9d2674f37d0463180f50e0329d80a..cf7399c7019dd3b5c29303d15b1b67529971fe9d 100644
--- a/c++/src/yumi.cpp
+++ b/c++/src/yumi.cpp
@@ -32,6 +32,7 @@ void Yumi::doForwardKinematics(){
m_orientation = t.rotation().eulerAngles(2, 1, 0).reverse();
m_rotationMatrix = t.rotation(); // rotation from world frame to ee frame
m_jacobian = kinematic->getJacobian();
+ m_manipulabilty = kinematic->calculateManipulabilityMeasure();
}
void Yumi::print_pose(){
@@ -77,7 +78,7 @@ void Yumi::compTaskSpaceInput(){
Eigen::Quaterniond errorQuaternion = currentOrientation.inverse() * desiredOrientation;
Eigen::Vector3d errorRotationInWorldFrame = currentOrientation * errorQuaternion.vec();
- m_effectiveTaskSpaceInput.head(3) = m_selectVelMatrix * (m_driftCompGain/m_sampleTime * (m_desPosition - m_position) + m_desPositionDot)
+ m_effectiveTaskSpaceInput.head(3) = m_modSelectVelMatrix * (m_driftCompGain/m_sampleTime * (m_desPosition - m_position) + m_desPositionDot)
+ m_forceTaskSpaceInput;
m_effectiveTaskSpaceInput.tail(3) = m_driftCompGain/m_sampleTime * errorRotationInWorldFrame + m_desOrientationDot;
@@ -89,16 +90,7 @@ void Yumi::process(){
modifySelectionMatrix();
compForce2VelocityController();
compTaskSpaceInput();
-
- Eigen::Matrix<double, 7, 1> jointVelocities;
- Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * m_nullSpaceGradient;
-
- jointVelocities = broccoli::core::math::solvePseudoInverseEquation(m_jacobian, m_inverseWeighing, m_effectiveTaskSpaceInput,
- nullSpaceVelocity, m_activationFactorTaskSpace);
-
- m_jointAnglesDelta << jointVelocities * m_sampleTime;
-
- m_jointAngles += m_jointAnglesDelta;
+ compIK();
}
Eigen::Matrix<double, 7, 1> Yumi::get_newJointValues(){
@@ -124,11 +116,26 @@ void Yumi::compForce2VelocityController(){
// have a look at chosen ee frame in RS
velocityEE << 0, m_force-m_forceOP, 0;
velocityEE *= m_kp;
- velocityEE = (Eigen::Matrix3d::Identity() - m_selectVelMatrix) * velocityEE; // perform blending - transition from position control to force control
+ velocityEE = (Eigen::Matrix3d::Identity() - m_modSelectVelMatrix) * velocityEE; // perform blending - transition from position control to force control
+ std::cout << "selectionmatrix force: " << (Eigen::Matrix3d::Identity() - m_modSelectVelMatrix) << std::endl;
+ std::cout << "selectionmatrix velocity: " << m_modSelectVelMatrix << std::endl;
+ std::cout << "velocityEE force: " << velocityEE << std::endl;
// transform the velocities computed in the ee frame to the world frame
m_forceTaskSpaceInput = m_rotationMatrix.transpose() * velocityEE;
}
+void Yumi::compIK(){
+ Eigen::Matrix<double, 7, 1> jointVelocities;
+ Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * m_nullSpaceGradient;
+
+ jointVelocities = broccoli::core::math::solvePseudoInverseEquation(m_jacobian, m_inverseWeighing, m_effectiveTaskSpaceInput,
+ nullSpaceVelocity, m_activationFactorTaskSpace);
+
+ m_jointAnglesDelta << jointVelocities * m_sampleTime;
+
+ m_jointAngles += m_jointAnglesDelta;
+}
+
void Yumi::set_kp(double kp){
m_kp = kp;
}
@@ -160,7 +167,7 @@ void Yumi::set_transitionTime(double transitionTime){
void Yumi::modifySelectionMatrix(){
Eigen::Matrix3d blendingMatrix;
if(m_deltaTime < m_transitionTime){
- m_deltaTime += 1.0/m_sampleTime;
+ m_deltaTime += m_sampleTime;
blendingMatrix << 0, 0, 0,
0, (1 - m_deltaTime/m_transitionTime), 0,
0, 0, 0;
@@ -168,5 +175,9 @@ void Yumi::modifySelectionMatrix(){
else {
blendingMatrix = Eigen::Matrix3d::Zero();
}
- m_selectVelMatrix += blendingMatrix;
+ m_modSelectVelMatrix = m_selectVelMatrix + blendingMatrix;
+}
+
+double Yumi::get_manipulabilityMeasure(){
+ return m_manipulabilty;
}
\ No newline at end of file
diff --git a/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py b/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py
index 96eb26c9f5b95c7e4b4bad3819230ab50e3ab0e5..d74bc294f4c052ec900c3e5d04708b9914415fe9 100644
--- a/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py
+++ b/python/abb_egm_pyclient/abb_egm_pyclient/feedback/control_final.py
@@ -47,6 +47,7 @@ yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/m
yumi_right.set_operationPoint(1.0)
yumi_right.set_kp(0.2)
yumi_right.set_hybridControl(True)
+yumi_right.set_transitionTime(5.0)
yumi_left = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
diff --git a/python/test.py b/python/test.py
deleted file mode 100644
index 60d8e7842665af2ea83afa74ce617509b35e952a..0000000000000000000000000000000000000000
--- a/python/test.py
+++ /dev/null
@@ -1,21 +0,0 @@
-import sys
-import argparse
-
-from urdf_parser_py.urdf import URDF
-
-parser = argparse.ArgumentParser(usage='Load an URDF file')
-parser.add_argument('file', type=argparse.FileType('r'), nargs='?',
- default=None, help='File to load. Use - for stdin')
-parser.add_argument('-o', '--output', type=argparse.FileType('w'),
- default=None, help='Dump file to XML')
-args = parser.parse_args()
-
-if args.file is None:
- robot = URDF.from_parameter_server()
-else:
- robot = URDF.from_xml_string(args.file.read())
-
-print(robot)
-
-if args.output is not None:
- args.output.write(robot.to_xml_string())
\ No newline at end of file
diff --git a/python/test2.py b/python/test2.py
deleted file mode 100644
index 9b58c573f98f9b992e6266c1afad4dc292493b0f..0000000000000000000000000000000000000000
--- a/python/test2.py
+++ /dev/null
@@ -1,14 +0,0 @@
-from urdfpy import URDF
-import numpy as np
-
-robot = URDF.load('/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf')
-
-# print the joint names
-for joint in robot.actuated_joints:
- print(joint.name)
-
-fk = robot.link_fk(cfg=np.array([2, 3, 0, -1, 0, 0, 2]))
-
-for i in range(len(robot.links)):
- print(fk[robot.links[i]])
-a = 1
\ No newline at end of file
diff --git a/python/test_c++pointer.py b/python/test_c++pointer.py
deleted file mode 100644
index 6c999fa5703f593f6b2b324dde10c78c4ac75f89..0000000000000000000000000000000000000000
--- a/python/test_c++pointer.py
+++ /dev/null
@@ -1,5 +0,0 @@
-from libs.invKin import get_pointer, set_pointer
-
-
-pointer = get_pointer()
-set_pointer(pointer)
\ No newline at end of file