diff --git a/ur_rtde_driver/config/ur10_controllers.yaml b/ur_rtde_driver/config/ur10_controllers.yaml index ebbd6be..c6a7a54 100644 --- a/ur_rtde_driver/config/ur10_controllers.yaml +++ b/ur_rtde_driver/config/ur10_controllers.yaml @@ -72,3 +72,58 @@ pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +vel_based_pos_traj_controller: + type: velocity_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values have not been optimized!! + shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + + # Use a feedforward term to reduce the size of PID gains + velocity_ff: + shoulder_pan_joint: 1.0 + shoulder_lift_joint: 1.0 + elbow_joint: 1.0 + wrist_1_joint: 1.0 + wrist_2_joint: 1.0 + wrist_3_joint: 1.0 + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #stop_trajectory_duration: 0 # Defaults to 0.0 + +# Pass an array of joint velocities directly to the joints +joint_group_vel_controller: + type: velocity_controllers/JointGroupVelocityController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index a8f8d58..8c6822f 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -102,11 +102,11 @@ protected: ur_controllers::ScaledPositionJointInterface spj_interface_; hardware_interface::PositionJointInterface pj_interface_; ur_controllers::SpeedScalingInterface speedsc_interface_; - // hardware_interface::VelocityJointInterface vj_interface_; + hardware_interface::VelocityJointInterface vj_interface_; hardware_interface::ForceTorqueSensorInterface fts_interface_; vector6d_t joint_position_command_; - // std::vector joint_velocity_command_; + vector6d_t joint_velocity_command_; vector6d_t joint_positions_; vector6d_t joint_velocities_; vector6d_t joint_efforts_; @@ -124,6 +124,7 @@ protected: uint32_t runtime_state_; bool position_controller_running_; + bool velocity_controller_running_; PausingState pausing_state_; double pausing_ramp_up_increment_; diff --git a/ur_rtde_driver/launch/ur10_speed_bringup.launch b/ur_rtde_driver/launch/ur10_speed_bringup.launch new file mode 100644 index 0000000..13695c0 --- /dev/null +++ b/ur_rtde_driver/launch/ur10_speed_bringup.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur_rtde_driver/launch/ur_control.launch b/ur_rtde_driver/launch/ur_control.launch index aad3ff6..3063e64 100644 --- a/ur_rtde_driver/launch/ur_control.launch +++ b/ur_rtde_driver/launch/ur_control.launch @@ -11,9 +11,9 @@ - - - + + + diff --git a/ur_rtde_driver/resources/speedj.urscript b/ur_rtde_driver/resources/speedj.urscript new file mode 100644 index 0000000..14d6a8c --- /dev/null +++ b/ur_rtde_driver/resources/speedj.urscript @@ -0,0 +1,62 @@ +{{BEGIN_REPLACE}} + +global steptime = get_steptime() +global speedj_time = steptime - 0.1*steptime +textmsg("steptime=", steptime) +MULT_jointstate = {{JOINT_STATE_REPLACE}} + +global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +cmd_speedj_active = True +keepalive = -2 + +def set_speed(qd): + enter_critical + cmd_servo_qd = qd + cmd_speedj_active = True + exit_critical +end + +thread speedThread(): + while True: + enter_critical + qd = cmd_servo_qd + exit_critical + #textmsg("loop") + if cmd_speedj_active: + # Having this shortly under steptime seems to help + speedj(qd, 40.0, speedj_time) + else: + stopj(5.0) + end + sync() + end + stopj(5.0) +end + +socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") + +thread_speed = run speedThread() + +params_mult = socket_read_binary_integer(1+6, "reverse_socket") +keepalive = params_mult[1] +while keepalive > 0: + params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) + if params_mult[0] > 0: + keepalive = params_mult[1] + if keepalive > 1: + qd = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + set_speed(qd) + #speedj(qd, 40.0, 0.007) + else: + cmd_speedj_active = False + keepalive = 1 + end + else: + textmsg("Missing package from remote pc") + keepalive = keepalive - 1 + end +end +sleep(.1) +socket_close() +kill thread_speed + diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index ac33cdb..56b8627 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -39,12 +39,14 @@ namespace ur_driver { HardwareInterface::HardwareInterface() : joint_position_command_({ 0, 0, 0, 0, 0, 0 }) + , joint_velocity_command_({ 0, 0, 0, 0, 0, 0 }) , joint_positions_{ { 0, 0, 0, 0, 0, 0 } } , joint_velocities_{ { 0, 0, 0, 0, 0, 0 } } , joint_efforts_{ { 0, 0, 0, 0, 0, 0 } } , joint_names_(6) , runtime_state_(static_cast(rtde_interface::RUNTIME_STATE::STOPPED)) , position_controller_running_(false) + , velocity_controller_running_(false) , pausing_state_(PausingState::RUNNING) , pausing_ramp_up_increment_(0.01) , controllers_initialized_(false) @@ -174,6 +176,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h // Create joint position control interface pj_interface_.registerHandle( hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); + vj_interface_.registerHandle( + hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i])); spj_interface_.registerHandle(ur_controllers::ScaledJointHandle( js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_)); } @@ -188,6 +192,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h registerInterface(&js_interface_); registerInterface(&spj_interface_); registerInterface(&pj_interface_); + registerInterface(&vj_interface_); registerInterface(&speedsc_interface_); registerInterface(&fts_interface_); @@ -294,6 +299,10 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio { ur_driver_->writeJointCommand(joint_position_command_); } + else if (velocity_controller_running_) + { + ur_driver_->writeJointCommand(joint_velocity_command_); + } else { ur_driver_->writeKeepalive(); @@ -327,6 +336,7 @@ void HardwareInterface ::doSwitch(const std::list& stop_list) { position_controller_running_ = false; + velocity_controller_running_ = false; for (auto& controller_it : start_list) { for (auto& resource_it : controller_it.claimed_resources) @@ -339,6 +349,10 @@ void HardwareInterface ::doSwitch(const std::list