Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Real robot works with servo

Browse Source 
gripper needs different update loop and sometimes has too much current and then the whole robot goes into error recovery mode
This commit is contained in:
NikoFeith 2024-05-13 17:38:57 +02:00
parent bcba4ef3d2
commit 5077372894
11 changed files with 36 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -88,7 +88,7 @@ After launching you need to change the controller to enable the servo mode.\
Terminal 4:
```bash
ros2 control switch_controllers --deactivate joint_trajectory_controller
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
ros2 control switch_controllers --deactivate robotiq_gripper_controller
ros2 control switch_controllers --activate forward_position_controller
ros2 control switch_controllers --activate forward_gripper_position_controller
```

30
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
View File

@ -436,8 +436,8 @@
<axis xyz="0 0 1" />
</joint>
<!-- Pseudo Link and Joint for gripper gap -->
<link name="${prefix}dummy_link">
<!-- Dummy link to facilitate the pseudo joint -->
<link name="dummy_link">
<visual>
<geometry>
<box size="0.001 0.001 0.001"/> <!-- Very small, effectively invisible -->
@ -446,25 +446,23 @@
<color rgba="0.0 0.0 0.0 0.0"/> <!-- Transparent color -->
</material>
</visual>
<collision>
<geometry>
<sphere radius="0.001"/> <!-- Negligible size -->
</geometry>
</collision>
</link>
<joint name="${prefix}base_to_dummy" type="fixed">
<parent link="${prefix}robotiq_140_base_link"/>
<child link="${prefix}dummy_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Adjust origin as needed -->
<!-- Fixed joint to attach the dummy link to part of the gripper -->
<joint name="dummy_to_pad" type="fixed">
<parent link="dummy_link"/>
<child link="robotiq_140_left_inner_finger_pad"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Position it appropriately -->
</joint>
<joint name="${prefix}gripper_gap" type="prismatic">
<parent link="${prefix}dummy_link"/>
<child link="${prefix}robotiq_140_right_inner_finger_pad"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Set origin appropriately -->
<axis xyz="0 1 0"/> <!-- Axis of movement; adjust according to actual alignment -->
<!-- Pseudo prismatic joint for controlling the gripper gap using the dummy link -->
<joint name="gripper_gap" type="prismatic">
<parent link="robotiq_140_base_link"/> <!-- Adjust the parent link as necessary -->
<child link="dummy_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Properly position this joint -->
<axis xyz="0 1 1"/> <!-- Ensure the axis aligns with the intended motion -->
<limit lower="0.0" upper="0.14" effort="235" velocity="0.15"/>
</joint>
</xacro:macro>
</robot>

27
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
View File

@ -304,32 +304,5 @@
<origin xyz="-0.00563134 0.0 0.04718515" rpy="0 0 0" />
<mimic joint="${prefix}finger_joint" />
</joint>
<!-- Pseudo Link and Joint for gripper gap -->
<link name="${prefix}dummy_link">
<visual>
<geometry>
<box size="0.001 0.001 0.001"/> <!-- Very small, effectively invisible -->
</geometry>
<material name="transparent">
<color rgba="0.0 0.0 0.0 0.0"/> <!-- Transparent color -->
</material>
</visual>
</link>
<joint name="base_to_dummy" type="fixed">
<parent link="${prefix}robotiq_140_base_link"/>
<child link="dummy_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Adjust origin as needed -->
</joint>
<joint name="gripper_gap" type="prismatic">
<parent link="${prefix}dummy_link"/>
<child link="${prefix}robotiq_85_right_finger_tip_link"/>
<origin xyz="0 0 0" rpy="0 0 0"/> <!-- Set origin appropriately -->
<axis xyz="0 1 0"/> <!-- Axis of movement; adjust according to actual alignment -->
<limit lower="0.0" upper="0.085" effort="235" velocity="0.15"/>
</joint>
</xacro:macro>
</robot>

2
src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
View File

@ -510,7 +510,7 @@ double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) {
if (std::isnan(max_angle_) || 0.0 >= max_angle_) {
throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly.");
}
double scaled_angle = (static_cast<double>(raw_position) / 255.0) * max_angle_;
double scaled_angle = ((230 - static_cast<double>(raw_position)) / 227.0) * max_angle_;
return std::max(0.0, std::min(scaled_angle, max_angle_));
}

8
src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
View File

@ -217,13 +217,6 @@ def launch_setup(context, *args, **kwargs):
package="controller_manager",
executable="spawner",
arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
condition=UnlessCondition(use_fake_hardware),
)
robotiq_gripper_joint_trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["robotiq_gripper_joint_trajectory_controller", "-c", "/controller_manager"],
condition=IfCondition(use_fake_hardware),
)
robotiq_activation_controller_spawner = Node(
@ -347,7 +340,6 @@ def launch_setup(context, *args, **kwargs):
initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started,
robotiq_gripper_controller_spawner,
robotiq_gripper_joint_trajectory_controller_spawner,
robotiq_activation_controller_spawner,
] + controller_spawners

