Flexiv RDK APIs  1.4
Public Member Functions | Friends | List of all members
flexiv::rdk::Robot Class Reference

Main interface with the robot, containing several function categories and background services. More...

#include <robot.hpp>

Public Member Functions

 Robot (const std::string &robot_sn)
 [Blocking] Create an instance as the main robot control interface. RDK services will initialize and connection with the robot will be established. More...
 
bool connected () const
 [Non-blocking] Whether the connection with the robot is established. More...
 
const RobotInfo info () const
 [Non-blocking] Access general information of the robot. More...
 
Mode mode () const
 [Non-blocking] Access current control mode of the connected robot. More...
 
const RobotStates states () const
 [Non-blocking] Access the current robot states. More...
 
bool stopped () const
 [Non-blocking] Whether the robot has come to a complete stop. More...
 
bool operational (bool verbose=true) const
 [Non-blocking] Whether the robot is normally operational, which requires the following conditions to be met: enabled, brakes fully released, in auto mode, no fault, and not in reduced state. More...
 
bool busy () const
 [Non-blocking] Whether the robot is currently executing a task. This includes any user commanded operations that requires the robot to execute. For example, plans, primitives, Cartesian and joint motions, etc. More...
 
bool fault () const
 [Non-blocking] Whether the robot is in fault state. More...
 
bool recovery () const
 [Non-blocking] Whether the robot system is in recovery state. More...
 
bool estop_released () const
 [Non-blocking] Whether the emergency stop is released. More...
 
bool enabling_button_pressed () const
 [Non-blocking] Whether the enabling button is pressed. More...
 
const std::vector< std::string > mu_log () const
 [Non-blocking] Access the multilingual log messages of the connected robot. More...
 
void Enable ()
 [Blocking] Enable the robot, if E-stop is released and there's no fault, the robot will release brakes, and becomes operational a few seconds later. More...
 
void Brake (bool engage)
 [Blocking] Force robot brakes to engage or release during normal operation. Restrictions apply, see warning. More...
 
void SwitchMode (Mode mode)
 [Blocking] Switch to a new control mode and wait until mode transition is finished. More...
 
void Stop ()
 [Blocking] Stop the robot and transit robot mode to IDLE. More...
 
bool ClearFault (unsigned int timeout_sec=30)
 [Blocking] Try to clear minor or critical fault of the robot without a power cycle. More...
 
void RunAutoRecovery ()
 [Blocking] Run automatic recovery to bring joints that are outside the allowed position range back into allowed range. More...
 
void SetVelocityScale (unsigned int velocity_scale)
 [Blocking] Set overall velocity scale for robot motions during plan and primitive execution. More...
 
void ExecutePlan (unsigned int index, bool continue_exec=false)
 [Blocking] Execute a plan by specifying its index. More...
 
void ExecutePlan (const std::string &name, bool continue_exec=false)
 [Blocking] Execute a plan by specifying its name. More...
 
void PausePlan (bool pause)
 [Blocking] Pause or resume the execution of the current plan. More...
 
const std::vector< std::string > plan_list () const
 [Blocking] Get a list of all available plans. More...
 
const PlanInfo plan_info () const
 [Blocking] Get detailed information about the currently executing plan. Contains information like plan name, primitive name, node name, node path, node path time period, etc. More...
 
void SetGlobalVariables (const std::string &global_vars)
 [Blocking] Set global variables for the robot by specifying name and value. More...
 
const std::vector< std::string > global_variables () const
 [Blocking] Get available global variables from the robot. More...
 
void SetBreakpointMode (bool is_enabled)
 [Blocking] Enable or disable the breakpoint mode during plan execution. When enabled, the currently executing plan will pause at the pre-defined breakpoints. Use StepBreakpoint() to continue the execution and pause at the next breakpoint. More...
 
void StepBreakpoint ()
 [Blocking] If breakpoint mode is enabled, step to the next breakpoint. The plan execution will continue and pause at the next breakpoint. More...
 
void ExecutePrimitive (const std::string &pt_cmd)
 [Blocking] Execute a primitive by specifying its name and parameters, which can be found in the Flexiv Primitives documentation. More...
 
const std::vector< std::string > primitive_states () const
 [Blocking] Get feedback states of the currently executing primitive. More...
 
void StreamJointTorque (const std::vector< double > &torques, bool enable_gravity_comp=true, bool enable_soft_limits=true)
 [Non-blocking] Continuously stream joint torque command to the robot. More...
 
void StreamJointPosition (const std::vector< double > &positions, const std::vector< double > &velocities, const std::vector< double > &accelerations)
 [Non-blocking] Continuously stream joint position, velocity, and acceleration command to the robot. The commands are tracked by either the joint impedance controller or the joint position controller, depending on the control mode. More...
 
void SendJointPosition (const std::vector< double > &positions, const std::vector< double > &velocities, const std::vector< double > &accelerations, const std::vector< double > &max_vel, const std::vector< double > &max_acc)
 [Non-blocking] Discretely send joint position, velocity, and acceleration command to the robot. The robot's internal motion generator will smoothen the discrete commands, which are tracked by either the joint impedance controller or the joint position controller, depending on the control mode. More...
 
