From 2d8ac1371d7c292edfe59b63f401ea44229b7f52 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 12 Nov 2024 21:13:21 -0800 Subject: [PATCH 01/53] Add ros2_control block for Articulable Fork, and initial positions --- ada_moveit/config/ada.ros2_control.xacro | 54 ++++++++++++++++++++++++ ada_moveit/config/initial_positions.yaml | 4 +- 2 files changed, 57 insertions(+), 1 deletion(-) diff --git a/ada_moveit/config/ada.ros2_control.xacro b/ada_moveit/config/ada.ros2_control.xacro index 5a6282a..1ec0f5c 100644 --- a/ada_moveit/config/ada.ros2_control.xacro +++ b/ada_moveit/config/ada.ros2_control.xacro @@ -133,4 +133,58 @@ + + + + + + + + mock_components/GenericSystem + false + 0.0 + + + ada_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/initial_positions.yaml b/ada_moveit/config/initial_positions.yaml index 40ec6e7..030bfb5 100644 --- a/ada_moveit/config/initial_positions.yaml +++ b/ada_moveit/config/initial_positions.yaml @@ -19,4 +19,6 @@ initial_positions: j2n6s200_joint_5: 1.43588 j2n6s200_joint_6: 1.32575 j2n6s200_joint_finger_1: 1.317 - j2n6s200_joint_finger_2: 1.317 \ No newline at end of file + j2n6s200_joint_finger_2: 1.317 + af_joint_1: 0.0 + af_joint_2: 0.0 From 2f9129abd0f7757c27874b428ae2aba4d311a407 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 13 Nov 2024 08:18:07 -0800 Subject: [PATCH 02/53] Add Articulable Fork info to SRDF --- ada_moveit/config/ada.srdf | 45 ++++++++++++++++++++++++- ada_moveit/config/mock_controllers.yaml | 2 +- 2 files changed, 45 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index 7cd884b..f54f14a 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -10,7 +10,7 @@ - + @@ -21,6 +21,9 @@ + + + @@ -38,6 +41,14 @@ + + + + + + + + @@ -261,4 +272,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 7dc3acc..130fa45 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -23,7 +23,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: From e6d962e8c08b2978398991856192ee1f4a0d0d61 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 13 Nov 2024 08:45:57 -0800 Subject: [PATCH 03/53] Add Articulable Fork mock controllers --- ada_moveit/config/joint_limits.yaml | 12 ++++++- ada_moveit/config/mock_controllers.yaml | 40 +++++++++++++++++++++++ ada_moveit/config/mock_servo.yaml | 2 +- ada_moveit/config/moveit_controllers.yaml | 19 +++++++++++ 4 files changed, 71 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/joint_limits.yaml b/ada_moveit/config/joint_limits.yaml index ae7b07b..3275f07 100644 --- a/ada_moveit/config/joint_limits.yaml +++ b/ada_moveit/config/joint_limits.yaml @@ -47,4 +47,14 @@ joint_limits: has_velocity_limits: true max_velocity: 1.0 has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file + 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/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 130fa45..95bd4b3 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -18,9 +18,18 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + jaco_arm_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController + + articulable_fork_controller: + type: force_gate_controller/ForceGateController + + + articulable_fork_cartesian_controller: + type: cartesian_twist_controller/CartesianTwistController + jaco_arm_cartesian_controller: ros__parameters: end_effector_link: "j2n6s200_end_effector" @@ -82,6 +91,37 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 +articulable_fork_cartesian_controller: + ros__parameters: + end_effector_link: "forkTip" + robot_base_link: "af_link_base" + ik_solver: "selectively_damped_least_squares" + joints: + - af_joint_1 + - af_joint_2 + command_interfaces: + - position + +articulable_fork_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 + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index 07526be..68f41dd 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -47,7 +47,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/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 480d3cc..6e3d4ef 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -7,6 +7,8 @@ moveit_simple_controller_manager: - jaco_arm_controller - jaco_arm_cartesian_controller - jaco_arm_servo_controller + - articulable_fork_controller + - articulable_fork_cartesian_controller - hand_controller jaco_arm_controller: @@ -44,6 +46,23 @@ moveit_simple_controller_manager: - j2n6s200_joint_4 - j2n6s200_joint_5 - j2n6s200_joint_6 + + articulable_fork_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - af_joint_1 + - af_joint_2 + + articulable_fork_cartesian_controller: + type: "" + action_ns: cart_commands + default: false + joints: + - af_joint_1 + - af_joint_2 + hand_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory From 8e0769ad2b64ddf89819e85fd7432dfb0ee40d22 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 13 Nov 2024 09:17:06 -0800 Subject: [PATCH 04/53] Add af_ros2_control and fix readonly param --- ada_moveit/config/ada.ros2_control.xacro | 12 ++++-------- ada_moveit/config/ada.urdf.xacro | 4 ++++ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ada_moveit/config/ada.ros2_control.xacro b/ada_moveit/config/ada.ros2_control.xacro index 1ec0f5c..9a67ea4 100644 --- a/ada_moveit/config/ada.ros2_control.xacro +++ b/ada_moveit/config/ada.ros2_control.xacro @@ -153,10 +153,8 @@ 1 - - - - + + ${initial_positions['af_joint_1']} @@ -169,10 +167,8 @@ 2 - - - - + + ${initial_positions['af_joint_2']} diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index 69b7e3f..d6265a4 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -14,4 +14,8 @@ initial_positions_file="$(arg initial_positions_file)" sim="$(arg sim)" /> + + From 7e876aafe0b0c4d9d12b71072219d44de41fa039 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 13 Nov 2024 09:47:03 -0800 Subject: [PATCH 05/53] Fix indentation --- ada_moveit/config/moveit_controllers.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 6e3d4ef..0909613 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -47,7 +47,7 @@ moveit_simple_controller_manager: - j2n6s200_joint_5 - j2n6s200_joint_6 - articulable_fork_controller: + articulable_fork_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true From 3bb0bc94b8b72dd1851fc2381accf544fc692b42 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Nov 2024 08:45:07 -0800 Subject: [PATCH 06/53] Fix namespace conflict by renaming ada_hardware::DynamixelHardware to dynamixel_hardware::DynamixelHardware --- ada_moveit/config/ada.ros2_control.xacro | 4 +- ada_moveit/config/real_controllers.yaml | 50 +++++++++++++++++++++++- 2 files changed, 51 insertions(+), 3 deletions(-) diff --git a/ada_moveit/config/ada.ros2_control.xacro b/ada_moveit/config/ada.ros2_control.xacro index 9a67ea4..481f8e2 100644 --- a/ada_moveit/config/ada.ros2_control.xacro +++ b/ada_moveit/config/ada.ros2_control.xacro @@ -144,8 +144,8 @@ false 0.0 - - ada_hardware/DynamixelHardware + + dynamixel_hardware/DynamixelHardware /dev/ttyUSB0 1000000 diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index ef29d2e..3178b1b 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -18,12 +18,22 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + jaco_arm_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController + + articulable_fork_controller: + type: force_gate_controller/ForceGateController + + + articulable_fork_cartesian_controller: + type: cartesian_twist_controller/CartesianTwistController + + 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: @@ -98,6 +108,44 @@ jaco_arm_servo_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 +articulable_fork_cartesian_controller: + ros__parameters: + end_effector_link: "forkTip" + robot_base_link: "af_link_base" + ik_solver: "selectively_damped_least_squares" + joints: + - af_joint_1 + - af_joint_2 + command_interfaces: + - velocity + +articulable_fork_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + command_interfaces: + - velocity + state_interfaces: + - position + - velocity + allow_partial_joints_goal: false + open_loop_control: false + stopped_velocity_tolerance: 0.01 + 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} + j2n6s200_joint_2: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} + j2n6s200_joint_3: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} + j2n6s200_joint_4: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} + j2n6s200_joint_5: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} + j2n6s200_joint_6: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 + hand_controller: ros__parameters: joints: From 301926fb33870e53fc0c9e629d723f2a83304eb9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Nov 2024 12:12:18 -0800 Subject: [PATCH 07/53] Add PickIK kinematics solver for articulable_fork group --- ada_moveit/config/kinematics.yaml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index f9b2175..6f03dd3 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -12,3 +12,18 @@ jaco_arm: 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 + From 790772638521ab366f9c095351fbf01d89235d07 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Nov 2024 17:07:46 -0800 Subject: [PATCH 08/53] Add OMPL planner for Articulable Fork --- ada_moveit/config/ompl_planning.yaml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index f4cc886..9aec325 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -76,6 +76,21 @@ jaco_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault +articulable_fork: + 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 hand: enforce_constrained_state_space: true projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) From 678d95ddb9dfd7d07b0ab1f8f21c2ba5788e1851 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 17 Nov 2024 14:12:53 -0800 Subject: [PATCH 09/53] Add af_velocity_controller and af_joint_trajectory_controller, add hybrid control mode --- ada_moveit/config/ada.ros2_control.xacro | 10 + ada_moveit/config/hybrid_controllers.yaml | 126 ++++++++++ ada_moveit/config/moveit_controllers.yaml | 8 +- ada_moveit/config/ompl_planning.yaml | 15 -- ada_moveit/config/real_controllers.yaml | 48 +--- ada_moveit/launch/articulable_fork.launch.py | 243 +++++++++++++++++++ 6 files changed, 396 insertions(+), 54 deletions(-) create mode 100644 ada_moveit/config/hybrid_controllers.yaml create mode 100644 ada_moveit/launch/articulable_fork.launch.py diff --git a/ada_moveit/config/ada.ros2_control.xacro b/ada_moveit/config/ada.ros2_control.xacro index 481f8e2..16855ff 100644 --- a/ada_moveit/config/ada.ros2_control.xacro +++ b/ada_moveit/config/ada.ros2_control.xacro @@ -17,6 +17,11 @@ ada_hardware/Jaco2 + + mock_components/GenericSystem + false + 0.0 + @@ -149,6 +154,11 @@ /dev/ttyUSB0 1000000 + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 + diff --git a/ada_moveit/config/hybrid_controllers.yaml b/ada_moveit/config/hybrid_controllers.yaml new file mode 100644 index 0000000..d1fb2dc --- /dev/null +++ b/ada_moveit/config/hybrid_controllers.yaml @@ -0,0 +1,126 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + jaco_arm_controller: + type: force_gate_controller/ForceGateController + + + jaco_arm_servo_controller: + type: force_gate_controller/ForceGatePositionController + + + hand_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + + jaco_arm_cartesian_controller: + type: cartesian_twist_controller/CartesianTwistController + + + af_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + af_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +jaco_arm_cartesian_controller: + ros__parameters: + end_effector_link: "j2n6s200_end_effector" + robot_base_link: "j2n6s200_link_base" + ik_solver: "selectively_damped_least_squares" + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + command_interfaces: + - position + +jaco_arm_controller: + ros__parameters: + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + 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 + j2n6s200_joint_1: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_2: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_3: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_4: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_5: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_6: + goal: 0.02 + trajectory: 0.05 + +jaco_arm_servo_controller: + ros__parameters: + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + +af_velocity_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + +af_joint_trajectory_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + + command_interfaces: + - position + + state_interfaces: + - position + +hand_controller: + ros__parameters: + joints: + - j2n6s200_joint_finger_1 + - j2n6s200_joint_finger_2 + command_interfaces: + - position + state_interfaces: + - position + allow_partial_joints_goal: false + open_loop_control: false + constraints: + stopped_velocity_tolerance: 0.01 + gains: # Required because we're controlling a velocity interface + j2n6s200_joint_finger_1: {p: 1.0, d: 0.0, i: 0.0, i_clamp: 1.0} + j2n6s200_joint_finger_2: {p: 1.0, d: 0.0, i: 0.0, i_clamp: 1.0} diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 0909613..d97e98b 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -7,8 +7,8 @@ moveit_simple_controller_manager: - jaco_arm_controller - jaco_arm_cartesian_controller - jaco_arm_servo_controller - - articulable_fork_controller - - articulable_fork_cartesian_controller + - af_velocity_controller + - af_joint_trajectory_controller - hand_controller jaco_arm_controller: @@ -47,7 +47,7 @@ moveit_simple_controller_manager: - j2n6s200_joint_5 - j2n6s200_joint_6 - articulable_fork_controller: + af_velocity_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true @@ -55,7 +55,7 @@ moveit_simple_controller_manager: - af_joint_1 - af_joint_2 - articulable_fork_cartesian_controller: + af_joint_trajectory_controller: type: "" action_ns: cart_commands default: false diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index 9aec325..f4cc886 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -76,21 +76,6 @@ jaco_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault -articulable_fork: - 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 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 3178b1b..620e6ea 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -23,13 +23,11 @@ controller_manager: type: cartesian_twist_controller/CartesianTwistController - articulable_fork_controller: - type: force_gate_controller/ForceGateController - - - articulable_fork_cartesian_controller: - type: cartesian_twist_controller/CartesianTwistController + af_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + af_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController jaco_arm_cartesian_controller: ros__parameters: @@ -108,43 +106,23 @@ jaco_arm_servo_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 -articulable_fork_cartesian_controller: - ros__parameters: - end_effector_link: "forkTip" - robot_base_link: "af_link_base" - ik_solver: "selectively_damped_least_squares" - joints: - - af_joint_1 - - af_joint_2 - command_interfaces: - - velocity +af_velocity_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 -articulable_fork_controller: +af_joint_trajectory_controller: ros__parameters: joints: - af_joint_1 - af_joint_2 + command_interfaces: - - velocity + - position + state_interfaces: - position - - velocity - allow_partial_joints_goal: false - open_loop_control: false - stopped_velocity_tolerance: 0.01 - 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} - j2n6s200_joint_2: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} - j2n6s200_joint_3: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} - j2n6s200_joint_4: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} - j2n6s200_joint_5: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} - j2n6s200_joint_6: {p: 4.0, d: 0.0, i: 0.0, i_clamp: 1.0, normalize_error: true} - af_joint_1: - goal: 0.02 - trajectory: 0.05 - af_joint_2: - goal: 0.02 - trajectory: 0.05 hand_controller: ros__parameters: diff --git a/ada_moveit/launch/articulable_fork.launch.py b/ada_moveit/launch/articulable_fork.launch.py new file mode 100644 index 0000000..3baafcd --- /dev/null +++ b/ada_moveit/launch/articulable_fork.launch.py @@ -0,0 +1,243 @@ +# Copyright 2020 Yutaka Kondo +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import yaml +import xacro + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launch_utils import ( + add_debuggable_node, + DeclareBooleanLaunchArg, +) +from moveit_configs_utils.launches import generate_demo_launch +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +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 + + +def generate_launch_description(): + + # MoveIt Config + moveit_config = MoveItConfigsBuilder( + "ada", package_name="ada_moveit" + ).to_moveit_configs() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + + ctrl_da = DeclareLaunchArgument( + "controllers_file", + default_value=[sim, "_controllers.yaml"], + # default_value=["hybrid_controllers.yaml"], + description="ROS2 Controller YAML configuration in config folder", + ) + controllers_file = LaunchConfiguration("controllers_file") + + # Log Level + log_level_da = DeclareLaunchArgument( + "log_level", + default_value="info", + description="Logging level (debug, info, warn, error, fatal)", + ) + log_level = LaunchConfiguration("log_level") + + # Copy from generate_demo_launch + ld = LaunchDescription() + ld.add_action(sim_da) + ld.add_action(ctrl_da) + ld.add_action(log_level_da) + + ld.add_action( + DeclareBooleanLaunchArg( + "debug", + default_value=False, + description="By default, we are not in debug mode", + ) + ) + ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True)) + + # If there are virtual joints, broadcast static tf by including virtual_joints launch + virtual_joints_launch = ( + moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" + ) + if virtual_joints_launch.exists(): + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(virtual_joints_launch)), + launch_arguments={ + "log_level": log_level, + }.items(), + ) + ) + + # Given the published joint states, publish tf for the robot links + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/rsp.launch.py") + ), + launch_arguments={ + "log_level": log_level, + }.items(), + ) + ) + + # Launch the Move Group + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/move_group.launch.py") + ), + launch_arguments={ + "sim": sim, + "log_level": log_level, + }.items(), + ) + ) + + # Run Rviz and load the default config to see the state of the move_group node + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/moveit_rviz.launch.py") + ), + launch_arguments={ + "log_level": log_level, + }.items(), + condition=IfCondition(LaunchConfiguration("use_rviz")), + ) + ) + + # 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) + } + + robot_controllers = PathJoinSubstitution( + [str(moveit_config.package_path), "config", controllers_file] + ) + + # Joint Controllers + ld.add_action( + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + arguments=["--ros-args", "--log-level", log_level], + ) + ) + + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/spawn_controllers.launch.py") + ), + launch_arguments={ + "log_level": log_level, + }.items(), + ) + ) + + return ld + + # OLD CODE STARTS HERE + + # robot_name = "ada" + # package_name = robot_name + "_moveit" + # robot_description = os.path.join(get_package_share_directory( + # package_name), "config", robot_name + ".urdf.xacro") + # robot_description_config = xacro.process_file(robot_description) + # + # controller_config = os.path.join( + # get_package_share_directory( + # package_name), "config", "real_controllers.yaml" + # ) + # + # return LaunchDescription([ + # Node( + # package="controller_manager", + # executable="ros2_control_node", + # parameters=[ + # {"robot_description": robot_description_config.toxml()}, controller_config], + # output="screen", + # ), + # + # Node( + # package="controller_manager", + # executable="spawner", + # arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + # output="screen", + # ), + # + # Node( + # package="controller_manager", + # executable="spawner", + # arguments=["af_velocity_controller", "-c", "/controller_manager"], + # output="screen", + # ), + # + # Node( + # package="controller_manager", + # executable="spawner", + # arguments=["af_joint_trajectory_controller", "-c", "/controller_manager"], + # output="screen", + # ), + # + # Node( + # package="robot_state_publisher", + # executable="robot_state_publisher", + # name="robot_state_publisher", + # parameters=[ + # {"robot_description": robot_description_config.toxml()}], + # output="screen", + # ), + # + # ]) + From 1e243d2bcec4c466bfa31b307dfb6d8fcf5c943a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 17 Nov 2024 15:27:50 -0800 Subject: [PATCH 10/53] Change mock controllers for AF --- ada_moveit/config/mock_controllers.yaml | 38 +++++++------------- ada_moveit/launch/articulable_fork.launch.py | 1 - 2 files changed, 12 insertions(+), 27 deletions(-) diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 95bd4b3..d1fb2dc 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -23,12 +23,11 @@ controller_manager: type: cartesian_twist_controller/CartesianTwistController - articulable_fork_controller: - type: force_gate_controller/ForceGateController - + af_velocity_controller: + type: velocity_controllers/JointGroupVelocityController - articulable_fork_cartesian_controller: - type: cartesian_twist_controller/CartesianTwistController + af_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController jaco_arm_cartesian_controller: ros__parameters: @@ -91,36 +90,23 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 -articulable_fork_cartesian_controller: - ros__parameters: - end_effector_link: "forkTip" - robot_base_link: "af_link_base" - ik_solver: "selectively_damped_least_squares" - joints: - - af_joint_1 - - af_joint_2 - command_interfaces: - - position +af_velocity_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 -articulable_fork_controller: +af_joint_trajectory_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 hand_controller: ros__parameters: diff --git a/ada_moveit/launch/articulable_fork.launch.py b/ada_moveit/launch/articulable_fork.launch.py index 3baafcd..8b99bbc 100644 --- a/ada_moveit/launch/articulable_fork.launch.py +++ b/ada_moveit/launch/articulable_fork.launch.py @@ -63,7 +63,6 @@ def generate_launch_description(): ctrl_da = DeclareLaunchArgument( "controllers_file", default_value=[sim, "_controllers.yaml"], - # default_value=["hybrid_controllers.yaml"], description="ROS2 Controller YAML configuration in config folder", ) controllers_file = LaunchConfiguration("controllers_file") From b50e25ad3c26b874884ecda2a53ef7844a61f8bb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 18 Nov 2024 12:42:06 -0800 Subject: [PATCH 11/53] Add planning group for ADA + AF end-to-end control --- ada_moveit/config/ada.srdf | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index f54f14a..83aca02 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -24,6 +24,9 @@ + + + @@ -61,6 +64,26 @@ + + + + + + + + + + + + + + + + + + + + From bfb28ee88d43a2633ad7faeec4c7e5067475580e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Nov 2024 20:14:24 -0800 Subject: [PATCH 12/53] Add spacing --- ada_moveit/config/mock_controllers.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index d1fb2dc..b2986ce 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -26,6 +26,7 @@ controller_manager: af_velocity_controller: type: velocity_controllers/JointGroupVelocityController + af_joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController From d3f43911d07d18e9a504c97d03f971213c14cecc Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Nov 2024 09:01:55 -0800 Subject: [PATCH 13/53] Add mock jaco_af_controller and jaco_af_cartesian_controller --- ada_moveit/config/mock_controllers.yaml | 69 +++++++++++++++++++++++ ada_moveit/config/moveit_controllers.yaml | 30 ++++++++++ 2 files changed, 99 insertions(+) diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index b2986ce..de727f7 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -30,6 +30,14 @@ controller_manager: af_joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController + + jaco_af_controller: + type: force_gate_controller/ForceGateController + + + jaco_af_cartesian_controller: + type: cartesian_twist_controller/CartesianTwistController + jaco_arm_cartesian_controller: ros__parameters: end_effector_link: "j2n6s200_end_effector" @@ -109,6 +117,67 @@ af_joint_trajectory_controller: state_interfaces: - position +jaco_af_controller: + ros__parameters: + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + - 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 + j2n6s200_joint_1: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_2: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_3: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_4: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_5: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_6: + goal: 0.02 + trajectory: 0.05 + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 + +jaco_af_cartesian_controller: + ros__parameters: + end_effector_link: "forkTip" + robot_base_link: "j2n6s200_link_base" + ik_solver: "selectively_damped_least_squares" + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + - af_joint_1 + - af_joint_2 + command_interfaces: + - position + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index d97e98b..c1c991a 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -9,6 +9,8 @@ moveit_simple_controller_manager: - jaco_arm_servo_controller - af_velocity_controller - af_joint_trajectory_controller + - jaco_af_controller + - jaco_af_cartesian_controller - hand_controller jaco_arm_controller: @@ -63,6 +65,34 @@ moveit_simple_controller_manager: - af_joint_1 - af_joint_2 + jaco_af_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + - af_joint_1 + - af_joint_2 + + jaco_af_cartesian_controller: + type: "" + action_ns: cart_commands + default: false + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + - af_joint_1 + - af_joint_2 + hand_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory From f9ec2af2b1e4937f5774c2d3257d2905ae0ca357 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Nov 2024 09:05:18 -0800 Subject: [PATCH 14/53] Rename move group all to jaco_af --- ada_moveit/config/ada.srdf | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index 83aca02..e012fe0 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -24,7 +24,7 @@ - + @@ -64,7 +64,7 @@ - + @@ -74,7 +74,7 @@ - + From 2b17d17f9bc3c1c049e3b0be25822ef919c341da Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Nov 2024 09:10:54 -0800 Subject: [PATCH 15/53] Add jaco_af OMPL planning --- ada_moveit/config/ompl_planning.yaml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index f4cc886..e12cce8 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -76,6 +76,21 @@ jaco_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault +jaco_af: + 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 hand: enforce_constrained_state_space: true projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) From 926943f5aeae7edbcf9b18c5244b2de35ea4c418 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 19:54:36 -0800 Subject: [PATCH 16/53] Add jaco_af PickIkPlugin --- ada_moveit/config/kinematics.yaml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index 6f03dd3..092b853 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -13,6 +13,20 @@ jaco_arm: minimal_displacement_weight: 0.0 gd_step_size: 0.0001 +jaco_af: + 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 From 3d3c8cd01168b98dbb898bff1c786dffffad0992 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 19:59:29 -0800 Subject: [PATCH 17/53] Change Jaco+AF cartesian controller to damped least squares, Remove old AF controllers, Add Jaco+AF servo controller --- ada_moveit/config/mock_controllers.yaml | 44 ++++++++++--------------- 1 file changed, 17 insertions(+), 27 deletions(-) diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index de727f7..36e5a1a 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -23,18 +23,14 @@ controller_manager: type: cartesian_twist_controller/CartesianTwistController - af_velocity_controller: - type: velocity_controllers/JointGroupVelocityController - - - af_joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - - jaco_af_controller: type: force_gate_controller/ForceGateController + jaco_af_servo_controller: + type: force_gate_controller/ForceGatePositionController + + jaco_af_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController @@ -99,24 +95,6 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 -af_velocity_controller: - ros__parameters: - joints: - - af_joint_1 - - af_joint_2 - -af_joint_trajectory_controller: - ros__parameters: - joints: - - af_joint_1 - - af_joint_2 - - command_interfaces: - - position - - state_interfaces: - - position - jaco_af_controller: ros__parameters: joints: @@ -161,11 +139,23 @@ jaco_af_controller: goal: 0.02 trajectory: 0.05 +jaco_af_servo_controller: + ros__parameters: + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + - af_joint_1 + - af_joint_2 + jaco_af_cartesian_controller: ros__parameters: end_effector_link: "forkTip" robot_base_link: "j2n6s200_link_base" - ik_solver: "selectively_damped_least_squares" + ik_solver: "damped_least_squares" joints: - j2n6s200_joint_1 - j2n6s200_joint_2 From 4f2396f365d4dccf05f92e8c2c80ba328c445298 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 20:05:59 -0800 Subject: [PATCH 18/53] Use Jaco+AF move group for mock servo control --- ada_moveit/config/mock_servo.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index 68f41dd..dd3219a 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -43,11 +43,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_af planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames - ee_frame_name: j2n6s200_end_effector # The name of the end effector link, used to return the EE pose + ee_frame_name: forkTip 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 @@ -67,7 +67,7 @@ servo_node: joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_arm_servo_controller/commands # Publish outgoing commands here + command_out_topic: /jaco_af_servo_controller/commands ## Collision checking for the entire robot body check_collisions: true # Check collisions? From c414d0ebb6a56288c64bba698d5b9fe8d5a3ea5b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 20:09:01 -0800 Subject: [PATCH 19/53] Add Jaco+AF MoveIt2 controllers, Remove old AF MoveIt2 controllers --- ada_moveit/config/moveit_controllers.yaml | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index c1c991a..2eb6ab0 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -7,10 +7,9 @@ moveit_simple_controller_manager: - jaco_arm_controller - jaco_arm_cartesian_controller - jaco_arm_servo_controller - - af_velocity_controller - - af_joint_trajectory_controller - jaco_af_controller - jaco_af_cartesian_controller + - jaco_af_servo_controller - hand_controller jaco_arm_controller: @@ -49,26 +48,24 @@ moveit_simple_controller_manager: - j2n6s200_joint_5 - j2n6s200_joint_6 - af_velocity_controller: + jaco_af_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 - af_joint_1 - af_joint_2 - af_joint_trajectory_controller: + jaco_af_servo_controller: type: "" - action_ns: cart_commands + action_ns: commands default: false - joints: - - af_joint_1 - - af_joint_2 - - jaco_af_controller: - type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - j2n6s200_joint_1 - j2n6s200_joint_2 From 3a98276edd4e05021e25e81a74cf4ef35d845d01 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 20:11:50 -0800 Subject: [PATCH 20/53] Use Jaco+AF move group for real servo control --- ada_moveit/config/real_servo.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/real_servo.yaml b/ada_moveit/config/real_servo.yaml index e36551b..817bf4e 100644 --- a/ada_moveit/config/real_servo.yaml +++ b/ada_moveit/config/real_servo.yaml @@ -43,7 +43,7 @@ servo_node: monitored_planning_scene_topic: "monitored_planning_scene" ## MoveIt properties - move_group_name: jaco_arm # Often 'manipulator' or 'arm' + move_group_name: jaco_af # Often 'manipulator' or 'arm' planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames @@ -67,7 +67,7 @@ servo_node: joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_arm_servo_controller/commands # Publish outgoing commands here + command_out_topic: /jaco_af_servo_controller/commands # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? From d8514305145571192824f634a43ec2ab9ab692ae Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 20:13:22 -0800 Subject: [PATCH 21/53] Use Jaco+AF cartesian control for keyboard tele-op --- ada_moveit/scripts/ada_keyboard_teleop.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ada_moveit/scripts/ada_keyboard_teleop.py b/ada_moveit/scripts/ada_keyboard_teleop.py index 7f5e12f..cc219c9 100755 --- a/ada_moveit/scripts/ada_keyboard_teleop.py +++ b/ada_moveit/scripts/ada_keyboard_teleop.py @@ -90,6 +90,8 @@ def get_key(settings): "4": "j2n6s200_joint_4", "5": "j2n6s200_joint_5", "6": "j2n6s200_joint_6", + "7": "af_joint_1", + "8": "af_joint_2", } reverse_joint_direction_key = "r" # pylint: disable=invalid-name @@ -109,7 +111,7 @@ def main(args=None): rclpy.init(args=args) node = rclpy.create_node("ada_keyboard_teleop") twist_pub = node.create_publisher( - TwistStamped, "/jaco_arm_cartesian_controller/twist_cmd", 1 + TwistStamped, "/jaco_af_cartesian_controller/twist_cmd", 1 ) joint_pub = node.create_publisher(JointJog, "/servo_node/delta_joint_cmds", 1) From e537c76b9116e17e3a5f0b05e14ca183803447c1 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 20:13:45 -0800 Subject: [PATCH 22/53] Add AF joints to servo republisher --- ada_moveit/scripts/servo_republisher.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ada_moveit/scripts/servo_republisher.py b/ada_moveit/scripts/servo_republisher.py index cf94783..1d7ed85 100755 --- a/ada_moveit/scripts/servo_republisher.py +++ b/ada_moveit/scripts/servo_republisher.py @@ -43,6 +43,8 @@ "j2n6s200_joint_4", "j2n6s200_joint_5", "j2n6s200_joint_6", + "af_joint_1", + "af_joint_2", ] POSITION_INTERFACE_NAME = "position" VELOCITY_INTERFACE_NAME = "velocity" From eb8d630a2841dcf657bc19bb4e5cabe5b6b82a92 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 23 Nov 2024 21:34:45 -0800 Subject: [PATCH 23/53] Add real Jaco+AF controllers --- ada_moveit/config/real_controllers.yaml | 91 ++++++++++++++++++++++--- 1 file changed, 80 insertions(+), 11 deletions(-) diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index 620e6ea..1bd4a02 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -23,11 +23,16 @@ controller_manager: type: cartesian_twist_controller/CartesianTwistController - af_velocity_controller: - type: velocity_controllers/JointGroupVelocityController + jaco_af_controller: + type: force_gate_controller/ForceGateController - af_joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController + + jaco_af_servo_controller: + type: force_gate_controller/ForceGatePositionController + + + jaco_af_cartesian_controller: + type: cartesian_twist_controller/CartesianTwistController jaco_arm_cartesian_controller: ros__parameters: @@ -106,23 +111,87 @@ jaco_arm_servo_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 -af_velocity_controller: +jaco_af_controller: ros__parameters: joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 - 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 + j2n6s200_joint_1: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_2: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_3: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_4: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_5: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_6: + goal: 0.02 + trajectory: 0.05 + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 -af_joint_trajectory_controller: +jaco_af_servo_controller: ros__parameters: joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 - af_joint_1 - af_joint_2 + wrench_threshold: + topic: /wireless_ft/ftSensor3 + fMag: 1.0 - command_interfaces: - - position - - state_interfaces: - - position +jaco_af_cartesian_controller: + ros__parameters: + end_effector_link: "forkTip" + robot_base_link: "j2n6s200_link_base" + ik_solver: "damped_least_squares" + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + - af_joint_1 + - af_joint_2 + command_interfaces: + - position + wrench_threshold: + topic: /wireless_ft/ftSensor3 + fMag: 1.0 hand_controller: ros__parameters: From 80543015b35aa95e161dc5503752d0c29f2b389e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 25 Nov 2024 14:14:09 -0800 Subject: [PATCH 24/53] Update to selectively_damped_least_squares --- ada_moveit/config/mock_controllers.yaml | 2 +- ada_moveit/config/real_controllers.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 36e5a1a..57e090d 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -155,7 +155,7 @@ jaco_af_cartesian_controller: ros__parameters: end_effector_link: "forkTip" robot_base_link: "j2n6s200_link_base" - ik_solver: "damped_least_squares" + ik_solver: "selectively_damped_least_squares" joints: - j2n6s200_joint_1 - j2n6s200_joint_2 diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index 1bd4a02..9e45d59 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -177,7 +177,7 @@ jaco_af_cartesian_controller: ros__parameters: end_effector_link: "forkTip" robot_base_link: "j2n6s200_link_base" - ik_solver: "damped_least_squares" + ik_solver: "selectively_damped_least_squares" joints: - j2n6s200_joint_1 - j2n6s200_joint_2 From 507a5304a2060130bbf5e5b1d8e1b4c8fbdb7f08 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 25 Nov 2024 14:16:12 -0800 Subject: [PATCH 25/53] Update hybrid_controllers --- ada_moveit/config/hybrid_controllers.yaml | 82 ++++++++++++++++++++--- 1 file changed, 71 insertions(+), 11 deletions(-) diff --git a/ada_moveit/config/hybrid_controllers.yaml b/ada_moveit/config/hybrid_controllers.yaml index d1fb2dc..57e090d 100644 --- a/ada_moveit/config/hybrid_controllers.yaml +++ b/ada_moveit/config/hybrid_controllers.yaml @@ -23,11 +23,16 @@ controller_manager: type: cartesian_twist_controller/CartesianTwistController - af_velocity_controller: - type: velocity_controllers/JointGroupVelocityController + jaco_af_controller: + type: force_gate_controller/ForceGateController + + + jaco_af_servo_controller: + type: force_gate_controller/ForceGatePositionController - af_joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController + + jaco_af_cartesian_controller: + type: cartesian_twist_controller/CartesianTwistController jaco_arm_cartesian_controller: ros__parameters: @@ -90,23 +95,78 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 -af_velocity_controller: +jaco_af_controller: ros__parameters: joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 - 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 + j2n6s200_joint_1: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_2: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_3: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_4: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_5: + goal: 0.02 + trajectory: 0.05 + j2n6s200_joint_6: + goal: 0.02 + trajectory: 0.05 + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 -af_joint_trajectory_controller: +jaco_af_servo_controller: ros__parameters: joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 - af_joint_1 - af_joint_2 - command_interfaces: - - position - - state_interfaces: - - position +jaco_af_cartesian_controller: + ros__parameters: + end_effector_link: "forkTip" + robot_base_link: "j2n6s200_link_base" + ik_solver: "selectively_damped_least_squares" + joints: + - j2n6s200_joint_1 + - j2n6s200_joint_2 + - j2n6s200_joint_3 + - j2n6s200_joint_4 + - j2n6s200_joint_5 + - j2n6s200_joint_6 + - af_joint_1 + - af_joint_2 + command_interfaces: + - position hand_controller: ros__parameters: From 260e945cb0036cc7c9159dd4665049996fcec516 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 27 Nov 2024 07:32:09 -0800 Subject: [PATCH 26/53] Add hybrid servo yaml file --- ada_moveit/config/hybrid_servo.yaml | 76 +++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 ada_moveit/config/hybrid_servo.yaml diff --git a/ada_moveit/config/hybrid_servo.yaml b/ada_moveit/config/hybrid_servo.yaml new file mode 100644 index 0000000..dd3219a --- /dev/null +++ b/ada_moveit/config/hybrid_servo.yaml @@ -0,0 +1,76 @@ +# Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +############################################### +# Modify all parameters related to servoing here +############################################### +servo_node: + ros__parameters: + moveit_servo: + use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + + ## Properties of incoming commands + command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s + scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.1 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + + # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) + # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + + ## Properties of outgoing commands + publish_period: 0.034 # 1/Nominal publish rate [seconds] + low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + + # What type of topic does your robot driver expect? + # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory + command_out_type: std_msgs/Float64MultiArray + + # What to publish? Can save some bandwidth as most robots only require positions or velocities + publish_joint_positions: true + publish_joint_velocities: false + publish_joint_accelerations: false + + ## Plugins for smoothing outgoing commands + smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + + # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, + # which other nodes can use as a source for information about the planning environment. + # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), + # then is_primary_planning_scene_monitor needs to be set to false. + is_primary_planning_scene_monitor: false + monitored_planning_scene_topic: "monitored_planning_scene" + + ## MoveIt properties + move_group_name: jaco_af + planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' + + ## Other frames + ee_frame_name: forkTip + 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 + incoming_command_timeout: 0.33 # Stop servoing if X seconds elapse without a new command + # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. + # Important because ROS may drop some messages and we need the robot to halt reliably. + num_outgoing_halt_msgs_to_publish: 0 + + ## Configure handling of singularities and joint limits + lower_singularity_threshold: 200.0 # Start decelerating when the condition number hits this (close to singularity) + hard_stop_singularity_threshold: 400.0 # Stop when the condition number hits this + joint_limit_margin: 0.2 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) + + ## Topic names + cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands + joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands + joint_topic: /joint_states + status_topic: ~/status # Publish status to this topic + command_out_topic: /jaco_af_servo_controller/commands + + ## Collision checking for the entire robot body + check_collisions: true # Check collisions? + collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. + self_collision_proximity_threshold: 0.001 # Start decelerating when a self-collision is this far [m] + scene_collision_proximity_threshold: 0.0075 # Start decelerating when a scene collision is this far [m] From fb7b8c5b3e385f31cd7bfd4a9240ad174803deab Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 29 Nov 2024 14:21:06 -0800 Subject: [PATCH 27/53] Add OverPlate configuration --- ada_moveit/config/ada.srdf | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index e012fe0..68e8c73 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -84,6 +84,16 @@ + + + + + + + + + + From 32b4ddb17c0a9b91fe5dfc5bfc8507155b63b677 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 29 Nov 2024 14:21:43 -0800 Subject: [PATCH 28/53] Add script to command twists --- ada_moveit/CMakeLists.txt | 1 + ada_moveit/scripts/ada_twist_teleop.py | 146 +++++++++++++++++++++++++ 2 files changed, 147 insertions(+) create mode 100755 ada_moveit/scripts/ada_twist_teleop.py diff --git a/ada_moveit/CMakeLists.txt b/ada_moveit/CMakeLists.txt index 3037056..7231d69 100644 --- a/ada_moveit/CMakeLists.txt +++ b/ada_moveit/CMakeLists.txt @@ -8,6 +8,7 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS + scripts/ada_twist_teleop.py scripts/ada_keyboard_teleop.py scripts/servo_republisher.py DESTINATION lib/${PROJECT_NAME} diff --git a/ada_moveit/scripts/ada_twist_teleop.py b/ada_moveit/scripts/ada_twist_teleop.py new file mode 100755 index 0000000..ddb50fb --- /dev/null +++ b/ada_moveit/scripts/ada_twist_teleop.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 + +import time +import yaml +import numpy as np +import argparse + +from control_msgs.msg import JointJog +from geometry_msgs.msg import TwistStamped +import rclpy +from rclpy.time import Time +from rclpy.duration import Duration +from tf2_geometry_msgs import Vector3Stamped # pylint: disable=unused-import +import tf2_py as tf2 +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from sensor_msgs.msg import JointState + +BASE_FRAME = "j2n6s200_link_base" +EE_FRAME = "forkTip" +JOINT_NAMES = [ + 'j2n6s200_joint_1', + 'j2n6s200_joint_2', + 'j2n6s200_joint_3', + 'j2n6s200_joint_4', + 'j2n6s200_joint_5', + 'j2n6s200_joint_6', + 'af_joint_1', + 'af_joint_2', +] +DEFAULT_ANGULAR = [0.0, 0.0, 0.0] +DEFAULT_LINEAR = [0.0, 0.0, 0.0] +DEFAULT_DURATION_S = 1.0 +WAIT_FOR_TF_S = 1.0 + +def parse_args(): + parser = argparse.ArgumentParser(description='Control robot arm with specified angular and linear velocities.') + parser.add_argument('--angular', nargs=3, type=float, default=DEFAULT_ANGULAR, + help='Angular velocity in x, y, z directions (rad/s)') + parser.add_argument('--linear', nargs=3, type=float, default=DEFAULT_LINEAR, + help='Linear velocity in x, y, z directions (m/s)') + parser.add_argument('--duration', type=float, default=DEFAULT_DURATION_S, + help='Duration of the motion (seconds)') + return parser.parse_args() + +def publish_zero_twist(node, twist_pub): + twist_msg = TwistStamped() + twist_msg.header.stamp = node.get_clock().now().to_msg() + twist_msg.header.frame_id = BASE_FRAME + twist_msg.twist.linear.x = 0.0 + twist_msg.twist.linear.y = 0.0 + twist_msg.twist.linear.z = 0.0 + twist_msg.twist.angular.x = 0.0 + twist_msg.twist.angular.y = 0.0 + twist_msg.twist.angular.z = 0.0 + + twist_pub.publish(twist_msg) + +def main(args=None): + # Initialize ROS context + rclpy.init(args=args) + node = rclpy.create_node("ada_twist_teleop") + twist_pub = node.create_publisher( + TwistStamped, "/jaco_af_cartesian_controller/twist_cmd", 1 + ) + + # Initialize the tf2 buffer and listener + tf_buffer = Buffer() + tf_listener = TransformListener(tf_buffer, node) # pylint: disable=unused-variable + + # Create the cartesian control messages + # The linear velocity is always in the base frame + linear_msg = Vector3Stamped() + linear_msg.header.stamp = Time().to_msg() # use latest time + # linear_msg.header.frame_id = BASE_FRAME + linear_msg.header.frame_id = EE_FRAME + # The angular velocity is always in the end effector frame + angular_msg = Vector3Stamped() + angular_msg.header.stamp = Time().to_msg() # use latest time + angular_msg.header.frame_id = EE_FRAME + # The final message should be either in the base or end effector frame. + # It should match the `robot_link_command_frame`` servo param. + twist_msg = TwistStamped() + twist_msg.header.frame_id = BASE_FRAME + + # Parse command-line arguments + args = parse_args() + angular = args.angular + linear = args.linear + duration_s = args.duration + print(f"angular (rad/s) = {angular}, linear (m/s) = {linear}, duration (s) = {duration_s}") + + # Delay for transforms to populate + start_time_s = time.time() + while time.time() - start_time_s < WAIT_FOR_TF_S: + rclpy.spin_once(node, timeout_sec=0) + + start_time_s = time.time() + while time.time() - start_time_s < duration_s: + rclpy.spin_once(node, timeout_sec=0) + + linear_msg.vector.x = linear[0] + linear_msg.vector.y = linear[1] + linear_msg.vector.z = linear[2] + twist_msg.twist.linear = linear_msg.vector + if linear_msg.header.frame_id != twist_msg.header.frame_id: + try: + linear_transformed = tf_buffer.transform( + linear_msg, twist_msg.header.frame_id + ) + twist_msg.twist.linear = linear_transformed.vector + except tf2.ExtrapolationException as exc: + node.get_logger().warning( + f"Transform from {linear_msg.header.frame_id} to " + f"{twist_msg.header.frame_id} failed: {type(exc)}: {exc}\n" + f"Interpreting the linear velocity in {twist_msg.header.frame_id} " + "without transforming." + ) + + angular_msg.vector.x = angular[0] + angular_msg.vector.y = angular[1] + angular_msg.vector.z = angular[2] + twist_msg.twist.angular = angular_msg.vector + if angular_msg.header.frame_id != twist_msg.header.frame_id: + try: + angular_transformed = tf_buffer.transform( + angular_msg, twist_msg.header.frame_id + ) + twist_msg.twist.angular = angular_transformed.vector + except tf2.ExtrapolationException as exc: + node.get_logger().warning( + f"Transform from {angular_msg.header.frame_id} to " + f"{twist_msg.header.frame_id} failed: {type(exc)}: {exc}\n" + f"Interpreting the angular velocity in {twist_msg.header.frame_id}" + " without transforming." + ) + + twist_msg.header.stamp = node.get_clock().now().to_msg() + twist_pub.publish(twist_msg) + publish_zero_twist(node, twist_pub) + + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() From 1a43bf98f8f7971123126e5c45af39083c2a7a2c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 29 Nov 2024 14:22:55 -0800 Subject: [PATCH 29/53] Add OverPlate configuration to initial positions --- ada_moveit/config/initial_positions.yaml | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ada_moveit/config/initial_positions.yaml b/ada_moveit/config/initial_positions.yaml index 030bfb5..c22fcc1 100644 --- a/ada_moveit/config/initial_positions.yaml +++ b/ada_moveit/config/initial_positions.yaml @@ -22,3 +22,15 @@ initial_positions: j2n6s200_joint_finger_2: 1.317 af_joint_1: 0.0 af_joint_2: 0.0 + + # # OverPlate Position + # j2n6s200_joint_1: -2.3149168248766614 # j2n6s200_joint_1 + # j2n6s200_joint_2: 3.1444595465032634 # j2n6s200_joint_2 + # j2n6s200_joint_3: 1.7332586075115999 # j2n6s200_joint_3 + # j2n6s200_joint_4: -2.3609596843308234 # j2n6s200_joint_4 + # j2n6s200_joint_5: 4.43936623280362 # j2n6s200_joint_5 + # j2n6s200_joint_6: 3.06866544924739 # j2n6s200_joint_6 + # j2n6s200_joint_finger_1: 1.317 + # j2n6s200_joint_finger_2: 1.317 + # af_joint_1: 0.0 + # af_joint_2: 0.0 From a8aea27cfc88b55c42434698ebf5f0c469e22fd9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 2 Dec 2024 10:06:23 -0800 Subject: [PATCH 30/53] Add frame_id argument to ada_twist_teleop --- ada_moveit/scripts/ada_twist_teleop.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/ada_moveit/scripts/ada_twist_teleop.py b/ada_moveit/scripts/ada_twist_teleop.py index ddb50fb..b4b8109 100755 --- a/ada_moveit/scripts/ada_twist_teleop.py +++ b/ada_moveit/scripts/ada_twist_teleop.py @@ -41,6 +41,7 @@ def parse_args(): help='Linear velocity in x, y, z directions (m/s)') parser.add_argument('--duration', type=float, default=DEFAULT_DURATION_S, help='Duration of the motion (seconds)') + parser.add_argument('--frame_id', type=str, default=BASE_FRAME, choices=[BASE_FRAME, EE_FRAME], help='Frame ID for the twist message') return parser.parse_args() def publish_zero_twist(node, twist_pub): @@ -68,12 +69,19 @@ def main(args=None): tf_buffer = Buffer() tf_listener = TransformListener(tf_buffer, node) # pylint: disable=unused-variable + # Parse command-line arguments + args = parse_args() + frame_id = args.frame_id + angular = args.angular + linear = args.linear + duration_s = args.duration + print(f"angular (rad/s) = {angular}, linear (m/s) = {linear}, duration (s) = {duration_s}") + # Create the cartesian control messages # The linear velocity is always in the base frame linear_msg = Vector3Stamped() linear_msg.header.stamp = Time().to_msg() # use latest time - # linear_msg.header.frame_id = BASE_FRAME - linear_msg.header.frame_id = EE_FRAME + linear_msg.header.frame_id = frame_id # The angular velocity is always in the end effector frame angular_msg = Vector3Stamped() angular_msg.header.stamp = Time().to_msg() # use latest time @@ -83,13 +91,6 @@ def main(args=None): twist_msg = TwistStamped() twist_msg.header.frame_id = BASE_FRAME - # Parse command-line arguments - args = parse_args() - angular = args.angular - linear = args.linear - duration_s = args.duration - print(f"angular (rad/s) = {angular}, linear (m/s) = {linear}, duration (s) = {duration_s}") - # Delay for transforms to populate start_time_s = time.time() while time.time() - start_time_s < WAIT_FOR_TF_S: From ebc95f376092b3251b104f73aad65a36a729f455 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 6 Dec 2024 17:25:46 -0800 Subject: [PATCH 31/53] TEMP: Set default velocity scaling factor to 1.0 --- ada_moveit/config/joint_limits.yaml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/joint_limits.yaml b/ada_moveit/config/joint_limits.yaml index 3275f07..7bb4e23 100644 --- a/ada_moveit/config/joint_limits.yaml +++ b/ada_moveit/config/joint_limits.yaml @@ -2,8 +2,10 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +# default_velocity_scaling_factor: 0.1 +# default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 1.0 +default_acceleration_scaling_factor: 1.0 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] From a402eec7acb91401a2fc557fd6603d82b77c8cab Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 6 Dec 2024 17:41:10 -0800 Subject: [PATCH 32/53] Remove jaco_af controllers, add af controllers --- ada_moveit/config/mock_controllers.yaml | 50 ++++--------------------- ada_moveit/config/mock_servo.yaml | 6 +-- ada_moveit/config/real_controllers.yaml | 50 ++++--------------------- ada_moveit/config/real_servo.yaml | 6 +-- 4 files changed, 20 insertions(+), 92 deletions(-) diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 57e090d..b63c799 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -23,15 +23,15 @@ controller_manager: type: cartesian_twist_controller/CartesianTwistController - jaco_af_controller: + af_controller: type: force_gate_controller/ForceGateController - jaco_af_servo_controller: + af_servo_controller: type: force_gate_controller/ForceGatePositionController - jaco_af_cartesian_controller: + af_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController jaco_arm_cartesian_controller: @@ -95,15 +95,9 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 -jaco_af_controller: +af_controller: ros__parameters: joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 command_interfaces: @@ -114,24 +108,6 @@ jaco_af_controller: open_loop_control: false stopped_velocity_tolerance: 0.01 # No gains on position interface - j2n6s200_joint_1: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_2: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_3: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_4: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_5: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_6: - goal: 0.02 - trajectory: 0.05 af_joint_1: goal: 0.02 trajectory: 0.05 @@ -139,30 +115,18 @@ jaco_af_controller: goal: 0.02 trajectory: 0.05 -jaco_af_servo_controller: +af_servo_controller: ros__parameters: joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 -jaco_af_cartesian_controller: +af_cartesian_controller: ros__parameters: end_effector_link: "forkTip" - robot_base_link: "j2n6s200_link_base" + robot_base_link: "af_link_base" ik_solver: "selectively_damped_least_squares" joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 command_interfaces: diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index dd3219a..24e5451 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -43,11 +43,11 @@ servo_node: monitored_planning_scene_topic: "monitored_planning_scene" ## MoveIt properties - move_group_name: jaco_af + move_group_name: jaco_arm planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames - ee_frame_name: forkTip + ee_frame_name: j2n6s200_end_effector 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 @@ -67,7 +67,7 @@ servo_node: joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_af_servo_controller/commands + command_out_topic: /jaco_servo_controller/commands ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index 9e45d59..7a3a277 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -23,15 +23,15 @@ controller_manager: type: cartesian_twist_controller/CartesianTwistController - jaco_af_controller: + af_controller: type: force_gate_controller/ForceGateController - jaco_af_servo_controller: + af_servo_controller: type: force_gate_controller/ForceGatePositionController - jaco_af_cartesian_controller: + af_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController jaco_arm_cartesian_controller: @@ -111,15 +111,9 @@ jaco_arm_servo_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 -jaco_af_controller: +af_controller: ros__parameters: joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 command_interfaces: @@ -133,24 +127,6 @@ jaco_af_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 # No gains on position interface - j2n6s200_joint_1: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_2: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_3: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_4: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_5: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_6: - goal: 0.02 - trajectory: 0.05 af_joint_1: goal: 0.02 trajectory: 0.05 @@ -158,33 +134,21 @@ jaco_af_controller: goal: 0.02 trajectory: 0.05 -jaco_af_servo_controller: +af_servo_controller: ros__parameters: joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 wrench_threshold: topic: /wireless_ft/ftSensor3 fMag: 1.0 -jaco_af_cartesian_controller: +af_cartesian_controller: ros__parameters: end_effector_link: "forkTip" - robot_base_link: "j2n6s200_link_base" + robot_base_link: "af_link_base" ik_solver: "selectively_damped_least_squares" joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 command_interfaces: diff --git a/ada_moveit/config/real_servo.yaml b/ada_moveit/config/real_servo.yaml index 817bf4e..d57b73c 100644 --- a/ada_moveit/config/real_servo.yaml +++ b/ada_moveit/config/real_servo.yaml @@ -43,11 +43,11 @@ servo_node: monitored_planning_scene_topic: "monitored_planning_scene" ## MoveIt properties - move_group_name: jaco_af # Often 'manipulator' or 'arm' + 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 @@ -67,7 +67,7 @@ servo_node: joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_af_servo_controller/commands # Publish outgoing commands here + command_out_topic: /jaco_servo_controller/commands # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? From 2fda53714f7e02e7666eabf7fd14ef1b60a7f53c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 6 Dec 2024 17:41:47 -0800 Subject: [PATCH 33/53] Update MoveIt controllers to use af controllers --- ada_moveit/config/moveit_controllers.yaml | 30 +++++------------------ 1 file changed, 6 insertions(+), 24 deletions(-) diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 2eb6ab0..e906167 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -7,9 +7,9 @@ moveit_simple_controller_manager: - jaco_arm_controller - jaco_arm_cartesian_controller - jaco_arm_servo_controller - - jaco_af_controller - - jaco_af_cartesian_controller - - jaco_af_servo_controller + - af_controller + - af_cartesian_controller + - af_servo_controller - hand_controller jaco_arm_controller: @@ -48,45 +48,27 @@ moveit_simple_controller_manager: - j2n6s200_joint_5 - j2n6s200_joint_6 - jaco_af_controller: + af_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 - jaco_af_servo_controller: + af_servo_controller: type: "" action_ns: commands default: false joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 - jaco_af_cartesian_controller: + af_cartesian_controller: type: "" action_ns: cart_commands default: false joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - af_joint_1 - af_joint_2 From 0e5f8bc6e4c7bbfa2fb55a56e7500d183e3e178b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 6 Dec 2024 17:43:04 -0800 Subject: [PATCH 34/53] Update OMPL planners to use articulable_fork move group --- ada_moveit/config/ompl_planning.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index e12cce8..ce787bd 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -76,9 +76,9 @@ jaco_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault -jaco_af: +articulable_fork: enforce_constrained_state_space: true - projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) + projection_evaluator: joints(af_joint_1,af_joint_2) planner_configs: - SBLkConfigDefault - ESTkConfigDefault From adcf2a297beed614a0abb279dbc56475addcd29d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 7 Dec 2024 10:37:58 -0800 Subject: [PATCH 35/53] Remove af_cartesian_controller --- ada_moveit/config/mock_controllers.yaml | 15 --------------- ada_moveit/config/moveit_controllers.yaml | 9 --------- ada_moveit/config/real_controllers.yaml | 18 ------------------ 3 files changed, 42 deletions(-) diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index b63c799..e673532 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -30,10 +30,6 @@ controller_manager: af_servo_controller: type: force_gate_controller/ForceGatePositionController - - af_cartesian_controller: - type: cartesian_twist_controller/CartesianTwistController - jaco_arm_cartesian_controller: ros__parameters: end_effector_link: "j2n6s200_end_effector" @@ -121,17 +117,6 @@ af_servo_controller: - af_joint_1 - af_joint_2 -af_cartesian_controller: - ros__parameters: - end_effector_link: "forkTip" - robot_base_link: "af_link_base" - ik_solver: "selectively_damped_least_squares" - joints: - - af_joint_1 - - af_joint_2 - command_interfaces: - - position - hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index e906167..c9cc696 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -8,7 +8,6 @@ moveit_simple_controller_manager: - jaco_arm_cartesian_controller - jaco_arm_servo_controller - af_controller - - af_cartesian_controller - af_servo_controller - hand_controller @@ -64,14 +63,6 @@ moveit_simple_controller_manager: - af_joint_1 - af_joint_2 - af_cartesian_controller: - type: "" - action_ns: cart_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/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index 7a3a277..c20bb38 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -30,10 +30,6 @@ controller_manager: af_servo_controller: type: force_gate_controller/ForceGatePositionController - - af_cartesian_controller: - type: cartesian_twist_controller/CartesianTwistController - jaco_arm_cartesian_controller: ros__parameters: end_effector_link: "j2n6s200_end_effector" @@ -143,20 +139,6 @@ af_servo_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 -af_cartesian_controller: - ros__parameters: - end_effector_link: "forkTip" - robot_base_link: "af_link_base" - ik_solver: "selectively_damped_least_squares" - joints: - - af_joint_1 - - af_joint_2 - command_interfaces: - - position - wrench_threshold: - topic: /wireless_ft/ftSensor3 - fMag: 1.0 - hand_controller: ros__parameters: joints: From 197dfcc6df754b3c7a0d04908fcd939c00124af2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 8 Dec 2024 18:25:36 -0800 Subject: [PATCH 36/53] Revert to using jaco_arm_cartesian_controller for keyboard tele-op --- ada_moveit/scripts/ada_keyboard_teleop.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_moveit/scripts/ada_keyboard_teleop.py b/ada_moveit/scripts/ada_keyboard_teleop.py index cc219c9..4df2489 100755 --- a/ada_moveit/scripts/ada_keyboard_teleop.py +++ b/ada_moveit/scripts/ada_keyboard_teleop.py @@ -111,7 +111,7 @@ def main(args=None): rclpy.init(args=args) node = rclpy.create_node("ada_keyboard_teleop") twist_pub = node.create_publisher( - TwistStamped, "/jaco_af_cartesian_controller/twist_cmd", 1 + TwistStamped, "/jaco_arm_cartesian_controller/twist_cmd", 1 ) joint_pub = node.create_publisher(JointJog, "/servo_node/delta_joint_cmds", 1) From 80c710f54d804aa7a79cfbe6b4e2af3ef52537bd Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 8 Dec 2024 18:26:51 -0800 Subject: [PATCH 37/53] Fix typo in jaco_arm_servo_controller --- ada_moveit/config/mock_servo.yaml | 2 +- ada_moveit/config/real_servo.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index 24e5451..939325f 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -67,7 +67,7 @@ servo_node: joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_servo_controller/commands + command_out_topic: /jaco_arm_servo_controller/commands ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/ada_moveit/config/real_servo.yaml b/ada_moveit/config/real_servo.yaml index d57b73c..dfe7df4 100644 --- a/ada_moveit/config/real_servo.yaml +++ b/ada_moveit/config/real_servo.yaml @@ -67,7 +67,7 @@ servo_node: joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_servo_controller/commands # Publish outgoing commands here + command_out_topic: /jaco_arm_servo_controller/commands # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? From 477a47dbb7a572c8fb0ba83dfd16bd60d5e2f758 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 9 Dec 2024 09:37:15 -0800 Subject: [PATCH 38/53] Remove articulable_fork.launch.py --- ada_moveit/launch/articulable_fork.launch.py | 242 ------------------- 1 file changed, 242 deletions(-) delete mode 100644 ada_moveit/launch/articulable_fork.launch.py diff --git a/ada_moveit/launch/articulable_fork.launch.py b/ada_moveit/launch/articulable_fork.launch.py deleted file mode 100644 index 8b99bbc..0000000 --- a/ada_moveit/launch/articulable_fork.launch.py +++ /dev/null @@ -1,242 +0,0 @@ -# Copyright 2020 Yutaka Kondo -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import yaml -import xacro - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launch_utils import ( - add_debuggable_node, - DeclareBooleanLaunchArg, -) -from moveit_configs_utils.launches import generate_demo_launch -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -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 - - -def generate_launch_description(): - - # MoveIt Config - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() - - # Sim Launch Argument - sim_da = DeclareLaunchArgument( - "sim", - default_value="real", - description="Which sim to use: 'mock', 'isaac', or 'real'", - ) - sim = LaunchConfiguration("sim") - - ctrl_da = DeclareLaunchArgument( - "controllers_file", - default_value=[sim, "_controllers.yaml"], - description="ROS2 Controller YAML configuration in config folder", - ) - controllers_file = LaunchConfiguration("controllers_file") - - # Log Level - log_level_da = DeclareLaunchArgument( - "log_level", - default_value="info", - description="Logging level (debug, info, warn, error, fatal)", - ) - log_level = LaunchConfiguration("log_level") - - # Copy from generate_demo_launch - ld = LaunchDescription() - ld.add_action(sim_da) - ld.add_action(ctrl_da) - ld.add_action(log_level_da) - - ld.add_action( - DeclareBooleanLaunchArg( - "debug", - default_value=False, - description="By default, we are not in debug mode", - ) - ) - ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True)) - - # If there are virtual joints, broadcast static tf by including virtual_joints launch - virtual_joints_launch = ( - moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" - ) - if virtual_joints_launch.exists(): - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource(str(virtual_joints_launch)), - launch_arguments={ - "log_level": log_level, - }.items(), - ) - ) - - # Given the published joint states, publish tf for the robot links - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/rsp.launch.py") - ), - launch_arguments={ - "log_level": log_level, - }.items(), - ) - ) - - # Launch the Move Group - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/move_group.launch.py") - ), - launch_arguments={ - "sim": sim, - "log_level": log_level, - }.items(), - ) - ) - - # Run Rviz and load the default config to see the state of the move_group node - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/moveit_rviz.launch.py") - ), - launch_arguments={ - "log_level": log_level, - }.items(), - condition=IfCondition(LaunchConfiguration("use_rviz")), - ) - ) - - # 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) - } - - robot_controllers = PathJoinSubstitution( - [str(moveit_config.package_path), "config", controllers_file] - ) - - # Joint Controllers - ld.add_action( - Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, robot_controllers], - arguments=["--ros-args", "--log-level", log_level], - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/spawn_controllers.launch.py") - ), - launch_arguments={ - "log_level": log_level, - }.items(), - ) - ) - - return ld - - # OLD CODE STARTS HERE - - # robot_name = "ada" - # package_name = robot_name + "_moveit" - # robot_description = os.path.join(get_package_share_directory( - # package_name), "config", robot_name + ".urdf.xacro") - # robot_description_config = xacro.process_file(robot_description) - # - # controller_config = os.path.join( - # get_package_share_directory( - # package_name), "config", "real_controllers.yaml" - # ) - # - # return LaunchDescription([ - # Node( - # package="controller_manager", - # executable="ros2_control_node", - # parameters=[ - # {"robot_description": robot_description_config.toxml()}, controller_config], - # output="screen", - # ), - # - # Node( - # package="controller_manager", - # executable="spawner", - # arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - # output="screen", - # ), - # - # Node( - # package="controller_manager", - # executable="spawner", - # arguments=["af_velocity_controller", "-c", "/controller_manager"], - # output="screen", - # ), - # - # Node( - # package="controller_manager", - # executable="spawner", - # arguments=["af_joint_trajectory_controller", "-c", "/controller_manager"], - # output="screen", - # ), - # - # Node( - # package="robot_state_publisher", - # executable="robot_state_publisher", - # name="robot_state_publisher", - # parameters=[ - # {"robot_description": robot_description_config.toxml()}], - # output="screen", - # ), - # - # ]) - From 4d3b0486a8e71062a831b2bc6fb73309f862c02f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 12 Dec 2024 15:39:24 -0800 Subject: [PATCH 39/53] Cleanup --- ada_moveit/CMakeLists.txt | 1 - ada_moveit/config/ada.ros2_control.xacro | 11 -- ada_moveit/config/ada.srdf | 14 -- ada_moveit/config/hybrid_controllers.yaml | 186 ---------------------- ada_moveit/config/hybrid_servo.yaml | 76 --------- ada_moveit/config/initial_positions.yaml | 12 -- ada_moveit/config/joint_limits.yaml | 6 +- ada_moveit/config/kinematics.yaml | 1 - ada_moveit/config/mock_servo.yaml | 7 +- ada_moveit/scripts/ada_keyboard_teleop.py | 2 - ada_moveit/scripts/ada_twist_teleop.py | 147 ----------------- ada_moveit/scripts/servo_republisher.py | 2 - 12 files changed, 6 insertions(+), 459 deletions(-) delete mode 100644 ada_moveit/config/hybrid_controllers.yaml delete mode 100644 ada_moveit/config/hybrid_servo.yaml delete mode 100755 ada_moveit/scripts/ada_twist_teleop.py diff --git a/ada_moveit/CMakeLists.txt b/ada_moveit/CMakeLists.txt index 7231d69..3037056 100644 --- a/ada_moveit/CMakeLists.txt +++ b/ada_moveit/CMakeLists.txt @@ -8,7 +8,6 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS - scripts/ada_twist_teleop.py scripts/ada_keyboard_teleop.py scripts/servo_republisher.py DESTINATION lib/${PROJECT_NAME} diff --git a/ada_moveit/config/ada.ros2_control.xacro b/ada_moveit/config/ada.ros2_control.xacro index 16855ff..99b0d30 100644 --- a/ada_moveit/config/ada.ros2_control.xacro +++ b/ada_moveit/config/ada.ros2_control.xacro @@ -17,11 +17,6 @@ ada_hardware/Jaco2 - - mock_components/GenericSystem - false - 0.0 - @@ -154,11 +149,6 @@ /dev/ttyUSB0 1000000 - - dynamixel_hardware/DynamixelHardware - /dev/ttyUSB0 - 1000000 - @@ -192,5 +182,4 @@ - diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index 68e8c73..49aa813 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -48,10 +48,6 @@ - - - - @@ -84,16 +80,6 @@ - - - - - - - - - - diff --git a/ada_moveit/config/hybrid_controllers.yaml b/ada_moveit/config/hybrid_controllers.yaml deleted file mode 100644 index 57e090d..0000000 --- a/ada_moveit/config/hybrid_controllers.yaml +++ /dev/null @@ -1,186 +0,0 @@ -# This config file is used by ros2_control -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - jaco_arm_controller: - type: force_gate_controller/ForceGateController - - - jaco_arm_servo_controller: - type: force_gate_controller/ForceGatePositionController - - - hand_controller: - type: joint_trajectory_controller/JointTrajectoryController - - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - - jaco_arm_cartesian_controller: - type: cartesian_twist_controller/CartesianTwistController - - - jaco_af_controller: - type: force_gate_controller/ForceGateController - - - jaco_af_servo_controller: - type: force_gate_controller/ForceGatePositionController - - - jaco_af_cartesian_controller: - type: cartesian_twist_controller/CartesianTwistController - -jaco_arm_cartesian_controller: - ros__parameters: - end_effector_link: "j2n6s200_end_effector" - robot_base_link: "j2n6s200_link_base" - ik_solver: "selectively_damped_least_squares" - joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - command_interfaces: - - position - -jaco_arm_controller: - ros__parameters: - joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - 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 - j2n6s200_joint_1: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_2: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_3: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_4: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_5: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_6: - goal: 0.02 - trajectory: 0.05 - -jaco_arm_servo_controller: - ros__parameters: - joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - -jaco_af_controller: - ros__parameters: - joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - - 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 - j2n6s200_joint_1: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_2: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_3: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_4: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_5: - goal: 0.02 - trajectory: 0.05 - j2n6s200_joint_6: - goal: 0.02 - trajectory: 0.05 - af_joint_1: - goal: 0.02 - trajectory: 0.05 - af_joint_2: - goal: 0.02 - trajectory: 0.05 - -jaco_af_servo_controller: - ros__parameters: - joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - - af_joint_1 - - af_joint_2 - -jaco_af_cartesian_controller: - ros__parameters: - end_effector_link: "forkTip" - robot_base_link: "j2n6s200_link_base" - ik_solver: "selectively_damped_least_squares" - joints: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 - - af_joint_1 - - af_joint_2 - command_interfaces: - - position - -hand_controller: - ros__parameters: - joints: - - j2n6s200_joint_finger_1 - - j2n6s200_joint_finger_2 - command_interfaces: - - position - state_interfaces: - - position - allow_partial_joints_goal: false - open_loop_control: false - constraints: - stopped_velocity_tolerance: 0.01 - gains: # Required because we're controlling a velocity interface - j2n6s200_joint_finger_1: {p: 1.0, d: 0.0, i: 0.0, i_clamp: 1.0} - j2n6s200_joint_finger_2: {p: 1.0, d: 0.0, i: 0.0, i_clamp: 1.0} diff --git a/ada_moveit/config/hybrid_servo.yaml b/ada_moveit/config/hybrid_servo.yaml deleted file mode 100644 index dd3219a..0000000 --- a/ada_moveit/config/hybrid_servo.yaml +++ /dev/null @@ -1,76 +0,0 @@ -# Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml -############################################### -# Modify all parameters related to servoing here -############################################### -servo_node: - ros__parameters: - moveit_servo: - use_gazebo: false # Whether the robot is started in a Gazebo simulation environment - - ## Properties of incoming commands - command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s - scale: - # Scale parameters are only used if command_in_type=="unitless" - linear: 0.1 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. - joint: 0.5 - - # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) - # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] - - ## Properties of outgoing commands - publish_period: 0.034 # 1/Nominal publish rate [seconds] - low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) - - # What type of topic does your robot driver expect? - # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory - command_out_type: std_msgs/Float64MultiArray - - # What to publish? Can save some bandwidth as most robots only require positions or velocities - publish_joint_positions: true - publish_joint_velocities: false - publish_joint_accelerations: false - - ## Plugins for smoothing outgoing commands - smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" - - # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, - # which other nodes can use as a source for information about the planning environment. - # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), - # then is_primary_planning_scene_monitor needs to be set to false. - is_primary_planning_scene_monitor: false - monitored_planning_scene_topic: "monitored_planning_scene" - - ## MoveIt properties - move_group_name: jaco_af - planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' - - ## Other frames - ee_frame_name: forkTip - 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 - incoming_command_timeout: 0.33 # Stop servoing if X seconds elapse without a new command - # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. - # Important because ROS may drop some messages and we need the robot to halt reliably. - num_outgoing_halt_msgs_to_publish: 0 - - ## Configure handling of singularities and joint limits - lower_singularity_threshold: 200.0 # Start decelerating when the condition number hits this (close to singularity) - hard_stop_singularity_threshold: 400.0 # Stop when the condition number hits this - joint_limit_margin: 0.2 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. - leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) - - ## Topic names - cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands - joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands - joint_topic: /joint_states - status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_af_servo_controller/commands - - ## Collision checking for the entire robot body - check_collisions: true # Check collisions? - collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. - self_collision_proximity_threshold: 0.001 # Start decelerating when a self-collision is this far [m] - scene_collision_proximity_threshold: 0.0075 # Start decelerating when a scene collision is this far [m] diff --git a/ada_moveit/config/initial_positions.yaml b/ada_moveit/config/initial_positions.yaml index c22fcc1..030bfb5 100644 --- a/ada_moveit/config/initial_positions.yaml +++ b/ada_moveit/config/initial_positions.yaml @@ -22,15 +22,3 @@ initial_positions: j2n6s200_joint_finger_2: 1.317 af_joint_1: 0.0 af_joint_2: 0.0 - - # # OverPlate Position - # j2n6s200_joint_1: -2.3149168248766614 # j2n6s200_joint_1 - # j2n6s200_joint_2: 3.1444595465032634 # j2n6s200_joint_2 - # j2n6s200_joint_3: 1.7332586075115999 # j2n6s200_joint_3 - # j2n6s200_joint_4: -2.3609596843308234 # j2n6s200_joint_4 - # j2n6s200_joint_5: 4.43936623280362 # j2n6s200_joint_5 - # j2n6s200_joint_6: 3.06866544924739 # j2n6s200_joint_6 - # 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 7bb4e23..3275f07 100644 --- a/ada_moveit/config/joint_limits.yaml +++ b/ada_moveit/config/joint_limits.yaml @@ -2,10 +2,8 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -# default_velocity_scaling_factor: 0.1 -# default_acceleration_scaling_factor: 0.1 -default_velocity_scaling_factor: 1.0 -default_acceleration_scaling_factor: 1.0 +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index 092b853..887b13d 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -40,4 +40,3 @@ articulable_fork: cost_threshold: 0.001 minimal_displacement_weight: 0.0 gd_step_size: 0.0001 - diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index 939325f..68b02d7 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -43,11 +43,12 @@ servo_node: monitored_planning_scene_topic: "monitored_planning_scene" ## MoveIt properties - move_group_name: jaco_arm + 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: j2n6s200_end_effector + 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 @@ -67,7 +68,7 @@ servo_node: joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic - command_out_topic: /jaco_arm_servo_controller/commands + command_out_topic: /jaco_arm_servo_controller/commands # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/ada_moveit/scripts/ada_keyboard_teleop.py b/ada_moveit/scripts/ada_keyboard_teleop.py index 4df2489..7f5e12f 100755 --- a/ada_moveit/scripts/ada_keyboard_teleop.py +++ b/ada_moveit/scripts/ada_keyboard_teleop.py @@ -90,8 +90,6 @@ def get_key(settings): "4": "j2n6s200_joint_4", "5": "j2n6s200_joint_5", "6": "j2n6s200_joint_6", - "7": "af_joint_1", - "8": "af_joint_2", } reverse_joint_direction_key = "r" # pylint: disable=invalid-name diff --git a/ada_moveit/scripts/ada_twist_teleop.py b/ada_moveit/scripts/ada_twist_teleop.py deleted file mode 100755 index b4b8109..0000000 --- a/ada_moveit/scripts/ada_twist_teleop.py +++ /dev/null @@ -1,147 +0,0 @@ -#!/usr/bin/env python3 - -import time -import yaml -import numpy as np -import argparse - -from control_msgs.msg import JointJog -from geometry_msgs.msg import TwistStamped -import rclpy -from rclpy.time import Time -from rclpy.duration import Duration -from tf2_geometry_msgs import Vector3Stamped # pylint: disable=unused-import -import tf2_py as tf2 -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener -from sensor_msgs.msg import JointState - -BASE_FRAME = "j2n6s200_link_base" -EE_FRAME = "forkTip" -JOINT_NAMES = [ - 'j2n6s200_joint_1', - 'j2n6s200_joint_2', - 'j2n6s200_joint_3', - 'j2n6s200_joint_4', - 'j2n6s200_joint_5', - 'j2n6s200_joint_6', - 'af_joint_1', - 'af_joint_2', -] -DEFAULT_ANGULAR = [0.0, 0.0, 0.0] -DEFAULT_LINEAR = [0.0, 0.0, 0.0] -DEFAULT_DURATION_S = 1.0 -WAIT_FOR_TF_S = 1.0 - -def parse_args(): - parser = argparse.ArgumentParser(description='Control robot arm with specified angular and linear velocities.') - parser.add_argument('--angular', nargs=3, type=float, default=DEFAULT_ANGULAR, - help='Angular velocity in x, y, z directions (rad/s)') - parser.add_argument('--linear', nargs=3, type=float, default=DEFAULT_LINEAR, - help='Linear velocity in x, y, z directions (m/s)') - parser.add_argument('--duration', type=float, default=DEFAULT_DURATION_S, - help='Duration of the motion (seconds)') - parser.add_argument('--frame_id', type=str, default=BASE_FRAME, choices=[BASE_FRAME, EE_FRAME], help='Frame ID for the twist message') - return parser.parse_args() - -def publish_zero_twist(node, twist_pub): - twist_msg = TwistStamped() - twist_msg.header.stamp = node.get_clock().now().to_msg() - twist_msg.header.frame_id = BASE_FRAME - twist_msg.twist.linear.x = 0.0 - twist_msg.twist.linear.y = 0.0 - twist_msg.twist.linear.z = 0.0 - twist_msg.twist.angular.x = 0.0 - twist_msg.twist.angular.y = 0.0 - twist_msg.twist.angular.z = 0.0 - - twist_pub.publish(twist_msg) - -def main(args=None): - # Initialize ROS context - rclpy.init(args=args) - node = rclpy.create_node("ada_twist_teleop") - twist_pub = node.create_publisher( - TwistStamped, "/jaco_af_cartesian_controller/twist_cmd", 1 - ) - - # Initialize the tf2 buffer and listener - tf_buffer = Buffer() - tf_listener = TransformListener(tf_buffer, node) # pylint: disable=unused-variable - - # Parse command-line arguments - args = parse_args() - frame_id = args.frame_id - angular = args.angular - linear = args.linear - duration_s = args.duration - print(f"angular (rad/s) = {angular}, linear (m/s) = {linear}, duration (s) = {duration_s}") - - # Create the cartesian control messages - # The linear velocity is always in the base frame - linear_msg = Vector3Stamped() - linear_msg.header.stamp = Time().to_msg() # use latest time - linear_msg.header.frame_id = frame_id - # The angular velocity is always in the end effector frame - angular_msg = Vector3Stamped() - angular_msg.header.stamp = Time().to_msg() # use latest time - angular_msg.header.frame_id = EE_FRAME - # The final message should be either in the base or end effector frame. - # It should match the `robot_link_command_frame`` servo param. - twist_msg = TwistStamped() - twist_msg.header.frame_id = BASE_FRAME - - # Delay for transforms to populate - start_time_s = time.time() - while time.time() - start_time_s < WAIT_FOR_TF_S: - rclpy.spin_once(node, timeout_sec=0) - - start_time_s = time.time() - while time.time() - start_time_s < duration_s: - rclpy.spin_once(node, timeout_sec=0) - - linear_msg.vector.x = linear[0] - linear_msg.vector.y = linear[1] - linear_msg.vector.z = linear[2] - twist_msg.twist.linear = linear_msg.vector - if linear_msg.header.frame_id != twist_msg.header.frame_id: - try: - linear_transformed = tf_buffer.transform( - linear_msg, twist_msg.header.frame_id - ) - twist_msg.twist.linear = linear_transformed.vector - except tf2.ExtrapolationException as exc: - node.get_logger().warning( - f"Transform from {linear_msg.header.frame_id} to " - f"{twist_msg.header.frame_id} failed: {type(exc)}: {exc}\n" - f"Interpreting the linear velocity in {twist_msg.header.frame_id} " - "without transforming." - ) - - angular_msg.vector.x = angular[0] - angular_msg.vector.y = angular[1] - angular_msg.vector.z = angular[2] - twist_msg.twist.angular = angular_msg.vector - if angular_msg.header.frame_id != twist_msg.header.frame_id: - try: - angular_transformed = tf_buffer.transform( - angular_msg, twist_msg.header.frame_id - ) - twist_msg.twist.angular = angular_transformed.vector - except tf2.ExtrapolationException as exc: - node.get_logger().warning( - f"Transform from {angular_msg.header.frame_id} to " - f"{twist_msg.header.frame_id} failed: {type(exc)}: {exc}\n" - f"Interpreting the angular velocity in {twist_msg.header.frame_id}" - " without transforming." - ) - - twist_msg.header.stamp = node.get_clock().now().to_msg() - twist_pub.publish(twist_msg) - publish_zero_twist(node, twist_pub) - - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/ada_moveit/scripts/servo_republisher.py b/ada_moveit/scripts/servo_republisher.py index 1d7ed85..cf94783 100755 --- a/ada_moveit/scripts/servo_republisher.py +++ b/ada_moveit/scripts/servo_republisher.py @@ -43,8 +43,6 @@ "j2n6s200_joint_4", "j2n6s200_joint_5", "j2n6s200_joint_6", - "af_joint_1", - "af_joint_2", ] POSITION_INTERFACE_NAME = "position" VELOCITY_INTERFACE_NAME = "velocity" From 062356b15dff831877464ebf2a34c4b4a1c9c7fe Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 13 Dec 2024 12:33:21 -0800 Subject: [PATCH 40/53] Add configuration for end_effector_tool --- ada_moveit/config/ada.urdf.xacro | 12 +++++++++--- ada_moveit/launch/demo.launch.py | 11 +++++++++++ 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index d6265a4..cc67286 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -3,6 +3,10 @@ + + + + @@ -14,8 +18,10 @@ initial_positions_file="$(arg initial_positions_file)" sim="$(arg sim)" /> - + + + diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 4360371..0b7ba2a 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -48,6 +48,13 @@ def generate_launch_description(): ) sim = LaunchConfiguration("sim") + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: none, fork, articulable_fork", + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ctrl_da = DeclareLaunchArgument( "controllers_file", default_value=[sim, "_controllers.yaml"], @@ -74,6 +81,7 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(calib_da) ld.add_action(sim_da) + ld.add_action(eet_da) ld.add_action(ctrl_da) ld.add_action(servo_da) ld.add_action(log_level_da) @@ -208,6 +216,9 @@ def generate_launch_description(): " ", "sim:=", sim, + " ", + "end_effector_tool:=", + end_effector_tool, ] ) robot_description = { From ed730e036b4d739c87c639c7f853aa1769ae8b4d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 13 Dec 2024 22:35:47 -0800 Subject: [PATCH 41/53] Use updated ada macro --- ada_moveit/config/ada.urdf.xacro | 12 +++++++----- ada_moveit/launch/demo.launch.py | 3 ++- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index cc67286..86e3345 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -2,13 +2,15 @@ - - - - + + + + + + @@ -18,7 +20,7 @@ initial_positions_file="$(arg initial_positions_file)" sim="$(arg sim)" /> - + diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 0b7ba2a..daca0f2 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -51,7 +51,8 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: none, fork, articulable_fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=['none', 'fork', 'articulable_fork'] ) end_effector_tool = LaunchConfiguration("end_effector_tool") From b6821e20aa82753c57a8c76f1d32b080369c1d74 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 14 Dec 2024 19:29:03 -0800 Subject: [PATCH 42/53] Update launch files to enable end_effector_tool parameterization --- ada_moveit/launch/demo.launch.py | 49 +++++++++---------- ada_moveit/launch/move_group.launch.py | 16 ++++-- ada_moveit/launch/moveit_rviz.launch.py | 30 ++++++++++-- ada_moveit/launch/rsp.launch.py | 30 ++++++++++-- ada_moveit/launch/spawn_controllers.launch.py | 30 ++++++++++-- .../launch/static_virtual_joint_tfs.launch.py | 30 ++++++++++-- ada_moveit/launch/warehouse_db.launch.py | 30 ++++++++++-- 7 files changed, 170 insertions(+), 45 deletions(-) diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index daca0f2..f477405 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -17,6 +17,7 @@ from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare from srdfdom.srdf import SRDF @@ -27,11 +28,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", @@ -48,6 +44,7 @@ def generate_launch_description(): ) sim = LaunchConfiguration("sim") + # End-effector Tool Launch Argument eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", @@ -87,6 +84,13 @@ def generate_launch_description(): 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} + ) + 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)) @@ -150,7 +154,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(), ) ) @@ -162,7 +168,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(), ) ) @@ -176,6 +184,7 @@ def generate_launch_description(): launch_arguments={ "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) @@ -187,7 +196,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")), ) @@ -200,32 +211,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, - " ", - "end_effector_tool:=", - end_effector_tool, - ] - ) - 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] @@ -237,7 +230,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 ], @@ -256,7 +249,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], ) ) @@ -267,7 +260,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 e417266..5a8bfc8 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -17,11 +17,14 @@ def get_move_group_launch(context): """ sim = LaunchConfiguration("sim").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} + ) + moveit_config = builder.to_moveit_configs() # If sim is mock, set moveit_config.sensors_3d to an empty dictionary if sim == "mock": @@ -53,9 +56,16 @@ 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: 'none', 'fork', 'articulable_fork'", + choices=['none', 'fork', 'articulable_fork'] + ) ld = LaunchDescription() ld.add_action(sim_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 003213d..749936e 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -9,6 +9,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", @@ -19,9 +29,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: 'none', 'fork', 'articulable_fork'", + choices=['none', '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 38f9495..5e5e6de 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -9,6 +9,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", @@ -19,9 +29,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: 'none', 'fork', 'articulable_fork'", + choices=['none', '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 8812a82..fd231a0 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -9,6 +9,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", @@ -19,9 +29,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: 'none', 'fork', 'articulable_fork'", + choices=['none', '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 16e67bd..017cdaf 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -9,6 +9,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", @@ -19,9 +29,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: 'none', 'fork', 'articulable_fork'", + choices=['none', '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 7e9b378..48ab786 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -9,6 +9,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", @@ -19,9 +29,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: 'none', 'fork', 'articulable_fork'", + choices=['none', '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): From 0fae9395e430ed9dc1c46ae6edcda931e1ff853c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 15 Dec 2024 20:31:54 -0800 Subject: [PATCH 43/53] Rename jaco_af move group to ada --- ada_moveit/config/ada.srdf | 6 +++--- ada_moveit/config/kinematics.yaml | 2 +- ada_moveit/config/ompl_planning.yaml | 15 +++++++++++++++ 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index 49aa813..bb90982 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -24,7 +24,7 @@ - + @@ -60,7 +60,7 @@ - + @@ -70,7 +70,7 @@ - + diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index 887b13d..6a82700 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -13,7 +13,7 @@ jaco_arm: minimal_displacement_weight: 0.0 gd_step_size: 0.0001 -jaco_af: +ada: kinematics_solver: pick_ik/PickIkPlugin kinematics_solver_timeout: 0.05 kinematics_solver_attempts: 3 diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index ce787bd..66fac46 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -61,6 +61,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) From 69d0976b94d947acf485b03daf4b8910446f3727 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 18 Dec 2024 09:43:06 -0800 Subject: [PATCH 44/53] Disable collision between af_link_base and forkHandle --- ada_moveit/config/ada.srdf | 1 + 1 file changed, 1 insertion(+) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index 21b872c..0bbd235 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -297,6 +297,7 @@ + From ef1592b7678889fb3544664b151e89eec81dbe1f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 18 Dec 2024 09:44:06 -0800 Subject: [PATCH 45/53] Run pre-commit --- ada_moveit/config/ada.urdf.xacro | 2 +- ada_moveit/launch/demo.launch.py | 4 ++-- ada_moveit/launch/move_group.launch.py | 4 ++-- ada_moveit/launch/moveit_rviz.launch.py | 4 ++-- ada_moveit/launch/rsp.launch.py | 4 ++-- ada_moveit/launch/spawn_controllers.launch.py | 4 ++-- ada_moveit/launch/static_virtual_joint_tfs.launch.py | 4 ++-- ada_moveit/launch/warehouse_db.launch.py | 4 ++-- 8 files changed, 15 insertions(+), 15 deletions(-) diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index 8785c5c..3a324ef 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -5,7 +5,7 @@ - + diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index a88e754..be19184 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -55,7 +55,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") @@ -104,7 +104,7 @@ def generate_launch_description(): # Get MoveIt Configs builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") builder = builder.robot_description( - mappings={'sim': sim, 'end_effector_tool': end_effector_tool} + mappings={"sim": sim, "end_effector_tool": end_effector_tool} ) moveit_config = builder.to_moveit_configs() diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index 57d60c5..c26824e 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -26,7 +26,7 @@ def get_move_group_launch(context): # Get MoveIt Configs builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") builder = builder.robot_description( - mappings={'sim': sim, 'end_effector_tool': end_effector_tool} + mappings={"sim": sim, "end_effector_tool": end_effector_tool} ) moveit_config = builder.to_moveit_configs() @@ -70,7 +70,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) ld = LaunchDescription() diff --git a/ada_moveit/launch/moveit_rviz.launch.py b/ada_moveit/launch/moveit_rviz.launch.py index 84e5592..ec43525 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) @@ -45,7 +45,7 @@ def generate_launch_description(): # Get MoveIt Configs builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") builder = builder.robot_description( - mappings={'sim': sim, 'end_effector_tool': end_effector_tool} + mappings={"sim": sim, "end_effector_tool": end_effector_tool} ) moveit_config = builder.to_moveit_configs() diff --git a/ada_moveit/launch/rsp.launch.py b/ada_moveit/launch/rsp.launch.py index 40c33dc..521535e 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) @@ -45,7 +45,7 @@ def generate_launch_description(): # Get MoveIt Configs builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") builder = builder.robot_description( - mappings={'sim': sim, 'end_effector_tool': end_effector_tool} + mappings={"sim": sim, "end_effector_tool": end_effector_tool} ) moveit_config = builder.to_moveit_configs() diff --git a/ada_moveit/launch/spawn_controllers.launch.py b/ada_moveit/launch/spawn_controllers.launch.py index 5a9b778..15769ae 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) @@ -45,7 +45,7 @@ def generate_launch_description(): # Get MoveIt Configs builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") builder = builder.robot_description( - mappings={'sim': sim, 'end_effector_tool': end_effector_tool} + mappings={"sim": sim, "end_effector_tool": end_effector_tool} ) moveit_config = builder.to_moveit_configs() diff --git a/ada_moveit/launch/static_virtual_joint_tfs.launch.py b/ada_moveit/launch/static_virtual_joint_tfs.launch.py index 2874709..b005c7a 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) @@ -45,7 +45,7 @@ def generate_launch_description(): # Get MoveIt Configs builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") builder = builder.robot_description( - mappings={'sim': sim, 'end_effector_tool': end_effector_tool} + mappings={"sim": sim, "end_effector_tool": end_effector_tool} ) moveit_config = builder.to_moveit_configs() diff --git a/ada_moveit/launch/warehouse_db.launch.py b/ada_moveit/launch/warehouse_db.launch.py index 7954578..90d89e7 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=['none', 'fork', 'articulable_fork'] + choices=["none", "fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) @@ -45,7 +45,7 @@ def generate_launch_description(): # Get MoveIt Configs builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") builder = builder.robot_description( - mappings={'sim': sim, 'end_effector_tool': end_effector_tool} + mappings={"sim": sim, "end_effector_tool": end_effector_tool} ) moveit_config = builder.to_moveit_configs() From 669aa7d5a4f4f702bc6465c5465a9ef7680a808b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 19 Dec 2024 17:55:15 -0800 Subject: [PATCH 46/53] Only load Dynamixel hardware interface if end_effector_tool is set to articulable_fork --- ada_hardware/urdf/ada_hardware.xacro | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index 62ac5d5..b725f79 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -8,6 +8,7 @@ + @@ -23,9 +24,11 @@ sim="$(arg sim)" readonly="$(arg readonly)" /> - + + + From 8852f74dc04714dcab4af01f51a7eb278519b9bb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 19 Dec 2024 17:55:51 -0800 Subject: [PATCH 47/53] Revert "Only load Dynamixel hardware interface if end_effector_tool is set to articulable_fork" This reverts commit 669aa7d5a4f4f702bc6465c5465a9ef7680a808b. --- ada_hardware/urdf/ada_hardware.xacro | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index b725f79..62ac5d5 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -8,7 +8,6 @@ - @@ -24,11 +23,9 @@ sim="$(arg sim)" readonly="$(arg readonly)" /> - - - + From a902c0b66a733c5e8ad54378f55ee703f0f5d2d7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 19 Dec 2024 18:32:17 -0800 Subject: [PATCH 48/53] Configure semantic robot description with end_effector_tool --- ada_moveit/config/ada.srdf | 99 +++++++++++++++----------- ada_moveit/launch/demo.launch.py | 3 + ada_moveit/launch/move_group.launch.py | 3 + 3 files changed, 62 insertions(+), 43 deletions(-) diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index 0bbd235..ff09e0b 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -3,7 +3,10 @@ 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,9 +24,11 @@ - - - + + + + + @@ -44,10 +49,6 @@ - - - - @@ -60,6 +61,12 @@ + + + + + + @@ -67,8 +74,10 @@ - - + + + + @@ -77,8 +86,10 @@ - - + + + + @@ -297,36 +308,38 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index be19184..281c3d9 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -106,6 +106,9 @@ def generate_launch_description(): 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 diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index c26824e..11fe135 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -28,6 +28,9 @@ def get_move_group_launch(context): 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 From 40a4a66eeeb7d49436cd5063a496837439c6968b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 13 Jan 2025 16:42:08 -0800 Subject: [PATCH 49/53] Change IMU serial port to /dev/ttyUSB1 to avoid conflict with Articulable Fork --- ada_imu/config/imu_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_imu/config/imu_params.yaml b/ada_imu/config/imu_params.yaml index 1b70fb8..6395ef3 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 From 2f9bc66776fe9179c92f80d498e6f3d543f3b5f5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 15 Jan 2025 08:10:25 -0800 Subject: [PATCH 50/53] Remove end_effector_tool choice none since controlling the ada move group requires the forkTip frame to exist --- ada_moveit/launch/demo.launch.py | 2 +- ada_moveit/launch/move_group.launch.py | 2 +- ada_moveit/launch/moveit_rviz.launch.py | 2 +- ada_moveit/launch/rsp.launch.py | 2 +- ada_moveit/launch/spawn_controllers.launch.py | 2 +- ada_moveit/launch/static_virtual_joint_tfs.launch.py | 2 +- ada_moveit/launch/warehouse_db.launch.py | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 4eaf7ed..48bf1c1 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -55,7 +55,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=["none", "fork", "articulable_fork"], + choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index 1ea7d15..9163e76 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -73,7 +73,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=["none", "fork", "articulable_fork"], + choices=["fork", "articulable_fork"], ) ld = LaunchDescription() diff --git a/ada_moveit/launch/moveit_rviz.launch.py b/ada_moveit/launch/moveit_rviz.launch.py index 3594165..0f114a3 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=["none", "fork", "articulable_fork"], + choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) diff --git a/ada_moveit/launch/rsp.launch.py b/ada_moveit/launch/rsp.launch.py index 0dc6899..3320030 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=["none", "fork", "articulable_fork"], + choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) diff --git a/ada_moveit/launch/spawn_controllers.launch.py b/ada_moveit/launch/spawn_controllers.launch.py index b673286..856800e 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=["none", "fork", "articulable_fork"], + choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) diff --git a/ada_moveit/launch/static_virtual_joint_tfs.launch.py b/ada_moveit/launch/static_virtual_joint_tfs.launch.py index 623228b..15445df 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=["none", "fork", "articulable_fork"], + choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) diff --git a/ada_moveit/launch/warehouse_db.launch.py b/ada_moveit/launch/warehouse_db.launch.py index d8323b3..45097d4 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): "end_effector_tool", default_value="fork", description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", - choices=["none", "fork", "articulable_fork"], + choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") ld.add_action(eet_da) From 6eacdd7d958c894ddc3d8536689e65a224fcbb12 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 15 Jan 2025 08:14:46 -0800 Subject: [PATCH 51/53] Remove redundant list of choices in end_effector_tool launch argument --- ada_moveit/launch/demo.launch.py | 2 +- ada_moveit/launch/move_group.launch.py | 2 +- ada_moveit/launch/moveit_rviz.launch.py | 2 +- ada_moveit/launch/rsp.launch.py | 2 +- ada_moveit/launch/spawn_controllers.launch.py | 2 +- ada_moveit/launch/static_virtual_joint_tfs.launch.py | 2 +- ada_moveit/launch/warehouse_db.launch.py | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 48bf1c1..08ed53f 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -54,7 +54,7 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + description="The end-effector tool being used", choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index 9163e76..9b35375 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -72,7 +72,7 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + description="The end-effector tool being used", choices=["fork", "articulable_fork"], ) diff --git a/ada_moveit/launch/moveit_rviz.launch.py b/ada_moveit/launch/moveit_rviz.launch.py index 0f114a3..79d187e 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + description="The end-effector tool being used", choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") diff --git a/ada_moveit/launch/rsp.launch.py b/ada_moveit/launch/rsp.launch.py index 3320030..0476471 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + description="The end-effector tool being used", choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") diff --git a/ada_moveit/launch/spawn_controllers.launch.py b/ada_moveit/launch/spawn_controllers.launch.py index 856800e..589d6e9 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + description="The end-effector tool being used", choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") diff --git a/ada_moveit/launch/static_virtual_joint_tfs.launch.py b/ada_moveit/launch/static_virtual_joint_tfs.launch.py index 15445df..f88d867 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + description="The end-effector tool being used", choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") diff --git a/ada_moveit/launch/warehouse_db.launch.py b/ada_moveit/launch/warehouse_db.launch.py index 45097d4..c95fa44 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): eet_da = DeclareLaunchArgument( "end_effector_tool", default_value="fork", - description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + description="The end-effector tool being used", choices=["fork", "articulable_fork"], ) end_effector_tool = LaunchConfiguration("end_effector_tool") From 311e40944eb650f65b2a0954d0202b24147ccdef Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 15 Jan 2025 10:14:09 -0800 Subject: [PATCH 52/53] Address pylint warnings --- ada_moveit/launch/demo.launch.py | 4 ---- ada_moveit/launch/move_group.launch.py | 6 ++++++ ada_moveit/launch/moveit_rviz.launch.py | 3 +++ ada_moveit/launch/rsp.launch.py | 3 +++ ada_moveit/launch/spawn_controllers.launch.py | 3 +++ ada_moveit/launch/static_virtual_joint_tfs.launch.py | 3 +++ ada_moveit/launch/warehouse_db.launch.py | 3 +++ 7 files changed, 21 insertions(+), 4 deletions(-) diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 08ed53f..bbc0d08 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -15,15 +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 launch_ros.substitutions import FindPackageShare from srdfdom.srdf import SRDF diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index 9b35375..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. @@ -51,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", diff --git a/ada_moveit/launch/moveit_rviz.launch.py b/ada_moveit/launch/moveit_rviz.launch.py index 79d187e..e6ef7f7 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -11,6 +11,9 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() # Sim Launch Argument diff --git a/ada_moveit/launch/rsp.launch.py b/ada_moveit/launch/rsp.launch.py index 0476471..6708939 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -11,6 +11,9 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() # Sim Launch Argument diff --git a/ada_moveit/launch/spawn_controllers.launch.py b/ada_moveit/launch/spawn_controllers.launch.py index 589d6e9..3d6061d 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -11,6 +11,9 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() # Sim Launch Argument diff --git a/ada_moveit/launch/static_virtual_joint_tfs.launch.py b/ada_moveit/launch/static_virtual_joint_tfs.launch.py index f88d867..40aa73c 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -11,6 +11,9 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() # Sim Launch Argument diff --git a/ada_moveit/launch/warehouse_db.launch.py b/ada_moveit/launch/warehouse_db.launch.py index c95fa44..7ef2146 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -11,6 +11,9 @@ def generate_launch_description(): + # pylint: disable=duplicate-code + # Launch arguments must be re-declared to be evaluated in context + ld = LaunchDescription() # Sim Launch Argument From 219f0ffada14b20545b431d606abc8f72578a320 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 28 Jan 2025 21:50:31 -0800 Subject: [PATCH 53/53] Remove base_parent argument when instantiating ada macro --- ada_moveit/config/ada.urdf.xacro | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index 3a324ef..05032bc 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -7,10 +7,8 @@ - - - +