diff --git a/ada_imu/config/imu_params.yaml b/ada_imu/config/imu_params.yaml index 8a92584..ea16e06 100644 --- a/ada_imu/config/imu_params.yaml +++ b/ada_imu/config/imu_params.yaml @@ -15,7 +15,7 @@ imu_jointstate_publisher: - -232.50333333333336 - 40.96 - serial_port: '/dev/ttyUSB0' + serial_port: '/dev/ttyUSB1' velocity_thresh: 0.05 # radians per second # weights used in exponential rolling averages, between 0 and 1 diff --git a/ada_moveit/config/ada.ros2_control.xacro b/ada_moveit/config/ada.ros2_control.xacro index 5a6282a..99b0d30 100644 --- a/ada_moveit/config/ada.ros2_control.xacro +++ b/ada_moveit/config/ada.ros2_control.xacro @@ -133,4 +133,53 @@ + + + + + + + + mock_components/GenericSystem + false + 0.0 + + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 + + + + + 1 + + + + ${initial_positions['af_joint_1']} + + + 0.0 + + + 0.0 + + + + 2 + + + + ${initial_positions['af_joint_2']} + + + 0.0 + + + 0.0 + + + + + diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index f5a2cb8..ff09e0b 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -3,14 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + + + + - + @@ -21,6 +24,14 @@ + + + + + + + + @@ -50,6 +61,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -266,4 +307,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index fa76185..05032bc 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -2,10 +2,14 @@ + + + + @@ -14,4 +18,10 @@ initial_positions_file="$(arg initial_positions_file)" sim="$(arg sim)" /> + + + + diff --git a/ada_moveit/config/initial_positions.yaml b/ada_moveit/config/initial_positions.yaml index 5077ed5..50181f7 100644 --- a/ada_moveit/config/initial_positions.yaml +++ b/ada_moveit/config/initial_positions.yaml @@ -23,3 +23,5 @@ initial_positions: j2n6s200_joint_6: 1.32575 j2n6s200_joint_finger_1: 1.317 j2n6s200_joint_finger_2: 1.317 + af_joint_1: 0.0 + af_joint_2: 0.0 diff --git a/ada_moveit/config/joint_limits.yaml b/ada_moveit/config/joint_limits.yaml index bcd07ce..0a84b83 100644 --- a/ada_moveit/config/joint_limits.yaml +++ b/ada_moveit/config/joint_limits.yaml @@ -51,3 +51,13 @@ joint_limits: max_velocity: 1.0 has_acceleration_limits: false max_acceleration: 0 + af_joint_1: + has_velocity_limits: true + max_velocity: 0.83775804095727813 + has_acceleration_limits: false + max_acceleration: 0 + af_joint_2: + has_velocity_limits: true + max_velocity: 0.83775804095727813 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index a1a97d7..d9f3b38 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -15,3 +15,31 @@ jaco_arm: cost_threshold: 0.001 minimal_displacement_weight: 0.0 gd_step_size: 0.0001 + +ada: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + stop_optimization_on_valid_solution: true + position_scale: 1.0 + rotation_scale: 0.5 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 + +articulable_fork: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + stop_optimization_on_valid_solution: true + position_scale: 1.0 + rotation_scale: 0.5 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index b926bfb..11ba86c 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -21,12 +21,21 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + jaco_arm_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController + + af_controller: + type: force_gate_controller/ForceGateController + + + af_servo_controller: + type: force_gate_controller/ForceGatePositionController + jaco_arm_cartesian_controller: ros__parameters: - end_effector_link: "forkTip" + end_effector_link: "j2n6s200_end_effector" robot_base_link: "j2n6s200_link_base" ik_solver: "selectively_damped_least_squares" joints: @@ -85,6 +94,32 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 +af_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + command_interfaces: + - position + state_interfaces: + - position + allow_partial_joints_goal: false + open_loop_control: false + stopped_velocity_tolerance: 0.01 + # No gains on position interface + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 + +af_servo_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index a9e61cd..3334dc7 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -47,10 +47,11 @@ servo_node: ## MoveIt properties move_group_name: jaco_arm # Often 'manipulator' or 'arm' + planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames - ee_frame_name: forkTip # The name of the end effector link, used to return the EE pose + ee_frame_name: j2n6s200_end_effector # The name of the end effector link, used to return the EE pose robot_link_command_frame: j2n6s200_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 33af3a7..6911256 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -10,6 +10,8 @@ moveit_simple_controller_manager: - jaco_arm_controller - jaco_arm_cartesian_controller - jaco_arm_servo_controller + - af_controller + - af_servo_controller - hand_controller jaco_arm_controller: @@ -47,6 +49,23 @@ moveit_simple_controller_manager: - j2n6s200_joint_4 - j2n6s200_joint_5 - j2n6s200_joint_6 + + af_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - af_joint_1 + - af_joint_2 + + af_servo_controller: + type: "" + action_ns: commands + default: false + joints: + - af_joint_1 + - af_joint_2 + hand_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index e9f7998..3e255b9 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -64,6 +64,21 @@ planner_configs: max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 PRMstarkConfigDefault: type: geometric::PRMstar +ada: + enforce_constrained_state_space: true + projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault jaco_arm: enforce_constrained_state_space: true projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) @@ -79,6 +94,21 @@ jaco_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault +articulable_fork: + enforce_constrained_state_space: true + projection_evaluator: joints(af_joint_1,af_joint_2) + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault hand: enforce_constrained_state_space: true projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index 3ccb89b..677940a 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -21,12 +21,21 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + jaco_arm_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController + + af_controller: + type: force_gate_controller/ForceGateController + + + af_servo_controller: + type: force_gate_controller/ForceGatePositionController + jaco_arm_cartesian_controller: ros__parameters: - end_effector_link: "forkTip" + end_effector_link: "j2n6s200_end_effector" robot_base_link: "j2n6s200_link_base" ik_solver: "selectively_damped_least_squares" joints: @@ -101,6 +110,38 @@ jaco_arm_servo_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 +af_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + command_interfaces: + - position + state_interfaces: + - position + allow_partial_joints_goal: false + open_loop_control: false + stopped_velocity_tolerance: 0.01 + wrench_threshold: + topic: /wireless_ft/ftSensor3 + fMag: 1.0 + # No gains on position interface + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 + +af_servo_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + wrench_threshold: + topic: /wireless_ft/ftSensor3 + fMag: 1.0 + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/real_servo.yaml b/ada_moveit/config/real_servo.yaml index c7f328b..bc15fba 100644 --- a/ada_moveit/config/real_servo.yaml +++ b/ada_moveit/config/real_servo.yaml @@ -50,7 +50,7 @@ servo_node: planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames - ee_frame_name: forkTip # The name of the end effector link, used to return the EE pose + ee_frame_name: j2n6s200_end_effector # The name of the end effector link, used to return the EE pose robot_link_command_frame: j2n6s200_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 5012163..bbc0d08 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -15,14 +15,11 @@ ) from launch.substitutions import ( LaunchConfiguration, - Command, PathJoinSubstitution, - FindExecutable, TextSubstitution, ) from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue from srdfdom.srdf import SRDF @@ -33,11 +30,6 @@ def generate_launch_description(): - # MoveIt Config - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() - # Calibration Launch Argument calib_da = DeclareLaunchArgument( "calib", @@ -54,6 +46,15 @@ def generate_launch_description(): ) sim = LaunchConfiguration("sim") + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + # Use Octomap Launch Argument use_octomap_da = DeclareLaunchArgument( "use_octomap", @@ -90,11 +91,22 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(calib_da) ld.add_action(sim_da) + ld.add_action(eet_da) ld.add_action(use_octomap_da) ld.add_action(ctrl_da) ld.add_action(servo_da) ld.add_action(log_level_da) + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + builder = builder.robot_description_semantic( + mappings={"end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + # Launch argument for whether to use moveit servo or not ld.add_action(DeclareBooleanLaunchArg("use_servo", default_value=False)) @@ -156,7 +168,9 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(str(virtual_joints_launch)), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) @@ -168,7 +182,9 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/rsp.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) @@ -183,6 +199,7 @@ def generate_launch_description(): "sim": sim, "use_octomap": use_octomap, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) @@ -194,7 +211,9 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/moveit_rviz.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), condition=IfCondition(LaunchConfiguration("use_rviz")), ) @@ -207,29 +226,14 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/warehouse_db.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), condition=IfCondition(LaunchConfiguration("db")), ) ) - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - str(moveit_config.package_path / "config/ada.urdf.xacro") - ), - " ", - "sim:=", - sim, - ] - ) - robot_description = { - "robot_description": ParameterValue(robot_description_content, value_type=str) - } - # Launch MoveIt Servo servo_config = PathJoinSubstitution( [str(moveit_config.package_path), "config", servo_file] @@ -241,7 +245,7 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_config, - robot_description, + moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, # If set, use IK instead of the inverse jacobian ], @@ -260,7 +264,7 @@ def generate_launch_description(): Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[moveit_config.robot_description, robot_controllers], arguments=["--ros-args", "--log-level", log_level], ) ) @@ -271,7 +275,9 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/spawn_controllers.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index aea779a..994a3ae 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -12,6 +12,9 @@ def get_move_group_launch(context): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + """ Gets the launch description for MoveGroup, after removing sensors_3d if sim is mock. @@ -21,11 +24,17 @@ def get_move_group_launch(context): sim = LaunchConfiguration("sim").perform(context) use_octomap = LaunchConfiguration("use_octomap").perform(context) log_level = LaunchConfiguration("log_level").perform(context) + end_effector_tool = LaunchConfiguration("end_effector_tool").perform(context) # Get MoveIt Configs - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + builder = builder.robot_description_semantic( + mappings={"end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() # If sim is mock, set moveit_config.sensors_3d to an empty dictionary if sim == "mock" or use_octomap == "false": @@ -45,6 +54,9 @@ def get_move_group_launch(context): def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + # Sim Launch Argument sim_da = DeclareLaunchArgument( "sim", @@ -63,10 +75,17 @@ def generate_launch_description(): default_value="info", description="Logging level (debug, info, warn, error, fatal)", ) + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "articulable_fork"], + ) ld = LaunchDescription() ld.add_action(sim_da) ld.add_action(octomap_da) ld.add_action(log_level_da) + ld.add_action(eet_da) ld.add_action(OpaqueFunction(function=get_move_group_launch)) return ld diff --git a/ada_moveit/launch/moveit_rviz.launch.py b/ada_moveit/launch/moveit_rviz.launch.py index 8dc331e..e6ef7f7 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -11,7 +11,20 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +35,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_moveit_rviz_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/rsp.launch.py b/ada_moveit/launch/rsp.launch.py index ae0bfe7..6708939 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -11,7 +11,20 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +35,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_rsp_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/spawn_controllers.launch.py b/ada_moveit/launch/spawn_controllers.launch.py index 59392a8..3d6061d 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -11,7 +11,20 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +35,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_spawn_controllers_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/static_virtual_joint_tfs.launch.py b/ada_moveit/launch/static_virtual_joint_tfs.launch.py index 0b122d2..40aa73c 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -11,7 +11,20 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +35,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_static_virtual_joint_tfs_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/warehouse_db.launch.py b/ada_moveit/launch/warehouse_db.launch.py index 612dc3f..7ef2146 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -11,7 +11,20 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +35,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_warehouse_db_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node):