From 7878e9fdc0d6da5773bae02c094966c6333d849b Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Thu, 20 Feb 2025 15:06:26 +0800 Subject: [PATCH 1/3] =?UTF-8?q?Update=20v1.0.17=20=E4=BF=AE=E5=A4=8Dhardwa?= =?UTF-8?q?re=E5=88=9D=E5=A7=8B=E5=8C=96=E6=97=B6setup=5Fmotion=E6=8F=90?= =?UTF-8?q?=E5=89=8D=E9=97=AE=E9=A2=98=E5=92=8C=E5=88=9D=E5=A7=8B=E4=BD=8D?= =?UTF-8?q?=E7=BD=AE=E5=A7=8B=E7=BB=88=E4=B8=BA0=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 3 +++ .../src/coarm_hardware_interface.cpp | 25 +++++++++++-------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 3d7fb67..f968de6 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,9 @@ The coarm_ros2 repository allowing efficient control of the Coarm robots within Coarm机械臂ROS2开发包，支持Coarm机械臂在ROS2下的集成控制。 +### Update v1.0.17 +1. 修复hardware初始化时setup_motion提前问题和初始位置始终为0问题 + ### Update v1.0.16 1. 新增coarm_omnipicker_moveit_config包 2. 修复eye_to_hand相关错误 diff --git a/coarm_hardware/src/coarm_hardware_interface.cpp b/coarm_hardware/src/coarm_hardware_interface.cpp index cfd4885..d6c32fd 100644 --- a/coarm_hardware/src/coarm_hardware_interface.cpp +++ b/coarm_hardware/src/coarm_hardware_interface.cpp @@ -119,10 +119,13 @@ namespace coarm_hardware } RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), "Send request to CoarmServer setup pvt motion"); auto setup_motion_result = coarmlibs_setup_motion_client_->async_send_request(setup_motion_request); - std::future_status setup_motion_status = setup_motion_result.wait_for(2s); - if (setup_motion_status == std::future_status::ready) { - RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), - "Received CoarmServer setup_motion service response"); + while(rclcpp::ok()){ + std::future_status setup_motion_status = setup_motion_result.wait_for(2s); + if (setup_motion_status == std::future_status::ready) { + RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), + "Received CoarmServer setup_motion service response"); + break; + } } point_count_ = (point_count_ + 1) & MAX_COUNT; RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), "Coarm Hardware Sucessfully configured!"); @@ -139,8 +142,8 @@ namespace coarm_hardware { if (std::isnan(hw_position_states_[i])) { - hw_position_states_[i] = 0; - hw_position_cmds_[i] = 0; + hw_position_states_[i] = position_states_[i]; + hw_position_cmds_[i] = position_states_[i]; } else { @@ -227,13 +230,13 @@ namespace coarm_hardware for (size_t i = 0; i < hw_position_cmds_.size(); i++) { send_point_request->joint_ids.push_back(std::to_string(i + 1)); - send_point_request->position.push_back(next_position_cmds_[i]); + send_point_request->position.push_back(position_cmds_[i]); // Velocity = (distance) / (duration) between last position and next position -// RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), -// "%f, %f, %f, %f", next_position_cmds_[i], last_position_cmds_[i], duration_, next_duration_); - double velocity = (next_position_cmds_[i] - last_position_cmds_[i]) / (0.01 + 0.01); + // RCLCPP_INFO(rclcpp::get_logger("CoarmHardwareInterface"), + // "%f, %f, %f, %f", next_position_cmds_[i], last_position_cmds_[i], duration_, next_duration_); + double velocity = (next_position_cmds_[i] - last_position_cmds_[i]) / (duration_ + next_duration_); send_point_request->velocity.push_back(velocity); - send_point_request->duration.push_back(0.01); + send_point_request->duration.push_back(0.02); send_point_request->count.push_back(point_count_); send_point_request->last.push_back(false); } -- Gitee From f8767a0bc0f8b496b938ec4b84e57ee97f035a91 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Thu, 20 Feb 2025 15:10:18 +0800 Subject: [PATCH 2/3] =?UTF-8?q?Update=20v1.0.17=20=E4=BF=AE=E6=94=B9contro?= =?UTF-8?q?l=E7=9A=84=E8=BF=90=E5=8A=A8=E7=82=B9=E4=BD=8D=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E5=92=8C=E5=8A=A0=E9=80=9F=E5=BA=A6=E5=8F=97moveit?= =?UTF-8?q?=E5=8F=98=E5=8C=96=E6=8E=A7=E5=88=B6=EF=BC=8Cmovej=E6=8E=A5?= =?UTF-8?q?=E6=94=B6=E5=A4=9A=E4=B8=AA=E8=BD=A8=E8=BF=B9=E7=82=B9=E4=BD=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 1 + coarm_planner/src/coarm_control.cpp | 137 ++++++++++++++++++++++++++-- 2 files changed, 131 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index f968de6..888ac90 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ Coarm机械臂ROS2开发包，支持Coarm机械臂在ROS2下的集成控制。 ### Update v1.0.17 1. 修复hardware初始化时setup_motion提前问题和初始位置始终为0问题 +2. 修改control的运动点位速度和加速度受moveit变化控制，movej接收多个轨迹点位 ### Update v1.0.16 1. 新增coarm_omnipicker_moveit_config包 diff --git a/coarm_planner/src/coarm_control.cpp b/coarm_planner/src/coarm_control.cpp index 5fb4ca4..5d2e0b3 100644 --- a/coarm_planner/src/coarm_control.cpp +++ b/coarm_planner/src/coarm_control.cpp @@ -49,6 +49,11 @@ public: !coarmlibs_movej_client_->wait_for_service(1s) || !coarmlibs_stop_client_->wait_for_service(1s)) { + if (!rclcpp::ok()) + { + std::cerr << "[ERROR]\tInterrupted while waiting for the service. Exiting." << std::endl; + return; + } RCLCPP_INFO(this->get_logger(), "CoarmServer not available, waiting again..."); } RCLCPP_INFO(this->get_logger(), "CoarmControl initialized"); @@ -168,14 +173,96 @@ private: { // Call CoarmServer MoveJ service and send last trajectory point to MoveJ service auto movej_request = std::make_shared(); - for (size_t i = 0; i < joint_names_.size(); i++) + // float max_vel = 0; + // for (size_t i = 1; i < point_num - 1; i++){ + // for (size_t j = 0; j < joint_names_.size(); j++) + // { + // float vel = std::fabs(torso_goal->trajectory.points[i].velocities[joint_match[j]] * 180 / PI); + // if (max_vel < 20.0) + // { + // max_vel = 20.0; + // continue; + // } + // if (max_vel > vel) + // { + // continue; + // } + // max_vel = vel; + // } + // } + // RCLCPP_INFO(this->get_logger(), "Accepted joint vel: %f ", max_vel); + float max_acc = 0; + for (size_t i = 1; i < point_num - 1; i++){ + for (size_t j = 0; j < joint_names_.size(); j++) + { + float acc = std::fabs(torso_goal->trajectory.points[i].accelerations[joint_match[j]] * 180 / PI); + if (max_acc < 30.0) + { + max_acc = 30.0; + continue; + } + if (max_acc > acc) + { + continue; + } + max_acc = acc; + } + } + for (size_t i = 2; i < point_num - 3; i += 2){ + movej_request->joint_ids.clear(); + movej_request->joints.clear(); + for (size_t j = 0; j < joint_names_.size(); j++) + { + movej_request->joint_ids.push_back(std::to_string(j + 1)); + movej_request->joints.push_back( + torso_goal->trajectory.points[i].positions[joint_match[j]] * 180 / PI); + } + float max_vel = 0; + for (size_t j = 0; j < joint_names_.size(); j++) + { + float vel = std::fabs(torso_goal->trajectory.points[i].velocities[joint_match[j]] * 180 / PI); + if (max_vel < 10.0) + { + max_vel = 10.0; + continue; + } + if (max_vel > vel) + { + continue; + } + max_vel = vel; + } + movej_request->velocity = max_vel; + movej_request->acceleration = max_acc; + movej_request->block = false; + movej_request->advance = false; + coarmlibs_movej_client_->async_send_request(movej_request); + } + movej_request->joint_ids.clear(); + movej_request->joints.clear(); + for (size_t j = 0; j < joint_names_.size(); j++) { - movej_request->joint_ids.push_back(std::to_string(i + 1)); + movej_request->joint_ids.push_back(std::to_string(j + 1)); movej_request->joints.push_back( - torso_goal->trajectory.points[point_num - 1].positions[joint_match[i]] * 180 / PI); + torso_goal->trajectory.points[point_num - 1].positions[joint_match[j]] * 180 / PI); } - movej_request->velocity = 50.0; - movej_request->acceleration = 100.0; + float max_vel = 0; + for (size_t j = 0; j < joint_names_.size(); j++) + { + float vel = std::fabs(torso_goal->trajectory.points[point_num - 1].velocities[joint_match[j]] * 180 / PI); + if (max_vel < 10.0) + { + max_vel = 10.0; + continue; + } + if (max_vel > vel) + { + continue; + } + max_vel = vel; + } + movej_request->velocity = max_vel; + movej_request->acceleration = max_acc; movej_request->block = true; movej_request->advance = false; auto movej_result = coarmlibs_movej_client_->async_send_request(movej_request); @@ -205,14 +292,50 @@ private: { // Call CoarmServer MoveJoints service and send last trajectory point to MoveJoints service auto move_joints_request = std::make_shared(); + float max_vel = 0; + for (size_t i = 1; i < point_num - 1; i++){ + for (size_t j = 0; j < joint_names_.size(); j++) + { + float vel = std::fabs(torso_goal->trajectory.points[i].velocities[joint_match[j]] * 180 / PI); + if (max_vel < 20.0) + { + max_vel = 20.0; + continue; + } + if (max_vel > vel) + { + continue; + } + max_vel = vel; + } + } + RCLCPP_INFO(this->get_logger(), "Accepted joint vel: %f ", max_vel); + float max_acc = 0; + for (size_t i = 1; i < point_num - 1; i++){ + for (size_t j = 0; j < joint_names_.size(); j++) + { + float acc = std::fabs(torso_goal->trajectory.points[i].accelerations[joint_match[j]] * 180 / PI); + if (max_acc < 40.0) + { + max_acc = 40.0; + continue; + } + if (max_acc > acc) + { + continue; + } + max_acc = acc; + } + } + RCLCPP_INFO(this->get_logger(), "Accepted joint acc: %f ", max_acc); for (size_t i = 0; i < joint_names_.size(); i++) { move_joints_request->joint_ids.push_back(std::to_string(i + 1)); move_joints_request->joints.push_back( torso_goal->trajectory.points[point_num - 1].positions[joint_match[i]] * 180 / PI); } - move_joints_request->velocity = 50.0; - move_joints_request->acceleration = 100.0; + move_joints_request->velocity = max_vel; + move_joints_request->acceleration = max_acc; move_joints_request->block = true; move_joints_request->advance = false; auto move_joints_result = coarmlibs_move_joints_client_->async_send_request(move_joints_request); -- Gitee From 5c3c8d6d06d5a3eb4f28b2012a3432a89719b6d3 Mon Sep 17 00:00:00 2001 From: DzC-0i <11778119+DzC-0i@user.noreply.gitee.com> Date: Thu, 20 Feb 2025 15:13:30 +0800 Subject: [PATCH 3/3] =?UTF-8?q?Update=20v1.0.17=20=E4=BF=AE=E6=94=B9send?= =?UTF-8?q?=5Fpoint=E6=9C=8D=E5=8A=A1=E6=8E=A5=E5=8F=A3=E4=B8=8B=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=BB=B6=E6=97=B6=EF=BC=8Cros=E6=9C=8D=E5=8A=A1?= =?UTF-8?q?=E5=90=AF=E5=8A=A8=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 1 + coarm_driver/coarm_driver/server.py | 19 +++++++++++-------- coarm_driver/config/coarm_server.yaml | 4 ++-- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 888ac90..f91beb8 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,7 @@ Coarm机械臂ROS2开发包，支持Coarm机械臂在ROS2下的集成控制。 ### Update v1.0.17 1. 修复hardware初始化时setup_motion提前问题和初始位置始终为0问题 2. 修改control的运动点位速度和加速度受moveit变化控制，movej接收多个轨迹点位 +3. 修改send_point服务接口下控制延时，ros服务启动参数 ### Update v1.0.16 1. 新增coarm_omnipicker_moveit_config包 diff --git a/coarm_driver/coarm_driver/server.py b/coarm_driver/coarm_driver/server.py index 1fd5c12..3755860 100644 --- a/coarm_driver/coarm_driver/server.py +++ b/coarm_driver/coarm_driver/server.py @@ -37,8 +37,8 @@ class StatePublisher(Node): # Create publisher self.joints_publisher = self.create_publisher(Joints, '/coarm_server/joints', 10) self.pose_publisher = self.create_publisher(Pose, '/coarm_server/pose', 10) - # Create publish timer - self.timer = self.create_timer(0.01, self.publish_callback) + # Create publish timer (50hz) + self.timer = self.create_timer(0.02, self.publish_callback) def publish_callback(self): joints = Joints() @@ -328,11 +328,12 @@ class SendPointService(Node): duration = list(request.duration) count = list(request.count) last = list(request.last) - self.get_logger().info('Incoming send_point request with ' - 'joint_ids: %s, position: %s, ' - 'velocity: %s, duration: %s, ' - 'count: %s, last: %s' - % (joint_ids, position, velocity, duration, count, last)) + # if (sum(velocity) != 0): + # self.get_logger().info('Incoming send_point request with ' + # 'joint_ids: %s, position: %s, ' + # 'velocity: %s, duration: %s, ' + # 'count: %s, last: %s' + # % (joint_ids, position, velocity, duration, count, last)) position = {str(joint_ids[i]): float(position[i]) for i in range(len(joint_ids))} velocity = {str(joint_ids[i]): float(velocity[i]) for i in range(len(joint_ids))} duration = {str(joint_ids[i]): float(duration[i]) for i in range(len(joint_ids))} @@ -350,7 +351,9 @@ class SendPointService(Node): for status in buffer_status: value &= status if value > 0: - time.sleep(PVT_DURATION) + # pvt_motion_client 测试时需要 + # time.sleep(PVT_DURATION) + pass else: break response.status = CONTROLLER_OK diff --git a/coarm_driver/config/coarm_server.yaml b/coarm_driver/config/coarm_server.yaml index fb06405..330f47c 100644 --- a/coarm_driver/config/coarm_server.yaml +++ b/coarm_driver/config/coarm_server.yaml @@ -1,4 +1,4 @@ /get_parameters: ros__parameters: - robot_mode: 0 - robot_name: COARM \ No newline at end of file + robot_mode: 1 + robot_name: coarm \ No newline at end of file -- Gitee