Skip to content

C++ IndyDCP3 Client

To use the IndyDCP3 client with C++, follow the steps below. This guide assumes that you have the appropriate development environment set up with C++17 or later.

Installation Requirements

  • Requires CMake for project configuration and building.
  • A C++ compiler that supports C++17 or later.
  • IndyDCP3 library files and headers.

Installation Instructions

On Linux

1. Install gRPC

This gRPC installation guide for Linux is summarized from gRPC website. For more information, please visit grpc.io.

Choose a directory to hold locally installed packages. This page assumes that the environment variable MY_INSTALL_DIR holds this directory path. For example:

export MY_INSTALL_DIR=$HOME/.local
Ensure that the directory exists:
mkdir -p $MY_INSTALL_DIR

Add the local bin folder to your path variable, for example:

export PATH="$MY_INSTALL_DIR/bin:$PATH"

Install CMake You need version 3.13 or later of cmake. Install it by following these instructions:

sudo apt install -y cmake
Check the version of cmake:
cmake --version

On Linux, the system-wide CMake version might be outdated. To install a more recent version in your local installation directory, use:

wget -q -O cmake-linux.sh https://github.com/Kitware/CMake/releases/download/v3.19.6/cmake-3.19.6-Linux-x86_64.sh
sh cmake-linux.sh -- --skip-license --prefix=$MY_INSTALL_DIR
rm cmake-linux.sh

Install other required tools Install the basic tools required to build gRPC:

sudo apt install -y build-essential autoconf libtool pkg-config

Clone the grpc repo and its submodules:

git clone --recurse-submodules -b v1.59.0 --depth 1 --shallow-submodules https://github.com/grpc/grpc

Build and Install gRPC and Protocol Buffers

gRPC applications often use Protocol Buffers for service definitions and data serialization, as in the example code that follows. To build and locally install gRPC and Protocol Buffers, run:

cd grpc
mkdir -p cmake/build
pushd cmake/build
cmake -DgRPC_INSTALL=ON \
      -DgRPC_BUILD_TESTS=OFF \
      -DCMAKE_INSTALL_PREFIX=$MY_INSTALL_DIR \
      ../..
make -j 4
make install
popd

Important

Note

It is strongly recommended to install gRPC locally with a properly set CMAKE_INSTALL_PREFIX, as there is no simple way to uninstall gRPC after a global installation.

2. Download and Build example

Clone the Example Repository

git clone https://github.com/neuromeka-robotics/neuromeka-package.git

Navigate to the C++ Folder

cd neuromeka-package/cpp/

Create and Enter the Build Directory

mkdir build && cd build

Run CMake Configuration and Build the Example

cmake -DBUILD_PROTO=OFF ..
cmake --build .

C++ (On Windows)

It is recommended to use Visual Studio Code for this setup.

1. Setup your environment

Install Visual Studio Code - Install following extensions: C/C++, C/C++ Extension Pack and CMake Tools.

Install vs_BuildTools - In BuildTools option, please choose Desktop Development with C++ and Visual Studio extension development.

2. Install GRPC

When you are all set, please open Developer Command Prompt and run the following commands:

git clone -b v1.59.0 https://github.com/grpc/grpc
cd grpc
git submodule update --init

Create build folder

cd cmake
mkdir build
cd build

Configure and build using Cmake

cmake -G "Visual Studio 17 2022" -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_STANDARD=17 -DBUILD_SHARED_LIBS=OFF -DABSL_PROPAGATE_CXX_STD=ON -DgRPC_INSTALL=ON -DgRPC_BUILD_TESTS=OFF -DCMAKE_INSTALL_PREFIX=..\..\..\install ../..
You can set you install path with "-DCMAKE_INSTALL_PREFIX=\<your path>

Build and install grpc

cmake --build . --config Release --target install

3. Download and Build example

Clone the Example Repository

git clone https://github.com/neuromeka-robotics/neuromeka-package.git

Open Visual Studio Code and open neuromeka-package/cpp/ folder Config your build with settings.json in .vscode folder.

Then, modify the CMakeList.txt. Change following lines to point to your grpc installation directory.

set(absl_DIR "E:/Example/install/lib/cmake/absl")
set(utf8_range_DIR "E:/Example/install/lib/cmake/utf8_range")
set(protobuf_DIR "E:/Example/install/cmake")
set(gRPC_DIR "E:/Example/install/lib/cmake/grpc")

Open Visual Studio Code Press Ctrl+Shift+P or go to View -> Command Palette to open the Command Palette, and choose the following commands: - CMake: Scan for kits: To scan for compilers on your computer. - CMake: Select a kit: To select a specific compiler (e.g., Visual Studio Build Tools 2022 Release - amd64).

When you are done, press the Build button at the bottom left.


Using IndyDCP3

To create a client object, include the IndyDCP3 class header from the IndyDCP3 library and initialize it with the robot's IP address.

#include "indydcp3.h"

int main() {
    IndyDCP3 indy("192.168.xxx.xxx");

    // Use the indy object to interact with the robot...
    return 0;
}
  • 192.168.xxx.xxx: IP address of the robot controller.

Methods and Functions

Below is a list of protocol functions that can be called using the IndyDCP3 object. Please refer to the usage methods and input/output examples for each function.

Real-Time Data Acquisition Functions

You can retrieve the robot's current motion status, control data, state, servo motor state, error information, and program status.

Method Description
get_robot_data() Robot status data
get_control_state() Control state
get_motion_data() Motion data
get_servo_data() Servo data
get_violation_data() Error data
get_program_data() Robot program data

Example for get_robot_data()

Nrmk::IndyFramework::ControlData2 control_data;
bool is_success = indy.get_robot_data(control_data);
if (is_success) {
    // Process control_data
}
{
  'running_mins': 6,
  'running_secs': 11,
  'op_state': 5,
  'sim_mode': true,  
  'ref_frame': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
  'tool_frame': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
  'running_hours': 0
}
  • running_hours: Robot operating hours
  • running_mins: Robot operating minutes
  • running_secs: Robot operating seconds
  • op_state: Robot state as defined in the OpState class
    • OpState: SYSTEM_OFF(0), SYSTEM_ON(1), VIOLATE(2), RECOVER_HARD(3), RECOVER_SOFT(4), IDLE(5), MOVING(6), TEACHING(7), COLLISION(8), STOP_AND_OFF(9), COMPLIANCE(10), BRAKE_CONTROL(11), SYSTEM_RESET(12), SYSTEM_SWITCH(13), VIOLATE_HARD(15), MANUAL_RECOVER(16), TELE_OP(17)

Example for get_control_state()

Nrmk::IndyFramework::ControlData control_data;
bool is_success = indy.get_control_state(control_data);
if (is_success) {
    // Access joint positions
    for (int i = 0; i < control_data.q_size(); i++) {
        float joint_position = control_data.q(i);
    }
}
{
  'q': [...],
  'qdot': [...],
  'qddot': [...],
  'qdes': [...],
  'qdotdes': [...],
  'qddotdes': [...],
  'p': [...],
  'pdot': [...],
  'pddot': [...],
  'pdes': [...],
  'pdotdes': [...],
  'pddotdes': [...],
  'tau': [...],
  'tau_act': [...],
  'tau_ext': [...],
  'tau_jts': [...],
  'tau_jts_raw1': [...],
  'tau_jts_raw2': [...],
}
  • q, qdot, qddot: Current positions (deg), velocities (deg/s), and accelerations (deg/s²) of each joint
  • qdes, qdotdes, qddotdes: Target positions (deg), velocities (deg/s), and accelerations (deg/s²) of each joint
  • p, pdot, pddot: Current workspace pose positions (mm), velocities (mm/s), and accelerations (mm/s²)
  • pdes, pdotdes, pddotdes: Target workspace pose positions (mm), velocities (mm/s), and accelerations (mm/s²)
  • tau, tau_act, tau_ext: Current input torques, actual torques, and external torques (Nm) for each joint
  • tau_jts, tau_jts_raw1, tau_jts_raw2: joint torque sensor values.

Example for get_servo_data()

Nrmk::IndyFramework::ServoData servo_data;
bool is_success = indy.get_servo_data(servo_data);
if (is_success) {
    std::cout << "servo data:" << std::endl;
    for (int i = 0; i < servo_data.temperatures_size(); i++){
        std::cout << "temperature" << i << ": " << servo_data.temperatures(i) << std::endl;
    }   
}
else{
    std::cout << "Failed to get_servo_data" << std::endl;
}
{
  'status_codes': ['0x1237', '0x1237', '0x1237', '0x1237', '0x1237', '0x1237'],
  'temperatures': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
  'voltages': [48.0, 48.0, 48.0, 48.0, 48.0, 48.0],
  'currents': [3.4812942e-18,
              26.195215,
              26.963966,
              0.12320003,
              0.55777645,
              4.536144e-05],
  'servo_actives': [true, true, true, true, true, true],
  'brake_actives': [false, false, false, false, false, false]
 }
  • status_codes: Status codes (CiA402) for each joint servo
  • temperatures: Temperatures for each joint servo
  • voltages: Voltages for each joint servo
  • currents: Currents for each joint servo
  • servo_actives: Activation state of each joint servo (true/false)
  • brake_actives: Activation state of the brakes for each joint servo (true/false)

Example for get_violation_data()

Nrmk::IndyFramework::ViolationData violation_data;
bool is_success = indy.get_violation_data(violation_data);
if (is_success) {
    int j_index = violation_data.j_index();
}
{
  'violation_code': '262144',
  'j_index': 4,
  'i_args': [2],
  'f_args': [0.0012204262],
  'violation_str': 'EMG Button Activated'
}
  • j_index: Index of the joint where the violation occurred
  • i_args: Integer information about the violation
  • f_args: Float information about the violation
  • violation_str: Error message
  • violation_code: Error code
violation_code (Binary) violation_str
0b00 None
0b01 << 0 Joint [axisIdx/DOF] Position Error Over Limit (Current: X deg, Desired: Y, Limit: Z)
0b01 << 1 Joint [axisIdx/DOF] Position Close To Limit (Current: X deg, Allowed: A ~ B)
0b01 << 2 Joint [axisIdx/DOF] Velocity Close To Limit (Current: X deg/s, Allowed: A ~ B)
0b01 << 6 TCP Singular Closed
0b01 << 7 Joint [axisIdx/DOF] Position Exceeded Limit (Current: X, Allowed: A ~ B)
0b01 << 8 Joint [axisIdx/DOF] Velocity Exceeded Limit (Current: X, Allowed: A ~ B)
0b01 << 9 Joint [axisIdx/DOF] Torque Exceeded Limit (Current: X, Allowed: A ~ B)
0b01 << 10 TCP Speed Limit: X m/s
0b01 << 11 TCP Force Limit: X N
0b01 << 12 Power Over Limit (Current: X, Allowed: Y)
0b01 << 13 Standstill Failed
0b01 << 14 Loss of Conty Communication
0b01 << 15 Loss of IndyKey Communication
0b01 << 16 Safety Guard Input 1
0b01 << 17 Safety Guard Input 2
0b01 << 18 EMG Button Activated
0b01 << 19 Robot Specs Reading Failed
0b01 << 20 Connection Lost - Master: masterState, Detected Joints Num: numSlavesDetected / DOF+2
0b01 << 21 Motor Status Error: joint [axisIdx/DOF], Code = errorCode
0b01 << 22 Motor Over Current: joint [axisIdx/DOF], Code errorCode
0b01 << 23 Motor Over Heated: joint [axisIdx/DOF], Temperature X celsius
0b01 << 24 Motor Error Bit: joint [axisIdx/DOF], Code = errorCode
0b01 << 25 Motor Low Battery: joint [axisIdx/DOF], Code = errorCode
0b01 << 26 Motor Firmware Error
0b01 << 27 Task Time Over Limit (Current: X us, Allowed: Y us)
0b01 << 28 Soft Reset Failed
0b01 << 29 Auto Servo-Off Activated
0b01 << 30 TeleOp Starting Point Too Far

Example for get_program_data()

Nrmk::IndyFramework::ProgramData program_data;
bool is_success = indy.get_program_data(program_data);
if (is_success) {
    std::string program_name = program_data.program_name();
}
{
  'program_name': 'ProgramScripts/',
  'program_state': 0,
  'cmd_id': 0,
  'sub_cmd_id': 0,
  'running_hours': 0,
  'running_mins': 0,
  'running_secs': 0,
  'program_alarm': '',
  'program_annotation': ''
}   
  • program_name: Name of the currently running program file
  • program_state: Program execution state
    • ProgramState: IDLE(0), RUNNING(1), PAUSING(2), STOPPING(3)
  • running_hours: Program operating time (hours)
  • running_mins: Program operating time (minutes)
  • running_secs: Program operating time (seconds)
  • program_alarm: Alarm of Conty Flow control
  • program_annotation: Annotation of Conty Flow control

I/O Device functions are used to interact with various robot hardware components like digital and analog I/O, end-tool I/O, and sensors.

Method Description
get_di(DigitalList &data) Retrieves IO board DI data
get_do(DigitalList &data) Retrieves IO board DO data
set_do(const DigitalList &data) Sets IO board DO data
get_ai(AnalogList &data) Retrieves IO board AI data
get_ao(AnalogList &data) Retrieves IO board AO data
set_ao(const AnalogList &data) Sets IO board AO data
get_endtool_di(EndtoolSignalList &data) Retrieves end-tool DI data
get_endtool_do(EndtoolSignalList &data) Retrieves end-tool DO data
set_endtool_do(const EndtoolSignalList &data) Sets end-tool DO data
get_endtool_ai(AnalogList &data) Retrieves end-tool AI data
get_endtool_ao(AnalogList &data) Retrieves end-tool AO data
set_endtool_ao(const AnalogList &data) Sets end-tool AO data
get_device_info(DeviceInfo &data) Retrieves device information
get_ft_sensor_data(FTSensorData &data) Retrieves end-tool F/T sensor data
get_gripper_data(GripperData& gripper_data) Gripper status & telemetry
set_gripper_command(GripperCommand& gripper_command) Send gripper actuation profile
set_endtool_rs485_rx(EndtoolRS485Rx& request) Write endtool RS485 RX buffer words
get_endtool_rs485_rx(EndtoolRS485Rx& rx_data) Read endtool RS485 RX buffer
get_endtool_rs485_tx(EndtoolRS485Tx& tx_data) Read endtool RS485 TX buffer
set_sander_command(SanderType& sander_type, string& ip, const float speed, const bool state) Configure/start/stop sander
get_sander_command(SanderCommand& sander_command) Last sander command state
add_photoneo_calib_point(string& vision_name, double px, double py, double pz) Photoneo calibration point
get_photoneo_detection(VisionServer& vision_server, string& object, VisionFrameType frame_type, VisionResult& result) Photoneo detection pose(s)
get_photoneo_retrieval(VisionServer& vision_server, const string& object, VisionFrameType frame_type, VisionResult& result) Photoneo retrieval pose(s)

