TM5-900 getting results with large errors when using velocity mode

[kaihung1]

I tried to use Listen node in TMFlow connected to a ubuntu22.04 to use the velocity node. here is my code:

include "rclcpp/rclcpp.hpp"

include "tm_msgs/srv/send_script.hpp"

include

include

include

include

void send_cmd(const std::string& cmd, rclcpp::Node::SharedPtr node, rclcpp::Client::SharedPtr client) { auto request = std::make_shared(); request->id = "demo"; request->script = cmd;

while (!client->wait_for_service(std::chrono::seconds(1))) {
    if (!rclcpp::ok()) {
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
        return;
    }
    RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "Service not available, waiting again...");
}

auto result = client->async_send_request(request);

// Wait for the result.
if (rclcpp::spin_until_future_complete(node, result) !=
    rclcpp::FutureReturnCode::SUCCESS)
{
    RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "Failed to call service");
    return;
}

if (result.get()->ok) {
    RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"),"OK");
} else {
    RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"),"Not OK");
}

}

int main() { rclcpp::init(0, nullptr);

auto node = std::make_shared<rclcpp::Node>("demo_send_script");
auto client = node->create_client<tm_msgs::srv::SendScript>("send_script");

// Initial position
double target_position[] = {530, -400, 440, -180, 0, 90};
// Move to initial position
std::string motion_cmd = "PTP(\"CPP\"," + std::to_string(target_position[0]) + "," + 
                         std::to_string(target_position[1]) + "," + std::to_string(target_position[2]) + 
                         ",-180,0,90,35,200,0,false)";
send_cmd(motion_cmd, node, client);
std::this_thread::sleep_for(std::chrono::milliseconds(15000)); // Wait to reach target position

// Six specified positions
double positions[][6] = {
    {530, -400, 440, -180, 0, 90},
    {520, -300, 560, -180, 0, 90},
    {480, -60, 650, -180, 0, 90},
    {480, 120, 660, -180, 0, 90},
    {510, 315, 580, -180, 0, 90},
    {600, 370, 370, -180, 0, 90}
};

// Calculate the distance between each point and send the corresponding movement command
for (int i = 0; i < 5; ++i) {
    // Calculate the differences in the x, y, and z directions
    double dx = positions[i+1][0] - positions[i][0];
    double dy = positions[i+1][1] - positions[i][1];
    double dz = positions[i+1][2] - positions[i][2];
    // Calculate the velocity for each direction
    double velocity_x = dx / 5000;
    double velocity_y = dy / 5000;
    double velocity_z = dz / 5000;
    // Set continuous velocity command
    std::string velocity_cmd = "SetContinueVLine(" + std::to_string(velocity_x) + ", " + std::to_string(velocity_y) + ", " + std::to_string(velocity_z) + ", 0, 0, 0)";
    send_cmd(velocity_cmd, node, client);
    // Set the movement duration
    std::string distance_cmd = "ContinueVLine(5000, 5000)";
    send_cmd(distance_cmd, node, client);
    std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // Wait for 5 seconds
}
rclcpp::shutdown();
return 0;

}

But the results I obtained are very different from what I expected. For example. the first point should arrive to { 520 , -300 , 560 , -180 , 0 , 90 } but the result i get from TM-controller { 532 , -377 , 527 , -180 , 0 , 90 } I have no idea where it went wrong. What should I do to fix it?