diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
index 2722dc9e..a784b1fc 100644
--- a/ros/kxr_controller/launch/kxr_controller.launch
+++ b/ros/kxr_controller/launch/kxr_controller.launch
@@ -18,6 +18,7 @@
+
@@ -52,6 +53,7 @@
temperature_limit: $(arg temperature_limit)
read_current: $(arg read_current)
read_temperature: $(arg read_temperature)
+ spawn_fullbody_controller: $(arg spawn_fullbody_controller)
diff --git a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch
index 52b99831..e18e82df 100644
--- a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch
+++ b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch
@@ -15,6 +15,7 @@
+
@@ -32,6 +33,7 @@
+
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index a01fd8b8..536a47ce 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -177,6 +177,8 @@ def setup_paths_and_params(self):
self.joint_name_to_id, self.servo_infos = load_yaml(servo_config_path)
self.urdf_path = rospy.get_param("~urdf_path", None)
self.use_rcb4 = rospy.get_param("~use_rcb4", False)
+ self.spawn_fullbody_controller = rospy.get_param(
+ "~spawn_fullbody_controller", True)
self.control_pressure = rospy.get_param("~control_pressure", False)
self.read_temperature = rospy.get_param("~read_temperature", False) and not self.use_rcb4
self.read_current = rospy.get_param("~read_current", False) and not self.use_rcb4
@@ -204,8 +206,9 @@ def setup_urdf_and_model(self):
def setup_ros_parameters(self):
"""Configure joint state and full body controllers."""
- set_joint_state_controler()
- self.set_fullbody_controller()
+ if self.spawn_fullbody_controller:
+ set_joint_state_controler()
+ self.set_fullbody_controller()
def setup_publishers_and_servers(self):
"""Set up ROS publishers and action servers."""