Example for get_di()

Nrmk::IndyFramework::DigitalList di_data;
bool is_success = indy.get_di(di_data);
if (is_success) {
    for (int i = 0; i < di_data.signals_size(); ++i) {
        std::cout << "DI " << i << " Address: " << di_data.signals(i).address()
                  << ", State: " << di_data.signals(i).state() << std::endl;
    }
}

Example for get_do(), set_do()

Nrmk::IndyFramework::DigitalList do_signal_list;
auto *do_signal = do_signal_list.add_signals();
do_signal->set_address(1);
do_signal->set_state(Nrmk::IndyFramework::DigitalState::ON_STATE);

bool is_success = indy.set_do(do_signal_list);
if (is_success) {
    std::cout << "Set DO successfully." << std::endl;
} else {
    std::cerr << "Failed to set DO." << std::endl;
}

Nrmk::IndyFramework::DigitalList do_data;
is_success = indy.get_do(do_data);
if (is_success) {
    std::cout << "GetDO RPC succeeded." << std::endl;
    for (int i = 0; i < do_data.signals_size(); i++) {
        const auto& signal = do_data.signals(i);
        std::cout << "Address: " << signal.address() << ", State: " << signal.state() << std::endl;
    }
}
else {
    std::cerr << "GetDO RPC failed." << std::endl;
}

Example for get_ai()

Nrmk::IndyFramework::AnalogList ai_data;
bool is_success = indy.get_ai(ai_data);
if (is_success) {
    for (int i = 0; i < ai_data.signals_size(); ++i) {
        std::cout << "AI " << i << " Address: " << ai_data.signals(i).address()
                  << ", Voltage: " << ai_data.signals(i).voltage() << std::endl; //mV
    }
}
- The control box's IO board has 2 AI channels assigned to address 0 and 1. The remaining channels are spare.

Example for get_ao(), set_ao()

Nrmk::IndyFramework::AnalogList ao_signal_list;
auto* ao_signal1 = ao_signal_list.add_signals();
ao_signal1->set_address(1);
ao_signal1->set_voltage(1000); //mV

auto* ao_signal2 = ao_signal_list.add_signals();
ao_signal2->set_address(2);
ao_signal2->set_voltage(2000); //mV

is_success = indy.set_ao(ao_signal_list);
if (is_success) {
    std::cout << "SetAO RPC succeeded." << std::endl;
} else {
    std::cerr << "SetAO RPC failed." << std::endl;
}

Nrmk::IndyFramework::AnalogList ao_data;
is_success = indy.get_ao(ao_data);
if (is_success) {
    std::cout << "GetAO RPC succeeded." << std::endl;
    for (int i = 0; i < ao_data.signals_size(); ++i) {
        const auto &signal = ao_data.signals(i);
        std::cout << "Address: " << signal.address() << ", Voltage: " << signal.voltage() << std::endl;
    }
} else {
    std::cerr << "GetAO RPC failed." << std::endl;
}

Example for get_endtool_di()

Nrmk::IndyFramework::EndtoolSignalList endtool_di_data;
is_success = indy.get_endtool_di(endtool_di_data);
if (is_success) {
    std::cout << "GetEndDI RPC succeeded." << std::endl;
    for (int i = 0; i < endtool_di_data.signals_size(); i++) {
        const auto& signal = endtool_di_data.signals(i);
        std::cout << "Port: " << signal.port() << std::endl;
        for (const auto& state : signal.states()) {
            std::cout << "State: " << state << std::endl;
        }
    }
} else {
    std::cerr << "GetEndDI RPC failed." << std::endl;
}

Example for get_endtool_do(), set_endtool_do()

Nrmk::IndyFramework::EndtoolSignalList end_do_signal_list;
auto* end_signal1 = end_do_signal_list.add_signals();
end_signal1->set_port("C");
end_signal1->add_states(EndtoolState::UNUSED);

is_success = indy.set_endtool_do(end_do_signal_list);
if (is_success) {
    std::cout << "SetEndDO RPC succeeded." << std::endl;
} else {
    std::cerr << "SetEndDO RPC failed." << std::endl;
}

Nrmk::IndyFramework::EndtoolSignalList endtool_do_data;
is_success = indy.get_endtool_do(endtool_do_data);
if (is_success) {
    std::cout << "GetEndDO RPC succeeded." << std::endl;
    for (int i = 0; i < endtool_do_data.signals_size(); i++) {
        const auto& signal = endtool_do_data.signals(i);
        std::cout << "Port: " << signal.port() << std::endl;
        for (const auto& state : signal.states()) {
            std::cout << "State: " << state << std::endl;
        }
    }
} else {
    std::cerr << "GetEndDO RPC failed." << std::endl;
}

Example for get_endtool_ai()

This can only be used with the RevE endtool board. The RevE endtool board has two AI channels, which are assigned to address 0 and 1, respectively. The remaining channels are reserved.

Nrmk::IndyFramework::AnalogList endtool_ai_data;
is_success = indy.get_endtool_ai(endtool_ai_data);
if (is_success) {
    std::cout << "GetEndAI RPC succeeded." << std::endl;
    for (int i = 0; i < endtool_ai_data.signals_size(); i++) {
        const auto& signal = endtool_ai_data.signals(i);
        std::cout << "Address: " << signal.address() << ", Voltage: " << signal.voltage() << std::endl; //mV
    }
} else {
    std::cerr << "GetEndAI RPC failed." << std::endl;
}

Example for get_endtool_ao(), set_endtool_ao()

This can only be used with the RevE endtool board. The RevE endtool board has two AO channels, which are assigned to address 0 and 1, respectively. The remaining channels are reserved.

Nrmk::IndyFramework::AnalogList end_ao_signal_list;
auto* end_ao_signal1 = end_ao_signal_list.add_signals();
end_ao_signal1->set_address(1);
end_ao_signal1->set_voltage(1000); //mV

is_success = indy.set_endtool_ao(end_ao_signal_list);
if (is_success) {
    std::cout << "SetEndAO RPC succeeded." << std::endl;
} else {
    std::cerr << "SetEndAO RPC failed." << std::endl;
}

Nrmk::IndyFramework::AnalogList endtool_ao_data;
is_success = indy.get_endtool_ao(endtool_ao_data);
if (is_success) {
    std::cout << "GetEndAO RPC succeeded." << std::endl;
    for (int i = 0; i < endtool_ao_data.signals_size(); i++) {
        const auto& signal = endtool_ao_data.signals(i);
        std::cout << "Address: " << signal.address() << ", Voltage: " << signal.voltage() << std::endl; //mV
    }
} else {
    std::cerr << "GetEndAO RPC failed." << std::endl;
}

Example for get_device_info()

Nrmk::IndyFramework::DeviceInfo dinfo;
if (indy.get_device_info(dinfo)) {
    std::cout << "Joints: " << dinfo.num_joints()
              << " Serial: " << dinfo.robot_serial()
              << " Payload: " << dinfo.payload() << std::endl;
}
joints=6 serial=Robot-151 payload=7

get_ft_sensor_data()

Nrmk::IndyFramework::FTSensorData ft;
if (indy.get_ft_sensor_data(ft)) {
    std::cout << "Fx=" << ft.ft_fx() << " Fy=" << ft.ft_fy() << " Fz=" << ft.ft_fz()
              << " Tx=" << ft.ft_tx() << " Ty=" << ft.ft_ty() << " Tz=" << ft.ft_tz() << std::endl;
}
Fx=0 Fy=0 Fz=0 Tx=0 Ty=0 Tz=0

Hardware I/O Examples

Gripper

Nrmk::IndyFramework::GripperData gdata;
if (indy.get_gripper_data(gdata)) {
    std::cout << "[GripperData] type=" << gdata.gripper_type()
                << " pos=" << gdata.gripper_position()
                << " state=" << gdata.gripper_state() << "\n";
} else {
    std::cerr << "get_gripper_data failed\n";
}
[DeviceInfo] joints=6 serial=Robot-151 payload=7

Nrmk::IndyFramework::GripperCommand gcmd;
gcmd.set_type(Nrmk::IndyFramework::GripperCommand::GRIPPER_ROBOTIQ); // example
gcmd.set_position(50); 
gcmd.set_speed(100); 
gcmd.set_force(40);
if (indy.set_gripper_command(gcmd)) {
    std::cout << "set_gripper_command sent (position=50,speed=100,force=40)\n";
} else {
    std::cerr << "set_gripper_command failed\n";
}
set_gripper_command sent (position=50,speed=100,force=40)

RS485 Endtool

Nrmk::IndyFramework::EndtoolRS485Rx write_rx; 
write_rx.set_word1(0x1234); 
write_rx.set_word2(0xABCD);
if (indy.set_endtool_rs485_rx(write_rx)) {
    std::cout << "set_endtool_rs485_rx (word1=0x1234 word2=0xABCD)\n";
} else {
    std::cerr << "set_endtool_rs485_rx failed\n";
}
set_endtool_rs485_rx (word1=0x1234 word2=0xABCD)

Nrmk::IndyFramework::EndtoolRS485Rx read_rx; 
if (indy.get_endtool_rs485_rx(read_rx)) {
    std::cout << "[RS485 RX] word1=0x" << std::hex << read_rx.word1()
                << " word2=0x" << read_rx.word2() << std::dec << "\n";
} else {
    std::cerr << "get_endtool_rs485_rx failed\n";
}
[RS485 RX] word1=0x1234 word2=0xabcd

Nrmk::IndyFramework::EndtoolRS485Tx read_tx; 
if (indy.get_endtool_rs485_tx(read_tx)) {
    std::cout << "[RS485 TX] word1=0x" << std::hex << read_tx.word1()
                << " word2=0x" << read_tx.word2() << std::dec << "\n";
} else {
    std::cerr << "get_endtool_rs485_tx failed\n";
}
[RS485 TX] word1=0x0 word2=0x0

Motion Command Functions

Motion command functions allow you to control the robot's movement, whether it's stopping, moving to specific positions, or executing complex motion paths.

Method Description
stop_motion(StopCategory stop_category) Stops motion according to the specified stop category
move_home() Moves the robot to the home position
movej(const std::vector<float> jtarget, const int base_type, const int blending_type, const float blending_radius, const float vel_ratio, const float acc_ratio, const bool const_cond, const int cond_type, const int react_type, DCPDICond di_condition, DCPVarCond var_condition, const bool teaching_mode) Moves the robot to a specified joint target position
movej_time(const std::vector<float> jtarget, const int base_type, const int blending_type, const float blending_radius, const float move_time, const bool const_cond, const int cond_type, const int react_type, DCPDICond di_condition, DCPVarCond var_condition, const bool teaching_mode) Moves the robot to a specified joint target position over a specified time
movel(const std::array<float, 6> ttarget, const int base_type, const int blending_type, const float blending_radius, const float vel_ratio, const float acc_ratio, const bool const_cond, const int cond_type, const int react_type, DCPDICond di_condition, DCPVarCond var_condition, const bool teaching_mode) Moves the robot to a specified linear target position
movel_time(const std::array<float, 6> ttarget, const int base_type, const int blending_type, const float blending_radius, const float move_time, const bool const_cond, const int cond_type, const int react_type, DCPDICond di_condition, DCPVarCond var_condition, const bool teaching_mode) Moves the robot to a specified linear target position over a specified time
movec(const std::array<float, 6> tpos1, const std::array<float, 6> tpos2, const float angle, const int setting_type, const int move_type, const int base_type, const int blending_type, const float blending_radius, const float vel_ratio, const float acc_ratio, const bool const_cond, const int cond_type, const int react_type, DCPDICond di_condition, DCPVarCond var_condition, const bool teaching_mode) Moves the robot along a circular path
movec_time(const std::array<float, 6>& tpos1, const std::array<float, 6>& tpos2, float angle, int setting_type, int move_type, int base_type, int blending_type, float blending_radius, float move_time, bool const_cond, int cond_type, int react_type, DCPDICond di_condition, DCPVarCond var_condition) Executes a circular motion in task space over a specified time
movelf(const std::array<float, 6> ttarget, int base_type, int blending_type, float blending_radius, float vel_ratio, float acc_ratio, bool const_cond, int cond_type, int react_type, DCPDICond di_condition, DCPVarCond var_condition, bool teaching_mode) Force-guided linear move (supports singularity bypass)
move_conveyor(float object_length, float approach_dist, float retract_dist, float vel_ratio, float acc_ratio) Synchronized pick/place along conveyor (tracking)
move_axis(const std::vector<float> atarget, int base_type, int blending_type, float blending_radius, float vel_ratio, float acc_ratio, bool const_cond, int cond_type, int react_type, DCPDICond di_condition, DCPVarCond var_condition, bool teaching_mode) Move external/aux axis joints
move_gcode(const std::string& gcode_file, bool is_smooth_mode, float smooth_radius, float vel_ratio, float acc_ratio) Executes a motion defined in a G-code file
add_joint_waypoint(const std::vector<float>& waypoint) get_joint_waypoint(std::vector<std::vector<float>>& waypoints) clear_joint_waypoint() move_joint_waypoint(float move_time) Move the robot through a set of joint waypoints
add_task_waypoint(const std::array<float, 6>& waypoint) get_task_waypoint(std::vector<std::array<float, 6>>& waypoints) clear_task_waypoint() move_task_waypoint(float move_time) Move the robot through a set of task waypoints
move_joint_traj(const std::vector<std::vector<float>>& q_list, const std::vector<std::vector<float>>& qdot_list, const std::vector<std::vector<float>>& qddot_list) Move the robot along a joint position-velocity-acceleration trajectory of full control frequency
move_task_traj(const std::vector<std::vector<float>>& p_list, const std::vector<std::vector<float>>& pdot_list, const std::vector<std::vector<float>>& pddot_list) Move the robot along a workspace position-velocity-acceleration trajectory of full control frequency

Warning

Be cautious when using motion commands, as the robot will move immediately if the command is successfully applied. While using the robot through the pendant allows movement only while the user is holding the touch button, using the API allows movement through program commands, so always ensure the area is clear of objects that could cause collisions. Especially, be mindful of safety if setting high speeds or accelerations, as the robot may move faster than expected.

Example for stop_motion()

Stops the robot's motion according to the specified stop category.

StopCategory stop_category = StopCategory::SMOOTH_BRAKE;
bool is_success = indy.stop_motion(stop_category);
if (is_success) {
    std::cout << "Motion stopped successfully." << std::endl;
} else {
    std::cerr << "Failed to stop motion." << std::endl;
}

Example for move_home()

Moves the robot to the predefined home position.

bool is_success = indy.move_home();
if (is_success) {
    std::cout << "Robot moved to home position successfully." << std::endl;
} else {
    std::cerr << "Failed to move robot to home position." << std::endl;
}

