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

servo working without collision checking for twist and joints

Browse Source 
gripper not included in servo
This commit is contained in:
Niko Feith 2024-04-04 18:33:56 +02:00
parent 1026fbd939
commit 5d45c4ee6e
11 changed files with 312 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
README.md
View File

@ -18,13 +18,15 @@ To download this repository and all of its included submodules, follow these ste
* Navigate to the directory where you want to download the project.
* Run the following Git command:
```bash
git clone git@git.cps.unileoben.ac.at:Niko/UR_Robotiq.git
```
```bash
git clone git@git.cps.unileoben.ac.at:Niko/UR_Robotiq.git
```
4. **Download necessary repos**
```bash
vcs import src < UR_Robotiq.humble.repos
```
The packages cartesian_controller_simulation and cartesian_controller_tests are excluded from colcon build by default.
If you want to build them to be build delete the COLCON_IGNORE file!
## How to Start the Docker Compose
1. **Prerequisites**
@ -33,7 +35,7 @@ To download this repository and all of its included submodules, follow these ste
2. **Starting the Containers**
* Navigate to the project directory in your terminal:
```bash
cd UR-Robotiq
cd <path/to/UR-Robotiq>
```
* Run the following command to start the Docker Compose:
@ -76,7 +78,8 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa
```
### servoing
Terminal 3:
Terminal 3:\
**At the moment the servoing doesnt work with the collision checker (self collision still works).**
```bash
ros2 launch ur_robotiq_servo ps5_servo.launch.py
```
@ -84,5 +87,7 @@ ros2 launch ur_robotiq_servo ps5_servo.launch.py
After launching you need to change the controller to enable the servo mode.\
Terminal 4:
```bash
ros2 control switch_controllers --deactivate joint_trajectory_controller --activate forward_position_controller
ros2 control switch_controllers --deactivate joint_trajectory_controller
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
ros2 control switch_controllers --activate forward_position_controller
```

233
src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml Normal file
View File

@ -0,0 +1,233 @@
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

2
src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
View File

@ -165,7 +165,7 @@ robotiq_activation_controller:
ros__parameters:
default: true
combined_trajectory_controller:
combined_joint_trajectory_controller:
ros__parameters:
joints:
- $(var tf_prefix)shoulder_pan_joint

0
src/ur_robotiq_description/launch/ur_robotiq_cartesian_control.launch.py Normal file
View File

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

@ -316,7 +316,7 @@ def launch_setup(context, *args, **kwargs):
"speed_scaling_state_broadcaster",
"force_torque_sensor_broadcaster",
]
controller_spawner_inactive_names = ["forward_position_controller"]
controller_spawner_inactive_names = ["forward_position_controller", "combined_joint_trajectory_controller"]
controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
controller_spawner(name, active=False) for name in controller_spawner_inactive_names

6
src/ur_robotiq_moveit_config/config/initial_positions.yaml
View File

@ -1,7 +1,7 @@
shoulder_pan_joint: 0.0
shoulder_pan_joint: 1.4
shoulder_lift_joint: -1.57
elbow_joint: 0.0
elbow_joint: 1.57
wrist_1_joint: -1.57
wrist_2_joint: 0.0
wrist_2_joint: -1.57
wrist_3_joint: 0.0
finger_joint: 20.0

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

@ -7,8 +7,8 @@ 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.05 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
rotational: 0.05 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
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.
# Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
joint: 0.01
# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
@ -17,7 +17,7 @@ scale:
system_latency_compensation: 0.04
## Properties of outgoing commands
publish_period: 0.004 # 1/Nominal publish rate [seconds]
publish_period: 0.01 # 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?
@ -63,14 +63,14 @@ command_out_topic: /forward_position_controller/commands # Publish outgoing comm
## Collision checking for the entire robot body
check_collisions: false # Check collisions?
# collision_check_rate: 40.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
collision_check_rate: 40.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Two collision check algorithms are available:
# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
# collision_check_type: threshold_distance
collision_check_type: threshold_distance
# Parameters for "threshold_distance"-type collision checking
# self_collision_proximity_threshold: 0.005 # Start decelerating when a self-collision is this far [m]
# scene_collision_proximity_threshold: 0.01 # Start decelerating when a scene collision is this far [m]
self_collision_proximity_threshold: 0.005 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.01 # Start decelerating when a scene collision is this far [m]
# Parameters for "stop_distance"-type collision checking
# collision_distance_safety_factor: 1.0 # Must be >= 1. A large safety factor is recommended to account for latency
# min_allowable_collision_distance: 0.005 # Stop if a collision is closer than this [m]
collision_distance_safety_factor: 1.0 # Must be >= 1. A large safety factor is recommended to account for latency
min_allowable_collision_distance: 0.005 # Stop if a collision is closer than this [m]

34
src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro → src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
View File

@ -5,11 +5,11 @@
-->
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
<xacro:macro name="ur_robotiq_srdf" params="name prefix">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<!--GROUPS Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS Groups can also be formed by referencing to already defined group names-->
<group name="${prefix}${name}_manipulator">
<chain base_link="${prefix}base_link" tip_link="${prefix}tool0"/>
</group>
@ -17,17 +17,21 @@
<joint name="${prefix}robotiq_base_joint"/>
<joint name="${prefix}robotiq_140_base_joint"/>
<joint name="${prefix}finger_joint"/>
<joint name="${prefix}left_inner_knuckle_joint"/>
<joint name="${prefix}left_outer_finger_joint"/>
<joint name="${prefix}left_inner_finger_joint"/>
<joint name="${prefix}left_inner_finger_pad_joint"/>
<joint name="${prefix}left_inner_knuckle_joint"/>
<joint name="${prefix}right_inner_knuckle_joint"/>
<joint name="${prefix}right_outer_knuckle_joint"/>
<joint name="${prefix}right_outer_finger_joint"/>
<joint name="${prefix}right_inner_finger_joint"/>
<joint name="${prefix}right_inner_finger_pad_joint"/>
</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 name="${prefix}${name}_manipulator_combined">
<group name="${prefix}${name}_manipulator"/>
<group name="${prefix}gripper"/>
</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="${prefix}close" group="${prefix}gripper">
<joint name="${prefix}finger_joint" value="0.7"/>
</group_state>
@ -58,17 +62,19 @@
<joint name="${prefix}wrist_2_joint" value="-1.6" />
<joint name="${prefix}wrist_3_joint" value="-0.11" />
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<!--END EFFECTOR Purpose Represent information about an end effector.-->
<end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<!--VIRTUAL JOINT Purpose this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link"/>
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
<passive_joint name="${prefix}right_inner_finger_joint"/>
<passive_joint name="${prefix}right_outer_knuckle_joint"/>
<passive_joint name="${prefix}right_inner_knuckle_joint"/>
<!--PASSIVE JOINT Purpose this element is used to mark joints that are not actuated-->
<passive_joint name="${prefix}left_inner_knuckle_joint"/>
<passive_joint name="${prefix}left_outer_finger_joint"/>
<passive_joint name="${prefix}left_inner_finger_joint"/>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<passive_joint name="${prefix}right_inner_knuckle_joint"/>
<passive_joint name="${prefix}right_outer_knuckle_joint"/>
<passive_joint name="${prefix}right_outer_finger_joint"/>
<passive_joint name="${prefix}right_inner_finger_joint"/>
<!--DISABLE COLLISIONS By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}upper_arm_link" reason="Never"/>
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}wrist_1_link" reason="Never"/>

3
src/ur_robotiq_servo/config/ps5-params.yaml Normal file
View File

@ -0,0 +1,3 @@
ps5_servo_node:
ros__parameters:
linear_speed_multiplier: 0.2

10
src/ur_robotiq_servo/launch/ps5_servo.launch.py
View File

@ -8,24 +8,22 @@ def generate_launch_description():
config_directory = os.path.join(
ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'),
'config')
params = os.path.join(config_directory, 'joy-params.yaml')
joy_params = os.path.join(config_directory, 'joy-params.yaml')
ps5_params = os.path.join(config_directory, 'ps5-params.yaml')
return LaunchDescription([
Node(
package='joy',
executable='joy_node',
name='joy_node',
output='screen',
parameters=[{
params,
}],
parameters=[joy_params],
),
Node(
package='ur_robotiq_servo',
executable='ps5_servo',
name='ps5_servo_node',
output='screen',
parameters=[{
}],
parameters=[ps5_params],
arguments=[],
),
])

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

@ -13,16 +13,27 @@ class PS5ControllerNode(Node):
self.mode = 'twist' # Initialize mode to 'twist'. Alternatives: 'twist', 'joint'
self.last_button_state = 0 # Track the last state of the toggle button to detect presses
# Parameters
self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0)
self.linear_speed_multiplier = self.get_parameter('linear_speed_multiplier').get_parameter_value().double_value
self.get_logger().info(f"Linear speed multiplier: {self.linear_speed_multiplier}")
self.use_fake_hardware = self.declare_parameter('use_fake_hardware', False)
self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value
self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}")
# Subscriber and Publisher
self.joy_sub = self.create_subscription(
Joy,
'/joy',
self.joy_callback,
10)
self.twist_pub = self.create_publisher(
TwistStamped,
'/servo_node/delta_twist_cmds',
10)
self.joint_pub = self.create_publisher(
JointJog,
'/servo_node/delta_joint_cmds',
@ -66,20 +77,25 @@ class PS5ControllerNode(Node):
twist_msg = TwistStamped()
twist_msg.header.frame_id = 'tool0'
twist_msg.header.stamp = self.get_clock().now().to_msg()
twist_msg.twist.linear.x = msg.axes[0]
twist_msg.twist.linear.y = msg.axes[1]
twist_msg.twist.linear.z = left_trigger - right_trigger
twist_msg.twist.linear.x = msg.axes[0] * self.linear_speed_multiplier
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
self.twist_pub.publish(twist_msg)
elif self.mode == 'joint':
joint_msg = JointJog()
joint_msg.header.frame_id = 'tool0'
joint_msg.header.stamp = self.get_clock().now().to_msg()
joint_msg.joint_names = ["shoulder_pan_joint",
"shoulder_lift_joint",
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"]
joint_msg.joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint",
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
]
joint_msg.velocities = [msg.axes[0],
msg.axes[1],
msg.axes[2],
@ -88,6 +104,7 @@ class PS5ControllerNode(Node):
(msg.buttons[13] - msg.buttons[14]) * 1.0]
self.joint_pub.publish(joint_msg)
def main(args=None):
rclpy.init(args=args)
node = PS5ControllerNode()
@ -95,5 +112,6 @@ def main(args=None):
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()