From 5498003cb1885dd1b735bbe9fe8f6c051d1b07b8 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Tue, 18 Jul 2023 15:22:26 +0300
Subject: [PATCH] add compliance controller

---
 .../launch/rbs_simulation.launch.py           | 20 +++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py
index 9c57807..1f9328e 100644
--- a/rbs_simulation/launch/rbs_simulation.launch.py
+++ b/rbs_simulation/launch/rbs_simulation.launch.py
@@ -104,7 +104,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "start_joint_controller",
-            default_value="true",
+            default_value="false",
             description="Enable headless mode for robot control",
         )
     )
@@ -240,11 +240,11 @@ def generate_launch_description():
         remappings=[
              ('motion_control_handle/target_frame', 'target_frame'),
              ('cartesian_motion_controller/target_frame', 'target_frame'),
-        #     ('cartesian_compliance_controller/target_frame', 'target_frame'),
+            ('cartesian_compliance_controller/target_frame', 'target_frame'),
         #     ('cartesian_force_controller/target_wrench', 'target_wrench'),
-        #     ('cartesian_compliance_controller/target_wrench', 'target_wrench'),
+            ('cartesian_compliance_controller/target_wrench', 'target_wrench'),
         #     ('cartesian_force_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
-        #     ('cartesian_compliance_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
+            ('cartesian_compliance_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
             ]
     )
 
@@ -271,7 +271,7 @@ def generate_launch_description():
     initial_joint_controller_spawner_stopped = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=[initial_joint_controller, "-c", "/controller_manager", "--incactive"],
+        arguments=[initial_joint_controller, "-c", "/controller_manager", "--inactive"],
         condition=UnlessCondition(start_joint_controller),
     )
     
@@ -291,6 +291,12 @@ def generate_launch_description():
         executable="spawner",
         arguments=["motion_control_handle", "--inactive", "-c", "/controller_manager"],
     )
+    
+    cartesian_compliance_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["cartesian_compliance_controller", "--inactive", "-c", "/controller_manager"],
+    )
 
     # Gazebo nodes
     # gazebo = IncludeLaunchDescription(
@@ -414,7 +420,8 @@ def generate_launch_description():
             robot_description_semantic,
             robot_description_kinematics,
             {"use_sim_time": use_sim_time},
-        ]
+        ],
+        condition=IfCondition(with_gripper_condition)
     )
 
     #move_topose_action_server = Node(
@@ -546,6 +553,7 @@ def generate_launch_description():
         #bridge,
         cartesian_motion_controller_spawner,
         motion_control_handle_spawner,
+        cartesian_compliance_controller_spawner,
         #pc_filter
         #add_planning_scene_object
     ]