Example for movej()

Moves the robot to a specified joint target position.

std::vector<float> target_joints = {0.0, 0.0, -90.0, 0.0, -90.0, 0.0};

bool is_success = indy.movej(target_joints);
if (is_success) {
    std::cout << "MoveJ command executed successfully." << std::endl;
} else {
    std::cerr << "MoveJ command failed." << std::endl;
}

Example for movel()

Moves the robot to a specified linear target position in the workspace.

std::array<float, 6> target_pose = {350.0, -186.5, 522.0, -180.0, 0.0, 180.0};

bool is_success = indy.movel(target_pose);
if (is_success) {
    std::cout << "MoveL command executed successfully." << std::endl;
} else {
    std::cerr << "MoveL command failed." << std::endl;
}

Example for movec()

Moves the robot along a circular path passing through two specified workspace positions.

std::array<float, 6> t_pos1 = {241.66, -51.11, 644.20, 0.0, 180.0, 23.3};
std::array<float, 6> t_pos2 = {401.53, -47.74, 647.50, 0.0, 180.0, 23.3};
float angle = 720.0;

bool is_success = indy.movec(tpos1, tpos2, angle);
if (is_success) {
    std::cout << "Circular move executed successfully." << std::endl;
} else {
    std::cerr << "Failed to execute circular move." << std::endl;
}

Example for move_axis(...)

Controls attached linear axis

std::array<float,3> start_mm {0.f, 0.f, 0.f};
std::array<float,3> target_mm {200.f, 0.f, 0.f};
bool is_success = indy.move_axis(start_mm, target_mm, false, 50.f, 100.f);
if (is_success) {
    std::cout << "move_axis sent (start=[0,0,0]mm target=[200,0,0]mm is_absolute=false vel_ratio=50 acc_ratio=100)\n";
} else {
    std::cerr << "move_axis failed (check robot state / servo on / mode)\n";
}

Example for move_gcode()

Executes a motion defined in a G-code file.

std::string gcode_path = "/path/to/gcode/<gcode file>";
bool is_smooth = false;
float smooth_radius = 0.0f;
float velocity_ratio = 15.0f;
float acceleration_ratio = 100.0f;

bool is_success = indy.move_gcode(
    gcode_path,
    is_smooth,
    smooth_radius,
    velocity_ratio,
    acceleration_ratio
);

if (is_success) {
    std::cout << "G-code motion executed successfully." << std::endl;
} else {
    std::cerr << "Failed to execute G-code motion." << std::endl;
}

Move joint waypoints and move task waypoints

Joint Waypoints

Joint waypoints are specific positions for each of the robot's joints that the robot will move through sequentially.

add_joint_waypoint(const std::vector<float>& waypoint) adds a joint waypoint to the list of waypoints. The waypoint is a vector representing the target positions for each joint of the robot. waypoint: a vector of floats, each representing a joint position in degrees.

indy.add_joint_waypoint({0, 0, 0, 0, 0, 0});
indy.add_joint_waypoint({-44, 25, -63, 48, -7, -105});
indy.add_joint_waypoint({0, 0, 90, 0, 90, 0});
indy.add_joint_waypoint({-145, 31, -33, 117, -7, -133});
indy.add_joint_waypoint({-90, -15, -90, 0, -75, 0});

get_joint_waypoint() retrieves the current list of joint waypoints that have been added. This allows you to review or verify the waypoints before executing the movement.

std::vector<std::vector<float>> waypoints;
if (indy.get_joint_waypoint(waypoints)) {
    std::cout << "Successfully retrieved joint waypoints:" << std::endl;
    for (const auto& wp : waypoints) {
        for (float joint : wp) {
            std::cout << joint << " ";
        }
        std::cout << std::endl;
    }
} else {
    std::cerr << "No joint waypoints available." << std::endl;
}

clear_joint_waypoint() clears the current list of joint waypoints. This function is useful if you need to reset the waypoints and start adding new ones.

indy.clear_joint_waypoint();

move_joint_waypoint(float move_time) moves the robot through the sequence of joint waypoints that have been added. The robot will attempt to reach each waypoint sequentially. move_time: (optional) The time in seconds for the robot to move through the waypoints. If not provided, the robot will move normally.

indy.move_joint_waypoint();
float move_time = 3.0;
indy.move_joint_waypoint(move_time);

Task Waypoints

Task waypoints refer to specific positions and orientations in space that the robot's endtool should move through.

add_task_waypoint(const std::array<float, 6>& waypoint) adds a task waypoint to the list of waypoints. The waypoint is an array representing the target position and orientation in Cartesian space. waypoint: an array of six floats representing the x, y, z position and the roll, pitch, yaw orientation of the endtool.

indy.add_task_waypoint({-186.54f, -454.45f, 415.61f, 179.99f, -0.06f, 89.98f});
indy.add_task_waypoint({-334.67f, -493.07f, 259.00f, 179.96f, -0.12f, 89.97f});
indy.add_task_waypoint({224.79f, -490.20f, 508.08f, 179.96f, -0.14f, 89.97f});
indy.add_task_waypoint({-129.84f, -416.84f, 507.38f, 179.95f, -0.16f, 89.96f});
indy.add_task_waypoint({-186.54f, -454.45f, 415.61f, 179.99f, -0.06f, 89.98f});

get_task_waypoint(std::vector<std::array<float, 6>>& waypoints) retrieves the current list of task waypoints that have been added. This allows you to review or verify the waypoints before executing the movement.

std::vector<std::array<float, 6>> t_waypoints;
if (indy.get_task_waypoint(t_waypoints)) {
    std::cout << "Successfully retrieved task waypoints:" << std::endl;
    for (const auto& wp : t_waypoints) {
        for (float task : wp) {
            std::cout << task << " ";
        }
        std::cout << std::endl;
    }
} else {
    std::cerr << "No task waypoints available." << std::endl;
}

clear_task_waypoint() clears the current list of task waypoints. This function is useful if you need to reset the waypoints and start adding new ones.

indy.clear_task_waypoint();

move_task_waypoint(float move_time) moves the robot's endtool through the sequence of task waypoints that have been added. The robot will attempt to reach each waypoint sequentially. move_time: (optional) The time in seconds for the robot to move through the waypoints. If not provided, the robot will move normally.

indy.move_task_waypoint();
indy.move_task_waypoint(move_time=1.5);

Trajectory Motion

move_joint_traj is a joint-space trajectory movement movement command. Below, q_list, qdot_list, qddot_list are list values of joint position, velocity and acceleration in radians. The time step between trajectory is the control period, which is 250us by default.

std::vector<std::vector<float>> q_list = {{...}, {...}, ...};
std::vector<std::vector<float>> qdot_list = {{...}, {...}, ...};
std::vector<std::vector<float>> qddot_list = {{...}, {...}, ...};

indy.move_joint_traj(q_list, qdot_list, qddot_list)

move_task_traj is a task-space trajectory movement movement command. Below, p_list, pdot_list, pddot_list are list values of endtool position, velocity and acceleration in meters and radians. The time step between trajectory is the control period, which is 250us by default.

std::vector<std::vector<float>> p_list = {{...}, {...}, ...};
std::vector<std::vector<float>> pdot_list = {{...}, {...}, ...};
std::vector<std::vector<float>> pddot_list = {{...}, {...}, ...};

indy.move_task_traj(p_list, pdot_list, pddot_list)

move_axis() (External / Auxiliary Axis)

Moves external axes (e.g., linear tracks, rotary tables) together with robot joints.

std::vector<float> axis_targets = {100.0f};
bool ok = indy.move_axis(axis_targets);

Waiting Functions

Wait IO: Waits for specific digital or analog I/O signals to meet the given conditions.
Wait Time: Waits for a specified time, after starting the first motion command.
Wait Progress: Waits for progress status.
Wait Radius: Waits until movement within a specified radius is complete.
Wait Traj: Waits until trajectory movement is complete.
- Wait Traj conditions
- TRAJ_STARTED(0): trajectory has just started
- TRAJ_ACC_DONE(1): acceleration phase of the trajectory has been completed
- TRAJ_CRZ_DONE(2): cruising phase (constant speed phase) of the trajectory has been completed.
- TRAJ_DEC_DONE(3): deceleration phase of the trajectory has been completed.

std::vector<float> j_pos_1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
indy.movej(j_pos_1, JointBaseType::ABSOLUTE_JOINT, BlendingType_Type::BlendingType_Type_OVERRIDE);

// indy.wait_time(0.1f);
// indy.wait_radius(10);
// indy.wait_traj(TrajCondition::TRAJ_ACC_DONE);
indy.wait_progress(15);

std::vector<float> j_pos_2 = {0.0, 0.0, -90.0, 0.0, -90.0, 0.0};
indy.movej(j_pos_2, JointBaseType::ABSOLUTE_JOINT, BlendingType_Type::BlendingType_Type_OVERRIDE);

wait_io()

Waits for specified digital and analog I/O signals to meet the given conditions.

std::vector<Nrmk::IndyFramework::DigitalSignal> di_signals = {
    Nrmk::IndyFramework::DigitalSignal(1, Nrmk::IndyFramework::DigitalState::ON),
    Nrmk::IndyFramework::DigitalSignal(2, Nrmk::IndyFramework::DigitalState::OFF)
};

std::vector<Nrmk::IndyFramework::DigitalSignal> end_di_signals = {
    Nrmk::IndyFramework::DigitalSignal(3, Nrmk::IndyFramework::DigitalState::ON)
};

bool is_success = indy.wait_io(
    di_signals,                           // Digital input signals to wait for
    {},                                   // Digital output signals to wait for
    end_di_signals,                       // Digital input signals to end waiting
    {},                                   // Digital output signals to end waiting
    1                                     // Conjunction (AND condition)
);

if (is_success) {
    std::cout << "WaitIO condition met successfully." << std::endl;
} else {
    std::cerr << "WaitIO condition failed." << std::endl;
}

Teleoperation-related functions provide the capability to remotely control the robot. These functions allow real-time adjustment of the robot's motion and control in various operation modes. You can start the teleoperation mode using the start_teleop function and end the mode using the stop_teleop function. Additionally, you can update robot movements in joint space and workspace using the movetelej and movetelel functions, respectively.

Note

When using teleoperation motion through IndyDCP3, continuously updating the target position in real-time allows the robot to stream and generate smooth and immediate motion. Traditional Motion commands (movej, movel) perform path generation smoothly but cannot respond immediately like remote control. This is a characteristic of teleoperation motion.

Function Description
start_teleop(TeleMethod method) Starts teleoperation mode
stop_teleop() Ends teleoperation mode
get_teleop_device(Nrmk::IndyFramework::TeleOpDevice& device) Retrieve information about the currently connected teleoperation device
get_teleop_state(Nrmk::IndyFramework::TeleOpState& state) Retrieve the current state of teleoperation
connect_teleop_device(const std::string& name, Nrmk::IndyFramework::TeleOpDevice_TeleOpDeviceType type, const std::string& ip, uint32_t port) Connect to a teleoperation device with specified parameters
disconnect_teleop_device() Disconnect the currently connected teleoperation device
read_teleop_input(Nrmk::IndyFramework::TeleP& teleop_input) Read the current input values from the teleoperation device
get_tele_file_list(std::vector<std::string>& files) Retrieve a list of saved teleoperation motion files
save_tele_motion(const std::string& name) Save the current teleoperation motion with a specified name
load_tele_motion(const std::string& name) Load a saved teleoperation motion by name
delete_tele_motion(const std::string& name) Delete a saved teleoperation motion by name
enable_tele_key(const bool enable) Enable or disable teleoperation key input
set_play_rate(const float rate) Set the playback rate for teleoperation motion
get_play_rate(float& rate) Retrieve the current playback rate for teleoperation motion

Example for start_teleop()

Starts teleoperation mode with the specified method.

TeleMethod method = TeleMethod::TELE_JOINT_RELATIVE;
bool is_success = indy.start_teleop(method);
if (is_success) {
    std::cout << "Teleoperation mode started successfully." << std::endl;
} else {
    std::cerr << "Failed to start teleoperation mode." << std::endl;
}

Example for stop_teleop()

Stops the teleoperation mode.

bool is_success = indy.stop_teleop();
if (is_success) {
    std::cout << "Teleoperation mode stopped successfully." << std::endl;
} else {
    std::cerr << "Failed to stop teleoperation mode." << std::endl;
}

Example for get_teleop_device()

Retrieve information about the currently connected teleoperation device.

Nrmk::IndyFramework::TeleOpDevice device;

bool is_success = indy.get_teleop_device(device);

if (is_success) {
    std::cout << "Teleoperation device name: " << device.name() << std::endl;
    std::cout << "Device IP: " << device.ip() << std::endl;
} else {
    std::cerr << "Failed to retrieve teleoperation device information." << std::endl;
}

Example for get_teleop_state()

Retrieve the current teleoperation state.

Nrmk::IndyFramework::TeleOpState state;

bool is_success = indy.get_teleop_state(state);

if (is_success) {
    std::cout << "Teleoperation state: " << state.state() << std::endl;
} else {
    std::cerr << "Failed to retrieve teleoperation state." << std::endl;
}

Example for connect_teleop_device()

Connects to a teleoperation device with specified parameters.

std::string device_name = "TeleOpDevice1";
Nrmk::IndyFramework::TeleOpDevice_TeleOpDeviceType type = Nrmk::IndyFramework::TeleOpDevice_TeleOpDeviceType::TeleOpDeviceType_JOYSTICK;
std::string ip = "192.168.0.2";
uint32_t port = 5555;

bool is_success = indy.connect_teleop_device(device_name, type, ip, port);

if (is_success) {
    std::cout << "Teleoperation device connected successfully." << std::endl;
} else {
    std::cerr << "Failed to connect to teleoperation device." << std::endl;
}

Example for disconnect_teleop_device()

Disconnects the currently connected teleoperation device.

bool is_success = indy.disconnect_teleop_device();

if (is_success) {
    std::cout << "Teleoperation device disconnected successfully." << std::endl;
} else {
    std::cerr << "Failed to disconnect teleoperation device." << std::endl;
}

Example for read_teleop_input()

Reads the current input values from the teleoperation device.

Nrmk::IndyFramework::TeleP teleop_input;

bool is_success = indy.read_teleop_input(teleop_input);

if (is_success) {
    std::cout << "Teleoperation input read successfully." << std::endl;
    // Process teleop_input as needed
} else {
    std::cerr << "Failed to read teleoperation input." << std::endl;
}

Example for set_play_rate()

Sets the playback rate for teleoperation motion.

float playback_rate = 1.2;

bool is_success = indy.set_play_rate(playback_rate);

if (is_success) {
    std::cout << "Playback rate set successfully to " << playback_rate << std::endl;
} else {
    std::cerr << "Failed to set playback rate." << std::endl;
}

