diff --git a/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py b/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py index 54760b5a..0d1b451e 100644 --- a/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py +++ b/alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py @@ -244,6 +244,7 @@ class Arm(Platform): gripper (bool): Whether the arm has a gripper attached. moveit (bool) : Whether to enable MoveIt motion planning. ip_address (str): IP address of the arm controller. + controller (Literal["fr3_arm_controller", "demo_controller"]): Arm controller to run. moveit_config (MoveitConfig): MoveIt configuration settings. """ @@ -251,6 +252,10 @@ class Arm(Platform): gripper: bool = False moveit: bool = False ip_address: str = "10.15.20.4" + controller: Literal[ + "fr3_arm_controller", + "demo_controller", + ] = "fr3_arm_controller" moveit_config: MoveitConfig = field(default_factory=MoveitConfig) diff --git a/alliander_franka/src/alliander_franka/config/controllers.yaml b/alliander_franka/src/alliander_franka/config/controllers.yaml index e737ea66..c0a8f6b2 100644 --- a/alliander_franka/src/alliander_franka/config/controllers.yaml +++ b/alliander_franka/src/alliander_franka/config/controllers.yaml @@ -14,6 +14,9 @@ fr3_arm_controller: type: joint_trajectory_controller/JointTrajectoryController + demo_controller: + type: franka_example_controllers/CartesianVelocityExampleController + fr3_arm_controller: ros__parameters: type: joint_trajectory_controller/JointTrajectoryController @@ -61,3 +64,7 @@ fr3_joint7: goal: 0.01 trajectory: 0.1 + + demo_controller: + ros__parameters: + robot_type: "fr3" diff --git a/alliander_franka/src/alliander_franka/launch/controllers.launch.py b/alliander_franka/src/alliander_franka/launch/controllers.launch.py index eaa41740..21ae984e 100644 --- a/alliander_franka/src/alliander_franka/launch/controllers.launch.py +++ b/alliander_franka/src/alliander_franka/launch/controllers.launch.py @@ -38,11 +38,11 @@ def launch_setup(context: LaunchContext) -> list: namespace=arm_config.namespace, ) - fr3_arm_controller_spawner = Node( + arm_controller_spawner = Node( package="controller_manager", executable="spawner", arguments=[ - "fr3_arm_controller", + f"{arm_config.controller}", "--switch-timeout", str(TIMEOUT), ], @@ -80,7 +80,7 @@ def launch_setup(context: LaunchContext) -> list: return [ Register.on_exit(joint_state_broadcaster_spawner, context), - Register.on_exit(fr3_arm_controller_spawner, context), + Register.on_exit(arm_controller_spawner, context), Register.on_start(fr3_gripper, context), ( Register.on_exit(gripper_action_controller_spawner, context) diff --git a/predefined_configurations.py b/predefined_configurations.py index 5f0233a2..c108c848 100644 --- a/predefined_configurations.py +++ b/predefined_configurations.py @@ -240,7 +240,7 @@ def config_lynx_ouster(self) -> None: # noqa: D102 @register_configuration("mm") def config_mm(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) - arm = Arm("franka", (0, 0, 0.14), gripper=True, moveit=True) + arm = Arm("franka", (0.20, 0, 0.20), gripper=True, moveit=True) link(vehicle, arm) self.plat_conf.platforms = [vehicle, arm] @@ -249,7 +249,7 @@ def config_mm(self) -> None: # noqa: D102 def config_mm_velodyne(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) vehicle.nav2_config.navigation = True - arm = Arm("franka", (0, 0, 0.14), gripper=True, moveit=True) + arm = Arm("franka", (0.20, 0, 0.20), gripper=True, moveit=True) lidar = Lidar("velodyne", (0.13, -0.13, 0.35)) link(vehicle, arm) @@ -262,3 +262,18 @@ def config_panther_and_franka(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, -0.5, 0.2)) arm = Arm("franka", (0, 0.5, 0)) self.plat_conf.platforms = [vehicle, arm] + + # Demo configurations: + @register_configuration("demo_panther_franka") + def config_demo_panther_franka(self) -> None: # noqa: D102 + vehicle = Vehicle("panther", (0, 0, 0.2)) + arm = Arm( + "franka", + (0.20, 0, 0.20), + gripper=True, + moveit=True, + controller="demo_controller", + ) + + link(vehicle, arm) + self.plat_conf.platforms = [vehicle, arm]