void SetJointImpedance (const std::vector< double > &K_q, const std::vector< double > &Z_q={0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7})
 [Non-blocking] Set impedance properties of the robot's joint motion controller used in the joint impedance control modes. More...
 
void ResetJointImpedance ()
 [Non-blocking] Reset impedance properties of the robot's joint motion controller to nominal values. More...
 
void StreamCartesianMotionForce (const std::array< double, kPoseSize > &pose, const std::array< double, kCartDoF > &wrench={}, const std::array< double, kCartDoF > &acceleration={})
 [Non-blocking] Continuously stream Cartesian motion and/or force command for the robot to track using its unified motion-force controller, which allows doing force control in zero or more Cartesian axes and motion control in the rest axes. More...
 
void SendCartesianMotionForce (const std::array< double, kPoseSize > &pose, const std::array< double, kCartDoF > &wrench={}, double max_linear_vel=0.5, double max_angular_vel=1.0, double max_linear_acc=2.0, double max_angular_acc=5.0)
 [Non-blocking] Discretely send Cartesian motion and/or force command for the robot to track using its unified motion-force controller, which allows doing force control in zero or more Cartesian axes and motion control in the rest axes. The robot's internal motion generator will smoothen the discrete commands. More...
 
void SetCartesianImpedance (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})
 [Non-blocking] Set impedance properties of the robot's Cartesian motion controller used in the Cartesian motion-force control modes. More...
 
void ResetCartesianImpedance ()
 [Non-blocking] Reset impedance properties of the robot's Cartesian motion controller to nominal values. More...
 