Example for get_play_rate()

Retrieves the current playback rate for teleoperation motion.

float playback_rate;

bool is_success = indy.get_play_rate(playback_rate);

if (is_success) {
    std::cout << "Current playback rate: " << playback_rate << std::endl;
} else {
    std::cerr << "Failed to retrieve playback rate." << std::endl;
}

Example for get_tele_file_list()

Retrieves a list of saved teleoperation motion files.

std::vector<std::string> files;

bool is_success = indy.get_tele_file_list(files);

if (is_success) {
    std::cout << "Retrieved teleoperation files:" << std::endl;
    for (const auto& file : files) {
        std::cout << " - " << file << std::endl;
    }
} else {
    std::cerr << "Failed to retrieve teleoperation file list." << std::endl;
}

Example for save_tele_motion()

Saves the current teleoperation motion with a specified name.

std::string motion_name = "MyTeleMotion";

bool is_success = indy.save_tele_motion(motion_name);

if (is_success) {
    std::cout << "Teleoperation motion saved successfully as " << motion_name << std::endl;
} else {
    std::cerr << "Failed to save teleoperation motion." << std::endl;
}

Example for load_tele_motion()

Loads a saved teleoperation motion by name.

std::string motion_name = "MyTeleMotion";

bool is_success = indy.load_tele_motion(motion_name);

if (is_success) {
    std::cout << "Teleoperation motion " << motion_name << " loaded successfully." << std::endl;
} else {
    std::cerr << "Failed to load teleoperation motion." << std::endl;
}

Example for delete_tele_motion()

Deletes a saved teleoperation motion by name.

std::string motion_name = "MyTeleMotion";

bool is_success = indy.delete_tele_motion(motion_name);

if (is_success) {
    std::cout << "Teleoperation motion " << motion_name << " deleted successfully." << std::endl;
} else {
    std::cerr << "Failed to delete teleoperation motion." << std::endl;
}

Example for enable_tele_key()

Enables or disables teleoperation key input.

bool enable_key = true;

bool is_success = indy.enable_tele_key(enable_key);

if (is_success) {
    std::cout << "Teleoperation key input enabled." << std::endl;
} else {
    std::cerr << "Failed to enable teleoperation key input." << std::endl;
}

Variable Handling Functions

The IndyDCP3 C++ API provides several functions to handle different types of variables, such as boolean, integer, float, joint position (JPos), and task position (TPos) variables. These variables can be used to exchange information between the robot and external devices.

Method Description
get_bool_variable(std::vector<BoolVariable> &variables) Retrieves Bool type variables
get_int_variable(std::vector<IntVariable> &variables) Retrieves Integer type variables
get_float_variable(std::vector<FloatVariable> &variables) Retrieves Float type variables
get_jpos_variable(std::vector<JPosVariable> &variables) Retrieves JPos type variables
get_tpos_variable(std::vector<TPosVariable> &variables) Retrieves TPos type variables
set_bool_variable(const std::vector<BoolVariable> &variables) Sets Bool type variables
set_int_variable(const std::vector<IntVariable> &variables) Sets Integer type variables
set_float_variable(const std::vector<FloatVariable> &variables) Sets Float type variables
set_jpos_variable(const std::vector<JPosVariable> &variables) Sets JPos type variables
set_tpos_variable(const std::vector<TPosVariable> &variables) Sets TPos type variables
set_plugin_bool_variable(name, value) / get_plugin_bool_variable(name) Set / get plugin Bool variable
set_plugin_int_variable(name, value) / get_plugin_int_variable(name) Set / get plugin Int variable
set_plugin_float_variable(name, value) / get_plugin_float_variable(name) Set / get plugin Float variable
set_plugin_jpos_variable(name, jpos) / get_plugin_jpos_variable(name) Set / get plugin JPos variable
set_plugin_tpos_variable(name, tpos) / get_plugin_tpos_variable(name) Set / get plugin TPos variable

Example for get_bool_variable()

Retrieves the current values of Bool type variables from the robot.

std::vector<Nrmk::IndyFramework::BoolVariable> bool_vars;
bool is_success = indy.get_bool_variable(bool_vars);
if (is_success) {
    std::cout << "Bool variables retrieved successfully." << std::endl;
    for (const auto &var : bool_vars) {
        std::cout << "Address: " << var.addr() << ", Value: " << var.value() << std::endl;
    }
} else {
    std::cerr << "Failed to retrieve Bool variables." << std::endl;
}

Example for get_int_variable()

Retrieves the current values of Integer type variables from the robot.

std::vector<Nrmk::IndyFramework::IntVariable> int_vars;
bool is_success = indy.get_int_variable(int_vars);
if (is_success) {
    std::cout << "Integer variables retrieved successfully." << std::endl;
    for (const auto &var : int_vars) {
        std::cout << "Address: " << var.addr() << ", Value: " << var.value() << std::endl;
    }
} else {
    std::cerr << "Failed to retrieve Integer variables." << std::endl;
}

Example for get_float_variable()

Retrieves the current values of Float type variables from the robot.

std::vector<Nrmk::IndyFramework::FloatVariable> float_vars;
bool is_success = indy.get_float_variable(float_vars);
if (is_success) {
    std::cout << "Float variables retrieved successfully." << std::endl;
    for (const auto &var : float_vars) {
        std::cout << "Address: " << var.addr() << ", Value: " << var.value() << std::endl;
    }
} else {
    std::cerr << "Failed to retrieve Float variables." << std::endl;
}

Example for get_jpos_variable()

Retrieves the current values of Joint Position (JPos) type variables from the robot.

std::vector<Nrmk::IndyFramework::JPosVariable> jpos_vars;
bool is_success = indy.get_jpos_variable(jpos_vars);
if (is_success) {
    std::cout << "JPos variables retrieved successfully." << std::endl;
    for (const auto &var : jpos_vars) {
        std::cout << "Address: " << var.addr() << ", JPos: ";
        for (const auto &jpos : var.jpos()) {
            std::cout << jpos << " ";
        }
        std::cout << std::endl;
    }
} else {
    std::cerr << "Failed to retrieve JPos variables." << std::endl;
}

Example for get_tpos_variable()

Retrieves the current values of Task Position (TPos) type variables from the robot.

std::vector<Nrmk::IndyFramework::TPosVariable> tpos_vars;
bool is_success = indy.get_tpos_variable(tpos_vars);
if (is_success) {
    std::cout << "TPos variables retrieved successfully." << std::endl;
    for (const auto &var : tpos_vars) {
        std::cout << "Address: " << var.addr() << ", TPos: ";
        for (const auto &tpos : var.tpos()) {
            std::cout << tpos << " ";
        }
        std::cout << std::endl;
    }
} else {
    std::cerr << "Failed to retrieve TPos variables." << std::endl;
}

Example for set_bool_variable()

Sets the values of Bool type variables in the robot.

std::vector<Nrmk::IndyFramework::BoolVariable> bool_vars;

Nrmk::IndyFramework::BoolVariable var1;
var1.set_addr(1);
var1.set_value(true);
bool_vars.push_back(var1);

bool is_success = indy.set_bool_variable(bool_vars);
if (is_success) {
    std::cout << "Bool variables set successfully." << std::endl;
} else {
    std::cerr << "Failed to set Bool variables." << std::endl;
}

Example for set_int_variable()

Sets the values of Integer type variables in the robot.

std::vector<Nrmk::IndyFramework::IntVariable> int_vars;

Nrmk::IndyFramework::IntVariable var1;
var1.set_addr(2);
var1.set_value(42);
int_vars.push_back(var1);

bool is_success = indy.set_int_variable(int_vars);
if (is_success) {
    std::cout << "Integer variables set successfully." << std::endl;
} else {
    std::cerr << "Failed to set Integer variables." << std::endl;
}

Example for set_float_variable()

Sets the values of Float type variables in the robot.

std::vector<Nrmk::IndyFramework::FloatVariable> float_vars;

Nrmk::IndyFramework::FloatVariable var1;
var1.set_addr(3);
var1.set_value(3.14f);
float_vars.push_back(var1);

bool is_success = indy.set_float_variable(float_vars);
if (is_success) {
    std::cout << "Float variables set successfully." << std::endl;
} else {
    std::cerr << "Failed to set Float variables." << std::endl;
}

Example for set_jpos_variable()

Sets the values of Joint Position (JPos) type variables in the robot.

std::vector<Nrmk::IndyFramework::JPosVariable> jpos_vars;

Nrmk::IndyFramework::JPosVariable var1;
var1.set_addr(1);
var1.add_jpos(0.0f);
var1.add_jpos(0.0f);
var1.add_jpos(-90.0f);
var1.add_jpos(0.0f);
var1.add_jpos(-90.0f);
var1.add_jpos(0.0f);
jpos_vars.push_back(var1);

bool is_success = indy.set_jpos_variable(jpos_vars);
if (is_success) {
    std::cout << "JPos variables set successfully." << std::endl;
} else {
    std::cerr << "Failed to set JPos variables." << std::endl;
}

Example for set_tpos_variable()

Sets the values of Tool Position (TPos) type variables in the robot.

std::vector<Nrmk::IndyFramework::TPosVariable> tpos_vars;

Nrmk::IndyFramework::TPosVariable var1;
var1.set_addr(1);
var1.add_tpos(350.0f);
var1.add_tpos(-186.5f);
var1.add_tpos(522.0f);
var1.add_tpos(-180.0f);
var1.add_tpos(0.0f);
var1.add_tpos(180.0f);
tpos_vars.push_back(var1);

bool is_success = indy.set_tpos_variable(tpos_vars);
if (is_success) {
    std::cout << "TPos variables set successfully." << std::endl;
} else {
    std::cerr << "Failed to set TPos variables." << std::endl;
}

Example for set_plugin_bool_variable(), get_plugin_bool_variable()

bool ok = indy.set_plugin_bool_variable("bool_1", true);
std::cout << "set_plugin_bool_variable(bool_1,true): " << (ok?"OK":"FAIL") << "\n";

bool bval=false; 
ok = indy.get_plugin_bool_variable("bool_1", bval);
std::cout << "get_plugin_bool_variable(bool_1): " << (ok?"OK":"FAIL") << ", value=" << bval << "\n";
set_plugin_bool_variable(bool_1,true): OK
get_plugin_bool_variable(bool_1): OK, value=1

Example for set_plugin_int_variable(), get_plugin_int_variable()

ok = indy.set_plugin_int_variable("int_1", 3);
std::cout << "set_plugin_int_variable(int_1,3): " << (ok?"OK":"FAIL") << "\n";

int64_t ival=0; 
ok = indy.get_plugin_int_variable("int_1", ival);
std::cout << "get_plugin_int_variable(int_1): " << (ok?"OK":"FAIL") << ", value=" << ival << "\n";
set_plugin_int_variable(int_1,3): OK
get_plugin_int_variable(int_1): OK, value=3

Example for set_plugin_float_variable(), get_plugin_float_variable()

ok = indy.set_plugin_float_variable("float_1", 0.8f);
std::cout << "set_plugin_float_variable(float_1,0.8): " << (ok?"OK":"FAIL") << "\n";

float fval=0.f; 
ok = indy.get_plugin_float_variable("float_1", fval);
std::cout << "get_plugin_float_variable(float_1): " << (ok?"OK":"FAIL") << ", value=" << fval << "\n";
set_plugin_float_variable(float_1,0.8): OK
get_plugin_float_variable(float_1): OK, value=0.8

Example for set_plugin_jpos_variable(), get_plugin_jpos_variable()

std::vector<float> jpos = {0.0f, 0.0f, -90.0f, 0.0f, -90.0f, 0.0f};
ok = indy.set_plugin_jpos_variable("ex_home_pose", jpos);
std::cout << "set_plugin_jpos_variable(ex_home_pose): " << (ok?"OK":"FAIL") << "\n";

std::vector<float> jpos_read; 
ok = indy.get_plugin_jpos_variable("ex_home_pose", jpos_read);
std::cout << "get_plugin_jpos_variable(ex_home_pose): " << (ok?"OK":"FAIL") << ", size=" << jpos_read.size() << "\n";
set_plugin_jpos_variable(ex_home_pose): OK
get_plugin_jpos_variable(ex_home_pose): OK, size=6

Example for set_plugin_tpos_variable(), get_plugin_tpos_variable()

std::vector<float> tpos = {100.f, 0.f, 300.f, 0.f, 90.f, 0.f};
ok = indy.set_plugin_tpos_variable("ex_t_pose", tpos);
std::cout << "set_plugin_tpos_variable(ex_t_pose): " << (ok?"OK":"FAIL") << "\n";

std::vector<float> tpos_read; 
ok = indy.get_plugin_tpos_variable("ex_t_pose", tpos_read);
std::cout << "get_plugin_tpos_variable(ex_t_pose): " << (ok?"OK":"FAIL") << ", size=" << tpos_read.size() << "\n";
set_plugin_tpos_variable(ex_t_pose): OK
get_plugin_tpos_variable(ex_t_pose): OK, size=6

The IndyDCP3 C++ API provides functions for performing inverse kinematics, enabling/disabling direct teaching mode, setting simulation mode, and recovering the robot from error states.

Method Description
inverse_kin(const std::array<float, 6> &tpos, const std::vector<float> &init_jpos, std::vector<float> &jpos) Calculates joint positions to reach a target workspace position
set_direct_teaching(bool enable) Enables or disables direct teaching mode
set_simulation_mode(bool enable) Enables or disables simulation mode
recover() Recovers the robot from an error or collision state

Example for inverse_kin()

std::array<float, 6> target_pose = {350.0, -186.5, 522.0, -180.0, 0.0, 180.0};
std::vector<float> init_jpos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
std::vector<float> calculated_jpos;

bool is_success = indy.inverse_kin(target_pose, init_jpos, calculated_jpos);
if (is_success) {
    std::cout << "Inverse kinematics successful. Joint positions: ";
    for (float jp : calculated_jpos) {
        std::cout << jp << " ";
    }
    std::cout << std::endl;
} else {
    std::cerr << "Inverse kinematics failed." << std::endl;
}

Example for set_direct_teaching()

bool enable_direct_teaching = true;
bool is_success = indy.set_direct_teaching(enable_direct_teaching);
if (is_success) {
    std::cout << "Direct teaching mode enabled." << std::endl;
} else {
    std::cerr << "Failed to enable direct teaching mode." << std::endl;
}

Example for set_simulation_mode()

bool enable_simulation = true;
bool is_success = indy.set_simulation_mode(enable_simulation);
if (is_success) {
    std::cout << "Simulation mode enabled." << std::endl;
} else {
    std::cerr << "Failed to enable simulation mode." << std::endl;
}

Example for recover()

bool is_success = indy.recover();
if (is_success) {
    std::cout << "Recovery successful." << std::endl;
} else {
    std::cerr << "Recovery failed." << std::endl;
}

