diff --git a/arduino/loadcell/loadcell.ino b/arduino/loadcell/loadcell.ino
index befc9701aedbfd778d6f09a0a63389b9dedaac01..7b2e06622a121bbee7ce90fca51d3bb398500b41 100644
--- a/arduino/loadcell/loadcell.ino
+++ b/arduino/loadcell/loadcell.ino
@@ -10,6 +10,7 @@ const float calibration_weight = 4900.0;
float zero_reading, load_reading;
const float calibration_factor = -99.52;
+const float calibration_offset = 2674;
HX711 scale;
@@ -28,9 +29,10 @@ void setup() {
delay(3);
// determine the offset - setup: no load
- scale.tare(10); // sets the offset value within scale to current value of reading
- zero_reading = scale. //average over 10 vales
-
+ //scale.tare(10); // sets the offset value within scale to current value of reading
+ //Serial.print("The offset is: ");
+ //Serial.println(scale.get_offset()); //average over 10 vales
+ scale.set_offset(calibration_offset);
//delay(2);
//Serial.println("mount the known weight to the load cell and press ENTER to proceed with calibration");
diff --git a/c++/include/yumi.hpp b/c++/include/yumi.hpp
index 7a48c70fe01e307e68e81e612aa9458ebe1129cb..4b0621d97ffb8de0abc1a61f9da752d2d39dcc99 100644
--- a/c++/include/yumi.hpp
+++ b/c++/include/yumi.hpp
@@ -27,6 +27,8 @@ class Yumi {
void set_operationPoint(double op);
void set_hybridControl(bool hybridControl);
void set_transitionTime(double transitionTime);
+ void set_additionalManipConstraint(bool flag);
+ void set_nullspaceWeight(double nullspaceWeight);
void process();
@@ -56,6 +58,9 @@ class Yumi {
// gain for control
double m_kp = 1.0;
+ // activate computation of manipulability gradient
+ bool m_additionalManipConstraint = false;
+
// time for transition between position control to force control in the wires axis
double m_transitionTime = 0.0;
double m_deltaTime = 0.0;
@@ -63,6 +68,8 @@ class Yumi {
// manipulability meassure
double m_manipulabilty;
+ double m_nullspaceWeight = 1.0;
+
// vars to store configuration
rl::math::Vector3 m_desPosition = rl::math::Vector3::Zero();
rl::math::Vector3 m_desPositionDot = rl::math::Vector3::Zero();
@@ -89,13 +96,15 @@ class Yumi {
Eigen::Matrix3d m_selectVelMatrix = Eigen::Matrix3d::Identity();
Eigen::Matrix3d m_modSelectVelMatrix = Eigen::Matrix3d::Identity();
- // private functions
+
+ // private functions
void doForwardKinematics();
Eigen::Matrix3d euler2rotMatrix(rl::math::Vector3 orientation);
void compTaskSpaceInput();
void compForce2VelocityController();
void compIK();
void modifySelectionMatrix();
+ void computeManipulabilityGradient();
diff --git a/c++/models/urdf/yumi_left.urdf b/c++/models/urdf/yumi_left.urdf
index 5514de39875ec76e837581f289692c65bd32a72b..a25f237abea4f895ebcd3418784013517903e054 100644
--- a/c++/models/urdf/yumi_left.urdf
+++ b/c++/models/urdf/yumi_left.urdf
@@ -118,7 +118,7 @@
<joint name="yumi_joint_6_l" type="revolute">
<parent link="yumi_link_6_l"/>
<child link="yumi_link_7_l"/>
- <origin rpy="0.0 0.0 1.57079632679" xyz="0.0283 0.0575 -0.0005"/>
+ <origin rpy="0.0 0.0 1.57079632679" xyz="0.02716 0.056592 -0.001031"/>
<axis xyz="1 0 0"/>
<limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
<dynamics damping="0.5"/>
diff --git a/c++/models/urdf/yumi_left_original.urdf b/c++/models/urdf/yumi_left_original.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..e2e5b8220869f0cbd9f2373b2e7c11b876cdb8c0
--- /dev/null
+++ b/c++/models/urdf/yumi_left_original.urdf
@@ -0,0 +1,134 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from yumi.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="yumi" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <link name="yumi_base_link"/>
+ <joint name="yumi_base_link_to_body" type="fixed">
+ <parent link="yumi_base_link"/>
+ <child link="yumi_body"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </joint>
+ <link name="yumi_body">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="15"/>
+ <inertia ixx="0.5" ixy="0" ixz="0" iyy="0.6" iyz="0" izz="0.3"/>
+ </inertial>
+ </link>
+ <!--///////////////////////////////////// LEFT ARM //////////////////////////////// -->
+ <!-- joint between body and link_1_l -->
+ <joint name="yumi_joint_1_l" type="revolute">
+ <parent link="yumi_body"/>
+ <child link="yumi_link_1_l"/>
+ <origin rpy="0.9781 -0.5716 2.3180" xyz="0.05355 0.07250 0.41492"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_1_l">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_1_l and link_2_l -->
+ <joint name="yumi_joint_2_l" type="revolute">
+ <parent link="yumi_link_1_l"/>
+ <child link="yumi_link_2_l"/>
+ <origin rpy="1.57079632679 0.0 0.0" xyz="0.03 0.0 0.1"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.50454747661" upper="0.759218224618" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_2_l">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_2_l and link_3_l -->
+ <joint name="yumi_joint_7_l" type="revolute">
+ <parent link="yumi_link_2_l"/>
+ <child link="yumi_link_3_l"/>
+ <origin rpy="-1.57079632679 0.0 0.0" xyz="-0.03 0.17283 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_3_l">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_3_l and link_4_l -->
+ <joint name="yumi_joint_3_l" type="revolute">
+ <parent link="yumi_link_3_l"/>
+ <child link="yumi_link_4_l"/>
+ <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.04188 0.0 0.07873"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.15548162621" upper="1.3962634016" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_4_l">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_4_l and link_5_l -->
+ <joint name="yumi_joint_4_l" type="revolute">
+ <parent link="yumi_link_4_l"/>
+ <child link="yumi_link_5_l"/>
+ <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0405 0.16461 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-5.06145483078" upper="5.06145483078" velocity="6.98131700798"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_5_l">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_5_l and link_6_l -->
+ <joint name="yumi_joint_5_l" type="revolute">
+ <parent link="yumi_link_5_l"/>
+ <child link="yumi_link_6_l"/>
+ <origin rpy="1.57079632679 0.0 0.0" xyz="-0.027 0 0.10039"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-1.53588974176" upper="2.40855436775" velocity="6.98131700798"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_6_l">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_6_l and link_7_l -->
+ <joint name="yumi_joint_6_l" type="revolute">
+ <parent link="yumi_link_6_l"/>
+ <child link="yumi_link_7_l"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.027 0.029 0.0"/>
+ <axis xyz="1 0 0"/>
+ <limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_7_l">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+</robot>
+
diff --git a/c++/models/urdf/yumi_right.urdf b/c++/models/urdf/yumi_right.urdf
index a649b14ac966a971b67ad8ba6a09fc610ef50800..74a44ae5ba555e11161c00670659905204216ff3 100644
--- a/c++/models/urdf/yumi_right.urdf
+++ b/c++/models/urdf/yumi_right.urdf
@@ -118,7 +118,7 @@
<joint name="yumi_joint_6_r" type="revolute">
<parent link="yumi_link_6_r"/>
<child link="yumi_link_7_r"/>
- <origin rpy="0.0 0.0 1.57079632679" xyz="0.0273 0.1165 -0.0007"/>
+ <origin rpy="0.0 0.0 1.57079632679" xyz="0.027101 0.116653 -0.00047"/>
<axis xyz="1 0 0"/>
<limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
<dynamics damping="0.5"/>
diff --git a/c++/models/urdf/yumi_right_original.urdf b/c++/models/urdf/yumi_right_original.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..0d01412676763cfe148c00546b8261f63f86f945
--- /dev/null
+++ b/c++/models/urdf/yumi_right_original.urdf
@@ -0,0 +1,134 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from yumi.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="yumi" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <link name="yumi_base_link"/>
+ <joint name="yumi_base_link_to_body" type="fixed">
+ <parent link="yumi_base_link"/>
+ <child link="yumi_body"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </joint>
+ <link name="yumi_body">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="15"/>
+ <inertia ixx="0.5" ixy="0" ixz="0" iyy="0.6" iyz="0" izz="0.3"/>
+ </inertial>
+ </link>
+ <!--///////////////////////////////////// RIGHT ARM //////////////////////////////// -->
+ <!-- joint between body and link_1_r -->
+ <joint name="yumi_joint_1_r" type="revolute">
+ <parent link="yumi_body"/>
+ <child link="yumi_link_1_r"/>
+ <origin rpy="-0.9795 -0.5682 -2.3155" xyz="0.05355 -0.0725 0.41492"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_1_r">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_1_r and link_2_r -->
+ <joint name="yumi_joint_2_r" type="revolute">
+ <parent link="yumi_link_1_r"/>
+ <child link="yumi_link_2_r"/>
+ <origin rpy="1.57079632679 0.0 0.0" xyz="0.03 0.0 0.1"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.50454747661" upper="0.759218224618" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_2_r">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_2_r and link_3_r -->
+ <joint name="yumi_joint_7_r" type="revolute">
+ <parent link="yumi_link_2_r"/>
+ <child link="yumi_link_3_r"/>
+ <origin rpy="-1.57079632679 0.0 0.0" xyz="-0.03 0.17283 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_3_r">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_3_r and link_4_r -->
+ <joint name="yumi_joint_3_r" type="revolute">
+ <parent link="yumi_link_3_r"/>
+ <child link="yumi_link_4_r"/>
+ <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.04188 0.0 0.07873"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-2.15548162621" upper="1.3962634016" velocity="3.14159265359"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_4_r">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_4_r and link_5_r -->
+ <joint name="yumi_joint_4_r" type="revolute">
+ <parent link="yumi_link_4_r"/>
+ <child link="yumi_link_5_r"/>
+ <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0405 0.16461 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-5.06145483078" upper="5.06145483078" velocity="6.98131700798"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_5_r">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_5_r and link_6_r -->
+ <joint name="yumi_joint_5_r" type="revolute">
+ <parent link="yumi_link_5_r"/>
+ <child link="yumi_link_6_r"/>
+ <origin rpy="1.57079632679 0.0 0.0" xyz="-0.027 0 0.10039"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-1.53588974176" upper="2.40855436775" velocity="6.98131700798"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_6_r">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+ <!-- joint between link_6_r and link_7_r -->
+ <joint name="yumi_joint_6_r" type="revolute">
+ <parent link="yumi_link_6_r"/>
+ <child link="yumi_link_7_r"/>
+ <origin rpy="-1.57079632679 0.0 0.0" xyz="0.027 0.029 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
+ <dynamics damping="0.5"/>
+ </joint>
+ <link name="yumi_link_7_r">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="2"/>
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
+ </inertial>
+ </link>
+</robot>
+
diff --git a/c++/src/main.cpp b/c++/src/main.cpp
index 0587fe14c331c93897b71411ff9bafbda062165c..44f73f44c16cda94b50e9db81412fd53efb7ae53 100644
--- a/c++/src/main.cpp
+++ b/c++/src/main.cpp
@@ -10,35 +10,40 @@
int main(int, char**) {
- std::string path2yumi_l = "/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf";
- Yumi yumi_l(path2yumi_l);
+ std::string path2yumi_r = "/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf";
+ Yumi yumi_r(path2yumi_r);
Eigen::Matrix<double, 7, 1> jointAngles;
- jointAngles << 90.48, 17.87, -25.0, 48.0, -137.0, 122.0, -74.21;
+ //jointAngles << 90.48, 17.87, -25.0, 48.0, -137.0, 122.0, -74.21;
+ jointAngles << -110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0;
+ //jointAngles << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
jointAngles *= rl::math::DEG2RAD;
Eigen::Matrix<double, 7, 1> jointVelocities;
jointVelocities << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
- yumi_l.set_jointValues(jointAngles, jointVelocities);
+ yumi_r.set_jointValues(jointAngles, jointVelocities);
+ Eigen::Matrix<double, 6, 1> pose = yumi_r.get_pose();
+ pose.tail(3) *= rl::math::RAD2DEG;
+
+ std::cout << "current pose is: " << pose << std::endl;
+ /*
// Desired Values
Eigen::Matrix<double, 6, 1> desPose;
desPose << 0.300, 0.200, 0.200, 0.0*rl::math::DEG2RAD, 0.0*rl::math::DEG2RAD, 0.0*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;
- yumi_l.set_desPoseVel(desPose, desVelocity);
-
- yumi_l.process();
-
-
+
+ yumi_r.set_desPoseVel(desPose, desVelocity);
+ yumi_r.process();
Eigen::Matrix<double, 7, 1> newJointValues;
- newJointValues = yumi_l.get_newJointValues();
-
+ newJointValues = yumi_r.get_newJointValues();
std::cout << "old joint values: " << jointAngles << std::endl;
std::cout << "new joint values: " << newJointValues << std::endl;
+ */
diff --git a/c++/src/py2gpmbind.cpp b/c++/src/py2gpmbind.cpp
index 08efd535b5a5ec071ebe0d488acb7b8ee5221fe9..46484c6f3c0e7a88995a25934a4fd2d4126b851e 100644
--- a/c++/src/py2gpmbind.cpp
+++ b/c++/src/py2gpmbind.cpp
@@ -23,5 +23,8 @@ PYBIND11_MODULE(invKin, m) {
.def("set_hybridControl", &Yumi::set_hybridControl)
.def("set_transitionTime", &Yumi::set_transitionTime)
.def("get_manipulabilityMeasure", &Yumi::get_manipulabilityMeasure)
+ .def("set_additionalManipConstraint", &Yumi::set_additionalManipConstraint)
+ .def("set_nullspaceWeight", &Yumi::set_nullspaceWeight)
+ .def("set_driftCompGain", &Yumi::set_driftCompGain)
.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 cf7399c7019dd3b5c29303d15b1b67529971fe9d..235b4b559dbaffc3c51aa8a38dda1d76c3acb5db 100644
--- a/c++/src/yumi.cpp
+++ b/c++/src/yumi.cpp
@@ -87,6 +87,9 @@ void Yumi::compTaskSpaceInput(){
void Yumi::process(){
doForwardKinematics();
+ if(m_additionalManipConstraint){
+ computeManipulabilityGradient();
+ }
modifySelectionMatrix();
compForce2VelocityController();
compTaskSpaceInput();
@@ -117,16 +120,17 @@ void Yumi::compForce2VelocityController(){
velocityEE << 0, m_force-m_forceOP, 0;
velocityEE *= m_kp;
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;
+ //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;
+ //Eigen::Matrix<double, 7, 1> nullSpaceVelocity = -m_inverseWeighing * m_nullSpaceGradient;
+ Eigen::Matrix<double, 7, 1> nullSpaceVelocity = m_nullspaceWeight * m_nullSpaceGradient;
jointVelocities = broccoli::core::math::solvePseudoInverseEquation(m_jacobian, m_inverseWeighing, m_effectiveTaskSpaceInput,
nullSpaceVelocity, m_activationFactorTaskSpace);
@@ -180,4 +184,44 @@ void Yumi::modifySelectionMatrix(){
double Yumi::get_manipulabilityMeasure(){
return m_manipulabilty;
+}
+
+void Yumi::computeManipulabilityGradient(){
+ // Eigen mainly supports 2D. 3D matrices are available in develop branches. We will use a container of 2D
+ // matrices here.
+ std::array<Eigen::Matrix<double, 6, 7>, 7> Jdq; // NOLINT
+ for (auto& matrix : Jdq) {
+ matrix = Eigen::Matrix<double, 6, 7>::Zero();
+ }
+ Eigen::Matrix<double, 3, 7> transJ = Eigen::Matrix<double, 3, 7>::Zero();
+ transJ = m_jacobian.block<3, 7>(0, 0);
+ Eigen::Matrix<double, 3, 7> rotJ = Eigen::Matrix<double, 3, 7>::Zero();
+ rotJ = m_jacobian.block<3, 7>(3, 0);
+ const int numOfJoints = 7;
+ for (int jj = 0; jj < numOfJoints; ++jj) {
+ for (int ii = jj; ii < numOfJoints; ++ii) {
+ Jdq.at(jj).block<3, 1>(0, ii) = rotJ.col(jj).cross(transJ.col(ii));
+ Jdq.at(jj).block<3, 1>(3, ii) = rotJ.col(jj).cross(rotJ.col(ii));
+ if (ii != jj) {
+ Jdq.at(ii).block<3, 1>(0, jj) = Jdq.at(jj).block<3, 1>(0, ii);
+ }
+ }
+ }
+ //return Jdq;
+
+ // Current cost.
+ double cost = sqrt((m_jacobian * m_jacobian.transpose()).determinant());
+ // Compute the manipulability gradient.
+ //Eigen::Matrix<double, 7, 1> gradient = Eigen::Matrix<double, 7, 1>::Zero();
+ for (int jj = 0; jj < 7; ++jj) {
+ m_nullSpaceGradient[jj] = cost * ((m_jacobian * m_jacobian.transpose()).inverse() * Jdq.at(jj) * m_jacobian.transpose()).trace();
+ }
+}
+
+void Yumi::set_additionalManipConstraint(bool flag){
+ m_additionalManipConstraint = flag;
+}
+
+void Yumi::set_nullspaceWeight(double nullspaceWeight){
+ m_nullspaceWeight = nullspaceWeight;
}
\ 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 027c0725c17491f6daf52e15495830f35e1ba53c..195c1f5a087b792f5a13745378cd3d5999c4e5e0 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
@@ -9,7 +9,7 @@ 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 data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, logic2abb, get_step
from matplotlib import pyplot as plt
import serial.tools.list_ports
from tqdm import tqdm
@@ -24,15 +24,18 @@ def get_realJointAngles(egm_client):
joint7 = robot_msg.feedBack.externalJoints.joints[0]
conf.insert(2, joint7)
jointAngles = np.radians(np.array(conf))
- return jointAngles
+ pose = np.array([robot_msg.feedBack.cartesian.pos.x, robot_msg.feedBack.cartesian.pos.y, robot_msg.feedBack.cartesian.pos.z])
+ return jointAngles, pose
# 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 = get_step()
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.32, 0.3, 0.1]) # synced fine pose was np.array([0.3, 0.2, 0.2])
+p1_start_des = np.array([0.35, 0.1, 0.15]) # synced fine pose was
+#p1_start_des = np.array([0.31, 0.2, 0.2])
p1, p2 = place_trajectory(p1_start_des, p1, p2)
# setup for UDP connection
@@ -58,12 +61,14 @@ egm_client_R = EGMClient(port=UDP_PORT_RIGHT)
egm_client_L = EGMClient(port=UDP_PORT_LEFT)
yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
-yumi_right.set_operationPoint(5.0)
+yumi_right.set_operationPoint(4.0)
yumi_right.set_kp(0.05)
yumi_right.set_hybridControl(False)
yumi_right.set_transitionTime(3.0)
+yumi_right.set_additionalManipConstraint(True)
yumi_left = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
+yumi_left.set_additionalManipConstraint(True)
desVelocities_R_start = 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])
@@ -101,7 +106,7 @@ i = 0
force = 0.0 # 0.5 [N] seems to be a good value for the control
timestamp = time.time()
traj_samples = len(p1[:, 0])
-moveToStartVel = 0.005
+moveToStartVel = 0.05
# delta is for both arms the same
poseDelta = desPose_R_start - syncPose_R
@@ -118,11 +123,13 @@ toStartVel_R = (toStartPose_R[1, :] - toStartPose_R[0, :]) * rate
toStartVel_L = (toStartPose_L[1, :] - toStartPose_L[0, :]) * rate
# arrays to store the results for later plotting
+log_abbPos_R = np.zeros((traj_samples, 3))
log_realPose_R = np.zeros((traj_samples, 6))
log_compPose_R = np.zeros((traj_samples, 6))
log_realJoints_R = np.zeros((traj_samples, 7))
log_compJoints_R = np.zeros((traj_samples, 7))
+log_abbPos_L = np.zeros((traj_samples, 3))
log_realPose_L = np.zeros((traj_samples, 6))
log_compPose_L = np.zeros((traj_samples, 6))
log_realJoints_L = np.zeros((traj_samples, 7))
@@ -133,22 +140,22 @@ log_force = np.zeros((traj_samples, 1))
print("\n Force control only to tension the wire...")
foamCutting.start_syncronizing()
-
+force = 0
i = 0
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)/1000.0 # mN to Newton
+ force = float(str_receive)/1000.0 # mN to Newton
if foamCutting.is_moveToStartPose:
if (time.time() - timestamp) >= (1.0/rate):
timestamp = time.time()
# get the current joints angles for the right arm
- jointAngles_R = get_realJointAngles(egm_client_R)
- jointAngles_L = get_realJointAngles(egm_client_L)
+ jointAngles_R, _posR = get_realJointAngles(egm_client_R)
+ jointAngles_L, _posL= get_realJointAngles(egm_client_L)
# compute the resulting jointVelocities
if i > 0:
@@ -199,8 +206,8 @@ while True and arduino.isOpen():
timestamp = time.time()
# get the current joints angles for the right arm
- jointAngles_R = get_realJointAngles(egm_client_R)
- jointAngles_L = get_realJointAngles(egm_client_L)
+ jointAngles_R, _posR = get_realJointAngles(egm_client_R)
+ jointAngles_L, _posL = get_realJointAngles(egm_client_L)
# compute the resulting jointVelocities
if i > 0:
@@ -233,7 +240,8 @@ while True and arduino.isOpen():
jointAngles_L_old = copy.copy(jointAngles_L)
i = i+1
- if (force > 0.65) and keyboard.is_pressed('enter'):
+ if (force > 3) and keyboard.is_pressed('enter'):
+ #if keyboard.is_pressed('enter'):
i = 0 # reset counter
print("Changing to hybrid control now...")
foamCutting.start_cutting()
@@ -252,12 +260,16 @@ while True and arduino.isOpen():
phi_dot = dphi[i, :]
# get the current joints angles for the left arm
- jointAngles_L = get_realJointAngles(egm_client_L)
+ jointAngles_L, abb_posL = get_realJointAngles(egm_client_L)
log_realJoints_L[i, :] = jointAngles_L
+ log_abbPos_L[i, :] = abb_posL
# get the current joints angles for the right arm
- jointAngles_R = get_realJointAngles(egm_client_R)
+ jointAngles_R, abb_posR = get_realJointAngles(egm_client_R)
log_realJoints_R[i, :] = jointAngles_R
+ log_abbPos_R[i, :] = abb_posR
+
+ #print("getting egm messages takes " + str(time.time() - timestamp))
# compute the resulting jointVelocities
if i > 0:
@@ -296,6 +308,8 @@ while True and arduino.isOpen():
egm_client_L.send_planned_configuration(logic2abb(des_conf_L))
egm_client_R.send_planned_configuration(logic2abb(des_conf_R))
+ #print("time for egm message + IK takes " + str(time.time() - timestamp))
+
# save joint values to compute joint velocities in the next iteration
jointAngles_L_old = copy.copy(jointAngles_L)
jointAngles_R_old = copy.copy(jointAngles_R)
@@ -337,5 +351,8 @@ else:
raise TimeoutError(f"Joint positions for the left arm did not converge.")
-experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
-#np.save('./data/experimentLogs300-0,05-1-x', experimentLogs)
\ No newline at end of file
+
+experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L, log_abbPos_R, log_abbPos_L))
+#experimentLogs = np.hstack((p2, phi_delta, log_realPose_R, p1, log_realPose_L))
+
+#np.save('./plots/force_control/refKp0.05-Op4_v3', experimentLogs)
\ No newline at end of file
diff --git a/python/data/get_data.py b/python/data/get_data.py
index 8f9bf004e9866a1309a902b7b9523a1273ecff41..8c040582994f441f28a6468efea36b72f800704f 100644
--- a/python/data/get_data.py
+++ b/python/data/get_data.py
@@ -2,6 +2,23 @@ import numpy as np
import copy
from enum import Enum
+def get_step():
+ p1 = np.zeros((800, 3))
+ v1 = np.zeros((800, 3))
+ p2 = np.zeros((800, 3))
+ v2 = np.zeros((800, 3))
+ phi_delta = np.zeros((800, 3))
+ dphi = np.zeros((800, 3))
+
+ p1[50:800, 0] = 0.01
+ v1[49:50, 0] = 0.01/(1.0/80.0) # equals 0.8 m/s
+
+ p2[50:800, 0] = 0.01
+ p2[:, 2] = 0.4 # corresponds to -0.4 in y axis after the placement to yumi workspace
+ v2[49:50, 0] = 0.01/(1.0/80.0) # equals 0.8 m/s
+
+ return p1, v1, p2, v2, phi_delta, dphi
+
def get_trajectory():
try:
data = np.load('data/traj_data.npy')
diff --git a/python/experiment/plots.py b/python/experiment/plots.py
index d102870fce8dead8d8cd136e6a1245f9b86549ec..d40fc05639b10927a3201bfd3715b21b5d2f4807 100644
--- a/python/experiment/plots.py
+++ b/python/experiment/plots.py
@@ -22,6 +22,20 @@ time = np.linspace(0, round(1.0/80.0 * noSamples), num=noSamples)
time2 = np.linspace(0, round(1.0/80.0 * len(data2)), num=len(data2))
#time.shape = (time.size//1, 1)
+fig = plt.figure()
+
+p2_is = realPose[:,0:3]
+p2_des = desPose[:, 0:3]
+e = p2_des - p2_is
+ax1 = fig.add_subplot(111)
+ax1.plot(e[0:-10, 1])
+ax1.set_ylabel('mm')
+ax1.set_xlabel('samples')
+plt.show()
+
+
+
+
fig = plt.figure()
diff --git a/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py b/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py
index 3801b7974d0af3944d57c0c10d535f8f71355988..98709b9c532b201410941b309b5d4801bd81c281 100644
--- a/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py
+++ b/python/optimizeTaskSpacePlacement/checkManipulabiltyMeasure.py
@@ -22,6 +22,11 @@ rate = 1.0/80.0
yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
yumi_left = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
+yumi_right.set_additionalManipConstraint(True)
+#yumi_right.set_nullspaceWeight(100.0)
+yumi_left.set_additionalManipConstraint(True)
+#yumi_left.set_nullspaceWeight(100.0)
+
manipulability_R = np.zeros((len(p1)))
manipulability_L = np.zeros((len(p1)))
manipulabilityMin_R = np.zeros((3, 3, 3))
@@ -43,19 +48,21 @@ myY_L = []
myZ_L = []
myMan_L = []
+smallestMan = []
+
oldPos = np.zeros((3))
newPos = np.zeros((3))
-for x in range(-10, 11, 10):
+for x in range(-20, 21, 5):
# add 5 cm to every point of trajectory in x direction
dx = np.array([x*0.01, 0.0, 0.0])
dX = np.ones((len(p1), 3)) * dx # elementwise multiplication
- for y in range(-10, 11, 10):
+ for y in range(-20, 21, 5):
dy = np.array([0.0, y*0.01, 0.0])
dY = np.ones((len(p1), 3)) * dy # elementwise multiplication
- for z in range(-10, 11, 10):
+ for z in range(-10, 31, 5):
dz = np.array([0.0, 0.0, z*0.01])
dZ = np.ones((len(p1), 3)) * dz # elementwise multiplication
p1_ = copy.copy(p1) + dX + dY + dZ
@@ -125,13 +132,22 @@ for x in range(-10, 11, 10):
myZ_L.append(p1_[0, 2])
myMan_L.append(np.min(manipulability_L))
-print("manipulability measure min right: ", manipulabilityMin_R)
-print("manipulability measure min left: ", manipulabilityMin_L)
+ if (np.min(manipulability_R) < np.min(manipulability_L)): # if right is smaller
+ smallestMan.append(np.min(manipulability_R))
+ else:
+ smallestMan.append(np.min(manipulability_L))
+#print("manipulability measure min right: ", manipulabilityMin_R)
+#print("manipulability measure min left: ", manipulabilityMin_L)
+plot_path = '/home/joschua/Coding/forceControl/master-project/python/plots/taskSpacePlacement/'
+np.save(plot_path +'myX_L_nullgradient', myX_L)
+np.save(plot_path +'myY_L_nullgradient', myY_L)
+np.save(plot_path +'myZ_L_nullgradient', myZ_L)
+np.save(plot_path +'smallestMan_nullgradient', smallestMan)
-def scatter3d(x,y,z, cs, colorsMap='jet'):
+""" def scatter3d(x,y,z, cs, colorsMap='jet'):
cm = plt.get_cmap(colorsMap)
cNorm = matplotlib.colors.Normalize(vmin=min(cs), vmax=max(cs))
scalarMap = cmx.ScalarMappable(norm=cNorm, cmap=cm)
@@ -140,10 +156,10 @@ def scatter3d(x,y,z, cs, colorsMap='jet'):
ax.scatter(x, y, z, c=scalarMap.to_rgba(cs))
scalarMap.set_array(cs)
fig.colorbar(scalarMap)
- plt.show()
+ plt.show() """
-scatter3d(np.array(myX_R), np.array(myY_R), np.array(myZ_R), np.array(myMan_R))
-scatter3d(np.array(myX_L), np.array(myY_L), np.array(myZ_L), np.array(myMan_L))
-scatter3d(np.array(myX_L), np.array(myY_L), np.array(myZ_L), np.array(myMan_L)+np.array(myMan_R))
+#scatter3d(np.array(myX_R), np.array(myY_R), np.array(myZ_R), np.array(myMan_R))
+#scatter3d(np.array(myX_L), np.array(myY_L), np.array(myZ_L), np.array(myMan_L))
+#scatter3d(np.array(myX_L), np.array(myY_L), np.array(myZ_L), np.array(myMan_L)+np.array(myMan_R))
# do not add, but search iterate through and choose index where both array entries are higher
diff --git a/python/plots/control_position/plotting.py b/python/plots/control_position/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..0240844a762979583a9908caf235105db49f483c
--- /dev/null
+++ b/python/plots/control_position/plotting.py
@@ -0,0 +1,170 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.gridspec import GridSpec
+
+
+R_01_80 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
+R_01_250 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250.npy')
+
+''' Only 300-351015 and 300-351015-rawc20_v2 were saved in this format'''
+#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
+datao = np.load('./plots/control_position/300-351015.npy') # o for original
+p1_des = datao[:, 12:15]* 1000
+p1_is = datao[:, 15:18]* 1000
+p2_des = datao[:, 0:3]* 1000
+p2_is = datao[:, 6:9]* 1000
+e = (p1_des - p1_is)
+
+''' Measurements were saved in this format '''
+#p2, phi_delta, log_realPose_R, p1, log_realPose_L))
+dataf80 = np.load('./plots/control_position/300-351015-rawf80.npy')
+p1_desf80 = dataf80[:, 12:15]* 1000
+p1_isf80 = dataf80[:, 15:18]* 1000
+#p1_desf80 = dataf80[:, 0:3]* 1000
+#p1_isf80 = dataf80[:, 6:9]* 1000
+# error in workspace
+ef80 = (p1_desf80 - p1_isf80)
+
+datac20 = np.load('./plots/control_position/300-351015-rawc20.npy')
+p1_desc20 = datac20[:, 12:15]* 1000
+p1_isc20 = datac20[:, 15:18]* 1000
+# error in workspace
+ec20 = (p1_desc20 - p1_isc20)
+
+datatc250 = np.load('./plots/control_position/300-351015-rawtc250.npy')
+p1_destc250 = datatc250[:, 12:15]* 1000
+p1_istc250 = datatc250[:, 15:18]* 1000
+# error in workspace
+etc250 = (p1_destc250 - p1_istc250)
+
+
+#noSamples = len(p1_desf80)
+noSamples80 = len(ef80)
+noSamples250 = len(etc250)
+
+# transform error into eef80frame
+for i in range(noSamples80):
+ e[i, :] = np.transpose(R_01_80[i] @ e[i, :].T)
+ ef80[i, :] = np.transpose(R_01_80[i] @ ef80[i, :].T)
+ ec20[i, :] = np.transpose(R_01_80[i] @ ec20[i, :].T)
+
+for i in range(noSamples250):
+ etc250[i, :] = np.transpose(R_01_250[i] @ etc250[i, :].T)
+
+time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+time250 = np.linspace(0, round(1.0/250.0 * noSamples250), num=noSamples250)
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+# error plot
+fig = plt.figure()
+
+ax1 = fig.add_subplot(311)
+ax2 = fig.add_subplot(312)
+ax3 = fig.add_subplot(313)
+
+ax1.plot(time80 [30:-10], e [30:-10, 0])
+ax1.plot(time80 [30:-10], ec20 [30:-10, 0], linestyle='dashed')
+ax1.plot(time80 [30:-10], ef80 [30:-10, 0], linestyle='dotted')
+ax1.plot(time250[30:-10], etc250[30:-10, 0])
+
+ax2.plot(time80 [30:-10], e [30:-10, 1])
+ax2.plot(time80 [30:-10], ec20 [30:-10, 1], linestyle='dashed')
+ax2.plot(time80 [30:-10], ef80 [30:-10, 1], linestyle='dotted')
+ax2.plot(time250[30:-10], etc250[30:-10, 1])
+
+ax3.plot(time80 [30:-10], e [30:-10, 2])
+ax3.plot(time80 [30:-10], ec20 [30:-10, 2], linestyle='dashed')
+ax3.plot(time80 [30:-10], ef80 [30:-10, 2], linestyle='dotted')
+ax3.plot(time250[30:-10], etc250[30:-10, 2])
+
+
+#ax4 = ax2.twinx()
+#ax4.plot(time[0:-10], force[0:-10], label='force', color='r')
+#ax4.set_ylim(-1, 1)
+#ax4.set_ylabel('N')
+
+#ax1.set_title('position error in EE frame')
+ax1.set_ylabel('$x_{EE,R}$ in mm')
+ax2.set_ylabel('$y_{EE,R}$ in mm')
+ax3.set_ylabel('$z_{EE,R}$ in mm')
+ax3.set_xlabel('time in s')
+
+labels = ["case 1", "case 2", "case 3", "case 4"]
+fig.legend( labels=labels,
+ loc="upper center", ncol=4)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+fig.subplots_adjust(top=0.9)
+plt.show()
+
+# profile plot
+""" fig = plt.figure() """
+""" """
+""" ax1 = fig.add_subplot(311) """
+""" ax2 = fig.add_subplot(312) """
+""" ax3 = fig.add_subplot(323) """
+
+fig = plt.figure(constrained_layout=True)
+gs = GridSpec(2, 2, figure=fig)
+
+ax1 = fig.add_subplot(gs[0, :])
+ax2 = fig.add_subplot(gs[1, 0:1])
+ax3 = fig.add_subplot(gs[1, 1:2])
+
+
+ax1.plot(p1_is [30:-10, 0], p1_is [30:-10, 2])
+ax1.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
+ax1.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
+ax1.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
+ax1.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted')
+
+ax2.plot(p1_is [30:-10, 0], p1_is [30:-10, 2])
+ax2.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
+ax2.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
+ax2.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
+ax2.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted')
+ax2.set_ylim([147.5, 152])
+ax2.set_xlim([359, 361])
+
+ax3.plot(p1_is [30:-10, 0], p1_is [30:-10, 2])
+ax3.plot(p1_isc20 [30:-10, 0], p1_isc20 [30:-10, 2])
+ax3.plot(p1_isf80 [30:-10, 0], p1_isf80 [30:-10, 2])
+ax3.plot(p1_istc250 [30:-10, 0], p1_istc250 [30:-10, 2])
+ax3.plot(p1_des [30:-10, 0], p1_des [30:-10, 2], linestyle='dotted')
+ax3.set_xlim([519, 520.4])
+ax3.set_ylim([153, 153.6])
+
+ax1.set_ylabel('$z$ in mm')
+ax2.set_ylabel('$z$ in mm')
+ax1.set_xlabel('$x$ in mm')
+ax2.set_xlabel('$x$ in mm')
+ax3.set_xlabel('$x$ in mm')
+
+labels = ["case 1", "case 2", "case 3", "case 4", "reference"]
+fig.legend( labels=labels,
+ loc="upper center", ncol=5)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+fig.subplots_adjust(top=0.9)
+plt.show()
+
+# make a 3D visualization
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+ax.plot(p2_des[0:-10,0], p2_des[0:-10,1], p2_des[0:-10,2])
+ax.plot(p2_is[0:-10,0], p2_is[0:-10,1], p2_is[0:-10,2])
+#ax.set_xlim(54, 66)
+#ax.set_ylim(99, 101)
+ax.view_init(elev=25., azim=-115.)
+plt.show()
diff --git a/python/plots/force_control/plotting.py b/python/plots/force_control/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..99892da5a28d6aa2648d37070a027aff4755ca21
--- /dev/null
+++ b/python/plots/force_control/plotting.py
@@ -0,0 +1,188 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.gridspec import GridSpec
+from numpy.linalg import norm
+
+
+R_01_80 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01.npy')
+R_01_250 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250.npy')
+
+data01 = np.load('./plots/force_control/Kp0.1-Op4.npy') # o for original
+force01 = data01[:, 32:33]-4
+
+data015 = np.load('./plots/force_control/Kp0.15-Op4.npy') # o for original
+force015 = data015[:, 32:33]-4
+
+datanof = np.load('./plots/force_control/noforceControl.npy') # o for original
+forcenof = datanof[:, 32:33]-4
+
+# dataabb = np.load('./plots/force_control/refKp0.05-Op4_v2.npy') # o for original
+# p2_abb = dataabb[:, 63:66]
+# p1_abb = dataabb[:, 66:69]
+
+data_newUrdf = np.load('./plots/force_control/refKp0.05-Op4_v3.npy') # o for original
+p1_is_new = data_newUrdf[:, 43:46]* 1000
+p2_is_new = data_newUrdf[:, 12:15]* 1000
+p2_abb = data_newUrdf[:, 63:66]
+p1_abb = data_newUrdf[:, 66:69]
+
+
+#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
+datao = np.load('./plots/force_control/Kp0.05-Op4.npy') # o for original
+force = datao[:, 32:33]
+p1_des = datao[:, 33:36]* 1000
+p2_des = datao[:, 0:3]* 1000
+p1_is = datao[:, 43:46]* 1000
+p2_is = datao[:, 12:15]* 1000
+e_f = force - 4.0
+e_p = p2_des - p2_is
+
+noSamples80 = len(e_p)
+
+
+# transform error into eef80frame
+for i in range(noSamples80):
+ e_p[i, :] = np.transpose(R_01_80[i] @ e_p[i, :].T)
+
+
+time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+
+distance = norm(p1_is_new - p2_is_new, axis=1)
+distance_abb = norm(p1_abb - p2_abb, axis=1)
+
+
+y = e_f[10:1000]
+
+n = len(y) # length of the signal
+k = np.arange(n)
+T = n/80
+frq = k/T # two sides frequency range
+frq = frq[:len(frq)//2] # one side frequency range
+
+Y = np.fft.fft(y)/n # dft and normalization
+Y = Y[:n//2]
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+# error plot
+fig = plt.figure(figsize=(6.25, 2))
+
+ax1 = fig.add_subplot(111)
+ax1.plot(time80[0:-10] , e_p [0:-10, 1], label='position error')
+ax1.set_ylabel('$y_{EE, R}$ in mm')
+ax1.set_xlabel('time in s')
+
+ax4 = ax1.twinx()
+ax4.plot(time80[0:-10] , e_f[0:-10] , color=next(ax1._get_lines.prop_cycler)['color'], label='force')
+ax4.set_ylim(-1, 1)
+ax4.set_ylabel('force in N')
+labels = ["position error", "force error"]
+fig.legend( labels=labels,
+ loc='upper right', bbox_to_anchor=(0.6, 0.92),
+ ncol=1)
+
+fig.tight_layout()
+#fig.subplots_adjust(top=0.9)
+plt.show()
+
+
+
+fig = plt.figure(figsize=(6.25, 2))
+ax2 = fig.add_subplot(111)
+#ax1.set_title('position error in EE frame')
+ax2.plot(time80[0:-10] , distance [0:-10], label='comp. pose')
+ax2.plot(time80[0:-10] , distance_abb [0:-10], label='meas. pose')
+ax2.set_ylabel('$l^2$-norm in mm')
+plt.yticks([402, 402.5, 403, 403.5, 404])
+ax2.set_xlabel('time in s')
+ax2.legend()
+fig.tight_layout()
+#plt.title('hybrid control at Kp=0.05 and OP=4N')
+#fig.subplots_adjust(top=0.9)
+plt.show()
+
+
+
+fig = plt.figure()
+
+ax3 = fig.add_subplot(111)
+ax3.plot(frq, abs(Y), color=next(ax2._get_lines.prop_cycler)['color'], label='force')
+ax3.set_xlabel('freq (Hz)')
+ax3.set_ylabel('|Y(freq)|')
+ax3.legend()
+
+
+
+#labels = ["case 1", "case 2", "case 3", "case 4"]
+#fig.legend( labels=labels,
+# loc="upper center", ncol=4)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+#fig.subplots_adjust(top=0.9)
+plt.show()
+
+
+# make a 3D visualization
+# right
+fig = plt.figure(figsize=(3, 3))
+ax = fig.add_subplot(111, projection='3d')
+ax.plot(p2_des[0:-10,0], p2_des[0:-10,1], p2_des[0:-10,2], label='des. pose')
+#ax.plot(p2_is[0:-10,0], p2_is[0:-10,1], p2_is[0:-10,2], label='real_pathR')
+ax.plot(p2_is_new[0:-10,0], p2_is_new[0:-10,1], p2_is_new[0:-10,2], label='comp. pose')
+ax.plot(p2_abb[0:-10,0], p2_abb[0:-10,1], p2_abb[0:-10,2], label='meas. pose')
+
+#ax.set_xlim(54, 66)
+#ax.set_ylim(99, 101)
+#ax.set_xlabel('$x$ in mm')
+ax.set_ylabel('$y$ in mm')
+ax.set_zlabel('$z$ in mm')
+ax.view_init(elev=33., azim=0.)
+ax.legend(loc='upper right')
+#fig.tight_layout()
+plt.show()
+
+#left
+fig = plt.figure(figsize=(3, 3))
+ax2 = fig.add_subplot(111, projection='3d')
+ax2.plot(p1_des[0:-10,0], p1_des[0:-10,1], p1_des[0:-10,2], label='des. pose')
+#ax2.plot(p1_is[0:-10,0], p1_is[0:-10,1], p1_is[0:-10,2], label='real_pathR')
+ax2.plot(p1_is_new[0:-10,0], p1_is_new[0:-10,1], p1_is_new[0:-10,2], label='comp. pose')
+ax2.plot(p1_abb[0:-10,0], p1_abb[0:-10,1], p1_abb[0:-10,2], label='meas. pose')
+ax2.set_xlabel('$x$ in mm')
+ax2.set_ylabel('$y$ in mm')
+#ax2.set_zlabel('$z$ in mm')
+ax2.set_ylim(98, 101)
+ax2.view_init(elev=33., azim=0.)
+ax2.legend(loc='upper right')
+plt.yticks([98, 99, 100, 101])
+#fig.tight_layout()
+plt.show()
+
+
+fig = plt.figure()
+
+ax1 = fig.add_subplot(111)
+ax1.plot(time80[0:-10], force015[0:-10], label='0.15')
+ax1.plot(time80[0:-10], force01[0:-10], label='0.1')
+ax1.plot(time80[0:-10], e_f[0:-10], label='0.05')
+ax1.plot(time80[0:-10], forcenof[0:-10], label='pure position')
+
+ax1.set_ylabel('N')
+ax1.set_xlabel('s')
+ax1.legend()
+plt.title('force errors in comparision')
+fig.tight_layout()
+plt.show()
+
+
+
diff --git a/python/plots/ik_tuning/plotting.py b/python/plots/ik_tuning/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..7fe4cb22f17fea770928c8819e52bbc853881f8b
--- /dev/null
+++ b/python/plots/ik_tuning/plotting.py
@@ -0,0 +1,215 @@
+from turtle import color
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib.patches import Rectangle
+from numpy.linalg import norm
+
+import_path = '/home/joschua/Coding/forceControl/master-project/python/plots/ik_tuning/'
+error_right = np.load(import_path+'error_right.npy')
+#el00 = np.load(import_path+'error_left0.npy')
+#el01 = np.load(import_path+'error_left0.1.npy')
+error_left = np.load(import_path+'error_left1.npy')
+#el2 = np.load(import_path+'error_left2.npy')
+#el5 = np.load(import_path+'error_left5.npy')
+#error_left0 = np.load(import_path+'error_left10.npy')
+computedPose_left = np.load(import_path+'computedPose_left.npy')
+computedPose_right= np.load(import_path+'computedPose_right.npy')
+p1 = np.load(import_path+'p1.npy')
+p2 = np.load(import_path+'p2.npy')
+compJointAngles_right = np.load(import_path+'compJointAngles_right.npy')
+compJointAngles_left = np.load(import_path+'compJointAngles_left.npy')
+jointVelociies_left = np.load(import_path+'jointVelocities_left.npy')
+jointVelociies_right = np.load(import_path+'jointVelocities_right.npy')
+
+noSamples = len(jointVelociies_left)
+time = np.linspace(0, round(1.0/80.0 * noSamples), num=noSamples)
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+for i in range(len(error_left)):
+ if error_left[i,3] < -np.pi/2:
+ error_left[i,3] = error_left[i,3] + np.pi
+ elif error_left[i,3] > np.pi/2:
+ error_left[i,3] = error_left[i,3] - np.pi
+ if error_left[i,4] < -np.pi/2:
+ error_left[i,4] = error_left[i,4] + np.pi
+ elif error_left[i,4] > np.pi/2:
+ error_left[i,4] = error_left[i,4] - np.pi
+ if error_left[i,5] < -np.pi/2:
+ error_left[i,5] = error_left[i,5] + np.pi
+ elif error_left[i,5] > np.pi/2:
+ error_left[i,5] = error_left[i,5] - np.pi
+
+""" fig = plt.figure()
+plt.plot(error_left[10:-1, 3:6])
+plt.show()
+ """
+print('biggest translational error norm' + str(max(np.linalg.norm(error_left[10:-1,0:3]*1000, axis=1))))
+print('biggest rotational error x' + str(max(error_left[10:-1,3])))
+print('biggest rotational error y' + str(max(error_left[10:-1,4])))
+print('biggest rotational error z' + str(max(error_left[10:-1,5])))
+
+# see development of joint values
+""" fig = plt.figure()
+plt.plot(compJointAngles_left[:,0], label='joint1')
+plt.plot(compJointAngles_left[:,1], label='joint2')
+plt.plot(compJointAngles_left[:,2], label='joint3')
+plt.plot(compJointAngles_left[:,3], label='joint4')
+plt.plot(compJointAngles_left[:,4], label='joint5')
+plt.plot(compJointAngles_left[:,5], label='joint6')
+plt.plot(compJointAngles_left[:,6], label='joint7')
+plt.title('joint values over samples, left arm')
+plt.legend()
+plt.show() """
+
+""" base_error = np.array((el00[0]))
+
+error_left = np.concatenate(([base_error], error_left), axis=0)
+el01 = np.concatenate(([base_error], el01), axis=0)
+el00 = np.concatenate(([base_error], el00), axis=0) """
+
+
+""" # error
+fig = plt.figure(figsize=(6.25, 2))
+ax = fig.add_subplot(111)
+#ax.plot(time, np.linalg.norm(el00[:,0:3]*1000, axis=1), label='$0.0$')
+#ax.plot(time, np.linalg.norm(el01[:,0:3]*1000, axis=1), label='$0.1$')
+#ax.plot(time, np.linalg.norm(error_left[:,0:3]*1000, axis=1), label='$1.0$', color='#333333')
+ax.plot(np.linalg.norm(error_left[:,0:3]*1000, axis=1), label='$1.0$', color='#333333')
+ax.set_ylabel('mm')
+ax.set_xlabel('s')
+#plt.plot(el2[:,0], label='el2')
+#plt.plot(el5[:,0], label='el5')
+
+
+#plt.plot(error_left[:,1], label='y')
+#plt.plot(error_left[:,2], label='z')
+#plt.plot(error_left[:,3], label='rx')
+#plt.plot(error_left[:,4], label='ry')
+#plt.plot(error_left[:,5], label='rz')
+plt.legend()
+fig.tight_layout()
+plt.show() """
+
+""" # show trajectory in workspace of yumi
+fig = plt.figure()
+plt.plot(p1[:,0], p1[:,2], label='desired profile') # plot z over x
+plt.scatter(computedPose_left[:,0], computedPose_left[:,2], label='resulting pose from IK')
+fig.get_axes()[0].set_xlabel('x axis of yumi')
+fig.get_axes()[0].set_ylabel('z axis of yumi')
+plt.legend()
+plt.title('poses left')
+plt.show() """
+
+""" fig = plt.figure()
+plt.plot(computedPose_left[:,3], label='rx')
+plt.plot(computedPose_left[:,4], label='ry')
+plt.plot(computedPose_left[:,5], label='rz')
+plt.legend()
+plt.title('euler angles over trajectories')
+plt.show() """
+
+""" fig = plt.figure()
+plt.plot(compJointAngles_right[:,0], label='joint1')
+plt.plot(compJointAngles_right[:,1], label='joint2')
+plt.plot(compJointAngles_right[:,2], label='joint3')
+plt.plot(compJointAngles_right[:,3], label='joint4')
+plt.plot(compJointAngles_right[:,4], label='joint5')
+plt.plot(compJointAngles_right[:,5], label='joint6')
+plt.plot(compJointAngles_right[:,6], label='joint7')
+plt.title('joint values over samples, right arm')
+plt.legend()
+plt.show() """
+
+""" fig = plt.figure()
+plt.plot(error_right[:,0], label='x')
+plt.plot(error_right[:,1], label='y')
+plt.plot(error_right[:,2], label='z')
+#plt.plot(error_right[:,3], label='rx')
+#plt.plot(error_right[:,4], label='ry')
+#plt.plot(error_right[:,5], label='rz')
+plt.legend()
+plt.title('errors right')
+plt.show()
+
+# show trajectory in workspace of yumi
+fig = plt.figure()
+plt.plot(p2[:,0], p2[:,2], label='desired profile') # plot z over x
+plt.scatter(computedPose_right[:,0], computedPose_right[:,2], label='resulting pose from IK')
+fig.get_axes()[0].set_xlabel('x axis of yumi')
+fig.get_axes()[0].set_ylabel('z axis of yumi')
+plt.legend()
+plt.title('poses right')
+plt.show() """
+
+
+fig = plt.figure(figsize=(6.25, 5))
+
+ax1 = fig.add_subplot(221)
+ax2 = fig.add_subplot(223)
+ax3 = fig.add_subplot(222)
+ax4 = fig.add_subplot(224)
+
+ax1.plot(time[10:-1], compJointAngles_right[10:-1,0], label='joint1')
+ax1.plot(time[10:-1], compJointAngles_right[10:-1,1], label='joint2')
+ax1.plot(time[10:-1], compJointAngles_right[10:-1,2], label='joint3')
+ax1.plot(time[10:-1], compJointAngles_right[10:-1,3], label='joint4')
+ax1.plot(time[10:-1], compJointAngles_right[10:-1,4], label='joint5')
+ax1.plot(time[10:-1], compJointAngles_right[10:-1,5], label='joint6')
+ax1.plot(time[10:-1], compJointAngles_right[10:-1,6], label='joint7')
+
+ax2.plot(time[10:-1], jointVelociies_right[10:-1,0], label='joint1')
+ax2.plot(time[10:-1], jointVelociies_right[10:-1,1], label='joint2')
+ax2.plot(time[10:-1], jointVelociies_right[10:-1,2], label='joint3')
+ax2.plot(time[10:-1], jointVelociies_right[10:-1,3], label='joint4')
+ax2.plot(time[10:-1], jointVelociies_right[10:-1,4], label='joint5')
+ax2.plot(time[10:-1], jointVelociies_right[10:-1,5], label='joint6')
+ax2.plot(time[10:-1], jointVelociies_right[10:-1,6], label='joint7')
+
+ax3.plot(time[10:-1], compJointAngles_left[10:-1,0], label='joint1')
+ax3.plot(time[10:-1], compJointAngles_left[10:-1,1], label='joint2')
+ax3.plot(time[10:-1], compJointAngles_left[10:-1,2], label='joint3')
+ax3.plot(time[10:-1], compJointAngles_left[10:-1,3], label='joint4')
+ax3.plot(time[10:-1], compJointAngles_left[10:-1,4], label='joint5')
+ax3.plot(time[10:-1], compJointAngles_left[10:-1,5], label='joint6')
+ax3.plot(time[10:-1], compJointAngles_left[10:-1,6], label='joint7')
+
+ax4.plot(time[10:-1], jointVelociies_left[10:-1,0], label='joint1')
+ax4.plot(time[10:-1], jointVelociies_left[10:-1,1], label='joint2')
+ax4.plot(time[10:-1], jointVelociies_left[10:-1,2], label='joint3')
+ax4.plot(time[10:-1], jointVelociies_left[10:-1,3], label='joint4')
+ax4.plot(time[10:-1], jointVelociies_left[10:-1,4], label='joint5')
+ax4.plot(time[10:-1], jointVelociies_left[10:-1,5], label='joint6')
+ax4.plot(time[10:-1], jointVelociies_left[10:-1,6], label='joint7')
+
+ax1.set_title('')
+ax1.set_ylabel('angle in rad')
+#ax1.set_xlabel('s')
+ax2.set_ylabel('velocity in rad/s')
+ax2.set_xlabel('time in s')
+#ax3.set_ylabel('rad')
+#ax3.set_xlabel('s')
+#ax4.set_ylabel('rad/s')
+ax4.set_xlabel('time in s')
+#ax1.legend()
+#ax2.legend()
+#ax3.legend()
+#ax4.legend()
+labels = ["joint 1", "joint 2", "joint 3", "joint 4", "joint 5", "joint 6", "joint 7",]
+fig.legend( labels=labels,
+ loc="upper center", ncol=4)
+fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+fig.subplots_adjust(top=0.85)
+plt.show()
\ No newline at end of file
diff --git a/python/plots/postprocessing/plotting.py b/python/plots/postprocessing/plotting.py
index a70106273df6f76e07babc090ef9077244e450c0..7b53866c08f22fe5535c519676df9a85afbc3a7d 100644
--- a/python/plots/postprocessing/plotting.py
+++ b/python/plots/postprocessing/plotting.py
@@ -6,7 +6,7 @@ R_01 = np.load('/home/joschua/Coding/forceControl/master-project/python/plots/po
#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
-data = np.load('./data/experimentLogs300-0,2-1.npy')
+data = np.load('./data/experimentLogs300-0,4-1.npy')
p2_des = data[:, 0:3]
p2_is = data[:, 12:15]
p2_comp = data[:, 6:9]
diff --git a/python/plots/preprocessing/plotting.py b/python/plots/preprocessing/plotting.py
index aa92766abbefd1c3ce53a9e638e4bccc71a5d03b..a803585275f4a44c4ad57775a84fbd13a5dd75dc 100644
--- a/python/plots/preprocessing/plotting.py
+++ b/python/plots/preprocessing/plotting.py
@@ -27,8 +27,8 @@ plt.rcParams.update({
"savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
})
-""" # do plots for tcp1
-#fig = plt.figure()
+ # do plots for tcp1
+""" #fig = plt.figure()
fig = plt.figure(figsize=(6.25, 2))
ax = fig.add_subplot(111)
ax.scatter(p1[:,0]*1000, p1[:,1]*1000-53.7, label='$p_{1*}$', color='#005293')
@@ -68,9 +68,9 @@ ax.legend()
#plt.ylim(bottom=0)
fig.tight_layout()
plt.show()
+
-"""
# make a 3D visualization
fig = plt.figure(figsize=(10, 5))
ax = fig.add_subplot(111, projection='3d')
@@ -116,4 +116,4 @@ ax.set_zlabel('$y_2$')
ax.legend()
plt.show()
-fig.tight_layout() """
\ No newline at end of file
+fig.tight_layout()
\ No newline at end of file
diff --git a/python/plots/step_response/plotting.py b/python/plots/step_response/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..13e41aacd7429af475155c1898afa7868533bbf1
--- /dev/null
+++ b/python/plots/step_response/plotting.py
@@ -0,0 +1,54 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.gridspec import GridSpec
+
+
+
+''' Only 300-351015 and 300-351015-rawc20_v2 were saved in this format'''
+#experimentLogs = np.hstack((p2, phi_delta, log_compPose_R, log_realPose_R, log_compJoints_R, log_realJoints_R, log_force, p1, log_compJoints_R, log_realPose_L, log_compJoints_L, log_realJoints_L))
+datao = np.load('./plots/step_response/step_x.npy') # o for original
+p1_des = datao[:, 33:36]* 1000
+p1_is = datao[:, 43:46]* 1000
+e = (p1_des - p1_is)
+
+#noSamples = len(p1_desf80)
+noSamples80 = len(p1_des)
+
+time80 = np.linspace(0, round(1.0/80.0 * noSamples80), num=noSamples80)
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+# error plot
+fig = plt.figure()
+
+ax1 = fig.add_subplot(311)
+ax2 = fig.add_subplot(312)
+#ax3 = fig.add_subplot(313)
+
+ax1.plot(time80 [30:-10], e [30:-10, 0])
+ax2.plot(time80 [30:-10], p1_des [30:-10, 0])
+ax2.plot(time80 [30:-10], p1_is [30:-10, 0])
+
+
+#ax1.set_title('position error in EE frame')
+ax1.set_ylabel('$x$ in mm')
+ax2.set_ylabel('$x$ in mm')
+ax2.set_xlabel('s')
+
+#labels = ["case 1", "case 2", "case 3", "case 4"]
+#fig.legend( labels=labels,
+# loc="upper center", ncol=4)
+#fig.subplots_adjust(top=0.9, hspace=0.3)
+fig.tight_layout()
+#fig.subplots_adjust(top=0.9)
+plt.show()
diff --git a/python/plots/taskSpacePlacement/plotting.py b/python/plots/taskSpacePlacement/plotting.py
new file mode 100644
index 0000000000000000000000000000000000000000..251bde98fabce634e5c79efb47f8710737ed5fca
--- /dev/null
+++ b/python/plots/taskSpacePlacement/plotting.py
@@ -0,0 +1,59 @@
+from cgitb import small
+from operator import index
+from unicodedata import decimal
+import matplotlib.cm as cmx
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib import pyplot as plt
+import matplotlib
+
+import numpy as np
+
+import_path = '/home/joschua/Coding/forceControl/master-project/python/plots/taskSpacePlacement/'
+myX_L = np.load(import_path+'myX_L_nullgradient.npy')
+myY_L = np.load(import_path+'myY_L_nullgradient.npy')
+myZ_L = np.load(import_path+'myZ_L_nullgradient.npy')
+smallestMan = np.load(import_path+'smallestMan_nullgradient.npy')
+
+# use self-defined tum-cycler for TUM-blue colors
+plt.style.use('mylatex')
+
+# plotting for thesis
+plt.rcParams.update({
+ "font.family": "serif", # use serif/main font for text elements
+ "text.usetex": True, # use inline math for ticks
+ "pgf.rcfonts": False, # don't setup fonts from rc parameters
+ #"legend.loc": 'upper right',
+ "savefig.directory": '/home/joschua/Documents/Studium/TUM/Thesis/documentation/thesis/myWorkFiles/AMStudentThesis/figures/plots/'
+ })
+
+transparancy = np.ones((len(myX_L), 4))
+dsmallestMan = (max(smallestMan)-min(smallestMan))
+transparancy[:, 3] = np.array(smallestMan * (1/dsmallestMan) * 0.8 + 0.2)
+
+man_list = smallestMan.tolist()
+indexMax = man_list.index(max(man_list))
+print("biggest manipulability coordintes xyz ")
+print(str(myX_L.tolist()[indexMax]) + '\t' + str(myY_L.tolist()[indexMax]) + '\t'+ str(myZ_L.tolist()[indexMax]))
+
+
+def scatter3d(x,y,z, cs, colorsMap='RdBu'):
+ cm = plt.get_cmap(colorsMap)
+ cNorm = matplotlib.colors.Normalize(vmin=min(cs), vmax=max(cs))
+ scalarMap = cmx.ScalarMappable(norm=cNorm, cmap=cm)
+ fig = plt.figure()
+ ax = Axes3D(fig)
+ ax.scatter(x, y, z, c=np.round_(scalarMap.to_rgba(cs)*transparancy, decimals=4), depthshade=False)
+ ax.set_ylim(50, 10)
+ ax.set_xlabel('$y$ in cm')
+ ax.set_ylabel('$x$ in cm')
+ ax.set_zlabel('$z$ in cm')
+ #ax.scatter(x, y, z, c=scalarMap.to_rgba(cs))
+ scalarMap.set_array(cs)
+ fig.colorbar(scalarMap)
+ #fig.tight_layout()
+ ax.view_init(elev=9., azim=-86.)
+ plt.yticks([10, 20, 30, 40, 50])
+
+ plt.show()
+
+scatter3d(np.array(myY_L)*100, np.array(myX_L)*100, np.array(myZ_L)*100, np.array(smallestMan))
\ No newline at end of file
diff --git a/python/preprocessing/genPaths.py b/python/preprocessing/genPaths.py
index 40819a58727d0daa9dab1d730d493e5fa50bbf5b..189912e2ce0346d3078f0ccca9c43f77d461b59e 100644
--- a/python/preprocessing/genPaths.py
+++ b/python/preprocessing/genPaths.py
@@ -183,7 +183,7 @@ for i, (r12,r22_) in enumerate(zip(dist_12, dist_22_)):
# make data ready for export
traj_data = np.hstack((p1m, v1, p2m, v2, ang, odot))
# save data for application
-#np.save('./data/traj_data', traj_data)
+np.save('./data/traj_data', traj_data)
# save data to decouple preprocessing and plotting
@@ -199,4 +199,4 @@ np.save(plot_path+'p2m_ref', p2m_ref)
np.save(plot_path+'v1', v1)
np.save(plot_path+'v2', v2)
-np.save('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01', R_01)
\ No newline at end of file
+#np.save('/home/joschua/Coding/forceControl/master-project/python/plots/postprocessing/R_01_250', R_01)
\ No newline at end of file
diff --git a/python/taskspace_placement/computeJoints.py b/python/taskspace_placement/computeJoints.py
index c7bce378ba3d197f3de6c462618dbc22c3823fee..a808a44172701dc9ded7e07af5c7e1658ccb8a51 100644
--- a/python/taskspace_placement/computeJoints.py
+++ b/python/taskspace_placement/computeJoints.py
@@ -6,7 +6,7 @@ import numpy as np
import copy
import libs.invKin as invKin
-from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory, Yumi
+from data.get_data import get_trajectory, transform2yumi_workspace, place_trajectory
# READ IN THE TRAJECTORY
@@ -15,7 +15,8 @@ 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])
+#x: 0.25, y: 0.1, z: 0.05
+p1_start_des = np.array([0.35, 0.1, 0.15])
p1, p2 = place_trajectory(p1_start_des, p1, p2)
# START CONFIGURATION FOR THE LEFT ARM
@@ -31,9 +32,16 @@ dt = 0.0125
compJointAngles_left = np.zeros((len(p1[:,0]),7))
computedPose_left = np.zeros((len(p1[:,0]),6))
error_left = np.zeros((len(p1[:,0]),6))
+jointVelocities_left = np.zeros((len(p1[:,0]),7))
yumi_right = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_right.urdf")
+yumi_right.set_driftCompGain(1.0)
+yumi_right.set_additionalManipConstraint(True)
yumi_left = invKin.Yumi("/home/joschua/Coding/forceControl/master-project/c++/models/urdf/yumi_left.urdf")
+yumi_left.set_driftCompGain(1.0)
+yumi_left.set_additionalManipConstraint(True)
+
+desPose_old = np.concatenate((p1[0,: ], phi_delta[0, :]), axis=0)
# loop for the left arm
for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_delta, dphi)): # loop through all the desired position of left arm
@@ -41,118 +49,70 @@ for index, (pos, vel, phi, phi_dot) in enumerate(zip(p1, v1, phi_delta, dphi)):
desVelocities = np.concatenate((vel, phi_dot), axis=0)
# call the c++ egm function, return joint values and resulting pose
yumi_left.set_jointValues(jointAngles, jointVelocities)
- yumi_left.set_desPoseVel(desPose, desVelocities)
+ yumi_left.set_desPoseVel(desPose_old, desVelocities)
yumi_left.process()
compJointAngles_left[index,:] = yumi_left.get_newJointValues() # computed joint values from IK
- computedPose_left[index, :] = yumi_left.get_newPose() # resulting pose with joint values from IK
+ computedPose_left[index, :] = yumi_left.get_pose() # resulting pose with joint values from IK
if index > 0:
jointVelocities = (compJointAngles_left[index, :] - compJointAngles_left[index-1, :])/dt # only true in the ideal case where result of ik matches the desired pose
error_left[index, :] = desPose - computedPose_left[index, :]
+ jointVelocities_left[index, :] = jointVelocities
jointAngles = compJointAngles_left[index, :]
+ # save desired pose to give it to ik in the next iteration since drift compensation must consider if current task space pose is the old desired task space pose
+ desPose_old = copy.copy(desPose)
compJointAngles_right = np.zeros((len(p1[:,0]),7))
computedPose_right = np.zeros((len(p1[:,0]),6))
error_right = np.zeros((len(p1[:,0]),6))
+jointVelocities_right = np.zeros((len(p1[:,0]),7))
jointAngles = np.array([-110.0, 29.85, 35.92, 49.91, 117.0, 123.0, -117.0]) * np.pi/180.0
jointVelocities = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+desPose_old = np.concatenate((p2[0,: ], phi_delta[0, :]), axis=0)
+
# loop for the right arm
for index, (pos, vel, phi, phi_dot) in enumerate(zip(p2, v2, phi_delta, dphi)): # loop through all the desired position of left arm
desPose = np.concatenate((pos, phi), axis=0)
desVelocities = np.concatenate((vel, phi_dot), axis=0)
yumi_right.set_jointValues(jointAngles, jointVelocities)
- yumi_right.set_desPoseVel(desPose, desVelocities)
+ yumi_right.set_desPoseVel(desPose_old, desVelocities)
yumi_right.process()
compJointAngles_right[index, :] = yumi_right.get_newJointValues() # computed joint values from IK
- computedPose_right[index, :] = yumi_right.get_newPose() # resulting pose with joint values from IK
+ computedPose_right[index, :] = yumi_right.get_pose() # resulting pose with joint values from IK
if index > 0:
jointVelocities = (compJointAngles_left[index, :] - compJointAngles_left[index-1, :])/dt
error_right[index, :] = desPose - computedPose_right[index, :]
+ jointVelocities_right[index, :] = jointVelocities
jointAngles = compJointAngles_right[index, :]
-# see development of joint values
-fig = plt.figure()
-plt.plot(compJointAngles_left[:,0], label='joint1')
-plt.plot(compJointAngles_left[:,1], label='joint2')
-plt.plot(compJointAngles_left[:,2], label='joint3')
-plt.plot(compJointAngles_left[:,3], label='joint4')
-plt.plot(compJointAngles_left[:,4], label='joint5')
-plt.plot(compJointAngles_left[:,5], label='joint6')
-plt.plot(compJointAngles_left[:,6], label='joint7')
-plt.title('joint values over samples, left arm')
-plt.legend()
-plt.show()
-
-# error
-fig = plt.figure()
-plt.plot(error_left[:,0], label='x')
-plt.plot(error_left[:,1], label='y')
-plt.plot(error_left[:,2], label='z')
-plt.plot(error_left[:,3], label='rx')
-plt.plot(error_left[:,4], label='ry')
-plt.plot(error_left[:,5], label='rz')
-plt.legend()
-plt.title('errors left')
-plt.show()
-
-# show trajectory in workspace of yumi
-fig = plt.figure()
-plt.plot(p1[:,0], p1[:,2], label='desired profile') # plot z over x
-plt.scatter(computedPose_left[:,0], computedPose_left[:,2], label='resulting pose from IK')
-fig.get_axes()[0].set_xlabel('x axis of yumi')
-fig.get_axes()[0].set_ylabel('z axis of yumi')
-plt.legend()
-plt.title('poses left')
-plt.show()
-
-""" fig = plt.figure()
-plt.plot(computedPose_left[:,3], label='rx')
-plt.plot(computedPose_left[:,4], label='ry')
-plt.plot(computedPose_left[:,5], label='rz')
-plt.legend()
-plt.title('euler angles over trajectories')
-plt.show() """
-
-fig = plt.figure()
-plt.plot(compJointAngles_right[:,0], label='joint1')
-plt.plot(compJointAngles_right[:,1], label='joint2')
-plt.plot(compJointAngles_right[:,2], label='joint3')
-plt.plot(compJointAngles_right[:,3], label='joint4')
-plt.plot(compJointAngles_right[:,4], label='joint5')
-plt.plot(compJointAngles_right[:,5], label='joint6')
-plt.plot(compJointAngles_right[:,6], label='joint7')
-plt.title('joint values over samples, right arm')
-plt.legend()
-plt.show()
-
-fig = plt.figure()
-plt.plot(error_right[:,0], label='x')
-plt.plot(error_right[:,1], label='y')
-plt.plot(error_right[:,2], label='z')
-plt.plot(error_right[:,3], label='rx')
-plt.plot(error_right[:,4], label='ry')
-plt.plot(error_right[:,5], label='rz')
-plt.legend()
-plt.title('errors right')
-plt.show()
-
-# show trajectory in workspace of yumi
-fig = plt.figure()
-plt.plot(p2[:,0], p2[:,2], label='desired profile') # plot z over x
-plt.scatter(computedPose_right[:,0], computedPose_right[:,2], label='resulting pose from IK')
-fig.get_axes()[0].set_xlabel('x axis of yumi')
-fig.get_axes()[0].set_ylabel('z axis of yumi')
-plt.legend()
-plt.title('poses right')
-plt.show()
-
-np.save('data/compJointAngles_left', compJointAngles_left)
-np.save('data/compJointAngles_right', compJointAngles_right)
+ # save desired pose to give it to ik in the next iteration since drift compensation must consider if current task space pose is the old desired task space pose
+ desPose_old = copy.copy(desPose)
+
+
+plot_path = '/home/joschua/Coding/forceControl/master-project/python/plots/ik_tuning/'
+np.save(plot_path +'error_left', error_left)
+np.save(plot_path +'error_right', error_right)
+np.save(plot_path +'computedPose_left', computedPose_left)
+np.save(plot_path +'computedPose_right', computedPose_right)
+np.save(plot_path +'compJointAngles_right', compJointAngles_right)
+np.save(plot_path +'compJointAngles_left', compJointAngles_left)
+np.save(plot_path + 'jointVelocities_right', jointVelocities_right)
+np.save(plot_path + 'jointVelocities_left', jointVelocities_left)
+np.save(plot_path +'p1', p1)
+np.save(plot_path +'p2', p2)
+
+
+
+
+
+
+#np.save('data/compJointAngles_left', compJointAngles_left)
+#np.save('data/compJointAngles_right', compJointAngles_right)