void SetMaxContactWrench (const std::array< double, kCartDoF > &max_wrench)
 [Non-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. More...
 
void ResetMaxContactWrench ()
 [Non-blocking] Reset max contact wrench regulation to nominal state, i.e. disabled. More...
 
void SetNullSpacePosture (const std::vector< double > &preferred_positions)
 [Non-blocking] Set preferred joint positions for the null-space posture control module used in the Cartesian motion-force control modes. More...
 
void ResetNullSpacePosture ()
 [Non-blocking] Reset preferred joint positions to the ones automatically recorded when entering the applicable control modes. More...
 
void SetForceControlAxis (const std::array< bool, kCartDoF > &enabled_axis, const std::array< double, kCartDoF/2 > &max_linear_vel={1.0, 1.0, 1.0})
 [Blocking] Set force-controlled Cartesian axis(s) for the Cartesian motion-force control modes. The axis(s) not enabled for force control will be motion controlled. This function can only be called when the robot is in IDLE mode. More...
 
void SetForceControlFrame (const std::string &reference_frame)
 [Blocking] Set force control reference frame for the Cartesian motion-force control modes. This function can only be called when the robot is in IDLE mode. More...
 
void SetPassiveForceControl (bool is_enabled)
 [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. This function can only be called when the robot is in IDLE mode. More...
 
void WriteDigitalOutput (const std::vector< unsigned int > &port_idx, const std::vector< bool > &values)
 [Blocking] Write to single or multiple digital output port(s) on the control box. More...
 
const std::array< bool, kIOPorts > ReadDigitalInput ()
 [Non-blocking] Read all digital input ports on the control box. More...
 

Friends

class Model
 
class Gripper
 
class Device
 
class Tool
 
class FileIO
 

Detailed Description

Main interface with the robot, containing several function categories and background services.

Examples
basics1_display_robot_states.cpp, basics2_clear_fault.cpp, basics3_primitive_execution.cpp, basics4_plan_execution.cpp, basics5_zero_force_torque_sensors.cpp, basics6_gripper_control.cpp, basics7_auto_recovery.cpp, basics8_update_robot_tool.cpp, basics9_logging_behavior.cpp, intermediate1_realtime_joint_position_control.cpp, intermediate2_realtime_joint_impedance_control.cpp, intermediate3_realtime_joint_torque_control.cpp, intermediate4_realtime_joint_floating.cpp, intermediate5_realtime_cartesian_pure_motion_control.cpp, intermediate6_realtime_cartesian_motion_force_control.cpp, and intermediate7_robot_dynamics.cpp.

Definition at line 23 of file robot.hpp.

Constructor & Destructor Documentation

◆ Robot()

flexiv::rdk::Robot::Robot ( const std::string &  robot_sn)

[Blocking] Create an instance as the main robot control interface. RDK services will initialize and connection with the robot will be established.

Parameters
[in]robot_snSerial number of the robot to connect. The accepted formats are: "Rizon 4s-123456" and "Rizon4s-123456".
Exceptions
std::invalid_argumentif the format of [robot_sn] is invalid.
std::runtime_errorif the initialization sequence failed.
std::logic_errorif the connected robot does not have a valid RDK license; or this RDK library version is incompatible with the connected robot; or model of the connected robot is not supported.
Warning
This constructor blocks until the initialization sequence is successfully finished and connection with the robot is established.

Member Function Documentation

◆ Brake()

void flexiv::rdk::Robot::Brake ( bool  engage)

[Blocking] Force robot brakes to engage or release during normal operation. Restrictions apply, see warning.

Parameters
[in]engageTrue: engage brakes, false: release brakes.
Exceptions
std::logic_errorif the connected robot is not a medical one or the robot is moving.
std::runtime_errorif 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 one AND b) the robot is not moving.

◆ busy()

bool flexiv::rdk::Robot::busy ( ) const

[Non-blocking] Whether the robot is currently executing a task. This includes any user commanded operations that requires the robot to execute. For example, plans, primitives, Cartesian and joint motions, etc.

Returns
True: busy, false: idle.
Warning
Some exceptions exist for primitives, see ExecutePrimitive() for more details.
Examples
intermediate1_realtime_joint_position_control.cpp, intermediate2_realtime_joint_impedance_control.cpp, intermediate3_realtime_joint_torque_control.cpp, intermediate4_realtime_joint_floating.cpp, intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ ClearFault()

bool flexiv::rdk::Robot::ClearFault ( unsigned int  timeout_sec = 30)

[Blocking] Try to clear minor or critical fault of the robot without a power cycle.

Parameters
[in]timeout_secMaximum 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.
Returns
True: successfully cleared fault, false: failed to clear fault.
Exceptions
std::runtime_errorif 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.
Examples
basics1_display_robot_states.cpp, intermediate1_realtime_joint_position_control.cpp, intermediate2_realtime_joint_impedance_control.cpp, intermediate3_realtime_joint_torque_control.cpp, intermediate4_realtime_joint_floating.cpp, intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ connected()

bool flexiv::rdk::Robot::connected ( ) const

[Non-blocking] Whether the connection with the robot is established.

Returns
True: connected, false: disconnected.

◆ Enable()

void flexiv::rdk::Robot::Enable ( )

[Blocking] Enable the robot, if E-stop is released and there's no fault, the robot will release brakes, and becomes operational a few seconds later.

Exceptions
std::logic_errorif the robot is not connected.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
This function blocks until the request is successfully delivered.
Examples
basics1_display_robot_states.cpp, intermediate1_realtime_joint_position_control.cpp, intermediate2_realtime_joint_impedance_control.cpp, intermediate3_realtime_joint_torque_control.cpp, intermediate4_realtime_joint_floating.cpp, intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ enabling_button_pressed()

bool flexiv::rdk::Robot::enabling_button_pressed ( ) const

[Non-blocking] Whether the enabling button is pressed.

Returns
True: pressed, false: released.

◆ estop_released()

bool flexiv::rdk::Robot::estop_released ( ) const

[Non-blocking] Whether the emergency stop is released.

Returns
True: released, false: pressed.

◆ ExecutePlan() [1/2]

void flexiv::rdk::Robot::ExecutePlan ( const std::string &  name,
bool  continue_exec = false 
)

[Blocking] Execute a plan by specifying its name.

Parameters
[in]nameName of the plan to execute, can be obtained via plan_list().
[in]continue_execWhether to continue executing the plan when the RDK program is closed or the connection is lost.
Exceptions
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION.
This function blocks until the request is successfully delivered.
busy() can be used to check if a plan task has finished.

◆ ExecutePlan() [2/2]

void flexiv::rdk::Robot::ExecutePlan ( unsigned int  index,
bool  continue_exec = false 
)

[Blocking] Execute a plan by specifying its index.

Parameters
[in]indexIndex of the plan to execute, can be obtained via plan_list().
[in]continue_execWhether to continue executing the plan when the RDK program is closed or the connection is lost.
Exceptions
std::invalid_argumentif [index] is outside the valid range.
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION.
This function blocks until the request is successfully delivered.
busy() can be used to check if a plan task has finished.

◆ ExecutePrimitive()

void flexiv::rdk::Robot::ExecutePrimitive ( const std::string &  pt_cmd)

[Blocking] Execute a primitive by specifying its name and parameters, which can be found in the Flexiv Primitives documentation.

Parameters
[in]pt_cmdPrimitive command with the following string format: "primitive_name(input_param1=xxx, input_param2=xxx, ...)". For an input parameter of type ARRAY_COORD (e.g. waypoints), use colon with space on both sides (" : ") to separate coordinate sets within this parameter.
Exceptions
std::length_errorif size of pt_cmd exceeds the limit (10 Kb).
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PRIMITIVE_EXECUTION.
This function blocks until the request is successfully delivered.
Warning
The primitive input parameters may not use SI units, please refer to the Flexiv Primitives documentation for exact unit definition.
Some primitives may not terminate automatically and require users to manually terminate them based on specific primitive states, for example, most [Move] primitives. In such case, busy() will stay true even if it seems everything is done for that primitive.
Examples
intermediate1_realtime_joint_position_control.cpp, intermediate2_realtime_joint_impedance_control.cpp, intermediate3_realtime_joint_torque_control.cpp, intermediate4_realtime_joint_floating.cpp, intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ fault()

bool flexiv::rdk::Robot::fault ( ) const

◆ global_variables()

const std::vector<std::string> flexiv::rdk::Robot::global_variables ( ) const

[Blocking] Get available global variables from the robot.

Returns
Global variables in the format of a string list.
Exceptions
std::runtime_errorif failed to get a reply from the connected robot.
Note
This function blocks until a reply is received.

◆ info()

const RobotInfo flexiv::rdk::Robot::info ( ) const

◆ mode()

Mode flexiv::rdk::Robot::mode ( ) const

[Non-blocking] Access current control mode of the connected robot.

Returns
flexiv::rdk::Mode enum.

◆ mu_log()

const std::vector<std::string> flexiv::rdk::Robot::mu_log ( ) const

[Non-blocking] Access the multilingual log messages of the connected robot.

Returns
Robot log messages stored since the last successful instantiation of this class. Each element in the string list corresponds to one message with timestamp and log level added. New message is pushed to the back of the vector.
Note
Possible log level tags are: [info], [warning], [error], and [critical].
Warning
Messages before the last successful instantiation of this class are not available.

◆ operational()

bool flexiv::rdk::Robot::operational ( bool  verbose = true) const

[Non-blocking] Whether the robot is normally operational, which requires the following conditions to be met: enabled, brakes fully released, in auto mode, no fault, and not in reduced state.

Parameters
[in]verboseWhether to print warning message indicating why the robot is not operational when this function returns false.
Returns
True: operational, false: not operational.
Warning
The robot won't execute any user command until it becomes normally operational.
Examples
basics1_display_robot_states.cpp, intermediate1_realtime_joint_position_control.cpp, intermediate2_realtime_joint_impedance_control.cpp, intermediate3_realtime_joint_torque_control.cpp, intermediate4_realtime_joint_floating.cpp, intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ PausePlan()

void flexiv::rdk::Robot::PausePlan ( bool  pause)

[Blocking] Pause or resume the execution of the current plan.

Parameters
[in]pauseTrue: pause plan, false: resume plan.
Exceptions
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION.
This function blocks until the request is successfully delivered.

◆ plan_info()

const PlanInfo flexiv::rdk::Robot::plan_info ( ) const

[Blocking] Get detailed information about the currently executing plan. Contains information like plan name, primitive name, node name, node path, node path time period, etc.

Returns
PlanInfo data struct.
Exceptions
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to get a reply from the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION.
This function blocks until a reply is received.

◆ plan_list()

const std::vector<std::string> flexiv::rdk::Robot::plan_list ( ) const

[Blocking] Get a list of all available plans.

Returns
Available plans in the format of a string list.
Exceptions
std::runtime_errorif failed to get a reply from the connected robot.
Note
This function blocks until a reply is received.

◆ primitive_states()

const std::vector<std::string> flexiv::rdk::Robot::primitive_states ( ) const

[Blocking] Get feedback states of the currently executing primitive.

Returns
Primitive states in the format of a string list.
Exceptions
std::runtime_errorif failed to get a reply from the connected robot or the result is invalid.
Note
This function blocks until a reply is received.

◆ ReadDigitalInput()

const std::array<bool, kIOPorts> flexiv::rdk::Robot::ReadDigitalInput ( )

[Non-blocking] Read all digital input ports on the control box.

Returns
Digital input readings array whose index corresponds to the digital input port index. True: port high, false: port low.

◆ recovery()

bool flexiv::rdk::Robot::recovery ( ) const

[Non-blocking] Whether the robot system is in recovery state.

Returns
True: in recovery state, false: not in recovery state.
Note
Use RunAutoRecovery() to execute automatic recovery operation.
Recovery state
The robot system 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). Refer to user manual for more details about system recovery state.