Program Control Functions

Program control functions allow you to start, pause, resume, and stop robot programs. These functions are essential for managing the execution flow of pre-written programs.

Method Description
play_program(const std::string &prog_name, int prog_idx) Starts a program by name or index
pause_program() Pauses the currently running program
resume_program() Resumes a paused program
stop_program() Stops the currently running program

Example for play_program()

std::string program_name = "example_program.indy7v2.json";
int program_index = 1;

bool is_success = indy.play_program(program_name, program_index);
if (is_success) {
    std::cout << "Program started successfully." << std::endl;
} else {
    std::cerr << "Failed to start program." << std::endl;
}

Example for pause_program()

bool is_success = indy.pause_program();
if (is_success) {
    std::cout << "Program paused successfully." << std::endl;
} else {
    std::cerr << "Failed to pause program." << std::endl;
}

Example for resume_program()

bool is_success = indy.resume_program();
if (is_success) {
    std::cout << "Program resumed successfully." << std::endl;
} else {
    std::cerr << "Failed to resume program." << std::endl;
}

Example for stop_program()

bool is_success = indy.stop_program();
if (is_success) {
    std::cout << "Program stopped successfully." << std::endl;
} else {
    std::cerr << "Failed to stop program." << std::endl;
}

Using IndySDK

The IndyDCP3 C++ API provides functions to activate the IndySDK, set custom control modes, and manage control gains. These functions are crucial for advanced users who want to develop custom controllers.

Method Description
activate_sdk(const SDKLicenseInfo &request, SDKLicenseResp &response) Activates the IndySDK
set_custom_control_mode(bool mode) Sets the custom control mode
get_custom_control_mode(int &mode) Retrieves the current custom control mode
set_custom_control_gain(const CustomGainSet &gain_set) Sets custom control gain for the user controller
get_custom_control_gain(CustomGainSet &gain_set) Retrieves the current custom control gain settings
set_joint_control_gain(const std::vector<float>& kp, const std::vector<float>& kv, const std::vector<float>& kl2) Set the control gains for joint-level control
get_joint_control_gain(std::vector<float>& kp, std::vector<float>& kv, std::vector<float>& kl2) Retrieve the control gains for joint-level control
set_task_control_gain(const std::vector<float>& kp, const std::vector<float>& kv, const std::vector<float>& kl2) Set the control gains for task-level control
get_task_control_gain(std::vector<float>& kp, std::vector<float>& kv, std::vector<float>& kl2) Retrieve the control gains for task-level control
set_impedance_control_gain(const std::vector<float>& mass, const std::vector<float>& damping, const std::vector<float>& stiffness, const std::vector<float>& kl2) Set the impedance control gains
get_impedance_control_gain(std::vector<float>& mass, std::vector<float>& damping, std::vector<float>& stiffness, std::vector<float>& kl2) Retrieve the impedance control gains
set_force_control_gain(const std::vector<float>& kp, const std::vector<float>& kv, const std::vector<float>& kl2, const std::vector<float>& mass, const std::vector<float>& damping, const std::vector<float>& stiffness, const std::vector<float>& kpf, const std::vector<float>& kif) Set the force control gains
get_force_control_gain(std::vector<float>& kp, std::vector<float>& kv, std::vector<float>& kl2, std::vector<float>& mass, std::vector<float>& damping, std::vector<float>& stiffness, std::vector<float>& kpf, std::vector<float>& kif) Retrieve the force control gains

Example for activate_sdk()

Nrmk::IndyFramework::SDKLicenseInfo request;
request.set_license_key("YOUR_LICENSE_KEY");
request.set_expire_date("yyyy-mm-dd");

Nrmk::IndyFramework::SDKLicenseResp response;
bool is_success = indy.activate_sdk(request, response);
if (is_success) {
    std::cout << "SDK Activated: " << (response.activated() ? "Yes" : "No") << std::endl;
    std::cout << "Response Code: " << response.response().code() << ", Message: " << response.response().msg() << std::endl;
} else {
    std::cerr << "Failed to activate SDK." << std::endl;
}

Example for set_custom_control_mode() and get_custom_control_mode()

int mode = 0;
bool is_success = indy.set_custom_control_mode(mode);
if (is_success) {
    std::cout << "Custom control mode set successfully." << std::endl;
} else {
    std::cerr << "Failed to set custom control mode." << std::endl;
}
int get_mode;
bool is_success = indy.get_custom_control_mode(get_mode);
if (is_success) {
    std::cout << "Custom control mode: " << get_mode << std::endl;
} else {
    std::cerr << "Failed to get custom control mode." << std::endl;
}

Example for set_custom_control_gain()

Nrmk::IndyFramework::CustomGainSet gain_set;
gain_set.add_gain0(0.0);
gain_set.add_gain0(0.0);
gain_set.add_gain0(0.0);
gain_set.add_gain0(0.0);
gain_set.add_gain0(0.0);
gain_set.add_gain0(0.0);

bool is_success = indy.set_custom_control_gain(gain_set);
if (is_success) {
    std::cout << "Custom control gain set successfully." << std::endl;
} else {
    std::cerr << "Failed to set custom control gain." << std::endl;
}

Example for get_custom_control_gain()

Nrmk::IndyFramework::CustomGainSet gain_set;
bool is_success = indy.get_custom_control_gain(gain_set);
if (is_success) {
    std::cout << "Custom control gains retrieved successfully." << std::endl;
    for (int i = 0; i < gain_set.gain0_size(); ++i) {
        std::cout << "Gain0: " << gain_set.gain0(i) << " ";
    }
    std::cout << std::endl;
} else {
    std::cerr << "Failed to retrieve custom control gains." << std::endl;
}

Example for set_joint_control_gain()

std::vector<float> kp = {...};
std::vector<float> kv = {...};
std::vector<float> kl2 = {...};

bool is_success = indy.set_joint_control_gain(kp, kv, kl2);

if (is_success) {
    std::cout << "Joint control gains set successfully." << std::endl;
} else {
    std::cerr << "Failed to set joint control gains." << std::endl;
}

Example for get_joint_control_gain()

std::vector<float> kp, kv, kl2;

bool is_success = indy.get_joint_control_gain(kp, kv, kl2);

if (is_success) {
    std::cout << "Joint control gains retrieved successfully." << std::endl;
    std::cout << "KP: ";
    for (const auto& value : kp) std::cout << value << " ";
    std::cout << "\nKV: ";
    for (const auto& value : kv) std::cout << value << " ";
    std::cout << "\nKL2: ";
    for (const auto& value : kl2) std::cout << value << " ";
    std::cout << std::endl;
} else {
    std::cerr << "Failed to retrieve joint control gains." << std::endl;
}

Example for set_task_control_gain()

std::vector<float> kp = {...};
std::vector<float> kv = {...};
std::vector<float> kl2 = {...};

bool is_success = indy.set_task_control_gain(kp, kv, kl2);

if (is_success) {
    std::cout << "Task control gains set successfully." << std::endl;
} else {
    std::cerr << "Failed to set task control gains." << std::endl;
}

Example for get_task_control_gain()

std::vector<float> kp, kv, kl2;

bool is_success = indy.get_task_control_gain(kp, kv, kl2);

if (is_success) {
    std::cout << "Task control gains retrieved successfully." << std::endl;
    std::cout << "KP: ";
    for (const auto& value : kp) std::cout << value << " ";
    std::cout << "\nKV: ";
    for (const auto& value : kv) std::cout << value << " ";
    std::cout << "\nKL2: ";
    for (const auto& value : kl2) std::cout << value << " ";
    std::cout << std::endl;
} else {
    std::cerr << "Failed to retrieve task control gains." << std::endl;
}

Example for set_impedance_control_gain()

std::vector<float> mass = {...};
std::vector<float> damping = {...};
std::vector<float> stiffness = {...};
std::vector<float> kl2 = {...};

bool is_success = indy.set_impedance_control_gain(mass, damping, stiffness, kl2);

if (is_success) {
    std::cout << "Impedance control gains set successfully." << std::endl;
} else {
    std::cerr << "Failed to set impedance control gains." << std::endl;
}

Example for get_impedance_control_gain()

std::vector<float> mass, damping, stiffness, kl2;

bool is_success = indy.get_impedance_control_gain(mass, damping, stiffness, kl2);

if (is_success) {
    std::cout << "Impedance control gains retrieved successfully." << std::endl;
    std::cout << "Mass: ";
    for (const auto& value : mass) std::cout << value << " ";
    std::cout << "\nDamping: ";
    for (const auto& value : damping) std::cout << value << " ";
    std::cout << "\nStiffness: ";
    for (const auto& value : stiffness) std::cout << value << " ";
    std::cout << "\nKL2: ";
    for (const auto& value : kl2) std::cout << value << " ";
    std::cout << std::endl;
} else {
    std::cerr << "Failed to retrieve impedance control gains." << std::endl;
}

Example for set_force_control_gain()

std::vector<float> kp = {...};
std::vector<float> kv = {...};
std::vector<float> kl2 = {...};
std::vector<float> mass = {...};
std::vector<float> damping = {...};
std::vector<float> stiffness = {...};
std::vector<float> kpf = {...};
std::vector<float> kif = {...};

bool is_success = indy.set_force_control_gain(kp, kv, kl2, mass, damping, stiffness, kpf, kif);

if (is_success) {
    std::cout << "Force control gains set successfully." << std::endl;
} else {
    std::cerr << "Failed to set force control gains." << std::endl;
}

Example for get_force_control_gain()

std::vector<float> kp, kv, kl2, mass, damping, stiffness, kpf, kif;
bool is_success = indy.get_force_control_gain(kp, kv, kl2, mass, damping, stiffness, kpf, kif);

if (is_success) {
    std::cout << "Force control gains retrieved successfully." << std::endl;
    std::cout << "KP: ";
    for (const auto& value : kp) std::cout << value << " ";
    std::cout << "\nKV: ";
    for (const auto& value : kv) std::cout << value << " ";
    std::cout << "\nKL2: ";
    for (const auto& value : kl2) std::cout << value << " ";
    std::cout << "\nMass: ";
    for (const auto& value : mass) std::cout << value << " ";
    std::cout << "\nDamping: ";
    for (const auto& value : damping) std::cout << value << " ";
    std::cout << "\nStiffness: ";
    for (const auto& value : stiffness) std::cout << value << " ";
    std::cout << "\nKPF: ";
    for (const auto& value : kpf) std::cout << value << " ";
    std::cout << "\nKIF: ";
    for (const auto& value : kif) std::cout << value << " ";
    std::cout << std::endl;
} else {
    std::cerr << "Failed to retrieve force control gains." << std::endl;
}

Utility Functions

Utility functions provide additional features such as data logging and setting simulation mode.

Method Description
start_log() Starts realtime data logging
end_log() Ends and saves realtime data logging
wait_for_operation_state(op_state) Allows you to wait until the robot reaches a specific operation state.
wait_for_motion_state(motion_state) Allows you to wait until the robot reaches a specific motion state.
set_friction_comp_state(const bool enable) Enable or disable friction compensation
get_friction_comp_state() Retrieve the current state of friction compensation
set_mount_pos(float rot_y, float rot_z) Set the mounting angles for the robot base using Y and Z rotations
get_mount_pos(float &rot_y, float &rot_z) Retrieve the current mounting angles for the robot base
set_brakes(const std::vector<bool>& brake_state_list) Set the brake states for each motor
set_servo_all(const bool enable) Enable or disable all servos
set_servo(const uint32_t index, const bool enable) Enable or disable a specific servo by index
set_endtool_led_dim(const uint32_t led_dim) Set the LED brightness for the end-effector tool
execute_tool(const std::string& name) Execute a tool function by its name
get_el5001(int& status, int& value, int& delta, float& average) Retrieve data from the EL5001 device
get_el5101(int& status, int& value, int& latch, int& delta, float& average) Retrieve data from the EL5101 device
get_brake_control_style(int& style) Retrieve the current brake control style
set_conveyor_name(const std::string& name) Set the name of the conveyor
set_conveyor_encoder(int encoder_type, int64_t channel1, int64_t channel2, int64_t sample_num, float mm_per_tick, float vel_const_mmps, bool reversed) Configure the conveyor encoder settings
set_conveyor_trigger(int trigger_type, int64_t channel, bool detect_rise) Set the conveyor trigger settings
set_conveyor_offset(float offset_mm) Set the offset value for the conveyor (in millimeters)
set_conveyor_starting_pose(const std::vector<float>& jpos, const std::vector<float>& tpos) Set the starting joint and task poses for the conveyor
set_conveyor_terminal_pose(const std::vector<float>& jpos, const std::vector<float>& tpos) Set the terminal joint and task poses for the conveyor
set_compliance_mode(bool enable, const std::vector<float>* stiffness) Enable/disable compliance mode (optional stiffness)
get_compliance_mode(bool &enabled, std::vector<float>& stiffness) Query compliance mode state and stiffness
set_force_mode(const std::map<std::string, float>& force_mode_cfg) Apply force mode configuration (axes, wrench, limits)
get_force_mode(std::map<std::string, float>& force_mode_cfg) Retrieve current force mode configuration
get_transformed_ft_sensor_data(std::array<float,6>& wrench_ref, std::array<float,6>& wrench_tool) Force/torque transformed to reference & tool frame
get_ft_zero(std::array<float,6>& bias) Current FT sensor zero (bias)
get_load_factors(std::vector<float>& load_factors) Load factor estimation data
push_bus_event(uint32_t event_id, const std::vector<int32_t>& args) Publish internal bus event
catch_bus_event(uint32_t event_id, uint32_t timeout_ms, std::vector<int32_t>& args) Wait for internal bus event
get_friction_comp(std::map<std::string,float>& params) Get friction compensation parameters
set_friction_comp(const std::map<std::string,float>& params) Set friction compensation parameters
get_coll_sens_level(int &level) Get collision sensitivity level
set_coll_sens_level(int level) Set collision sensitivity level
get_coll_sens_param(std::map<std::string,float>& params) Get collision threshold parameters
set_coll_sens_param(const std::map<std::string,float>& params) Set collision threshold parameters
get_coll_policy(int &policy, float &sleep_time, float &gravity_time) Get collision policy
set_coll_policy(int policy, float sleep_time, float gravity_time) Set collision policy
get_default_coll_sens_param(std::map<std::string,float>& params) Get default collision thresholds
get_collison_model_margin(float &collision_margin, float &recover_margin) Get collision model margins
set_collison_model_margin(float collision_margin, float recover_margin) Set collision model margins
get_home_pos(std::vector<float>& jpos) Get joint home position
set_home_pos(const std::vector<float>& jpos) Set joint home position
get_ref_frame(std::array<float,6>& fpos) Get reference frame pose
set_ref_frame(const std::array<float,6>& fpos) Set reference frame pose
set_ref_frame_planar(const std::array<float,6>& fpos0, const std::array<float,6>& fpos1, const std::array<float,6>& fpos2) Define planar ref frame via 3 poses
save_reference_frame(const std::vector<std::array<float,6>>& frames, const std::string& default_name) Save reference frames
load_reference_frame(std::vector<std::array<float,6>>& frames, std::string &default_name) Load reference frames
get_tool_frame_list(std::map<std::string,std::array<float,6>>& frames) Get list of tool frames
set_tool_frame_list(const std::map<std::string,std::array<float,6>>& frames) Set list of tool frames
get_tool_list(std::map<std::string,std::map<std::string,float>>& tool_props) Get list of tool properties
set_tool_list(const std::map<std::string,std::map<std::string,float>>& tool_props) Set list of tool properties
get_tool_property(std::map<std::string,float>& props) Get active tool physical properties
set_tool_property(const std::map<std::string,float>& props) Set active tool physical properties
get_pack_pos(std::vector<float>& jpos) Get joint pack position
get_path_config(std::map<std::string,std::string>& info) Get config path information
set_locked_joint(uint32_t index) Lock a joint for kinematics
set_tool_link(uint32_t index) Set tool link index
get_custom_pos_list(std::map<std::string,std::array<float,6>>& poses) Get custom pose list
set_custom_pos_list(const std::map<std::string,std::array<float,6>>& poses) Set custom pose list
get_tool_shape_list(std::map<std::string,std::vector<float>>& shapes) Get tool shape models
set_tool_shape_list(const std::map<std::string,std::vector<float>>& shapes) Set tool shape models
get_environment_list(std::map<std::string,std::vector<float>>& envs) Get environment models
set_environment_list(const std::map<std::string,std::vector<float>>& envs) Set environment models
get_sensorless_params(std::map<std::string,float>& params) Get sensorless collision parameters
set_sensorless_params(const std::map<std::string,float>& params) Set sensorless collision parameters
get_compliance_control_joint_gain(std::map<std::string,float>& gains) Get compliance control joint gains
set_compliance_control_joint_gain(const std::map<std::string,float>& gains) Set compliance control joint gains
get_on_start_program_config(std::map<std::string,std::string>& cfg) Get on-start program config
set_on_start_program_config(const std::map<std::string,std::string>& cfg) Set on-start program config
get_auto_servo_off(bool &enable, float &time_s) Get auto servo-off configuration
set_auto_servo_off(bool enable, float time_s) Set auto servo-off configuration
get_teleop_params(float &smooth_factor, float &cutoff_freq, float &error_gain) Get teleoperation filtering params
set_teleop_params(float smooth_factor, float cutoff_freq, float error_gain) Set teleoperation filtering params
get_kinematics_params(std::map<std::string,float>& params) Snapshot of kinematics parameters
get_io_data(std::map<std::string,int>& io) Snapshot of all IO states
set_safety_limits(const std::map<std::string,float>& limits) Set safety power/force/speed limits
get_safety_limits(std::map<std::string,float>& limits) Get safety power/force/speed limits
set_safety_stop_config(const std::map<std::string,int>& cfg) Set safety stop configuration
get_safety_stop_config(std::map<std::string,int>& cfg) Get safety stop configuration
set_reduced_speed(float speed) Set reduced mode speed
get_reduced_speed(float &speed) Get reduced mode speed
get_reduced_ratio(float &ratio) Get reduced mode speed ratio
request_safety_function(int id, int state) Request safety function state change
get_safety_function_state(std::map<std::string,int>& states) Get safety function states
get_safety_control_data(std::map<std::string,float>& data) Low-level safety diagnostics
set_auto_mode(bool on) Set auto mode state
check_auto_mode(bool &on) Check auto mode state
check_reduced_mode(bool &active) Check reduced mode active
get_control_info(std::map<std::string,std::string>& info) Controller meta information
check_aproach_retract_valid(const std::map<std::string,float>& params, bool &valid) Validate approach/retract sequence
get_pallet_point_list(const std::map<std::string,float>& cfg, std::vector<std::array<float,6>>& points) Generate palletizing points grid
move_recover_joint(const std::vector<float>& jtarget, int base_type) Recovery joint move used after collision/violation

