Initial import of ORISYS arm workstation
This commit is contained in:
cheng
+896
@@ -0,0 +1,896 @@
|
||||
/**
|
||||
* @file data.hpp
|
||||
* @brief Header file containing various constant expressions, data structures, and enums.
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_DATA_HPP_
|
||||
#define FLEXIV_RDK_DATA_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <ostream>
|
||||
#include <variant>
|
||||
#include <chrono>
|
||||
#include <map>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
/** Cartesian-space degrees of freedom */
|
||||
constexpr size_t kCartDoF = 6;
|
||||
|
||||
/** Joint-space degrees of freedom of Flexiv's serial robots */
|
||||
constexpr size_t kSerialJointDoF = 7;
|
||||
|
||||
/** Size of pose array (3 position + 4 quaternion) */
|
||||
constexpr size_t kPoseSize = 7;
|
||||
|
||||
/** Number of digital IO ports: 16 on the control box + 2 in each wrist connector * maximum 2 wrists
|
||||
*/
|
||||
constexpr size_t kIOPorts = 16 + 2 * 2;
|
||||
|
||||
/** Maximum number of external axes */
|
||||
constexpr size_t kMaxExtAxes = 6;
|
||||
|
||||
/**
|
||||
* @brief All supported product models of the robot.
|
||||
*/
|
||||
enum class ProductModel
|
||||
{
|
||||
UNKNOWN = 0,
|
||||
Enlight_L, /// Enlight-L standard version
|
||||
Enlight_LL, /// Enlight-LL: Dual Enlight-L with customizable mounting poses
|
||||
MICO_Core, /// MICO-Core: Dual Enlight-L with fixed-mounting upper body
|
||||
MICO_Plus, /// MICO-Plus: MICO-Core with pan-tilt torso
|
||||
MICO_Ultra, /// MICO-Ultra: MICO-Plus with mobile base
|
||||
};
|
||||
|
||||
/** Map ProductModel enums to strings */
|
||||
inline const std::map<ProductModel, std::string> kProductModelNames
|
||||
= {{ProductModel::UNKNOWN, "UNKNOWN"}, {ProductModel::Enlight_L, "Enlight-L"},
|
||||
{ProductModel::Enlight_LL, "Enlight-LL"}, {ProductModel::MICO_Core, "MICO-Core"},
|
||||
{ProductModel::MICO_Plus, "MICO-Plus"}, {ProductModel::MICO_Ultra, "MICO-Ultra"}};
|
||||
|
||||
/**
|
||||
* @brief All possible joint groups of the robot.
|
||||
*/
|
||||
enum class JointGroup
|
||||
{
|
||||
UNKNOWN = -1, ///< Unknown group
|
||||
ALL = 0, ///< The full system, including all actuated joints
|
||||
ARMS = 2, ///< The dual arms as a whole, only applicable to dual-arm robots
|
||||
ARM_1 = 3, ///< The 1st single arm in a dual-arm robot or the only arm in a single-arm robot
|
||||
ARM_2 = 4, ///< The 2nd single arm in a dual-arm robot, not applicable to single-arm robots
|
||||
EXT_AXIS = 5, ///< External axis(es) for workspace extension
|
||||
|
||||
FIRST = ALL,
|
||||
LAST = EXT_AXIS,
|
||||
};
|
||||
|
||||
/** Map JointGroup enums to strings */
|
||||
inline const std::map<JointGroup, std::string> kJointGroupNames {
|
||||
{JointGroup::UNKNOWN, "UNKNOWN"},
|
||||
{JointGroup::ALL, "ALL"},
|
||||
{JointGroup::ARMS, "ARMS"},
|
||||
{JointGroup::ARM_1, "ARM_1"},
|
||||
{JointGroup::ARM_2, "ARM_2"},
|
||||
{JointGroup::EXT_AXIS, "EXT_AXIS"},
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief All possible operational statuses of the robot. Except for the first two, the other
|
||||
* enumerators indicate the cause of the robot being not ready to operate.
|
||||
* @see Robot::operational_status().
|
||||
*/
|
||||
enum class OperationalStatus
|
||||
{
|
||||
UNKNOWN = 0, ///< Unknown status.
|
||||
READY, ///< Ready to be operated.
|
||||
BOOTING, ///< System still booting, please wait.
|
||||
ESTOP_NOT_RELEASED, ///< E-Stop is not released.
|
||||
NOT_SERVO_ON, ///< Not servo on, call ServoOn() to send the signal.
|
||||
RELEASING_BRAKE, ///< Brake release in progress, please wait.
|
||||
MINOR_FAULT, ///< Minor fault occurred, call ClearFault() to try clearing it.
|
||||
CRITICAL_FAULT, ///< Critical fault occurred, call ClearFault() to try clearing it.
|
||||
IN_REDUCED_STATE, ///< In reduced state, see reduced().
|
||||
IN_RECOVERY_STATE, ///< In recovery state, see recovery().
|
||||
IN_MANUAL_MODE, ///< In Manual mode, need to switch to Auto (Remote) mode.
|
||||
IN_AUTO_MODE, ///< In regular Auto mode, need to switch to Auto (Remote) mode.
|
||||
};
|
||||
|
||||
/** Map OperationalStatus enums to strings */
|
||||
inline const std::map<OperationalStatus, std::string> kOpStatusNames {
|
||||
{OperationalStatus::UNKNOWN, "Unknown status"},
|
||||
{OperationalStatus::READY, "Ready"},
|
||||
{OperationalStatus::BOOTING, "System booting"},
|
||||
{OperationalStatus::ESTOP_NOT_RELEASED, "E-Stop not released"},
|
||||
{OperationalStatus::NOT_SERVO_ON, "Not servo on"},
|
||||
{OperationalStatus::RELEASING_BRAKE, "Releasing brakes"},
|
||||
{OperationalStatus::MINOR_FAULT, "Minor fault occurred"},
|
||||
{OperationalStatus::CRITICAL_FAULT, "Critical fault occurred"},
|
||||
{OperationalStatus::IN_REDUCED_STATE, "In reduced state"},
|
||||
{OperationalStatus::IN_RECOVERY_STATE, "In recovery state"},
|
||||
{OperationalStatus::IN_MANUAL_MODE, "In Manual mode"},
|
||||
{OperationalStatus::IN_AUTO_MODE, "In regular Auto mode"},
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Type of commonly-used reference coordinates.
|
||||
*/
|
||||
enum class CoordType
|
||||
{
|
||||
WORLD, ///< World frame (fixed).
|
||||
TCP, ///< TCP frame (move with the robot's end effector).
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RobotEvent
|
||||
* @brief Information about a robot event.
|
||||
* @see Robot::event_log().
|
||||
*/
|
||||
struct RobotEvent
|
||||
{
|
||||
enum Level
|
||||
{
|
||||
UNKNOWN = 0, ///< Not set
|
||||
INFO, ///< Informational event
|
||||
WARNING, ///< Warning event
|
||||
ERROR, ///< Error event
|
||||
CRITICAL, ///< Critical error event
|
||||
};
|
||||
|
||||
/** Level of the event */
|
||||
Level level = UNKNOWN;
|
||||
|
||||
/** Unique ID of the event */
|
||||
int id = 0;
|
||||
|
||||
/** Brief description of the event */
|
||||
std::string description = "";
|
||||
|
||||
/** Consequences caused by the event */
|
||||
std::string consequences = "";
|
||||
|
||||
/** Probable causes of the event */
|
||||
std::string probable_causes = "";
|
||||
|
||||
/** Recommended actions after the event */
|
||||
std::string recommended_actions = "";
|
||||
|
||||
/** Timestamp (since epoch) of the event */
|
||||
std::chrono::time_point<std::chrono::system_clock> timestamp;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RobotInfo
|
||||
* @brief General information about the connected robot.
|
||||
* @see Robot::info().
|
||||
*/
|
||||
struct RobotInfo
|
||||
{
|
||||
/** Product model of the robot */
|
||||
ProductModel product_model = {};
|
||||
|
||||
/** Serial number of the robot */
|
||||
std::string serial_num = {};
|
||||
|
||||
/** Software version of the robot */
|
||||
std::string software_ver = {};
|
||||
|
||||
/** Type of license */
|
||||
std::string license_type = {};
|
||||
|
||||
/** All available joint groups in the robot and their names */
|
||||
std::map<JointGroup, std::string> all_groups = {};
|
||||
|
||||
/** Available single-arm joint groups in the robot and their names */
|
||||
std::map<JointGroup, std::string> single_arm_groups = {};
|
||||
|
||||
/** Joint-space degrees of freedom for each joint group */
|
||||
std::map<JointGroup, size_t> DoF = {};
|
||||
|
||||
/** Nominal Cartesian motion stiffness of all available single-arm joint groups when in
|
||||
* Cartesian motion-force control modes: \f$ K_x^{nom} \in \mathbb{R}^{6 \times 1} \f$. Consists
|
||||
* of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$ \mathbb{R}^{3 \times 1} \f$
|
||||
* angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T \f$.
|
||||
* Unit: \f$ [N/m]:[Nm/rad] \f$. */
|
||||
std::map<JointGroup, std::array<double, kCartDoF>> K_x_nom = {};
|
||||
|
||||
/** Nominal joint motion stiffness of all available single-arm joint groups when in joint
|
||||
* impedance control modes: \f$ K_q^{nom} \in \mathbb{R}^{n \times 1} \f$.
|
||||
* Unit: \f$ [Nm/rad] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> K_q_nom = {};
|
||||
|
||||
/** Lower software limits of joint positions of all available joint groups: \f$ q_{min} \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> q_min = {};
|
||||
|
||||
/** Upper software limits of joint positions of all available joint groups: \f$ q_{max} \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> q_max = {};
|
||||
|
||||
/** Upper software limits of joint velocities of all available joint groups: \f$ \dot{q}_{max}
|
||||
* \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> dq_max = {};
|
||||
|
||||
/** Upper software limits of joint torques of all available joint groups: \f$ \tau_{max} \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> tau_max = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RobotStates
|
||||
* @brief Robot states data in joint and Cartesian space for a joint group.
|
||||
*/
|
||||
struct RobotStates
|
||||
{
|
||||
/** Current time since epoch of the robot system. The pair consists of {seconds since epoch,
|
||||
* nanoseconds since last full second} */
|
||||
std::pair<int, int> timestamp = {};
|
||||
|
||||
/**
|
||||
* Measured joint positions from the link-side encoder: \f$ q \in \mathbb{R}^{n \times 1} \f$.
|
||||
* This is the direct measurement of joint positions. Unit: \f$ [rad] or [m] \f$.
|
||||
* @note If a joint has only one encoder, then \f$ \theta = q \f$.
|
||||
*/
|
||||
std::vector<double> q = {};
|
||||
|
||||
/**
|
||||
* Measured joint positions from the motor-side encoder: \f$ \theta \in \mathbb{R}^{n \times 1}
|
||||
* \f$. This is the indirect measurement of joint positions. \f$ \theta = q + \Delta \f$, where
|
||||
* \f$ \Delta \f$ is the joint's internal deflection between motor and link. Unit: \f$ [rad] or
|
||||
* [m] \f$.
|
||||
* @note If a joint has only one encoder, then \f$ \theta = q \f$.
|
||||
*/
|
||||
std::vector<double> theta = {};
|
||||
|
||||
/**
|
||||
* Measured joint velocities from the link-side encoder: \f$ \dot{q} \in \mathbb{R}^{n \times
|
||||
* 1} \f$. This is the direct but more noisy measurement of joint velocities. Unit: \f$ [rad/s]
|
||||
* or [m/s] \f$.
|
||||
* @note If a joint has only one encoder, then \f$ \dot{\theta} = \dot{q} \f$.
|
||||
*/
|
||||
std::vector<double> dq = {};
|
||||
|
||||
/**
|
||||
* Measured joint velocities from the motor-side encoder: \f$ \dot{\theta} \in \mathbb{R}^{n
|
||||
* \times 1} \f$. This is the indirect but less noisy measurement of joint velocities. Unit: \f$
|
||||
* [rad/s] or [m/s] \f$.
|
||||
* @note If a joint has only one encoder, then \f$ \dot{\theta} = \dot{q} \f$.
|
||||
*/
|
||||
std::vector<double> dtheta = {};
|
||||
|
||||
/**
|
||||
* Measured joint torques: \f$ \tau \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
|
||||
* @note If a joint has no torque measurement, the corresponding value will be 0.
|
||||
*/
|
||||
std::vector<double> tau = {};
|
||||
|
||||
/**
|
||||
* Numerical derivative of measured joint torques: \f$ \dot{\tau} \in \mathbb{R}^{n \times 1}
|
||||
* \f$. Unit: \f$ [Nm/s] \f$.
|
||||
* @note If a joint has no torque measurement, the corresponding value will be 0.
|
||||
*/
|
||||
std::vector<double> tau_dot = {};
|
||||
|
||||
/**
|
||||
* Estimated external joint torques: \f$ \hat \tau_{ext} \in \mathbb{R}^{n \times 1} \f$.
|
||||
* Produced by any external contact (with robot body or end-effector) that does not belong to
|
||||
* the known robot model. Unit: \f$ [Nm] \f$.
|
||||
* @note If a joint has no torque measurement, the corresponding value will be 0.
|
||||
*/
|
||||
std::vector<double> tau_ext = {};
|
||||
|
||||
/**
|
||||
* Estimated interaction joint torques: \f$ \hat \tau_{int} \in \mathbb{R}^{n \times 1} \f$.
|
||||
* Produced by any interaction forces at the TCP. Unit: \f$ [Nm] \f$.
|
||||
* @note If a joint has no torque measurement, the corresponding value will be 0.
|
||||
*/
|
||||
std::vector<double> tau_interact = {};
|
||||
|
||||
/**
|
||||
* Measured joint temperatures: \f$ temp \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [°C] \f$.
|
||||
* @note If a joint has no temperature measurement, the corresponding value will be 0.
|
||||
*/
|
||||
std::vector<double> temperature = {};
|
||||
|
||||
/**
|
||||
* Measured flange pose w.r.t. world frame: \f$ ^{O}T_{flange} \in \mathbb{R}^{7 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
|
||||
* quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
|
||||
*/
|
||||
std::array<double, kPoseSize> flange_pose = {};
|
||||
|
||||
/**
|
||||
* Measured TCP pose w.r.t. world frame: \f$ ^{O}T_{TCP} \in \mathbb{R}^{7 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
|
||||
* quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
|
||||
*/
|
||||
std::array<double, kPoseSize> tcp_pose = {};
|
||||
|
||||
/**
|
||||
* Measured TCP twist w.r.t. world frame: \f$ ^{O}\dot{X} \in \mathbb{R}^{6 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear velocity and \f$ \mathbb{R}^{3 \times 1}
|
||||
* \f$ angular velocity: \f$ [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. Unit: \f$
|
||||
* [m/s]:[rad/s] \f$.
|
||||
*/
|
||||
std::array<double, kCartDoF> tcp_twist = {};
|
||||
|
||||
/**
|
||||
* Measured or estimated external wrench applied on TCP w.r.t. world frame: \f$ ^{O}F_{ext} \in
|
||||
* \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
|
||||
* \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
|
||||
* Unit: \f$ [N]:[Nm] \f$.
|
||||
*/
|
||||
std::array<double, kCartDoF> tcp_wrench = {};
|
||||
|
||||
/**
|
||||
* Measured or estimated external wrench applied on TCP w.r.t. local frame: \f$ ^{TCP}F_{ext}
|
||||
* \in \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
|
||||
* \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
|
||||
* Unit: \f$ [N]:[Nm] \f$.
|
||||
*/
|
||||
std::array<double, kCartDoF> tcp_wrench_local = {};
|
||||
|
||||
/**
|
||||
* Unfiltered tcp_wrench. The data is more noisy but has no filter latency.
|
||||
*/
|
||||
std::array<double, kCartDoF> raw_tcp_wrench = {};
|
||||
|
||||
/**
|
||||
* Unfiltered tcp_wrench_local. The data is more noisy but has no filter latency.
|
||||
*/
|
||||
std::array<double, kCartDoF> raw_tcp_wrench_local = {};
|
||||
|
||||
/**
|
||||
* Raw reading from the force-torque (FT) sensor w.r.t. flange frame: \f$ ^{flange}F_{raw}
|
||||
* \in \mathbb{R}^{6 \times 1} \f$. The value is 0 if no FT sensor is installed. Consists of
|
||||
* \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$
|
||||
* [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$ [N]:[Nm] \f$.
|
||||
*/
|
||||
std::array<double, kCartDoF> raw_ft_sensor = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RobotActions
|
||||
* @brief Robot actions data in joint and Cartesian space for a joint group.
|
||||
*/
|
||||
struct RobotActions
|
||||
{
|
||||
/** Current time since epoch of the robot system. The pair consists of {seconds since epoch,
|
||||
* nanoseconds since last full second} */
|
||||
std::pair<int, int> timestamp = {};
|
||||
|
||||
/**
|
||||
* Desired joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] or [m] \f$.
|
||||
*/
|
||||
std::vector<double> q_d = {};
|
||||
|
||||
/**
|
||||
* Desired joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] or
|
||||
* [m/s] \f$.
|
||||
*/
|
||||
std::vector<double> dq_d = {};
|
||||
|
||||
/**
|
||||
* Desired joint torques excluding the compensation of nonlinear dynamics: \f$ \tau_d \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
|
||||
* @note Nonlinear dynamics include: gravity, centrifugal, and Coriolis. If the robot is static,
|
||||
* tau_d will be zeros.
|
||||
* @note If a joint has no torque control capability, the corresponding value will be 0.
|
||||
*/
|
||||
std::vector<double> tau_d = {};
|
||||
|
||||
/**
|
||||
* Desired TCP pose w.r.t. world frame: \f$ {^{O}T_{TCP}}_d \in \mathbb{R}^{7 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
|
||||
* quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]~[] \f$.
|
||||
*/
|
||||
std::array<double, kPoseSize> tcp_pose_d = {};
|
||||
|
||||
/**
|
||||
* Desired TCP twist w.r.t. world frame: \f$ {^{O}\dot{X}}_d \in \mathbb{R}^{6 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear velocity and \f$ \mathbb{R}^{3 \times 1}
|
||||
* \f$ angular velocity: \f$ [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. Unit: \f$
|
||||
* [m/s]:[rad/s] \f$.
|
||||
*/
|
||||
std::array<double, kCartDoF> tcp_twist_d = {};
|
||||
|
||||
/**
|
||||
* Desired TCP wrench w.r.t. the current force control frame: \f$ {^{ctrl}F_{ext}}_d \in
|
||||
* \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
|
||||
* \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$
|
||||
* [N]:[Nm] \f$.
|
||||
*/
|
||||
std::array<double, kCartDoF> tcp_wrench_d = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct PlanInfo
|
||||
* @brief Information of the on-going primitive/plan.
|
||||
* @see Robot::plan_info().
|
||||
*/
|
||||
struct PlanInfo
|
||||
{
|
||||
/** Current primitive name */
|
||||
std::string pt_name = {};
|
||||
|
||||
/** Current node name */
|
||||
std::string node_name = {};
|
||||
|
||||
/** Current node path */
|
||||
std::string node_path = {};
|
||||
|
||||
/** Current node path time period */
|
||||
std::string node_path_time_period = {};
|
||||
|
||||
/** Current node path number */
|
||||
std::string node_path_number = {};
|
||||
|
||||
/** Assigned plan name */
|
||||
std::string assigned_plan_name = {};
|
||||
|
||||
/** Velocity scale */
|
||||
double velocity_scale = {};
|
||||
|
||||
/** Waiting for user signal to step the breakpoint */
|
||||
bool waiting_for_step = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct JPos
|
||||
* @brief Data structure representing the customized data type "JPOS" in Flexiv Elements.
|
||||
* @warning Here [m] is used as the unit of length, whereas [mm] is used in Flexiv Elements. The
|
||||
* conversion is automatically done when exchanging "JPOS" data type with the robot via functions
|
||||
* like Robot::ExecutePrimitive(), Robot::SetGlobalVariables(), etc.
|
||||
*/
|
||||
struct JPos
|
||||
{
|
||||
/** Default constructor */
|
||||
JPos() = default;
|
||||
|
||||
/**
|
||||
* @brief Custom constructor.
|
||||
* @param[in] q_m Sets struct member [q_m].
|
||||
* @param[in] q_e Sets struct member [q_e]. Leave empty if there's no external axis.
|
||||
*/
|
||||
JPos(const std::array<double, kSerialJointDoF>& q_m,
|
||||
const std::array<double, kMaxExtAxes>& q_e = {})
|
||||
: q_m(q_m)
|
||||
, q_e(q_e)
|
||||
{
|
||||
}
|
||||
|
||||
/** Joint positions of the robot manipulator. Unit: [degree] */
|
||||
std::array<double, kSerialJointDoF> q_m = {};
|
||||
|
||||
/** Joint positions (linear or angular) of the external axes. Unit: [m] or [degree]
|
||||
* @note If the number of external axes \f$ n_e < kMaxExtAxes \f$, set the first \f$ n_e \f$
|
||||
* elements and leave the rest 0. Leave the whole array empty if there's no external axis. */
|
||||
std::array<double, kMaxExtAxes> q_e = {};
|
||||
|
||||
/** String representation of all data in the struct, separated by space */
|
||||
std::string str() const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct Coord
|
||||
* @brief Data structure representing the customized data type "COORD" in Flexiv Elements.
|
||||
* @warning Here [m] is used as the unit of length, whereas [mm] is used in Flexiv Elements. The
|
||||
* conversion is automatically done when exchanging "COORD" data type with the robot via functions
|
||||
* like Robot::ExecutePrimitive(), Robot::SetGlobalVariables(), Robot::global_variables(), etc.
|
||||
*/
|
||||
struct Coord
|
||||
{
|
||||
/** Default constructor */
|
||||
Coord() = default;
|
||||
|
||||
/**
|
||||
* @brief Custom constructor.
|
||||
* @param[in] position Sets struct member [position].
|
||||
* @param[in] orientation Sets struct member [orientation].
|
||||
* @param[in] ref_frame Sets struct member [ref_frame].
|
||||
* @param[in] ref_q_m Sets struct member [ref_q_m]. Leave empty to use default values.
|
||||
* @param[in] ref_q_e Sets struct member [ref_q_e]. Leave empty if there's no external axis.
|
||||
*/
|
||||
Coord(const std::array<double, kCartDoF / 2>& position,
|
||||
const std::array<double, kCartDoF / 2>& orientation,
|
||||
const std::array<std::string, 2>& ref_frame,
|
||||
const std::array<double, kSerialJointDoF>& ref_q_m = {},
|
||||
const std::array<double, kMaxExtAxes>& ref_q_e = {})
|
||||
: position(position)
|
||||
, orientation(orientation)
|
||||
, ref_frame(ref_frame)
|
||||
, ref_q_m(ref_q_m)
|
||||
, ref_q_e(ref_q_e)
|
||||
{
|
||||
}
|
||||
|
||||
/** Position in [ref_frame]. Unit: [m] */
|
||||
std::array<double, kCartDoF / 2> position = {};
|
||||
|
||||
/** Orientation in terms of Euler angles in [ref_frame]. Unit: [degree] */
|
||||
std::array<double, kCartDoF / 2> orientation = {};
|
||||
|
||||
/** Name of the reference frame "root::branch" represented as {"root", "branch"}.
|
||||
* Refer to Flexiv Elements for available options. Some common ones are:
|
||||
* - World origin: {"WORLD", "WORLD_ORIGIN"}
|
||||
* - Current pose: {"TRAJ", "START"}
|
||||
* - A work coordinate: {"WORK", "WorkCoord0"}
|
||||
* - A global variable: {"GVAR", "MyCoord0"}
|
||||
*/
|
||||
std::array<std::string, 2> ref_frame = {};
|
||||
|
||||
/** Reference joint positions of the robot manipulator. Only effective on robots with redundant
|
||||
* degrees of freedom. Unit: [degree]
|
||||
* @note Leave empty to use default values. However, this array cannot be empty if [ref_q_e] has
|
||||
* values */
|
||||
std::array<double, kSerialJointDoF> ref_q_m = {};
|
||||
|
||||
/** Reference joint positions (linear or angular) of the external axes. Only effective on
|
||||
* robots with redundant degrees of freedom and external axes. Unit: [m] or [degree]
|
||||
* @note If the number of external axes \f$ n_e < kMaxExtAxes \f$, set the first \f$ n_e \f$
|
||||
* elements and leave the rest 0. Leave the whole array empty if there's no external axis. */
|
||||
std::array<double, kMaxExtAxes> ref_q_e = {};
|
||||
|
||||
/** String representation of all data in the struct, separated by space */
|
||||
std::string str() const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Modes for synchronous motions.
|
||||
* @see PrimitiveArgs::sync_motion_mode
|
||||
*/
|
||||
enum class SyncMotionMode
|
||||
{
|
||||
DISABLE = 0, ///< Don't sync with any target
|
||||
ARM1_TCP = 1, ///< Sync with arm1 tcp
|
||||
ARM2_TCP = 2, ///< sync with arm2 tcp
|
||||
POSITIONER = 3, ///< sync with positioner
|
||||
};
|
||||
|
||||
/** Alias of the variant that holds all possible types of data exchanged with Flexiv robots */
|
||||
using FlexivDataTypes = std::variant<int, double, std::string, rdk::JPos, rdk::Coord,
|
||||
std::vector<int>, std::vector<double>, std::vector<std::string>, std::vector<rdk::JPos>,
|
||||
std::vector<rdk::Coord>>;
|
||||
|
||||
/**
|
||||
* @struct PrimitiveArgs
|
||||
* @brief Arguments of a primitive command.
|
||||
* @see Robot::ExecutePrimitive().
|
||||
* @see [Flexiv Primitives documentation](https://www.flexiv.com/primitives/)
|
||||
*/
|
||||
struct PrimitiveArgs
|
||||
{
|
||||
/** Default constructor */
|
||||
PrimitiveArgs() = default;
|
||||
|
||||
/** Custom constructor */
|
||||
PrimitiveArgs(
|
||||
const std::string& pt_name, const std::map<std::string, FlexivDataTypes>& input_params)
|
||||
: pt_name(pt_name)
|
||||
, input_params(input_params)
|
||||
{
|
||||
}
|
||||
|
||||
/** Name of the primitive to execute. For example, "Home", "MoveL", "ZeroFTSensor", etc. */
|
||||
std::string pt_name = {};
|
||||
|
||||
/** Input parameter names and values of this primitive. Use int 1 and 0 to represent booleans.
|
||||
* E.g. {{"target", rdk::Coord({0.65, -0.3, 0.2}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})},
|
||||
* {"vel", 0.6}, {"zoneRadius", "Z50"}}.
|
||||
* @warning The primitive input parameters may not use SI units, please refer to the Flexiv
|
||||
* Primitives documentation for exact unit definition. */
|
||||
std::map<std::string, FlexivDataTypes> input_params = {};
|
||||
|
||||
/** True: The external axis motion is actively controlled by this primitive. When this primitive
|
||||
* is executed, the external axis will move to the position defined in target points or
|
||||
* waypoints. False: The external axis motion is not actively controlled by this primitive.
|
||||
* @note This setting is only effective when the external axis exists. */
|
||||
bool external_axis_control = true;
|
||||
|
||||
/** Mode for synchronous motions between the TCP of this primitive and the selected arm or
|
||||
* positioner. Once configured, the motion of the TCP will be automatically adjusted in
|
||||
* accordance with the movement of the selected arm or positioner.
|
||||
* @note Only certain primitives support this feature, this setting will be ignored if this
|
||||
* primitive does not support it. */
|
||||
SyncMotionMode sync_motion_mode = SyncMotionMode::DISABLE;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct PrimitiveStates
|
||||
* @brief States data of a primitive.
|
||||
* @see Robot::primitive_states().
|
||||
*/
|
||||
struct PrimitiveStates
|
||||
{
|
||||
/** Name of the currently running primitive */
|
||||
std::string pt_name = {};
|
||||
|
||||
/** Names and corresponding values of this primitive's states. Booleans are represented by int 1
|
||||
* and 0. For example:
|
||||
* {{"reachedTarget", 1}, {"timePeriod", 5.6}, {"forceOffset", {0.1, 0.2, -1.3}}}.
|
||||
*/
|
||||
std::map<std::string, FlexivDataTypes> names_and_values = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RtJointTorqueCmd
|
||||
* @brief Commands data for real-time joint torque control.
|
||||
* @see Robot::StreamJointTorque().
|
||||
*/
|
||||
struct RtJointTorqueCmd
|
||||
{
|
||||
/** Default constructor */
|
||||
RtJointTorqueCmd() = default;
|
||||
|
||||
/** Custom constructor */
|
||||
RtJointTorqueCmd(const std::vector<double>& tau_d, bool enable_gravity_comp = true,
|
||||
bool enable_soft_limits = true, double friction_comp_scale = 100.0)
|
||||
: tau_d(tau_d)
|
||||
, enable_gravity_comp(enable_gravity_comp)
|
||||
, enable_soft_limits(enable_soft_limits)
|
||||
, friction_comp_scale(friction_comp_scale)
|
||||
{
|
||||
}
|
||||
|
||||
/** Target joint torques: \f$ \tau_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$ */
|
||||
std::vector<double> tau_d = {};
|
||||
|
||||
/** Enable/disable robot gravity compensation */
|
||||
bool enable_gravity_comp = true;
|
||||
|
||||
/** Enable/disable soft limits to keep the joints from moving outside allowed position range,
|
||||
* which will trigger a safety fault that requires recovery operation */
|
||||
bool enable_soft_limits = true;
|
||||
|
||||
/** Percentage of joint friction to be compensated. Valid range: [0, 100]. Setting to 100 means
|
||||
* to compensate all joint friction, and 0 means no friction compensation at all.
|
||||
* Under-compensation increases natural damping of the joints, which can be useful in some
|
||||
* cases, e.g. zero-torque floating. */
|
||||
double friction_comp_scale = 100.0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RtJointPositionCmd
|
||||
* @brief Commands data for real-time joint position control.
|
||||
* @see Robot::StreamJointPosition().
|
||||
*/
|
||||
struct RtJointPositionCmd
|
||||
{
|
||||
/** Default constructor */
|
||||
RtJointPositionCmd() = default;
|
||||
|
||||
/** Custom constructor */
|
||||
RtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
|
||||
const std::vector<double>& ddq_d)
|
||||
: q_d(q_d)
|
||||
, dq_d(dq_d)
|
||||
, ddq_d(ddq_d)
|
||||
{
|
||||
}
|
||||
|
||||
/** Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$ */
|
||||
std::vector<double> q_d = {};
|
||||
|
||||
/** Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s]
|
||||
* \f$ */
|
||||
std::vector<double> dq_d = {};
|
||||
|
||||
/** Target joint accelerations: \f$ \ddot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$
|
||||
* [rad/s^2] \f$ */
|
||||
std::vector<double> ddq_d = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct NrtJointPositionCmd
|
||||
* @brief Commands data for non-real-time joint position control.
|
||||
* @see Robot::SendJointPosition().
|
||||
*/
|
||||
struct NrtJointPositionCmd
|
||||
{
|
||||
/** Default constructor */
|
||||
NrtJointPositionCmd() = default;
|
||||
|
||||
/** Custom constructor */
|
||||
NrtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
|
||||
const std::vector<double>& dq_max, const std::vector<double>& ddq_max)
|
||||
: q_d(q_d)
|
||||
, dq_d(dq_d)
|
||||
, dq_max(dq_max)
|
||||
, ddq_max(ddq_max)
|
||||
{
|
||||
}
|
||||
|
||||
/** Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$ */
|
||||
std::vector<double> q_d = {};
|
||||
|
||||
/** Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Each joint will
|
||||
* maintain this amount of velocity when it reaches the target position. Unit: \f$ [rad/s] \f$
|
||||
*/
|
||||
std::vector<double> dq_d = {};
|
||||
|
||||
/** Maximum joint velocities for the planned trajectory: \f$ \dot{q}_{max} \in \mathbb{R}^{n
|
||||
* \times 1} \f$. Unit: \f$ [rad/s] \f$ */
|
||||
std::vector<double> dq_max = {};
|
||||
|
||||
/** Maximum joint accelerations for the planned trajectory: \f$ \ddot{q}_{max} \in \mathbb{R}^{n
|
||||
* \times 1} \f$. Unit: \f$ [rad/s^2] \f$ */
|
||||
std::vector<double> ddq_max = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RtCartesianCmd
|
||||
* @brief Commands data for real-time Cartesian motion-force control.
|
||||
* @see Robot::StreamCartesianMotionForce().
|
||||
*/
|
||||
struct RtCartesianCmd
|
||||
{
|
||||
/** Default constructor */
|
||||
RtCartesianCmd() = default;
|
||||
|
||||
/** Custom constructor */
|
||||
RtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
|
||||
const std::array<double, kCartDoF>& wrench_d = {},
|
||||
const std::array<double, kCartDoF>& twist_d = {},
|
||||
const std::array<double, kCartDoF>& acc_d = {})
|
||||
: pose_d(pose_d)
|
||||
, wrench_d(wrench_d)
|
||||
, twist_d(twist_d)
|
||||
, acc_d(acc_d)
|
||||
{
|
||||
}
|
||||
|
||||
/** Target TCP pose in world frame: \f$ {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
|
||||
* quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
|
||||
std::array<double, kPoseSize> pose_d = {};
|
||||
|
||||
/** Target TCP wrench in the force control reference frame (configured by
|
||||
* SetForceControlFrame()): \f$ ^{O}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track
|
||||
* the target wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 \times 1}
|
||||
* \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T
|
||||
* \f$. Unit: \f$ [N]:[Nm] \f$ */
|
||||
std::array<double, kCartDoF> wrench_d = {};
|
||||
|
||||
/** Target TCP twist in world frame: \f$ ^{O}\dot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
|
||||
* Providing properly calculated target twist can improve the robot's overall tracking
|
||||
* performance at the cost of reduced robustness. Leaving this input 0 can maximize robustness
|
||||
* at the cost of reduced tracking performance. Consists of \f$ \mathbb{R}^{3 \times 1} \f$
|
||||
* linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular velocity. Unit: \f$ [m/s]:[rad/s] \f$ */
|
||||
std::array<double, kCartDoF> twist_d = {};
|
||||
|
||||
/** Target TCP acceleration in world frame: \f$ ^{O}\ddot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
|
||||
* Feeding forward target acceleration can improve the robot's tracking performance for highly
|
||||
* dynamic motions, but it's also okay to leave this input 0. Consists of \f$ \mathbb{R}^{3
|
||||
* \times 1} \f$ linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular acceleration. Unit: \f$
|
||||
* [m/s^2]:[rad/s^2] \f$ */
|
||||
std::array<double, kCartDoF> acc_d = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct NrtCartesianCmd
|
||||
* @brief Commands data for non-real-time Cartesian motion-force control.
|
||||
* @see Robot::SendCartesianMotionForce().
|
||||
*/
|
||||
struct NrtCartesianCmd
|
||||
{
|
||||
/** Default constructor */
|
||||
NrtCartesianCmd() = default;
|
||||
|
||||
/** Custom constructor */
|
||||
NrtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
|
||||
const std::array<double, kCartDoF>& wrench_d = {},
|
||||
const std::array<double, kCartDoF>& twist_d = {}, double max_linear_vel = 0.5,
|
||||
double max_angular_vel = 1.0, double max_linear_acc = 2.0, double max_angular_acc = 5.0)
|
||||
: pose_d(pose_d)
|
||||
, wrench_d(wrench_d)
|
||||
, twist_d(twist_d)
|
||||
, max_linear_vel(max_linear_vel)
|
||||
, max_angular_vel(max_angular_vel)
|
||||
, max_linear_acc(max_linear_acc)
|
||||
, max_angular_acc(max_angular_acc)
|
||||
{
|
||||
}
|
||||
|
||||
/** Target TCP pose in world frame: \f$ {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
|
||||
* quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
|
||||
std::array<double, kPoseSize> pose_d = {};
|
||||
|
||||
/** Target TCP wrench in the force control reference frame (configured by
|
||||
* SetForceControlFrame()): \f$ ^{O}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track
|
||||
* the target wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 \times 1}
|
||||
* \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T
|
||||
* \f$. Unit: \f$ [N]:[Nm] \f$ */
|
||||
std::array<double, kCartDoF> wrench_d = {};
|
||||
|
||||
/** Target TCP twist in world frame: \f$ ^{O}\dot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
|
||||
* Providing properly calculated target twist can improve the robot's overall tracking
|
||||
* performance at the cost of reduced robustness. Leaving this input 0 can maximize robustness
|
||||
* at the cost of reduced tracking performance. Consists of \f$ \mathbb{R}^{3 \times 1} \f$
|
||||
* linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular velocity. Unit: \f$ [m/s]:[rad/s] \f$ */
|
||||
std::array<double, kCartDoF> twist_d = {};
|
||||
|
||||
/** Maximum Cartesian linear velocity when moving to the target pose. A safe value is provided
|
||||
* as default. Unit: \f$ [m/s] \f$ */
|
||||
double max_linear_vel = 0.5;
|
||||
|
||||
/** Maximum Cartesian angular velocity when moving to the target pose. A safe value is provided
|
||||
* as default. Unit: \f$ [rad/s] \f$ */
|
||||
double max_angular_vel = 1.0;
|
||||
|
||||
/** Maximum Cartesian linear acceleration when moving to the target pose. A safe value is
|
||||
* provided as default. Unit: \f$ [m/s^2] \f$ */
|
||||
double max_linear_acc = 2.0;
|
||||
|
||||
/** Maximum Cartesian angular acceleration when moving to the target pose. A safe value is
|
||||
* provided as default. Unit: \f$ [rad/s^2] \f$ */
|
||||
double max_angular_acc = 5.0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct NrtCartesianMultiWaypointCmd
|
||||
* @brief Commands data for non-real-time Cartesian multi-waypoint motion-force control.
|
||||
* @see Robot::SendCartesianMotionForceMultiWaypoint().
|
||||
*/
|
||||
struct NrtCartesianMultiWaypointCmd
|
||||
{
|
||||
/** Default constructor */
|
||||
NrtCartesianMultiWaypointCmd() = default;
|
||||
|
||||
/** Custom constructor */
|
||||
NrtCartesianMultiWaypointCmd(const std::vector<NrtCartesianCmd>& waypoints)
|
||||
: waypoints(waypoints)
|
||||
{
|
||||
}
|
||||
|
||||
/** Sequence of Cartesian waypoints to execute. Each waypoint uses the same data layout as
|
||||
* NrtCartesianCmd. */
|
||||
std::vector<NrtCartesianCmd> waypoints = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Operator overloading to out stream all members of RobotEvent in JSON format.
|
||||
* @param[in] ostream Ostream instance.
|
||||
* @param[in] robot_event RobotEvent data structure to out stream.
|
||||
* @return Updated ostream instance.
|
||||
* @note The event timestamp is converted to local timezone when printed.
|
||||
*/
|
||||
std::ostream& operator<<(std::ostream& ostream, const RobotEvent& robot_event);
|
||||
|
||||
/**
|
||||
* @brief Operator overloading to out stream all members of RobotInfo in JSON format.
|
||||
* @param[in] ostream Ostream instance.
|
||||
* @param[in] robot_info RobotInfo data structure to out stream.
|
||||
* @return Updated ostream instance.
|
||||
*/
|
||||
std::ostream& operator<<(std::ostream& ostream, const RobotInfo& robot_info);
|
||||
|
||||
/**
|
||||
* @brief Operator overloading to out stream all members of RobotStates in JSON format.
|
||||
* @param[in] ostream Ostream instance.
|
||||
* @param[in] robot_states RobotStates data structure to out stream.
|
||||
* @return Updated ostream instance.
|
||||
*/
|
||||
std::ostream& operator<<(std::ostream& ostream, const RobotStates& robot_states);
|
||||
|
||||
/**
|
||||
* @brief Operator overloading to out stream all members of RobotActions in JSON format.
|
||||
* @param[in] ostream Ostream instance.
|
||||
* @param[in] robot_actions RobotActions data structure to out stream.
|
||||
* @return Updated ostream instance.
|
||||
*/
|
||||
std::ostream& operator<<(std::ostream& ostream, const RobotActions& robot_actions);
|
||||
|
||||
/**
|
||||
* @brief Operator overloading to out stream all members of PlanInfo in JSON format.
|
||||
* @param[in] ostream Ostream instance.
|
||||
* @param[in] plan_info PlanInfo data structure to out stream.
|
||||
* @return Updated ostream instance.
|
||||
*/
|
||||
std::ostream& operator<<(std::ostream& ostream, const PlanInfo& plan_info);
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_DATA_HPP_ */
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
/**
|
||||
* @file device.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_DEVICE_HPP_
|
||||
#define FLEXIV_RDK_DEVICE_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
#include <map>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
using DeviceParamDataTypes
|
||||
= std::variant<int, double, std::string, std::vector<double>, std::vector<std::string>>;
|
||||
|
||||
/**
|
||||
* @class Device
|
||||
* @brief Interface to control the peripheral device(s) connected to the robot.
|
||||
*/
|
||||
class Device
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the device control interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
*/
|
||||
Device(const Robot& robot);
|
||||
virtual ~Device();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] A list of all existing devices.
|
||||
* @return Device names as a string list.
|
||||
*/
|
||||
std::vector<std::string> list() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the specified device already exists.
|
||||
* @param[in] name Name of the device to check.
|
||||
* @return True if the specified device exists.
|
||||
*/
|
||||
bool exist(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Whether the specified device is enabled.
|
||||
* @param[in] name Name of the device to check.
|
||||
* @return True if the specified device is enabled.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
bool enabled(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Whether the specified device is connected.
|
||||
* @param[in] name Name of the device to check.
|
||||
* @return True if the specified device is connected.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
bool connected(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Configuration parameters of the specified device.
|
||||
* @param[in] name Name of the device to get parameters for.
|
||||
* @return A map of {param_name, param_value}. Booleans are represented by int 1 and 0. For
|
||||
* example, {{"maxVel", 0.5}, {"absolutePosition", {0.7, -0.4, 0.05}}, {"conveyorName",
|
||||
* "conveyor0"}}.
|
||||
* @throw std::invalid_argument if the specified device does not exist.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::map<std::string, DeviceParamDataTypes> params(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Enable the specified device.
|
||||
* @param[in] name Name of the device to enable.
|
||||
* @throw std::invalid_argument if the specified device does not exist.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Enable(const std::string& name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Disable the specified device.
|
||||
* @param[in] name Name of the device to disable.
|
||||
* @throw std::invalid_argument if the specified device does not exist.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Disable(const std::string& name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Send command(s) to the specified device.
|
||||
* @param[in] name Name of the device to send command(s) to.
|
||||
* @param[in] commands A map of {command_name, command_value}. For example, {{"setSpeed", 6000},
|
||||
* {"openLaser", true}}. All commands in the map will be sent to the device simultaneously. Make
|
||||
* sure the command name(s) are valid and can be accepted by the specified device.
|
||||
* @throw std::invalid_argument if the specified device does not exist.
|
||||
* @throw std::logic_error if the specified device is not enabled yet.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Command(const std::string& name,
|
||||
const std::map<std::string, std::variant<bool, int, double>>& commands);
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_DEVICE_HPP_ */
|
||||
@@ -0,0 +1,128 @@
|
||||
/**
|
||||
* @file file_io.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_FILE_IO_HPP_
|
||||
#define FLEXIV_RDK_FILE_IO_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @class FileIO
|
||||
* @brief Interface to exchange files with the robot. Only certain types of file can be transferred.
|
||||
*/
|
||||
class FileIO
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the file exchange interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
*/
|
||||
FileIO(const Robot& robot);
|
||||
virtual ~FileIO();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] A list of all trajectory files currently stored in the connected robot.
|
||||
* @return Trajectory filenames as a string list.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::vector<std::string> traj_files_list() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] A list of all user projects currently stored in the connected robot.
|
||||
* @return Project names as a string list.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::vector<std::string> projects_list() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Upload a local trajectory file (.traj) to the robot.
|
||||
* @param[in] file_dir Relative or absolute path of the directory that contains the file to
|
||||
* upload, e.g. /home/user/Documents/. Do not include the file name here.
|
||||
* @param[in] file_name Full name of the trajectory file to upload, including the extension.
|
||||
* E.g. PolishSpiral.traj. Do not include the directory path here.
|
||||
* @throw std::invalid_argument if failed to find or load the specified file.
|
||||
* @throw std::runtime_error if failed to upload the file.
|
||||
* @note This function blocks until the file is successfully uploaded.
|
||||
*/
|
||||
void UploadTrajFile(const std::string& file_dir, const std::string& file_name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Download a trajectory file (.traj) from the robot.
|
||||
* @param[in] file_name Full name of the trajectory file to download, including the extension.
|
||||
* E.g. PolishSpiral.traj.
|
||||
* @return String content of the trajectory file.
|
||||
* @throw std::invalid_argument if the specified file does not exist.
|
||||
* @throw std::runtime_error if failed to download the file.
|
||||
* @note This function blocks until the file is successfully downloaded.
|
||||
*/
|
||||
std::string DownloadTrajFile(const std::string& file_name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Download a trajectory file (.traj) from the robot and save to the specified
|
||||
* directory.
|
||||
* @param[in] file_name Full name of the trajectory file to download, including the extension.
|
||||
* E.g. "PolishSpiral.traj".
|
||||
* @param[in] save_dir Relative or absolute path of the directory to save the downloaded file.
|
||||
* E.g. /home/user/Documents/. Do not include the file name here. The specified directory will
|
||||
* be created if not yet exist.
|
||||
* @throw std::invalid_argument if the specified file does not exist.
|
||||
* @throw std::runtime_error if failed to download or save the file.
|
||||
* @throw std::filesystem::filesystem_error if failed to create the specified directory.
|
||||
* @note This function blocks until the file is successfully downloaded.
|
||||
*/
|
||||
void DownloadTrajFile(const std::string& file_name, const std::string& save_dir);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Upload a local project to the robot.
|
||||
* @param[in] project_dir Relative or absolute path of the project directory, which should
|
||||
* contain a .proj file and a .plan file.
|
||||
* @throw std::invalid_argument if failed to find the specified directory or the project files
|
||||
* are incomplete.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to upload the project.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the project is successfully uploaded.
|
||||
* @warning If the uploaded project was created by Flexiv Elements with a software package
|
||||
* version different from the connected robot, the project might not be executable before it's
|
||||
* converted to the compatible version.
|
||||
*/
|
||||
void UploadProject(const std::string& project_dir);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Download a project from the robot and save to the specified directory.
|
||||
* @param[in] project_name Full name of the project to download. E.g. "polish_table".
|
||||
* @param[in] save_dir Relative or absolute path of the directory to save the downloaded
|
||||
* project. E.g. /home/user/Documents/.
|
||||
* @throw std::invalid_argument if the specified project does not exist.
|
||||
* @throw std::runtime_error if failed to download or save the project.
|
||||
* @throw std::filesystem::filesystem_error if failed to create the specified directory.
|
||||
* @note This function blocks until the project is successfully downloaded.
|
||||
*/
|
||||
void DownloadProject(const std::string& project_name, const std::string& save_dir);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Download all collision mesh files of the connected robot and save to the
|
||||
* specified directory.
|
||||
* @param[in] save_dir Relative or absolute path of the directory to save the downloaded
|
||||
* mesh files. E.g. /home/user/Documents/.
|
||||
* @throw std::runtime_error if failed to download or save the mesh files.
|
||||
* @throw std::filesystem::filesystem_error if failed to create the specified directory.
|
||||
* @note This function blocks until all collision mesh files are successfully downloaded.
|
||||
*/
|
||||
void DownloadCollisionMesh(const std::string& save_dir);
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_FILE_IO_HPP_ */
|
||||
@@ -0,0 +1,182 @@
|
||||
/**
|
||||
* @file gripper.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_GRIPPER_HPP_
|
||||
#define FLEXIV_RDK_GRIPPER_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
#include <map>
|
||||
#include <memory>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @struct GripperParams
|
||||
* @brief Data structure containing the gripper parameters.
|
||||
* @see Gripper::params().
|
||||
*/
|
||||
struct GripperParams
|
||||
{
|
||||
/** Gripper name */
|
||||
std::string name = {};
|
||||
|
||||
/** Minimum finger opening width [m] */
|
||||
double min_width = {};
|
||||
|
||||
/** Maximum finger opening width [m] */
|
||||
double max_width = {};
|
||||
|
||||
/** Minimum finger moving velocity [m/s] */
|
||||
double min_vel = {};
|
||||
|
||||
/** Maximum finger moving velocity [m/s] */
|
||||
double max_vel = {};
|
||||
|
||||
/** Minimum grasping force [N] */
|
||||
double min_force = {};
|
||||
|
||||
/** Maximum grasping force [N] */
|
||||
double max_force = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct GripperStates
|
||||
* @brief Data structure containing the gripper states.
|
||||
* @see Gripper::states().
|
||||
*/
|
||||
struct GripperStates
|
||||
{
|
||||
/** Measured finger opening width [m] */
|
||||
double width = {};
|
||||
|
||||
/** Measured finger force. Positive: opening force, negative: closing force.
|
||||
* Reads 0 if the enabled gripper has no force sensing capability [N] */
|
||||
double force = {};
|
||||
|
||||
/** Whether the gripper fingers are moving */
|
||||
bool is_moving = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Operator overloading to out stream all members of GripperStates in JSON format.
|
||||
* @param[in] ostream Ostream instance.
|
||||
* @param[in] gripper_states GripperStates data structure to out stream.
|
||||
* @return Updated ostream instance.
|
||||
*/
|
||||
std::ostream& operator<<(std::ostream& ostream, const GripperStates& gripper_states);
|
||||
|
||||
/**
|
||||
* @class Gripper
|
||||
* @brief Interface to control the gripper installed on the robot. Because gripper is also a type of
|
||||
* robot device, this API uses the same underlying infrastructure as rdk::Device, but with functions
|
||||
* tailored specifically for gripper controls.
|
||||
*/
|
||||
class Gripper
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the gripper control interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
*/
|
||||
Gripper(const Robot& robot);
|
||||
virtual ~Gripper();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Enable the specified gripper as a device used by the specified joint group.
|
||||
* @param[in] group Joint group that uses this gripper. Only existing single-arm joint groups
|
||||
* like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] name Name of the gripper to enable.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, or if the specified gripper does not exist.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot or failed
|
||||
* to sync gripper parameters.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning There's no enforced check on whether the enabled device is a gripper or not. Using
|
||||
* this function to enable a non-gripper device will likely lead to undefined behaviors.
|
||||
*/
|
||||
void Enable(JointGroup group, const std::string& name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Disable the gripper currently used by the specified joint group.
|
||||
* @param[in] group Joint group whose gripper to be disabled.
|
||||
* @throw std::logic_error if no gripper is enabled for the specified joint group.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Disable(JointGroup group);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Manually trigger the initialization of the enabled gripper used by the
|
||||
* specified joint group. This step is not needed for grippers that automatically initialize
|
||||
* upon power-on.
|
||||
* @param[in] group Joint group whose gripper is to be initialized.
|
||||
* @throw std::logic_error if no gripper is enabled for the specified joint group.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning This function does not wait for the initialization sequence to finish, the user may
|
||||
* need to implement wait after calling this function before commanding the gripper.
|
||||
*/
|
||||
void Init(JointGroup group);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Command the gripper used by the specified joint group to grasp with direct
|
||||
* force control. This function requires the enabled gripper to support direct force control.
|
||||
* @param[in] group Joint group whose gripper to send this command to.
|
||||
* @param[in] force Target gripping force. Positive: closing force, negative: opening force.
|
||||
* Valid range: [GripperParams::min_force, GripperParams::max_force]. Unit: \f$ [N] \f$.
|
||||
* @throw std::invalid_argument if [force] is outside the valid range.
|
||||
* @throw std::logic_error if no gripper is enabled for the specified joint group.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Grasp(JointGroup group, double force);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Command the gripper used by the specified joint group to move the fingers
|
||||
* with position control.
|
||||
* @param[in] group Joint group whose gripper to send this command to.
|
||||
* @param[in] width Target opening width. Valid range: [GripperParams::min_width,
|
||||
* GripperParams::max_width]. Unit: \f$ [m] \f$.
|
||||
* @param[in] velocity Closing/opening velocity, cannot be 0. Valid range:
|
||||
* [GripperParams::min_vel, GripperParams::max_vel]. Unit: \f$ [m/s] \f$.
|
||||
* @param[in] force_limit Maximum contact force during movement. Valid range:
|
||||
* [GripperParams::min_force, GripperParams::max_force]. Unit: \f$ [N] \f$.
|
||||
* @throw std::invalid_argument if any input parameter is outside its valid range.
|
||||
* @throw std::logic_error if no gripper is enabled for the specified joint group.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Move(JointGroup group, double width, double velocity, double force_limit);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Stop and hold the gripper used by the specified joint group.
|
||||
* @param[in] group Joint group whose gripper to send this command to.
|
||||
* @throw std::logic_error if no gripper is enabled for the specified joint group.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Stop(JointGroup group);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Parameters of all enabled grippers.
|
||||
* @return Gripper parameters mapped by joint group.
|
||||
*/
|
||||
std::map<JointGroup, GripperParams> params() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current states data of all enabled grippers.
|
||||
* @return Gripper states data mapped by joint group.
|
||||
*/
|
||||
std::map<JointGroup, GripperStates> states() const;
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_GRIPPER_HPP_ */
|
||||
@@ -0,0 +1,64 @@
|
||||
/**
|
||||
* @file maintenance.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_MAINTENANCE_HPP_
|
||||
#define FLEXIV_RDK_MAINTENANCE_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @class Maintenance
|
||||
* @brief Interface to run maintenance operations on the robot.
|
||||
*/
|
||||
class Maintenance
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the robot maintenance interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
*/
|
||||
Maintenance(const Robot& robot);
|
||||
virtual ~Maintenance();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Calibrate all joint torque sensors. The robot will first move to a proper
|
||||
* calibration posture, then start the low-level calibration of all joint torque sensors.
|
||||
* Trigger this calibration if the sensed joint torques have noticeable deviations from true
|
||||
* values. See below for more details.
|
||||
* @param[in] group The joint group to run calibration for.
|
||||
* @param[in] cali_posture Joint positions to move to before starting the calibration: \f$
|
||||
* q_cali \in \mathbb{R}^{n \times 1} \f$. If left empty, the robot will use the recommended
|
||||
* upright posture for calibration. Otherwise the specified posture will be used, which is NOT
|
||||
* recommended. Valid range: [RobotInfo::q_min, RobotInfo::q_max]. Unit: \f$ [rad] \f$.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, if [cali_posture] contains any value outside the valid range, or if its size
|
||||
* does not match robot DoF.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if fault occurred during the calibration or failed to save the
|
||||
* calibration result.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the calibration is finished.
|
||||
* @warning The robot needs to be rebooted for the calibration result to take effect.
|
||||
* @par How to determine when this calibration is needed?
|
||||
* 1. When the robot is static and there's no payload or external force exerted on it, if
|
||||
* RobotStates::tcp_wrench_local still gives greater than 5N reading, then this calibration
|
||||
* should be triggered once.
|
||||
* 2. When running the "intermediate4_realtime_joint_floating.cpp" example, if the joints drift
|
||||
* swiftly toward one direction, then this calibration should be triggered once.
|
||||
*/
|
||||
void CalibrateJointTorqueSensors(
|
||||
JointGroup group, const std::vector<double>& cali_posture = {});
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_MAINTENANCE_HPP_ */
|
||||
+126
@@ -0,0 +1,126 @@
|
||||
/**
|
||||
* @file mode.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_MODE_HPP_
|
||||
#define FLEXIV_RDK_MODE_HPP_
|
||||
|
||||
#include <array>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @enum Mode
|
||||
* @brief Robot control modes. The robot needs to be switched into the correct control mode before
|
||||
* the corresponding commands can be sent. Refer to Flexiv RDK Manual for more details.
|
||||
* @see Robot::SwitchMode().
|
||||
*/
|
||||
enum class Mode
|
||||
{
|
||||
/** Mode not set */
|
||||
UNKNOWN = 0,
|
||||
|
||||
/**
|
||||
* No operation to execute, the robot holds position and waits for new command.
|
||||
*/
|
||||
IDLE,
|
||||
|
||||
/**
|
||||
* Run real-time joint torque control to track continuous commands @1kHz.
|
||||
* @note Real-time (RT) mode.
|
||||
* @see Robot::StreamJointTorque().
|
||||
*/
|
||||
RT_JOINT_TORQUE,
|
||||
|
||||
/**
|
||||
* Run real-time joint impedance control to track continuous commands @ 1kHz.
|
||||
* @note Real-time (RT) mode.
|
||||
* @see Robot::StreamJointPosition().
|
||||
*/
|
||||
RT_JOINT_IMPEDANCE,
|
||||
|
||||
/**
|
||||
* Run non-real-time joint impedance control to track discrete commands (smoothened by internal
|
||||
* motion generator).
|
||||
* @note Non-real-time (NRT) mode.
|
||||
* @see Robot::SendJointPosition().
|
||||
*/
|
||||
NRT_JOINT_IMPEDANCE,
|
||||
|
||||
/**
|
||||
* Run real-time joint position control to track continuous commands @ 1kHz.
|
||||
* @note Real-time (RT) mode.
|
||||
* @see Robot::StreamJointPosition().
|
||||
*/
|
||||
RT_JOINT_POSITION,
|
||||
|
||||
/**
|
||||
* Run non-real-time joint position control to track discrete commands (smoothened by internal
|
||||
* motion generator).
|
||||
* @note Non-real-time (NRT) mode.
|
||||
* @see Robot::SendJointPosition().
|
||||
*/
|
||||
NRT_JOINT_POSITION,
|
||||
|
||||
/**
|
||||
* Execute pre-configured robot task plans.
|
||||
* @note Non-real-time (NRT) mode.
|
||||
* @see Robot::ExecutePlan().
|
||||
*/
|
||||
NRT_PLAN_EXECUTION,
|
||||
|
||||
/**
|
||||
* Execute robot primitives (unit skills).
|
||||
* @note Non-real-time (NRT) mode.
|
||||
* @see Robot::ExecutePrimitive().
|
||||
* @see [Flexiv Primitives](https://www.flexiv.com/primitives/) documentation.
|
||||
*/
|
||||
NRT_PRIMITIVE_EXECUTION,
|
||||
|
||||
/**
|
||||
* Run real-time Cartesian motion-force control to track continuous commands @ 1kHz.
|
||||
* @note Real-time (RT) mode.
|
||||
* @see Robot::StreamCartesianMotionForce().
|
||||
*/
|
||||
RT_CARTESIAN_MOTION_FORCE,
|
||||
|
||||
/**
|
||||
* Run non-real-time Cartesian motion-force control to track discrete commands, smoothened by
|
||||
* internal motion generator.
|
||||
* @note Non-real-time (NRT) mode.
|
||||
* @see Robot::SendCartesianMotionForce().
|
||||
*/
|
||||
NRT_CARTESIAN_MOTION_FORCE,
|
||||
|
||||
/**
|
||||
* Run non-real-time super primitives, i.e. composite tasks that consist of various components
|
||||
* like motion, force, vision, planning, etc.
|
||||
* @note Non-real-time (NRT) mode.
|
||||
* @see Robot::SendCartesianMotionForce().
|
||||
* @see Robot::SendCartesianMotionForceMultiWaypoint().
|
||||
*/
|
||||
NRT_SUPER_PRIMITIVE,
|
||||
};
|
||||
|
||||
/** Map Mode enums to strings */
|
||||
inline const std::map<Mode, std::string> kModeNames {
|
||||
{Mode::UNKNOWN, "UNKNOWN"},
|
||||
{Mode::IDLE, "IDLE"},
|
||||
{Mode::RT_JOINT_TORQUE, "RT_JOINT_TORQUE"},
|
||||
{Mode::RT_JOINT_IMPEDANCE, "RT_JOINT_IMPEDANCE"},
|
||||
{Mode::NRT_JOINT_IMPEDANCE, "NRT_JOINT_IMPEDANCE"},
|
||||
{Mode::RT_JOINT_POSITION, "RT_JOINT_POSITION"},
|
||||
{Mode::NRT_JOINT_POSITION, "NRT_JOINT_POSITION"},
|
||||
{Mode::NRT_PLAN_EXECUTION, "NRT_PLAN_EXECUTION"},
|
||||
{Mode::NRT_PRIMITIVE_EXECUTION, "NRT_PRIMITIVE_EXECUTION"},
|
||||
{Mode::RT_CARTESIAN_MOTION_FORCE, "RT_CARTESIAN_MOTION_FORCE"},
|
||||
{Mode::NRT_CARTESIAN_MOTION_FORCE, "NRT_CARTESIAN_MOTION_FORCE"},
|
||||
{Mode::NRT_SUPER_PRIMITIVE, "NRT_SUPER_PRIMITIVE"},
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_MODE_HPP_ */
|
||||
+254
@@ -0,0 +1,254 @@
|
||||
/**
|
||||
* @file model.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_MODEL_HPP_
|
||||
#define FLEXIV_RDK_MODEL_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
#include <Eigen/Eigen>
|
||||
#include <memory>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @struct IKParams
|
||||
* @brief Per-joint-group IK input parameters for SolveConstrainedIK().
|
||||
*/
|
||||
struct IKParams
|
||||
{
|
||||
/** Cartesian pose to be solved. */
|
||||
std::array<double, kPoseSize> cartesian_pose = {};
|
||||
|
||||
/** Seed joint positions for IK solver. Unit: [rad]. */
|
||||
std::vector<double> seed_q = {};
|
||||
|
||||
/** Whether to only constrain Cartesian position and allow orientation to vary. */
|
||||
bool free_orientation = false;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct IKResult
|
||||
* @brief Result from SolveConstrainedIK().
|
||||
*/
|
||||
struct IKResult
|
||||
{
|
||||
/** Whether the IK solution is successful. */
|
||||
bool success = false;
|
||||
|
||||
/** Solved joint positions by active joint group. Unit: \f$ [rad] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> solved_q = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @class Model
|
||||
* @brief Interface to obtain certain model data of the robot, including kinematics and dynamics.
|
||||
*/
|
||||
class Model
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the robot model interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @param[in] gravity_vector Earth's gravity vector in world frame. Default to \f$ [0.0, 0.0,
|
||||
* -9.81]^T \f$. Unit: \f$ [m/s^2] \f$.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
* @throw std::logic_error if the connected robot does not have an RDK professional license; or
|
||||
* the parsed robot model is not supported.
|
||||
*/
|
||||
Model(const Robot& robot,
|
||||
const Eigen::Vector3d& gravity_vector = Eigen::Vector3d(0.0, 0.0, -9.81));
|
||||
virtual ~Model();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Names of all links in the robot model.
|
||||
* @return Name vector in the same order as the robot's kinematic chain.
|
||||
*/
|
||||
std::vector<std::string> link_names() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Names of all actuated joints in the robot model.
|
||||
* @return Name vector in the same order as the robot's kinematic chain.
|
||||
*/
|
||||
std::vector<std::string> joint_names() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Reload (refresh) parameters of the robot model stored locally in this class
|
||||
* using the latest data synced from the connected robot. Tool model is also synced.
|
||||
* @throw std::runtime_error if failed to sync model data.
|
||||
* @throw std::logic_error if the synced robot model contains invalid data.
|
||||
* @note This function blocks until the model parameters are synced and reloaded.
|
||||
* @note This function does not affect the kinematics functions.
|
||||
* @warning Parameters of the locally-stored robot model must be manually refreshed using this
|
||||
* function whenever a physical change is made to the connected robot (e.g. a tool is added or
|
||||
* changed). Otherwise the locally computed functions will return incorrect results.
|
||||
*/
|
||||
void Reload();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Update the configuration (posture) of the locally-stored robot model so
|
||||
* that the locally computed functions return results based on the updated configuration.
|
||||
* @param[in] full_q Current joint positions of the whole robot and matching the order of
|
||||
* joint_names(): \f$ q \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$.
|
||||
* @param[in] full_dq Current joint velocities of the whole robot and matching the order of
|
||||
* joint_names(): \f$ \dot{q} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$.
|
||||
* @throw std::invalid_argument if size of any input vector does not match robot full DoF.
|
||||
*/
|
||||
void Update(const std::vector<double>& full_q, const std::vector<double>& full_dq);
|
||||
|
||||
//========================================== DYNAMICS ==========================================
|
||||
/**
|
||||
* @brief [Non-blocking] Compute the time derivative of Jacobian matrix at the specified frame
|
||||
* w.r.t. world frame.
|
||||
* @param[in] link_name Name of the link whose frame is the specified one.
|
||||
* @return Time derivative of the Jacobian matrix: \f$ ^{O}\dot{J_i} \in \mathbb{R}^{m \times n}
|
||||
* \f$.
|
||||
* @throw std::invalid_argument if [link_name] does not exist.
|
||||
* @note Call Update() before this function.
|
||||
* @see link_names().
|
||||
*/
|
||||
Eigen::MatrixXd dJ(const std::string& link_name);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Compute the mass matrix in generalized coordinates, i.e. joint space.
|
||||
* @return Symmetric positive definite mass matrix: \f$ M(q) \in \mathbb{S}^{n \times n}_{++}
|
||||
* \f$. Unit: \f$ [kgm^2] \f$.
|
||||
* @note Call Update() before this function.
|
||||
*/
|
||||
Eigen::MatrixXd M();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Compute the Coriolis/centripetal matrix in generalized coordinates,
|
||||
* i.e. joint space.
|
||||
* @return Coriolis/centripetal matrix: \f$ C(q,\dot{q}) \in \mathbb{R}^{n \times n} \f$.
|
||||
* @note Call Update() before this function.
|
||||
*/
|
||||
Eigen::MatrixXd C();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Compute the gravity force vector in generalized coordinates, i.e.
|
||||
* joint space.
|
||||
* @return Gravity force vector: \f$ g(q) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
|
||||
* @note Call Update() before this function.
|
||||
*/
|
||||
Eigen::VectorXd g();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Compute the Coriolis force vector in generalized coordinates, i.e.
|
||||
* joint space.
|
||||
* @return Coriolis force vector: \f$ c(q,\dot{q}) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$
|
||||
* [Nm] \f$.
|
||||
* @note Call Update() before this function.
|
||||
*/
|
||||
Eigen::VectorXd c();
|
||||
|
||||
//========================================= KINEMATICS =========================================
|
||||
/**
|
||||
* @brief [Non-blocking] Compute the Jacobian matrix at the specified frame w.r.t. world frame.
|
||||
* @param[in] link_name Name of the link whose frame is the specified one.
|
||||
* @return Jacobian matrix: \f$ ^{O}J_i \in \mathbb{R}^{m \times n} \f$.
|
||||
* @throw std::invalid_argument if [link_name] does not exist.
|
||||
* @note Call Update() before this function.
|
||||
* @see link_names().
|
||||
*/
|
||||
Eigen::MatrixXd J(const std::string& link_name);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Compute the transformation matrix of the specified frame w.r.t. world
|
||||
* frame.
|
||||
* @param[in] link_name Name of the link whose frame is the specified one.
|
||||
* @return Transformation matrix: \f$ ^{O}T_i \in \mathbb{R}^{4 \times 4} \f$.
|
||||
* @throw std::invalid_argument if [link_name] does not exist.
|
||||
* @note Call Update() before this function.
|
||||
* @see link_names().
|
||||
*/
|
||||
Eigen::Isometry3d T(const std::string& link_name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Sync the actual kinematic parameters of the connected robot into the
|
||||
* template URDF file.
|
||||
* @param[in] template_urdf_path Path to the template URDF file that can be generated in
|
||||
* [flexiv_description](https://github.com/flexivrobotics/flexiv_description). This template
|
||||
* URDF file will be updated when the sync is finished.
|
||||
* @throw std::invalid_argument if failed to load the template URDF file.
|
||||
* @throw std::runtime_error if failed to sync the URDF file.
|
||||
* @note This function blocks until the sync is finished.
|
||||
* @par Why is this function needed?
|
||||
* The template URDF contains kinematic parameters of the latest version of robot hardware,
|
||||
* which might be different from older versions. This function is therefore provided to sync the
|
||||
* actual kinematic parameters of the connected robot into the template URDF.
|
||||
*/
|
||||
void SyncURDF(const std::string& template_urdf_path);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Sync the actual kinematic parameters of the connected robot into the
|
||||
* template YAML file.
|
||||
* @param[in] template_yaml_path Path to the template YAML file located at
|
||||
* [flexiv_description/config/.../default_kinematics.yaml]. This template YAML file will be
|
||||
* updated when the sync is finished.
|
||||
* @return Total number of joints that have been successfully synced.
|
||||
* @throw std::invalid_argument if failed to load the template YAML file.
|
||||
* @throw std::runtime_error if failed to sync the YAML file.
|
||||
* @note This function blocks until the sync is finished.
|
||||
*/
|
||||
size_t SyncKinematicsYAML(const std::string& template_yaml_path);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Solve constrained IK using one or more active joint groups.
|
||||
* @param[in] ik_params_by_group IK input parameters mapped by active joint group. Joint groups
|
||||
* not included in this map are treated as inactive. Only single-arm joint groups like ARM_1 and
|
||||
* ARM_2 are accepted.
|
||||
* @return Solver result. [solved_q] contains one entry per requested active joint group.
|
||||
* @throw std::invalid_argument if input map is empty, if any joint group is not an existing
|
||||
* single-arm joint group, or if any [seed_q] size does not match DoF of its joint group.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
IKResult SolveConstrainedIK(const std::map<JointGroup, IKParams>& ik_params_by_group);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Score of each joint group's current configuration (posture), calculated
|
||||
* from the manipulability measurements.
|
||||
* @return Configuration score mapped by joint group as {translation_score,
|
||||
* orientation_score}. The quality of configuration based on the score can be interpreted as:
|
||||
* poor = [0, 20), medium = [20, 40), good = [40, 100].
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
* @warning A poor configuration score means the robot is near or at singularity, which can lead
|
||||
* to degraded Cartesian performance. Use configuration with high scores for better
|
||||
* manipulability and task results.
|
||||
*/
|
||||
std::map<JointGroup, std::pair<double, double>> configuration_score() const;
|
||||
|
||||
//======================================= MULTI-CONTACT ========================================
|
||||
/**
|
||||
* @brief [Non-blocking] Estimated multi-contact forces applied on each link of applicable joint
|
||||
* groups, calculated using the force-torque sensors installed in every joint of the robot.
|
||||
* @return \f$ f_c \in \mathbb{R}^{n \times 1} \f$ mapped by joint group. Each vector element is
|
||||
* a \f$ \mathbb{R}^{3 \times 1} \f$ force vector w.r.t. the corresponding link frame. Only
|
||||
* contains joint groups that are capable of multi-contact estimation.
|
||||
* @warning This data is only available on certain robot models. An empty vector will be
|
||||
* returned if the connected robot does not support multi-contact estimation.
|
||||
*/
|
||||
std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_forces() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Estimated multi-contact positions on each link of applicable joint
|
||||
* groups, calculated using the force-torque sensors installed in every joint of the robot.
|
||||
* @return \f$ p_c \in \mathbb{R}^{n \times 1} \f$ mapped by joint group. Each vector element is
|
||||
* a \f$ \mathbb{R}^{3 \times 1} \f$ position vector w.r.t. the corresponding link frame. Only
|
||||
* contains joint groups that are capable of multi-contact estimation.
|
||||
* @warning This data is only available on certain robot models. An empty vector will be
|
||||
* returned if the connected robot does not support multi-contact estimation.
|
||||
*/
|
||||
std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_positions() const;
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_MODEL_HPP_ */
|
||||
+896
@@ -0,0 +1,896 @@
|
||||
/**
|
||||
* @file robot.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_ROBOT_HPP_
|
||||
#define FLEXIV_RDK_ROBOT_HPP_
|
||||
|
||||
#include "data.hpp"
|
||||
#include "mode.hpp"
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <exception>
|
||||
#include <map>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @class Robot
|
||||
* @brief Main interface to control the robot, containing several function categories and background
|
||||
* services.
|
||||
*/
|
||||
class Robot
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Blocking] Instantiate the robot control interface. RDK services will be started and
|
||||
* establish connection with the target robot.
|
||||
* @param[in] robot_sn Serial number of the robot to connect. The accepted formats are:
|
||||
* "Enlight-L-123456" and "Enlight L-123456". See ProductModel enum for all supported models.
|
||||
* @param[in] verbose Enable/disable info and warning prints.
|
||||
* @param[in] lite Whether to create a lite instance. See details below. A normal instance must
|
||||
* have already connected to the robot for lite instances to function properly.
|
||||
* @throw std::invalid_argument if the format of [robot_sn] is invalid.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
* @throw std::logic_error if the connected robot does not have a valid RDK license, or is
|
||||
* incompatible with this RDK library version, or is an unsupported product model.
|
||||
* @warning This constructor blocks until the initialization sequence is successfully finished
|
||||
* and connection with the robot is established.
|
||||
* @par Lite Instance
|
||||
* Creating multiple instances of rdk::Robot that connect to the same robot is allowed. However,
|
||||
* every additional instance brings extra overhead that can restrict the total number of
|
||||
* instances due to limited computation power and network bandwidth. To mitigate the performance
|
||||
* impact, it is recommended to create a lite instance of rdk::Robot if it only needs to send
|
||||
* one-shot requests, which are functions marked with the [Blocking] tag. A lite instance does
|
||||
* not receive robot states or system status, nor send motion commands.
|
||||
*/
|
||||
Robot(const std::string& robot_sn, bool verbose = true, bool lite = false);
|
||||
virtual ~Robot();
|
||||
|
||||
//========================================= ACCESSORS ==========================================
|
||||
/**
|
||||
* @brief [Non-blocking] Whether this rdk::Robot instance is a lite one.
|
||||
* @return True: is a lite instance; false: not a lite instance.
|
||||
*/
|
||||
bool lite() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the connection with the robot is established.
|
||||
* @return True: connected; false: disconnected.
|
||||
*/
|
||||
bool connected() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] General information about the robot.
|
||||
* @return Value copy of RobotInfo struct.
|
||||
*/
|
||||
RobotInfo info() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current control mode of the robot.
|
||||
* @return Mode enum.
|
||||
*/
|
||||
Mode mode() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current states data of all existing joint groups of the robot.
|
||||
* @return Robot states data mapped by joint group.
|
||||
* @warning Cartesian states of non-single-arm joint groups are not populated.
|
||||
*/
|
||||
std::map<JointGroup, RobotStates> states() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current actions data of all existing joint groups of the robot.
|
||||
* @return Robot actions data mapped by joint group.
|
||||
* @warning Cartesian actions of non-single-arm joint groups are not populated.
|
||||
*/
|
||||
std::map<JointGroup, RobotActions> actions() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether specific joint groups of the robot have come to a complete
|
||||
* stop.
|
||||
* @return Stopped flag mapped by joint group. True: this joint group has stopped; false: this
|
||||
* joint group is still moving.
|
||||
*/
|
||||
std::map<JointGroup, bool> stopped() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether all joint groups of the robot have come to a complete stop.
|
||||
* @return True: all stopped; false: not all stopped.
|
||||
*/
|
||||
bool all_stopped() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the robot is ready to be operated, which requires the
|
||||
* following conditions to be met: enabled, brakes fully released, in auto mode, no fault, and
|
||||
* not in reduced state.
|
||||
* @return True: operational (operational_status() == READY); false: not operational.
|
||||
* @warning The robot won't execute any user command until it's ready to be operated.
|
||||
*/
|
||||
bool operational() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current operational status of the robot.
|
||||
* @return OperationalStatus enum.
|
||||
*/
|
||||
OperationalStatus operational_status() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the robot is busy. This includes any user commanded operations
|
||||
* that requires the robot to execute. For example, plans, primitives, Cartesian and joint
|
||||
* motions, etc.
|
||||
* @return True: busy; false: idle.
|
||||
* @warning Some exceptions exist for primitives, see ExecutePrimitive() for more details.
|
||||
*/
|
||||
bool busy() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the robot is in fault state.
|
||||
* @return True: robot has fault; false: robot normal.
|
||||
*/
|
||||
bool fault() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the robot is in reduced state.
|
||||
* @return True: in reduced state; false: not in reduced state.
|
||||
* @par Reduced state
|
||||
* The robot will enter reduced state if a) the safety input for reduced state goes low or b)
|
||||
* robot TCP passes through any safety plane. The safety limits are lowered in reduced state
|
||||
* compared to normal state. Specific values for the safety limits can be configured in Flexiv
|
||||
* Elements under Settings -> Safety Configuration. Please refer to the robot user manual for
|
||||
* more details about system reduced state.
|
||||
*/
|
||||
bool reduced() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the robot is in recovery state.
|
||||
* @return True: in recovery state; false: not in recovery state.
|
||||
* @note Please use Flexiv Elements to get the robot out of recovery state.
|
||||
* @par Recovery state
|
||||
* The robot will enter recovery state if it needs to recover from joint position limit
|
||||
* violation (a critical system fault that requires a recovery operation, during which the
|
||||
* joints that moved outside the allowed position range will need to move very slowly back into
|
||||
* the allowed range). Please refer to the robot user manual for more details about system
|
||||
* recovery state.
|
||||
*/
|
||||
bool recovery() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether all connected emergency stops (E-stops) are released.
|
||||
* @return True: released; false: pressed.
|
||||
*/
|
||||
bool estop_released() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the 3-position enabling device (e.g. enabling button on the
|
||||
* motion bar) is in the ON position (the middle position).
|
||||
* @return True: ON; false: OFF.
|
||||
*/
|
||||
bool enabling_device_on() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Whether the timeliness failure limit has been reached within a
|
||||
* 1-second moving window.
|
||||
* @return True: reached limit; false: not reached limit.
|
||||
* @see SetTimelinessFailureLimit().
|
||||
*/
|
||||
bool reached_timeliness_failure_limit() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Robot events stored since the last successful instantiation of this
|
||||
* class.
|
||||
* @return Event log as a vector of RobotEvent, with the first element being the
|
||||
* oldest event and the last element being the latest event.
|
||||
* @warning Events before the last successful instantiation of this class are not stored.
|
||||
*/
|
||||
std::vector<RobotEvent> event_log() const;
|
||||
|
||||
//======================================= SYSTEM CONTROL =======================================
|
||||
/**
|
||||
* @brief [Blocking] Servo on the robot. If E-stop is released and there's no fault, the robot
|
||||
* will release brakes, and becomes operational a few seconds later.
|
||||
* @throw std::logic_error if the robot is not connected or E-stop is not released.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void ServoOn();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Move the specified joint groups to the home posture using Home primitive.
|
||||
* @param[in] groups Joint group(s) to home. Only existing single-arm joint groups like ARM_1
|
||||
* and ARM_2 are accepted. If left empty, home all applicable joint groups.
|
||||
* @throw std::invalid_argument if [groups] contains joint groups that are not existing
|
||||
* single-arm joint groups in the connected robot.
|
||||
* @throw std::runtime_error if failed to execute the Home primitive.
|
||||
* @note The robot's control mode will be temporarily switched to NRT_PRIMITIVE_EXECUTION and
|
||||
* restored to the previous mode before returning.
|
||||
* @note This function blocks until all specified joint groups have reached the home posture.
|
||||
*/
|
||||
void Home(const std::vector<JointGroup>& groups = {});
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Force robot brakes to engage or release during normal operation.
|
||||
* Restrictions apply, see warning.
|
||||
* @param[in] engage True: engage brakes; false: release brakes.
|
||||
* @throw std::logic_error if the connected robot is not a medical model or is moving.
|
||||
* @throw std::runtime_error if failed to engage/release the brakes.
|
||||
* @note This function blocks until the brakes are successfully engaged/released.
|
||||
* @warning This function is accessible only if a) the connected robot is a medical model AND
|
||||
* b) the robot is not moving.
|
||||
*/
|
||||
void Brake(bool engage);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Switch the robot to a new control mode and wait for the mode transition
|
||||
* to finish.
|
||||
* @param[in] mode Control mode to switch to.
|
||||
* @throw std::invalid_argument if the requested mode is invalid or unlicensed.
|
||||
* @throw std::logic_error if robot is in an unknown control mode or is not operational.
|
||||
* @throw std::runtime_error if failed to transit the robot into the specified control mode
|
||||
* after several attempts.
|
||||
* @note This function blocks until the robot has successfully transited into the specified
|
||||
* control mode.
|
||||
* @warning If the robot is still moving when this function is called, it will automatically
|
||||
* stop before making the mode transition.
|
||||
*/
|
||||
void SwitchMode(Mode mode);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Stop the robot and transit its control mode to IDLE.
|
||||
* @throw std::runtime_error if failed to stop the robot.
|
||||
* @note This function blocks until the robot comes to a complete stop.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Try to clear minor or critical fault for the robot without a power cycle.
|
||||
* @param[in] timeout_sec Maximum time in seconds to wait for the fault to be successfully
|
||||
* cleared. Normally, a minor fault should take no more than 3 seconds to clear, and a critical
|
||||
* fault should take no more than 30 seconds to clear.
|
||||
* @return True: successfully cleared fault; false: failed to clear fault.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the fault is successfully cleared or [timeout_sec] has
|
||||
* elapsed.
|
||||
* @warning Clearing a critical fault through this function without a power cycle requires a
|
||||
* dedicated device, which may not be installed in older robot models.
|
||||
*/
|
||||
bool ClearFault(unsigned int timeout_sec = 30);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set values to global variables that already exist in the robot.
|
||||
* @param[in] global_vars A map of {global_var_name, global_var_value(s)}. Use int 1 and 0 to
|
||||
* represent booleans. For example, {{"camera_offset", {0.1, -0.2, 0.3}}, {"start_plan", 1}}.
|
||||
* @throw std::length_error if [global_vars] is empty.
|
||||
* @throw std::invalid_argument if any of the specified global variables does not exist.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the global variables are successfully set.
|
||||
* @warning The specified global variables need to be created first using Flexiv Elements.
|
||||
* @see global_variables().
|
||||
*/
|
||||
void SetGlobalVariables(const std::map<std::string, FlexivDataTypes>& global_vars);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Existing global variables and their current values.
|
||||
* @return A map of {global_var_name, global_var_value(s)}. Booleans are represented by int 1
|
||||
* and 0. For example, {{"camera_offset", {0.1, -0.2, 0.3}}, {"start_plan", 1}}.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
* @see SetGlobalVariables().
|
||||
*/
|
||||
std::map<std::string, FlexivDataTypes> global_variables() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Set the maximum tolerable percentage of real-time commands that arrived
|
||||
* the robot too late within a 1-second moving window. This only applies to the real-time
|
||||
* control modes and functions marked as "Real-time (RT)" will throw an exception when this
|
||||
* limit is reached.
|
||||
* @param[in] percentage Percentage limit, default to 2%. Valid range: [0, 100].
|
||||
* @throw std::invalid_argument if [percentage] is outside the valid range.
|
||||
* @see reached_timeliness_failure_limit().
|
||||
*/
|
||||
void SetTimelinessFailureLimit(double percentage = 2.0);
|
||||
|
||||
//======================================= PLAN EXECUTION =======================================
|
||||
/**
|
||||
* @brief [Blocking] Execute a plan by specifying its index.
|
||||
* @param[in] index Index of the plan to execute, can be obtained via plan_list().
|
||||
* @param[in] continue_exec Whether to continue executing the plan when the RDK program is
|
||||
* closed or the connection is lost.
|
||||
* @param[in] block_until_started Whether to wait for the commanded plan to finish loading
|
||||
* and start execution before the function returns. Depending on the amount of computation
|
||||
* needed to get the plan ready, the loading process typically takes no more than 200 ms.
|
||||
* @throw std::invalid_argument if [index] is outside the valid range.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot or the
|
||||
* robot is not operational.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered if
|
||||
* [block_until_started] is disabled, or until the plan has started execution if
|
||||
* [block_until_started] is enabled.
|
||||
* @note busy() can be used to check if the plan has finished.
|
||||
*/
|
||||
void ExecutePlan(
|
||||
unsigned int index, bool continue_exec = false, bool block_until_started = true);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Execute a plan by specifying its name.
|
||||
* @param[in] name Name of the plan to execute, can be obtained via plan_list().
|
||||
* @param[in] continue_exec Whether to continue executing the plan when the RDK program is
|
||||
* closed or the connection is lost.
|
||||
* @param[in] block_until_started Whether to wait for the commanded plan to finish loading
|
||||
* and start execution before the function returns. Depending on the amount of computation
|
||||
* needed to get the plan ready, the loading process typically takes no more than 200 ms.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot or the
|
||||
* robot is not operational.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered if
|
||||
* [block_until_started] is disabled, or until the plan has started execution if
|
||||
* [block_until_started] is enabled.
|
||||
* @note busy() can be used to check if the plan has finished.
|
||||
*/
|
||||
void ExecutePlan(
|
||||
const std::string& name, bool continue_exec = false, bool block_until_started = true);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Pause or resume the execution of the current plan.
|
||||
* @param[in] toggle True: pause plan; false: resume plan.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning Internal plans (not created by user) cannot be resumed due to safety concerns.
|
||||
*/
|
||||
void PausePlan(bool toggle);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Stop the execution of the current plan.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void StopPlan();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] A list of all available plans.
|
||||
* @return Available plans as a string list.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::vector<std::string> plan_list() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Detailed information about the executing plan. Contains plan name,
|
||||
* primitive name, node name, node path, node path time period, etc.
|
||||
* @return PlanInfo value copy.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
PlanInfo plan_info() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Enable or disable the breakpoint mode during plan execution. When enabled,
|
||||
* the executing plan will pause at the pre-defined breakpoints. Use StepBreakpoint() to
|
||||
* continue the execution and pause at the next breakpoint.
|
||||
* @param[in] is_enabled True: enable; false: disable. By default, breakpoint mode is disabled.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void SetBreakpointMode(bool is_enabled);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] If breakpoint mode is enabled, step to the next breakpoint. The plan
|
||||
* execution will continue and pause at the next breakpoint.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @note Use PlanInfo::waiting_for_step to check if the plan is waiting for user signal to step
|
||||
* the breakpoint.
|
||||
*/
|
||||
void StepBreakpoint();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set overall velocity scale of robot motions during plan and primitive
|
||||
* execution.
|
||||
* @param[in] velocity_scale Percentage scale to adjust the overall velocity of robot motions.
|
||||
* Valid range: [0, 100]. Setting to 100 means to move with 100% of specified motion velocity,
|
||||
* and 0 means not moving at all.
|
||||
* @throw std::invalid_argument if [velocity_scale] is outside the valid range.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: NRT_PLAN_EXECUTION, NRT_PRIMITIVE_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void SetVelocityScale(unsigned int velocity_scale);
|
||||
|
||||
//==================================== PRIMITIVE EXECUTION =====================================
|
||||
/**
|
||||
* @brief [Blocking] Execute primitive(s) on specified joint group(s).
|
||||
* @param[in] primitive_args Primitive arguments mapped by joint group. Only existing
|
||||
* single-arm joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] block_until_started Whether to wait for the commanded primitive to finish loading
|
||||
* and start execution before the function returns. Depending on the amount of computation
|
||||
* needed to get the primitive ready, the loading process typically takes no more than 200 ms.
|
||||
* @throw std::invalid_argument if [primitive_args] contains joint groups that are not
|
||||
* existing single-arm joint groups in the connected robot.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot or the
|
||||
* robot is not operational.
|
||||
* @note Applicable control modes: NRT_PRIMITIVE_EXECUTION.
|
||||
* @note This function blocks until the request is successfully delivered if
|
||||
* [block_until_started] is disabled, or until the primitive has started execution if
|
||||
* [block_until_started] is enabled. After this function returns, the primitive will still be
|
||||
* running, thus the users need to implement their own blocking after calling this function. See
|
||||
* examples for reference.
|
||||
* @warning The primitive input parameters may not use SI units, please refer to the Flexiv
|
||||
* Primitives documentation for exact unit definition.
|
||||
* @warning Most primitives won't exit by themselves and require users to explicitly trigger
|
||||
* transitions based on specific primitive states. In such case, busy() will stay true even if
|
||||
* it seems everything is done for that primitive.
|
||||
*/
|
||||
void ExecutePrimitive(
|
||||
const std::map<JointGroup, PrimitiveArgs>& primitive_args, bool block_until_started = true);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] States data of the primitive(s) that are currently running.
|
||||
* @return Primitive states data mapped by joint group. Only joint groups that are currently
|
||||
* running primitives will be included.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::map<JointGroup, PrimitiveStates> primitive_states() const;
|
||||
|
||||
//==================================== DIRECT JOINT CONTROL ====================================
|
||||
/**
|
||||
* @brief [Non-blocking] Continuously stream joint torque commands of the specified joint
|
||||
* group(s) to the robot. The commands are tracked by a high-performance joint torque
|
||||
* controller.
|
||||
* @param[in] cmds Real-time joint torque commands mapped by joint group. Only existing
|
||||
* single-arm joint groups and external axis joint groups are accepted.
|
||||
* @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
|
||||
* single-arm or external axis joint group in the connected robot, if size of any input vector
|
||||
* does not match robot DoF, or [friction_comp_scale] is outside the valid range.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if the robot is not operational.
|
||||
* @note Applicable control modes: RT_JOINT_TORQUE.
|
||||
* @note Real-time (RT).
|
||||
* @warning Always stream smooth and continuous commands to avoid sudden movements.
|
||||
*/
|
||||
void StreamJointTorque(const std::map<JointGroup, RtJointTorqueCmd>& cmds);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Continuously stream joint position, velocity, and acceleration commands
|
||||
* of the specified joint group(s) to the robot. The commands are tracked by either the joint
|
||||
* impedance controller or the joint position controller, depending on the control mode.
|
||||
* @param[in] cmds Real-time joint position-velocity-acceleration commands mapped by joint
|
||||
* group. Only existing single-arm joint groups and external axis joint groups are accepted.
|
||||
* @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
|
||||
* single-arm or external axis joint group in the connected robot, or if size of any input
|
||||
* vector does not match robot DoF.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if the robot is not operational.
|
||||
* @note Applicable control modes: RT_JOINT_IMPEDANCE, RT_JOINT_POSITION.
|
||||
* @note Real-time (RT).
|
||||
* @warning Always stream smooth and continuous commands to avoid sudden movements.
|
||||
* @see SetJointImpedance().
|
||||
*/
|
||||
void StreamJointPosition(const std::map<JointGroup, RtJointPositionCmd>& cmds);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Discretely send joint position and velocity commands of the specified
|
||||
* joint group(s) to the robot. An internal motion generator will smoothen the discrete
|
||||
* commands, which are then tracked by either the joint impedance controller or the joint
|
||||
* position controller, depending on the control mode.
|
||||
* @param[in] cmds Non-real-time joint position-velocity-acceleration commands mapped by joint
|
||||
* group. Only existing single-arm joint groups and external axis joint groups are accepted.
|
||||
* @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
|
||||
* single-arm or external axis joint group in the connected robot, if size of any input vector
|
||||
* does not match robot DoF, or [dq_max] or [ddq_max] contains any non-positive value.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if the robot is not operational.
|
||||
* @note Applicable control modes: NRT_JOINT_IMPEDANCE, NRT_JOINT_POSITION.
|
||||
* @warning Calling this function a second time while the motion from the previous call is still
|
||||
* ongoing will trigger an online re-planning of the joint trajectory, such that the previous
|
||||
* command is aborted and the new command starts to execute.
|
||||
* @see SetJointImpedance().
|
||||
*/
|
||||
void SendJointPosition(const std::map<JointGroup, NrtJointPositionCmd>& cmds);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set impedance properties of the robot's joint motion controller used in
|
||||
* the joint impedance control modes.
|
||||
* @param[in] group The joint group to set impedance properties for. Only existing single-arm
|
||||
* joint groups and external axis joint groups are accepted.
|
||||
* @param[in] K_q Joint motion stiffness: \f$ K_q \in \mathbb{R}^{n \times 1} \f$.
|
||||
* Setting motion stiffness of a joint axis to 0 will make this axis free-floating. Valid range:
|
||||
* [0, RobotInfo::K_q_nom]. Unit: \f$ [Nm/rad] \f$.
|
||||
* @param[in] Z_q Joint motion damping ratio: \f$ Z_q \in \mathbb{R}^{n \times 1} \f$.
|
||||
* Valid range: [0.3, 0.8]. The default value 0.7 will be used if left empty.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
|
||||
* group in the connected robot, if [K_q] or [Z_q] contains any value outside the valid range,
|
||||
* or if size of any input vector does not match robot DoF.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning Changing damping ratio [Z_q] to a non-nominal value may lead to performance and
|
||||
* stability issues, please use with caution.
|
||||
*/
|
||||
void SetJointImpedance(
|
||||
JointGroup group, const std::vector<double>& K_q, const std::vector<double>& Z_q = {});
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set maximum contact torques for the robot's joint motion controller used in
|
||||
* the joint impedance control modes. The controller will regulate its output to maintain
|
||||
* contact torques with the environment under the set values.
|
||||
* @param[in] group The joint group to set maximum contact torques for. Only existing single-arm
|
||||
* joint groups and external axis joint groups are accepted.
|
||||
* @param[in] max_torques Maximum contact torques: \f$ tau_q \in \mathbb{R}^{n \times 1} \f$.
|
||||
* Valid range: [0, RobotInfo::tau_max]. Unit: \f$ [Nm] \f$.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
|
||||
* group in the connected robot, if [max_torques] contains any value outside the valid range, or
|
||||
* if its size does not match robot DoF.
|
||||
* @throw std::logic_error if the robot is not in an applicable control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void SetMaxContactTorque(JointGroup group, const std::vector<double>& max_torques);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set inertia shaping scales for the robot's joint motion controller used in
|
||||
* the joint impedance control modes.
|
||||
* @param[in] group The joint group to set inertia shaping scales for. Only existing single-arm
|
||||
* joint groups and external axis joint groups are accepted.
|
||||
* @param[in] inertia_scales Inertia shaping scales: \f$ \sigma_q \in \mathbb{R}^{n \times 1}
|
||||
* \f$. Valid range: [0.75, 1.0]. The nominal (safe) value is 1.0, which means no shaping.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
|
||||
* group in the connected robot, if [inertia_scales] contains any value outside the valid range,
|
||||
* or if its size does not match robot DoF.
|
||||
* @throw std::logic_error if the robot is not in an applicable control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @par Joint inertia shaping
|
||||
* In joint impedance control modes, it is possible to shape down the natural inertia of the
|
||||
* joints to make them behave as if they are lighter. The parameter [inertia_scales] sets the
|
||||
* scale of shaped/natural inertia for each joint. Smaller scale corresponds to lighter inertia.
|
||||
*/
|
||||
void SetJointInertiaScale(JointGroup group, const std::vector<double>& inertia_scales);
|
||||
|
||||
//================================== DIRECT CARTESIAN CONTROL ==================================
|
||||
/**
|
||||
* @brief [Non-blocking] Continuously stream Cartesian motion and/or force commands of the
|
||||
* specified joint group(s) to the robot. The commands are tracked by a unified motion-force
|
||||
* controller that allows force control in zero or more Cartesian axes and motion control in
|
||||
* the other axes.
|
||||
* @param[in] cmds Real-time Cartesian motion/force commands mapped by joint group. Only
|
||||
* existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
|
||||
* single-arm joint group in the connected robot.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if the robot is not operational.
|
||||
* @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE.
|
||||
* @note Real-time (RT).
|
||||
* @warning Always stream smooth and continuous motion commands to avoid sudden movements. The
|
||||
* force commands don't need to be continuous.
|
||||
* @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
|
||||
* instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
|
||||
* towards -Z direction, so that upon contact, the sensed force will be +5 N.
|
||||
* @par How to achieve pure motion control?
|
||||
* Use SetForceControlAxis() to disable force control for all Cartesian axes to achieve pure
|
||||
* motion control. This function does pure motion control by default.
|
||||
* @par How to achieve pure force control?
|
||||
* Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure
|
||||
* force control, active or passive.
|
||||
* @par How to achieve unified motion-force control?
|
||||
* Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave
|
||||
* the rest axes motion-controlled, then provide target pose for the motion-controlled axes and
|
||||
* target wrench for the force-controlled axes.
|
||||
* @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
|
||||
* SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
|
||||
*/
|
||||
void StreamCartesianMotionForce(const std::map<JointGroup, RtCartesianCmd>& cmds);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Discretely send Cartesian motion and/or force commands of the specified
|
||||
* joint group(s) to the robot. An internal motion generator will smoothen the discrete
|
||||
* commands, which are then tracked by a unified motion-force controller that allows force
|
||||
* control in zero or more Cartesian axes and motion control in the other axes. The robot's
|
||||
* internal motion generator will smoothen the discrete commands.
|
||||
* @param[in] cmds Non-real-time Cartesian motion/force commands mapped by joint group. Only
|
||||
* existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
|
||||
* single-arm joint group in the connected robot, or if any of the last 4 input parameters is
|
||||
* not positive.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if the robot is not operational.
|
||||
* @note Applicable control modes: NRT_CARTESIAN_MOTION_FORCE, NRT_SUPER_PRIMITIVE.
|
||||
* @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
|
||||
* instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
|
||||
* towards -Z direction, so that upon contact, the sensed force will be +5 N.
|
||||
* @par How to achieve pure motion control?
|
||||
* Use SetForceControlAxis() to disable force control for all Cartesian axes to achieve pure
|
||||
* motion control. This function does pure motion control by default.
|
||||
* @par How to achieve pure force control?
|
||||
* Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure
|
||||
* force control, active or passive.
|
||||
* @par How to achieve unified motion-force control?
|
||||
* Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave
|
||||
* the rest axes motion-controlled, then provide target pose for the motion-controlled axes and
|
||||
* target wrench for the force-controlled axes.
|
||||
* @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
|
||||
* SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
|
||||
*/
|
||||
void SendCartesianMotionForce(const std::map<JointGroup, NrtCartesianCmd>& cmds);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Discretely send Cartesian multi-waypoint motion and/or force commands
|
||||
* for the robot to track using non-real-time super primitives.
|
||||
* @param[in] cmds Non-real-time Cartesian multi-waypoint commands mapped by joint group. Only
|
||||
* existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
|
||||
* single-arm joint group in the connected robot, or if any waypoint list is empty or any
|
||||
* waypoint's last 4 input parameters is not positive.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if the robot is not operational.
|
||||
* @note Applicable control modes: NRT_SUPER_PRIMITIVE.
|
||||
* @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
|
||||
* instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
|
||||
* towards -Z direction, so that upon contact, the sensed force will be +5 N.
|
||||
* @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
|
||||
* SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
|
||||
*/
|
||||
void SendCartesianMotionForceMultiWaypoint(
|
||||
const std::map<JointGroup, NrtCartesianMultiWaypointCmd>& cmds);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set impedance properties of the robot's Cartesian motion controller
|
||||
* used in the Cartesian motion-force control modes.
|
||||
* @param[in] group The joint group to set Cartesian impedance for. Only existing single-arm
|
||||
* joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] K_x Cartesian motion stiffness: \f$ K_x \in \mathbb{R}^{6 \times 1} \f$.
|
||||
* Setting motion stiffness of a motion-controlled Cartesian axis to 0 will make this axis
|
||||
* free-floating. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$
|
||||
* \mathbb{R}^{3 \times 1} \f$ angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T
|
||||
* \f$. Valid range: [0, RobotInfo::K_x_nom]. Unit: \f$ [N/m]:[Nm/rad] \f$.
|
||||
* @param[in] Z_x Cartesian motion damping ratio: \f$ Z_x \in \mathbb{R}^{6 \times 1} \f$.
|
||||
* Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear damping ratio and \f$ \mathbb{R}^{3 \times
|
||||
* 1} \f$ angular damping ratio: \f$ [\zeta_x, \zeta_y, \zeta_z, \zeta_{Rx}, \zeta_{Ry},
|
||||
* \zeta_{Rz}]^T \f$. Valid range: [0.3, 0.8]. The nominal (safe) value is provided as default.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, or if [K_x] or [Z_x] contains any value outside the valid range.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
|
||||
* NRT_SUPER_PRIMITIVE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning Changing damping ratio [Z_x] to a non-nominal value may lead to performance and
|
||||
* stability issues, please use with caution.
|
||||
*/
|
||||
void SetCartesianImpedance(JointGroup group, const std::array<double, kCartDoF>& K_x,
|
||||
const std::array<double, kCartDoF>& Z_x = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7});
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set maximum contact wrench for the motion control part of the Cartesian
|
||||
* motion-force control modes. The controller will regulate its output to maintain contact
|
||||
* wrench (force and moment) with the environment under the set values.
|
||||
* @param[in] group The joint group to set maximum contact wrench for. Only existing
|
||||
* single-arm joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] max_wrench Maximum contact wrench (force and moment): \f$ F_{max} \in
|
||||
* \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ maximum force and
|
||||
* \f$ \mathbb{R}^{3 \times 1} \f$ maximum moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
|
||||
* Unit: \f$ [N]:[Nm] \f$.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, or if [max_wrench] contains any negative value.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note The maximum contact wrench regulation only applies to the motion control part.
|
||||
* @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
|
||||
* NRT_SUPER_PRIMITIVE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning The maximum contact wrench regulation cannot be enabled if any of the rotational
|
||||
* Cartesian axes is enabled for moment control.
|
||||
*/
|
||||
void SetMaxContactWrench(JointGroup group, const std::array<double, kCartDoF>& max_wrench);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set reference joint positions for the null-space posture control module
|
||||
* used in the Cartesian motion-force control modes.
|
||||
* @param[in] group The joint group to set null-space posture for. Only existing single-arm
|
||||
* joint groups and external axis joint groups are accepted.
|
||||
* @param[in] ref_positions Reference joint positions for the null-space posture control:
|
||||
* \f$ q_{ns} \in \mathbb{R}^{n \times 1} \f$. Valid range: [RobotInfo::q_min,
|
||||
* RobotInfo::q_max]. Unit: \f$ [rad] \f$.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm or external axis
|
||||
* joint group in the connected robot, if [ref_positions] contains any value outside the valid
|
||||
* range, or if its size does not match robot DoF.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
|
||||
* NRT_SUPER_PRIMITIVE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning The reference joint positions will be automatically reset to the robot's current
|
||||
* joint positions upon re-entering the applicable control modes.
|
||||
* @par Null-space posture control
|
||||
* Similar to human arm, a robotic arm with redundant joint-space degree(s) of freedom (DoF > 6)
|
||||
* can change its overall posture without affecting the ongoing primary task. This is achieved
|
||||
* through a technique called "null-space control". After the reference joint positions of a
|
||||
* desired robot posture are set using this function, the robot's null-space control module will
|
||||
* try to pull the arm as close to this posture as possible without affecting the primary
|
||||
* Cartesian motion-force control task.
|
||||
*/
|
||||
void SetNullSpacePosture(JointGroup group, const std::vector<double>& ref_positions);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set weights of the three optimization objectives while computing the
|
||||
* robot's null-space posture. Change the weights to optimize robot performance for different
|
||||
* use cases.
|
||||
* @param[in] group The joint group to set null-space objectives for. Only existing
|
||||
* single-arm joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] linear_manipulability Increase this weight to improve the robot's capability to
|
||||
* translate freely in Cartesian space, i.e. a broader range of potential translation movements.
|
||||
* Valid range: [0.0, 1.0].
|
||||
* @param[in] angular_manipulability Increase this weight to improve the robot's capability to
|
||||
* rotate freely in Cartesian space, i.e. a broader range of potential rotation movements. Valid
|
||||
* range: [0.0, 1.0].
|
||||
* @param[in] ref_positions_tracking Increase this weight to make the robot track closer to the
|
||||
* reference joint positions specified using SetNullSpacePosture(). Valid range: [0.1, 1.0].
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, or if any of the input parameters is outside its valid range.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note The default value is provided for each parameter.
|
||||
* @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
|
||||
* NRT_SUPER_PRIMITIVE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning The optimization weights will be automatically reset to the provided default values
|
||||
* upon re-entering the applicable control modes.
|
||||
*/
|
||||
void SetNullSpaceObjectives(JointGroup group, double linear_manipulability = 0.0,
|
||||
double angular_manipulability = 0.0, double ref_positions_tracking = 0.5);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set Cartesian axes to enable force control while in the Cartesian
|
||||
* motion-force control modes. Axes not enabled for force control will be motion-controlled.
|
||||
* @param[in] group The joint group to set force control axes for. Only existing single-arm
|
||||
* joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] enabled_axes Flags to enable/disable force control for certain Cartesian axes in
|
||||
* the force control reference frame (configured by SetForceControlFrame()). The axis order is
|
||||
* \f$ [X, Y, Z, Rx, Ry, Rz] \f$.
|
||||
* @param[in] max_linear_vel For linear Cartesian axes that are enabled for force control, limit
|
||||
* the moving velocity to these values as a protection mechanism in case of contact loss. The
|
||||
* axis order is \f$ [X, Y, Z] \f$. Valid range: [0.005, 2.0]. Unit: \f$ [m/s] \f$.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, or if [max_linear_vel] contains any value outside the valid range.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
|
||||
* NRT_SUPER_PRIMITIVE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @note If not set, force control is disabled for all Cartesian axes by default.
|
||||
* @warning The maximum linear velocity protection for force control axes is only effective
|
||||
* under active force control (i.e. passive force control disabled), see
|
||||
* SetPassiveForceControl().
|
||||
*/
|
||||
void SetForceControlAxis(JointGroup group, const std::array<bool, kCartDoF>& enabled_axes,
|
||||
const std::array<double, kCartDoF / 2>& max_linear_vel = {1.0, 1.0, 1.0});
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set reference frame for force control while in the Cartesian motion-force
|
||||
* control modes. The force control frame is defined by specifying its transformation with
|
||||
* regard to the root coordinate.
|
||||
* @param[in] group The joint group to set force control frame for. Only existing single-arm
|
||||
* joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] root_coord Reference coordinate of [T_in_root].
|
||||
* @param[in] T_in_root Transformation from [root_coord] to the user-defined force control
|
||||
* frame: \f$ ^{root}T_{force} \in \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3
|
||||
* \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w,
|
||||
* q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$. If root coordinate is a fixed one (e.g. WORLD),
|
||||
* then the force control frame will also be fixed; if root coordinate is a moving one (e.g.
|
||||
* TCP), then the force control frame will also be moving with the root coordinate. An identity
|
||||
* transformation is provided as default.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, or if [root_coord] is invalid.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
|
||||
* NRT_SUPER_PRIMITIVE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @note If not set, the robot will use WORLD origin as the force control frame by default.
|
||||
* @par Force control frame
|
||||
* In Cartesian motion-force control modes, the reference frame of motion control is always the
|
||||
* world frame, but the reference frame of force control can be an arbitrary one. While the
|
||||
* world frame is the commonly used global coordinate, the current TCP frame is a dynamic local
|
||||
* coordinate whose transformation with regard to the world frame changes as the robot TCP
|
||||
* moves. When using world frame with no transformation as the force control frame, the
|
||||
* force-controlled axes and motion-controlled axes are guaranteed to be orthogonal. Otherwise,
|
||||
* the force-controlled axes and motion-controlled axes are NOT guaranteed to be orthogonal
|
||||
* because different reference frames are used. In this case, it's recommended but not required
|
||||
* to set the target pose such that the actual robot motion direction(s) are orthogonal to force
|
||||
* direction(s). If they are not orthogonal, the motion control's vector component(s) in the
|
||||
* force direction(s) will be eliminated.
|
||||
*/
|
||||
void SetForceControlFrame(JointGroup group, CoordType root_coord,
|
||||
const std::array<double, kPoseSize>& T_in_root = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0});
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Enable or disable passive force control for the Cartesian motion-force
|
||||
* control modes. When enabled, an open-loop force controller will be used to feed forward the
|
||||
* target wrench, i.e. passive force control. When disabled, a closed-loop force controller will
|
||||
* be used to track the target wrench, i.e. active force control.
|
||||
* @param[in] group The joint group to toggle passive force control for. Only existing
|
||||
* single-arm joint groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] is_enabled True: enable; false: disable. By default, passive force control is
|
||||
* disabled and active force control is used.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot.
|
||||
* @throw std::logic_error if the robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @note If not set, the passive force control is disabled by default.
|
||||
* @par Difference between active and passive force control
|
||||
* Active force control uses a feedback loop to reduce the error between target wrench and
|
||||
* measured wrench. This method results in better force tracking performance, but at the cost of
|
||||
* additional Cartesian damping which could potentially decrease motion tracking performance. On
|
||||
* the other hand, passive force control simply feeds forward the target wrench. This methods
|
||||
* results in worse force tracking performance, but is more robust and does not introduce
|
||||
* additional Cartesian damping. The choice of active or passive force control depends on the
|
||||
* actual application.
|
||||
*/
|
||||
void SetPassiveForceControl(JointGroup group, bool is_enabled);
|
||||
|
||||
//======================================== IO CONTROL ========================================
|
||||
/**
|
||||
* @brief [Blocking] Set one or more digital output ports, including 16 on the control box plus
|
||||
* 2 inside each wrist connector.
|
||||
* @param[in] digital_outputs A map of {port_index, port_value}. For [port_index], the valid
|
||||
* range is [0, 17]. For [port_value], true: set port high, false: set port low. For example,
|
||||
* {{1, true}, {3, false}, {10, true}}.
|
||||
* @throw std::invalid_argument if any provided port index is outside the valid range.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void SetDigitalOutputs(const std::map<unsigned int, bool>& digital_outputs);
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current reading from all digital input ports, including 16 on the
|
||||
* control box plus 2 in each wrist connector.
|
||||
* @return A boolean array whose index corresponds to that of the digital input ports.
|
||||
* True: port high; false: port low.
|
||||
*/
|
||||
std::array<bool, kIOPorts> digital_inputs() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current reading from all digital output ports, including 16 on the
|
||||
* control box plus 2 in each wrist connector.
|
||||
* @return A boolean array whose index corresponds to that of the digital output ports.
|
||||
* True: port high; false: port low.
|
||||
*/
|
||||
std::array<bool, kIOPorts> digital_outputs() const;
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
|
||||
friend class Device;
|
||||
friend class FileIO;
|
||||
friend class Gripper;
|
||||
friend class Maintenance;
|
||||
friend class Model;
|
||||
friend class Safety;
|
||||
friend class Tool;
|
||||
friend class WorkCoord;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_ROBOT_HPP_ */
|
||||
+168
@@ -0,0 +1,168 @@
|
||||
/**
|
||||
* @file safety.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_SAFETY_HPP_
|
||||
#define FLEXIV_RDK_SAFETY_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/** Number of safety IO ports */
|
||||
constexpr size_t kSafetyIOPorts = 8;
|
||||
|
||||
/**
|
||||
* @struct SafetyLimits
|
||||
* @brief Data structure containing configurable robot safety limits.
|
||||
*/
|
||||
struct SafetyLimits
|
||||
{
|
||||
/** Lower safety limits of joint positions for each joint group: \f$ q_{min} \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> q_min = {};
|
||||
|
||||
/** Upper safety limits of joint positions for each joint group: \f$ q_{max} \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> q_max = {};
|
||||
|
||||
/** Upper safety limits of joint velocities for each joint group when the robot is in normal
|
||||
* state: \f$ \dot{q}^{normal}_{max} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$. */
|
||||
std::map<JointGroup, std::vector<double>> dq_max_normal = {};
|
||||
|
||||
/** Upper safety limits of joint velocities for each joint group when the robot is in reduced
|
||||
* state: \f$ \dot{q}^{reduced}_{max} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$.
|
||||
* @see Robot::reduced() */
|
||||
std::map<JointGroup, std::vector<double>> dq_max_reduced = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @class Safety
|
||||
* @brief Interface to manage safety settings of the robot. A password is required to authenticate
|
||||
* this interface.
|
||||
* @note As mentioned in the function doc, certain safety settings are only applicable to the arms
|
||||
* but not the external axis.
|
||||
*/
|
||||
class Safety
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the safety settings interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @param[in] password Password to authorize making changes to the robot's safety settings.
|
||||
* @throw std::invalid_argument if the provided password is incorrect.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
*/
|
||||
Safety(const Robot& robot, const std::string& password);
|
||||
virtual ~Safety();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Default values of the safety limits of the connected robot.
|
||||
* @return SafetyLimits value copy.
|
||||
*/
|
||||
SafetyLimits default_limits() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current values of the safety limits of the connected robot.
|
||||
* @return SafetyLimits value copy.
|
||||
*/
|
||||
SafetyLimits current_limits() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Current reading from all safety input ports.
|
||||
* @return A boolean array whose index corresponds to that of the safety input ports.
|
||||
* True: port high; false: port low.
|
||||
*/
|
||||
std::array<bool, kSafetyIOPorts> safety_inputs() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set safety limits on the joint positions of the manipulator, which will
|
||||
* honor this setting when making movements.
|
||||
* @param[in] min_positions Minimum joint positions: \f$ q_{min} \in \mathbb{R}^{n \times 1}
|
||||
* \f$. Valid range: [default_min_joint_positions, default_max_joint_positions]. Unit: \f$ [rad]
|
||||
* \f$.
|
||||
* @param[in] max_positions Maximum joint positions: \f$ q_{max} \in \mathbb{R}^{n \times 1}
|
||||
* \f$. Valid range: [default_min_joint_positions, default_max_joint_positions]. Unit: \f$ [rad]
|
||||
* \f$.
|
||||
* @param[in] group Joint group to apply the limits to.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, if [min_positions] or [max_positions] contains any value outside the valid
|
||||
* range, or if size of any input vector does not match manipulator DoF.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning A reboot is required for the updated safety settings to take effect. After reboot,
|
||||
* make sure to use current_limits() to double check the updated safety limits are correct.
|
||||
*/
|
||||
void SetJointPositionLimits(JointGroup group, const std::vector<double>& min_positions,
|
||||
const std::vector<double>& max_positions);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set safety limits on the joint velocities of the manipulator, which will
|
||||
* honor this setting when making movements under the normal state.
|
||||
* @param[in] max_velocities Maximum joint velocities for normal state: \f$ \dot{q}_{max} \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Valid range: [0.8727, joint_velocity_normal_limits]. Unit: \f$
|
||||
* [rad/s] \f$.
|
||||
* @param[in] group Joint group to apply the limits to.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, if [max_velocities] contains any value outside the valid range, or if its
|
||||
* size does not match manipulator DoF.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning A reboot is required for the updated safety settings to take effect. After reboot,
|
||||
* make sure to use current_limits() to double check the updated safety limits are correct.
|
||||
*/
|
||||
void SetJointVelocityNormalLimits(JointGroup group, const std::vector<double>& max_velocities);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Set safety limits on the joint velocities of the manipulator, which will
|
||||
* honor this setting when making movements under the reduced state.
|
||||
* @param[in] max_velocities Maximum joint velocities for reduced state: \f$ \dot{q}_{max} \in
|
||||
* \mathbb{R}^{n \times 1} \f$. Valid range: [0.8727, joint_velocity_normal_limits]. Unit: \f$
|
||||
* [rad/s] \f$.
|
||||
* @param[in] group Joint group to apply the limits to.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot, if [max_velocities] contains any value outside the valid range, or if its
|
||||
* size does not match manipulator DoF.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @warning A reboot is required for the updated safety settings to take effect. After reboot,
|
||||
* make sure to use current_limits() to double check the updated safety limits are correct.
|
||||
*/
|
||||
void SetJointVelocityReducedLimits(JointGroup group, const std::vector<double>& max_velocities);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Change settings of the regulator on the joint output torques of the
|
||||
* manipulator and external axes. The regulator will limit the joints' total output torques so
|
||||
* that the measured torques are less likely to exceed safety limits under large payload or
|
||||
* fast/abrupt motions.
|
||||
* @param[in] limiting_factor Factor to limit the total output torques: \f$ \tau^{o}_{max} =
|
||||
* \tau_{max} \times factor \f$.
|
||||
* @param[in] error_threshold If the unregulated output torque of any joint exceeds the
|
||||
* configured regulator limit for more than [error_threshold] cycles consecutively, the robot
|
||||
* will trigger a minor fault and stop.
|
||||
* @throw std::invalid_argument if [limiting_factor] or [error_threshold] is not positive.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
* @note This setting takes effect immediately and doesn't require a reboot.
|
||||
* @warning Setting the limiting factor too small can cause performance degradation.
|
||||
*/
|
||||
void SetJointOutputTorqueRegulator(
|
||||
double limiting_factor = 1.3, unsigned int error_threshold = 50);
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_SAFETY_HPP_ */
|
||||
@@ -0,0 +1,113 @@
|
||||
/**
|
||||
* @file scheduler.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_SCHEDULER_HPP_
|
||||
#define FLEXIV_RDK_SCHEDULER_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @class Scheduler
|
||||
* @brief Real-time scheduler that can simultaneously run multiple periodic tasks. Parameters for
|
||||
* each task are configured independently.
|
||||
*/
|
||||
class Scheduler
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Blocking] Instantiate a real-time scheduler.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
* @warning This constructor blocks until the initialization sequence is successfully finished.
|
||||
*/
|
||||
Scheduler();
|
||||
virtual ~Scheduler();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Add a new periodic task to the scheduler's task pool. Each task in the
|
||||
* pool is assigned to a dedicated thread with independent thread configuration.
|
||||
* @param[in] callback Callback function of user task.
|
||||
* @param[in] task_name A unique name for this task.
|
||||
* @param[in] interval Execution interval of this periodic task [ms]. The minimum available
|
||||
* interval is 1 ms, equivalent to 1 kHz loop frequency.
|
||||
* @param[in] priority Priority for this task thread, can be set to
|
||||
* min_priority()–max_priority() for real-time scheduling, or 0 for non-real-time scheduling.
|
||||
* When the priority is set to use real-time scheduling, this thread becomes a real-time thread
|
||||
* and can only be interrupted by threads with higher priority. When the priority is set to use
|
||||
* non-real-time scheduling (i.e. 0), this thread becomes a non-real-time thread and can be
|
||||
* interrupted by any real-time threads. The common practice is to set priority of the most
|
||||
* critical tasks to max_priority() or near, and set priority of other non-critical tasks to 0
|
||||
* or near. To avoid race conditions, the same priority should be assigned to only one task.
|
||||
* @param[in] cpu_affinity CPU core for this task thread to bind to, can be set to 2–(num_cores
|
||||
* - 1). This task thread will only run on the specified CPU core. If left with the default
|
||||
* value (-1), then this task thread will not bind to any CPU core, and the system will decide
|
||||
* which core to run this task thread on according to the system's own strategy. The common
|
||||
* practice is to bind the high-priority task to a dedicated spare core, and bind low-priority
|
||||
* tasks to other cores or just leave them unbound (cpu_affinity = -1).
|
||||
* @throw std::logic_error if the scheduler is already started or is not fully initialized yet.
|
||||
* @throw std::invalid_argument if the specified interval/priority/affinity is invalid or the
|
||||
* specified task name is duplicate.
|
||||
* @throw std::runtime_error if an error is triggered by the client computer.
|
||||
* @note Setting CPU affinity on macOS has no effect, as its Mach kernel takes full control of
|
||||
* thread placement so CPU binding is not supported.
|
||||
* @warning Calling this function after start() is not allowed.
|
||||
* @warning For maximum scheduling performance, setting CPU affinity to 0 or 1 is not allowed:
|
||||
* core 0 is usually the default core for system processes and can be crowded; core 1 is
|
||||
* reserved for the scheduler itself.
|
||||
*/
|
||||
void AddTask(std::function<void(void)>&& callback, const std::string& task_name, int interval,
|
||||
int priority, int cpu_affinity = -1);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Start all added tasks. A dedicated thread will be created for each added
|
||||
* task and the periodic execution will begin.
|
||||
* @throw std::logic_error if the scheduler is not initialized yet.
|
||||
* @throw std::runtime_error if failed to start the tasks.
|
||||
* @note This function blocks until all added tasks are started.
|
||||
*/
|
||||
void Start();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Stop all added tasks. The periodic execution will stop and all task threads
|
||||
* will be closed with the resources released.
|
||||
* @throw std::logic_error if the scheduler is not initialized or the tasks are not started yet.
|
||||
* @throw std::runtime_error if failed to stop the tasks.
|
||||
* @note Calling start() again can restart the added tasks.
|
||||
* @note This function blocks until all task threads have exited and resources are released.
|
||||
* @warning This function cannot be called from within a task thread.
|
||||
*/
|
||||
void Stop();
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Maximum available priority for user tasks.
|
||||
* @return The maximum priority that can be set for a user task with real-time scheduling policy
|
||||
* when calling AddTask().
|
||||
*/
|
||||
int max_priority() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Minimum available priority for user tasks.
|
||||
* @return The minimum priority that can be set for a user task with real-time scheduling policy
|
||||
* when calling AddTask().
|
||||
*/
|
||||
int min_priority() const;
|
||||
|
||||
/**
|
||||
* @brief [Non-blocking] Number of tasks added to the scheduler.
|
||||
* @return Number of added tasks.
|
||||
*/
|
||||
size_t num_tasks() const;
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_SCHEDULER_HPP_ */
|
||||
+187
@@ -0,0 +1,187 @@
|
||||
/**
|
||||
* @file tool.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_TOOL_HPP_
|
||||
#define FLEXIV_RDK_TOOL_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @struct ToolParams
|
||||
* @brief Data structure containing robot tool parameters.
|
||||
* @see Tool::params().
|
||||
*/
|
||||
struct ToolParams
|
||||
{
|
||||
/** Total mass. Unit: \f$ [kg] \f$ */
|
||||
double mass = 0.0;
|
||||
|
||||
/** Center of mass in robot flange frame: \f$ [x, y, z] \f$. Unit: \f$ [m] \f$ */
|
||||
std::array<double, 3> CoM = {};
|
||||
|
||||
/** Inertia at center of mass: \f$ [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] \f$. Unit: \f$ [kg m^2] \f$ */
|
||||
std::array<double, 6> inertia = {};
|
||||
|
||||
/** Position and orientation of the tool center point (TCP) in flange frame. Consists of \f$
|
||||
* \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x,
|
||||
* y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
|
||||
std::array<double, kPoseSize> tcp_location = {};
|
||||
};
|
||||
|
||||
/**
|
||||
* @class Tool
|
||||
* @brief Interface to manage tools of the robot. All updates take effect immediately without a
|
||||
* power cycle. However, the robot must be in IDLE mode when applying changes.
|
||||
*/
|
||||
class Tool
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the robot tool interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
*/
|
||||
Tool(const Robot& robot);
|
||||
virtual ~Tool();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] A list of all configured tools.
|
||||
* @return Tool names as a string list.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::vector<std::string> list() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Name of the active tool currently used by the specified joint group.
|
||||
* @param[in] group Joint group to get tool name for. Only existing single-arm joint groups
|
||||
* like ARM_1 and ARM_2 are accepted.
|
||||
* @return Name of the active tool. Return "Flange" if there's no active tool.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::string name(JointGroup group) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Whether the specified tool already exists.
|
||||
* @param[in] name Name of the tool to check.
|
||||
* @return True if the specified tool exists.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
bool exist(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Parameters of the active tool currently used by the specified joint group.
|
||||
* @param[in] group Joint group to get tool parameters for. Only existing single-arm joint
|
||||
* groups like ARM_1 and ARM_2 are accepted.
|
||||
* @return ToolParams data struct.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
ToolParams params(JointGroup group) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Parameters of the specified tool.
|
||||
* @param[in] name Name of the tool to get parameters for, must be an existing one.
|
||||
* @return ToolParams data struct.
|
||||
* @throw std::invalid_argument if the specified tool does not exist.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
ToolParams params(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Add a new tool with user-specified parameters.
|
||||
* @param[in] name Name of the new tool, must be unique.
|
||||
* @param[in] params Parameters of the new tool.
|
||||
* @throw std::invalid_argument if the specified tool already exists.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Add(const std::string& name, const ToolParams& params);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Switch active tool for the specified joint group. All following operations
|
||||
* will default to use this tool.
|
||||
* @param[in] group Joint group to switch active tool for. Only existing single-arm joint
|
||||
* groups like ARM_1 and ARM_2 are accepted.
|
||||
* @param[in] name Name of the tool to switch to, must be an existing one.
|
||||
* @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
|
||||
* connected robot.
|
||||
* @throw std::invalid_argument if the specified tool does not exist.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Switch(JointGroup group, const std::string& name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Update the parameters of an existing tool.
|
||||
* @param[in] name Name of the tool to update, must be an existing one.
|
||||
* @param[in] params New parameters for the specified tool.
|
||||
* @throw std::invalid_argument if the specified tool does not exist.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Update(const std::string& name, const ToolParams& params);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Remove an existing tool.
|
||||
* @param[in] name Name of the tool to remove, must be an existing one but cannot be "Flange".
|
||||
* @throw std::invalid_argument if the specified tool does not exist.
|
||||
* @throw std::logic_error if robot is not in the correct control mode or trying to remove
|
||||
* "Flange".
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Remove(const std::string& name);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Calibrate the payload parameters (mass, CoM, and inertia) of a tool.
|
||||
* @param[in] tool_mounted Whether the tool to be calibrated is mounted on the robot flange when
|
||||
* triggering this calibration process. See details below.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if fault occurred during the calibration or failed to get the
|
||||
* calibration result.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the calibration is finished.
|
||||
* @warning [tcp_location] in the returned struct will be zeros and should be ignored.
|
||||
* @par How to properly calibrate the payload parameters of a tool?
|
||||
* 1. Call Switch("Flange") to disable any active tool from the robot software.
|
||||
* 2. Physically mount the tool to be calibrated to robot flange.
|
||||
* 3. Call this function with [tool_mounted] set to TRUE, then wait for completion. If the robot
|
||||
* has a force-torque (FT) sensor, then the returned result will be accurate enough and the
|
||||
* optional steps can be skipped. If the robot does not have an FT sensor, then the optional
|
||||
* steps are recommended to improve accuracy.
|
||||
* 4. (Optional) Physically unmount the tool from robot flange.
|
||||
* 5. (Optional) Call this function again but with [tool_mounted] set to FALSE, then wait for
|
||||
* completion.
|
||||
* 6. Review the returned result and call Add() or Update() to apply the calibrated payload
|
||||
* parameters to a new or existing tool. Note that [tcp_location] in the returned struct is
|
||||
* invalid and cannot be used directly.
|
||||
*/
|
||||
ToolParams CalibratePayloadParams(bool tool_mounted);
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_TOOL_HPP_ */
|
||||
@@ -0,0 +1,245 @@
|
||||
/**
|
||||
* @file utility.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_UTILITY_HPP_
|
||||
#define FLEXIV_RDK_UTILITY_HPP_
|
||||
|
||||
#include "data.hpp"
|
||||
#include <Eigen/Eigen>
|
||||
#include <algorithm>
|
||||
#include <sstream>
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
namespace utility {
|
||||
|
||||
/** @brief Whether the specified group represents a single arm. */
|
||||
inline bool IsSingleArmGroup(JointGroup group)
|
||||
{
|
||||
return group == JointGroup::ARM_1 || group == JointGroup::ARM_2;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check whether one primitive state is true.
|
||||
* @param[in] primitive_states Primitive states.
|
||||
* @param[in] state_name Primitive state name to check, e.g. "reachedTarget".
|
||||
* @return True if the state exists and its integer value is non-zero.
|
||||
*/
|
||||
inline bool PrimitiveStateTrue(
|
||||
const PrimitiveStates& primitive_states, const std::string& state_name)
|
||||
{
|
||||
const auto state_it = primitive_states.names_and_values.find(state_name);
|
||||
if (state_it == primitive_states.names_and_values.end()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!std::holds_alternative<int>(state_it->second)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return std::get<int>(state_it->second) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check whether one primitive state is true for all groups included in [primitive_states].
|
||||
* @param[in] primitive_states Primitive states keyed by joint group.
|
||||
* @param[in] state_name Primitive state name to check, e.g. "reachedTarget".
|
||||
* @return True if all included groups contain the state and its integer value is non-zero.
|
||||
*/
|
||||
inline bool PrimitiveStateTrueForGroups(
|
||||
const std::map<JointGroup, PrimitiveStates>& primitive_states, const std::string& state_name)
|
||||
{
|
||||
return std::all_of(primitive_states.begin(), primitive_states.end(),
|
||||
[&](const auto& entry) { return PrimitiveStateTrue(entry.second, state_name); });
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check whether one primitive state is true for each group included in [primitive_states],
|
||||
* using a per-group state name.
|
||||
* @param[in] primitive_states Primitive states keyed by joint group.
|
||||
* @param[in] state_names Primitive state names keyed by joint group.
|
||||
* @return True if every included group has a matching state name entry and its integer value is
|
||||
* non-zero.
|
||||
*/
|
||||
inline bool PrimitiveStateTrueForGroups(
|
||||
const std::map<JointGroup, PrimitiveStates>& primitive_states,
|
||||
const std::map<JointGroup, std::string>& state_names)
|
||||
{
|
||||
return std::all_of(primitive_states.begin(), primitive_states.end(), [&](const auto& entry) {
|
||||
if (!state_names.contains(entry.first)) {
|
||||
return false;
|
||||
}
|
||||
return PrimitiveStateTrue(entry.second, state_names.at(entry.first));
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert quaternion to Euler angles with ZYX axis rotations.
|
||||
* @param[in] quat Quaternion input in [w,x,y,z] order.
|
||||
* @return Euler angles in [x,y,z] order [rad].
|
||||
* @note The return value, when converted to degrees, is the same Euler angles used by Move
|
||||
* primitives.
|
||||
*/
|
||||
inline std::array<double, 3> Quat2EulerZYX(const std::array<double, 4>& quat)
|
||||
{
|
||||
// Form quaternion
|
||||
Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
|
||||
|
||||
// The returned array is in [z,y,x] order
|
||||
auto euler_ZYX = q.toRotationMatrix().eulerAngles(2, 1, 0);
|
||||
|
||||
// Convert to general [x,y,z] order
|
||||
return (std::array<double, 3> {euler_ZYX[2], euler_ZYX[1], euler_ZYX[0]});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert radians to degrees for a single value.
|
||||
*/
|
||||
inline double Rad2Deg(double rad)
|
||||
{
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
return (rad / kPi * 180.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert radians to degrees for an array.
|
||||
*/
|
||||
template <size_t N>
|
||||
inline std::array<double, N> Rad2Deg(const std::array<double, N>& rad_arr)
|
||||
{
|
||||
std::array<double, N> deg_arr;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
deg_arr[i] = Rad2Deg(rad_arr[i]);
|
||||
}
|
||||
return deg_arr;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert radians to degrees for a vector.
|
||||
*/
|
||||
inline std::vector<double> Rad2Deg(const std::vector<double>& rad_vec)
|
||||
{
|
||||
std::vector<double> deg_vec;
|
||||
for (const auto& v : rad_vec) {
|
||||
deg_vec.push_back(Rad2Deg(v));
|
||||
}
|
||||
return deg_vec;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert an std::vector to a string.
|
||||
* @param[in] vec std::vector of any type and size.
|
||||
* @param[in] decimal Decimal places to keep for each floating-point number in the vector.
|
||||
* @param[in] separator Character to separate between numbers.
|
||||
* @return The converted string.
|
||||
*/
|
||||
template <typename T>
|
||||
inline std::string Vec2Str(
|
||||
const std::vector<T>& vec, size_t decimal = 3, const std::string& separator = " ")
|
||||
{
|
||||
std::string padding = "";
|
||||
std::ostringstream oss;
|
||||
oss.precision(decimal);
|
||||
oss << std::fixed;
|
||||
|
||||
for (const auto& v : vec) {
|
||||
oss << padding << v;
|
||||
padding = separator;
|
||||
}
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert an std::array to a string.
|
||||
* @param[in] arr std::array of any type and size.
|
||||
* @param[in] decimal Decimal places to keep for each floating-point number in the array.
|
||||
* @param[in] separator Character to separate between numbers.
|
||||
* @return The converted string.
|
||||
*/
|
||||
template <typename T, size_t N>
|
||||
inline std::string Arr2Str(
|
||||
const std::array<T, N>& arr, size_t decimal = 3, const std::string& separator = " ")
|
||||
{
|
||||
std::vector<T> vec(N);
|
||||
std::copy(arr.begin(), arr.end(), vec.begin());
|
||||
return Vec2Str(vec, decimal, separator);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert the commonly used std::variant to a string.
|
||||
* @param[in] variant std::variant used by multiple rdk::Robot functions.
|
||||
* @param[in] decimal Decimal places to keep for each floating-point number in the variant.
|
||||
* @param[in] separator Character to separate between numbers in the vector.
|
||||
* @return The converted string.
|
||||
*/
|
||||
inline std::string FlexivTypes2Str(
|
||||
const rdk::FlexivDataTypes& variant, size_t decimal = 3, const std::string& separator = " ")
|
||||
{
|
||||
if (auto* val = std::get_if<int>(&variant)) {
|
||||
return Vec2Str(std::vector<int> {*val}, decimal);
|
||||
} else if (auto* val = std::get_if<double>(&variant)) {
|
||||
return Vec2Str(std::vector<double> {*val}, decimal);
|
||||
} else if (auto* val = std::get_if<std::string>(&variant)) {
|
||||
return *val;
|
||||
} else if (auto* val = std::get_if<rdk::Coord>(&variant)) {
|
||||
return (*val).str();
|
||||
} else if (auto* vec = std::get_if<std::vector<int>>(&variant)) {
|
||||
return Vec2Str(*vec, decimal, separator);
|
||||
} else if (auto* vec = std::get_if<std::vector<double>>(&variant)) {
|
||||
return Vec2Str(*vec, decimal, separator);
|
||||
} else if (auto* vec = std::get_if<std::vector<std::string>>(&variant)) {
|
||||
return Vec2Str(*vec, decimal, separator);
|
||||
} else if (auto* vec = std::get_if<std::vector<rdk::Coord>>(&variant)) {
|
||||
std::string ret;
|
||||
// Separate two Coord by " : "
|
||||
for (const auto& v : (*vec)) {
|
||||
ret += v.str() + " : ";
|
||||
}
|
||||
// Remove the trailing " : "
|
||||
if (!ret.empty()) {
|
||||
ret.erase(ret.size() - 3);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if any provided strings exist in the program arguments.
|
||||
* @param[in] argc Argument count passed to main() of the program.
|
||||
* @param[in] argv Argument vector passed to main() of the program, where argv[0] is the program
|
||||
* name.
|
||||
* @param[in] ref_strings Reference strings to check against.
|
||||
* @return True if the program arguments contain one or more reference strings.
|
||||
*/
|
||||
inline bool ProgramArgsExistAny(int argc, char** argv, const std::vector<std::string>& ref_strings)
|
||||
{
|
||||
for (int i = 0; i < argc; i++) {
|
||||
for (const auto& v : ref_strings) {
|
||||
if (v == std::string(argv[i])) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if one specific string exists in the program arguments.
|
||||
* @param[in] argc Argument count passed to main() of the program.
|
||||
* @param[in] argv Argument vector passed to main() of the program, with argv[0] being the program
|
||||
* name.
|
||||
* @param[in] ref_strings Reference string to check against.
|
||||
* @return True if the program arguments contain this specific reference string.
|
||||
*/
|
||||
inline bool ProgramArgsExist(int argc, char** argv, const std::string& ref_strings)
|
||||
{
|
||||
return ProgramArgsExistAny(argc, argv, {ref_strings});
|
||||
}
|
||||
|
||||
} /* namespace utility */
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_UTILITY_HPP_ */
|
||||
@@ -0,0 +1,105 @@
|
||||
/**
|
||||
* @file work_coord.hpp
|
||||
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
|
||||
*/
|
||||
|
||||
#ifndef FLEXIV_RDK_WORK_COORD_HPP_
|
||||
#define FLEXIV_RDK_WORK_COORD_HPP_
|
||||
|
||||
#include "robot.hpp"
|
||||
|
||||
namespace flexiv::rdk {
|
||||
|
||||
/**
|
||||
* @class WorkCoord
|
||||
* @brief Interface to manage work coordinates of the robot. All updates take effect immediately
|
||||
* without a power cycle. However, the robot must be in IDLE mode when applying changes.
|
||||
*/
|
||||
class WorkCoord
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief [Non-blocking] Instantiate the work coordinate interface.
|
||||
* @param[in] robot Reference to the instance of flexiv::rdk::Robot.
|
||||
* @throw std::runtime_error if the initialization sequence failed.
|
||||
*/
|
||||
WorkCoord(const Robot& robot);
|
||||
virtual ~WorkCoord();
|
||||
|
||||
/**
|
||||
* @brief [Blocking] A list of all configured work coordinates.
|
||||
* @return Work coordinate names as a string list.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::vector<std::string> list() const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Whether the specified work coordinate already exists.
|
||||
* @param[in] name Name of the work coordinate to check.
|
||||
* @return True if the specified work coordinate exists.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
bool exist(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Pose of an existing work coordinate.
|
||||
* @param[in] name Name of the work coordinate to get pose for, must be an existing one.
|
||||
* @return Pose of the work coordinate in world frame: \f$ ^{O}T_{work} \in \mathbb{R}^{7 \times
|
||||
* 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1}
|
||||
* \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
|
||||
* @throw std::runtime_error if failed to get a reply from the connected robot.
|
||||
* @note This function blocks until a reply is received.
|
||||
*/
|
||||
std::array<double, kPoseSize> pose(const std::string& name) const;
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Add a new work coordinate with user-specified parameters.
|
||||
* @param[in] name Name of the new work coordinate, must be unique.
|
||||
* @param[in] pose Pose of the new work coordinate in world frame: \f$ ^{O}T_{work} \in
|
||||
* \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$
|
||||
* \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$
|
||||
* [m]:[] \f$.
|
||||
* @throw std::invalid_argument if the specified work coordinate already exists.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Add(const std::string& name, const std::array<double, kPoseSize>& pose);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Update the pose of an existing work coordinate.
|
||||
* @param[in] name Name of the work coordinate to update, must be an existing one.
|
||||
* @param[in] pose New pose for the specified work coordinate in world frame: \f$ ^{O}T_{work}
|
||||
* \in \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$
|
||||
* \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$
|
||||
* [m]:[] \f$.
|
||||
* @throw std::invalid_argument if the specified work coordinate does not exist.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Update(const std::string& name, const std::array<double, kPoseSize>& pose);
|
||||
|
||||
/**
|
||||
* @brief [Blocking] Remove an existing work coordinate.
|
||||
* @param[in] name Name of the work coordinate to remove, must be an existing one.
|
||||
* @throw std::invalid_argument if the specified work coordinate does not exist.
|
||||
* @throw std::logic_error if robot is not in the correct control mode.
|
||||
* @throw std::runtime_error if failed to deliver the request to the connected robot.
|
||||
* @note Applicable control modes: IDLE.
|
||||
* @note This function blocks until the request is successfully delivered.
|
||||
*/
|
||||
void Remove(const std::string& name);
|
||||
|
||||
private:
|
||||
class Impl;
|
||||
std::unique_ptr<Impl> pimpl_;
|
||||
};
|
||||
|
||||
} /* namespace flexiv::rdk */
|
||||
|
||||
#endif /* FLEXIV_RDK_WORK_COORD_HPP_ */
|
||||
Reference in New Issue
Block a user