350 lines
12 KiB
C++
350 lines
12 KiB
C++
#include "KinematicsWebAPI.h"
|
|
|
|
json KinematicsWebAPI::handleRobotCommand(const std::string &req_cmd, const json &req_param)
|
|
{
|
|
json res_data;
|
|
|
|
if (req_cmd == "Cmd_Kinematics_inverse_pose_str")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_Kinematics_inverse_pose_str command");
|
|
|
|
std::string pose_str = req_param.value("pose_str", "");
|
|
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
|
|
std::string robot_uuid = req_param.value("robot_uuid", "default");
|
|
|
|
auto robot = RobotManager::getRobot(robot_uuid);
|
|
if (!robot || !robot->isInitialized())
|
|
{
|
|
res_data = {{"error", "Robot not found or not initialized"}};
|
|
}
|
|
else
|
|
{
|
|
json result = robot->inversePoseStr(pose_str, q_init_str);
|
|
if (result.empty())
|
|
{
|
|
res_data = {{"error", "Inverse kinematics calculation failed"}};
|
|
}
|
|
else
|
|
{
|
|
res_data = {{"joints", result}, {"success", true}};
|
|
}
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_Kinematics_inverse_pose_str_2PSteps command");
|
|
|
|
std::string pose_str = req_param.value("pose_str", "");
|
|
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
|
|
std::string robot_uuid = req_param.value("robot_uuid", "default");
|
|
std::string steps_str = req_param.value("steps_str", "default");
|
|
int steps = std::stoi(steps_str);
|
|
|
|
auto robot = RobotManager::getRobot(robot_uuid);
|
|
if (!robot || !robot->isInitialized())
|
|
{
|
|
res_data = {{"error", "Robot not found or not initialized"}};
|
|
}
|
|
else
|
|
{
|
|
json result = robot->inversePoseStr2PSteps(pose_str, q_init_str, steps);
|
|
if (result.empty())
|
|
{
|
|
res_data = {{"error", "Inverse kinematics calculation failed"}};
|
|
}
|
|
else
|
|
{
|
|
res_data = {{"joints", result}, {"success", true}};
|
|
}
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_Kinematics_inverse_pose_str_NoDifference command");
|
|
|
|
std::string pose_str = req_param.value("pose_str", "");
|
|
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
|
|
std::string robot_uuid = req_param.value("robot_uuid", "default");
|
|
|
|
auto robot = RobotManager::getRobot(robot_uuid);
|
|
if (!robot || !robot->isInitialized())
|
|
{
|
|
res_data = {{"error", "Robot not found or not initialized"}};
|
|
}
|
|
else
|
|
{
|
|
json result = robot->inversePoseStrNoDifference(pose_str, q_init_str);
|
|
if (result.empty())
|
|
{
|
|
res_data = {{"error", "Inverse kinematics calculation failed"}};
|
|
}
|
|
else
|
|
{
|
|
res_data = {{"joints", result}, {"success", true}};
|
|
}
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_Kinematics_forward_pose_str")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_Kinematics_forward_pose_str command");
|
|
|
|
std::string joints_str = req_param.value("q_init_str", "0,0,0,0,0,0");
|
|
std::string robot_uuid = req_param.value("robot_uuid", "default");
|
|
|
|
auto robot = RobotManager::getRobot(robot_uuid);
|
|
if (!robot || !robot->isInitialized())
|
|
{
|
|
res_data = {{"error", "Robot not found or not initialized"}};
|
|
}
|
|
else
|
|
{
|
|
auto joints = robot->parseJointString(joints_str);
|
|
if (joints.size() != 6)
|
|
{
|
|
res_data = {{"error", "Invalid joints format"}};
|
|
}
|
|
else
|
|
{
|
|
double tcp_pose[7];
|
|
if (robot->calculateFK_TCP(joints.data(), tcp_pose))
|
|
{
|
|
res_data = {
|
|
{"position", {tcp_pose[0], tcp_pose[1], tcp_pose[2]}},
|
|
{"orientation", {tcp_pose[3], tcp_pose[4], tcp_pose[5], tcp_pose[6]}},
|
|
{"joints", joints},
|
|
{"success", true}};
|
|
}
|
|
else
|
|
{
|
|
res_data = {{"error", "Forward kinematics calculation failed"}};
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to calculate forward kinematics: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_Kinematics_forward_all_joints")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_Kinematics_forward_all_joints command");
|
|
|
|
std::string joints_str = req_param.value("joints_str", "0,0,0,0,0,0");
|
|
std::string robot_uuid = req_param.value("robot_uuid", "default");
|
|
|
|
auto robot = RobotManager::getRobot(robot_uuid);
|
|
if (!robot || !robot->isInitialized())
|
|
{
|
|
res_data = {{"error", "Robot not found or not initialized"}};
|
|
}
|
|
else
|
|
{
|
|
res_data = {
|
|
{"OPERATION", robot->handleKinematicsForwardAllJoints(joints_str)},
|
|
{"success", true}};
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_Kinematics_check_singularity")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_Kinematics_check_singularity command");
|
|
|
|
std::string joints_str = req_param.value("joints_str", req_param.value("q_init_str", "0,0,0,0,0,0"));
|
|
std::string robot_uuid = req_param.value("robot_uuid", "default");
|
|
double singular_threshold = req_param.value("singular_threshold", 1e-4);
|
|
double warning_threshold = req_param.value("warning_threshold", 1e-2);
|
|
double condition_threshold = req_param.value("condition_threshold", 1e6);
|
|
double condition_warning_threshold = req_param.value("condition_warning_threshold", 1e4);
|
|
|
|
auto robot = RobotManager::getRobot(robot_uuid);
|
|
if (!robot || !robot->isInitialized())
|
|
{
|
|
res_data = {{"error", "Robot not found or not initialized"}};
|
|
}
|
|
else
|
|
{
|
|
res_data = robot->checkSingularity(
|
|
joints_str,
|
|
singular_threshold,
|
|
warning_threshold,
|
|
condition_threshold,
|
|
condition_warning_threshold);
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to check singularity: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_InitRobot")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_InitRobot command");
|
|
|
|
std::string urdf_base64 = req_param.value("urdf_base64", "");
|
|
std::string uuid = req_param.value("robot_uuid", "");
|
|
bool force_update = req_param.value("force_update", true);
|
|
std::string urdf_content = utils::base64_to_urdf(urdf_base64);
|
|
|
|
if (!utils::validate_urdf_base64(urdf_base64))
|
|
{
|
|
res_data = {
|
|
{"success", false},
|
|
{"message", "Invalid URDF format"},
|
|
{"timestamp", utils::get_current_time()}};
|
|
}
|
|
else
|
|
{
|
|
auto result = RobotManager::initRobot(urdf_content, uuid, force_update);
|
|
res_data = {
|
|
{"success", result.first},
|
|
{"message", result.second},
|
|
{"timestamp", utils::get_current_time()}};
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to initialize robot: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_GetRobot")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_GetRobot command");
|
|
|
|
std::string uuid = req_param.value("uuid", "");
|
|
auto robot = RobotManager::getRobot(uuid);
|
|
|
|
if (!robot)
|
|
{
|
|
res_data = {{"error", "Robot not found"}};
|
|
}
|
|
else
|
|
{
|
|
res_data = {
|
|
{"success", true},
|
|
{"uuid", uuid},
|
|
{"initialized", robot->isInitialized()},
|
|
{"joints_count", robot->getNumberOfJoints()},
|
|
{"timestamp", utils::get_current_time()}};
|
|
}
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to get robot: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_RemoveRobot")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_RemoveRobot command");
|
|
|
|
std::string uuid = req_param.value("uuid", "");
|
|
auto result = RobotManager::removeRobot(uuid);
|
|
|
|
res_data = {
|
|
{"success", result.first},
|
|
{"message", result.second},
|
|
{"timestamp", utils::get_current_time()}};
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to remove robot: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_ListRobots")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_ListRobots command");
|
|
|
|
bool detail = req_param.value("detail", false);
|
|
auto robots = RobotManager::listRobots(detail);
|
|
|
|
res_data = {
|
|
{"success", true},
|
|
{"robots", robots},
|
|
{"count", robots.size()},
|
|
{"timestamp", utils::get_current_time()}};
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to list robots: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_SelectCraftTree")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_SelectCraftTree command");
|
|
std::string tree_id = req_param.value("tree_id", "");
|
|
res_data = {
|
|
{"success", true},
|
|
{"tree_id", tree_id},
|
|
{"message", "Craft tree selected successfully"},
|
|
{"timestamp", utils::get_current_time()}};
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to select craft tree: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
else if (req_cmd == "Cmd_AddOperationTree")
|
|
{
|
|
try
|
|
{
|
|
log("Handling Cmd_AddOperationTree command");
|
|
std::string tree_name = req_param.value("name", "");
|
|
json operations = req_param.value("operations", json::array());
|
|
|
|
res_data = {
|
|
{"success", true},
|
|
{"tree_name", tree_name},
|
|
{"operations_count", operations.size()},
|
|
{"message", "Operation tree added successfully"},
|
|
{"timestamp", utils::get_current_time()}};
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
res_data = {{"error", "Failed to add operation tree: " + std::string(e.what())}};
|
|
}
|
|
}
|
|
|
|
return res_data;
|
|
}
|