diff --git a/CMakeLists.txt b/CMakeLists.txt index 17506b3..2734733 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(FlexivRdkCSharp LANGUAGES C CXX) +project(FlexivRdkCSharp VERSION 1.8.0) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/build_and_install_flexiv_rdk_dll.sh b/build_and_install_flexiv_rdk_dll.sh index b6bcc91..494269f 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.7.0" +echo ">>> Build and install flexiv rdk dll v1.8.0" cd $CURRSCRIPTPATH rm -rf build/ cmake -S . -B build cmake --build build --config Release --target install -echo ">>> Installed successfully: flexiv rdk dll v1.7.0" +echo ">>> Installed successfully: flexiv rdk dll v1.8.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 c27a743..5a403df 100644 --- a/cpp_wrapper/include/wrapper_api.hpp +++ b/cpp_wrapper/include/wrapper_api.hpp @@ -47,9 +47,12 @@ struct WRobotInfo { double q_max[kSerialJointDoF]; double dq_max[kSerialJointDoF]; double tau_max[kSerialJointDoF]; + int has_FT_sensor; }; struct WRobotState { + int64_t sec; + int32_t nsec; double q[kSerialJointDoF]; double theta[kSerialJointDoF]; double dq[kSerialJointDoF]; @@ -58,6 +61,8 @@ struct WRobotState { double tau_des[kSerialJointDoF]; double tau_dot[kSerialJointDoF]; double tau_ext[kSerialJointDoF]; + double tau_interact[kSerialJointDoF]; + double temperature[kSerialJointDoF]; double tcp_pose[kPoseSize]; double tcp_vel[kCartDoF]; double flange_pose[kPoseSize]; diff --git a/cpp_wrapper/src/wrapper_api.cpp b/cpp_wrapper/src/wrapper_api.cpp index e8f81d1..97d9b54 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, int verbose, FlexivError* error) { + const char** interfaces, int interface_count, int verbose, int lite, 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, verbose); + flexiv::rdk::Robot* robot = new flexiv::rdk::Robot(robot_sn, white_list, verbose, lite); error->error_code = 0; return robot; } @@ -39,6 +39,10 @@ EXPORT_API void DeleteFlexivRobot(flexiv::rdk::Robot* robot) { } //========================================= ACCESSORS ========================================== +EXPORT_API int IsLite(Robot* robot) { + return robot->lite(); +} + EXPORT_API int IsConnected(Robot* robot) { return robot->connected(); } @@ -60,6 +64,7 @@ EXPORT_API void GetInfo(Robot* robot, WRobotInfo* info) { info->dq_max[i] = r_info.dq_max[i]; info->tau_max[i] = r_info.tau_max[i]; } + info->has_FT_sensor = r_info.has_FT_sensor; } EXPORT_API int GetMode(Robot* robot) { @@ -83,6 +88,8 @@ EXPORT_API int GetMode(Robot* robot) { EXPORT_API void GetStates(Robot* robot, WRobotState* robot_state) { const RobotStates& states = robot->states(); + robot_state->sec = states.timestamp.first; + robot_state->nsec = states.timestamp.second; for (int i = 0; i < kSerialJointDoF; ++i) { robot_state->q[i] = states.q[i]; robot_state->theta[i] = states.theta[i]; @@ -92,6 +99,8 @@ EXPORT_API void GetStates(Robot* robot, WRobotState* robot_state) { robot_state->tau_des[i] = states.tau_des[i]; robot_state->tau_dot[i] = states.tau_dot[i]; robot_state->tau_ext[i] = states.tau_ext[i]; + robot_state->tau_interact[i] = states.tau_interact[i]; + robot_state->temperature[i] = states.temperature[i]; } for (int i = 0; i < kPoseSize; ++i) { robot_state->tcp_pose[i] = states.tcp_pose[i]; @@ -281,6 +290,28 @@ EXPORT_API void LockExternalAxes(Robot* robot, int toggle, FlexivError* error) { } } +EXPORT_API void SyncWithPositioner(Robot* robot, int toggle, FlexivError* error) { + try { + robot->SyncWithPositioner(toggle); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void SetTimeLinessFailureLimit(Robot* robot, int limit, FlexivError* error) { + try { + robot->SetTimelinessFailureLimit(limit); + 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) { @@ -317,6 +348,17 @@ EXPORT_API void PausePlan(Robot* robot, int pause, FlexivError* error) { } } +EXPORT_API void StopPlan(Robot* robot, FlexivError* error) { + try { + robot->StopPlan(); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + EXPORT_API char* GetPlanList(Robot* robot, FlexivError* error) { try { const auto& plan_list = robot->plan_list(); @@ -435,15 +477,14 @@ EXPORT_API void StreamJointPosition(Robot* robot, const double* pos, int posLen, } EXPORT_API void SendJointPosition(Robot* robot, const double* pos, int posLen, const double* vel, - int velLen, const double* acc, int accLen, const double* maxVel, int maxVelLen, + int velLen, const double* maxVel, int maxVelLen, const double* maxAcc, int maxAccLen, FlexivError* error) { try { std::vector positions(pos, pos + posLen); std::vector velocities(vel, vel + velLen); - std::vector accelerations(acc, acc + accLen); std::vector max_vel(maxVel, maxVel + maxVelLen); std::vector max_acc(maxAcc, maxAcc + maxAccLen); - robot->SendJointPosition(positions, velocities, accelerations, + robot->SendJointPosition(positions, velocities, max_vel, max_acc); error->error_code = 0; } @@ -467,6 +508,30 @@ EXPORT_API void SetJointImpedance(Robot* robot, const double* Kq, int KqLen, } } +EXPORT_API void SetMaxContactTorque(Robot* robot, const double* maxTorques, int maxTorquesLen, FlexivError* error) { + try { + std::vector maxTorquesArr(maxTorques, maxTorques + maxTorquesLen); + robot->SetMaxContactTorque(maxTorquesArr); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void SetJointInertiaScale(Robot* robot, const double* inertiaScales, int inertiaScalesLen, FlexivError* error) { + try { + std::vector inertiaScalesArr(inertiaScales, inertiaScales + inertiaScalesLen); + robot->SetJointInertiaScale(inertiaScalesArr); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + EXPORT_API void StreamCartesianMotionForce(Robot* robot, const double* pose, int poseLen, const double* wrench, int wrenchLen, const double* velocity, int velocityLen, const double* acceleration, int accelerationLen, FlexivError* error) { @@ -501,7 +566,7 @@ EXPORT_API void StreamCartesianMotionForce(Robot* robot, const double* pose, int } EXPORT_API void SendCartesianMotionForce(Robot* robot, const double* pose, int poseLen, - const double* wrench, int wrenchLen, double maxLinearVel, double maxAngularVel, + const double* wrench, int wrenchLen, const double* vel, int velLen, double maxLinearVel, double maxAngularVel, double maxLinearAcc, double maxAngularAcc, FlexivError* error) { try { if (poseLen != kPoseSize) @@ -510,11 +575,16 @@ EXPORT_API void SendCartesianMotionForce(Robot* robot, const double* pose, int p if (wrenchLen != kCartDoF) throw std::invalid_argument("SendCartesianMotionForce wrench length must be " + std::to_string(kCartDoF)); + if (velLen != kCartDoF) + throw std::invalid_argument("SendCartesianMotionForce velocity length must be " + + std::to_string(kCartDoF)); std::array poseArr; std::copy(pose, pose + kPoseSize, poseArr.begin()); std::array wrenchArr{}; std::copy(wrench, wrench + kCartDoF, wrenchArr.begin()); - robot->SendCartesianMotionForce(poseArr, wrenchArr, maxLinearVel, maxAngularVel, + std::array velocityArr{}; + std::copy(vel, vel + velLen, velocityArr.begin()); + robot->SendCartesianMotionForce(poseArr, wrenchArr, velocityArr, maxLinearVel, maxAngularVel, maxLinearAcc, maxAngularAcc); error->error_code = 0; } @@ -1055,6 +1125,20 @@ EXPORT_API char* GetTrajFilesList(FileIO* fileIO, FlexivError* error) { return nullptr; } +EXPORT_API char* GetProjectsList(FileIO* fileIO, FlexivError* error) { + try { + json j = fileIO->projects_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); @@ -1090,6 +1174,28 @@ EXPORT_API void DownloadTrajFile2(FileIO* fileIO, const char* fileName, const ch } } +EXPORT_API void UploadProject(FileIO* fileIO, const char* projectDir, FlexivError* error) { + try { + fileIO->UploadProject(projectDir); + error->error_code = 0; + } + catch (const std::exception& e) { + error->error_code = 1; + CopyExceptionMsg(e, error); + } +} + +EXPORT_API void DownloadProject(FileIO* fileIO, const char* projectName, const char* saveDir, FlexivError* error) { + try { + fileIO->DownloadProject(projectName, 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 { @@ -1334,6 +1440,20 @@ EXPORT_API void DeleteModel(Model* model) { delete model; } +EXPORT_API char* GetLinkNames(Model* model, FlexivError* error) { + try { + json j = model->link_names(); + 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 Reload(Model* model, FlexivError* error) { try { model->Reload(); @@ -1372,6 +1492,20 @@ EXPORT_API void GetJacobian(Model* model, const char* linkName, double* buffer, } } +EXPORT_API void GetTransformation(Model* model, const char* linkName, double* buffer, int rows, int cols, FlexivError* error) { + try { + if (!model || !buffer || !linkName) return; + Eigen::Isometry3d t = model->T(std::string(linkName)); + if (t.rows() != rows || t.cols() != cols) return; + std::memcpy(buffer, t.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; diff --git a/csharp/Examples/Basics1DisplayRobotStates.cs b/csharp/Examples/Basics1DisplayRobotStates.cs index 0dac570..f9e0c91 100644 --- a/csharp/Examples/Basics1DisplayRobotStates.cs +++ b/csharp/Examples/Basics1DisplayRobotStates.cs @@ -43,7 +43,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { diff --git a/csharp/Examples/Basics2ClearFault.cs b/csharp/Examples/Basics2ClearFault.cs index 1d0775c..9b45a42 100644 --- a/csharp/Examples/Basics2ClearFault.cs +++ b/csharp/Examples/Basics2ClearFault.cs @@ -32,7 +32,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { diff --git a/csharp/Examples/Basics3PrimitiveExecution.cs b/csharp/Examples/Basics3PrimitiveExecution.cs index cac4216..4821a8c 100644 --- a/csharp/Examples/Basics3PrimitiveExecution.cs +++ b/csharp/Examples/Basics3PrimitiveExecution.cs @@ -35,7 +35,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { diff --git a/csharp/Examples/Basics4PlanExecution.cs b/csharp/Examples/Basics4PlanExecution.cs index ec334b1..34474e3 100644 --- a/csharp/Examples/Basics4PlanExecution.cs +++ b/csharp/Examples/Basics4PlanExecution.cs @@ -36,7 +36,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { diff --git a/csharp/Examples/Basics5ZeroFTSensor.cs b/csharp/Examples/Basics5ZeroFTSensor.cs index 1670576..3514091 100644 --- a/csharp/Examples/Basics5ZeroFTSensor.cs +++ b/csharp/Examples/Basics5ZeroFTSensor.cs @@ -36,7 +36,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { diff --git a/csharp/Examples/Basics6GripperControl.cs b/csharp/Examples/Basics6GripperControl.cs index 65ecd18..8748989 100644 --- a/csharp/Examples/Basics6GripperControl.cs +++ b/csharp/Examples/Basics6GripperControl.cs @@ -50,7 +50,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { diff --git a/csharp/Examples/Basics7AutoRecovery.cs b/csharp/Examples/Basics7AutoRecovery.cs index b769d4c..4ff74dc 100644 --- a/csharp/Examples/Basics7AutoRecovery.cs +++ b/csharp/Examples/Basics7AutoRecovery.cs @@ -35,7 +35,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Enable the robot, make sure the E-stop is released before enabling Utility.SpdlogInfo("Enabling robot ..."); robot.Enable(); diff --git a/csharp/Examples/Basics8UpdateRobotTool.cs b/csharp/Examples/Basics8UpdateRobotTool.cs index b56cd13..f3f831f 100644 --- a/csharp/Examples/Basics8UpdateRobotTool.cs +++ b/csharp/Examples/Basics8UpdateRobotTool.cs @@ -34,7 +34,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { @@ -59,7 +59,7 @@ public void Run(string[] args) // Update Robot Tool, make sure the robot is in IDLE mode robot.SwitchMode(RobotMode.IDLE); // Instantiate tool interface - var tool = new Tool(robot); + using 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.list(); diff --git a/csharp/Examples/Basics9GlobalVariables.cs b/csharp/Examples/Basics9GlobalVariables.cs index 3b61ab7..3a030dd 100644 --- a/csharp/Examples/Basics9GlobalVariables.cs +++ b/csharp/Examples/Basics9GlobalVariables.cs @@ -33,7 +33,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { diff --git a/csharp/Examples/Intermed1NRTJntPosCtrl.cs b/csharp/Examples/Intermed1NRTJntPosCtrl.cs index 12b2566..0cfc30d 100644 --- a/csharp/Examples/Intermed1NRTJntPosCtrl.cs +++ b/csharp/Examples/Intermed1NRTJntPosCtrl.cs @@ -43,7 +43,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { @@ -120,7 +120,7 @@ public void Run(string[] args) targetPos[i] = initPos[i] + SWING_AMP * Math.Sin(2 * Math.PI * SWING_FREQ * loopTime); } } - robot.SendJointPosition(targetPos, targetVel, targetAcc, maxVel, maxAcc); + robot.SendJointPosition(targetPos, targetVel, maxVel, maxAcc); loopTime += period; } } diff --git a/csharp/Examples/Intermed2NRTJntImpCtrl.cs b/csharp/Examples/Intermed2NRTJntImpCtrl.cs index 030929c..5a980d7 100644 --- a/csharp/Examples/Intermed2NRTJntImpCtrl.cs +++ b/csharp/Examples/Intermed2NRTJntImpCtrl.cs @@ -43,7 +43,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { @@ -136,7 +136,7 @@ public void Run(string[] args) Utility.SpdlogInfo("Joint stiffness reset to nominal."); } // Send commands - robot.SendJointPosition(targetPos, targetVel, targetAcc, maxVel, maxAcc); + robot.SendJointPosition(targetPos, targetVel, maxVel, maxAcc); // Increment loop counter loopCounter += 1; } diff --git a/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs b/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs index cc9ca8c..66b4f12 100644 --- a/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs +++ b/csharp/Examples/Intermed3NRTCartPureMotionCtrl.cs @@ -63,7 +63,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { @@ -101,8 +101,9 @@ public void Run(string[] args) // 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.busy()) + // Wait for the primitive completion + while (!(FlexivDataTypesUtils.TryGet(robot.primitive_states(), + "terminated", out var flag) && flag == 1)) { Thread.Sleep(1000); } @@ -219,7 +220,7 @@ public void Run(string[] args) } catch (Exception ex) { - Utility.SpdlogError(ex.Message); + Utility.SpdlogError($"Exception: {ex.Message}\n{ex.StackTrace}"); } } } diff --git a/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs b/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs index ed55dd2..7463e77 100644 --- a/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs +++ b/csharp/Examples/Intermed4NRTCartMotionForceCtrl.cs @@ -77,7 +77,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { @@ -142,7 +142,7 @@ public void Run(string[] args) targetPose[2] -= SEARCH_DISTANCE; // Send target point to robot to start searching for contact and limit the velocity. Keep // target wrench 0 at this stage since we are not doing force control yet - robot.SendCartesianMotionForce(targetPose, new double[] { 0, 0, 0, 0, 0, 0 }, SEARCH_VELOCITY); + robot.SendCartesianMotionForce(targetPose, new double[] { 0, 0, 0, 0, 0, 0 }, new double[] { 0, 0, 0, 0, 0, 0 }, SEARCH_VELOCITY); // Use a while loop to poll robot states and check if a contact is made bool IsContacted = false; while (!IsContacted) diff --git a/csharp/Examples/Intermed5RobotDynamics.cs b/csharp/Examples/Intermed5RobotDynamics.cs index b1b1222..1de441c 100644 --- a/csharp/Examples/Intermed5RobotDynamics.cs +++ b/csharp/Examples/Intermed5RobotDynamics.cs @@ -33,7 +33,7 @@ public void Run(string[] args) try { // Instantiate robot interface - var robot = new Robot(robotSN); + using var robot = new Robot(robotSN); // Clear fault on the connected robot if any if (robot.fault()) { @@ -68,7 +68,19 @@ public void Run(string[] args) // Robot Dynamics // Initialize dynamics engine - Model model = new Model(robot); + using Model model = new Model(robot); + // Update robot model in dynamics engine + model.Update(robot.states().Q, robot.states().DTheta); + var link_names = model.link_names(); + Console.WriteLine("link_names = "); + for (int i = 0; i < link_names.Count; i++) + { + Console.WriteLine($"[{i}] {link_names[i]}"); + model.Update(robot.states().Q, robot.states().DTheta); + var t = model.T(link_names[i]); + Console.WriteLine("T = "); + PrintMatrix(t); + } for (int i = 0; i < 5; ++i) { // Mark timer start point diff --git a/csharp/FlexivRdk/Data.cs b/csharp/FlexivRdk/Data.cs index a20274a..235d01b 100644 --- a/csharp/FlexivRdk/Data.cs +++ b/csharp/FlexivRdk/Data.cs @@ -386,6 +386,8 @@ public enum RobotMode : int [StructLayout(LayoutKind.Sequential)] public struct RobotStates { + public long sec; // seconds since epoch + public int nsec; // nanoseconds [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] public double[] Q; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] @@ -402,6 +404,10 @@ public struct RobotStates public double[] TauDot; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] public double[] TauExt; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] + public double[] tau_interact; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] + public double[] temperature; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kPoseSize)] public double[] TcpPose; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kCartDoF)] @@ -422,6 +428,9 @@ public struct RobotStates public override string ToString() { var sb = new System.Text.StringBuilder(); + sb.AppendLine($"Timestamp:"); + sb.AppendLine($" sec : {sec}"); + sb.AppendLine($" nsec: {nsec}"); void AppendArray(string name, double[] arr, int decimals = 5) { if (arr == null || arr.Length == 0) @@ -441,6 +450,8 @@ void AppendArray(string name, double[] arr, int decimals = 5) AppendArray(nameof(TauDes), TauDes); AppendArray(nameof(TauDot), TauDot); AppendArray(nameof(TauExt), TauExt); + AppendArray(nameof(tau_interact), tau_interact); + AppendArray(nameof(temperature), temperature); AppendArray(nameof(TcpPose), TcpPose); AppendArray(nameof(TcpVel), TcpVel); AppendArray(nameof(FlangePose), FlangePose); @@ -508,6 +519,7 @@ public struct RobotInfo public double[] DqMax; [MarshalAs(UnmanagedType.ByValArray, SizeConst = FlexivConstants.kSerialJointDoF)] public double[] TauMax; + public int has_FT_sensor; public override string ToString() { @@ -530,6 +542,7 @@ string FormatArray(double[] arr, int decimals = 5) sb.AppendLine($"QMax: {FormatArray(QMax)}"); sb.AppendLine($"DqMax: {FormatArray(DqMax)}"); sb.AppendLine($"TauMax: {FormatArray(TauMax)}"); + sb.AppendLine($"has_FT_sensor: {has_FT_sensor}"); return sb.ToString(); } } diff --git a/csharp/FlexivRdk/FileIO.cs b/csharp/FlexivRdk/FileIO.cs index 3ca33a2..25fe906 100644 --- a/csharp/FlexivRdk/FileIO.cs +++ b/csharp/FlexivRdk/FileIO.cs @@ -57,6 +57,17 @@ public List traj_files_list() return new List(ret); } + public List projects_list() + { + FlexivError error = new(); + IntPtr ptr = NativeFlexivRdk.GetProjectsList(_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(); @@ -80,5 +91,19 @@ public void DownloadTrajFile(string fileName, string saveDir) NativeFlexivRdk.DownloadTrajFile2(_fileIOPtr, fileName, saveDir, ref error); ThrowRdkException(error); } + + public void UploadProject(string projectDir) + { + FlexivError error = new(); + NativeFlexivRdk.UploadProject(_fileIOPtr, projectDir, ref error); + ThrowRdkException(error); + } + + public void DownloadProject(string projectName, string saveDir) + { + FlexivError error = new(); + NativeFlexivRdk.DownloadProject(_fileIOPtr, projectName, saveDir, ref error); + ThrowRdkException(error); + } } } diff --git a/csharp/FlexivRdk/Model.cs b/csharp/FlexivRdk/Model.cs index 39030b5..76fc48c 100644 --- a/csharp/FlexivRdk/Model.cs +++ b/csharp/FlexivRdk/Model.cs @@ -1,4 +1,7 @@ using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text.Json; namespace FlexivRdk { @@ -43,6 +46,17 @@ public Model(Robot robot, double gravityX = 0.0, double gravityY = 0.0, double g ~Model() => Dispose(false); + public List link_names() + { + FlexivError error = new(); + IntPtr ptr = NativeFlexivRdk.GetLinkNames(_modelPtr, ref error); + ThrowRdkException(error); + string str = Marshal.PtrToStringAnsi(ptr); + NativeFlexivRdk.FreeString(ptr); + List ret = JsonSerializer.Deserialize>(str); + return new List(ret); + } + public void Reload() { FlexivError error = new(); @@ -72,6 +86,21 @@ public void Update(double[] positions, double[] velocities) return result; } + public double[,] T(string linkName) + { + FlexivError error = new(); + int rows = 3; + int cols = 4; + var buffer = new double[rows * cols]; + NativeFlexivRdk.GetTransformation(_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(); diff --git a/csharp/FlexivRdk/Native.cs b/csharp/FlexivRdk/Native.cs index 2401270..ce6efd4 100644 --- a/csharp/FlexivRdk/Native.cs +++ b/csharp/FlexivRdk/Native.cs @@ -22,12 +22,15 @@ 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, int verbose, ref FlexivError error); + int interfaceCount, int verbose, int lite, ref FlexivError error); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void DeleteFlexivRobot(IntPtr robot); //========================================= ACCESSORS ========================================== + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern int IsLite(IntPtr robot); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern int IsConnected(IntPtr robot); @@ -98,6 +101,12 @@ public static extern IntPtr CreateFlexivRobot(string robotSN, [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void LockExternalAxes(IntPtr robot, int toggle, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SyncWithPositioner(IntPtr robot, int toggle, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SetTimelinessFailureLimit(IntPtr robot, int limit); + //======================================= PLAN EXECUTION ======================================= [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void ExecutePlanByIdx(IntPtr robot, int idx, int continueExec, @@ -110,6 +119,9 @@ public static extern void ExecutePlanByName(IntPtr robot, string name, int conti [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void PausePlan(IntPtr robot, int pause, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void StopPlan(IntPtr robot, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern IntPtr GetPlanList(IntPtr robot, ref FlexivError error); @@ -144,20 +156,26 @@ public static extern void StreamJointPosition(IntPtr robot, double[] pos, int po [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void SendJointPosition(IntPtr robot, double[] pos, int posLen, double[] vel, - int velLen, double[] acc, int accLen, double[] maxVel, int maxVelLen, double[] maxAcc, + int velLen, double[] maxVel, int maxVelLen, double[] maxAcc, int maxAccLen, ref FlexivError error); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void SetJointImpedance(IntPtr robot, double[] Kq, int KqLen, double[] Zq, int ZqLen, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SetMaxContactTorque(IntPtr robot, double[] maxTorques, int maxTorquesLen, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void SetJointInertiaScale(IntPtr robot, double[] inertiaScales, int inertiaScalesLen, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void StreamCartesianMotionForce(IntPtr robot, double[] pos, int posLen, double[] wrench, int wrenchLen, double[] vel, int velLen, double[] acc, int accLen, ref FlexivError error); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void SendCartesianMotionForce(IntPtr robot, double[] pose, int poseLen, - double[] wrench, int wrenchLen, double maxLinearVel, double maxAngularVel, double maxLinearAcc, + double[] wrench, int wrenchLen, double[] vel, int velLen, double maxLinearVel, double maxAngularVel, double maxLinearAcc, double maxAngularAcc, ref FlexivError error); [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] @@ -295,6 +313,9 @@ public static extern void GetWorkCoordPose(IntPtr workCoord, string name, ref do [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 IntPtr GetProjectsList(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); @@ -304,6 +325,12 @@ public static extern void GetWorkCoordPose(IntPtr workCoord, string name, ref do [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void DownloadTrajFile2(IntPtr fileIO, string fileName, string saveDir, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void UploadProject(IntPtr fileIO, string projectDir, ref FlexivError error); + + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern void DownloadProject(IntPtr fileIO, string projectName, string saveDir, ref FlexivError error); + //========================================= DEVICE =========================================== [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern IntPtr CreateDevice(IntPtr robot, ref FlexivError error); @@ -373,6 +400,9 @@ public static extern void SetJointPositionLimits(IntPtr safety, double[] minPosi [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void DeleteModel(IntPtr model); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] + public static extern IntPtr GetLinkNames(IntPtr model, ref FlexivError error); + [DllImport(k_flexivRdkDll, CallingConvention = CallingConvention.Cdecl)] public static extern void Reload(IntPtr model, ref FlexivError error); @@ -382,6 +412,9 @@ public static extern void SetJointPositionLimits(IntPtr safety, double[] minPosi [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 GetTransformation(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); diff --git a/csharp/FlexivRdk/Robot.cs b/csharp/FlexivRdk/Robot.cs index 9412067..8b6ba27 100644 --- a/csharp/FlexivRdk/Robot.cs +++ b/csharp/FlexivRdk/Robot.cs @@ -41,11 +41,16 @@ private static void ThrowRdkException(FlexivError error) } } - public Robot(string robotSN, string[] networkInterfaceWhiteList = null, bool verbose = true) + public Robot(string robotSN, string[] networkInterfaceWhiteList = null, bool verbose = true, bool lite = false) { FlexivError error = new(); string[] interfaces = networkInterfaceWhiteList ?? Array.Empty(); - _flexivRobotPtr = NativeFlexivRdk.CreateFlexivRobot(robotSN, interfaces, interfaces.Length, verbose ? 1 : 0, ref error); + _flexivRobotPtr = NativeFlexivRdk.CreateFlexivRobot(robotSN, + interfaces, + interfaces.Length, + verbose ? 1 : 0, + lite ? 1 : 0, + ref error); _options = new JsonSerializerOptions { WriteIndented = false, @@ -58,6 +63,11 @@ public Robot(string robotSN, string[] networkInterfaceWhiteList = null, bool ver ~Robot() => Dispose(false); //========================================= ACCESSORS ========================================== + public bool lite() + { + return NativeFlexivRdk.IsLite(_flexivRobotPtr) != 0; + } + public bool connected() { return NativeFlexivRdk.IsConnected(_flexivRobotPtr) != 0; @@ -212,6 +222,20 @@ public void LockExternalAxes(bool toggle) ThrowRdkException(error); } + public void SyncWithPositioner(bool toggle) + { + FlexivError error = new(); + NativeFlexivRdk.SyncWithPositioner(_flexivRobotPtr, toggle ? 1 : 0, ref error); + ThrowRdkException(error); + } + + public void SetTimelinessFailureLimit(int limit = 3) + { + FlexivError error = new(); + NativeFlexivRdk.SetTimelinessFailureLimit(_flexivRobotPtr, limit); + ThrowRdkException(error); + } + //======================================= PLAN EXECUTION ======================================= public void ExecutePlan(int index, bool continueExec = false, bool blockUntilStarted = true) { @@ -234,6 +258,13 @@ public void PausePlan(bool pause) ThrowRdkException(error); } + public void StopPlan() + { + FlexivError error = new(); + NativeFlexivRdk.StopPlan(_flexivRobotPtr, ref error); + ThrowRdkException(error); + } + public List plan_list() { FlexivError error = new(); @@ -318,12 +349,12 @@ public void StreamJointPosition(double[] positions, double[] velocities, double[ ThrowRdkException(error); } - public void SendJointPosition(double[] positions, double[] velocities, double[] accelerations, + public void SendJointPosition(double[] positions, double[] velocities, double[] maxVel, double[] maxAcc) { FlexivError error = new(); NativeFlexivRdk.SendJointPosition(_flexivRobotPtr, positions, positions.Length, velocities, - velocities.Length, accelerations, accelerations.Length, maxVel, maxVel.Length, + velocities.Length, maxVel, maxVel.Length, maxAcc, maxAcc.Length, ref error); ThrowRdkException(error); } @@ -336,6 +367,20 @@ public void SetJointImpedance(double[] Kq, double[] Zq = null) ThrowRdkException(error); } + public void SetMaxContactTorque(double[] maxTorques) + { + FlexivError error = new(); + NativeFlexivRdk.SetNullSpacePosture(_flexivRobotPtr, maxTorques, maxTorques.Length, ref error); + ThrowRdkException(error); + } + + public void SetJointInertiaScale(double[] inertia_scales) + { + FlexivError error = new(); + NativeFlexivRdk.SetJointInertiaScale(_flexivRobotPtr, inertia_scales, inertia_scales.Length, ref error); + ThrowRdkException(error); + } + public void StreamCartesianMotionForce(double[] pose, double[] wrench = null, double[] velocity = null, double[] acceleration = null) { @@ -348,13 +393,14 @@ public void StreamCartesianMotionForce(double[] pose, ThrowRdkException(error); } - public void SendCartesianMotionForce(double[] pose, double[] wrench = null, double maxLinearVel = 0.5, - double maxAngularVel = 1.0, double maxLinearAcc = 2.0, double maxAngularAcc = 5.0) + public void SendCartesianMotionForce(double[] pose, double[] wrench = null, double[] velocity = null, + double maxLinearVel = 0.5, double maxAngularVel = 1.0, double maxLinearAcc = 2.0, double maxAngularAcc = 5.0) { FlexivError error = new(); if (wrench == null) wrench = new double[] { 0, 0, 0, 0, 0, 0 }; + if (velocity == null) velocity = new double[6]; NativeFlexivRdk.SendCartesianMotionForce(_flexivRobotPtr, pose, pose.Length, wrench, wrench.Length, - maxLinearVel, maxAngularVel, maxLinearAcc, maxAngularAcc, ref error); + velocity, velocity.Length, maxLinearVel, maxAngularVel, maxLinearAcc, maxAngularAcc, ref error); ThrowRdkException(error); } diff --git a/csharp/Program.cs b/csharp/Program.cs index 382a5ee..02adf23 100644 --- a/csharp/Program.cs +++ b/csharp/Program.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Threading; using Examples; namespace FlexivRdkCSharp @@ -44,7 +45,12 @@ static void Main(string[] args) Console.WriteLine($"Unknown example: {selected}"); return; } - example.Run(args[1..]); + var thread = new Thread( + () => example.Run(args[1..]) , + 16 * 1024 * 1024 // 16MB stack size + ); + thread.Start(); + thread.Join(); } } } diff --git a/released_dll/flexiv_rdk.win64-vs2019.dll b/released_dll/flexiv_rdk.win64-vs2019.dll index 7dc05bd..0f7451e 100644 Binary files a/released_dll/flexiv_rdk.win64-vs2019.dll and b/released_dll/flexiv_rdk.win64-vs2019.dll differ