Merge pull request #2 from HIRO-group/sk_dev

Merge to dev to preserve necessary files
HIRO-group · Nov 18, 2024 · d07d1cf · d07d1cf
2 parents 84967ee + 4cfb84f
commit d07d1cf
Show file tree

Hide file tree

Showing 11 changed files with 475 additions and 100 deletions.
diff --git a/franka_control/config/franka_control_node.yaml b/franka_control/config/franka_control_node.yaml
@@ -20,11 +20,19 @@ internal_controller: joint_impedance
 realtime_config: enforce
 # Configure the initial defaults for the collision behavior reflexes.
 collision_config:
+  # lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  # upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  # lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  # upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  # lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  # upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  # lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  # upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
   lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
-  upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  upper_torque_thresholds_acceleration: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0]  # [Nm]
   lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
-  upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  upper_torque_thresholds_nominal: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0]  # [Nm]
   lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
-  upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_acceleration: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0]  # [N, N, N, Nm, Nm, Nm]
   lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
-  upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_nominal: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0]  # [N, N, N, Nm, Nm, Nm]
diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="robot_ip" />
+  <arg name="robot_ip" default="192.168.0.100" />
   <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
   <arg name="arm_id" default="$(arg robot)" />
   <arg name="load_gripper" default="true" />

diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml
@@ -73,21 +73,49 @@ hiro_joint_impedance_example_controller:
         - $(arg arm_id)_joint6
         - $(arg arm_id)_joint7
     k_gains:
-        - 10.0
-        - 10.0
-        - 10.0
-        - 10.0
-        - 5.0
-        - 3.0
-        - 1.0
+        - 80.0
+        - 80.0
+        - 80.0
+        - 80.0
+        - 40.0
+        - 24.0
+        - 8.0
+        # - 20.0
+        # - 20.0
+        # - 20.0
+        # - 20.0
+        # - 10.0
+        # - 6.0
+        # - 2.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
+        # - 0.0 
+        # - 0.0
     d_gains:
-        - 5.0
-        - 5.0
-        - 5.0
-        - 2.0
-        - 2.0
-        - 2.0
-        - 1.0
+        - 40.0
+        - 40.0
+        - 40.0
+        - 16.0
+        - 16.0
+        - 16.0
+        - 8.0
+        # - 10.0
+        # - 10.0
+        # - 10.0
+        # - 4.0
+        # - 4.0
+        # - 4.0
+        # - 2.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
+        # - 0.0
     radius: 0.1
     acceleration_time: 2.0
     vel_max: 0.15

diff --git a/..._controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h b/..._controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h
@@ -20,6 +20,7 @@
 #include <franka_hw/trigger_rate.h>
 #include <sensor_msgs/JointState.h>
 #include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Bool.h>
 
 namespace franka_example_controllers {
 
@@ -49,11 +50,9 @@ class HIROJointImpedanceExampleController : public controller_interface::MultiIn
   double angle_{0.0};
   double vel_current_{0.0};
 
-  std::vector<double> k_gains_;
-  std::vector<double> d_gains_;
   double coriolis_factor_{1.0};
   std::array<double, 7> dq_filtered_;
-  std::array<double, 16> initial_pose_;
+  std::array<double, 16> pose_from_cb_;
 
   franka_hw::TriggerRate rate_trigger_{1.0};
   std::array<double, 7> last_tau_d_{};
@@ -64,8 +63,24 @@ class HIROJointImpedanceExampleController : public controller_interface::MultiIn
   bool callback_done_once = false;
   void xboxCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands);
   void cuRoboCommandCb(const std_msgs::Float32MultiArray::ConstPtr& joint_pos_commands);
+  void impedanceChangeBoolCb(const std_msgs::Bool::ConstPtr& impedance_change_bool);
   ros::Subscriber sub_command_;
   ros::Subscriber sub_command2_;
+  ros::Subscriber sub_impedance_change_bool_;
+  // std::vector<double> non_zero_imp_k{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0};
+  // std::vector<double> non_zero_imp_d{50.0, 50.0, 50.0, 20.0, 20.0, 20.0, 10.0};
+  // std::vector<double> k_gains_{160.0, 160.0, 160.0, 160.0, 80.0, 48.0, 16.0};
+  // std::vector<double> d_gains_{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
+  std::vector<double> k_gains_{80.0, 80.0, 80.0, 80.0, 40.0, 24.0, 8.0};
+  std::vector<double> d_gains_{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
+  // std::vector<double> k_gains_{40.0, 40.0, 40.0, 40.0, 20.0, 12.0, 4.0};
+  // std::vector<double> d_gains_{20.0, 20.0, 20.0, 8.0, 8.0, 8.0, 4.0};
+  // std::vector<double> non_zero_imp_k{160.0, 160.0, 160.0, 160.0, 80.0, 48.0, 16.0};
+  // std::vector<double> non_zero_imp_d{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
+  std::vector<double> non_zero_imp_k{80.0, 80.0, 80.0, 80.0, 40.0, 24.0, 8.0};
+  std::vector<double> non_zero_imp_d{40.0, 40.0, 40.0, 16.0, 16.0, 16.0, 8.0};
+  // std::vector<double> non_zero_imp_k{40.0, 40.0, 40.0, 40.0, 20.0, 12.0, 4.0};
+  // std::vector<double> non_zero_imp_d{20.0, 20.0, 20.0, 8.0, 8.0, 8.0, 4.0};
 };
 
 }  // namespace franka_example_controllers
diff --git a/franka_example_controllers/launch/force_example_controller.launch b/franka_example_controllers/launch/force_example_controller.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
+  <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
   <arg name="arm_id" default="$(arg robot)" />
   <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/>
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" />

diff --git a/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch b/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch
@@ -2,9 +2,15 @@
 <launch>
   <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
   <arg name="arm_id" default="$(arg robot)" />
-  <arg name="robot_ip" default="192.168.0.198"/>
+  <arg name="robot_ip" default="192.168.0.100"/>
+  <!-- <arg name="robot_ip" default="192.168.1.3"/> -->
+  <arg name="launch_rviz" default="false"/>
   <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/>
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" />
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"  args="hiro_joint_impedance_example_controller"/>
-  <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz -f $(arg arm_id)_link0 --splash-screen $(find franka_visualization)/splash.png"/>
+
+  <group if="$(arg launch_rviz)">
+    <!-- stuff that will only be evaluated if foo is true -->
+    <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz -f $(arg arm_id)_link0 splash-screen $(find franka_visualization)/splash.png"/>
+  </group>
 </launch>
diff --git a/franka_example_controllers/launch/joint_impedance_example_controller.launch b/franka_example_controllers/launch/joint_impedance_example_controller.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
+  <arg name="robot" default="fr3" doc="choose your robot. Possible values: [panda, fr3]"/>
   <arg name="arm_id" default="$(arg robot)" />
   <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/>
   <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" />