8
src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
View File

@ -31,8 +31,8 @@ joint_trajectory_controller:
robotiq_gripper_controller:
action_ns: robotiq_2f_controllers
type: RobotiqGripperCommandController
action_ns: gripper_cmd
type: GripperCommand
default: true
joint: gripper_gap
max_gap: 0.14
joints:
- gripper_gap

7
src/ur_robotiq_moveit_config/config/ur_servo.yaml
View File

@ -7,10 +7,10 @@ use_intra_process_comms : True
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.00001 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
rotational: 0.001 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
# Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
joint: 0.01
joint: 0.5
# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
# controller performance. It essentially increases the timestep when calculating the target pose, to move the target
# pose farther away. [seconds]
@ -38,6 +38,7 @@ low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the
## MoveIt properties
move_group_name: ur_manipulator # Often 'manipulator' or 'arm'
planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world'
is_primary_planning_scene_monitor: false
## Other frames
ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose

3
src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
View File

@ -27,14 +27,15 @@
<joint name="${prefix}right_outer_finger_joint"/>
<joint name="${prefix}right_inner_finger_joint"/>
<joint name="${prefix}right_inner_finger_pad_joint"/>
<joint name="${prefix}base_to_dummy"/>
</group>
<!--GROUP STATES Purpose Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="open" group="${prefix}gripper">
<joint name="${prefix}gripper_gap" value="0.14"/>
<joint name="${prefix}finger_joint" value="0.0"/>
</group_state>
<group_state name="closed" group="${prefix}gripper">
<joint name="${prefix}gripper_gap" value="0.0"/>
<joint name="${prefix}finger_joint" value="0.695"/>
</group_state>
<group_state name="${prefix}home" group="${prefix}${name}_manipulator">
<joint name="${prefix}elbow_joint" value="0" />

2
src/ur_robotiq_servo/config/joy-params.yaml
View File

@ -1,7 +1,7 @@
joy_node:
ros__parameters:
device_id: 0
device_name: "PS5 Controller"
device_name: "Wireless Controller"
deadzone: 0.5
autorepeat_rate: 20.0
sticky_buttons: false

4
src/ur_robotiq_servo/config/ps5-params.yaml
View File

@ -1,6 +1,6 @@
ps5_servo_node:
ros__parameters:
linear_speed_multiplier: 0.2
gripper_position_multiplier: 0.02
linear_speed_multiplier: 0.5
gripper_position_multiplier: 0.004
gripper_lower_limit: 0.0
gripper_upper_limit: 0.14

14
src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
View File

@ -97,13 +97,13 @@ class PS5ControllerNode(Node):
def joy_callback(self, msg):
# Check for button press to toggle mode
# Assuming button 2 (e.g., Triangle on PS5) for toggling mode
current_button_state = msg.buttons[16]
current_button_state = msg.buttons[8]
if current_button_state == 1 and self.last_button_state == 0:
self.mode = 'joint' if self.mode == 'twist' else 'twist'
self.get_logger().info(f'Mode switched to: {self.mode}')
self.last_button_state = current_button_state
left_trigger = (msg.axes[4] - 1) / - 2.0
left_trigger = (msg.axes[2] - 1) / - 2.0
right_trigger = (msg.axes[5] - 1) / - 2.0
# Process control input based on current mode
@ -115,8 +115,8 @@ class PS5ControllerNode(Node):
twist_msg.twist.linear.y = -msg.axes[1] * self.linear_speed_multiplier
twist_msg.twist.linear.z = (left_trigger - right_trigger) * self.linear_speed_multiplier
twist_msg.twist.angular.x = msg.axes[3]
twist_msg.twist.angular.y = msg.axes[2]
twist_msg.twist.angular.z = (msg.buttons[9] - msg.buttons[10]) * 1.0
twist_msg.twist.angular.y = msg.axes[4]
twist_msg.twist.angular.z = (msg.buttons[4] - msg.buttons[5]) * 1.0
self.twist_pub.publish(twist_msg)
elif self.mode == 'joint':
joint_msg = JointJog()
@ -132,10 +132,10 @@ class PS5ControllerNode(Node):
]
joint_msg.velocities = [msg.axes[0],
msg.axes[1],
msg.axes[2],
msg.axes[3],
(msg.buttons[11] - msg.buttons[12]) * 1.0,
(msg.buttons[13] - msg.buttons[14]) * 1.0]
msg.axes[4],
msg.axes[6],
msg.axes[7]]
self.joint_pub.publish(joint_msg)
# Gripper controller