HiveCoreRD/hivecore_robot_arm
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix some issues

Browse Source
This commit is contained in:
NuoDaJia
2026-01-30 15:50:14 +08:00
parent fe80e94969
commit 79e6595e93
6 changed files with 43 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/dual_arm_action_server/include/dual_arm_action_server/real_robot_driver.hpp
View File

@@ -36,8 +36,8 @@ public:
bool execute_motion(
const interfaces::msg::ArmMotionParams & params,
uint8_t arm_id,
double velocity_scaling,
double blend_radius,
int velocity_scaling,
int blend_radius,
std::string * error_msg);
bool get_current_joints(
@@ -61,8 +61,8 @@ private:
int push_cycle);
rm_robot_handle * handle_for(uint8_t arm_id) const;
static int clamp_speed_percent(double scaling);
static int clamp_blend_radius_mm(double blend_radius_m);
static int clamp_speed_percent(int percent);
static int clamp_blend_radius_percent(int percent);
static RealRobotDriver * instance_;

12
src/dual_arm_action_server/src/dual_arm_action_server_node.cpp
View File

@@ -249,8 +249,8 @@ void DualArmActionServer::execute(const std::shared_ptr<GoalHandleDualArm> goal_
bool DualArmActionServer::validate_goal(const DualArm::Goal & goal, std::string & error_msg) const
{
if (goal.velocity_scaling < 0.0 || goal.velocity_scaling > 1.0) {
error_msg = "velocity_scaling out of range [0.0, 1.0]";
if (goal.velocity_scaling < 1 || goal.velocity_scaling > 100) {
error_msg = "velocity_scaling out of range [1, 100]";
return false;
}
if (goal.arm_motion_params.empty() || goal.arm_motion_params.size() > 2) {
@@ -267,10 +267,10 @@ bool DualArmActionServer::validate_goal(const DualArm::Goal & goal, std::string
error_msg = "unsupported motion_type";
return false;
}
// if (params.blend_radius < 0.0 || params.blend_radius > 0.5) {
// error_msg = "blend_radius out of range [0.0, 0.5]";
// return false;
// }
if (params.blend_radius < 0 || params.blend_radius > 100) {
error_msg = "blend_radius out of range [0, 100]";
return false;
}
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEJ &&
params.target_joints.size() != USED_ARM_DOF)
{

35
src/dual_arm_action_server/src/real_robot_driver.cpp
View File

@@ -89,13 +89,13 @@ bool RealRobotDriver::is_connected() const
bool RealRobotDriver::execute_motion(
const interfaces::msg::ArmMotionParams & params,
uint8_t arm_id,
double velocity_scaling,
double blend_radius,
int velocity_scaling,
int blend_radius,
std::string * error_msg)
{
const int blend_radius_mm = clamp_blend_radius_mm(blend_radius);
const int trajectory_connect = (blend_radius_mm > 0) ? 1 : 0;
const int block_mode = (blend_radius_mm > 0) ? 0 : 1;
const int blend_radius_percent = clamp_blend_radius_percent(blend_radius);
const int trajectory_connect = (blend_radius_percent > 0) ? 1 : 0;
const int block_mode = (blend_radius_percent > 0) ? 0 : 1;
rm_robot_handle * handle = handle_for(arm_id);
if (!handle) {
@@ -124,7 +124,7 @@ bool RealRobotDriver::execute_motion(
handle,
joints,
speed_percent,
blend_radius_mm,
blend_radius_percent,
trajectory_connect,
block_mode);
if (result != 0) {
@@ -165,7 +165,7 @@ bool RealRobotDriver::execute_motion(
handle,
pose,
speed_percent,
blend_radius_mm,
blend_radius_percent,
trajectory_connect,
block_mode);
if (result != 0) {
@@ -191,8 +191,8 @@ bool RealRobotDriver::get_current_joints(
joints_left.resize(USED_ARM_DOF);
joints_right.resize(USED_ARM_DOF);
for (size_t i = 0; i < USED_ARM_DOF; ++i) {
joints_left[i] = joints_left_[i] * M_PI / 180.0;
joints_right[i] = joints_right_[i] * M_PI / 180.0;
joints_left[i] = joints_left_[i];
joints_right[i] = joints_right_[i];
}
return true;
}
@@ -305,24 +305,17 @@ rm_robot_handle * RealRobotDriver::handle_for(uint8_t arm_id) const
return nullptr;
}
int RealRobotDriver::clamp_speed_percent(double scaling)
int RealRobotDriver::clamp_speed_percent(int percent)
{
if (!std::isfinite(scaling)) {
return 50;
}
int percent = static_cast<int>(std::round(scaling * 100.0));
percent = std::clamp(percent, 1, 100);
return percent;
return std::clamp(percent, 1, 100);
}
int RealRobotDriver::clamp_blend_radius_mm(double blend_radius_m)
int RealRobotDriver::clamp_blend_radius_percent(int percent)
{
if (!std::isfinite(blend_radius_m) || blend_radius_m <= 0.0) {
if (percent <= 0) {
return 0;
}
const double mm = blend_radius_m * 1000.0;
const int radius = static_cast<int>(std::lround(mm));
return std::clamp(radius, 0, 500);
return std::clamp(percent, 0, 100);
}
} // namespace dual_arm_action_server

2
src/dual_arm_moveit_action_server/include/dual_arm_moveit_action_server/dual_arm_moveit_action_server.hpp
View File

@@ -49,7 +49,7 @@ private:
bool plan_for_arm(
const interfaces::msg::ArmMotionParams & params,
moveit::planning_interface::MoveGroupInterface & move_group,
double velocity_scaling,
int velocity_scaling,
moveit_msgs::msg::RobotTrajectory * out_trajectory,
std::string * error_msg);

4
src/dual_arm_moveit_action_server/scripts/dualarm_action_test.py
View File

@@ -18,7 +18,7 @@ def make_movej(arm_id: int, joints: List[float]) -> ArmMotionParams:
msg.arm_id = arm_id
msg.motion_type = ArmMotionParams.MOVEJ
msg.target_joints = joints
msg.blend_radius = 0.0
msg.blend_radius = 0
return msg
@@ -66,7 +66,7 @@ def send_goal(action_client: ActionClient, goal_msg: DualArm.Goal, label: str) -
def main() -> int:
parser = argparse.ArgumentParser(description="DualArm action test client")
parser.add_argument("--velocity-scaling", type=float, default=0.2)
parser.add_argument("--velocity-scaling", type=int, default=20)
parser.add_argument("--dual", action="store_true", help="Send left+right in a single goal")
parser.add_argument(
"--mode",

22
src/dual_arm_moveit_action_server/src/dual_arm_moveit_action_server.cpp
View File

@@ -54,6 +54,15 @@ double compute_trajectory_duration(const moveit_msgs::msg::RobotTrajectory & tra
namespace dual_arm_moveit_action_server
{
namespace
{
double percent_to_scaling(int percent)
{
const double clamped = std::clamp(percent, 1, 100);
return clamped / 100.0;
}
} // namespace
DualArmMoveitActionServer::DualArmMoveitActionServer(const rclcpp::Node::SharedPtr & node)
: node_(node)
@@ -314,9 +323,9 @@ void DualArmMoveitActionServer::execute(const std::shared_ptr<GoalHandleDualArm>
bool DualArmMoveitActionServer::validate_goal(const DualArm::Goal & goal, std::string * error_msg) const
{
if (goal.velocity_scaling < 0.0 || goal.velocity_scaling > 1.0) {
if (goal.velocity_scaling < 1 || goal.velocity_scaling > 100) {
if (error_msg) {
*error_msg = "velocity_scaling out of range [0.0, 1.0]";
*error_msg = "velocity_scaling out of range [1, 100]";
}
return false;
}
@@ -401,7 +410,7 @@ std::vector<interfaces::msg::ArmMotionParams> DualArmMoveitActionServer::normali
bool DualArmMoveitActionServer::plan_for_arm(
const interfaces::msg::ArmMotionParams & params,
moveit::planning_interface::MoveGroupInterface & move_group,
double velocity_scaling,
int velocity_scaling,
moveit_msgs::msg::RobotTrajectory * out_trajectory,
std::string * error_msg)
{
@@ -412,8 +421,9 @@ bool DualArmMoveitActionServer::plan_for_arm(
return false;
}
const double velocity_factor = percent_to_scaling(velocity_scaling);
move_group.setStartStateToCurrentState();
move_group.setMaxVelocityScalingFactor(velocity_scaling);
move_group.setMaxVelocityScalingFactor(velocity_factor);
move_group.setMaxAccelerationScalingFactor(acceleration_scaling_);
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEJ) {
@@ -447,7 +457,7 @@ bool DualArmMoveitActionServer::plan_for_arm(
return false;
}
if (!time_parameterize_trajectory(move_group, velocity_scaling, plan.trajectory_, error_msg)) {
if (!time_parameterize_trajectory(move_group, velocity_factor, plan.trajectory_, error_msg)) {
return false;
}
@@ -482,7 +492,7 @@ bool DualArmMoveitActionServer::plan_for_arm(
return false;
}
if (!time_parameterize_trajectory(move_group, velocity_scaling, trajectory, error_msg)) {
if (!time_parameterize_trajectory(move_group, velocity_factor, trajectory, error_msg)) {
return false;
}