Example for start_log() and end_log()

bool is_success = indy.start_log();
if (is_success) {
    std::cout << "Started data logging." << std::endl;
}

// After some time...
is_success = indy.end_log();
if (is_success) {
    std::cout << "Ended data logging and saved data." << std::endl;
}

These two functions are used for realtime data logging. Upon calling start_log(), the robot's data is stored in memory. After a certain period, calling end_log() stops the memory logging and writes it to a file within STEP. The realtime data logging file can be found at the following path:

/home/user/release/IndyDeployments/RTlog/RTLog.csv

For more detailed usage and advanced features, please refer to C++ client at Neuromeka Github repository.

Example for wait_for_operation_state(op_state)

For example, if you want to wait until the robot is in the MOVING state, you can call:

bool is_success = indy.wait_for_operation_state(OpState::OP_MOVING);
if (is_success) {
    std::cout << "The robot has reached the target position." << std::endl;
} else {
    std::cerr << "Failed to wait for the robot to reach the target position." << std::endl;
}

Example for wait_for_motion_state(motion_state)

  • motion_state:
    • is_target_reached
    • is_in_motion
    • is_pausing
    • is_stopping
    • has_motion

For instance, if you want to wait until the robot's motion is complete, indicated by the state 'is_target_reached', you can call:

bool is_success = indy.wait_for_motion_state("is_target_reached");
if (is_success) {
    std::cout << "The robot has reached the target position." << std::endl;
} else {
    std::cerr << "Failed to wait for the robot to reach the target position." << std::endl;
}

Example for set_friction_comp_state()

bool is_success = indy.set_friction_comp_state(true);

if (is_success) {
    std::cout << "Friction compensation enabled successfully." << std::endl;
} else {
    std::cerr << "Failed to enable friction compensation." << std::endl;
}

Example for get_friction_comp_state()

bool is_enabled = indy.get_friction_comp_state();

if (is_enabled) {
    std::cout << "Friction compensation is currently enabled." << std::endl;
} else {
    std::cout << "Friction compensation is currently disabled." << std::endl;
}

Example for set_mount_pos(float rot_y, float rot_z)

Sets the mounting angles for the robot base using rotation angles around the Y and Z axes.

float rotation_y = 10.0f; // Rotation around Y-axis in degrees
float rotation_z = 5.0f;  // Rotation around Z-axis in degrees

bool is_success = indy.set_mount_pos(rotation_y, rotation_z);

if (is_success) {
    std::cout << "Mounting angles set successfully." << std::endl;
} else {
    std::cerr << "Failed to set mounting angles." << std::endl;
}

Example for get_mount_pos(float &rot_y, float &rot_z)

Retrieves the current mounting angles for the robot base.

float rotation_y, rotation_z;
bool is_success = indy.get_mount_pos(rotation_y, rotation_z);

if (is_success) {
    std::cout << "Retrieved mounting angles:" << std::endl;
    std::cout << "Rotation Y: " << rotation_y << " degrees" << std::endl;
    std::cout << "Rotation Z: " << rotation_z << " degrees" << std::endl;
} else {
    std::cerr << "Failed to retrieve mounting angles." << std::endl;
}

Example for set_brakes(const std::vector<bool>& brake_state_list)

Sets the brake states for each motor.

std::vector<bool> brake_states = {true, false, true, true, false, false};

bool is_success = indy.set_brakes(brake_states);

if (is_success) {
    std::cout << "Brakes set successfully." << std::endl;
} else {
    std::cerr << "Failed to set brakes." << std::endl;
}

Example for set_servo_all(const bool enable)

bool enable_servos = true;
bool is_success = indy.set_servo_all(enable_servos);

if (is_success) {
    std::cout << "All servos enabled successfully." << std::endl;
} else {
    std::cerr << "Failed to enable all servos." << std::endl;
}

Example for set_servo(const uint32_t index, const bool enable)

Enables or disables a specific servo by its index.

uint32_t servo_index = 3;
bool enable_servo = true;

bool is_success = indy.set_servo(servo_index, enable_servo);

if (is_success) {
    std::cout << "Servo " << servo_index << " enabled successfully." << std::endl;
} else {
    std::cerr << "Failed to enable servo " << servo_index << "." << std::endl;
}

Example for set_endtool_led_dim(const uint32_t led_dim)

Sets the LED brightness for the end-effector tool.

uint32_t led_brightness = ...;

bool is_success = indy.set_endtool_led_dim(led_brightness);

if (is_success) {
    std::cout << "End-effector LED brightness set to " << led_brightness << "." << std::endl;
} else {
    std::cerr << "Failed to set LED brightness." << std::endl;
}

Example for execute_tool(const std::string& name)

std::string tool_name = "GripperOpen";
bool is_success = indy.execute_tool(tool_name);

if (is_success) {
    std::cout << "Tool function '" << tool_name << "' executed successfully." << std::endl;
} else {
    std::cerr << "Failed to execute tool function '" << tool_name << "'." << std::endl;
}

Example for get_el5001(int& status, int& value, int& delta, float& average)

Retrieves data from the EL5001 device.

int status, value, delta;
float average;

bool is_success = indy.get_el5001(status, value, delta, average);

if (is_success) {
    std::cout << "EL5001 Data:" << std::endl;
    std::cout << "Status: " << status << ", Value: " << value << ", Delta: " << delta << ", Average: " << average << std::endl;
} else {
    std::cerr << "Failed to retrieve data from EL5001 device." << std::endl;
}

Example for get_el5101(int& status, int& value, int& latch, int& delta, float& average)

Retrieves data from the EL5101 device.

int status, value, latch, delta;
float average;

bool is_success = indy.get_el5101(status, value, latch, delta, average);

if (is_success) {
    std::cout << "EL5101 Data:" << std::endl;
    std::cout << "Status: " << status << ", Value: " << value << ", Latch: " << latch << ", Delta: " << delta << ", Average: " << average << std::endl;
} else {
    std::cerr << "Failed to retrieve data from EL5101 device." << std::endl;
}

Example for get_brake_control_style(int& style)

Retrieves the current brake control style.

int style;

bool is_success = indy.get_brake_control_style(style);

if (is_success) {
    std::cout << "Brake control style: " << style << std::endl;
} else {
    std::cerr << "Failed to retrieve brake control style." << std::endl;
}

Example for set_conveyor_name(const std::string& name)

Sets the name of the conveyor.

std::string conveyor_name = "MainConveyor";

bool is_success = indy.set_conveyor_name(conveyor_name);

if (is_success) {
    std::cout << "Conveyor name set to '" << conveyor_name << "'." << std::endl;
} else {
    std::cerr << "Failed to set conveyor name." << std::endl;
}

Example for set_conveyor_encoder(...)

int encoder_type = 1; // Type of encoder
int64_t channel1 = 0, channel2 = 1, sample_num = 100;
float mm_per_tick = 0.01, vel_const_mmps = 500.0;
bool reversed = false;

bool is_success = indy.set_conveyor_encoder(encoder_type, channel1, channel2, sample_num, mm_per_tick, vel_const_mmps, reversed);

if (is_success) {
    std::cout << "Conveyor encoder configured successfully." << std::endl;
} else {
    std::cerr << "Failed to configure conveyor encoder." << std::endl;
}

Example for set_conveyor_trigger(int trigger_type, int64_t channel, bool detect_rise)

int trigger_type = 2; // Trigger type
int64_t channel = 0;
bool detect_rise = true;

bool is_success = indy.set_conveyor_trigger(trigger_type, channel, detect_rise);

if (is_success) {
    std::cout << "Conveyor trigger set successfully." << std::endl;
} else {
    std::cerr << "Failed to set conveyor trigger." << std::endl;
}

Example for set_conveyor_offset(float offset_mm)

Sets the offset value for the conveyor.

float offset = ...;

bool is_success = indy.set_conveyor_offset(offset);

if (is_success) {
    std::cout << "Conveyor offset set to " << offset << " mm." << std::endl;
} else {
    std::cerr << "Failed to set conveyor offset." << std::endl;
}

Example for set_conveyor_starting_pose(const std::vector<float>& jpos, const std::vector<float>& tpos)

Sets the starting joint and task poses for the conveyor.

std::vector<float> joint_positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
std::vector<float> task_positions = {};

bool is_success = indy.set_conveyor_starting_pose(joint_positions, task_positions);

if (is_success) {
    std::cout << "Conveyor starting pose set successfully." << std::endl;
} else {
    std::cerr << "Failed to set conveyor starting pose." << std::endl;
}

Example for set_conveyor_terminal_pose(const std::vector<float>& jpos, const std::vector<float>& tpos)

Sets the terminal joint and task poses for the conveyor.

std::vector<float> joint_positions = {0.0, 0.0, -90.0, 0.0, -90.0, 0.0};
std::vector<float> task_positions = {};

bool is_success = indy.set_conveyor_terminal_pose(joint_positions, task_positions);

if (is_success) {
    std::cout << "Conveyor terminal pose set successfully." << std::endl;
} else {
    std::cerr << "Failed to set conveyor terminal pose." << std::endl;
}

Example for set_compliance_mode(), get_compliance_mode()

Nrmk::IndyFramework::ComplianceMode mode;
mode.set_enable(true);
int stiffness_vals[6] = {1000,1000,800,50,50,50};
for (int v : stiffness_vals) mode.add_stiffness(v);
bool ok = indy.set_compliance_mode(mode);
std::cout << "set_compliance_mode: " << (ok ? "OK" : "FAIL") << "\n";

Nrmk::IndyFramework::ComplianceMode mode_read;
ok = indy.get_compliance_mode(mode_read);
std::cout << "get_compliance_mode: " << (ok ? "OK" : "FAIL");
if (ok) {
    std::cout << " enable=" << mode_read.enable()
              << " stiffness_size=" << mode_read.stiffness_size()
              << " [";
    for (int i = 0; i < mode_read.stiffness_size(); ++i) {
        if (i) std::cout << ",";
        std::cout << mode_read.stiffness(i);
    }
    std::cout << "]";
}
std::cout << "\n";
set_compliance_mode: OK
get_compliance_mode:
  status: OK
  enable: 0
  stiffness: []

Example for get_transformed_ft_sensor_data()

Nrmk::IndyFramework::TransformedFTSensorData tft;
bool ok = indy.get_transformed_ft_sensor_data(tft);
std::cout << "get_transformed_ft_sensor_data: " << (ok?"OK":"FAIL");
if (ok) {
  std::cout << " Fx="<<tft.ft_fx()<<" Fy="<<tft.ft_fy()<<" Fz="<<tft.ft_fz()
            << " Tx="<<tft.ft_tx()<<" Ty="<<tft.ft_ty()<<" Tz="<<tft.ft_tz();
}
std::cout << "\n";
get_transformed_ft_sensor_data:
  status: OK
  ft: {Fx: 0, Fy: 0, Fz: 0, Tx: 0, Ty: 0, Tz: 0}