◆ ResetCartesianImpedance()

void flexiv::rdk::Robot::ResetCartesianImpedance ( )

[Non-blocking] Reset impedance properties of the robot's Cartesian motion controller to nominal values.

Note
Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE.
See also
SetCartesianImpedance().
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp.

◆ ResetJointImpedance()

void flexiv::rdk::Robot::ResetJointImpedance ( )

[Non-blocking] Reset impedance properties of the robot's joint motion controller to nominal values.

Note
Applicable control mode(s): RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
See also
SetJointImpedance().
Examples
intermediate2_realtime_joint_impedance_control.cpp.

◆ ResetMaxContactWrench()

void flexiv::rdk::Robot::ResetMaxContactWrench ( )

[Non-blocking] Reset max contact wrench regulation to nominal state, i.e. disabled.

Note
Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE.
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ ResetNullSpacePosture()

void flexiv::rdk::Robot::ResetNullSpacePosture ( )

[Non-blocking] Reset preferred joint positions to the ones automatically recorded when entering the applicable control modes.

Note
Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE.
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp.

◆ RunAutoRecovery()

void flexiv::rdk::Robot::RunAutoRecovery ( )

[Blocking] Run automatic recovery to bring joints that are outside the allowed position range back into allowed range.

Exceptions
std::runtime_errorif failed to enter automatic recovery mode.
Note
Refer to user manual for more details.
This function blocks until the automatic recovery process is finished.
See also
recovery().

◆ SendCartesianMotionForce()

