UniversalRobots / Universal_Robots_ROS2_Driver

Universal Robots ROS2 driver supporting CB3 and e-Series
BSD 3-Clause "New" or "Revised" License
438 stars 229 forks source link

Sending joint angles to UR10e #1086

Closed PeymanAmirii closed 2 months ago

PeymanAmirii commented 3 months ago

Affected ROS2 Driver version(s)

2.4.9

Used ROS distribution.

Humble, Iron

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

Real robot

Robot SW / URSim version(s)

UR10e

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

I can get real-time joint position feedback from a real robot UR10e by echoing the topic /joint_states. Even I can move the robot using the visual pakcage provided by the moveit package ur_moveit_config. But When I publish joint angles to the topic /forward_position_controller/commands, the robot does not move.

  1. At first I followed the instruction on https://docs.ros.org/en/ros2_packages/rolling/api/ur_robot_driver/ to install externalcontrol-1.0.5.urcap and make the robot ready. Then, I installed ur and moveit packages for ROS Iron:

    sudo apt-get install ros-iron-ur
    sudo apt install ros-iron-moveit
  2. The robot is robot controller is on and connected to the Ubuntu computer using Etherent connection. The ip addresses of the computer and the robot are 192.168.1.101 and 192.168.1.102, respectively. When I ping the robot using the following command, I get a response. ping 192.168.1.102

  3. Afterwards, I powered on the robot and launched ur_control.launch.py by the command below and could see the real-time configuraiton of the robot in RVIZ.

    source /opt/ros/iron/setup.bash
    ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=192.168.1.102
  4. I ran the following command to echo the joint angles that the topic /joint_states was publishing, and it worked correctly. ros2 topic echo /joint_states

  5. I started runnig the program on UR teach pendant containig the External Control node. Then, published the joint angles through the following command to move the robot:

ros2 topic pub --rate 1 /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: 
- 1.08
- -1.3
- -1.69
- -0.36
- 0.62
- 0.5"

or

ros2 topic pub --rate 1 /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: [1.08, -1.3, -1.69, -0.36, 0.62, 0.5]"

The message was published the topic successfully, but the robot did not move. The robot has been set well because I can move the robot if I launch the moveit package using the following command: ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur10e launch_rviz:=true

Please guide me on how to send joint angles through the command line.

Relevant log output

No response

Accept Public visibility

PeymanAmirii commented 3 months ago

We found this action that can be used as below to send the joint angles: ros2 action send_goal /move_action moveit_msgs/action/MoveGroup "{request: {group_name: 'ur_manipulator', goal_constraints: [{joint_constraints: [{joint_name: 'shoulder_pan_joint', position: 0.0}, {joint_name: 'shoulder_lift_joint', position: -1.57}, {joint_name: 'elbow_joint', position: 1.57}, {joint_name: 'wrist_1_joint', position: 0.0}, {joint_name: 'wrist_2_joint', position: 0.0}, {joint_name: 'wrist_3_joint', position: 0.0}]}]}}" However, I am still curious about using the aforementioned topic to send the joint angles to the robot.

Iaroslavrul commented 3 months ago

@PeymanAmirii Hi! It's work for me ros2 topic pub --once /scaled_joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], points: [{positions: [-0.45, -1.57, 0.0, -1.57, 0.0, 0.0], time_from_start: {sec: 2}}]}"

fmauch commented 2 months ago

The topic mentioned in the OP /forward_position_controller/commands is coming from the forward_position_controller which is by default not active. If you have the ros2controlcli package installed, you can check the active controllers easily:

$ ros2 control list_controllers                                                                                                               [ur_rolling]                                                                                                                                                                
[INFO 2024-09-04 15:20:36.380] [_ros2cli_388587]: waiting for service /controller_manager/list_controllers to become available...                                                                                                                                                                                           
force_torque_sensor_broadcaster    force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster  active                                                                                                                                                                                                                     
speed_scaling_state_broadcaster    ur_controllers/SpeedScalingStateBroadcaster                   active                                                                                                                                                                                                                     
io_and_status_controller           ur_controllers/GPIOController                                 active                                                                                                                                                                                                                     
joint_state_broadcaster            joint_state_broadcaster/JointStateBroadcaster                 active                                                                                                                                                                                                                     
scaled_joint_trajectory_controller ur_controllers/ScaledJointTrajectoryController                active                                                                                                                                                                                                                     
forward_position_controller        position_controllers/JointGroupPositionController             inactive

You can change controllers using

$ ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller --activate forward_position_controller
Successfully switched controllers

and then

$ ros2 topic pub --once /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:                                                                                                                                                                                                          [ur_rolling]
- 1.08
- -1.3
- -1.69
- -0.36
- 0.62
- 0.5"
publisher: beginning loop
publishing #1: std_msgs.msg.Float64MultiArray(layout=std_msgs.msg.MultiArrayLayout(dim=[], data_offset=0), data=[1.08, -1.3, -1.69, -0.36, 0.62, 0.5])

will actually move the robot.

NOTE: The robot will move to the target pose as fast as possible. Make sure that the robot is already close to that configuration.

To simply move the robot to a target pose which is further away, using the by default active scaled JTC as mentioned by @Iaroslavrul is probably the better approach, or even better use a planned motion with MoveIt.

I'll close this, as I think the original question has been answered. Feel free to comment and/or reopen if you disagree.