Damp z-axis rotation only, controller works

Author: jeremyxu1998

robot_controller.cpp

@@ -6,6 +6,8 @@ BaseController::BaseController()
     updateFreq = 10;
     qEpsilon = 1e-5;
     maxTimestep = 1e4;
+    lambda_zero = 1000;
+    manipul_th = 0.01;
     PGain = 5e-3;
 }
 
@@ -66,38 +68,26 @@ bool BaseController::PathPlanning(TendonRobot & robot, const Eigen::MatrixXd & t
             J_body.col(i) = J_bi;
         }
 
-        /* single segment */
-        // assert(J_body.cols() == 3);
-        // Eigen::MatrixXd J_body_pos = J_body.block(3, 0, 3, 3);  // Position part only
-        // Eigen::MatrixXd J_body_inv = J_body_pos.inverse();
-
-        /* 6dof robot, 6dof pose, using Damped Least Squares Method */
-        // Note: Not calculating inverse directly, because Jacobian is in singularity for z-axis rotation at initial pose
-        // Citation for damping factor calculation method:
-        // Y. Nakamura and H. Hanafusa, Inverse kinematics solutions with singularity robustness for robot manipulator control
+        /* 6-DOF pose using Damped Least Squares Method
+         * Not calculating inverse directly, because Jacobian is always in singularity for z-axis rotation due to parallel tendon routing
+         * Therefore, want to damp this DOF
+         * This method works for 6-DOF or redundant robot
+         * Citation for damping factor calculation method:
+         * Y. Nakamura and H. Hanafusa, Inverse kinematics solutions with singularity robustness for robot manipulator control
+         */
         Eigen::MatrixXd JJT = J_body * J_body.transpose();
         // Note: use SVD to calculate determinant because determinant() is not working properly near singularity
-        // double JJT_det = JJT.determinant();
         Eigen::JacobiSVD<Eigen::MatrixXd> svd(JJT);
         Eigen::VectorXd JJT_singVals = svd.singularValues();
         RoundValues(JJT_singVals, 1e-6);  // Round the values to 1e-6 to prevent the case of very_large_double * very_small_double
         double JJT_det = JJT_singVals.prod();
         double manipul = sqrt(JJT_det);  // Manipulability measure
-        double manipul_th = 0.01;  // Manipulability threshold
         double lambda = 0.0;  // Damping factor
-        double lambda_zero = 1000; // Maximum damping factor
         if (manipul < manipul_th)
             lambda = lambda_zero * pow((1 - manipul / manipul_th), 2);
-        Eigen::MatrixXd J_body_pseudo = J_body.transpose() * 
-                (JJT + lambda * Eigen::Matrix<double,6,6>::Identity()).inverse();  // Right Pseudo Inverse
-        
-        /* 6dof robot, 5dof pose, skip z-axis rotation */
-        // Eigen::MatrixXd J_body_5dof(5, 6);
-        // J_body_5dof.block(0, 0, 2, 6) = J_body.block(0, 0, 2, 6);
-        // J_body_5dof.block(2, 0, 3, 6) = J_body.block(3, 0, 3, 6);
-        // Eigen::MatrixXd J_body_pseudo = J_body_5dof.transpose() * (J_body_5dof * J_body_5dof.transpose()).inverse();  // Right Pseudo Inverse
-        /* redundant 12dof (or 9dof) robot */
-        // Eigen::MatrixXd J_body_pseudo = J_body.transpose() * (J_body * J_body.transpose()).inverse();  // Right Pseudo Inverse
+        Eigen::Matrix<double,6,6> modDampingMat = Eigen::Matrix<double,6,6>::Zero();
+        modDampingMat(2, 2) = 1;  // Only damp the z-axis rotation
+        Eigen::MatrixXd J_body_pseudo = J_body.transpose() * (JJT + lambda * modDampingMat).inverse();  // Right Pseudo Inverse
 
         Eigen::Matrix4d T_body_desired = T_cur.inverse() * T_target;
         double theta;
@@ -106,22 +96,8 @@ bool BaseController::PathPlanning(TendonRobot & robot, const Eigen::MatrixXd & t
         twist << S_skew(2,1), S_skew(0,2), S_skew(1,0),  // omega components
                  S_skew(0,3), S_skew(1,3), S_skew(2,3);  // v components
         twist *= theta;  // S is normalized, multiply by theta to get twist
-        
-        /* single segment */
-        // Eigen::VectorXd twist_v = twist.tail(3);
-        // Eigen::VectorXd q_dot = J_body_inv * twist_v;
 
-        /* 6dof robot, 6dof pose */
         Eigen::VectorXd q_dot = J_body_pseudo * twist;
-
-        /* 6dof robot, 5dof pose, skip z-axis rotation */
-        // Eigen::VectorXd twist_5dof(5);
-        // twist_5dof.head(2) = twist.head(2);
-        // twist_5dof.tail(3) = twist.tail(3);
-        // Eigen::VectorXd q_dot = J_body_pseudo * twist_5dof;
-        /* redundant 12dof (or 9dof) robot */
-        // Eigen::VectorXd q_dot = J_body_pseudo * twist;
-
         q_cur = q_cur + q_dot * PGain; // TODO: Time step length?
         UnpackRobotConfig(robot, numTendon, q_cur, curTendonLengthChange, curSegLength);
         T_cur = robot.CalcTipPose(curTendonLengthChange, curSegLength);

robot_controller.h

@@ -17,6 +17,8 @@ class BaseController
     int calcFreq, updateFreq;
     double qEpsilon;  // small change in q when estimating Jacobian
     int maxTimestep;
+    double lambda_zero; // Maximum damping factor
+    double manipul_th;  // Manipulability threshold
     double PGain;  // Proportional gain
 
     Eigen::Matrix4d MatrixLog(const Eigen::Matrix4d & T, double & theta);

tendon_robot.h

@@ -6,7 +6,7 @@
 #include <QDomElement>
 #include <Eigen/Dense>
 
-#define EPSILON 1e-7
+#define EPSILON 1e-9
 
 class TendonRobot
 {

vtk_visualizer.cpp

@@ -24,8 +24,8 @@ VtkVisualizer::VtkVisualizer(std::vector<TendonRobot> & robots)
     originAxes = vtkSmartPointer<vtkAxesActor>::New();
     originAxes->SetTotalLength(0.01, 0.01, 0.01);
     originAxes->SetShaftTypeToCylinder();
-    originAxes->SetCylinderRadius(0.08);
-    originAxes->SetConeRadius(0.4);
+    originAxes->SetCylinderRadius(0.06);
+    originAxes->SetConeRadius(0.5);
     originAxes->AxisLabelsOff();
 
     renderer = vtkSmartPointer<vtkRenderer>::New();
@@ -163,7 +163,7 @@ VtkVisualizer::TACRVisual::TACRVisual(TendonRobot & robot)
     targetAxes = vtkSmartPointer<vtkAxesActor>::New();
     setAxesStyle(baseAxes, 0.02, 0.03, 0.3);
     setAxesStyle(tipAxes, 0.02, 0.03, 0.3);
-    setAxesStyle(targetAxes, 0.01, 0.08, 0.4);
+    setAxesStyle(targetAxes, 0.01, 0.06, 0.5);
 }
 
 void VtkVisualizer::TACRVisual::setAxesStyle(vtkSmartPointer<vtkAxesActor> axes, double axisLength, double cylinderRadius, double coneRadius)