void flexiv::rdk::Robot::SendCartesianMotionForce ( const std::array< double, kPoseSize > &  pose,
const std::array< double, kCartDoF > &  wrench = {},
double  max_linear_vel = 0.5,
double  max_angular_vel = 1.0,
double  max_linear_acc = 2.0,
double  max_angular_acc = 5.0 
)

[Non-blocking] Discretely send Cartesian motion and/or force command for the robot to track using its unified motion-force controller, which allows doing force control in zero or more Cartesian axes and motion control in the rest axes. The robot's internal motion generator will smoothen the discrete commands.

Parameters
[in]poseTarget TCP pose in world frame: \( {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \). Consists of \( \mathbb{R}^{3 \times 1} \) position and \( \mathbb{R}^{4 \times 1} \) quaternion: \( [x, y, z, q_w, q_x, q_y, q_z]^T \). Unit: \( [m]~[] \).
[in]wrenchTarget TCP wrench (force and moment) in the force control reference frame (configured by SetForceControlFrame()): \( ^{0}F_d \in \mathbb{R}^{6 \times 1} \). The robot will track the target wrench using an explicit force controller. Consists of \( \mathbb{R}^{3 \times 1} \) force and \( \mathbb{R}^{3 \times 1} \) moment: \( [f_x, f_y, f_z, m_x, m_y, m_z]^T \). Unit: \( [N]~[Nm] \).
[in]max_linear_velMaximum Cartesian linear velocity when moving to the target pose. A safe value is provided as default. Unit: \( [m/s] \).
[in]max_angular_velMaximum Cartesian angular velocity when moving to the target pose. A safe value is provided as default. Unit: \( [rad/s] \).
[in]max_linear_accMaximum Cartesian linear acceleration when moving to the target pose. A safe value is provided as default. Unit: \( [m/s^2] \).
[in]max_angular_accMaximum Cartesian angular acceleration when moving to the target pose. A safe value is provided as default. Unit: \( [rad/s^2] \).
Exceptions
std::invalid_argumentif any of the last 4 input parameters is negative.
std::logic_errorif robot is not in the correct control mode.
Note
Applicable control mode(s): NRT_CARTESIAN_MOTION_FORCE.
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 out of the box.
How to achieve pure force control?
Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure force control, active or passive.
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 also
SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(), SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
Examples
intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ SendJointPosition()

void flexiv::rdk::Robot::SendJointPosition ( const std::vector< double > &  positions,
const std::vector< double > &  velocities,
const std::vector< double > &  accelerations,
const std::vector< double > &  max_vel,
const std::vector< double > &  max_acc 
)

[Non-blocking] Discretely send joint position, velocity, and acceleration command to the robot. The robot's internal motion generator will smoothen the discrete commands, which are tracked by either the joint impedance controller or the joint position controller, depending on the control mode.

Parameters
[in]positionsTarget joint positions: \( q_d \in \mathbb{R}^{n \times 1} \). Unit: \( [rad] \).
[in]velocitiesTarget joint velocities: \( \dot{q}_d \in \mathbb{R}^{n \times 1} \). Each joint will maintain this amount of velocity when it reaches the target position. Unit: \( [rad/s] \).
[in]accelerationsTarget joint accelerations: \( \ddot{q}_d \in \mathbb{R}^{n \times 1} \). Each joint will maintain this amount of acceleration when it reaches the target position. Unit: \( [rad/s^2] \).
[in]max_velMaximum joint velocities for the planned trajectory: \( \dot{q}_{max} \in \mathbb{R}^{n \times 1} \). Unit: \( [rad/s] \).
[in]max_accMaximum joint accelerations for the planned trajectory: \( \ddot{q}_{max} \in \mathbb{R}^{n \times 1} \). Unit: \( [rad/s^2] \).
Exceptions
std::invalid_argumentif size of any input vector does not match robot DoF.
std::logic_errorif robot is not in the correct control mode.
Note
Applicable control mode(s): 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 also
SetJointImpedance().

◆ SetBreakpointMode()

void flexiv::rdk::Robot::SetBreakpointMode ( bool  is_enabled)

[Blocking] Enable or disable the breakpoint mode during plan execution. When enabled, the currently executing plan will pause at the pre-defined breakpoints. Use StepBreakpoint() to continue the execution and pause at the next breakpoint.

Parameters
[in]is_enabledTrue: enable, false: disable. By default, breakpoint mode is disabled.
Exceptions
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION.
This function blocks until the request is successfully delivered.

◆ SetCartesianImpedance()

void flexiv::rdk::Robot::SetCartesianImpedance ( 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} 
)

[Non-blocking] Set impedance properties of the robot's Cartesian motion controller used in the Cartesian motion-force control modes.

