diff --git a/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py b/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py index c534411..7a21f6a 100755 --- a/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py +++ b/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py @@ -149,7 +149,7 @@ def __init__(self): # Create the service to re-tare the F/T sensor self.re_tare_ft_sensor_client = self.create_client( SetBool, - "/wireless_ft/set_bias", + "/ft_sensor/tare", qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE), ) diff --git a/ada_description/launch/view_ada.launch.py b/ada_description/launch/view_ada.launch.py index ce96c07..1834fa1 100644 --- a/ada_description/launch/view_ada.launch.py +++ b/ada_description/launch/view_ada.launch.py @@ -60,7 +60,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="ada.xacro", + default_value="ada_standalone.xacro", description="URDF/XACRO description file with the robot.", ) ) diff --git a/ada_description/urdf/ada.xacro b/ada_description/urdf/ada.xacro index 39ff92e..aa401cf 100644 --- a/ada_description/urdf/ada.xacro +++ b/ada_description/urdf/ada.xacro @@ -13,118 +13,119 @@ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" xmlns:xacro="http://www.ros.org/wiki/xacro" - name="ada" > - - - - - - - - - - + - - - - - - - - - + + + - + - + + + + + + + + + + - - - - - - - - - - - - - + - - + - + + - - + + + + + + + + + + + + + + + + + + + + + + diff --git a/ada_description/urdf/ada_standalone.xacro b/ada_description/urdf/ada_standalone.xacro new file mode 100644 index 0000000..71012ea --- /dev/null +++ b/ada_description/urdf/ada_standalone.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + diff --git a/ada_moveit/CMakeLists.txt b/ada_moveit/CMakeLists.txt index 3d535ba..afafcd2 100644 --- a/ada_moveit/CMakeLists.txt +++ b/ada_moveit/CMakeLists.txt @@ -10,6 +10,8 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS scripts/ada_keyboard_teleop.py scripts/servo_republisher.py + scripts/unified_joint_state_publisher.py + scripts/simulated_orientation_publisher_node.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index f5a2cb8..906b5ec 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -10,7 +10,7 @@ - + @@ -21,6 +21,12 @@ + + + + + + @@ -54,32 +60,12 @@ + + + - - - - - - - - - - - - - - - - - - - - - - - @@ -92,27 +78,7 @@ - - - - - - - - - - - - - - - - - - - - @@ -125,9 +91,6 @@ - - - @@ -140,50 +103,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -265,5 +184,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index 07f0d5b..44814c7 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -11,9 +11,29 @@ name="sim" default="real" /> + + + + + + + + diff --git a/ada_moveit/config/initial_positions.yaml b/ada_moveit/config/initial_positions.yaml index 5077ed5..b3eb271 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 + atool_joint1: 0.0 + atool_joint2: 0.0 diff --git a/ada_moveit/config/isaac_controllers.yaml b/ada_moveit/config/isaac_controllers.yaml index e37cf9d..4c23e27 100644 --- a/ada_moveit/config/isaac_controllers.yaml +++ b/ada_moveit/config/isaac_controllers.yaml @@ -22,7 +22,7 @@ controller_manager: jaco_arm_cartesian_controller: ros__parameters: - end_effector_link: "forkTip" + end_effector_link: "j2n6s200_end_effector" robot_base_link: "root" joints: - j2n6s200_joint_1 diff --git a/ada_moveit/config/joint_limits.yaml b/ada_moveit/config/joint_limits.yaml index bcd07ce..f6cb0e8 100644 --- a/ada_moveit/config/joint_limits.yaml +++ b/ada_moveit/config/joint_limits.yaml @@ -51,3 +51,17 @@ joint_limits: max_velocity: 1.0 has_acceleration_limits: false max_acceleration: 0 + atool_joint1: + min_position: -1.57 + max_position: 1.57 + has_velocity_limits: true + max_velocity: 0.62831853071795862 + has_acceleration_limits: false + max_acceleration: 0 + atool_joint2: + min_position: -3.14 # -4 * pi + max_position: 3.14 # 4 * pi + has_velocity_limits: true + max_velocity: 0.62831853071795862 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index a1a97d7..63b0088 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -4,8 +4,8 @@ # Adapted from: https://github.com/PickNikRobotics/pick_ik/blob/main/doc/USAGE.md jaco_arm: kinematics_solver: pick_ik/PickIkPlugin - kinematics_solver_timeout: 0.05 - kinematics_solver_attempts: 3 + kinematics_solver_timeout: 0.5 + kinematics_solver_attempts: 10 mode: global stop_optimization_on_valid_solution: true position_scale: 1.0 @@ -15,3 +15,29 @@ jaco_arm: cost_threshold: 0.001 minimal_displacement_weight: 0.0 gd_step_size: 0.0001 +articutool: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.5 + kinematics_solver_attempts: 10 + 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 +jaco_arm_with_articutool: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 5.0 + kinematics_solver_attempts: 250 + mode: global + stop_optimization_on_valid_solution: true + position_scale: 1.0 + rotation_scale: 0.5 + position_threshold: 0.005 + orientation_threshold: 0.05 + cost_threshold: 0.1 + 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..4863676 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -26,7 +26,7 @@ controller_manager: 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: diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index a9e61cd..a46b401 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -46,11 +46,11 @@ servo_node: monitored_planning_scene_topic: "monitored_planning_scene" ## MoveIt properties - move_group_name: jaco_arm # Often 'manipulator' or 'arm' + move_group_name: jaco_arm_with_articutool # 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: tool_tip # 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/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index d9b3133..f227b60 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -53,8 +53,9 @@ planner_configs: range: 0.05 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() RRTstarkConfigDefault: type: geometric::RRTstar - range: 100.0 # 3.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() (in practice, for ADA, this default is ~7, around 20% of the maximum extent of the space) - goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + range: 0.0 # 3.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() (in practice, for ADA, this default is ~7, around 20% of the maximum extent of the space) + goal_bias: 0.15 # When close to goal select goal, with this probability? default: 0.05 + longest_valid_segment_fraction: 0.05 delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 TRRTkConfigDefault: type: geometric::TRRT @@ -88,6 +89,36 @@ jaco_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault +articutool: + planner_configs: + - AnytimePathShortening + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault +jaco_arm_with_articutool: + enforce_constrained_state_space: true + projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) + planner_configs: + - AnytimePathShortening + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault hand: planner_configs: - SBLkConfigDefault diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index 3ccb89b..b4a64bf 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -26,7 +26,7 @@ controller_manager: 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: @@ -39,7 +39,7 @@ jaco_arm_cartesian_controller: command_interfaces: - velocity wrench_threshold: - topic: /wireless_ft/ftSensor3 + topic: /ft_sensor/wrench_raw fMag: 1.0 jaco_arm_controller: @@ -60,7 +60,7 @@ jaco_arm_controller: open_loop_control: false stopped_velocity_tolerance: 0.01 wrench_threshold: - topic: /wireless_ft/ftSensor3 + topic: /ft_sensor/wrench_raw fMag: 4.0 gains: # Required because we're controlling a velocity interface j2n6s200_joint_1: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} @@ -98,7 +98,7 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 wrench_threshold: - topic: /wireless_ft/ftSensor3 + topic: /ft_sensor/wrench_raw fMag: 1.0 hand_controller: diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 153fa3e..cb4840d 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -33,11 +33,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", @@ -86,6 +81,23 @@ def generate_launch_description(): ) log_level = LaunchConfiguration("log_level") + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used", + choices=["fork", "spoon"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + + # Lock Joints Launch Argument + lock_joints_da = DeclareLaunchArgument( + "lock_joints", + default_value="false", + description="Whether to lock the Articutool joints, setting them as fixed", + ) + lock_joints = LaunchConfiguration("lock_joints") + # Copy from generate_demo_launch ld = LaunchDescription() ld.add_action(calib_da) @@ -94,6 +106,19 @@ def generate_launch_description(): ld.add_action(ctrl_da) ld.add_action(servo_da) ld.add_action(log_level_da) + ld.add_action(eet_da) + ld.add_action(lock_joints_da) + + # MoveIt Config + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={ + "sim": sim, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, + } + ) + 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 +181,10 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(str(virtual_joints_launch)), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, }.items(), ) ) @@ -168,7 +196,10 @@ 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, + "lock_joints": lock_joints, }.items(), ) ) @@ -183,6 +214,8 @@ def generate_launch_description(): "sim": sim, "use_octomap": use_octomap, "log_level": log_level, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, }.items(), ) ) @@ -194,7 +227,10 @@ 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, + "lock_joints": lock_joints, }.items(), condition=IfCondition(LaunchConfiguration("use_rviz")), ) @@ -207,29 +243,15 @@ 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, + "lock_joints": lock_joints, }.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 +263,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,10 +282,11 @@ def generate_launch_description(): Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[moveit_config.robot_description, robot_controllers], # Commented out the log-level since the joint state publisher logs every joint read # when on debug level arguments=["--ros-args"], # , "--log-level", log_level], + remappings=[("/joint_states", "/ada/joint_states")], ) ) @@ -273,9 +296,23 @@ 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, + "lock_joints": lock_joints, }.items(), ) ) + ld.add_action( + Node( + package="ada_moveit", + executable="unified_joint_state_publisher.py", + name="unified_joint_state_publisher", + output="screen", + prefix=["python3"], + parameters=[{"lock_joints": lock_joints}], + ) + ) + return ld diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index aea779a..d433ee3 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -21,11 +21,22 @@ 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) + lock_joints = LaunchConfiguration("lock_joints").perform(context) # Get MoveIt Configs - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + moveit_config_builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + moveit_config_builder = moveit_config_builder.robot_description( + mappings={ + "sim": sim, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, + } + ) + moveit_config_builder.planning_pipelines( + pipelines=["ompl"], default_planning_pipeline="ompl" + ) + moveit_config = 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": @@ -63,10 +74,23 @@ 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", "spoon"], + ) + lock_joints_da = DeclareLaunchArgument( + "lock_joints", + default_value="false", + description="Whether to lock the Articutool joints, setting them as fixed", + ) 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(lock_joints_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..ab3e091 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): 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 +32,34 @@ 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", "spoon"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + lock_joints_da = DeclareLaunchArgument( + "lock_joints", + default_value="false", + description="Whether to lock the Articutool joints, setting them as fixed", + ) + lock_joints = LaunchConfiguration("lock_joints") + ld.add_action(lock_joints_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={ + "sim": sim, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, + } + ) + 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..93ad7d8 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): 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 +32,34 @@ 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", "spoon"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + lock_joints_da = DeclareLaunchArgument( + "lock_joints", + default_value="false", + description="Whether to lock the Articutool joints, setting them as fixed", + ) + lock_joints = LaunchConfiguration("lock_joints") + ld.add_action(lock_joints_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={ + "sim": sim, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, + } + ) + 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..86632ec 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): 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 +32,34 @@ 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", "spoon"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + lock_joints_da = DeclareLaunchArgument( + "lock_joints", + default_value="false", + description="Whether to lock the Articutool joints, setting them as fixed", + ) + lock_joints = LaunchConfiguration("lock_joints") + ld.add_action(lock_joints_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={ + "sim": sim, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, + } + ) + 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..c0999ee 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): 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 +32,34 @@ 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", "spoon"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + lock_joints_da = DeclareLaunchArgument( + "lock_joints", + default_value="false", + description="Whether to lock the Articutool joints, setting them as fixed", + ) + lock_joints = LaunchConfiguration("lock_joints") + ld.add_action(lock_joints_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={ + "sim": sim, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, + } + ) + 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..f689acb 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): 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 +32,34 @@ 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", "spoon"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + lock_joints_da = DeclareLaunchArgument( + "lock_joints", + default_value="false", + description="Whether to lock the Articutool joints, setting them as fixed", + ) + lock_joints = LaunchConfiguration("lock_joints") + ld.add_action(lock_joints_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={ + "sim": sim, + "end_effector_tool": end_effector_tool, + "lock_joints": lock_joints, + } + ) + moveit_config = builder.to_moveit_configs() entities = generate_warehouse_db_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/scripts/ada_keyboard_teleop.py b/ada_moveit/scripts/ada_keyboard_teleop.py index a513a2c..e034a0c 100755 --- a/ada_moveit/scripts/ada_keyboard_teleop.py +++ b/ada_moveit/scripts/ada_keyboard_teleop.py @@ -48,7 +48,7 @@ CTRL-C to quit """ BASE_FRAME = "j2n6s200_link_base" -EE_FRAME = "forkTip" +EE_FRAME = "j2n6s200_end_effector" LINEAR_VEL_CMD = 0.1 # m/s ANGULAR_VEL_CMD = 0.3 # rad/s JOINT_VEL_CMD = 0.5 # rad/s diff --git a/ada_moveit/scripts/servo_republisher.py b/ada_moveit/scripts/servo_republisher.py index 8c1e0a7..4af216a 100755 --- a/ada_moveit/scripts/servo_republisher.py +++ b/ada_moveit/scripts/servo_republisher.py @@ -158,7 +158,7 @@ def read_params(self): self.linear_velocity.header.frame_id = input_linear_velocity_frame input_angular_velocity_frame = self.declare_parameter( "input_angular_velocity_frame", - "forkTip", + "j2n6s200_end_effector", descriptor=ParameterDescriptor( name="input_angular_velocity_frame", type=ParameterType.PARAMETER_STRING, diff --git a/ada_moveit/scripts/simulated_orientation_publisher_node.py b/ada_moveit/scripts/simulated_orientation_publisher_node.py new file mode 100755 index 0000000..1be2bce --- /dev/null +++ b/ada_moveit/scripts/simulated_orientation_publisher_node.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 + +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +ROS 2 Node to publish the orientation of a target TF frame relative +to a reference TF frame, intended for simulating orientation sensors +based on kinematic information published to TF2. +""" + +import rclpy +from rclpy.node import Node +from rclpy.time import Time +from rclpy.duration import Duration +from rclpy.executors import ExternalShutdownException +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterDescriptor, ParameterType + +from geometry_msgs.msg import QuaternionStamped, Quaternion +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class SimulatedOrientationPublisherNode(Node): + """ + Listens to TF2 transforms and publishes the orientation of a specified + target link relative to a specified reference frame as a QuaternionStamped message. + Useful for providing kinematic orientation feedback in simulation environments + where TF is populated by a robot_state_publisher. + """ + + def __init__(self): + super().__init__("simulated_orientation_publisher_node") + + # --- Declare Parameters with descriptions --- + target_link_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='The name of the TF frame whose orientation should be published (e.g., "atool_imu_frame").', + ) + reference_frame_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='The name of the TF frame relative to which the orientation should be expressed (e.g., "base_link" or "world").', + ) + publish_topic_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="The topic name on which to publish the QuaternionStamped orientation data.", + ) + publish_rate_descriptor = ParameterDescriptor( + type=ParameterType.PARAMETER_DOUBLE, + description="The rate (in Hz) at which to look up the transform and publish the orientation.", + ) + + self.declare_parameter("target_link", "atool_imu_frame", target_link_descriptor) + self.declare_parameter( + "reference_frame", "base_link", reference_frame_descriptor + ) + self.declare_parameter( + "publish_topic", + "/articutool/estimated_orientation", + publish_topic_descriptor, + ) + self.declare_parameter("publish_rate", 50.0, publish_rate_descriptor) + + # --- Get Parameters --- + self.target_link_ = ( + self.get_parameter("target_link").get_parameter_value().string_value + ) + self.reference_frame_ = ( + self.get_parameter("reference_frame").get_parameter_value().string_value + ) + self.publish_topic_ = ( + self.get_parameter("publish_topic").get_parameter_value().string_value + ) + self.publish_rate_ = ( + self.get_parameter("publish_rate").get_parameter_value().double_value + ) + + # --- Validate Parameters --- + if not self.target_link_ or not self.reference_frame_: + self.get_logger().fatal( + "Parameters 'target_link' and 'reference_frame' must be non-empty strings. Shutting down." + ) + # Proper way to exit from init is tricky, rely on context shutdown + raise ValueError("Missing required parameters") + + if self.publish_rate_ <= 0: + self.get_logger().fatal( + "Parameter 'publish_rate' must be positive. Shutting down." + ) + raise ValueError("Invalid publish rate") + + self.timer_period_ = 1.0 / self.publish_rate_ + + self.get_logger().info( + f"Initializing SimulatedOrientationPublisherNode:\n" + f"\tTarget Link: '{self.target_link_}'\n" + f"\tReference Frame: '{self.reference_frame_}'\n" + f"\tPublish Topic: '{self.publish_topic_}'\n" + f"\tPublish Rate: {self.publish_rate_} Hz ({self.timer_period_:.4f} s period)" + ) + + # --- TF2 Setup --- + # Buffer stores received transforms, cache_time determines how long they are stored + self.tf_buffer_ = Buffer(cache_time=Duration(seconds=5.0)) + # Listener subscribes to /tf and /tf_static and fills the buffer + self.tf_listener_ = TransformListener( + self.tf_buffer_, self, spin_thread=True + ) # Use spin_thread=True for automatic background processing + + # --- Publisher Setup --- + self.publisher_ = self.create_publisher( + QuaternionStamped, + self.publish_topic_, + 10, # QoS history depth + ) + + # --- Timer Setup --- + self.timer_ = self.create_timer(self.timer_period_, self.timer_callback) + + self.get_logger().info( + "Simulated Orientation Publisher node started and listening to TF." + ) + + def timer_callback(self): + """ + Periodically looks up the TF transform and publishes the orientation quaternion. + """ + try: + # Lookup the transform at the latest available time using rclpy.time.Time() + # This asks TF2 for the most recent data relating the two frames. + lookup_time = ( + Time() + ) # Equivalent to Time(seconds=0, nanoseconds=0) for latest + transform_stamped = self.tf_buffer_.lookup_transform( + target_frame=self.reference_frame_, + source_frame=self.target_link_, + time=lookup_time, + timeout=Duration(seconds=0.1), + ) + + # Create and populate the QuaternionStamped message + quat_stamped_msg = QuaternionStamped() + + # Header: Timestamp of the message itself, and the frame the data is relative to + quat_stamped_msg.header.stamp = self.get_clock().now().to_msg() + quat_stamped_msg.header.frame_id = self.reference_frame_ + + # Quaternion: Copy the orientation directly from the lookup result + quat_stamped_msg.quaternion = transform_stamped.transform.rotation + + # Publish the message + self.publisher_.publish(quat_stamped_msg) + + except Exception as e: + # Catch any other unexpected errors + self.get_logger().error(f"Unexpected error in timer callback: {e}") + + +def main(args=None): + rclpy.init(args=args) + node = None + try: + node = SimulatedOrientationPublisherNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + except ValueError as e: + if node: + node.get_logger().fatal(f"Node initialization failed: {e}") + else: + print(f"Node initialization failed: {e}") + except Exception as e: + if node: + node.get_logger().error(f"Unhandled exception in main: {e}") + else: + print(f"Unhandled exception in main: {e}") + finally: + if node: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ada_moveit/scripts/unified_joint_state_publisher.py b/ada_moveit/scripts/unified_joint_state_publisher.py new file mode 100755 index 0000000..018dcb1 --- /dev/null +++ b/ada_moveit/scripts/unified_joint_state_publisher.py @@ -0,0 +1,144 @@ +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +class UnifiedJointStatePublisher(Node): + def __init__(self): + super().__init__("unified_joint_state_publisher") + # Declare and get the lock_joints parameter + self.declare_parameter("lock_joints", False) + self.lock_joints = ( + self.get_parameter("lock_joints").get_parameter_value().bool_value + ) + if self.lock_joints: + self.get_logger().info( + "Articutool joints are LOCKED. Will not publish their state." + ) + self.ada_joint_state = JointState() + self.articutool_joint_state = JointState() + + # Store the previous state + self.previous_ada_joint_state = JointState() + self.previous_articutool_joint_state = JointState() + + self.ada_sub = self.create_subscription( + JointState, "/ada/joint_states", self.ada_joint_state_callback, 10 + ) + self.articutool_sub = self.create_subscription( + JointState, + "/articutool/joint_states", + self.articutool_joint_state_callback, + 10, + ) + self.unified_pub = self.create_publisher(JointState, "/joint_states", 10) + self.timer = self.create_timer(0.05, self.publish_unified_joint_state) + + def ada_joint_state_callback(self, msg: JointState): + self.previous_ada_joint_state = self.ada_joint_state # Update previous + self.ada_joint_state = msg + + def articutool_joint_state_callback(self, msg: JointState): + self.previous_articutool_joint_state = ( + self.articutool_joint_state + ) # Update previous + self.articutool_joint_state = msg + + def publish_unified_joint_state(self): + unified_msg = JointState() + unified_msg.header.stamp = self.get_clock().now().to_msg() + + unified_msg.name = [] + unified_msg.position = [] + unified_msg.velocity = [] + unified_msg.effort = [] + + ada_names_set = set(self.ada_joint_state.name) + # Conditionally include Articutool joint states + if self.lock_joints: + articutool_names_set = set() + else: + articutool_names_set = set(self.articutool_joint_state.name) + unified_names = list(ada_names_set.union(articutool_names_set)) + unified_msg.name = unified_names + + for name in unified_names: + # Use current data if available, otherwise use previous + ada_pos = None + ada_vel = None + ada_eff = None + articutool_pos = None + articutool_vel = None + articutool_eff = None + + if name in self.ada_joint_state.name: + idx = self.ada_joint_state.name.index(name) + ada_pos = self.ada_joint_state.position[idx] + if self.ada_joint_state.velocity: + ada_vel = self.ada_joint_state.velocity[idx] + if self.ada_joint_state.effort: + ada_eff = self.ada_joint_state.effort[idx] + elif name in self.previous_ada_joint_state.name: + idx = self.previous_ada_joint_state.name.index(name) + ada_pos = self.previous_ada_joint_state.position[idx] + if self.previous_ada_joint_state.velocity: + ada_vel = self.previous_ada_joint_state.velocity[idx] + if self.previous_ada_joint_state.effort: + ada_eff = self.previous_ada_joint_state.effort[idx] + + if name in self.articutool_joint_state.name: + idx = self.articutool_joint_state.name.index(name) + articutool_pos = self.articutool_joint_state.position[idx] + if self.articutool_joint_state.velocity: + articutool_vel = self.articutool_joint_state.velocity[idx] + if self.articutool_joint_state.effort: + articutool_eff = self.articutool_joint_state.effort[idx] + elif name in self.previous_articutool_joint_state.name: + idx = self.previous_articutool_joint_state.name.index(name) + articutool_pos = self.previous_articutool_joint_state.position[idx] + if self.previous_articutool_joint_state.velocity: + articutool_vel = self.previous_articutool_joint_state.velocity[idx] + if self.previous_articutool_joint_state.effort: + articutool_eff = self.previous_articutool_joint_state.effort[idx] + + # Append the appropriate values to the unified message + unified_msg.position.append( + ada_pos + if ada_pos is not None + else articutool_pos + if articutool_pos is not None + else 0.0 + ) + if ada_vel is not None or articutool_vel is not None: + unified_msg.velocity.append( + ada_vel + if ada_vel is not None + else articutool_vel + if articutool_vel is not None + else 0.0 + ) + if ada_eff is not None or articutool_eff is not None: + unified_msg.effort.append( + ada_eff + if ada_eff is not None + else articutool_eff + if articutool_eff is not None + else 0.0 + ) + + self.unified_pub.publish(unified_msg) + + +def main(args=None): + rclpy.init(args=args) + unified_publisher = UnifiedJointStatePublisher() + rclpy.spin(unified_publisher) + unified_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()