Low-level "velocity" control for 7DOF sometimes works, sometimes some joints don't move

[rojikada]

Description

Hello, I have a problem with the low-level control "velocity" controller. When I move all the joints, it seems to act in weird way. Sometimes all the joints catch up and move, but sometimes they don't catch up and just stop moving, so only part of the robot is moving. It seems like the robot is restarting from time to time (reopening gripper). I was thinking that joint-velocity control might be a better fit for this, but there is no example code for that?

What would be the recommended approach, when I want to move all the joints at 1kHz please? The IK seems to be correct as can be seen on the last video. But sometimes the joints just stop moving after the initial start.

I was also checking the continuity of the joint positions and they seem to be continuous.

Now I am also trying true velocity control: Question: how to use set_velocity() in low-level servoing mode #144, but as posted in the other issue, I am running into problems with the errors - even though I am updating positions, I still get follower error/joint in fault and the communication with the robot stops (because I no longer receive feedback from lambda callback function). Should I send in the position in "future" - with the velocity added or any other way to fix this?

Video example: At first wrong, after some time it fixes itself Kinova3 - correct until 36s, after restart little off This is how it is supposed to be like

Version

At a minimum, provide the Kortex API and Kortex-enabled device versions.

KortexAPI : 2.5.0

Steps to reproduce

Code example

   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;

        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) {
                std::string serialized_data;
                // read real-time positions and store to shared_memory
                // check of errors
                //google::protobuf::util::MessageToJsonString(data.actuators(data.actuators_size() - 1), &serialized_data);
                //std::cout << serialized_data << std::endl << std::endl;
            };

            // 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
                            //std::cout << commands.transpose() << std::endl << std::endl << std::endl;
                        }
                        for(int i=0; i<actuator_count; i++) {
                            shared_data_robot.joint_angles(i) = base_feedback.actuators(i).position()*M_PI/180.; // Convert to rad
                        }
                        shared_data_robot.joint_measurement_timestamp = std::chrono::high_resolution_clock::now();
                    }

                    if (read_commands) {
                        base_command.mutable_actuators(0)->set_position(fmod(float(commands(0)), 360.0f));
                        base_command.mutable_actuators(1)->set_position(fmod(float(commands(1)), 360.0f));
                        base_command.mutable_actuators(2)->set_position(fmod(float(commands(2)), 360.0f));
                        base_command.mutable_actuators(3)->set_position(fmod(float(commands(3)), 360.0f));
                        base_command.mutable_actuators(4)->set_position(fmod(float(commands(4)), 360.0f));
                        base_command.mutable_actuators(5)->set_position(fmod(float(commands(5)), 360.0f));
                        base_command.mutable_actuators(6)->set_position(fmod(float(commands(6)), 360.0f));
                    }

                    try {
                        base_cyclic->Refresh_callback(base_command, lambda_fct_callback, 0);
                    } catch(...) {
                        timeout++;
                    }

                    last = GetTickUs();
                }
            }
        } 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));
    }

Expected behavior

I would expect the robot to move correctly. If the goal joints position is too far, it would move with its maximal speed to the goal position?

[rojikada]

An update, I am currently trying the low-level velocity, but it is very bad as the robot starts drifting. I am trying to use PID to compensate for that, however it does not get much better:

Joint ID: Current joint-angle/goal-joint angle (degree error, goal velocity - capped at 40.).

1: 14.5306/14.5052 (-0.03, -40), 2: 155.159/155.159 (-0, -23.11), 
1: 14.5306/14.5052 (-0.03, -0.26), 2: 155.159/155.159 (-0, -0), 
1: 14.7071/14.4948 (-0.21, -40), 2: 155.158/155.243 (0.09, 40), 
1: 14.7083/14.4947 (-0.21, -40), 2: 155.158/155.243 (0.09, 18.89), 
1: 14.7099/14.4947 (-0.22, -40), 2: 155.158/155.243 (0.09, 12.35), 
1: 14.7128/14.4948 (-0.22, -40), 2: 155.158/155.243 (0.09, 13.59), 
1: 14.7125/14.4948 (-0.22, 22.07), 2: 155.157/155.243 (0.09, 40), 
1: 14.7061/14.495 (-0.21, 40), 2: 155.148/155.241 (0.09, 40), 
1: 14.6994/14.495 (-0.2, 40), 2: 155.142/155.242 (0.1, 40), 
1: 14.6861/14.4941 (-0.19, 40), 2: 155.145/155.249 (0.1, 40), 
1: 14.6663/14.4931 (-0.17, 40), 2: 155.152/155.257 (0.1, 40), 
1: 14.6663/14.4931 (-0.17, -1.76), 2: 155.152/155.257 (0.1, 1.06), 
1: 14.5912/14.4913 (-0.1, 40), 2: 155.203/155.271 (0.07, -40), 
1: 14.5831/14.4902 (-0.09, 40), 2: 155.232/155.28 (0.05, -40), 
1: 14.5831/14.4902 (-0.09, -0.96), 2: 155.269/155.28 (0.01, -40), 
1: 14.5831/14.4887 (-0.09, -40), 2: 155.269/155.293 (0.02, 40), 
1: 14.5948/14.4851 (-0.11, -40), 2: 155.345/155.322 (-0.02, -40), 
1: 14.6107/14.4825 (-0.13, -40), 2: 155.382/155.344 (-0.04, -40), 
1: 14.6107/14.4825 (-0.13, -1.33), 2: 155.382/155.344 (-0.04, -0.37), 
1: 14.6234/14.4803 (-0.14, -40), 2: 155.411/155.361 (-0.05, -40), 

Is there an estimate when the velocity control will compensate for the gravity by itself? So far the velocity control does not look very useful as it ends up being torque control, but through velocities.

[rojikada]

Is it possible to turn off the breaks for the low-level set_position commands and have the goal joint angle as a "fluid" angle, which I will update on the go?

[rojikada]

Solved, I need to put in velocity, so it does not brake every ms + I had to limit the goal position not to be further then some threshold. Otherwise the position control is fine.