From d2768af3c559006d8a40b34f6cd0d393e423c37a Mon Sep 17 00:00:00 2001 From: NikoFeith Date: Tue, 7 May 2024 18:07:59 +0200 Subject: [PATCH] controller from ur_robotiq_control.launch.py starts up ur_robotiq_moveit.launch.py cant find gripper_gap joint --- src/cartesian_controllers | 1 - .../urdf/robotiq_2f_140_macro.urdf.xacro | 11 +- .../ur_robotiq_cartesian_controllers.yaml | 233 ------------------ .../config/ur_robotiq_controllers.yaml | 35 +-- .../ur_robotiq_cartesian_control.launch.py | 0 .../launch/ur_robotiq_control.launch.py | 26 +- .../urdf/ur_robotiq.urdf.xacro | 23 +- .../urdf/ur_to_robotiq_adapter.urdf.xacro | 39 --- .../config/initial_positions.yaml | 2 +- .../config/moveit_controllers.yaml | 20 +- .../launch/ur_robotiq_moveit.launch.py | 2 - .../srdf/old_ur_robotiq_macro.srdf.xacro | 134 ---------- .../srdf/ur_robotiq_macro.srdf.xacro | 185 +++++++------- 13 files changed, 132 insertions(+), 579 deletions(-) delete mode 160000 src/cartesian_controllers delete mode 100644 src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml delete mode 100644 src/ur_robotiq_description/launch/ur_robotiq_cartesian_control.launch.py delete mode 100644 src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro delete mode 100644 src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro diff --git a/src/cartesian_controllers b/src/cartesian_controllers deleted file mode 160000 index 5480fce..0000000 --- a/src/cartesian_controllers +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 5480fcee3a3b1ff813810775b19c795725b0fcd6 diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro index 380da23..37f50ee 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro @@ -446,15 +446,20 @@ + + + + + - + - + - + diff --git a/src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml deleted file mode 100644 index 2ed83db..0000000 --- a/src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml +++ /dev/null @@ -1,233 +0,0 @@ -controller_manager: - ros__parameters: - force_torque_sensor_broadcaster: - type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster - - scaled_joint_trajectory_controller: - type: ur_controllers/ScaledJointTrajectoryController - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - cartesian_compliance_controller: - type: cartesian_compliance_controller/CartesianComplianceController - - cartesian_force_controller: - type: cartesian_force_controller/CartesianForceController - - cartesian_motion_controller: - type: cartesian_motion_controller/CartesianMotionController - - motion_control_handle: - type: cartesian_controller_handles/MotionControlHandle - - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - - -force_torque_sensor_broadcaster: - ros__parameters: - sensor_name: tcp_fts_sensor - state_interface_names: - - force.x - - force.y - - force.z - - torque.x - - torque.y - - torque.z - frame_id: tool0 - topic_name: wrench - -scaled_joint_trajectory_controller: - ros__parameters: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.0 - shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - elbow_joint: { trajectory: 0.2, goal: 0.1 } - wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - wrist_3_joint: { trajectory: 0.2, goal: 0.1 } - -cartesian_compliance_controller: - ros__parameters: - - # This is the tip of the robot tool that you usually use for your task. - # For instance, it could be the drilling bit of a screwdriver or a grinding - # tool. When you specify a target_wrench, i.e. some additional forces that - # your robot should apply to its environment, that target_wrench gets - # applied in this frame. - end_effector_link: "tool0" - - # This is usually the link directly before the first actuated joint. All - # controllers will build a kinematic chain from this link up to - # end_effector_link. It's also the reference frame for the superposition - # of error components in all controllers. - robot_base_link: "base_link" - - # This is the URDF link of your sensor. Sensor signals are assumed to be - # given in this frame. It's important that this link is located somewhere - # between end_effector_link and robot_base_link. If that's not the case, - # the controllers won't initialize and will emit an error message. - ft_sensor_ref_link: "tool0" - - # This is the link that the robot feels compliant about. It does not need - # to coincide with the end_effector_link, but for many use cases, this - # configuration is handy. When working with a screwdriver, for instance, - # setting compliance_ref_link == end_effector_link makes it easy to specify - # downward pushing forces without generating unwanted offset moments. - # On the other hand, an application could benefit from yielding a little in - # the robot's wrist while drawing a line on a surface with a pen. - compliance_ref_link: "tool0" - - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - - # Choose between position or velocity. In fact, the controllers allow to - # set both at the same time, but not all drivers will support this. - # In general, position control is a little smoother due to the double - # time-integrated commands from the solver. If available on a robot, it - # should be the default. On some drivers, the velocity interface provides - # faster control cycles, and hence could improve stability in - # contact-dominated tasks. A drawback is that we lose one time integration - # step here and obtain noisier command signals in comparison to the - # position interface. It's probably suitable to test both on a new robot - # and decide for what works best. - command_interfaces: - - position - #- velocity - - stiffness: # w.r.t. compliance_ref_link coordinates - trans_x: 500.0 - trans_y: 500.0 - trans_z: 500.0 - rot_x: 20.0 - rot_y: 20.0 - rot_z: 20.0 - - solver: - error_scale: 0.5 - iterations: 1 - publish_state_feedback: True - - # For all controllers, these gains are w.r.t. the robot_base_link coordinates. - pd_gains: - trans_x: {p: 0.05, d: 0.005} - trans_y: {p: 0.05, d: 0.005} - trans_z: {p: 0.05, d: 0.005} - rot_x: {p: 1.5} - rot_y: {p: 1.5} - rot_z: {p: 1.5} - -cartesian_force_controller: - ros__parameters: - - # See the cartesian_compliance_controller - end_effector_link: "tool0" - robot_base_link: "base_link" - ft_sensor_ref_link: "tool0" - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - - # See the cartesian_compliance_controller - command_interfaces: - - position - #- velocity - - solver: - error_scale: 0.5 - publish_state_feedback: True - - pd_gains: - trans_x: {p: 0.05} - trans_y: {p: 0.05} - trans_z: {p: 0.05} - rot_x: {p: 1.5} - rot_y: {p: 1.5} - rot_z: {p: 1.5} - -cartesian_motion_controller: - ros__parameters: - - # See the cartesian_compliance_controller - end_effector_link: "tool0" - robot_base_link: "base_link" - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - - # See the cartesian_compliance_controller - command_interfaces: - - position - #- velocity - - solver: - error_scale: 1.0 - iterations: 10 - publish_state_feedback: True - - pd_gains: - trans_x: {p: 1.0} - trans_y: {p: 1.0} - trans_z: {p: 1.0} - rot_x: {p: 0.5} - rot_y: {p: 0.5} - rot_z: {p: 0.5} - -motion_control_handle: - ros__parameters: - end_effector_link: "tool0" - robot_base_link: "base_link" - ft_sensor_ref_link: "tool0" - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - -joint_trajectory_controller: - ros__parameters: - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity \ No newline at end of file diff --git a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml index da32265..e8b1668 100644 --- a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml +++ b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml @@ -25,16 +25,13 @@ controller_manager: type: position_controllers/JointGroupPositionController forward_gripper_position_controller: - type: position_controllers/JointGroupPositionController + type: robotiq_2f_controllers/ForwardController robotiq_gripper_controller: - type: position_controllers/GripperActionController - - robotiq_gripper_joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController + type: robotiq_2f_controllers/GripperCommand robotiq_activation_controller: - type: robotiq_controllers/RobotiqActivationController + type: robotiq_2f_controllers/RobotiqActivationController speed_scaling_state_broadcaster: ros__parameters: @@ -138,32 +135,12 @@ forward_position_controller: forward_gripper_position_controller: ros__parameters: - joints: - - $(var tf_prefix)finger_joint - + joint: $(var prefix)gripper_gap robotiq_gripper_controller: ros__parameters: - joint: $(var tf_prefix)finger_joint - use_effort_interface: true - use_speed_interface: true - -robotiq_gripper_joint_trajectory_controller: - ros__parameters: - joints: - - $(var tf_prefix)finger_joint - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.0 - $(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 } + joint: $(var prefix)gripper_gap + max_gap: 0.14 robotiq_activation_controller: ros__parameters: diff --git a/src/ur_robotiq_description/launch/ur_robotiq_cartesian_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_cartesian_control.launch.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py index 776bc8a..08e2ffb 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -165,6 +165,9 @@ def launch_setup(context, *args, **kwargs): "trajectory_port:=", trajectory_port, " ", + "prefix:=", + tf_prefix, + " ", ] ) @@ -240,20 +243,6 @@ def launch_setup(context, *args, **kwargs): parameters=[{"robot_ip": robot_ip}], ) - tool_communication_node = Node( - package="ur_robot_driver", - condition=IfCondition(use_tool_communication), - executable="tool_communication.py", - name="ur_tool_comm", - output="screen", - parameters=[ - { - "robot_ip": robot_ip, - "tcp_port": tool_tcp_port, - "device_name": tool_device_name, - } - ], - ) controller_stopper_node = Node( package="ur_robot_driver", @@ -451,6 +440,15 @@ def generate_launch_description(): have to be updated.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value="", + description="prefix of the joint names, useful for \ + multi-robot setup. If changed, also joint names in the controllers' configuration \ + have to be updated.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "use_fake_hardware", diff --git a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro index f4dc290..be5d28c 100644 --- a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro +++ b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro @@ -5,8 +5,7 @@ - - + @@ -34,15 +33,9 @@ - - - - - - - - - + + + @@ -79,12 +72,6 @@ initial_positions="${xacro.load_yaml(initial_positions_file)}" use_tool_communication="$(arg use_tool_communication)" tool_voltage="$(arg tool_voltage)" - tool_parity="$(arg tool_parity)" - tool_baud_rate="$(arg tool_baud_rate)" - tool_stop_bits="$(arg tool_stop_bits)" - tool_rx_idle_chars="$(arg tool_rx_idle_chars)" - tool_tx_idle_chars="$(arg tool_tx_idle_chars)" - tool_device_name="$(arg tool_device_name)" tool_tcp_port="$(arg tool_tcp_port)" robot_ip="$(arg robot_ip)" script_filename="$(arg script_filename)" @@ -103,6 +90,8 @@ prefix="$(arg tf_prefix)" parent="$(arg tf_prefix)tool0" use_fake_hardware="$(arg use_fake_hardware)" + robot_ip="$(arg robot_ip)" + robot_port="$(arg tool_tcp_port)" > diff --git a/src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro b/src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro deleted file mode 100644 index d35f7a3..0000000 --- a/src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro +++ /dev/null @@ -1,39 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/ur_robotiq_moveit_config/config/initial_positions.yaml b/src/ur_robotiq_moveit_config/config/initial_positions.yaml index dcd628e..87eb956 100644 --- a/src/ur_robotiq_moveit_config/config/initial_positions.yaml +++ b/src/ur_robotiq_moveit_config/config/initial_positions.yaml @@ -4,4 +4,4 @@ elbow_joint: 1.57 wrist_1_joint: -1.57 wrist_2_joint: -1.57 wrist_3_joint: 0.0 -finger_joint: 20.0 \ No newline at end of file +gripper_gap: 0.0 \ No newline at end of file diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml index cfb0748..d36929b 100644 --- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml +++ b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml @@ -3,8 +3,6 @@ controller_names: - scaled_joint_trajectory_controller - joint_trajectory_controller - robotiq_gripper_controller - - robotiq_gripper_joint_trajectory_controller - - combined_joint_trajectory_controller scaled_joint_trajectory_controller: @@ -32,16 +30,10 @@ joint_trajectory_controller: - wrist_2_joint - wrist_3_joint -robotiq_gripper_controller: - action_ns: gripper_cmd - type: GripperCommand - default: true - joints: - - finger_joint -robotiq_gripper_joint_trajectory_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: false - joints: - - finger_joint +robotiq_gripper_controller: + action_ns: robotiq_2f_controllers + type: RobotiqGripperCommandController + default: true + joint: gripper_gap + max_gap: 0.14 diff --git a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py index 9018cf2..6e1d62b 100644 --- a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py +++ b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py @@ -152,8 +152,6 @@ def launch_setup(context, *args, **kwargs): if use_fake_hw == "true": controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False controllers_yaml["joint_trajectory_controller"]["default"] = True - controllers_yaml["robotiq_gripper_controller"]["default"] = False - controllers_yaml["robotiq_gripper_joint_trajectory_controller"]["default"] = True moveit_controllers = { "moveit_simple_controller_manager": controllers_yaml, diff --git a/src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro deleted file mode 100644 index 034a71d..0000000 --- a/src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro +++ /dev/null @@ -1,134 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro index d0dc7a7..c1a3f4c 100644 --- a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro +++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro @@ -14,6 +14,7 @@ + @@ -26,17 +27,14 @@ - - - - + - - - - + + + + @@ -67,6 +65,7 @@ + @@ -81,90 +80,90 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -174,5 +173,7 @@ + +