Parameters
[in]K_xCartesian motion stiffness: \( K_x \in \mathbb{R}^{6 \times 1} \). Setting motion stiffness of a motion-controlled Cartesian axis to 0 will make this axis free-floating. Consists of \( \mathbb{R}^{3 \times 1} \) linear stiffness and \( \mathbb{R}^{3 \times 1} \) angular stiffness: \( [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T \). Valid range: [0, RobotInfo::K_x_nom]. Unit: \( [N/m]~[Nm/rad] \).
[in]Z_xCartesian motion damping ratio: \( Z_x \in \mathbb{R}^{6 \times 1} \). Consists of \( \mathbb{R}^{3 \times 1} \) linear damping ratio and \( \mathbb{R}^{3 \times 1} \) angular damping ratio: \( [\zeta_x, \zeta_y, \zeta_z, \zeta_{Rx}, \zeta_{Ry}, \zeta_{Rz}]^T \). Valid range: [0.3, 0.8]. The nominal (safe) value is provided as default.
Exceptions
std::invalid_argumentif [K_x] or [Z_x] contains any value outside the valid range.
std::logic_errorif robot is not in the correct control mode.
Note
Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE.
Warning
The robot will automatically reset to its nominal impedance properties upon re-entering the applicable control modes.
Changing damping ratio [Z_x] to a non-nominal value may lead to performance and stability issues, please use with caution.
See also
ResetCartesianImpedance().
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp.

◆ SetForceControlAxis()

void flexiv::rdk::Robot::SetForceControlAxis ( const std::array< bool, kCartDoF > &  enabled_axis,
const std::array< double, kCartDoF/2 > &  max_linear_vel = {1.0, 1.0, 1.0} 
)

[Blocking] Set force-controlled Cartesian axis(s) for the Cartesian motion-force control modes. The axis(s) not enabled for force control will be motion controlled. This function can only be called when the robot is in IDLE mode.

Parameters
[in]enabled_axisFlags to enable/disable force control for certain Cartesian axis(s) in the force control reference frame (configured by SetForceControlFrame()). The corresponding axis order is \( [X, Y, Z, Rx, Ry, Rz] \). By default, force control is disabled for all Cartesian axes.
[in]max_linear_velFor Cartesian linear axis(s) enabled with force control, limit the moving velocity to these values as a protection mechanism in case of contact loss. The corresponding axis order is \( [X, Y, Z] \). Valid range: [0.005, 2.0]. Unit: \( [m/s] \).
Exceptions
std::invalid_argumentif [max_linear_vel] contains any value outside the valid range.
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): IDLE.
This function blocks until the request is successfully delivered.
Warning
The maximum linear velocity protection for force control axes is only effective under active force control (passive force control disabled), see SetPassiveForceControl().
Upon disconnection, force control axes will be reset to all disabled and maximum linear velocity in force control axes will be reset to 1.0 m/s.
Examples
intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ SetForceControlFrame()

void flexiv::rdk::Robot::SetForceControlFrame ( const std::string &  reference_frame)

[Blocking] Set force control reference frame for the Cartesian motion-force control modes. This function can only be called when the robot is in IDLE mode.

Parameters
[in]reference_frameThe reference frame to use for force control. Options are: "TCP" and "WORLD". The target wrench and force control axis should also be expressed in the selected reference frame. By default, world frame is used for force control.
Exceptions
std::invalid_argumentif [reference_frame] is invalid.
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): IDLE.
This function blocks until the request is successfully delivered.
Warning
Upon disconnection, this setting will be reset to world frame.
Force control reference 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 either world frame or the robot's current TCP frame. 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 for force control, the force-controlled axis(s) and motion-controlled axis(s) are guaranteed to be orthogonal. However, when using current TCP frame for force control, the force-controlled axis(s) and motion-controlled axis(s) 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.
Examples
intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ SetGlobalVariables()

void flexiv::rdk::Robot::SetGlobalVariables ( const std::string &  global_vars)

[Blocking] Set global variables for the robot by specifying name and value.

Parameters
[in]global_varsCommand to set global variables using the format: global_var1=value(s), global_var2=value(s), ...
Exceptions
std::length_errorif size of global_vars exceeds the limit (10 Kb).
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION.
This function blocks until the request is successfully delivered.
Warning
The specified global variable(s) must have already been created in the robot using Flexiv Elements, otherwise setting a nonexistent global variable will have no effect. To check if a global variable is successfully set, use global_variables().

◆ SetJointImpedance()

void flexiv::rdk::Robot::SetJointImpedance ( const std::vector< double > &  K_q,
const std::vector< double > &  Z_q = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7} 
)

[Non-blocking] Set impedance properties of the robot's joint motion controller used in the joint impedance control modes.

Parameters
[in]K_qJoint motion stiffness: \( K_q \in \mathbb{R}^{n \times 1} \). Setting motion stiffness of a joint axis to 0 will make this axis free-floating. Valid range: [0, RobotInfo::K_q_nom]. Unit: \( [Nm/rad] \).
[in]Z_qJoint motion damping ratio: \( Z_q \in \mathbb{R}^{n \times 1} \). Valid range: [0.3, 0.8]. The nominal (safe) value is provided as default.
Exceptions
std::invalid_argumentif [K_q] or [Z_q] contains any value outside the valid range or size of any input vector does not match robot DoF.
std::logic_errorif robot is not in the correct control mode.
Note
Applicable control mode(s): RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
Warning
The robot will automatically reset to its nominal impedance properties upon re-entering the applicable control modes.
Changing damping ratio [Z_q] to a non-nominal value may lead to performance and stability issues, please use with caution.
See also
ResetJointImpedance().
Examples
intermediate2_realtime_joint_impedance_control.cpp.