Example for get_ft_zero()

bool ft_zero_ok = indy.get_ft_zero();
std::cout << "get_ft_zero: " << (ft_zero_ok ? "OK" : "FAIL") << "\n";
get_ft_zero: OK

Example for get_load_factors()

Nrmk::IndyFramework::GetLoadFactorsRes load;
bool ok = indy.get_load_factors(load);
std::cout << "get_load_factors: " << (ok?"OK":"FAIL");
if (ok) {
  std::cout << " percents=[";
  for (int i=0;i<load.percents_size();++i){ if(i) std::cout<<","; std::cout<<load.percents(i); }
  std::cout << "] torques=[";
  for (int i=0;i<load.torques_size();++i){ if(i) std::cout<<","; std::cout<<load.torques(i); }
  std::cout << "]";
}
std::cout << "\n";
get_load_factors:
  status: OK
  percents: [0, 0, 0, 0, 0, 0]
  torques: [-2.57318e-21, 0.00467472, 0.00420347, -7.87171e-16, 0.00145352, -2.36064e-19]

Example for push_bus_event(),catch_bus_event()

const uint32_t EVENT_ID_PART_ARRIVED = 10;
Nrmk::IndyFramework::BusEvent bevent;
bevent.set_event_id(EVENT_ID_PART_ARRIVED);
bevent.set_text_data("Part Arrived");
bool ok = indy.push_bus_event(bevent);
std::cout << "push_bus_event(10): " << (ok?"OK":"FAIL") << "\n";

Nrmk::IndyFramework::CatchBusEventReq req;
req.set_event_id(EVENT_ID_PART_ARRIVED);
req.set_timeout(5.0f);
Nrmk::IndyFramework::BusEvent caught;
ok = indy.catch_bus_event(req, caught);
std::cout << "catch_bus_event(10): " << (ok?"OK":"FAIL");
if (ok) {
  std::cout << " event_id=" << caught.event_id()
            << " text='" << caught.text_data() << "'";
}
std::cout << "\n";
push_bus_event:
  status: OK
catch_bus_event:
  status: OK
  event_id: 0
  text: ""

Example for get_friction_comp(),set_friction_comp()

