diff --git a/ur_rtde_driver/config/ur10_controllers.yaml b/ur_rtde_driver/config/ur10_controllers.yaml index ebbd6be..333dac8 100644 --- a/ur_rtde_driver/config/ur10_controllers.yaml +++ b/ur_rtde_driver/config/ur10_controllers.yaml @@ -4,7 +4,7 @@ hardware_control_loop: # Settings for ros_control hardware interface hardware_interface: - joints: + joints: &robot_joints - shoulder_pan_joint - shoulder_lift_joint - elbow_joint @@ -31,13 +31,7 @@ speed_scaling_state_controller: # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -48,18 +42,12 @@ scaled_pos_traj_controller: wrist_2_joint: {trajectory: 0.2, goal: 0.1} wrist_3_joint: {trajectory: 0.2, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 500 + state_publish_rate: 125 action_monitor_rate: 10 pos_traj_controller: type: position_controllers/JointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -70,5 +58,5 @@ pos_traj_controller: wrist_2_joint: {trajectory: 0.2, goal: 0.1} wrist_3_joint: {trajectory: 0.2, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 500 + state_publish_rate: 125 action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur10e_controllers.yaml b/ur_rtde_driver/config/ur10e_controllers.yaml index c100dfc..0608e0d 100644 --- a/ur_rtde_driver/config/ur10e_controllers.yaml +++ b/ur_rtde_driver/config/ur10e_controllers.yaml @@ -4,7 +4,7 @@ hardware_control_loop: # Settings for ros_control hardware interface hardware_interface: - joints: + joints: &robot_joints - shoulder_pan_joint - shoulder_lift_joint - elbow_joint @@ -31,13 +31,7 @@ speed_scaling_state_controller: # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -53,13 +47,7 @@ scaled_pos_traj_controller: pos_traj_controller: type: position_controllers/JointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 diff --git a/ur_rtde_driver/config/ur3_controllers.yaml b/ur_rtde_driver/config/ur3_controllers.yaml index ebbd6be..333dac8 100644 --- a/ur_rtde_driver/config/ur3_controllers.yaml +++ b/ur_rtde_driver/config/ur3_controllers.yaml @@ -4,7 +4,7 @@ hardware_control_loop: # Settings for ros_control hardware interface hardware_interface: - joints: + joints: &robot_joints - shoulder_pan_joint - shoulder_lift_joint - elbow_joint @@ -31,13 +31,7 @@ speed_scaling_state_controller: # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -48,18 +42,12 @@ scaled_pos_traj_controller: wrist_2_joint: {trajectory: 0.2, goal: 0.1} wrist_3_joint: {trajectory: 0.2, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 500 + state_publish_rate: 125 action_monitor_rate: 10 pos_traj_controller: type: position_controllers/JointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -70,5 +58,5 @@ pos_traj_controller: wrist_2_joint: {trajectory: 0.2, goal: 0.1} wrist_3_joint: {trajectory: 0.2, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 500 + state_publish_rate: 125 action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur3e_controllers.yaml b/ur_rtde_driver/config/ur3e_controllers.yaml index c100dfc..0608e0d 100644 --- a/ur_rtde_driver/config/ur3e_controllers.yaml +++ b/ur_rtde_driver/config/ur3e_controllers.yaml @@ -4,7 +4,7 @@ hardware_control_loop: # Settings for ros_control hardware interface hardware_interface: - joints: + joints: &robot_joints - shoulder_pan_joint - shoulder_lift_joint - elbow_joint @@ -31,13 +31,7 @@ speed_scaling_state_controller: # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -53,13 +47,7 @@ scaled_pos_traj_controller: pos_traj_controller: type: position_controllers/JointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 diff --git a/ur_rtde_driver/config/ur5_controllers.yaml b/ur_rtde_driver/config/ur5_controllers.yaml index ebbd6be..333dac8 100644 --- a/ur_rtde_driver/config/ur5_controllers.yaml +++ b/ur_rtde_driver/config/ur5_controllers.yaml @@ -4,7 +4,7 @@ hardware_control_loop: # Settings for ros_control hardware interface hardware_interface: - joints: + joints: &robot_joints - shoulder_pan_joint - shoulder_lift_joint - elbow_joint @@ -31,13 +31,7 @@ speed_scaling_state_controller: # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -48,18 +42,12 @@ scaled_pos_traj_controller: wrist_2_joint: {trajectory: 0.2, goal: 0.1} wrist_3_joint: {trajectory: 0.2, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 500 + state_publish_rate: 125 action_monitor_rate: 10 pos_traj_controller: type: position_controllers/JointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -70,5 +58,5 @@ pos_traj_controller: wrist_2_joint: {trajectory: 0.2, goal: 0.1} wrist_3_joint: {trajectory: 0.2, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 500 + state_publish_rate: 125 action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur5e_controllers.yaml b/ur_rtde_driver/config/ur5e_controllers.yaml index c100dfc..0608e0d 100644 --- a/ur_rtde_driver/config/ur5e_controllers.yaml +++ b/ur_rtde_driver/config/ur5e_controllers.yaml @@ -4,7 +4,7 @@ hardware_control_loop: # Settings for ros_control hardware interface hardware_interface: - joints: + joints: &robot_joints - shoulder_pan_joint - shoulder_lift_joint - elbow_joint @@ -31,13 +31,7 @@ speed_scaling_state_controller: # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 @@ -53,13 +47,7 @@ scaled_pos_traj_controller: pos_traj_controller: type: position_controllers/JointTrajectoryController - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + joints: *robot_joints constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 diff --git a/ur_rtde_driver/src/rtde/rtde_client.cpp b/ur_rtde_driver/src/rtde/rtde_client.cpp index d0a3a71..d9e512b 100644 --- a/ur_rtde_driver/src/rtde/rtde_client.cpp +++ b/ur_rtde_driver/src/rtde/rtde_client.cpp @@ -67,7 +67,7 @@ bool RTDEClient::init() tmp_version = dynamic_cast(package.get()); if (!tmp_version->accepted_) { - throw UrException("Neither protocol version 1 nor 2 were accepted by the robot. This should not happen!"); + throw UrException("Neither protocol version 1 nor 2 was accepted by the robot. This should not happen!"); } } @@ -110,7 +110,7 @@ bool RTDEClient::start() std::unique_ptr> package; stream_.write(buffer, size, written); if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) - throw UrException("Could not get respone to rtde communication start request from robot. This should not happen!"); + throw UrException("Could not get response to RTDE communication start request from robot. This should not happen!"); rtde_interface::ControlPackageStart* tmp = dynamic_cast(package.get()); return tmp->accepted_; }