Refactor code

- Add option to choose Ioana method or Yamane-san method
omron-sinicx · Feb 4, 2025 · 55d22a5 · 55d22a5
1 parent bbc50c6
commit 55d22a5
Show file tree

Hide file tree

Showing 3 changed files with 199 additions and 124 deletions.
diff --git a/cartesian_compliance_controller/cfg/ComplianceController.cfg b/cartesian_compliance_controller/cfg/ComplianceController.cfg
@@ -20,5 +20,7 @@ gen.add("sel_ay", double_t, 0, "Selection matrix rotatory along y-axis. Floating
 gen.add("sel_az", double_t, 0, "Selection matrix rotatory along z-axis. Floating value between [0,1]", 0.5, 0.0, 1.0)
 
 gen.add("use_parallel_force_position_control", bool_t, 0, "Use parallel force-position control", True)
+gen.add("use_parallel_force_position_control_in_gripper_frame", bool_t, 0, "Use parallel force-position control with selection matrix assumed to be given in the end-effector frame", True)
+gen.add("use_parallel_force_position_control_powder_grinding", bool_t, 0, "Use parallel force-position control with selection matrix rotated based on normal of bowl's surface", True)
 
 exit(gen.generate(PACKAGE, "cartesian_compliance_controller", "ComplianceController"))
diff --git a/...ance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h b/...ance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h
@@ -98,6 +98,15 @@ class CartesianComplianceController
 
     void update(const ros::Time& time, const ros::Duration& period);
 
+    void apply_selection_matrix_gripper_frame(ctrl::Vector6D pos_error_ref,
+                                              ctrl::Vector6D force_error_ref,
+                                              ctrl::Vector6D *pos_error_sel,
+                                              ctrl::Vector6D *force_error_sel,
+                                              ctrl::Matrix6D selection_matrix_gripper,
+                                              ctrl::Matrix3D R_gripper_to_ref);
+
+    void compute_selection_matrix_from_normal_surface(ctrl::Matrix6D *m_selection_matrix_pd);
+
     typedef cartesian_controller_base::CartesianControllerBase<HardwareInterface> Base;
     typedef cartesian_motion_controller::CartesianMotionController<HardwareInterface> MotionBase;
     typedef cartesian_force_controller::CartesianForceController<HardwareInterface> ForceBase;
@@ -113,6 +122,8 @@ class CartesianComplianceController
     ctrl::Matrix6D        m_stiffness;
     ctrl::Matrix6D        m_selection_matrix;
     bool                  m_use_parallel_force_position_control;
+    bool                  m_use_parallel_force_position_control_in_gripper_frame;
+    bool                  m_use_parallel_force_position_control_powder_grinding;
     std::string           m_compliance_ref_link;
 
     // Dynamic reconfigure for stiffness