Nrmk::IndyFramework::FrictionCompSet fric;
bool ok = indy.get_friction_comp(fric);
std::cout << "get_friction_comp: " << (ok?"OK":"FAIL");
if (ok) {
  std::cout << " control_enable="<<fric.control_comp_enable()
            << " teaching_enable="<<fric.teaching_comp_enable()
            << " control_levels="<<fric.control_comp_levels_size()
            << " teaching_levels="<<fric.teaching_comp_levels_size();
}
std::cout << "\n";
if (ok) {
  bool set_ok = indy.set_friction_comp(fric);
  std::cout << "set_friction_comp(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_friction_comp:
  status: OK
  control_enable: 0
  teaching_enable: 0
  control_levels: 6
  teaching_levels: 6
set_friction_comp: OK

Example for get_coll_sens_level(),set_coll_sens_level()

unsigned int level=0;
bool ok = indy.get_coll_sens_level(level);
std::cout << "get_coll_sens_level: " << (ok?"OK":"FAIL");
if (ok) std::cout << " level=" << level;
std::cout << "\n";
if (ok) {
  bool set_ok = indy.set_coll_sens_level(level);
  std::cout << "set_coll_sens_level(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_coll_sens_level:
  status: OK
  level: 3
set_coll_sens_level: OK

Example for get_coll_sens_param(),set_coll_sens_param(),get_default_coll_sens_param()

Nrmk::IndyFramework::CollisionThresholds thr;
bool ok = indy.get_coll_sens_param(thr);
std::cout << "get_coll_sens_param: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " j_torque_bases=[";
    for (int i=0;i<thr.j_torque_bases_size();++i) { if (i) std::cout << ","; std::cout << thr.j_torque_bases(i); }
    std::cout << "]";
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_coll_sens_param(thr);
    std::cout << "set_coll_sens_param(echo): " << (set_ok?"OK":"FAIL") << "\n";
}

Nrmk::IndyFramework::CollisionThresholds def_thr;
ok = indy.get_default_coll_sens_param(def_thr);
std::cout << "get_default_coll_sens_param: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " j_torque_bases=[";
    for (int i=0;i<def_thr.j_torque_bases_size();++i) { if (i) std::cout << ","; std::cout << def_thr.j_torque_bases(i); }
    std::cout << "]";
}
std::cout << "\n";
get_coll_sens_param:
  status: OK
  j_torque_bases: [28.6619, 39.3071, 12.5298, 500, 3.2722, 3.94439]
set_coll_sens_param: OK
get_default_coll_sens_param:
  status: OK
  j_torque_bases: [36.5185, 35.8493, 13.2479, 5.48176, 3.25222, 2.24444]

Example for get_coll_policy(),set_coll_policy()

Nrmk::IndyFramework::CollisionPolicy policy;
bool ok = indy.get_coll_policy(policy);
std::cout << "get_coll_policy: " << (ok?"OK":"FAIL");
if (ok) std::cout << " policy="<<policy.policy()
                  << " sleep="<<policy.sleep_time()
                  << " gravity="<<policy.gravity_time();
std::cout << "\n";
if (ok) {
  bool set_ok = indy.set_coll_policy(policy);
  std::cout << "set_coll_policy(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_coll_policy:
  status: OK
  policy: 1
  sleep_time: 2.0
  gravity_time: 1.0
set_coll_policy: OK

Example for get_collison_model_margin(),set_collison_model_margin()

Nrmk::IndyFramework::CollisionModelMargin margin;
bool ok = indy.get_collison_model_margin(margin);
std::cout << "get_collison_model_margin: " << (ok?"OK":"FAIL");
if (ok) std::cout << " collision_margin="<<margin.collision_margin()
                  << " recover_margin="<<margin.recover_margin();
std::cout << "\n";
if (ok) {
  bool set_ok = indy.set_collison_model_margin(margin);
  std::cout << "set_collison_model_margin(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_collison_model_margin:
  status: OK
  collision_margin: 0.01
  recover_margin: 0.0
set_collison_model_margin: OK

Example for get_home_pos(),set_home_pos()

Nrmk::IndyFramework::JointPos home;
bool ok = indy.get_home_pos(home);
std::cout << "get_home_pos: " << (ok?"OK":"FAIL");
if (ok) std::cout << " size=" << home.jpos_size();
std::cout << "\n";

if (ok) {
  Nrmk::IndyFramework::JointPos new_home;
  float vals[6] = {0,0,-90,0,-90,0};
  for (float v : vals) new_home.add_jpos(v);
  bool set_ok = indy.set_home_pos(new_home);
  std::cout << "set_home_pos: " << (set_ok?"OK":"FAIL") << "\n";
}
get_home_pos:
  status: OK
  size: 6
set_home_pos: OK

Example for get_ref_frame(),set_ref_frame()

std::array<float,6> fpos{};
bool ok = indy.get_ref_frame(fpos);
std::cout << "get_ref_frame: " << (ok?"OK":"FAIL");
if (ok) {
  std::cout << " fpos=["<<fpos[0]<<","<<fpos[1]<<","<<fpos[2]<<","<<fpos[3]<<","<<fpos[4]<<","<<fpos[5]<<"]";
}
std::cout << "\n";
if (ok) {
  bool set_ok = indy.set_ref_frame(fpos);
  std::cout << "set_ref_frame(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_ref_frame:
  status: OK
  fpos: [0, 0, 0, 0, 0, 0]
set_ref_frame: OK

Example for get_tool_frame_list(),set_tool_frame_list()

Nrmk::IndyFramework::ToolFrameList tfl;
bool ok = indy.get_tool_frame_list(tfl);
std::cout << "get_tool_frame_list: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " frames=[";
    for (int i=0;i<tfl.tool_frames_size();++i) {
        if (i) std::cout << ";";
        const auto& tf = tfl.tool_frames(i);
        std::cout << tf.name() << ":[";
        for (int j=0;j<tf.tpos_size();++j) { if (j) std::cout << ","; std::cout << tf.tpos(j); }
        std::cout << "]";
    }
    std::cout << "] default=" << tfl.default_name();
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_tool_frame_list(tfl);
    std::cout << "set_tool_frame_list(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_tool_frame_list:
  status: OK
  frames:
    - name: tool_0
      tpos: [0, 0, 0, 0, 0, 0]
  default: tool_0
set_tool_frame_list: OK

Example for get_tool_list(),set_tool_list()

Nrmk::IndyFramework::ToolList tlist;
bool ok = indy.get_tool_list(tlist);
std::cout << "get_tool_list: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " tools=[";
    for (int i=0;i<tlist.tools_size();++i) {
        if (i) std::cout << ";";
        std::cout << tlist.tools(i).name();
    }
    std::cout << "]";
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_tool_list(tlist);
    std::cout << "set_tool_list(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_tool_list:
  status: OK
  tools: [gripper_Init, gripper_Reset, gripper_on, gripper_off]
set_tool_list: OK

Example for get_tool_property(),set_tool_property()

Nrmk::IndyFramework::ToolProperties tp;
bool ok = indy.get_tool_property(tp);
std::cout << "get_tool_property: " << (ok?"OK":"FAIL");
if (ok) {
  std::cout << " mass="<<tp.mass();
  std::cout << " com=[";
  for (int i=0;i<tp.center_of_mass_size();++i){ if(i) std::cout<<","; std::cout<<tp.center_of_mass(i); }
  std::cout << "] inertia=[";
  for (int i=0;i<tp.inertia_size();++i){ if(i) std::cout<<","; std::cout<<tp.inertia(i); }
  std::cout << "]";
}
std::cout << "\n";
if (ok) {
  bool set_ok = indy.set_tool_property(tp);
  std::cout << "set_tool_property(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_tool_property:
  status: OK
  mass: 0
  com: [0, 0, 0]
  inertia: [0, 0, 0, 0, 0, 0]
set_tool_property: OK

Example for get_pack_pos()

std::vector<float> pack_jpos;
bool ok = indy.get_pack_pos(pack_jpos);
std::cout << "get_pack_pos: " << (ok?"OK":"FAIL");
if (ok) std::cout << " jpos_size=" << pack_jpos.size();
std::cout << "\n";
get_pack_pos:
  status: OK
  jpos_size: 6

Example for get_pack_pos()

std::vector<float> pack_jpos;
bool ok = indy.get_pack_pos(pack_jpos);
std::cout << "get_pack_pos: " << (ok?"OK":"FAIL");
if (ok) std::cout << " jpos_size=" << pack_jpos.size();
std::cout << "\n";
get_pack_pos:
  status: OK
  jpos_size: 6

Example for get_path_config()

Nrmk::IndyFramework::PathConfig pconf;
bool ok = indy.get_path_config(pconf);
std::cout << "get_path_config: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " config_path='" << pconf.config_path() << "' safety_path_list=[";
    for (int i=0;i<pconf.safety_path_list_size();++i) {
        if (i) std::cout << ";";
        std::cout << pconf.safety_path_list(i);
    }
    std::cout << "]";
}
std::cout << "\n";
get_path_config:
  status: OK
  config_path: "/home/user/dev/runtest/Release/IndyDeployment/../IndyConfigurations/Cobot/Configs"
  safety_path_list:
    - "/home/user/dev/runtest/Release/IndyDeployment/../IndyConfigurations/Cobot/Params/CollisionGain.json"
    - "/home/user/dev/runtest/Release/IndyDeployment/../IndyConfigurations/Cobot/Params/ControlGain.json"
    - "/home/user/dev/runtest/Release/IndyDeployment/../IndyConfigurations/Cobot/Configs/SafetyConfig.json"

bool lock_ok = indy.set_locked_joint(5);
std::cout << "set_locked_joint(5): " << (lock_ok?"OK":"FAIL") << "\n";
bool tool_link_ok = indy.set_tool_link(6);
std::cout << "set_tool_link(6): " << (tool_link_ok?"OK":"FAIL") << "\n";
set_locked_joint:
    status: OK
    joint_index: 5
set_tool_link:
    status: OK
    link_index: 6

Example for get_custom_pos_list(), set_custom_pos_list()

Nrmk::IndyFramework::CustomPosList cplist;
bool ok = indy.get_custom_pos_list(cplist);
std::cout << "get_custom_pos_list: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " custom_pos=[";
    for (int i=0;i<cplist.custom_pos_size();++i) {
        if (i) std::cout << ";";
        const auto& cp = cplist.custom_pos(i);
        std::cout << cp.name() << ":[";
        for (int j=0;j<cp.jpos_size();++j) { if (j) std::cout << ","; std::cout << cp.jpos(j); }
        std::cout << "]";
    }
    std::cout << "]";
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_custom_pos_list(cplist);
    std::cout << "set_custom_pos_list(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_custom_pos_list:
  status: OK
  custom_pos:
    - name: test_1
      jpos: [5.62114, -13.5501, -62.8307, -8.22606, -103.113, -0.0466959]
    - name: test_2
      jpos: [32.5509, -0.193804, -91.9896, -6.77046, -91.9321, -3.7074]
set_custom_pos_list: OK

Example for get_tool_shape_list(), set_tool_shape_list()

Nrmk::IndyFramework::ToolShapeList tshapes;
bool ok = indy.get_tool_shape_list(tshapes);
std::cout << "get_tool_shape_list: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " geometries=[";
    for (int i=0;i<tshapes.geometries_size();++i) {
        if (i) std::cout << ";";
        const auto& g = tshapes.geometries(i);
        std::cout << g.name() << "(shapes=" << g.shapes_size() << ")";
    }
    std::cout << "]";
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_tool_shape_list(tshapes);
    std::cout << "set_tool_shape_list(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_tool_shape_list:
  status: OK
  geometries: ["tool_0(shapes=0)"]
set_tool_shape_list: OK

Example for get_environment_list(), set_environment_list()

Nrmk::IndyFramework::EnvironmentList envs;
bool ok = indy.get_environment_list(envs);
std::cout << "get_environment_list: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " envs=[";
    for (int i=0;i<envs.environments_size();++i) {
        if (i) std::cout << ";";
        const auto& e = envs.environments(i);
        std::cout << e.name() << "(zones=" << e.zones_size() << ")";
    }
    std::cout << "] default=" << envs.default_name();
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_environment_list(envs);
    std::cout << "set_environment_list(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_environment_list:
  status: OK
  envs: ["env_0(zones=0)"]
  default: env_0
set_environment_list: OK

Example for get_sensorless_params(), set_sensorless_params()

Nrmk::IndyFramework::SensorlessParams sparams;
bool ok = indy.get_sensorless_params(sparams);
std::cout << "get_sensorless_params: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " tau_bound=[";
    for (int i=0;i<sparams.tau_bound_size();++i) { if (i) std::cout << ","; std::cout << sparams.tau_bound(i); }
    std::cout << "]";
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_sensorless_params(sparams);
    std::cout << "set_sensorless_params(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_sensorless_params:
  status: OK
  tau_bound: [1, 1, 1, 1, 1, 1]
set_sensorless_params: OK

Example for get_compliance_control_joint_gain(), set_compliance_control_joint_gain()

Nrmk::IndyFramework::ComplianceGainSet gains;
ok = indy.get_compliance_control_joint_gain(gains);
std::cout << "get_compliance_control_joint_gain: " << (ok?"OK":"FAIL")
                    << (ok ? " kp_size=" + std::to_string(gains.kp_size()) : "") << "\n";
if (ok) {
    bool set_ok = indy.set_compliance_control_joint_gain(gains);
    std::cout << "set_compliance_control_joint_gain(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_compliance_control_joint_gain:
    status: OK
    kp_size: 6
set_compliance_control_joint_gain: OK

Example for get_on_start_program_config(), set_on_start_program_config()

Nrmk::IndyFramework::OnStartProgramConfig onstart;
ok = indy.get_on_start_program_config(onstart);
std::cout << "get_on_start_program_config: " << (ok?"OK":"FAIL")
                    << (ok ? " index=" + std::to_string(onstart.index()) : "") << "\n";
if (ok) {
    bool set_ok = indy.set_on_start_program_config(onstart);
    std::cout << "set_on_start_program_config(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_on_start_program_config:
    status: OK
    index: 1
set_on_start_program_config: OK

Example for get_auto_servo_off(),set_auto_servo_off()

Nrmk::IndyFramework::AutoServoOffConfig as;
bool ok = indy.get_auto_servo_off(as);
std::cout << "get_auto_servo_off: " << (ok?"OK":"FAIL");
if (ok) std::cout << " enable="<<as.enable() << " time="<<as.time();
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_auto_servo_off(as);
    std::cout << "set_auto_servo_off(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_auto_servo_off:
    status: OK
    enable: 1
    time: 600
set_auto_servo_off: OK

Example for get_teleop_params(),set_teleop_params()

Nrmk::IndyFramework::TeleOpParams tele;
ok = indy.get_teleop_params(tele);
std::cout << "get_teleop_params: " << (ok?"OK":"FAIL");
if (ok) std::cout << " smooth_factor="<<tele.smooth_factor()
                                    << " cutoff_freq="<<tele.cutoff_freq()
                                    << " error_gain="<<tele.error_gain();
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_teleop_params(tele);
    std::cout << "set_teleop_params(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_teleop_params:
    status: OK
    smooth_factor: 0.6
    cutoff_freq: 1000
    error_gain: 1.2
set_teleop_params: OK

Example for get_kinematics_params()

Nrmk::IndyFramework::KinematicsParams kin;
bool ok = indy.get_kinematics_params(kin);
std::cout << "get_kinematics_params: " << (ok?"OK":"FAIL") << "\n";
if (ok) {
    for (int i=0;i<kin.mdh_size();++i) {
        const auto& l = kin.mdh(i);
        std::cout << "  mdh["<<i<<"] idx="<<l.index()
                  << " parent="<<l.parent()
                  << " a="<<l.a() << " d0="<<l.d0()
                  << " alpha="<<l.alpha() << " theta0="<<l.theta0()
                  << " type="<<l.type() << "\n";
    }
}
get_kinematics_params:
  status: OK
  mdh:
    - {idx: 0, parent: -1, a: 0,   d0: 300,  alpha: 0,   theta0: 0,   type: 0}
    - {idx: 1, parent: 0,  a: 0,   d0: 0,    alpha: 90,  theta0: 90,  type: 0}
    - {idx: 2, parent: 1,  a: 450, d0: 0,    alpha: 0,   theta0: 90,  type: 0}
    - {idx: 3, parent: 2,  a: 0,   d0: 350,  alpha: 90,  theta0: 180, type: 0}
    - {idx: 4, parent: 3,  a: 0,   d0: 186.5,alpha: 90,  theta0: 0,   type: 0}
    - {idx: 5, parent: 4,  a: 0,   d0: 228,  alpha: -90, theta0: 0,   type: 0}

Example for get_io_data()

Nrmk::IndyFramework::IOData io;
bool ok = indy.get_io_data(io);
std::cout << "get_io_data: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " di_count="<<io.di_size()
              << " do_count="<<io.do__size()
              << " ai_count="<<io.ai_size()
              << " ao_count="<<io.ao_size();
    if (io.di_size()>0) {
        int show = std::min(io.di_size(), 8);
        std::cout << " di_sample=[";
        for (int i=0;i<show;++i) {
            if (i) std::cout << ";";
            std::cout << io.di(i).address() << ":" << io.di(i).state();
        }
        std::cout << "]";
        if (io.di_size()>show) std::cout << " +"<<(io.di_size()-show);
    }
}
std::cout << "\n";
get_io_data:
  status: OK
  di_count: 32
  do_count: 32
  ai_count: 8
  ao_count: 8
  di_sample: ["0:2","1:2","2:2","3:2","4:2","5:2","6:2","7:2"]
  di_remaining: 24

Example for get_safety_limits(),set_safety_limits()

Nrmk::IndyFramework::SafetyLimits sl;
ok = indy.get_safety_limits(sl);
std::cout << "get_safety_limits: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << std::fixed << std::setprecision(1)
        << " power="<<sl.power_limit()<<" ("<<sl.power_limit_ratio()<<"%)"
        << " tcp_force="<<sl.tcp_force_limit()<<" ("<<sl.tcp_force_limit_ratio()<<"%)"
        << " tcp_speed="<<sl.tcp_speed_limit()<<" ("<<sl.tcp_speed_limit_ratio()<<"%)";
    std::cout << " j_upper=[";
    for (int i=0;i<sl.joint_upper_limits_size();++i){ if(i) std::cout<<","; std::cout<<sl.joint_upper_limits(i); }
    std::cout << "] j_lower=[";
    for (int i=0;i<sl.joint_lower_limits_size();++i){ if(i) std::cout<<","; std::cout<<sl.joint_lower_limits(i); }
    std::cout << "]";
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_safety_limits(sl);
    std::cout << "set_safety_limits(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_safety_limits:
    status: OK
    power_limit: {value: 1000.0, ratio_percent: 100.0}
    tcp_force_limit: {value: 50.0, ratio_percent: 100.0}
    tcp_speed_limit: {value: 500.0, ratio_percent: 100.0}
    joint_upper_limits: [175.0, 175.0, 175.0, 175.0, 175.0, 215.0]
    joint_lower_limits: [-175.0, -175.0, -175.0, -175.0, -175.0, -215.0]
set_safety_limits: OK

Example for get_safety_stop_config(),set_safety_stop_config()

Nrmk::IndyFramework::SafetyStopConfig ssc;
ok = indy.get_safety_stop_config(ssc);
std::cout << "get_safety_stop_config: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " jpos_cat="<<ssc.joint_position_limit_stop_cat()
                        << " jvel_cat="<<ssc.joint_speed_limit_stop_cat()
                        << " jtau_cat="<<ssc.joint_torque_limit_stop_cat()
                        << " tvel_cat="<<ssc.tcp_speed_limit_stop_cat()
                        << " tforce_cat="<<ssc.tcp_force_limit_stop_cat()
                        << " power_cat="<<ssc.power_limit_stop_cat()
                        << " safegd_stop_cat_size="<<ssc.safegd_stop_cat_size();
    if (ssc.safegd_stop_cat_size()>0) {
        std::cout << " safegd_stop_cat=[";
        for(int i=0;i<ssc.safegd_stop_cat_size();++i){ if(i) std::cout<<","; std::cout<<ssc.safegd_stop_cat(i); }
        std::cout << "]";
    }
    if (ssc.safegd_type_size()>0) {
        std::cout << " safegd_type=[";
        for(int i=0;i<ssc.safegd_type_size();++i){ if(i) std::cout<<","; std::cout<<ssc.safegd_type(i); }
        std::cout << "]";
    }
}
std::cout << "\n";
if (ok) {
    bool set_ok = indy.set_safety_stop_config(ssc);
    std::cout << "set_safety_stop_config(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_safety_stop_config:
    status: OK
    jpos_cat: 0
    jvel_cat: 2
    jtau_cat: 2
    tvel_cat: 2
    tforce_cat: 2
    power_cat: 2
    safegd_stop_cat: [2, 2]
    safegd_type: [1, 1]
set_safety_stop_config: OK

Example for get_reduced_speed(),set_reduced_speed(),get_reduced_ratio()

float rspeed=0.f; ok = indy.get_reduced_speed(rspeed);
std::cout << "get_reduced_speed: " << (ok?"OK":"FAIL");
if (ok) std::cout << " speed="<<rspeed;
std::cout << "\n";

float rratio=0.f; ok = indy.get_reduced_ratio(rratio);
std::cout << "get_reduced_ratio: " << (ok?"OK":"FAIL");
if (ok) std::cout << " ratio="<<rratio;
std::cout << "\n";

if (ok) {
    bool set_ok = indy.set_reduced_speed(rspeed);
    std::cout << "set_reduced_speed(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_reduced_speed:
    status: OK
    speed: 250.0
get_reduced_ratio:
    status: OK
    ratio: 25.0
set_reduced_speed: OK

Example for get_safety_function_state(),request_safety_function()

Nrmk::IndyFramework::SafetyFunctionState sfs;
ok = indy.get_safety_function_state(sfs);
std::cout << "get_safety_function_state: " << (ok?"OK":"FAIL");
if (ok) std::cout << " id="<<sfs.id()<<" state="<<sfs.state();
std::cout << "\n";

if (ok) {
    Nrmk::IndyFramework::SafetyFunctionState req;
    req.set_id(sfs.id());
    req.set_state(sfs.state());
    bool set_ok = indy.request_safety_function(req);
    std::cout << "request_safety_function(echo): " << (set_ok?"OK":"FAIL") << "\n";
}
get_safety_function_state:
    status: OK
    id: 0
    state: 0
request_safety_function: OK

Example for get_safety_control_data()

Nrmk::IndyFramework::SafetyControlData scd;
ok = indy.get_safety_control_data(scd);
std::cout << "get_safety_control_data: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " safety_state(id="<<scd.safety_state().id()
                        << ",state="<<scd.safety_state().state()<<")"
                        << " auto_mode="<<scd.auto_mode()
                        << " reduced_mode="<<scd.reduced_mode()
                        << " enabler_pressed="<<scd.enabler_pressed();
}
std::cout << "\n";
get_safety_control_data:
    status: OK
    safety_state: {id: 0, state: 0}
    auto_mode: 0
    reduced_mode: 0
    enabler_pressed: 0

Example for set_auto_mode(),check_auto_mode(),check_reduced_mode()

Nrmk::IndyFramework::CheckAutoModeRes am;
ok = indy.check_auto_mode(am);
std::cout << "check_auto_mode: " << (ok?"OK":"FAIL");
if (ok) std::cout << " on="<<am.on();
std::cout << "\n";

if (ok && !am.on()) {
    bool set_ok = indy.set_auto_mode(true);
    std::cout << "set_auto_mode(true): " << (set_ok?"OK":"FAIL") << "\n";
}

Nrmk::IndyFramework::CheckReducedModeRes rm;
ok = indy.check_reduced_mode(rm);
std::cout << "check_reduced_mode: " << (ok?"OK":"FAIL");
if (ok) std::cout << " on="<<rm.on();
std::cout << "\n";
check_auto_mode:
    status: OK
    on: 0
set_auto_mode: OK
check_reduced_mode:
    status: OK
    on: 0

Example for get_control_info()

Nrmk::IndyFramework::ControlInfo cinfo;
ok = indy.get_control_info(cinfo);
std::cout << "get_control_info: " << (ok?"OK":"FAIL");
if (ok) std::cout << " version="<<cinfo.control_version()
                                    << " model="<<cinfo.robot_model();
std::cout << "\n";
get_control_info:
    status: OK
    version: "3.4.0-rc11 [2025.08.13]"
    model: "Indy7"

Example for check_aproach_retract_valid()

std::array<float,6> zeros{0,0,0,0,0,0};
std::vector<float> init_jpos(6,0.f);
Nrmk::IndyFramework::CheckAproachRetractValidRes car_res;
ok = indy.check_aproach_retract_valid(zeros, init_jpos, zeros, zeros, car_res);
std::cout << "check_aproach_retract_valid: " << (ok?"OK":"FAIL");
if (ok) {
    std::cout << " is_valid=" << car_res.is_valid();
    std::cout << " tar_pos=[";
    for (int i=0;i<car_res.tar_pos_size();++i){ if(i) std::cout<<","; std::cout<<car_res.tar_pos(i); }
    std::cout << "] approach_pos=[";
    for (int i=0;i<car_res.approach_pos_size();++i){ if(i) std::cout<<","; std::cout<<car_res.approach_pos(i); }
    std::cout << "] retract_pos=[";
    for (int i=0;i<car_res.retract_pos_size();++i){ if(i) std::cout<<","; std::cout<<car_res.retract_pos(i); }
    std::cout << "]";
}
std::cout << "\n";
check_aproach_retract_valid:
    status: OK
    is_valid: 1
    tar_pos: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    approach_pos: [0.0, -186.5, 1328.0, 180.0, 180.0, 180.0]
    retract_pos:  [0.0, -186.5, 1328.0, 180.0, 180.0, 180.0]

Example for get_pallet_point_list()

std::array<float,6> zeros2{0,0,0,0,0,0};
std::vector<float> init_jpos2(6,0.f);
Nrmk::IndyFramework::GetPalletPointListRes pallet_res;
ok = indy.get_pallet_point_list(zeros2, init_jpos2, zeros2, zeros2, /*pattern*/1, /*width*/100, /*height*/100, pallet_res);
std::cout << "get_pallet_point_list: " << (ok?"OK":"FAIL");
if (ok) std::cout << " points=" << pallet_res.pallet_points_size();
std::cout << "\n";
if (ok && pallet_res.pallet_points_size()>0) {
    const auto& pp = pallet_res.pallet_points(0);
    std::cout << "  first.tar_pos_sz="<<pp.tar_pos_size()
                        << " approach_pos_sz="<<pp.approach_pos_size()
                        << " retract_pos_sz="<<pp.retract_pos_size()
                        << " tar_jpos_sz="<<pp.tar_jpos_size() << "\n";
}
get_pallet_point_list:
    status: OK
    points: 0

Example for move_recover_joint()

std::vector<float> jtarget = {0,0,-90,0,-90,0};
bool mrec_ok = indy.move_recover_joint(jtarget, /*base_type*/0);
std::cout << "move_recover_joint: " << (mrec_ok?"OK":"FAIL") << "\n";
move_recover_joint: OK