Question: how to use set_velocity() in low-level servoing mode

[zhao-sq]

Hi, I try to use set_velocity() function instead of set_position() one to send velocity command in low-level servoing mode but it didn't work on the 6-DoF KinovaGen3. Could you please give me an example about how to use it properly? Thank you.

[felixmaisonneuve]

Hi @zhao-sq,

Here is an example that uses set_velocity in low-level servoing : https://github.com/Kinovarobotics/kortex/blob/master/api_cpp/examples/107-Gripper_low_level_command/01-gripper_low_level_command.cpp

Also, this issue might also interest you : https://github.com/Kinovarobotics/kortex/issues/42

Best, Felix

[zhao-sq]

Hi,

Thank you very much for your answer.

-----Original Messages----- From:"Félix Maisonneuve" @.> Sent Time:2023-01-18 23:29:57 (Wednesday) To: Kinovarobotics/kortex @.> Cc: zhao-sq @.>, Mention @.> Subject: Re: [Kinovarobotics/kortex] Question: how to use set_velocity() in low-level servoing mode (Issue #144)

Hi @zhao-sq,

Here is an example that uses set_velocity in low-level servoing : https://github.com/Kinovarobotics/kortex/blob/master/api_cpp/examples/107-Gripper_low_level_command/01-gripper_low_level_command.cpp

Also, this issue might also interest you : #42

Best, Felix

— Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you were mentioned.Message ID: @.***>

[felixmaisonneuve]

I will close this issue, but if it doesn't work or if you have another question when testing, please leave a comment or re-open the issue and I will help you

Regards, Felix

[rojikada]

Hello, I am currently trying to use the low-level velocity commands, however I always run into "Error safety raised: Joint Fault". I update the set_position with current joint position, but the robot end-effectors spins only for a few degrees and then stops (right after rising the fault). Those are the errors + warnings that arise during the control (in that order):

1. 2x Warning safety raised: Joint Limit Low
2. Warning safety raised: Joint Limit Low
3. Error safety raised: Joint Fault (here the robot stops moving)
4. 2x Warning Safety cleared: Joint Limit Low
5. Warning Safety cleared: Following Error

• I am currently doing this:

    void operator()(std::stop_token should_run) {
        k_api::BaseCyclic::Feedback base_feedback;
        k_api::BaseCyclic::Command  base_command;
        Eigen::Matrix<double,7,1> commands;
        Eigen::Matrix<double,7,1> joint_angles;

        auto servoingMode = k_api::Base::ServoingModeInformation();

        int64_t now = 0;
        int64_t last = 0;
        int timeout = 0;

        spdlog::info("KortexController: Initializing the arm for velocity low-level control");
        try {
            // Set the base in low-level servoing mode
            servoingMode.set_servoing_mode(k_api::Base::ServoingMode::LOW_LEVEL_SERVOING);
            base->SetServoingMode(servoingMode);
            base_feedback = base_cyclic->RefreshFeedback();

            int actuator_count = base->GetActuatorCount().count();

            // Initialize each actuator to its current position
            {
                std::lock_guard<std::mutex> lock(shared_data_robot_mutex);
                for (int i = 0; i < actuator_count; i++) {
                    shared_data_robot.joint_angles(i) = base_feedback.actuators(i).position()*M_PI/180.;
                    shared_data_robot.goal_joint_angles(i) = base_feedback.actuators(i).position()*M_PI/180.;
                    base_command.add_actuators()->set_position(base_feedback.actuators(i).position());
                }
                shared_data_robot.joint_measurement_timestamp = std::chrono::high_resolution_clock::now();
            }

            // Define the callback function used in Refresh_callback
            auto lambda_fct_callback = [&](const Kinova::Api::Error &err, const k_api::BaseCyclic::Feedback data) {
                base_feedback = data;
                std::lock_guard<std::mutex> lock(shared_data_robot_mutex);
                for (int i = 0; i < actuator_count; i++) {
                    shared_data_robot.joint_angles(i) = base_feedback.actuators(i).position()*M_PI/180.;
                }
                shared_data_robot.joint_measurement_timestamp = std::chrono::high_resolution_clock::now();
            };

            // Send a first frame
            base_feedback = base_cyclic->Refresh(base_command);

            // Set first actuator in velocity mode now that the command is equal to measure
            auto control_mode_message = k_api::ActuatorConfig::ControlModeInformation();
            control_mode_message.set_control_mode(k_api::ActuatorConfig::ControlMode::VELOCITY);
            // Device id == index + 1
            actuator_config->SetControlMode(control_mode_message, 1);
            actuator_config->SetControlMode(control_mode_message, 2);
            actuator_config->SetControlMode(control_mode_message, 3);
            actuator_config->SetControlMode(control_mode_message, 4);
            actuator_config->SetControlMode(control_mode_message, 5);
            actuator_config->SetControlMode(control_mode_message, 6);
            actuator_config->SetControlMode(control_mode_message, 7);

            // Real-time loop
            while (!should_run.stop_requested()) {
                now = GetTickUs();
                if(now - last > 1000) {
                    bool read_commands = false;
                    {
                        std::lock_guard<std::mutex> lock(shared_data_robot_mutex);
                        if (std::chrono::duration_cast<std::chrono::duration<double>>(
                            std::chrono::high_resolution_clock::now() - shared_data_robot.goal_computed_timestamp).count() < 0.002) {
                            read_commands = true;
                            commands = shared_data_robot.goal_joint_angles*180./M_PI; // Convert to degrees
                        }
                    }

                    if (read_commands) {
                        for (int i=0;i<actuator_count; i++) {
                            base_command.mutable_actuators(i)->set_position(base_feedback.actuators(i).position());
                            base_command.mutable_actuators(i)->set_velocity(std::max({std::min({angle_difference(base_feedback.actuators(i).position(), commands(i)), 5.}), -5.}));
                        }
                    }

                    // Incrementing identifier ensures actuators can reject out of time frames
                    base_command.set_frame_id(base_command.frame_id() + 1);
                    if (base_command.frame_id() > 65535) base_command.set_frame_id(0);

                    for (int idx = 0; idx < actuator_count; idx++) {
                        base_command.mutable_actuators(idx)->set_command_id(base_command.frame_id());
                    }
                    try {
                        base_cyclic->Refresh_callback(base_command, lambda_fct_callback, 0);
                    } catch(...) {
                        timeout++;
                    }

                    last = GetTickUs();
                }
            }

            // Set actuators back to position control
            control_mode_message.set_control_mode(k_api::ActuatorConfig::ControlMode::POSITION);
            actuator_config->SetControlMode(control_mode_message, 1);
            actuator_config->SetControlMode(control_mode_message, 2);
            actuator_config->SetControlMode(control_mode_message, 3);
            actuator_config->SetControlMode(control_mode_message, 4);
            actuator_config->SetControlMode(control_mode_message, 5);
            actuator_config->SetControlMode(control_mode_message, 6);
            actuator_config->SetControlMode(control_mode_message, 7);
        } catch (k_api::KDetailedException& ex) {
            spdlog::error("KortexController: Real-time loop error: {}", ex.what());
        } catch (std::runtime_error& ex2) {
            spdlog::error("KortexController: Runtime error: {}", ex2.what());
        }

        // Set back the servoing mode to Single Level Servoing
        servoingMode.set_servoing_mode(k_api::Base::ServoingMode::SINGLE_LEVEL_SERVOING);
        base->SetServoingMode(servoingMode);

        // Wait for a bit
        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    }