◆ SetMaxContactWrench()

void flexiv::rdk::Robot::SetMaxContactWrench ( const std::array< double, kCartDoF > &  max_wrench)

[Non-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.

Parameters
[in]max_wrenchMaximum contact wrench (force and moment): \( F_max \in \mathbb{R}^{6 \times 1} \). Consists of \( \mathbb{R}^{3 \times 1} \) maximum force and \( \mathbb{R}^{3 \times 1} \) maximum moment: \( [f_x, f_y, f_z, m_x, m_y, m_z]^T \). Unit: \( [N]~[Nm] \).
Exceptions
std::invalid_argumentif [max_wrench] contains any negative value.
std::logic_errorif robot is not in the correct control mode.
Note
The maximum contact wrench regulation only applies to the motion control part.
Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE.
Warning
The maximum contact wrench regulation will automatically reset to disabled upon re-entering the applicable control modes.
The maximum contact wrench regulation cannot be enabled if any of the rotational Cartesian axes is enabled for moment control.
See also
ResetMaxContactWrench().
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ SetNullSpacePosture()

void flexiv::rdk::Robot::SetNullSpacePosture ( const std::vector< double > &  preferred_positions)

[Non-blocking] Set preferred joint positions for the null-space posture control module used in the Cartesian motion-force control modes.

Parameters
[in]preferred_positionsPreferred joint positions for the null-space posture control: \( q_{ns} \in \mathbb{R}^{n \times 1} \). Valid range: [RobotInfo::q_min, RobotInfo::q_max]. Unit: \( [rad] \).
Exceptions
std::invalid_argumentif [preferred_positions] contains any value outside the valid range or size of any input vector does not match robot DoF.
std::logic_errorif robot is not in the correct control mode.
Note
Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE.
Warning
Upon entering the applicable control modes, the robot will automatically set its current joint positions as the preferred joint positions.
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 preferred joint positions for a desired robot posture is 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.
See also
ResetNullSpacePosture().
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp.

◆ SetPassiveForceControl()

void flexiv::rdk::Robot::SetPassiveForceControl ( bool  is_enabled)

[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. This function can only be called when the robot is in IDLE mode.

Parameters
[in]is_enabledTrue: enable, false: disable. By default, passive force control is disabled and active force control is used.
Exceptions
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): IDLE.
This function blocks until the request is successfully delivered.
Warning
Upon disconnection, this setting will be reset to disabled.
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.

◆ SetVelocityScale()

void flexiv::rdk::Robot::SetVelocityScale ( unsigned int  velocity_scale)

[Blocking] Set overall velocity scale for robot motions during plan and primitive execution.

Parameters
[in]velocity_scalePercentage 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.
Exceptions
std::invalid_argumentif [velocity_scale] is outside the valid range.
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION, NRT_PRIMITIVE_EXECUTION.
This function blocks until the request is successfully delivered.

◆ states()

const RobotStates flexiv::rdk::Robot::states ( ) const

◆ StepBreakpoint()

void flexiv::rdk::Robot::StepBreakpoint ( )

[Blocking] If breakpoint mode is enabled, step to the next breakpoint. The plan execution will continue and pause at the next breakpoint.

Exceptions
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
Applicable control mode(s): NRT_PLAN_EXECUTION.
This function blocks until the request is successfully delivered.
Use PlanInfo::waiting_for_step to check if the plan is currently waiting for user signal to step the breakpoint.

◆ Stop()

void flexiv::rdk::Robot::Stop ( )

[Blocking] Stop the robot and transit robot mode to IDLE.

Exceptions
std::runtime_errorif failed to stop the robot.
Note
This function blocks until the robot comes to a complete stop.
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ stopped()

bool flexiv::rdk::Robot::stopped ( ) const

[Non-blocking] Whether the robot has come to a complete stop.

Returns
True: stopped, false: still moving.

◆ StreamCartesianMotionForce()

void flexiv::rdk::Robot::StreamCartesianMotionForce ( const std::array< double, kPoseSize > &  pose,
const std::array< double, kCartDoF > &  wrench = {},
const std::array< double, kCartDoF > &  acceleration = {} 
)

[Non-blocking] Continuously stream Cartesian motion and/or force command for the robot to track using its unified motion-force controller, which allows doing force control in zero or more Cartesian axes and motion control in the rest axes.

Parameters
[in]poseTarget TCP pose in world frame: \( {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \). Consists of \( \mathbb{R}^{3 \times 1} \) position and \( \mathbb{R}^{4 \times 1} \) quaternion: \( [x, y, z, q_w, q_x, q_y, q_z]^T \). Unit: \( [m]~[] \).
[in]wrenchTarget TCP wrench (force and moment) in the force control reference frame (configured by SetForceControlFrame()): \( ^{0}F_d \in \mathbb{R}^{6 \times 1} \). The robot will track the target wrench using an explicit force controller. Consists of \( \mathbb{R}^{3 \times 1} \) force and \( \mathbb{R}^{3 \times 1} \) moment: \( [f_x, f_y, f_z, m_x, m_y, m_z]^T \). Unit: \( [N]~[Nm] \).
[in]accelerationTarget TCP acceleration (linear and angular) in world frame: \( ^{0}\ddot{x}_d \in \mathbb{R}^{6 \times 1} \). 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 \( \mathbb{R}^{3 \times 1} \) linear and \( \mathbb{R}^{3 \times 1} \) angular acceleration. Unit: \( [m/s^2]~[rad/s^2] \).
Exceptions
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif number of timeliness failures has reached limit.
Note
Applicable control mode(s): RT_CARTESIAN_MOTION_FORCE.
Real-time (RT).
Warning
Always stream smooth and continuous motion commands to avoid sudden movements. The force commands don't need to be continuous.
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 out of the box.
How to achieve pure force control?
Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure force control, active or passive.
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 also
SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(), SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
Examples
intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ StreamJointPosition()

void flexiv::rdk::Robot::StreamJointPosition ( const std::vector< double > &  positions,
const std::vector< double > &  velocities,
const std::vector< double > &  accelerations 
)

[Non-blocking] Continuously stream joint position, velocity, and acceleration command to the robot. The commands are tracked by either the joint impedance controller or the joint position controller, depending on the control mode.

Parameters
[in]positionsTarget joint positions: \( q_d \in \mathbb{R}^{n \times 1} \). Unit: \( [rad] \).
[in]velocitiesTarget joint velocities: \( \dot{q}_d \in \mathbb{R}^{n \times 1} \). Unit: \( [rad/s] \).
[in]accelerationsTarget joint accelerations: \( \ddot{q}_d \in \mathbb{R}^{n \times 1} \). Unit: \( [rad/s^2] \).
Exceptions
std::invalid_argumentif size of any input vector does not match robot DoF.
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif number of timeliness failures has reached limit.
Note
Applicable control mode(s): RT_JOINT_IMPEDANCE, RT_JOINT_POSITION.
Real-time (RT).
Warning
Always stream smooth and continuous commands to avoid sudden movements.
See also
SetJointImpedance().
Examples
intermediate1_realtime_joint_position_control.cpp, and intermediate2_realtime_joint_impedance_control.cpp.

◆ StreamJointTorque()

void flexiv::rdk::Robot::StreamJointTorque ( const std::vector< double > &  torques,
bool  enable_gravity_comp = true,
bool  enable_soft_limits = true 
)

[Non-blocking] Continuously stream joint torque command to the robot.

Parameters
[in]torquesTarget joint torques: \( {\tau_J}_d \in \mathbb{R}^{n \times 1} \). Unit: \( [Nm] \).
[in]enable_gravity_compEnable/disable robot gravity compensation.
[in]enable_soft_limitsEnable/disable soft limits to keep the joints from moving outside allowed position range, which will trigger a safety fault that requires recovery operation.
Exceptions
std::invalid_argumentif size of any input vector does not match robot DoF.
std::logic_errorif robot is not in the correct control mode.
std::runtime_errorif number of timeliness failures has reached limit.
Note
Applicable control mode(s): RT_JOINT_TORQUE.
Real-time (RT).
Warning
Always stream smooth and continuous commands to avoid sudden movements.
Examples
intermediate3_realtime_joint_torque_control.cpp, and intermediate4_realtime_joint_floating.cpp.

◆ SwitchMode()

void flexiv::rdk::Robot::SwitchMode ( Mode  mode)

[Blocking] Switch to a new control mode and wait until mode transition is finished.

Parameters
[in]modeflexiv::rdk::Mode enum.
Exceptions
std::invalid_argumentif the requested mode is invalid or unlicensed.
std::logic_errorif robot is in an unknown control mode or is not operational.
std::runtime_errorif 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 then make the mode transition.
Examples
intermediate1_realtime_joint_position_control.cpp, intermediate2_realtime_joint_impedance_control.cpp, intermediate3_realtime_joint_torque_control.cpp, intermediate4_realtime_joint_floating.cpp, intermediate5_realtime_cartesian_pure_motion_control.cpp, and intermediate6_realtime_cartesian_motion_force_control.cpp.

◆ WriteDigitalOutput()

void flexiv::rdk::Robot::WriteDigitalOutput ( const std::vector< unsigned int > &  port_idx,
const std::vector< bool > &  values 
)

[Blocking] Write to single or multiple digital output port(s) on the control box.

Parameters
[in]port_idxIndex of port(s) to write, can be a single port or multiple ports. E.g. {0, 5, 7, 15} or {1, 3, 10} or {8}. Valid range of the index number is [0–15].
[in]valuesCorresponding values to write to the specified ports. True: set port high, false: set port low. Vector size must match the size of port_idx.
Exceptions
std::invalid_argumentif [port_idx] contains any index number outside the valid range.
std::length_errorif the two input vectors have different sizes.
std::runtime_errorif failed to deliver the request to the connected robot.
Note
This function blocks until the request is successfully delivered.

The documentation for this class was generated from the following file: