transformed selection matrix test

omron-sinicx · Dec 18, 2024 · 7d1a089 · 7d1a089
1 parent bfd5415
commit 7d1a089
Show file tree

Hide file tree

Showing 90 changed files with 82 additions and 7 deletions.
diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md
diff --git a/.github/ISSUE_TEMPLATE/general-issue.md b/.github/ISSUE_TEMPLATE/general-issue.md
diff --git a/.github/workflows/industrial_ci_melodic_action.yml b/.github/workflows/industrial_ci_melodic_action.yml
diff --git a/.github/workflows/industrial_ci_noetic_action.yml b/.github/workflows/industrial_ci_noetic_action.yml
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
diff --git a/LICENSE b/LICENSE
diff --git a/LICENSES/license.cpp b/LICENSES/license.cpp
diff --git a/README.md b/README.md
diff --git a/cartesian_compliance_controller/CMakeLists.txt b/cartesian_compliance_controller/CMakeLists.txt
diff --git a/cartesian_compliance_controller/README.md b/cartesian_compliance_controller/README.md
diff --git a/cartesian_compliance_controller/cartesian_compliance_controller_plugin.xml b/cartesian_compliance_controller/cartesian_compliance_controller_plugin.xml
diff --git a/...ance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h b/...ance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h
diff --git a/...ce_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.hpp b/...ce_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.hpp
@@ -150,6 +150,10 @@ template <class HardwareInterface>
 ctrl::Vector6D CartesianComplianceController<HardwareInterface>::
 computeComplianceError()
 {
+  std::cout << "m_selection_matrix: " << std::endl << m_selection_matrix << std::endl;
+  bool powder_grounding = true;
+  // bool powder_grounding = false;
+
   ctrl::Vector6D net_force;
   if (!m_use_parallel_force_position_control)
     net_force =
@@ -159,7 +163,77 @@ computeComplianceError()
 
       // Sensor and target force in base orientation
       + ForceBase::computeForceError();
-  else // Add a selection matrix to allow finer control of which control to use for each 
+  else if (powder_grounding) {
+    auto pose = Base::m_ik_solver->getEndEffectorPose();
+    std::cout << "xyz: " << pose.p.x() << ", " << pose.p.y() << ", " << pose.p.z() << std::endl;
+    ctrl::Vector3D ee_pos;
+    ee_pos << pose.p.x(), pose.p.y(), pose.p.z();
+    std::cout<<"ee_pos:"<<std::endl<<ee_pos<<std::endl;
+    ctrl::Vector3D center;
+    center << 0.0, 0.5, 0.04;
+    std::cout<<"center:"<<std::endl<<center<<std::endl;
+    ctrl::Vector3D normal_vec;
+    normal_vec = (ee_pos - center).normalized();
+    std::cout<<"normal_vec:"<<std::endl<<normal_vec<<std::endl;
+    ctrl::Vector3D x_basis;
+    x_basis << 1.0, 0.0, 0.0;
+    ctrl::Vector3D u_x = x_basis - x_basis.dot(normal_vec) * normal_vec;
+    std::cout<<"u_x:"<<std::endl<<u_x<<std::endl;
+    u_x.normalize();
+    ctrl::Vector3D y_basis;
+    y_basis << 0.0, 1.0, 0.0;
+    ctrl::Vector3D u_y = y_basis - y_basis.dot(normal_vec) * normal_vec - y_basis.dot(u_x) * u_x;
+    u_y.normalize();
+    std::cout << "u_y:" << std::endl << u_y << std::endl;
+    ctrl::Matrix3D R_base2surface;
+    R_base2surface << u_x(0), u_y(0), normal_vec(0),
+                      u_x(1), u_y(1), normal_vec(1),
+                      u_x(2), u_y(2), normal_vec(2);
+    std::cout << "R_base2surface:" << std::endl << R_base2surface << std::endl;
+    ctrl::Matrix3D R_check = R_base2surface * R_base2surface.transpose();
+    std::cout << "R_check:" << std::endl << R_check << std::endl;
+    ctrl::Matrix3D px;
+    px <<           0, -pose.p.z(),  pose.p.y(),
+           pose.p.z(),           0, -pose.p.x(),
+          -pose.p.y(),  pose.p.x(),           0;
+    std::cout << "px:" << std::endl << px << std::endl;
+    ctrl::Matrix3D pxR;
+    pxR = px * R_base2surface;
+    std::cout << "pxR:" << std::endl << pxR << std::endl;
+    ctrl::Matrix6D Adjoint_T;
+    Adjoint_T << R_base2surface, ctrl::Matrix3D::Zero(3, 3),
+                            pxR,             R_base2surface;
+    std::cout << "Adjoint_T:" << std::endl << Adjoint_T << std::endl;
+    ctrl::Matrix6D Adjoint_T_inv;
+    ctrl::Matrix3D R_base2surface_T = R_base2surface.transpose();
+    Adjoint_T_inv << R_base2surface_T, ctrl::Matrix3D::Zero(3, 3),
+                      pxR.transpose(),             R_base2surface;
+    std::cout << "Adjoint_T_inv:" << std::endl << Adjoint_T_inv << std::endl;
+    ctrl::Matrix6D Adjoint_T_inv_check;
+    Adjoint_T_inv_check = Adjoint_T * Adjoint_T_inv;
+    std::cout << "Adjoint_T_inv_check:" << std::endl << Adjoint_T_inv_check << std::endl;
+
+    ctrl::Matrix6D m_selection_matrix_pd = Adjoint_T_inv * m_selection_matrix * Adjoint_T;
+    std::cout << "m_selection_matrix_pd: " << std::endl << m_selection_matrix_pd << std::endl;
+
+    // ctrl::Matrix6D m_selection_matrix_pd_check;
+    // m_selection_matrix_pd_check =
+    //     m_selection_matrix_pd +
+    //     (ctrl::Matrix6D::Identity() - m_selection_matrix_pd);
+    // std::cout << "m_selection_matrix_pd_check: " << std::endl << m_selection_matrix_pd_check << std::endl;
+
+    net_force =
+
+        // Position controller: PID gains scale by m_stiffness
+        m_selection_matrix_pd *
+            Base::displayInBaseLink(m_stiffness, m_compliance_ref_link) *
+            MotionBase::computeMotionError()
+
+        // Sensor and target force in base orientation
+        + ((ctrl::Matrix6D::Identity() - m_selection_matrix_pd) *
+          ForceBase::computeForceError());
+  }
+  else // Add a selection matrix to allow finer control of which control to use for each
   {
     net_force =
 
@@ -169,7 +243,8 @@ computeComplianceError()
       // Sensor and target force in base orientation
       + ((ctrl::Matrix6D::Identity() - m_selection_matrix) * ForceBase::computeForceError());
   }
-
+
+
   return net_force;
 }
 

diff --git a/cartesian_compliance_controller/package.xml b/cartesian_compliance_controller/package.xml
diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp
diff --git a/cartesian_controller_base/CMakeLists.txt b/cartesian_controller_base/CMakeLists.txt
diff --git a/cartesian_controller_base/README.md b/cartesian_controller_base/README.md
diff --git a/cartesian_controller_base/ik_solver_plugin.xml b/cartesian_controller_base/ik_solver_plugin.xml
diff --git a/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h b/cartesian_controller_base/include/cartesian_controller_base/DampedLeastSquaresSolver.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h b/cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h b/cartesian_controller_base/include/cartesian_controller_base/IKSolver.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h b/cartesian_controller_base/include/cartesian_controller_base/JacobianTransposeSolver.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/PDController.h b/cartesian_controller_base/include/cartesian_controller_base/PDController.h
diff --git a/...n_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h b/...n_controller_base/include/cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h b/cartesian_controller_base/include/cartesian_controller_base/SpatialPDController.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/Utility.h b/cartesian_controller_base/include/cartesian_controller_base/Utility.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.h
diff --git a/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp b/cartesian_controller_base/include/cartesian_controller_base/cartesian_controller_base.hpp
@@ -66,7 +66,7 @@ CartesianControllerBase()
 template <class HardwareInterface>
 bool CartesianControllerBase<HardwareInterface>::
 init(HardwareInterface* hw, ros::NodeHandle& nh)
-{ 
+{
 
   if (m_already_initialized)
   {
@@ -203,16 +203,16 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
   // Connect dynamic reconfigure and overwrite the default values with values
   // on the parameter server. This is done automatically if parameters with
   // the according names exist.
-  
+
   m_callback_type = std::bind(
       &CartesianControllerBase<HardwareInterface>::dynamicReconfigureCallback, this, std::placeholders::_1, std::placeholders::_2);
 
   m_dyn_conf_server.reset(
       new dynamic_reconfigure::Server<ControllerConfig>(
         ros::NodeHandle(nh.getNamespace())));
   m_dyn_conf_server->setCallback(m_callback_type);
-  
-  
+
+
   m_error_scale = 1.0;
   m_iterations = 1;
   m_solver_callback_type = std::bind(
@@ -222,7 +222,7 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
       new dynamic_reconfigure::Server<SolverConfig>(
         ros::NodeHandle(nh.getNamespace() + "/solver")));
   m_solver_dyn_conf_server->setCallback(m_solver_callback_type);
-  
+
 
   m_already_initialized = true;
 

diff --git a/cartesian_controller_base/package.xml b/cartesian_controller_base/package.xml
diff --git a/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/DampedLeastSquaresSolver.cpp
diff --git a/cartesian_controller_base/src/ForwardDynamicsSolver.cpp b/cartesian_controller_base/src/ForwardDynamicsSolver.cpp
diff --git a/cartesian_controller_base/src/IKSolver.cpp b/cartesian_controller_base/src/IKSolver.cpp
diff --git a/cartesian_controller_base/src/JacobianTransposeSolver.cpp b/cartesian_controller_base/src/JacobianTransposeSolver.cpp
diff --git a/cartesian_controller_base/src/PDController.cpp b/cartesian_controller_base/src/PDController.cpp
diff --git a/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp b/cartesian_controller_base/src/SelectivelyDampedLeastSquaresSolver.cpp
diff --git a/cartesian_controller_base/src/SpatialPDController.cpp b/cartesian_controller_base/src/SpatialPDController.cpp
diff --git a/cartesian_controller_examples/CMakeLists.txt b/cartesian_controller_examples/CMakeLists.txt
diff --git a/cartesian_controller_examples/config/example_controllers.yaml b/cartesian_controller_examples/config/example_controllers.yaml
diff --git a/cartesian_controller_examples/config/example_hw_config.yaml b/cartesian_controller_examples/config/example_hw_config.yaml
diff --git a/cartesian_controller_examples/etc/examples.rviz b/cartesian_controller_examples/etc/examples.rviz
diff --git a/cartesian_controller_examples/launch/examples.launch b/cartesian_controller_examples/launch/examples.launch
diff --git a/cartesian_controller_examples/package.xml b/cartesian_controller_examples/package.xml
diff --git a/cartesian_controller_examples/urdf/robot.urdf.xacro b/cartesian_controller_examples/urdf/robot.urdf.xacro
diff --git a/cartesian_controller_handles/CMakeLists.txt b/cartesian_controller_handles/CMakeLists.txt
diff --git a/cartesian_controller_handles/README.md b/cartesian_controller_handles/README.md
diff --git a/cartesian_controller_handles/cartesian_controller_handles_plugin.xml b/cartesian_controller_handles/cartesian_controller_handles_plugin.xml
diff --git a/cartesian_controller_handles/include/cartesian_controller_handles/MotionControlHandle.h b/cartesian_controller_handles/include/cartesian_controller_handles/MotionControlHandle.h
diff --git a/cartesian_controller_handles/include/cartesian_controller_handles/MotionControlHandle.hpp b/cartesian_controller_handles/include/cartesian_controller_handles/MotionControlHandle.hpp
diff --git a/cartesian_controller_handles/launch/moving_target.launch b/cartesian_controller_handles/launch/moving_target.launch
diff --git a/cartesian_controller_handles/package.xml b/cartesian_controller_handles/package.xml
diff --git a/cartesian_controller_handles/src/cartesian_controller_handles.cpp b/cartesian_controller_handles/src/cartesian_controller_handles.cpp
diff --git a/cartesian_controller_tests/CMakeLists.txt b/cartesian_controller_tests/CMakeLists.txt
diff --git a/cartesian_controller_tests/README.md b/cartesian_controller_tests/README.md
diff --git a/cartesian_controller_tests/cartesian_controllers.test b/cartesian_controller_tests/cartesian_controllers.test
diff --git a/cartesian_controller_tests/config/invalid_controllers.yaml b/cartesian_controller_tests/config/invalid_controllers.yaml
diff --git a/cartesian_controller_tests/package.xml b/cartesian_controller_tests/package.xml
diff --git a/cartesian_controller_utilities/CMakeLists.txt b/cartesian_controller_utilities/CMakeLists.txt
diff --git a/cartesian_controller_utilities/README.md b/cartesian_controller_utilities/README.md
diff --git a/cartesian_controller_utilities/etc/button_cmds.yaml b/cartesian_controller_utilities/etc/button_cmds.yaml
diff --git a/cartesian_controller_utilities/launch/buttons_events.launch b/cartesian_controller_utilities/launch/buttons_events.launch
diff --git a/cartesian_controller_utilities/launch/spacenav.launch b/cartesian_controller_utilities/launch/spacenav.launch
diff --git a/cartesian_controller_utilities/launch/spacenav_to_pose.launch b/cartesian_controller_utilities/launch/spacenav_to_pose.launch
diff --git a/cartesian_controller_utilities/launch/spacenav_to_wrench.launch b/cartesian_controller_utilities/launch/spacenav_to_wrench.launch
diff --git a/cartesian_controller_utilities/package.xml b/cartesian_controller_utilities/package.xml
diff --git a/cartesian_controllers/CMakeLists.txt b/cartesian_controllers/CMakeLists.txt
diff --git a/cartesian_controllers/package.xml b/cartesian_controllers/package.xml
diff --git a/cartesian_force_controller/CMakeLists.txt b/cartesian_force_controller/CMakeLists.txt
diff --git a/cartesian_force_controller/README.md b/cartesian_force_controller/README.md
diff --git a/cartesian_force_controller/cartesian_force_controller_plugin.xml b/cartesian_force_controller/cartesian_force_controller_plugin.xml
diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.h
diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
diff --git a/cartesian_force_controller/package.xml b/cartesian_force_controller/package.xml
diff --git a/cartesian_force_controller/src/cartesian_force_controller.cpp b/cartesian_force_controller/src/cartesian_force_controller.cpp
diff --git a/cartesian_motion_controller/CMakeLists.txt b/cartesian_motion_controller/CMakeLists.txt
diff --git a/cartesian_motion_controller/README.md b/cartesian_motion_controller/README.md
diff --git a/cartesian_motion_controller/cartesian_motion_controller_plugin.xml b/cartesian_motion_controller/cartesian_motion_controller_plugin.xml
diff --git a/...esian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h b/...esian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.h
diff --git a/...ian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.hpp b/...ian_motion_controller/include/cartesian_motion_controller/cartesian_motion_controller.hpp
diff --git a/cartesian_motion_controller/package.xml b/cartesian_motion_controller/package.xml
diff --git a/cartesian_motion_controller/src/cartesian_motion_controller.cpp b/cartesian_motion_controller/src/cartesian_motion_controller.cpp
diff --git a/joint_to_cartesian_controller/CMakeLists.txt b/joint_to_cartesian_controller/CMakeLists.txt
diff --git a/joint_to_cartesian_controller/README.md b/joint_to_cartesian_controller/README.md
diff --git a/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h b/joint_to_cartesian_controller/include/joint_to_cartesian_controller/JointControllerAdapter.h
diff --git a/...artesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h b/...artesian_controller/include/joint_to_cartesian_controller/joint_to_cartesian_controller.h
diff --git a/joint_to_cartesian_controller/joint_to_cartesian_controller_plugin.xml b/joint_to_cartesian_controller/joint_to_cartesian_controller_plugin.xml
diff --git a/joint_to_cartesian_controller/package.xml b/joint_to_cartesian_controller/package.xml
diff --git a/joint_to_cartesian_controller/src/JointControllerAdapter.cpp b/joint_to_cartesian_controller/src/JointControllerAdapter.cpp
diff --git a/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp b/joint_to_cartesian_controller/src/joint_to_cartesian_controller.cpp
diff --git a/resources/doc/Solver_details.md b/resources/doc/Solver_details.md
diff --git a/resources/images/Control_Loop.png b/resources/images/Control_Loop.png