diff --git a/.gitignore b/.gitignore index 26a307e..ed65c8d 100644 --- a/.gitignore +++ b/.gitignore @@ -367,4 +367,5 @@ third_party/flexiv_rdk install/cpp_wrapper install/rdk_install bin*/ -obj*/ \ No newline at end of file +obj*/ +.vscode/ \ No newline at end of file diff --git a/build_and_install_flexiv_rdk_dll.sh b/build_and_install_flexiv_rdk_dll.sh index 9a804a2..b6bcc91 100644 --- a/build_and_install_flexiv_rdk_dll.sh +++ b/build_and_install_flexiv_rdk_dll.sh @@ -3,11 +3,11 @@ CURRSCRIPTPATH="$(dirname $(readlink -f $0))" bash third_party/build_and_install_flexiv_rdk.sh -echo ">>> Build and install flexiv rdk dll v1.5.1" +echo ">>> Build and install flexiv rdk dll v1.7.0" cd $CURRSCRIPTPATH rm -rf build/ cmake -S . -B build cmake --build build --config Release --target install -echo ">>> Installed successfully: flexiv rdk dll v1.5.1" +echo ">>> Installed successfully: flexiv rdk dll v1.7.0" echo ">>> Installed path: $CURRSCRIPTPATH/install" \ No newline at end of file diff --git a/cpp_wrapper/include/wrapper_api.hpp b/cpp_wrapper/include/wrapper_api.hpp index 8839a8f..c27a743 100644 --- a/cpp_wrapper/include/wrapper_api.hpp +++ b/cpp_wrapper/include/wrapper_api.hpp @@ -23,6 +23,9 @@ #include #include #include +#include +#include +#include #include "json.hpp" using json = nlohmann::json; using namespace flexiv::rdk; @@ -55,11 +58,7 @@ struct WRobotState { double tau_des[kSerialJointDoF]; double tau_dot[kSerialJointDoF]; double tau_ext[kSerialJointDoF]; - double q_e[kMaxExtAxes]; - double dq_e[kMaxExtAxes]; - double tau_e[kMaxExtAxes]; double tcp_pose[kPoseSize]; - double tcp_pose_des[kPoseSize]; double tcp_vel[kCartDoF]; double flange_pose[kPoseSize]; double ft_sensor_raw[kCartDoF]; @@ -87,10 +86,27 @@ struct WToolParams { double TcpLocation[kPoseSize]; }; +struct WGripperParams { + char name[256]; + double min_width; + double max_width; + double min_vel; + double max_vel; + double min_force; + double max_force; +}; + struct WGripperStates { double width; double force; - double max_width; + int is_moving; +}; + +struct WSafetyLimits { + double q_min[kSerialJointDoF]; + double q_max[kSerialJointDoF]; + double dq_max_normal[kSerialJointDoF]; + double dq_max_reduced[kSerialJointDoF]; }; void CopyExceptionMsg(const std::exception& e, FlexivError* error) { @@ -112,14 +128,35 @@ void CopyMsgSrc2Dst(const char* src, char* dst, size_t dstSize) { } namespace flexiv::rdk { + NLOHMANN_JSON_SERIALIZE_ENUM(RobotEvent::Level, { + { RobotEvent::Level::UNKNOWN, "UNKNOWN" }, + { RobotEvent::Level::INFO, "INFO" }, + { RobotEvent::Level::WARNING, "WARNING" }, + { RobotEvent::Level::ERROR, "ERROR" }, + { RobotEvent::Level::CRITICAL, "CRITICAL" }, + }) + + inline void to_json(json& j, const RobotEvent& event) { + using namespace std::chrono; + j = json{ + {"Level", event.level}, + {"ID", event.id}, + {"Description", event.description}, + {"Consequence", event.consequences}, + {"ProbableCause", event.probable_causes}, + {"RecommendedActions", event.recommended_actions}, + {"Timestamp", duration_cast(event.timestamp.time_since_epoch()).count()} + }; + } + inline void from_json(const json& j, JPos& pos) { - j.at("q").get_to(pos.q); + j.at("q_m").get_to(pos.q_m); j.at("q_e").get_to(pos.q_e); } inline void to_json(json& j, const JPos& pos) { j = json{ - {"q", pos.q}, + {"q_m", pos.q_m}, {"q_e", pos.q_e} }; } @@ -128,7 +165,7 @@ namespace flexiv::rdk { j.at("position").get_to(coord.position); j.at("orientation").get_to(coord.orientation); j.at("ref_frame").get_to(coord.ref_frame); - j.at("ref_q").get_to(coord.ref_q); + j.at("ref_q_m").get_to(coord.ref_q_m); j.at("ref_q_e").get_to(coord.ref_q_e); } @@ -137,7 +174,7 @@ namespace flexiv::rdk { {"position", coord.position}, {"orientation", coord.orientation}, {"ref_frame", coord.ref_frame}, - {"ref_q", coord.ref_q}, + {"ref_q_m", coord.ref_q_m}, {"ref_q_e", coord.ref_q_e} }; } diff --git a/cpp_wrapper/src/wrapper_api.cpp b/cpp_wrapper/src/wrapper_api.cpp index e8e8555..e8f81d1 100644 --- a/cpp_wrapper/src/wrapper_api.cpp +++ b/cpp_wrapper/src/wrapper_api.cpp @@ -17,13 +17,13 @@ EXPORT_API void SpdlogError(const char* msgs) { } EXPORT_API Robot* CreateFlexivRobot(const char* robot_sn, - const char** interfaces, int interface_count, FlexivError* error) { + const char** interfaces, int interface_count, int verbose, FlexivError* error) { std::vector white_list; for (int i = 0; i < interface_count; ++i) { white_list.push_back(interfaces[i]); } try { - flexiv::rdk::Robot* robot = new flexiv::rdk::Robot(robot_sn, white_list); + flexiv::rdk::Robot* robot = new flexiv::rdk::Robot(robot_sn, white_list, verbose); error->error_code = 0; return robot; } @@ -75,7 +75,8 @@ EXPORT_API int GetMode(Robot* robot) { case Mode::NRT_PRIMITIVE_EXECUTION: return 8; case Mode::RT_CARTESIAN_MOTION_FORCE: return 9; case Mode::NRT_CARTESIAN_MOTION_FORCE: return 10; - case Mode::MODES_CNT: return 11; + case Mode::NRT_SUPER_PRIMITIVE: return 11; + case Mode::MODES_CNT: return 12; default: return 0; } } @@ -92,15 +93,8 @@ EXPORT_API void GetStates(Robot* robot, WRobotState* robot_state) { robot_state->tau_dot[i] = states.tau_dot[i]; robot_state->tau_ext[i] = states.tau_ext[i]; } - size_t count = std::min(states.q_e.size(), kMaxExtAxes); - std::copy_n(states.q_e.begin(), count, robot_state->q_e); - count = std::min(states.dq_e.size(), kMaxExtAxes); - std::copy_n(states.dq_e.begin(), count, robot_state->dq_e); - count = std::min(states.tau_e.size(), kMaxExtAxes); - std::copy_n(states.tau_e.begin(), count, robot_state->tau_e); for (int i = 0; i < kPoseSize; ++i) { robot_state->tcp_pose[i] = states.tcp_pose[i]; - robot_state->tcp_pose_des[i] = states.tcp_pose_des[i]; robot_state->flange_pose[i] = states.flange_pose[i]; } for (int i = 0; i < kCartDoF; ++i) { @@ -117,8 +111,8 @@ EXPORT_API int IsStopped(Robot* robot) { return robot->stopped(); } -EXPORT_API int IsOperational(Robot* robot, int verbose) { - return robot->operational(verbose); +EXPORT_API int IsOperational(Robot* robot) { + return robot->operational(); } EXPORT_API int GetOperationalStatus(Robot* robot) { @@ -163,12 +157,10 @@ EXPORT_API int IsEnablingButtonReleased(Robot* robot) { return robot->enabling_button_pressed(); } -EXPORT_API char* GetMuLog(Robot* robot) { - const auto& mu = robot->mu_log(); - std::map tmp; - tmp["mu_log"] = mu; - std::string json_str = serializeParams(tmp); - return CopyInputString(json_str.c_str()); +EXPORT_API char* GetEventLog(Robot* robot) { + nlohmann::json j = robot->event_log(); + std::string str = j.dump(); + return CopyInputString(str.c_str()); } //======================================= SYSTEM CONTROL ======================================= @@ -195,11 +187,11 @@ EXPORT_API void Brake(Robot* robot, int engage, FlexivError* error) { } EXPORT_API void SwitchMode(Robot* robot, int mode, FlexivError* error) { - static const std::array modeMap = { + static const std::array modeMap = { Mode::UNKNOWN, Mode::IDLE, Mode::RT_JOINT_TORQUE, Mode::RT_JOINT_IMPEDANCE, Mode::NRT_JOINT_IMPEDANCE, Mode::RT_JOINT_POSITION, Mode::NRT_JOINT_POSITION, Mode::NRT_PLAN_EXECUTION, Mode::NRT_PRIMITIVE_EXECUTION, Mode::RT_CARTESIAN_MOTION_FORCE, - Mode::NRT_CARTESIAN_MOTION_FORCE, Mode::MODES_CNT }; + Mode::NRT_CARTESIAN_MOTION_FORCE, Mode::NRT_SUPER_PRIMITIVE, Mode::MODES_CNT }; if (mode < 0 || mode >= static_cast(modeMap.size())) { error->error_code = 1; CopyExceptionMsg(std::runtime_error("Invalid robot mode index"), error); @@ -268,6 +260,7 @@ EXPORT_API char* GetGlobalVariables(Robot* robot, FlexivError* error) { try { const auto& globalVars = robot->global_variables(); std::string json_str = serializeParams(globalVars); + error->error_code = 0; return CopyInputString(json_str.c_str()); } catch (const std::exception& e) { @@ -277,6 +270,17 @@ EXPORT_API char* GetGlobalVariables(Robot* robot, FlexivError* error) { } } +EXPORT_API void LockExternalAxes(Robot* robot, int toggle, FlexivError* error) { + try { + robot->LockExternalAxes(toggle); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + //======================================= PLAN EXECUTION ======================================= EXPORT_API void ExecutePlanByIdx(Robot* robot, int idx, int continueExec, int blockUntilStarted, FlexivError* error) { @@ -374,13 +378,11 @@ EXPORT_API void StepBreakpoint(Robot* robot, FlexivError* error) { EXPORT_API void ExecutePrimitive(Robot* robot, const char* primitiveName, const char* inputParams, - const char* properties, int blockUntilStarted, FlexivError* error) { try { auto input_params = parseJsonToParams(inputParams); - auto props = parseJsonToParams(properties); - robot->ExecutePrimitive(std::string(primitiveName), input_params, props, blockUntilStarted); + robot->ExecutePrimitive(std::string(primitiveName), input_params, blockUntilStarted); error->error_code = 0; } catch (const std::exception& e) { @@ -393,6 +395,7 @@ EXPORT_API char* GetPrimitiveStates(Robot* robot, FlexivError* error) { try { const auto& primitiveStates = robot->primitive_states(); std::string json_str = serializeParams(primitiveStates); + error->error_code = 0; return CopyInputString(json_str.c_str()); } catch (const std::exception& e) { @@ -639,9 +642,9 @@ EXPORT_API void SetPassiveForceControl(Robot* robot, int IsEnabled, FlexivError* EXPORT_API void SetDigitalOutput(Robot* robot, int idx, int value, FlexivError* error) { try { unsigned int port = idx; - std::vector port_idx{ port }; - std::vector values{ value == 1 }; - robot->SetDigitalOutputs(port_idx, values); + std::map outputs; + outputs[idx] = value; + robot->SetDigitalOutputs(outputs); error->error_code = 0; } catch (const std::exception& e) { @@ -801,6 +804,21 @@ EXPORT_API void RemoveTool(Tool* tool, const char* name, FlexivError* error) { } } +EXPORT_API void CalibratePayloadParams(Tool* tool, int toolMounted, WToolParams* toolParams, FlexivError* error) { + try { + const auto& params = tool->CalibratePayloadParams(toolMounted); + error->error_code = 0; + toolParams->Mass = params.mass; + for (int i = 0; i < 3; ++i) toolParams->CoM[i] = params.CoM[i]; + for (int i = 0; i < 6; ++i) toolParams->Intertia[i] = params.inertia[i]; + for (int i = 0; i < kPoseSize; ++i) toolParams->TcpLocation[i] = params.tcp_location[i]; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + //======================================= WORK COORD ========================================= EXPORT_API WorkCoord* CreateWorkCoord(Robot* robot, FlexivError* error) { try { @@ -928,6 +946,28 @@ EXPORT_API void DeleteGripper(Gripper* gripper) { delete gripper; } +EXPORT_API void EnableGripper(Gripper* gripper, const char* name, FlexivError* error) { + try { + gripper->Enable(name); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void DisableGripper(Gripper* gripper, FlexivError* error) { + try { + gripper->Disable(); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + EXPORT_API void Init(Gripper* gripper, FlexivError* error) { try { gripper->Init(); @@ -965,15 +1005,22 @@ EXPORT_API void StopGripper(Gripper* gripper) { gripper->Stop(); } -EXPORT_API int GripperIsMoving(Gripper* gripper) { - return gripper->moving(); +EXPORT_API void GetGripperParams(Gripper* gripper, WGripperParams* param) { + const auto& gp = gripper->params(); + CopyMsgSrc2Dst(gp.name.c_str(), param->name, sizeof(param->name)); + param->min_width = gp.min_width; + param->max_width = gp.max_width; + param->min_vel = gp.min_vel; + param->max_vel = gp.max_vel; + param->min_force = gp.min_force; + param->max_force = gp.max_force; } EXPORT_API void GetGripperStates(Gripper* gripper, WGripperStates* states) { const auto& st = gripper->states(); states->width = st.width; states->force = st.force; - states->max_width = st.max_width; + states->is_moving = st.is_moving; } //======================================== FILE IO =========================================== @@ -994,6 +1041,20 @@ EXPORT_API void DeleteFileIO(FileIO* fileIO) { delete fileIO; } +EXPORT_API char* GetTrajFilesList(FileIO* fileIO, FlexivError* error) { + try { + json j = fileIO->traj_files_list(); + std::string str = j.dump(); + error->error_code = 0; + return CopyInputString(str.c_str()); + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } + return nullptr; +} + EXPORT_API void UploadTrajFile(FileIO* fileIO, const char* fileDir, const char* fileName, FlexivError* error) { try { fileIO->UploadTrajFile(fileDir, fileName); @@ -1005,6 +1066,30 @@ EXPORT_API void UploadTrajFile(FileIO* fileIO, const char* fileDir, const char* } } +EXPORT_API char* DownloadTrajFile(FileIO* fileIO, const char* fileName, FlexivError* error) { + try { + std::string str = fileIO->DownloadTrajFile(fileName); + error->error_code = 0; + return CopyInputString(str.c_str()); + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } + return nullptr; +} + +EXPORT_API void DownloadTrajFile2(FileIO* fileIO, const char* fileName, const char* saveDir, FlexivError* error) { + try { + fileIO->DownloadTrajFile(fileName, saveDir); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + //========================================= DEVICE =========================================== EXPORT_API Device* CreateDevice(Robot* robot, FlexivError* error) { try { @@ -1049,6 +1134,26 @@ EXPORT_API int HasDevice(Device* device, const char* name, FlexivError* error) { } } +EXPORT_API char* GetDeviceParams(Device* device, const char* name, FlexivError* error) { + try { + const auto& deviceParams = device->params(name); + std::map ret; + for (const auto& [key, value] : deviceParams) { + std::visit([&](auto&& arg) { + ret[key] = arg; + }, value); + } + std::string json_str = serializeParams(ret); + error->error_code = 0; + return CopyInputString(json_str.c_str()); + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + return nullptr; + } +} + EXPORT_API void EnableDevice(Device* device, const char* name, FlexivError* error) { try { device->Enable(name); @@ -1074,11 +1179,14 @@ EXPORT_API void DisableDevice(Device* device, const char* name, FlexivError* err EXPORT_API void SendCommands(Device* device, const char* name, const char* cmds, FlexivError* error) { try { json json_obj = json::parse(cmds); - std::map> device_cmds; + std::map> device_cmds; for (auto& [key, value] : json_obj.items()) { if (value.is_number_integer()) { device_cmds[key] = value.get(); } + else if (value.is_boolean()) { + device_cmds[key] = value.get(); + } else if (value.is_number_float()) { device_cmds[key] = value.get(); } @@ -1090,4 +1198,271 @@ EXPORT_API void SendCommands(Device* device, const char* name, const char* cmds, error->error_code = 1; CopyExceptionMsg(e, error); } +} + +//======================================= MAINTENANCE ======================================== +EXPORT_API Maintenance* CreateMaintenance(Robot* robot, FlexivError* error) { + try { + Maintenance* maintenancePtr = new Maintenance(*robot); + error->error_code = 0; + return maintenancePtr; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } + return nullptr; +} + +EXPORT_API void DeleteMaintenance(Maintenance* maintenancePtr) { + delete maintenancePtr; +} + +EXPORT_API void CalibrateJointTorqueSensors(Maintenance* maintenancePtr, const double* posture, int postureLen, FlexivError* error) { + try { + std::vector cali_posture(posture, posture + postureLen); + maintenancePtr->CalibrateJointTorqueSensors(cali_posture); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +//========================================= SAFETY ============================================ +EXPORT_API Safety* CreateSafety(Robot* robot, const char* password, FlexivError* error) { + try { + Safety* safetyPtr = new Safety(*robot, std::string(password)); + error->error_code = 0; + return safetyPtr; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } + return nullptr; +} + +EXPORT_API void DeleteSafety(Safety* safetyPtr) { + delete safetyPtr; +} + +EXPORT_API void DefaultLimits(Safety* safetyPtr, WSafetyLimits* limits) { + const auto& val = safetyPtr->default_limits(); + for (int i = 0; i < kSerialJointDoF; ++i) { + limits->q_min[i] = val.q_min[i]; + limits->q_max[i] = val.q_max[i]; + limits->dq_max_normal[i] = val.dq_max_normal[i]; + limits->dq_max_reduced[i] = val.dq_max_reduced[i]; + } +} + +EXPORT_API void CurrentLimits(Safety* safetyPtr, WSafetyLimits* limits) { + const auto& val = safetyPtr->current_limits(); + for (int i = 0; i < kSerialJointDoF; ++i) { + limits->q_min[i] = val.q_min[i]; + limits->q_max[i] = val.q_max[i]; + limits->dq_max_normal[i] = val.dq_max_normal[i]; + limits->dq_max_reduced[i] = val.dq_max_reduced[i]; + } +} + +EXPORT_API void GetSafetyInputs(Safety* safetyPtr, int* temp) { + const auto& val = safetyPtr->safety_inputs(); + for (int i = 0; i < kSafetyIOPorts; ++i) { + temp[i] = val[i]; + } +} + +EXPORT_API void SetJointPositionLimits(Safety* safetyPtr, double* minPositions, + int minLen, double* maxPositions, int maxLen, FlexivError* error) { + try { + std::vector minPoses(minPositions, minPositions + minLen); + std::vector maxPoses(maxPositions, maxPositions + maxLen); + safetyPtr->SetJointPositionLimits(minPoses, maxPoses); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void SetJointVelocityNormalLimits(Safety* safetyPtr, double* maxVel, + int len, FlexivError* error) { + try { + std::vector maxVel(maxVel, maxVel + len); + safetyPtr->SetJointVelocityNormalLimits(maxVel); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void SetJointVelocityReducedLimits(Safety* safetyPtr, double* maxVel, + int len, FlexivError* error) { + try { + std::vector maxVel(maxVel, maxVel + len); + safetyPtr->SetJointVelocityReducedLimits(maxVel); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +//========================================= MODEL =========================================== +EXPORT_API Model* CreateModel(Robot* robot, double gravityX, double gravityY, + double gravityZ, FlexivError* error) { + try { + Model* model = new Model(*robot, Eigen::Vector3d(gravityX, gravityY, gravityZ)); + error->error_code = 0; + return model; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } + return nullptr; +} + +EXPORT_API void DeleteModel(Model* model) { + delete model; +} + +EXPORT_API void Reload(Model* model, FlexivError* error) { + try { + model->Reload(); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void Update(Model* model, double* pos, int posLen, double* vel, int velLen, FlexivError* error) { + try { + std::vector positions(pos, pos + posLen); + std::vector velocities(vel, vel + velLen); + model->Update(positions, velocities); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void GetJacobian(Model* model, const char* linkName, double* buffer, int rows, int cols, FlexivError* error) { + try { + if (!model || !buffer || !linkName) return; + Eigen::MatrixXd J = model->J(std::string(linkName)); + if (J.rows() != rows || J.cols() != cols) return; + std::memcpy(buffer, J.data(), sizeof(double) * rows * rows); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void GetJacobianDot(Model* model, const char* linkName, double* buffer, int rows, int cols, FlexivError* error) { + try { + if (!model || !buffer || !linkName) return; + Eigen::MatrixXd dJ = model->dJ(std::string(linkName)); + if (dJ.rows() != rows || dJ.cols() != cols) return; + std::memcpy(buffer, dJ.data(), sizeof(double) * rows * rows); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void GetMassMatrix(Model* model, double* buffer, int dof, FlexivError* error) { + try { + if (!model || !buffer) return; + Eigen::MatrixXd mass = model->M(); + if (mass.rows() != dof || mass.cols() != dof) return; + std::memcpy(buffer, mass.data(), sizeof(double) * dof * dof); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void GetCoriolisCentripetalMatrix(Model* model, double* buffer, int dof, FlexivError* error) { + try { + if (!model || !buffer) return; + Eigen::MatrixXd coriolis = model->C(); + if (coriolis.rows() != dof || coriolis.cols() != dof) return; + std::memcpy(buffer, coriolis.data(), sizeof(double) * dof * dof); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void GetGravityForceVector(Model* model, double* buffer, int dof) { + auto g = model->g(); + if (g.size() != dof) return; + std::memcpy(buffer, g.data(), sizeof(double) * dof); +} + +EXPORT_API void GetCoriolisForceVector(Model* model, double* buffer, int dof) { + auto c = model->c(); + if (c.size() != dof) return; + std::memcpy(buffer, c.data(), sizeof(double) * dof); +} + +EXPORT_API void SyncURDF(Model* model, char* path, FlexivError* error) { + try { + model->SyncURDF(std::string(path)); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void Reachable(Model* model, double* pose, int poseLen, double* seed, int seedLen, + int freeOri, int* reachable, double* ikSolution, FlexivError* error) { + try { + std::array poseArr; + for (int i = 0; i < kPoseSize; ++i) poseArr[i] = pose[i]; + std::vector seedVec(seed, seed + seedLen); + auto result = model->reachable(poseArr, seedVec, freeOri != 0); + *reachable = result.first ? 1 : 0; + for (int i = 0; i < kPoseSize; ++i) + ikSolution[i] = result.second[i]; + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void ConfigurationScore(Model* model, double* tScore, double* oScore, FlexivError* error) { + try { + auto score = model->configuration_score(); + *tScore = score.first; + *oScore = score.second; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } } \ No newline at end of file diff --git a/csharp/Examples/Basics1DisplayRobotStates.cs b/csharp/Examples/Basics1DisplayRobotStates.cs index 1952632..0dac570 100644 --- a/csharp/Examples/Basics1DisplayRobotStates.cs +++ b/csharp/Examples/Basics1DisplayRobotStates.cs @@ -1,8 +1,8 @@ using System; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics1DisplayRobotStates : IExample { @@ -25,7 +25,7 @@ static void PrintRobotStates(Robot robot) while (true) { Utility.SpdlogInfo("Current robot states:"); - Console.WriteLine(robot.GetStates().ToString()); + Console.WriteLine(robot.states().ToString()); Thread.Sleep(1000); } } @@ -45,7 +45,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -60,7 +60,7 @@ public void Run(string[] args) Utility.SpdlogInfo("Enabling robot ..."); robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } diff --git a/csharp/Examples/Basics2ClearFault.cs b/csharp/Examples/Basics2ClearFault.cs index fb64986..1d0775c 100644 --- a/csharp/Examples/Basics2ClearFault.cs +++ b/csharp/Examples/Basics2ClearFault.cs @@ -1,7 +1,7 @@ using System; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics2ClearFault : IExample { @@ -34,7 +34,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault diff --git a/csharp/Examples/Basics3PrimitiveExecution.cs b/csharp/Examples/Basics3PrimitiveExecution.cs index 13add3c..cac4216 100644 --- a/csharp/Examples/Basics3PrimitiveExecution.cs +++ b/csharp/Examples/Basics3PrimitiveExecution.cs @@ -1,9 +1,9 @@ using System; using System.Threading; using System.Collections.Generic; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics3PrimitiveExecution : IExample { @@ -37,7 +37,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -52,7 +52,7 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } @@ -61,36 +61,34 @@ public void Run(string[] args) robot.SwitchMode(RobotMode.NRT_PRIMITIVE_EXECUTION); // (1) Go to home pose Utility.SpdlogInfo("Executing primitive: Home"); - robot.ExecutePrimitive("Home", new Dictionary()); + robot.ExecutePrimitive("Home", new Dictionary()); // Wait for reached target - while (!(FlexivDataUtils.TryGet(robot.GetPrimitiveStates(), + while (!(FlexivDataTypesUtils.TryGet(robot.primitive_states(), "reachedTarget", out var flag) && flag == 1)) { Thread.Sleep(1000); } // (2) Move robot joints to target positions Utility.SpdlogInfo("Executing primitive: MoveJ"); - robot.ExecutePrimitive("MoveJ", new Dictionary { + robot.ExecutePrimitive("MoveJ", new Dictionary { // unit: deg {"target", new JPos(30, -45, 0, 90, 0, 40, 30, -50, 30) }, {"waypoints",new List { new JPos(10, -30, 10, 30, 10, 15, 10, -15, 10), new JPos(20, -60, -10, 60, -10, 30, 20, -30, 20) } }, - }, new Dictionary { - {"lockExternalAxes", 0 } }); // Wait for reached target - while (!(FlexivDataUtils.TryGet(robot.GetPrimitiveStates(), + while (!(FlexivDataTypesUtils.TryGet(robot.primitive_states(), "reachedTarget", out var flag) && flag == 1)) { Utility.SpdlogInfo("Current primitive states:"); - Console.WriteLine(Utility.FlexivDataDictToString(robot.GetPrimitiveStates())); + Console.WriteLine(Utility.FlexivDataTypesDictToString(robot.primitive_states())); Thread.Sleep(1000); } // (3) Move robot TCP to a target pose in world (base) frame Utility.SpdlogInfo("Executing primitive: MoveL"); - robot.ExecutePrimitive("MoveL", new Dictionary { + robot.ExecutePrimitive("MoveL", new Dictionary { {"target", new Coord(0.65, -0.3, 0.2, 180, 0, 180) }, // unit: m-deg {"waypoints", new List { @@ -102,21 +100,21 @@ public void Run(string[] args) {"vel", 0.6 }, {"zoneRadius", "Z50" } }); - while (!(FlexivDataUtils.TryGet(robot.GetPrimitiveStates(), + while (!(FlexivDataTypesUtils.TryGet(robot.primitive_states(), "reachedTarget", out var flag) && flag == 1)) // Wait for reached target { Thread.Sleep(1000); } // (4) Another MoveL that uses TCP frame Utility.SpdlogInfo("Executing primitive: MoveL"); - robot.ExecutePrimitive("MoveL", new Dictionary { + robot.ExecutePrimitive("MoveL", new Dictionary { // x y z qw qx qy qz {"target", new Coord(0.0, 0.0, 0.0, 0.9185587, 0.1767767, 0.3061862, 0.1767767, "TRAJ", "START") }, {"vel", 0.2 } }); // Wait for reached target - while (!(FlexivDataUtils.TryGet(robot.GetPrimitiveStates(), + while (!(FlexivDataTypesUtils.TryGet(robot.primitive_states(), "reachedTarget", out var flag) && flag == 1)) { Thread.Sleep(1000); diff --git a/csharp/Examples/Basics4PlanExecution.cs b/csharp/Examples/Basics4PlanExecution.cs index 0eeede4..ec334b1 100644 --- a/csharp/Examples/Basics4PlanExecution.cs +++ b/csharp/Examples/Basics4PlanExecution.cs @@ -1,8 +1,8 @@ using System; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics4PlanExecution : IExample { @@ -38,7 +38,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -53,14 +53,14 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } Utility.SpdlogInfo("Robot is now operational"); // Switch to plan execution mode robot.SwitchMode(RobotMode.NRT_PLAN_EXECUTION); - while (!robot.IsFault()) + while (!robot.fault()) { Utility.SpdlogInfo("Choose an action:"); Console.WriteLine(" [1] Show available plans"); @@ -83,7 +83,7 @@ public void Run(string[] args) switch (choice) { case 1: - var plan_list = robot.GetPlanList(); + var plan_list = robot.plan_list(); for (int i = 0; i < plan_list.Count; i++) { Console.WriteLine($"[{i}] {plan_list[i]}"); @@ -95,10 +95,10 @@ public void Run(string[] args) isValid = int.TryParse(user_input, out choice); if (!isValid) break; robot.ExecutePlan(choice, true); - while (robot.IsBusy()) + while (robot.busy()) { Utility.SpdlogInfo("Current plan info:"); - Console.WriteLine(robot.GetPlanInfo().ToString()); + Console.WriteLine(robot.plan_info().ToString()); Thread.Sleep(1000); } break; @@ -106,10 +106,10 @@ public void Run(string[] args) Console.WriteLine("Enter plan name to execute:"); user_input = Console.ReadLine(); robot.ExecutePlan(user_input, true); - while (robot.IsBusy()) + while (robot.busy()) { Utility.SpdlogInfo("Current plan info:"); - Console.WriteLine(robot.GetPlanInfo().ToString()); + Console.WriteLine(robot.plan_info().ToString()); Thread.Sleep(1000); } break; diff --git a/csharp/Examples/Basics5ZeroFTSensor.cs b/csharp/Examples/Basics5ZeroFTSensor.cs index d44b989..1670576 100644 --- a/csharp/Examples/Basics5ZeroFTSensor.cs +++ b/csharp/Examples/Basics5ZeroFTSensor.cs @@ -1,10 +1,10 @@ using System; using System.Threading; using System.Collections.Generic; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; using System.Linq; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics5ZeroFTSensor : IExample { @@ -38,7 +38,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -53,26 +53,27 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } Utility.SpdlogInfo($"TCP force and moment reading in base frame BEFORE sensor zeroing: " + - $"{string.Join(", ", robot.GetStates().ExtWrenchInWorld.Select(x => x.ToString("F3")))} N-Nm"); + $"{string.Join(", ", robot.states().ExtWrenchInWorld.Select(x => x.ToString("F3")))} N-Nm"); // Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors robot.SwitchMode(RobotMode.NRT_PRIMITIVE_EXECUTION); - robot.ExecutePrimitive("ZeroFTSensor", new Dictionary()); + robot.ExecutePrimitive("ZeroFTSensor", new Dictionary()); // WARNING: during the process, the robot must not contact anything, otherwise the result // will be inaccurate and affect following operations Utility.SpdlogWarn("Zeroing force/torque sensors, make sure nothing is in contact with the robot"); // Wait for the primitive completion - while (robot.IsBusy()) + while (!(FlexivDataTypesUtils.TryGet(robot.primitive_states(), + "terminated", out var flag) && flag == 1)) { Thread.Sleep(1000); } Utility.SpdlogInfo("Sensor zeroing complete"); Utility.SpdlogInfo($"TCP force and moment reading in base frame BEFORE sensor zeroing: " + - $"{string.Join(", ", robot.GetStates().ExtWrenchInWorld.Select(x => x.ToString("F3")))} N-Nm"); + $"{string.Join(", ", robot.states().ExtWrenchInWorld.Select(x => x.ToString("F3")))} N-Nm"); } catch (Exception ex) { diff --git a/csharp/Examples/Basics6GripperControl.cs b/csharp/Examples/Basics6GripperControl.cs index 3839522..65ecd18 100644 --- a/csharp/Examples/Basics6GripperControl.cs +++ b/csharp/Examples/Basics6GripperControl.cs @@ -1,8 +1,8 @@ using System; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics6GripperControl : IExample { @@ -17,6 +17,8 @@ Execute several basic robot primitives (unit skills). Required arguments: Serial number of the robot to connect to. Remove any space. For example: Rizon4s-123456 + Full name of the gripper to be controlled, + can be found in Flexiv Elements -> Settings -> Device Optional arguments: (none) "; @@ -26,11 +28,10 @@ static void PrintGripperStates(Gripper gripper) while (Interlocked.CompareExchange(ref _stopFlag, 0, 0) == 0) { Utility.SpdlogInfo("Current gripper states:"); - var states = gripper.GetGripperStates(); + var states = gripper.states(); Console.WriteLine($" width: {Math.Round(states.Width, 2)}"); Console.WriteLine($" force: {Math.Round(states.Force, 2)}"); - Console.WriteLine($" max_width: {Math.Round(states.MaxWidth, 2)}"); - Console.WriteLine($" moving: {gripper.IsMoving()}"); + Console.WriteLine($" moving: {states.IsMoving}"); Console.WriteLine(); Thread.Sleep(1000); } @@ -45,12 +46,13 @@ public void Run(string[] args) Utility.SpdlogInfo(">>> Tutorial description <<<\nThis tutorial does position and force (if available) " + "control of grippers supported by Flexiv.\n"); string robotSN = args[0]; + string gripperName = args[1]; try { // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -65,19 +67,58 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } Utility.SpdlogInfo("Robot is now operational"); - // Gripper control is not available if the robot is in IDLE mode, so switch to some mode - robot.SwitchMode(RobotMode.NRT_PLAN_EXECUTION); - robot.ExecutePlan("PLAN-Home"); - Thread.Sleep(1000); var gripper = new Gripper(robot); - Utility.SpdlogInfo("Initializing gripper, this process takes about 10 seconds ..."); - gripper.Init(); - Utility.SpdlogInfo("Initialization complete"); + var tool = new Tool(robot); + Utility.SpdlogInfo($"Enabling gripper [{gripperName}]"); + gripper.Enable(gripperName); + + // Print parameters of the enabled gripper + Utility.SpdlogInfo("Gripper params:"); + Console.WriteLine("{"); + Console.WriteLine($"name: {gripper.GetParams().Name}"); + Console.WriteLine($"min_width: {gripper.GetParams().MinWidth:F3}"); + Console.WriteLine($"max_width: {gripper.GetParams().MaxWidth:F3}"); + Console.WriteLine($"min_force: {gripper.GetParams().MinForce:F3}"); + Console.WriteLine($"max_force: {gripper.GetParams().MaxForce:F3}"); + Console.WriteLine($"min_vel: {gripper.GetParams().MinVel:F3}"); + Console.WriteLine($"max_vel: {gripper.GetParams().MaxVel:F3}"); + Console.WriteLine("}"); + + // Switch robot tool to gripper so the gravity compensation and TCP location is updated + Utility.SpdlogInfo($"Switching robot tool to [{gripperName}]"); + tool.Switch(gripperName); + // User needs to determine if this gripper requires manual initialization + Utility.SpdlogInfo($"Manually trigger initialization for the gripper now? Choose Yes if it's a 48v Grav gripper"); + Console.WriteLine("[1] No, it has already initialized automatically when power on"); + Console.WriteLine("[2] Yes, it does not initialize itself when power on"); + if (!int.TryParse(Console.ReadLine(), out int choice)) + { + Utility.SpdlogError("Invalid input"); + return; + } + + // Trigger manual initialization based on choice + if (choice == 1) + { + Utility.SpdlogInfo("Skipped manual initialization"); + } + else if (choice == 2) + { + gripper.Init(); + Utility.SpdlogInfo("Triggered manual initialization, press Enter when the initialization is finished to continue"); + Console.ReadLine(); + } + else + { + Utility.SpdlogError("Invalid choice"); + return; + } + // Start a separate thread to print gripper states Thread printThread = new Thread(() => PrintGripperStates(gripper)); printThread.Start(); // Loop until the printThread activates @@ -107,7 +148,7 @@ public void Run(string[] args) gripper.Stop(); Thread.Sleep(2000); // Force control, if available (sensed force is not zero) - if (Math.Abs(gripper.GetGripperStates().Force) > double.Epsilon) + if (Math.Abs(gripper.states().Force) > double.Epsilon) { Utility.SpdlogInfo("Gripper running zero force control"); gripper.Grasp(0); diff --git a/csharp/Examples/Basics7AutoRecovery.cs b/csharp/Examples/Basics7AutoRecovery.cs index 7bed798..b769d4c 100644 --- a/csharp/Examples/Basics7AutoRecovery.cs +++ b/csharp/Examples/Basics7AutoRecovery.cs @@ -1,8 +1,8 @@ using System; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics7AutoRecovery : IExample { @@ -45,7 +45,7 @@ public void Run(string[] args) Thread.Sleep(8000); // Run automatic recovery if the system is in recovery state, the involved joints will starts // to move back into allowed position range - if (robot.IsRecovery()) + if (robot.recovery()) robot.RunAutoRecovery(); // Otherwise the system is normal, do nothing else diff --git a/csharp/Examples/Basics8UpdateRobotTool.cs b/csharp/Examples/Basics8UpdateRobotTool.cs index 3ffb4c1..b56cd13 100644 --- a/csharp/Examples/Basics8UpdateRobotTool.cs +++ b/csharp/Examples/Basics8UpdateRobotTool.cs @@ -1,8 +1,8 @@ using System; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics8UpdateRobotTool : IExample { @@ -36,7 +36,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -51,7 +51,7 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } @@ -62,14 +62,14 @@ public void Run(string[] args) var tool = new Tool(robot); // Get and print a list of already configured tools currently in the robot's tools pool Utility.SpdlogInfo("All configured tools:"); - var toolList = tool.GetToolNames(); + var toolList = tool.list(); for (int i = 0; i < toolList.Count; i++) { Console.WriteLine($"[{i}] {toolList[i]}"); } Console.WriteLine(); // Get and print the current active tool - Utility.SpdlogInfo($"Current active tool: [{tool.GetToolName()}]"); + Utility.SpdlogInfo($"Current active tool: [{tool.name()}]"); // Set name and parameters for a new tool string newToolName = "ExampleTool1"; ToolParams newToolParams = new(); @@ -79,19 +79,19 @@ public void Run(string[] args) newToolParams.TcpLocation = new double[] { 0.0, -0.207, 0.09, 0.7071068, 0.7071068, 0.0, 0.0 }; // If there's already a tool with the same name in the robot's tools pool, then remove it // first, because duplicate tool names are not allowed - if (tool.HasTool(newToolName)) + if (tool.exist(newToolName)) { Utility.SpdlogWarn($"Tool with the same name [{newToolName}] already exists, removing it now"); // Switch to other tool or no tool (Flange) before removing the current tool - tool.SwitchTool("Flange"); - tool.RemoveTool(newToolName); + tool.Switch("Flange"); + tool.Remove(newToolName); } // Add the new tool Utility.SpdlogInfo($"Adding new tool [{newToolName}] to the robot"); - tool.AddNewTool(newToolName, newToolParams); + tool.Add(newToolName, newToolParams); // Get and print the tools list again, the new tool should appear at the end Utility.SpdlogInfo("All configured tools:"); - toolList = tool.GetToolNames(); + toolList = tool.list(); for (int i = 0; i < toolList.Count; i++) { Console.WriteLine($"[{i}] {toolList[i]}"); @@ -99,15 +99,15 @@ public void Run(string[] args) Console.WriteLine(); // Switch to the newly added tool, i.e. set it as the active tool Utility.SpdlogInfo($"Switch to tool [{newToolName}]"); - tool.SwitchTool(newToolName); + tool.Switch(newToolName); // Get and print the current active tool again, should be the new tool - Utility.SpdlogInfo($"Current active tool: [{tool.GetToolName()}]"); + Utility.SpdlogInfo($"Current active tool: [{tool.name()}]"); // Switch to other tool or no tool (Flange) before removing the current tool - tool.SwitchTool("Flange"); + tool.Switch("Flange"); // Clean up by removing the new tool Thread.Sleep(2000); Utility.SpdlogInfo($"Removing tool [{newToolName}]"); - tool.RemoveTool(newToolName); + tool.Remove(newToolName); Utility.SpdlogInfo("Program finished"); } catch (Exception ex) diff --git a/csharp/Examples/Basics9GlobalVariables.cs b/csharp/Examples/Basics9GlobalVariables.cs index cfea58d..3b61ab7 100644 --- a/csharp/Examples/Basics9GlobalVariables.cs +++ b/csharp/Examples/Basics9GlobalVariables.cs @@ -1,9 +1,9 @@ using System; using System.Collections.Generic; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Basics9GlobalVariables : IExample { @@ -35,7 +35,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -50,13 +50,13 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } Utility.SpdlogInfo("Robot is now operational"); // Get existing global variables - var globalVars = robot.GetGlobalVariables(); + var globalVars = robot.global_variables(); if (globalVars.Count == 0) { Utility.SpdlogWarn("No global variables available"); @@ -65,12 +65,12 @@ public void Run(string[] args) else { Utility.SpdlogInfo("Existing global variables and their original values:"); - Console.WriteLine(Utility.FlexivDataDictToString(globalVars)); + Console.WriteLine(Utility.FlexivDataTypesDictToString(globalVars)); } // Set global variables // WARNING: These specified global variables need to be created first using Flexiv Elements Utility.SpdlogInfo("Setting new values to existing global variables"); - robot.SetGlobalVariables(new Dictionary { + robot.SetGlobalVariables(new Dictionary { {"test_bool", 1}, {"test_int", 100}, {"test_double", 100.123}, @@ -89,7 +89,7 @@ public void Run(string[] args) }}, }); // Get updated global variables - globalVars = robot.GetGlobalVariables(); + globalVars = robot.global_variables(); if (globalVars.Count == 0) { Utility.SpdlogWarn("No global variables available"); @@ -98,7 +98,7 @@ public void Run(string[] args) else { Utility.SpdlogInfo("Existing global variables and their original values:"); - Console.WriteLine(Utility.FlexivDataDictToString(globalVars)); + Console.WriteLine(Utility.FlexivDataTypesDictToString(globalVars)); } Utility.SpdlogInfo("Program finished"); } diff --git a/csharp/Examples/IExample.cs b/csharp/Examples/IExample.cs index d256278..581e567 100644 --- a/csharp/Examples/IExample.cs +++ b/csharp/Examples/IExample.cs @@ -1,4 +1,4 @@ -namespace FlexivRdkCSharp.Examples +namespace Examples { public interface IExample { diff --git a/csharp/Examples/Intermed1NRTJntPosCtrl.cs b/csharp/Examples/Intermed1NRTJntPosCtrl.cs index 3dd791a..12b2566 100644 --- a/csharp/Examples/Intermed1NRTJntPosCtrl.cs +++ b/csharp/Examples/Intermed1NRTJntPosCtrl.cs @@ -1,9 +1,9 @@ using System; using System.Linq; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Intermed1NRTJntPosCtrl : IExample { @@ -45,7 +45,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -60,7 +60,7 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } @@ -70,7 +70,7 @@ public void Run(string[] args) robot.SwitchMode(RobotMode.NRT_PLAN_EXECUTION); robot.ExecutePlan("PLAN-Home"); // Wait for the plan to finish - while (robot.IsBusy()) + while (robot.busy()) { Thread.Sleep(1000); } @@ -80,10 +80,10 @@ public void Run(string[] args) double loopTime = 0.0; Utility.SpdlogInfo($"Sending command to robot at {frequency} Hz, or {period} seconds interval"); // Use current robot joint positions as initial positions - double[] initPos = (double[])robot.GetStates().Q.Clone(); + double[] initPos = (double[])robot.states().Q.Clone(); Utility.SpdlogInfo("Initial positions set to: " + string.Join(", ", initPos.Select(v => v.ToString("F6")))); // Robot joint degrees of freedom - int dof = robot.GetInfo().DoF; + int dof = robot.info().DoF; // Initialize target vectors double[] targetPos = (double[])initPos.Clone(); double[] targetVel = new double[dof]; @@ -108,7 +108,7 @@ public void Run(string[] args) { // Use sleep to control loop period Thread.Sleep(time); - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogError("Fault occurred on the connected robot, exiting ..."); return; diff --git a/csharp/Examples/Intermed2NRTJntImpCtrl.cs b/csharp/Examples/Intermed2NRTJntImpCtrl.cs index f448edf..030929c 100644 --- a/csharp/Examples/Intermed2NRTJntImpCtrl.cs +++ b/csharp/Examples/Intermed2NRTJntImpCtrl.cs @@ -1,9 +1,9 @@ using System; using System.Linq; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Intermed2NRTJntImpCtrl : IExample { @@ -45,7 +45,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -60,7 +60,7 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } @@ -70,7 +70,7 @@ public void Run(string[] args) robot.SwitchMode(RobotMode.NRT_PLAN_EXECUTION); robot.ExecutePlan("PLAN-Home"); // Wait for the plan to finish - while (robot.IsBusy()) + while (robot.busy()) { Thread.Sleep(1000); } @@ -80,10 +80,10 @@ public void Run(string[] args) int loopCounter = 0; Utility.SpdlogInfo($"Sending command to robot at {frequency} Hz, or {period} seconds interval"); // Use current robot joint positions as initial positions - double[] initPos = (double[])robot.GetStates().Q.Clone(); + double[] initPos = (double[])robot.states().Q.Clone(); Utility.SpdlogInfo("Initial positions set to: " + string.Join(", ", initPos.Select(v => v.ToString("F6")))); // Robot joint degrees of freedom - int dof = robot.GetInfo().DoF; + int dof = robot.info().DoF; // Initialize target vectors double[] targetPos = (double[])initPos.Clone(); double[] targetVel = new double[dof]; @@ -108,7 +108,7 @@ public void Run(string[] args) { // Use sleep to control loop period Thread.Sleep(time); - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogError("Fault occurred on the connected robot, exiting ..."); return; @@ -125,14 +125,14 @@ public void Run(string[] args) // Reduce stiffness to half of nominal values after 5 seconds if (loopCounter == (int)(5 / period)) { - double[] newKq = robot.GetInfo().KqNom.Select(k => k * 0.5).ToArray(); + double[] newKq = robot.info().KqNom.Select(k => k * 0.5).ToArray(); robot.SetJointImpedance(newKq); Utility.SpdlogInfo("Joint stiffness set to: " + string.Join(", ", newKq.Select(v => v.ToString("F6")))); } // Reset impedance properties to nominal values after another 5 seconds if (loopCounter == (int)(10 / period)) { - robot.SetJointImpedance(robot.GetInfo().KqNom); + robot.SetJointImpedance(robot.info().KqNom); Utility.SpdlogInfo("Joint stiffness reset to nominal."); } // Send commands diff --git a/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs b/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs index 9f5fc7c..cc9ca8c 100644 --- a/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs +++ b/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs @@ -3,9 +3,9 @@ using System.Linq; using System.Text; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { class Intermed3NRTCartPureMotionCtrl : IExample { @@ -65,7 +65,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -80,7 +80,7 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } @@ -90,19 +90,19 @@ public void Run(string[] args) robot.SwitchMode(RobotMode.NRT_PLAN_EXECUTION); robot.ExecutePlan("PLAN-Home"); // Wait for the plan to finish - while (robot.IsBusy()) + while (robot.busy()) { Thread.Sleep(1000); } // Zero Force-torque Sensor robot.SwitchMode(RobotMode.NRT_PRIMITIVE_EXECUTION); // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement - robot.ExecutePrimitive("ZeroFTSensor", new Dictionary { }); + robot.ExecutePrimitive("ZeroFTSensor", new Dictionary { }); // WARNING: during the process, the robot must not contact anything, otherwise the result // will be inaccurate and affect following operations Utility.SpdlogWarn("Zeroing force/torque sensors, make sure nothing is in contact with the robot"); // Wait for primitive completion - while (robot.IsBusy()) + while (robot.busy()) { Thread.Sleep(1000); } @@ -110,9 +110,9 @@ public void Run(string[] args) // Switch to non-real-time mode for discrete motion control robot.SwitchMode(RobotMode.NRT_CARTESIAN_MOTION_FORCE); // Set initial pose to current TCP pose - var initPose = (double[])robot.GetStates().TcpPose.Clone(); + var initPose = (double[])robot.states().TcpPose.Clone(); // Save initial joint positions - var initQ = (double[])robot.GetStates().Q.Clone(); + var initQ = (double[])robot.states().Q.Clone(); // Periodic Task, set loop period double period = 1.0 / frequency; int loopCounter = 0; @@ -124,7 +124,7 @@ public void Run(string[] args) // Use sleep to control loop period Thread.Sleep(time); // Monitor fault on the connected robot - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogError("Fault occurred on the connected robot, exiting ..."); return; @@ -152,7 +152,7 @@ public void Run(string[] args) // Online change stiffness to half of nominal at 6 seconds else if ((int)timeElapsed % 20 == 6) { - var newK = robot.GetInfo().KxNom.Select(k => k * 0.5).ToArray(); + var newK = robot.info().KxNom.Select(k => k * 0.5).ToArray(); robot.SetCartesianImpedance(newK); Utility.SpdlogInfo($"Cartesian stiffness set to: " + string.Join(", ", newK.Select(v => v.ToString("F6")))); } @@ -166,7 +166,7 @@ public void Run(string[] args) // Online reset impedance properties to nominal at 12 seconds else if ((int)timeElapsed % 20 == 12) { - robot.SetCartesianImpedance(robot.GetInfo().KxNom); + robot.SetCartesianImpedance(robot.info().KxNom); Utility.SpdlogInfo("Cartesian impedance properties are reset"); } // Online reset reference joint positions to nominal at 14 seconds @@ -194,11 +194,11 @@ public void Run(string[] args) if (collision) { bool collisionDetected = false; - var f = robot.GetStates().ExtWrenchInWorld; + var f = robot.states().ExtWrenchInWorld; var forceNorm = Math.Sqrt(f[0] * f[0] + f[1] * f[1] + f[2] * f[2]); if (forceNorm > EXT_FORCE_THRESHOLD) collisionDetected = true; - foreach (var tau in robot.GetStates().TauExt) + foreach (var tau in robot.states().TauExt) { if (Math.Abs(tau) > EXT_TORQUE_THRESHOLD) { diff --git a/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs b/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs index ac60ae3..ed55dd2 100644 --- a/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs +++ b/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs @@ -2,9 +2,9 @@ using System.Collections.Generic; using System.Linq; using System.Threading; -using FlexivRdkCSharp.FlexivRdk; +using FlexivRdk; -namespace FlexivRdkCSharp.Examples +namespace Examples { public class Intermed4NRTCartMotionForceCtrl : IExample { @@ -79,7 +79,7 @@ public void Run(string[] args) // Instantiate robot interface var robot = new Robot(robotSN); // Clear fault on the connected robot if any - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); // Try to clear the fault @@ -94,7 +94,7 @@ public void Run(string[] args) // Enable the robot, make sure the E-stop is released before enabling robot.Enable(); // Wait for the robot to become operational - while (!robot.IsOperational()) + while (!robot.operational()) { Thread.Sleep(1000); } @@ -104,7 +104,8 @@ public void Run(string[] args) robot.SwitchMode(RobotMode.NRT_PLAN_EXECUTION); robot.ExecutePlan("PLAN-Home"); // Wait for the plan to finish - while (robot.IsBusy()) + while (!(FlexivDataTypesUtils.TryGet(robot.primitive_states(), + "terminated", out var flag) && flag == 1)) { Thread.Sleep(1000); } @@ -112,12 +113,12 @@ public void Run(string[] args) // Zero Force-torque Sensor robot.SwitchMode(RobotMode.NRT_PRIMITIVE_EXECUTION); // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement - robot.ExecutePrimitive("ZeroFTSensor", new Dictionary { }); + robot.ExecutePrimitive("ZeroFTSensor", new Dictionary()); // WARNING: during the process, the robot must not contact anything, otherwise the result // will be inaccurate and affect following operations Utility.SpdlogWarn("Zeroing force/torque sensors, make sure nothing is in contact with the robot"); // Wait for primitive completion - while (robot.IsBusy()) + while (robot.busy()) { Thread.Sleep(1000); } @@ -129,7 +130,7 @@ public void Run(string[] args) // control for example. Utility.SpdlogInfo("Searching for contact ..."); // Set initial pose to current TCP pose - var initPose = (double[])robot.GetStates().TcpPose.Clone(); + var initPose = (double[])robot.states().TcpPose.Clone(); Utility.SpdlogInfo("Initial TCP pose set to: " + string.Join(", ", initPose.Select(v => v.ToString("F6"))) + " (position 3x1, rotation (quaternion) 4x1"); // Use non-real-time mode to make the robot go to a set point with its own motion generator @@ -147,7 +148,7 @@ public void Run(string[] args) while (!IsContacted) { // Compute norm of sensed external force applied on robot TCP - var f = robot.GetStates().ExtWrenchInWorld; + var f = robot.states().ExtWrenchInWorld; var forceNorm = Math.Sqrt(f[0] * f[0] + f[1] * f[1] + f[2] * f[2]); // Contact is considered to be made if sensed TCP force exceeds the threshold if (forceNorm > PRESSING_FORCE) @@ -181,7 +182,7 @@ public void Run(string[] args) double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity }); // Update initial pose to current TCP pose - initPose = (double[])robot.GetStates().TcpPose.Clone(); + initPose = (double[])robot.states().TcpPose.Clone(); Utility.SpdlogInfo("Initial TCP pose set to: " + string.Join(", ", initPose.Select(v => v.ToString("F6"))) + " (position 3x1, rotation (quaternion) 4x1"); @@ -196,7 +197,7 @@ public void Run(string[] args) // Use sleep to control loop period Thread.Sleep(time); // Monitor fault on the connected robot - if (robot.IsFault()) + if (robot.fault()) { Utility.SpdlogError("Fault occurred on the connected robot, exiting ..."); return; diff --git a/csharp/Examples/Intermed5RobotDynamics.cs b/csharp/Examples/Intermed5RobotDynamics.cs new file mode 100644 index 0000000..b1b1222 --- /dev/null +++ b/csharp/Examples/Intermed5RobotDynamics.cs @@ -0,0 +1,126 @@ +using System; +using System.Threading; +using FlexivRdk; + +namespace Examples +{ + class Intermed5RobotDynamics : IExample + { + public string Name => "intermed5_robot_dynamics"; + public string Description => "This tutorial runs the integrated dynamics engine."; + public string Usage => +@" +Usage: + intermed5_robot_dynamics +Description: + obtain robot Jacobian, mass matrix, and gravity torques. +Required arguments: + Serial number of the robot to connect to. + Remove any space. For example: Rizon4s-123456 +Optional arguments: + (none) +"; + public void Run(string[] args) + { + if (args.Length < 1) + { + Console.WriteLine(Usage); + return; + } + Utility.SpdlogInfo(">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain " + + "robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian pose.\n"); + string robotSN = args[0]; + try + { + // Instantiate robot interface + var robot = new Robot(robotSN); + // Clear fault on the connected robot if any + if (robot.fault()) + { + Utility.SpdlogWarn("Fault occurred on the connected robot, trying to clear ..."); + // Try to clear the fault + if (!robot.ClearFault()) + { + Utility.SpdlogError("Fault cannot be cleared, exiting ..."); + return; + } + Utility.SpdlogInfo("Fault on the connected robot is cleared"); + } + // Enable the robot, make sure the E-stop is released before enabling + Utility.SpdlogInfo("Enabling robot ..."); + robot.Enable(); + // Wait for the robot to become operational + while (!robot.operational()) + { + Thread.Sleep(1000); + } + Utility.SpdlogInfo("Robot is now operational"); + + // Move robot to home pose + Utility.SpdlogInfo("Moving to home pose"); + robot.SwitchMode(RobotMode.NRT_PLAN_EXECUTION); + robot.ExecutePlan("PLAN-Home"); + // Wait for the plan to finish + while (robot.busy()) + { + Thread.Sleep(1000); + } + + // Robot Dynamics + // Initialize dynamics engine + Model model = new Model(robot); + for (int i = 0; i < 5; ++i) + { + // Mark timer start point + var tic = DateTime.Now; + + // Update robot model in dynamics engine + model.Update(robot.states().Q, robot.states().DTheta); + // Compute gravity vector + var g = model.g(); + // Compute mass matrix + var M = model.M(); + // Compute Jacobian + var J = model.J("flange"); + + // Mark timer end point and get loop time + var toc = DateTime.Now; + var computationTime = (toc - tic).TotalMilliseconds; + Console.WriteLine($"Computation time = {computationTime:F3} ms"); + + Console.WriteLine("g = [" + string.Join(", ", g) + "]"); + Console.WriteLine("M = "); + PrintMatrix(M); + Console.WriteLine("J = "); + PrintMatrix(J); + Console.WriteLine(); + } + + var poseToCheck = (double[])robot.states().TcpPose.Clone(); + poseToCheck[0] += 0.1; + Console.WriteLine($"Checking reachability of Cartesian pose [{string.Join(", ", poseToCheck)}]"); + + var result = model.reachable(poseToCheck, robot.states().Q, true); + Console.WriteLine($"Reachable = {result.reachable}, IK solution = [{string.Join(", ", result.ikSolution)}]"); + } + catch (Exception ex) + { + Utility.SpdlogError(ex.Message); + } + } + + static void PrintMatrix(double[,] matrix) + { + int rows = matrix.GetLength(0); + int cols = matrix.GetLength(1); + for (int i = 0; i < rows; i++) + { + for (int j = 0; j < cols; j++) + { + Console.Write($"{matrix[i, j]:F5} "); + } + Console.WriteLine(); + } + } + } +} diff --git a/csharp/FlexivRdk/Data.cs b/csharp/FlexivRdk/Data.cs index b7d5873..a20274a 100644 --- a/csharp/FlexivRdk/Data.cs +++ b/csharp/FlexivRdk/Data.cs @@ -6,7 +6,7 @@ using System.Text.Json; using System.Text.Json.Serialization; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { public static class FlexivConstants { @@ -15,28 +15,77 @@ public static class FlexivConstants public const int kPoseSize = 7; public const int kIOPorts = 18; public const int kMaxExtAxes = 6; + public const int kSafetyIOPorts = 8; } - public enum FlexivDataType + public enum Level : int + { + UNKNOWN = 0, + INFO = 1, + WARNING = 2, + ERROR = 3, + CRITICAL = 4 + } + + public struct RobotEvent + { + [JsonConverter(typeof(JsonStringEnumConverter))] + public Level Level { get; set; } + public int Id { get; set; } + public string Description { get; set; } + public string Consequences { get; set; } + public string ProbableCauses { get; set; } + public string RecommendedActions { get; set; } + public long Timestamp { get; set; } + + public DateTime GetDateTime() + { + return DateTimeOffset.FromUnixTimeMilliseconds(Timestamp).UtcDateTime; + } + } + + public enum OperationalStatus : int + { + UNKNOWN = 0, + READY = 1, + BOOTING = 2, + ESTOP_NOT_RELEASED = 3, + NOT_ENABLED = 4, + RELEASING_BRAKE = 5, + MINOR_FAULT = 6, + CRITICAL_FAULT = 7, + IN_REDUCED_STATE = 8, + IN_RECOVERY_STATE = 9, + IN_MANUAL_MODE = 10, + IN_AUTO_MODE = 11 + } + + public enum CoordType : int + { + WORLD = 0, + TCP = 1, + } + + public enum DataType { Int, Double, String, JPos, Coord, VectorInt, VectorDouble, VectorString, VectorJPos, VectorCoord } - public class FlexivData + public class FlexivDataTypes { - public FlexivDataType Type { get; set; } + public DataType Type { get; set; } public object Value { get; set; } - public FlexivData() { } - public FlexivData(FlexivDataType type, object value) + public FlexivDataTypes() { } + public FlexivDataTypes(DataType type, object value) { Type = type; Value = value; } - public static FlexivData Create(T value) where T : notnull + public static FlexivDataTypes Create(T value) where T : notnull { var type = MapType(typeof(T)); - return new FlexivData(type, value); + return new FlexivDataTypes(type, value); } public T As() { @@ -44,66 +93,66 @@ public T As() return typedValue; throw new InvalidCastException($"Cannot convert {Type} to {typeof(T).Name}"); } - private static readonly Dictionary TypeMap = new() + private static readonly Dictionary TypeMap = new() { - { typeof(int), FlexivDataType.Int }, - { typeof(double), FlexivDataType.Double }, - { typeof(string), FlexivDataType.String }, - { typeof(JPos), FlexivDataType.JPos }, - { typeof(Coord), FlexivDataType.Coord }, - { typeof(List), FlexivDataType.VectorInt }, - { typeof(List), FlexivDataType.VectorDouble }, - { typeof(List), FlexivDataType.VectorString }, - { typeof(List), FlexivDataType.VectorJPos }, - { typeof(List), FlexivDataType.VectorCoord } + { typeof(int), DataType.Int }, + { typeof(double), DataType.Double }, + { typeof(string), DataType.String }, + { typeof(JPos), DataType.JPos }, + { typeof(Coord), DataType.Coord }, + { typeof(List), DataType.VectorInt }, + { typeof(List), DataType.VectorDouble }, + { typeof(List), DataType.VectorString }, + { typeof(List), DataType.VectorJPos }, + { typeof(List), DataType.VectorCoord } }; - private static FlexivDataType MapType(Type t) + private static DataType MapType(Type t) { if (TypeMap.TryGetValue(t, out var result)) return result; throw new NotSupportedException($"Unsupported type: {t.FullName}"); } - public static implicit operator FlexivData(int value) => Create(value); - public static implicit operator FlexivData(double value) => Create(value); - public static implicit operator FlexivData(string value) => Create(value); - public static implicit operator FlexivData(JPos value) => Create(value); - public static implicit operator FlexivData(Coord value) => Create(value); - public static implicit operator FlexivData(List value) => Create(value); - public static implicit operator FlexivData(List value) => Create(value); - public static implicit operator FlexivData(List value) => Create(value); - public static implicit operator FlexivData(List value) => Create(value); - public static implicit operator FlexivData(List value) => Create(value); + public static implicit operator FlexivDataTypes(int value) => Create(value); + public static implicit operator FlexivDataTypes(double value) => Create(value); + public static implicit operator FlexivDataTypes(string value) => Create(value); + public static implicit operator FlexivDataTypes(JPos value) => Create(value); + public static implicit operator FlexivDataTypes(Coord value) => Create(value); + public static implicit operator FlexivDataTypes(List value) => Create(value); + public static implicit operator FlexivDataTypes(List value) => Create(value); + public static implicit operator FlexivDataTypes(List value) => Create(value); + public static implicit operator FlexivDataTypes(List value) => Create(value); + public static implicit operator FlexivDataTypes(List value) => Create(value); - public static explicit operator int(FlexivData d) => d.As(); - public static explicit operator double(FlexivData d) => d.As(); - public static explicit operator string(FlexivData d) => d.As(); - public static explicit operator JPos(FlexivData d) => d.As(); - public static explicit operator Coord(FlexivData d) => d.As(); - public static explicit operator List(FlexivData d) => d.As>(); - public static explicit operator List(FlexivData d) => d.As>(); - public static explicit operator List(FlexivData d) => d.As>(); - public static explicit operator List(FlexivData d) => d.As>(); - public static explicit operator List(FlexivData d) => d.As>(); + public static explicit operator int(FlexivDataTypes d) => d.As(); + public static explicit operator double(FlexivDataTypes d) => d.As(); + public static explicit operator string(FlexivDataTypes d) => d.As(); + public static explicit operator JPos(FlexivDataTypes d) => d.As(); + public static explicit operator Coord(FlexivDataTypes d) => d.As(); + public static explicit operator List(FlexivDataTypes d) => d.As>(); + public static explicit operator List(FlexivDataTypes d) => d.As>(); + public static explicit operator List(FlexivDataTypes d) => d.As>(); + public static explicit operator List(FlexivDataTypes d) => d.As>(); + public static explicit operator List(FlexivDataTypes d) => d.As>(); public override string ToString() { string valueStr; switch (Type) { - case FlexivDataType.VectorInt: + case DataType.VectorInt: valueStr = string.Join(", ", As>()); break; - case FlexivDataType.VectorDouble: + case DataType.VectorDouble: valueStr = string.Join(", ", As>().Select(v => v.ToString("F3"))); break; - case FlexivDataType.VectorString: + case DataType.VectorString: valueStr = string.Join(", ", As>()); break; - case FlexivDataType.VectorJPos: + case DataType.VectorJPos: valueStr = string.Join("\n ", As>()); break; - case FlexivDataType.VectorCoord: + case DataType.VectorCoord: valueStr = string.Join("\n ", As>()); break; default: @@ -115,9 +164,9 @@ public override string ToString() } - public static class FlexivDataUtils + public static class FlexivDataTypesUtils { - public static T Get(Dictionary dict, string key) + public static T Get(Dictionary dict, string key) { if (!dict.TryGetValue(key, out var data)) throw new KeyNotFoundException($"Parameter '{key}' is missing."); @@ -131,7 +180,7 @@ public static T Get(Dictionary dict, string key) } } - public static bool TryGet(Dictionary dict, string key, out T result) + public static bool TryGet(Dictionary dict, string key, out T result) { if (dict.TryGetValue(key, out var data)) { @@ -148,34 +197,34 @@ public static bool TryGet(Dictionary dict, string key, ou } } - public class FlexivDataJsonConverter : JsonConverter + public class FlexivDataTypesJsonConverter : JsonConverter { - public override FlexivData Read(ref Utf8JsonReader reader, Type typeToConvert, JsonSerializerOptions options) + public override FlexivDataTypes Read(ref Utf8JsonReader reader, Type typeToConvert, JsonSerializerOptions options) { using var doc = JsonDocument.ParseValue(ref reader); var root = doc.RootElement; var typeStr = root.GetProperty("type").GetString(); - if (!Enum.TryParse(typeStr, out var type)) - throw new JsonException($"Unknown FlexivDataType: {typeStr}"); + if (!Enum.TryParse(typeStr, out var type)) + throw new JsonException($"Unknown DataType: {typeStr}"); var valueElem = root.GetProperty("value"); object value = type switch { - FlexivDataType.Int => valueElem.GetInt32(), - FlexivDataType.Double => valueElem.GetDouble(), - FlexivDataType.String => valueElem.GetString()!, - FlexivDataType.JPos => JsonSerializer.Deserialize(valueElem.GetRawText(), options)!, - FlexivDataType.Coord => JsonSerializer.Deserialize(valueElem.GetRawText(), options)!, - FlexivDataType.VectorInt => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, - FlexivDataType.VectorDouble => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, - FlexivDataType.VectorString => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, - FlexivDataType.VectorJPos => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, - FlexivDataType.VectorCoord => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, + DataType.Int => valueElem.GetInt32(), + DataType.Double => valueElem.GetDouble(), + DataType.String => valueElem.GetString()!, + DataType.JPos => JsonSerializer.Deserialize(valueElem.GetRawText(), options)!, + DataType.Coord => JsonSerializer.Deserialize(valueElem.GetRawText(), options)!, + DataType.VectorInt => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, + DataType.VectorDouble => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, + DataType.VectorString => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, + DataType.VectorJPos => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, + DataType.VectorCoord => JsonSerializer.Deserialize>(valueElem.GetRawText(), options)!, _ => throw new JsonException($"Unsupported type: {type}") }; - return new FlexivData(type, value); + return new FlexivDataTypes(type, value); } - public override void Write(Utf8JsonWriter writer, FlexivData value, JsonSerializerOptions options) + public override void Write(Utf8JsonWriter writer, FlexivDataTypes value, JsonSerializerOptions options) { writer.WriteStartObject(); writer.WriteString("type", value.Type.ToString()); @@ -193,13 +242,14 @@ public class Coord public double[] Orientation { get; set; } = new double[FlexivConstants.kCartDoF / 2]; [JsonPropertyName("ref_frame")] public string[] RefFrame { get; set; } = new string[2] { "WORLD", "WORLD_ORIGIN" }; - [JsonPropertyName("ref_q")] - public double[] RefQ { get; set; } = new double[FlexivConstants.kSerialJointDoF]; + // This is ref_q_m in cpp api + [JsonPropertyName("ref_q_m")] + public double[] RefQM { get; set; } = new double[FlexivConstants.kSerialJointDoF]; [JsonPropertyName("ref_q_e")] public double[] RefQE { get; set; } = new double[FlexivConstants.kMaxExtAxes]; public Coord() { } public Coord(double[] position, double[] orientation, string[] refFrame, - double[] refQ = null, double[] refQE = null) + double[] refQM = null, double[] refQE = null) { if (position != null && position.Length != FlexivConstants.kCartDoF / 2) throw new ArgumentException($"Position must have length {FlexivConstants.kCartDoF / 2}", nameof(position)); @@ -207,36 +257,36 @@ public Coord(double[] position, double[] orientation, string[] refFrame, throw new ArgumentException($"Orientation must have length {FlexivConstants.kCartDoF / 2}", nameof(orientation)); if (refFrame != null && refFrame.Length != 2) throw new ArgumentException("RefFrame must have length 2", nameof(refFrame)); - if (refQ != null && refQ.Length != FlexivConstants.kSerialJointDoF) - throw new ArgumentException($"RefQ must have length {FlexivConstants.kSerialJointDoF}", nameof(refQ)); + if (refQM != null && refQM.Length != FlexivConstants.kSerialJointDoF) + throw new ArgumentException($"RefQ must have length {FlexivConstants.kSerialJointDoF}", nameof(refQM)); if (refQE != null && refQE.Length != FlexivConstants.kMaxExtAxes) throw new ArgumentException($"RefQE must have length {FlexivConstants.kMaxExtAxes}", nameof(refQE)); Position = position ?? new double[FlexivConstants.kCartDoF / 2]; Orientation = orientation ?? new double[FlexivConstants.kCartDoF / 2]; RefFrame = refFrame ?? new string[2] { "WORLD", "WORLD_ORIGIN" }; - RefQ = refQ ?? new double[FlexivConstants.kSerialJointDoF]; + RefQM = refQM ?? new double[FlexivConstants.kSerialJointDoF]; RefQE = refQE ?? new double[FlexivConstants.kMaxExtAxes]; } public Coord(double x, double y, double z, double rx, double ry, double rz, string coordType = "WORLD", string coordName = "WORLD_ORIGIN", - double[] refQ = null, double[] refQE = null) + double[] refQM = null, double[] refQE = null) { - if (refQ != null && refQ.Length != FlexivConstants.kSerialJointDoF) - throw new ArgumentException($"RefQ must have length {FlexivConstants.kSerialJointDoF}", nameof(refQ)); + if (refQM != null && refQM.Length != FlexivConstants.kSerialJointDoF) + throw new ArgumentException($"RefQ must have length {FlexivConstants.kSerialJointDoF}", nameof(refQM)); if (refQE != null && refQE.Length != FlexivConstants.kMaxExtAxes) throw new ArgumentException($"RefQE must have length {FlexivConstants.kMaxExtAxes}", nameof(refQE)); Position = new double[] { x, y, z }; Orientation = new double[] { rx, ry, rz }; RefFrame = new string[] { coordType, coordName }; - RefQ = refQ ?? new double[FlexivConstants.kSerialJointDoF]; + RefQM = refQM ?? new double[FlexivConstants.kSerialJointDoF]; RefQE = refQE ?? new double[FlexivConstants.kMaxExtAxes]; } public Coord(double x, double y, double z, double qw, double qx, double qy, double qz, string coordType = "WORLD", string coordName = "WORLD_ORIGIN", - double[] refQ = null, double[] refQE = null) + double[] refQM = null, double[] refQE = null) { - if (refQ != null && refQ.Length != FlexivConstants.kSerialJointDoF) - throw new ArgumentException($"RefQ must have length {FlexivConstants.kSerialJointDoF}", nameof(refQ)); + if (refQM != null && refQM.Length != FlexivConstants.kSerialJointDoF) + throw new ArgumentException($"RefQ must have length {FlexivConstants.kSerialJointDoF}", nameof(refQM)); if (refQE != null && refQE.Length != FlexivConstants.kMaxExtAxes) throw new ArgumentException($"RefQE must have length {FlexivConstants.kMaxExtAxes}", nameof(refQE)); Position = new double[] { x, y, z }; @@ -244,7 +294,7 @@ public Coord(double x, double y, double z, double qw, double qx, double qy, doub Utility.Quat2EulerZYX(qw, qx, qy, qz, ref yaw, ref pitch, ref roll); Orientation = new double[] { Utility.Rad2Deg(roll), Utility.Rad2Deg(pitch), Utility.Rad2Deg(yaw) }; RefFrame = new string[] { coordType, coordName }; - RefQ = refQ ?? new double[FlexivConstants.kSerialJointDoF]; + RefQM = refQM ?? new double[FlexivConstants.kSerialJointDoF]; RefQE = refQE ?? new double[FlexivConstants.kMaxExtAxes]; } @@ -262,33 +312,32 @@ string FormatArray(double[] arr, int decimals = 5) } string pos = FormatArray(Position); string ori = FormatArray(Orientation); - string refQ = FormatArray(RefQ); + string refQ = FormatArray(RefQM); string refQE = FormatArray(RefQE); return $"Coord {{ Position: [{pos}], Orientation: [{ori}], RefFrame: \"{RefFrame}\", RefQ: [{refQ}], RefQE: [{refQE}] }}"; } - } public class JPos { - [JsonPropertyName("q")] - public double[] Q { get; set; } = new double[FlexivConstants.kSerialJointDoF]; + [JsonPropertyName("q_m")] + public double[] QM { get; set; } = new double[FlexivConstants.kSerialJointDoF]; [JsonPropertyName("q_e")] public double[] QE { get; set; } = new double[FlexivConstants.kMaxExtAxes]; public JPos() { } - public JPos(double[] q, double[] q_e = null) + public JPos(double[] q_m, double[] q_e = null) { - if (q != null && q.Length != FlexivConstants.kSerialJointDoF) + if (q_m != null && q_m.Length != FlexivConstants.kSerialJointDoF) throw new ArgumentException($"Q must have {FlexivConstants.kSerialJointDoF} elements."); if (q_e != null && q_e.Length != FlexivConstants.kMaxExtAxes) throw new ArgumentException($"Qe must have at most {FlexivConstants.kMaxExtAxes} elements."); - Q = q ?? new double[FlexivConstants.kSerialJointDoF]; + QM = q_m ?? new double[FlexivConstants.kSerialJointDoF]; QE = q_e ?? new double[FlexivConstants.kMaxExtAxes]; } public JPos(double j1, double j2, double j3, double j4, double j5, double j6, double j7, double e1 = 0, double e2 = 0, double e3 = 0, double e4 = 0, double e5 = 0, double e6 = 0) { - Q = new double[] { j1, j2, j3, j4, j5, j6, j7 }; + QM = new double[] { j1, j2, j3, j4, j5, j6, j7 }; QE = new double[] { e1, e2, e3, e4, e5, e6 }; } public static JPos FromJson(string json) @@ -303,7 +352,7 @@ string FormatArray(double[] arr, int decimals = 5) string formatStr = $"F{decimals}"; return string.Join(", ", arr.Select(v => v.ToString(formatStr))); } - var qStr = FormatArray(Q); + var qStr = FormatArray(QM); var qeStr = FormatArray(QE); return $"JPos {{ Q: [{qStr}], QE: [{qeStr}] }}"; } @@ -330,33 +379,12 @@ public enum RobotMode : int NRT_PRIMITIVE_EXECUTION = 8, RT_CARTESIAN_MOTION_FORCE = 9, NRT_CARTESIAN_MOTION_FORCE = 10, - MODES_CNT = 11 - } - - public enum OperationalStatus : int - { - UNKNOWN = 0, - READY = 1, - BOOTING = 2, - ESTOP_NOT_RELEASED = 3, - NOT_ENABLED = 4, - RELEASING_BRAKE = 5, - MINOR_FAULT = 6, - CRITICAL_FAULT = 7, - IN_REDUCED_STATE = 8, - IN_RECOVERY_STATE = 9, - IN_MANUAL_MODE = 10, - IN_AUTO_MODE = 11 - } - - public enum CoordType : int - { - WORLD = 0, - TCP = 1, + NRT_SUPER_PRIMITIVE = 11, + MODES_CNT = 12 } [StructLayout(LayoutKind.Sequential)] - public struct RobotState + public struct RobotStates { [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] public double[] Q; @@ -374,16 +402,8 @@ public struct RobotState public double[] TauDot; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] public double[] TauExt; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kMaxExtAxes)] - public double[] QE; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kMaxExtAxes)] - public double[] DQE; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kMaxExtAxes)] - public double[] TauE; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kPoseSize)] public double[] TcpPose; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kPoseSize)] - public double[] TcpPoseDes; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kCartDoF)] public double[] TcpVel; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kPoseSize)] @@ -421,11 +441,7 @@ void AppendArray(string name, double[] arr, int decimals = 5) AppendArray(nameof(TauDes), TauDes); AppendArray(nameof(TauDot), TauDot); AppendArray(nameof(TauExt), TauExt); - AppendArray(nameof(QE), QE); - AppendArray(nameof(DQE), DQE); - AppendArray(nameof(TauE), TauE); AppendArray(nameof(TcpPose), TcpPose); - AppendArray(nameof(TcpPoseDes), TcpPoseDes); AppendArray(nameof(TcpVel), TcpVel); AppendArray(nameof(FlangePose), FlangePose); AppendArray(nameof(FtSensorRaw), FtSensorRaw); @@ -517,24 +533,4 @@ string FormatArray(double[] arr, int decimals = 5) return sb.ToString(); } } - - [StructLayout(LayoutKind.Sequential)] - public struct ToolParams - { - public double Mass; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] - public double[] CoM; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] - public double[] Inertia; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kPoseSize)] - public double[] TcpLocation; - } - - [StructLayout(LayoutKind.Sequential)] - public struct GripperStates - { - public double Width; - public double Force; - public double MaxWidth; - } } diff --git a/csharp/FlexivRdk/Device.cs b/csharp/FlexivRdk/Device.cs index cde0ae7..4fed104 100644 --- a/csharp/FlexivRdk/Device.cs +++ b/csharp/FlexivRdk/Device.cs @@ -3,7 +3,7 @@ using System.Runtime.InteropServices; using System.Text.Json; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { public class Device : IDisposable { @@ -57,7 +57,7 @@ public Device(Robot robot) ~Device() => Dispose(false); - public Dictionary GetDevicesList() + public Dictionary list() { FlexivError error = new(); IntPtr ptr = NativeFlexivRdk.GetDevicesList(_devicePtr, ref error); @@ -69,7 +69,7 @@ public Dictionary GetDevicesList() return new Dictionary(tmp); } - public bool HasDevice(string deviceName) + public bool exist(string deviceName) { FlexivError error = new(); int flag = NativeFlexivRdk.HasDevice(_devicePtr, deviceName, ref error); @@ -77,21 +77,35 @@ public bool HasDevice(string deviceName) return flag != 0; } - public void EnableDevice(string deviceName) + // Uses partial types: Int, Double, String, VectorDouble, and VectorString from FlexivDataType. + // Represents the DeviceParamDataTypes defined in the C++ API. + public Dictionary GetParams(string deviceName) + { + FlexivError error = new(); + IntPtr ptr = NativeFlexivRdk.GetDeviceParams(_devicePtr, deviceName, ref error); + ThrowRdkException(error); + string str = Marshal.PtrToStringAnsi(ptr); + NativeFlexivRdk.FreeString(ptr); + var tmp = JsonSerializer.Deserialize>(str, _options); + if (tmp == null) tmp = new Dictionary(); + return new Dictionary(tmp); + } + + public void Enable(string deviceName) { FlexivError error = new(); NativeFlexivRdk.EnableDevice(_devicePtr, deviceName, ref error); ThrowRdkException(error); } - public void DisableDevice(string deviceName) + public void Disable(string deviceName) { FlexivError error = new(); NativeFlexivRdk.DisableDevice(_devicePtr, deviceName, ref error); ThrowRdkException(error); } - public void SendCommands(string deviceName, Dictionary cmds) + public void Command(string deviceName, Dictionary cmds) { FlexivError error = new(); string str = JsonSerializer.Serialize(cmds, _options); diff --git a/csharp/FlexivRdk/FileIO.cs b/csharp/FlexivRdk/FileIO.cs index cc69737..3ca33a2 100644 --- a/csharp/FlexivRdk/FileIO.cs +++ b/csharp/FlexivRdk/FileIO.cs @@ -1,6 +1,9 @@ using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text.Json; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { public class FileIO : IDisposable { @@ -43,11 +46,39 @@ public FileIO(Robot robot) ~FileIO() => Dispose(false); + public List traj_files_list() + { + FlexivError error = new(); + IntPtr ptr = NativeFlexivRdk.GetTrajFilesList(_fileIOPtr, ref error); + ThrowRdkException(error); + string str = Marshal.PtrToStringAnsi(ptr); + NativeFlexivRdk.FreeString(ptr); + List ret = JsonSerializer.Deserialize>(str); + return new List(ret); + } + public void UploadTrajFile(string fileDir, string fileName) { FlexivError error = new(); NativeFlexivRdk.UploadTrajFile(_fileIOPtr, fileDir, fileName, ref error); ThrowRdkException(error); } + + public string DownloadTrajFile(string fileName) + { + FlexivError error = new(); + IntPtr ptr = NativeFlexivRdk.DownloadTrajFile(_fileIOPtr, fileName, ref error); + ThrowRdkException(error); + string str = Marshal.PtrToStringAnsi(ptr); + NativeFlexivRdk.FreeString(ptr); + return str; + } + + public void DownloadTrajFile(string fileName, string saveDir) + { + FlexivError error = new(); + NativeFlexivRdk.DownloadTrajFile2(_fileIOPtr, fileName, saveDir, ref error); + ThrowRdkException(error); + } } } diff --git a/csharp/FlexivRdk/Gripper.cs b/csharp/FlexivRdk/Gripper.cs index 478abf5..fead670 100644 --- a/csharp/FlexivRdk/Gripper.cs +++ b/csharp/FlexivRdk/Gripper.cs @@ -1,8 +1,31 @@ using System; using System.Text.Json; +using System.Runtime.InteropServices; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { + [StructLayout(LayoutKind.Sequential)] + public struct GripperParams + { + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)] + public string Name; + public double MinWidth; + public double MaxWidth; + public double MinVel; + public double MaxVel; + public double MinForce; + public double MaxForce; + } + + [StructLayout(LayoutKind.Sequential)] + public struct GripperStates + { + public double Width; + public double Force; + // 0 = false = not moving, 1 = true = moving. + public int IsMoving; + } + public class Gripper : IDisposable { private IntPtr _gripperPtr; @@ -49,13 +72,27 @@ public Gripper(Robot robot) { WriteIndented = false, PropertyNamingPolicy = JsonNamingPolicy.CamelCase, - Converters = { new FlexivDataJsonConverter() } + Converters = { new FlexivDataTypesJsonConverter() } }; ThrowRdkException(error); } ~Gripper() => Dispose(false); + public void Enable(string gripperName) + { + FlexivError error = new(); + NativeFlexivRdk.EnableGripper(_gripperPtr, gripperName, ref error); + ThrowRdkException(error); + } + + public void Disable() + { + FlexivError error = new(); + NativeFlexivRdk.DisableGripper(_gripperPtr, ref error); + ThrowRdkException(error); + } + public void Init() { FlexivError error = new(); @@ -70,7 +107,7 @@ public void Grasp(double force) ThrowRdkException(error); } - public void Move(double width, double velocity, double forceLimit = 0) + public void Move(double width, double velocity, double forceLimit) { FlexivError error = new(); NativeFlexivRdk.Move(_gripperPtr, width, velocity, forceLimit, ref error); @@ -82,12 +119,14 @@ public void Stop() NativeFlexivRdk.StopGripper(_gripperPtr); } - public bool IsMoving() + public GripperParams GetParams() { - return NativeFlexivRdk.GripperIsMoving(_gripperPtr) != 0; + GripperParams gripperParams = new(); + NativeFlexivRdk.GetGripperParams(_gripperPtr, ref gripperParams); + return gripperParams; } - public GripperStates GetGripperStates() + public GripperStates states() { GripperStates states = new(); NativeFlexivRdk.GetGripperStates(_gripperPtr, ref states); diff --git a/csharp/FlexivRdk/Maintenance.cs b/csharp/FlexivRdk/Maintenance.cs new file mode 100644 index 0000000..89b04a5 --- /dev/null +++ b/csharp/FlexivRdk/Maintenance.cs @@ -0,0 +1,54 @@ +using System; + +namespace FlexivRdk +{ + class Maintenance : IDisposable + { + private IntPtr _maintenancePtr; + private bool _disposed = false; + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + protected virtual void Dispose(bool disposing) + { + if (_disposed) return; + if (_maintenancePtr != IntPtr.Zero) + { + NativeFlexivRdk.DeleteMaintenance(_maintenancePtr); + _maintenancePtr = IntPtr.Zero; + } + _disposed = true; + } + + private static void ThrowRdkException(FlexivError error) + { + if (error.error_code != 0) + { + string message = error.error_msg?.TrimEnd('\0') ?? "Unknown error"; + throw new Exception($"[Flexiv RDK Error {error.error_code}] {message}"); + } + } + + public Maintenance(Robot robot) + { + if (robot == null) throw new ArgumentNullException(nameof(robot)); + FlexivError error = new(); + _maintenancePtr = NativeFlexivRdk.CreateMaintenance(robot.NativePtr, ref error); + ThrowRdkException(error); + } + + ~Maintenance() => Dispose(false); + + public void CalibrateJointTorqueSensors(double[] calibrationPosture = null) + { + FlexivError error = new(); + NativeFlexivRdk.CalibrateJointTorqueSensor(_maintenancePtr, calibrationPosture, + calibrationPosture.Length, ref error); + ThrowRdkException(error); + } + } +} diff --git a/csharp/FlexivRdk/Model.cs b/csharp/FlexivRdk/Model.cs new file mode 100644 index 0000000..39030b5 --- /dev/null +++ b/csharp/FlexivRdk/Model.cs @@ -0,0 +1,161 @@ +using System; + +namespace FlexivRdk +{ + public class Model : IDisposable + { + private IntPtr _modelPtr; + private bool _disposed = false; + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + protected virtual void Dispose(bool disposing) + { + if (_disposed) return; + if (_modelPtr != IntPtr.Zero) + { + NativeFlexivRdk.DeleteModel(_modelPtr); + _modelPtr = IntPtr.Zero; + } + _disposed = true; + } + + private static void ThrowRdkException(FlexivError error) + { + if (error.error_code != 0) + { + string message = error.error_msg?.TrimEnd('\0') ?? "Unknown error"; + throw new Exception($"[Flexiv RDK Error {error.error_code}] {message}"); + } + } + + public Model(Robot robot, double gravityX = 0.0, double gravityY = 0.0, double gravityZ = -9.81) + { + if (robot == null) throw new ArgumentNullException(nameof(robot)); + FlexivError error = new(); + _modelPtr = NativeFlexivRdk.CreateModel(robot.NativePtr, gravityX, gravityY, gravityZ, ref error); + ThrowRdkException(error); + } + + ~Model() => Dispose(false); + + public void Reload() + { + FlexivError error = new(); + NativeFlexivRdk.Reload(_modelPtr, ref error); + ThrowRdkException(error); + } + + public void Update(double[] positions, double[] velocities) + { + FlexivError error = new(); + NativeFlexivRdk.Update(_modelPtr, positions, positions.Length, velocities, velocities.Length, ref error); + ThrowRdkException(error); + } + + public double[,] J(string linkName) + { + FlexivError error = new(); + int rows = 6; + int cols = 7; + var buffer = new double[rows * cols]; + NativeFlexivRdk.GetJacobian(_modelPtr, linkName, buffer, rows, cols, ref error); + ThrowRdkException(error); + var result = new double[rows, cols]; + for (int i = 0; i < rows; ++i) + for (int j = 0; j < cols; ++j) + result[i, j] = buffer[i * cols + j]; + return result; + } + + public double[,] dJ(string linkName) + { + FlexivError error = new(); + int rows = 6; + int cols = 7; + var buffer = new double[rows * cols]; + NativeFlexivRdk.GetJacobianDot(_modelPtr, linkName, buffer, rows, cols, ref error); + ThrowRdkException(error); + var result = new double[rows, cols]; + for (int i = 0; i < rows; ++i) + for (int j = 0; j < cols; ++j) + result[i, j] = buffer[i * cols + j]; + return result; + } + + public double[,] M() + { + FlexivError error = new(); + int dof = 7; + var buffer = new double[dof * dof]; + NativeFlexivRdk.GetMassMatrix(_modelPtr, buffer, dof, ref error); + ThrowRdkException(error); + var result = new double[dof, dof]; + for (int i = 0; i < dof; ++i) + for (int j = 0; j < dof; ++j) + result[i, j] = buffer[i * dof + j]; + return result; + } + + public double[,] C() + { + FlexivError error = new(); + int dof = 7; + var buffer = new double[dof * dof]; + NativeFlexivRdk.GetCoriolisCentripetalMatrix(_modelPtr, buffer, dof, ref error); + ThrowRdkException(error); + var result = new double[dof, dof]; + for (int i = 0; i < dof; ++i) + for (int j = 0; j < dof; ++j) + result[i, j] = buffer[i * dof + j]; + return result; + } + + public double[] g() + { + int dof = 7; + var buffer = new double[dof]; + NativeFlexivRdk.GetGravityForceVector(_modelPtr, buffer, dof); + return buffer; + } + + public double[] c() + { + int dof = 7; + var buffer = new double[dof]; + NativeFlexivRdk.GetCoriolisForceVector(_modelPtr, buffer, dof); + return buffer; + } + + public void SyncURDF(string templateUrdfPath) + { + FlexivError error = new(); + NativeFlexivRdk.SyncURDF(_modelPtr, templateUrdfPath, ref error); + ThrowRdkException(error); + } + + public (bool reachable, double[] ikSolution) reachable(double[] pose, double[] seed, bool freeOri) + { + FlexivError error = new(); + int reachableInt = 0; + var ikSolution = new double[7]; + NativeFlexivRdk.Reachable(_modelPtr, pose, pose.Length, seed, seed.Length, freeOri ? 1 : 0, + ref reachableInt, ikSolution, ref error); + ThrowRdkException(error); + return (reachableInt != 0, ikSolution); + } + + public (double transScore, double orientScore) configuration_score() + { + FlexivError error = new(); + double tScore = 0, oScore = 0; + NativeFlexivRdk.ConfigurationScore(_modelPtr, ref tScore, ref oScore, ref error); + ThrowRdkException(error); + return (tScore, oScore); + } + } +} diff --git a/csharp/FlexivRdk/Native.cs b/csharp/FlexivRdk/Native.cs index 8b85811..2401270 100644 --- a/csharp/FlexivRdk/Native.cs +++ b/csharp/FlexivRdk/Native.cs @@ -1,7 +1,7 @@ using System; using System.Runtime.InteropServices; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { internal static class NativeFlexivRdk { @@ -22,7 +22,7 @@ internal static class NativeFlexivRdk [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern IntPtr CreateFlexivRobot(string robotSN, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.LPStr)] string[] interfaces, - int interfaceCount, ref FlexivError error); + int interfaceCount, int verbose, ref FlexivError error); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void DeleteFlexivRobot(IntPtr robot); @@ -38,13 +38,13 @@ public static extern IntPtr CreateFlexivRobot(string robotSN, public static extern int GetMode(IntPtr robot); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] - public static extern void GetStates(IntPtr robot, ref RobotState robot_state); + public static extern void GetStates(IntPtr robot, ref RobotStates robot_state); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern int IsStopped(IntPtr robot); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] - public static extern int IsOperational(IntPtr robot, int verbose); + public static extern int IsOperational(IntPtr robot); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern int GetOperationalStatus(IntPtr robot); @@ -68,7 +68,7 @@ public static extern IntPtr CreateFlexivRobot(string robotSN, public static extern int IsEnablingButtonReleased(IntPtr robot); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] - public static extern IntPtr GetMuLog(IntPtr robot); + public static extern IntPtr GetEventLog(IntPtr robot); //======================================= SYSTEM CONTROL ======================================= [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] @@ -95,6 +95,9 @@ public static extern IntPtr CreateFlexivRobot(string robotSN, [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern IntPtr GetGlobalVariables(IntPtr robot, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void LockExternalAxes(IntPtr robot, int toggle, ref FlexivError error); + //======================================= PLAN EXECUTION ======================================= [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void ExecutePlanByIdx(IntPtr robot, int idx, int continueExec, @@ -125,7 +128,7 @@ public static extern void ExecutePlanByName(IntPtr robot, string name, int conti //==================================== PRIMITIVE EXECUTION ===================================== [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void ExecutePrimitive(IntPtr robot, string primitiveName, string inputParams, - string properties, int blockUntilStarted, ref FlexivError error); + int blockUntilStarted, ref FlexivError error); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern IntPtr GetPrimitiveStates(IntPtr robot, ref FlexivError error); @@ -222,6 +225,9 @@ public static extern void SetForceControlAxis(IntPtr robot, int[] enabledAxes, i [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void RemoveTool(IntPtr tool, string name, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void CalibratePayloadParams(IntPtr tool, int toolMounted, ref ToolParams toolParams, ref FlexivError error); + //======================================= WORK COORD ========================================= [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern IntPtr CreateWorkCoord(IntPtr robot, ref FlexivError error); @@ -255,6 +261,12 @@ public static extern void GetWorkCoordPose(IntPtr workCoord, string name, ref do [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void DeleteGripper(IntPtr gripper); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void EnableGripper(IntPtr gripper, string name, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void DisableGripper(IntPtr gripper, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void Init(IntPtr gripper, ref FlexivError error); @@ -268,7 +280,7 @@ public static extern void GetWorkCoordPose(IntPtr workCoord, string name, ref do public static extern void StopGripper(IntPtr gripper); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] - public static extern int GripperIsMoving(IntPtr gripper); + public static extern void GetGripperParams(IntPtr gripper, ref GripperParams param); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void GetGripperStates(IntPtr gripper, ref GripperStates states); @@ -280,9 +292,18 @@ public static extern void GetWorkCoordPose(IntPtr workCoord, string name, ref do [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void DeleteFileIO(IntPtr fileIO); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr GetTrajFilesList(IntPtr fileIO, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void UploadTrajFile(IntPtr fileIO, string fileDir, string fileName, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr DownloadTrajFile(IntPtr fileIO, string fileName, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void DownloadTrajFile2(IntPtr fileIO, string fileName, string saveDir, ref FlexivError error); + //========================================= DEVICE =========================================== [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern IntPtr CreateDevice(IntPtr robot, ref FlexivError error); @@ -296,6 +317,9 @@ public static extern void GetWorkCoordPose(IntPtr workCoord, string name, ref do [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern int HasDevice(IntPtr device, string deviceName, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr GetDeviceParams(IntPtr device, string deviceName, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void EnableDevice(IntPtr device, string deviceName, ref FlexivError error); @@ -304,5 +328,84 @@ public static extern void GetWorkCoordPose(IntPtr workCoord, string name, ref do [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void SendCommands(IntPtr device, string deviceName, string cmds, ref FlexivError error); + + //======================================= MAINTENANCE ========================================= + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr CreateMaintenance(IntPtr robot, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void DeleteMaintenance(IntPtr maintenance); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void CalibrateJointTorqueSensor(IntPtr maintenance, + double[] caliPosture, int caliPostureLen, ref FlexivError error); + + //========================================= SAFETY ============================================ + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr CreateSafety(IntPtr robot, string password, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void DeleteSafety(IntPtr safety); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void DefaultLimits(IntPtr safety, ref SafetyLimits limits); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void CurrentLimits(IntPtr safety, ref SafetyLimits limits); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void GetSafetyInputs(IntPtr safety, int[] temp); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SetJointPositionLimits(IntPtr safety, double[] minPositions, int minLen, + double[] maxPositions, int maxLen, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SetJointVelocityNormalLimits(IntPtr safety, double[] maxVel, int len, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SetJointVelocityReducedLimits(IntPtr safety, double[] maxVel, int len, ref FlexivError error); + + //========================================= MODEL =========================================== + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr CreateModel(IntPtr robot, double gravityX, double gravityY, double gravityZ, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void DeleteModel(IntPtr model); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void Reload(IntPtr model, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void Update(IntPtr model, double[] pos, int posLen, double[] vel, int velLen, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void GetJacobian(IntPtr model, string linkName, double[] buffer, int rows, int cols, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void GetJacobianDot(IntPtr model, string linkName, double[] buffer, int rows, int cols, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void GetMassMatrix(IntPtr model, double[] buffer, int dof, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void GetCoriolisCentripetalMatrix(IntPtr model, double[] buffer, int dof, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void GetGravityForceVector(IntPtr model, double[] buffer, int dof); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void GetCoriolisForceVector(IntPtr model, double[] buffer, int dof); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SyncURDF(IntPtr model, string templateUrdfPath, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void Reachable(IntPtr model, double[] pose, int poseLen, double[] seed, int seedLen, + int freeOri, ref int reachable, double[] ikSolution, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void ConfigurationScore(IntPtr model, ref double transScore, ref double orientScore, ref FlexivError error); + } } diff --git a/csharp/FlexivRdk/Robot.cs b/csharp/FlexivRdk/Robot.cs index 286550e..9412067 100644 --- a/csharp/FlexivRdk/Robot.cs +++ b/csharp/FlexivRdk/Robot.cs @@ -3,7 +3,7 @@ using System.Runtime.InteropServices; using System.Text.Json; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { public class Robot : IDisposable { @@ -41,16 +41,16 @@ private static void ThrowRdkException(FlexivError error) } } - public Robot(string robotSN, string[] networkInterfaceWhiteList = null) + public Robot(string robotSN, string[] networkInterfaceWhiteList = null, bool verbose = true) { FlexivError error = new(); string[] interfaces = networkInterfaceWhiteList ?? Array.Empty(); - _flexivRobotPtr = NativeFlexivRdk.CreateFlexivRobot(robotSN, interfaces, interfaces.Length, ref error); + _flexivRobotPtr = NativeFlexivRdk.CreateFlexivRobot(robotSN, interfaces, interfaces.Length, verbose ? 1 : 0, ref error); _options = new JsonSerializerOptions { WriteIndented = false, PropertyNamingPolicy = JsonNamingPolicy.CamelCase, - Converters = { new FlexivDataJsonConverter() } + Converters = { new FlexivDataTypesJsonConverter() } }; ThrowRdkException(error); } @@ -58,84 +58,87 @@ public Robot(string robotSN, string[] networkInterfaceWhiteList = null) ~Robot() => Dispose(false); //========================================= ACCESSORS ========================================== - public bool IsConnected() + public bool connected() { return NativeFlexivRdk.IsConnected(_flexivRobotPtr) != 0; } - public RobotInfo GetInfo() + public RobotInfo info() { RobotInfo robotInfo = new(); NativeFlexivRdk.GetInfo(_flexivRobotPtr, ref robotInfo); return robotInfo; } - public RobotMode GetMode() + public RobotMode mode() { return (RobotMode)NativeFlexivRdk.GetMode(_flexivRobotPtr); } - public RobotState GetStates() + public RobotStates states() { - RobotState robot_state = new(); + RobotStates robot_state = new(); NativeFlexivRdk.GetStates(_flexivRobotPtr, ref robot_state); return robot_state; } - public bool IsStopped() + public bool stopped() { return NativeFlexivRdk.IsStopped(_flexivRobotPtr) != 0; } - public bool IsOperational(bool verbose = true) + public bool operational() { - return NativeFlexivRdk.IsOperational(_flexivRobotPtr, verbose ? 1 : 0) != 0; + return NativeFlexivRdk.IsOperational(_flexivRobotPtr) != 0; } - public OperationalStatus GetOperationalStatus() + public OperationalStatus operational_status() { return (OperationalStatus)NativeFlexivRdk.GetOperationalStatus(_flexivRobotPtr); } - public bool IsBusy() + public bool busy() { return NativeFlexivRdk.IsBusy(_flexivRobotPtr) != 0; } - public bool IsFault() + public bool fault() { return NativeFlexivRdk.IsFault(_flexivRobotPtr) != 0; } - public bool IsReduced() + public bool reduced() { return NativeFlexivRdk.IsReduced(_flexivRobotPtr) != 0; } - public bool IsRecovery() + public bool recovery() { return NativeFlexivRdk.IsRecovery(_flexivRobotPtr) != 0; } - public bool IsEstopReleased() + public bool estop_released() { return NativeFlexivRdk.IsEstopReleased(_flexivRobotPtr) != 0; } - public bool IsEnablingButtonReleased() + public bool enabling_button_pressed() { return NativeFlexivRdk.IsEnablingButtonReleased(_flexivRobotPtr) != 0; } - public List GetMuLog() + public List event_log() { - IntPtr ptr = NativeFlexivRdk.GetMuLog(_flexivRobotPtr); + IntPtr ptr = NativeFlexivRdk.GetEventLog(_flexivRobotPtr); string str = Marshal.PtrToStringAnsi(ptr); NativeFlexivRdk.FreeString(ptr); - var tmp = JsonSerializer.Deserialize>(str, _options); - string json = JsonSerializer.Serialize(tmp, _options); - var ret = (List)(tmp["mu_log"]); - return new List(ret); + var options = new JsonSerializerOptions + { + WriteIndented = false, + PropertyNameCaseInsensitive = true, + }; + List ret = JsonSerializer.Deserialize>(str, options); + return new List(ret); } //======================================= SYSTEM CONTROL ======================================= @@ -182,7 +185,7 @@ public void RunAutoRecovery() ThrowRdkException(error); } - public void SetGlobalVariables(Dictionary globalVars) + public void SetGlobalVariables(Dictionary globalVars) { FlexivError error = new(); string json_str = JsonSerializer.Serialize(globalVars, _options); @@ -190,16 +193,23 @@ public void SetGlobalVariables(Dictionary globalVars) ThrowRdkException(error); } - public Dictionary GetGlobalVariables() + public Dictionary global_variables() { FlexivError error = new(); IntPtr ptr = NativeFlexivRdk.GetGlobalVariables(_flexivRobotPtr, ref error); ThrowRdkException(error); - NativeFlexivRdk.FreeString(ptr); string str = Marshal.PtrToStringAnsi(ptr); - var tmp = JsonSerializer.Deserialize>(str, _options); - if (tmp == null) tmp = new Dictionary(); - return new Dictionary(tmp); + NativeFlexivRdk.FreeString(ptr); + var tmp = JsonSerializer.Deserialize>(str, _options); + if (tmp == null) tmp = new Dictionary(); + return new Dictionary(tmp); + } + + public void LockExternalAxes(bool toggle) + { + FlexivError error = new(); + NativeFlexivRdk.LockExternalAxes(_flexivRobotPtr, toggle ? 1 : 0, ref error); + ThrowRdkException(error); } //======================================= PLAN EXECUTION ======================================= @@ -224,21 +234,20 @@ public void PausePlan(bool pause) ThrowRdkException(error); } - public List GetPlanList() + public List plan_list() { FlexivError error = new(); IntPtr ptr = NativeFlexivRdk.GetPlanList(_flexivRobotPtr, ref error); ThrowRdkException(error); string str = Marshal.PtrToStringAnsi(ptr); NativeFlexivRdk.FreeString(ptr); - Console.WriteLine(str); - var tmp = JsonSerializer.Deserialize>(str, _options); + var tmp = JsonSerializer.Deserialize>(str, _options); string json = JsonSerializer.Serialize(tmp, _options); var ret = (List)(tmp["plan_list"]); return new List(ret); } - public PlanInfo GetPlanInfo() + public PlanInfo plan_info() { PlanInfo plan_info = new(); FlexivError error = new FlexivError(); @@ -270,28 +279,26 @@ public void SetVelocityScale(int velocityScale) //==================================== PRIMITIVE EXECUTION ===================================== public void ExecutePrimitive(string primitiveName, - Dictionary inputParams, - Dictionary properties = null, + Dictionary inputParams, bool blockUntilStarted = true) { FlexivError error = new(); string input_params = JsonSerializer.Serialize(inputParams, _options); - string pro = properties != null ? JsonSerializer.Serialize(properties, _options) : "{}"; NativeFlexivRdk.ExecutePrimitive(_flexivRobotPtr, primitiveName, - input_params, pro, blockUntilStarted ? 1 : 0, ref error); + input_params, blockUntilStarted ? 1 : 0, ref error); ThrowRdkException(error); } - public Dictionary GetPrimitiveStates() + public Dictionary primitive_states() { FlexivError error = new(); IntPtr ptr = NativeFlexivRdk.GetPrimitiveStates(_flexivRobotPtr, ref error); ThrowRdkException(error); - NativeFlexivRdk.FreeString(ptr); string str = Marshal.PtrToStringAnsi(ptr); - var tmp = JsonSerializer.Deserialize>(str, _options); - if (tmp == null) tmp = new Dictionary(); - return new Dictionary(tmp); + NativeFlexivRdk.FreeString(ptr); + var tmp = JsonSerializer.Deserialize>(str, _options); + if (tmp == null) tmp = new Dictionary(); + return new Dictionary(tmp); } //==================================== DIRECT JOINT CONTROL ==================================== diff --git a/csharp/FlexivRdk/Safety.cs b/csharp/FlexivRdk/Safety.cs new file mode 100644 index 0000000..b0ea032 --- /dev/null +++ b/csharp/FlexivRdk/Safety.cs @@ -0,0 +1,106 @@ +using System; +using System.Runtime.InteropServices; + +namespace FlexivRdk +{ + [StructLayout(LayoutKind.Sequential)] + public struct SafetyLimits + { + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] + public double[] QMin; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] + public double[] QMax; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] + public double[] DQMaxNormal; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] + public double[] DQMaxReduced; + } + + class Safety : IDisposable + { + private IntPtr _safetyPtr; + private bool _disposed = false; + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + protected virtual void Dispose(bool disposing) + { + if (_disposed) return; + if (_safetyPtr != IntPtr.Zero) + { + NativeFlexivRdk.DeleteSafety(_safetyPtr); + _safetyPtr = IntPtr.Zero; + } + _disposed = true; + } + + private static void ThrowRdkException(FlexivError error) + { + if (error.error_code != 0) + { + string message = error.error_msg?.TrimEnd('\0') ?? "Unknown error"; + throw new Exception($"[Flexiv RDK Error {error.error_code}] {message}"); + } + } + + public Safety(Robot robot, string password) + { + if (robot == null) throw new ArgumentNullException(nameof(robot)); + FlexivError error = new(); + _safetyPtr = NativeFlexivRdk.CreateSafety(robot.NativePtr, password, ref error); + ThrowRdkException(error); + } + + ~Safety() => Dispose(false); + + SafetyLimits default_limits() + { + SafetyLimits limits = new(); + NativeFlexivRdk.DefaultLimits(_safetyPtr, ref limits); + return limits; + } + + SafetyLimits current_limits() + { + SafetyLimits limits = new(); + NativeFlexivRdk.CurrentLimits(_safetyPtr, ref limits); + return limits; + } + + public bool[] safety_inputs() + { + int[] temp = new int[FlexivConstants.kSafetyIOPorts]; + NativeFlexivRdk.GetSafetyInputs(_safetyPtr, temp); + return Array.ConvertAll(temp, b => b != 0); + } + + public void SetJointPositionLimits(double[] minPositions, double[] maxPositions) + { + FlexivError error = new(); + NativeFlexivRdk.SetJointPositionLimits(_safetyPtr, minPositions, minPositions.Length, + maxPositions, maxPositions.Length, ref error); + ThrowRdkException(error); + } + + public void SetJointVelocityNormalLimits(double[] maxVelocities) + { + FlexivError error = new(); + NativeFlexivRdk.SetJointVelocityNormalLimits(_safetyPtr, maxVelocities, + maxVelocities.Length, ref error); + ThrowRdkException(error); + } + + public void SetJointVelocityReducedLimits(double[] maxVelocities) + { + FlexivError error = new(); + NativeFlexivRdk.SetJointVelocityReducedLimits(_safetyPtr, + maxVelocities, maxVelocities.Length, ref error); + ThrowRdkException(error); + } + + } +} diff --git a/csharp/FlexivRdk/Tool.cs b/csharp/FlexivRdk/Tool.cs index 2fbabe3..2e28115 100644 --- a/csharp/FlexivRdk/Tool.cs +++ b/csharp/FlexivRdk/Tool.cs @@ -3,8 +3,20 @@ using System.Runtime.InteropServices; using System.Text.Json; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { + [StructLayout(LayoutKind.Sequential)] + public struct ToolParams + { + public double Mass; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] + public double[] CoM; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] + public double[] Inertia; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kPoseSize)] + public double[] TcpLocation; + } + public class Tool : IDisposable { private IntPtr _toolPtr; @@ -51,27 +63,29 @@ public Tool(Robot robot) { WriteIndented = false, PropertyNamingPolicy = JsonNamingPolicy.CamelCase, - Converters = { new FlexivDataJsonConverter() } + Converters = { new FlexivDataTypesJsonConverter() } }; ThrowRdkException(error); } ~Tool() => Dispose(false); - public List GetToolNames() + public List list() { FlexivError error = new(); IntPtr ptr = NativeFlexivRdk.GetToolNames(_toolPtr, ref error); ThrowRdkException(error); string str = Marshal.PtrToStringAnsi(ptr); - NativeFlexivRdk.FreeString(ptr); - var tmp = JsonSerializer.Deserialize>(str, _options); + + + + var tmp = JsonSerializer.Deserialize>(str, _options); string json = JsonSerializer.Serialize(tmp, _options); var ret = (List)tmp["tool_list"]; return new List(ret); } - public string GetToolName() + public string name() { FlexivError error = new(); IntPtr ptr = NativeFlexivRdk.GetToolName(_toolPtr, ref error); @@ -81,7 +95,7 @@ public string GetToolName() return str; } - public bool HasTool(string toolName) + public bool exist(string toolName) { FlexivError error = new(); int flag = NativeFlexivRdk.HasTool(_toolPtr, toolName, ref error); @@ -89,7 +103,7 @@ public bool HasTool(string toolName) return flag != 0; } - public ToolParams GetToolParams() + public ToolParams GetParams() { FlexivError error = new(); ToolParams toolParams = new(); @@ -98,7 +112,7 @@ public ToolParams GetToolParams() return toolParams; } - public ToolParams GetToolParams(string toolName) + public ToolParams GetParams(string toolName) { FlexivError error = new(); ToolParams toolParams = new(); @@ -107,32 +121,41 @@ public ToolParams GetToolParams(string toolName) return toolParams; } - public void AddNewTool(string toolName, ToolParams toolParams) + public void Add(string toolName, ToolParams toolParams) { FlexivError error = new(); NativeFlexivRdk.AddNewTool(_toolPtr, toolName, ref toolParams, ref error); ThrowRdkException(error); } - public void SwitchTool(string toolName) + public void Switch(string toolName) { FlexivError error = new(); NativeFlexivRdk.SwitchTool(_toolPtr, toolName, ref error); ThrowRdkException(error); } - public void UpdateTool(string toolName, ToolParams toolParams) + public void Update(string toolName, ToolParams toolParams) { FlexivError error = new(); NativeFlexivRdk.UpdateTool(_toolPtr, toolName, ref toolParams, ref error); ThrowRdkException(error); } - public void RemoveTool(string toolName) + public void Remove(string toolName) { FlexivError error = new(); NativeFlexivRdk.RemoveTool(_toolPtr, toolName, ref error); ThrowRdkException(error); } + + public ToolParams CalibratePayloadParams(bool toolMounted) + { + FlexivError error = new(); + ToolParams param = new(); + NativeFlexivRdk.CalibratePayloadParams(_toolPtr, toolMounted ? 1 : 0, ref param, ref error); + ThrowRdkException(error); + return param; + } } } diff --git a/csharp/FlexivRdk/Utility.cs b/csharp/FlexivRdk/Utility.cs index 71c1dba..0467b54 100644 --- a/csharp/FlexivRdk/Utility.cs +++ b/csharp/FlexivRdk/Utility.cs @@ -2,7 +2,7 @@ using System.Collections.Generic; using System.Text; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { public class Utility { @@ -55,7 +55,7 @@ public static void SpdlogWarn(string msgs) NativeFlexivRdk.SpdlogWarn(msgs); } - public static string FlexivDataDictToString(Dictionary dict) + public static string FlexivDataTypesDictToString(Dictionary dict) { if (dict == null || dict.Count == 0) return "{}"; diff --git a/csharp/FlexivRdk/WorkCoord.cs b/csharp/FlexivRdk/WorkCoord.cs index b8797e6..5a47f0f 100644 --- a/csharp/FlexivRdk/WorkCoord.cs +++ b/csharp/FlexivRdk/WorkCoord.cs @@ -3,7 +3,7 @@ using System.Runtime.InteropServices; using System.Text.Json; -namespace FlexivRdkCSharp.FlexivRdk +namespace FlexivRdk { public class WorkCoord : IDisposable { @@ -51,27 +51,27 @@ public WorkCoord(Robot robot) { WriteIndented = false, PropertyNamingPolicy = JsonNamingPolicy.CamelCase, - Converters = { new FlexivDataJsonConverter() } + Converters = { new FlexivDataTypesJsonConverter() } }; ThrowRdkException(error); } ~WorkCoord() => Dispose(false); - public List GetWorkCoordNames() + public List list() { FlexivError error = new(); IntPtr ptr = NativeFlexivRdk.GetWorkCoordNames(_workCoordPtr, ref error); ThrowRdkException(error); string str = Marshal.PtrToStringAnsi(ptr); NativeFlexivRdk.FreeString(ptr); - var tmp = JsonSerializer.Deserialize>(str, _options); + var tmp = JsonSerializer.Deserialize>(str, _options); string json = JsonSerializer.Serialize(tmp, _options); var ret = (List)tmp["work_coord_list"]; return new List(ret); } - public bool HasWorkCoord(string workCoordName) + public bool exist(string workCoordName) { FlexivError error = new(); int flag = NativeFlexivRdk.HasWorkCoord(_workCoordPtr, workCoordName, ref error); @@ -79,7 +79,7 @@ public bool HasWorkCoord(string workCoordName) return flag != 0; } - public double[] GetWorkCoordPose(string workCoordName) + public double[] pose(string workCoordName) { FlexivError error = new(); double x = 0, y = 0, z = 0, qw = 0, qx = 0, qy = 0, qz = 0; @@ -89,7 +89,7 @@ public double[] GetWorkCoordPose(string workCoordName) return new double[] { x, y, z, qw, qx, qy, qz }; } - public void AddWorkCoord(string workCoordName, double[] pose) + public void Add(string workCoordName, double[] pose) { if (pose.Length != FlexivConstants.kPoseSize) throw new ArgumentException("FlexivRdk AddWorkCoord pose length must be 7"); @@ -98,7 +98,7 @@ public void AddWorkCoord(string workCoordName, double[] pose) ThrowRdkException(error); } - public void UpdateWorkCoord(string workCoordName, double[] pose) + public void Update(string workCoordName, double[] pose) { if (pose.Length != FlexivConstants.kPoseSize) throw new ArgumentException("FlexivRdk UpdateWorkCoord pose length must be 7"); @@ -107,7 +107,7 @@ public void UpdateWorkCoord(string workCoordName, double[] pose) ThrowRdkException(error); } - public void RemoveWorkCoord(string workCoordName) + public void Remove(string workCoordName) { FlexivError error = new(); NativeFlexivRdk.RemoveWorkCoord(_workCoordPtr, workCoordName, ref error); diff --git a/csharp/Program.cs b/csharp/Program.cs index ac7ec0a..382a5ee 100644 --- a/csharp/Program.cs +++ b/csharp/Program.cs @@ -1,6 +1,6 @@ using System; using System.Collections.Generic; -using FlexivRdkCSharp.Examples; +using Examples; namespace FlexivRdkCSharp { @@ -21,6 +21,7 @@ class Program new Intermed2NRTJntImpCtrl(), new Intermed3NRTCartPureMotionCtrl(), new Intermed4NRTCartMotionForceCtrl(), + new Intermed5RobotDynamics(), }; static void Main(string[] args) diff --git a/csharp/csharp.sln b/csharp/csharp.sln new file mode 100644 index 0000000..a550fe0 --- /dev/null +++ b/csharp/csharp.sln @@ -0,0 +1,34 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.30114.105 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "csharp", "csharp.csproj", "{F0B6EC8D-BEB5-4E00-A958-DBA574608067}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Debug|Any CPU.ActiveCfg = Debug|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Debug|Any CPU.Build.0 = Debug|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Debug|x64.ActiveCfg = Debug|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Debug|x64.Build.0 = Debug|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Debug|x86.ActiveCfg = Debug|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Debug|x86.Build.0 = Debug|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Release|Any CPU.ActiveCfg = Release|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Release|Any CPU.Build.0 = Release|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Release|x64.ActiveCfg = Release|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Release|x64.Build.0 = Release|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Release|x86.ActiveCfg = Release|x64 + {F0B6EC8D-BEB5-4E00-A958-DBA574608067}.Release|x86.Build.0 = Release|x64 + EndGlobalSection +EndGlobal diff --git a/released_dll/flexiv_rdk.win64-vs2019.dll b/released_dll/flexiv_rdk.win64-vs2019.dll index f5f8a95..7dc05bd 100644 Binary files a/released_dll/flexiv_rdk.win64-vs2019.dll and b/released_dll/flexiv_rdk.win64-vs2019.dll differ diff --git a/third_party/build_and_install_flexiv_rdk.sh b/third_party/build_and_install_flexiv_rdk.sh index 661ef77..58a9086 100644 --- a/third_party/build_and_install_flexiv_rdk.sh +++ b/third_party/build_and_install_flexiv_rdk.sh @@ -16,14 +16,14 @@ fi cd $SCRIPTPATH/flexiv_rdk # Use specific version git fetch -p -git checkout v1.5.1 +git checkout v1.7 # git submodule update --init --recursive # Build and install dependencies cd thirdparty bash build_and_install_dependencies.sh $INSTALL_DIR/rdk_install cd .. # Configure CMake -cmake -B build -DCMAKE_PREFIX_PATH=$INSTALL_DIR/rdk_install +cmake -S . -B build -DCMAKE_PREFIX_PATH=$INSTALL_DIR/rdk_install # Build and install cmake --build build --target install --config release -j $NUM_JOBS echo ">>> Installed components: flexiv rdk" \ No newline at end of file