KDL::ChainIkSolverVel_pinv always returns zero joint velocity

[adkoessler]

Context

Problem encountered in ROS2 Jazzy, using the integrated simulator of the Universal Robots ROS2 Driver.

Description

My goal is to create a node that subscribes to a geometry_msgs::msg::Twist and publishes a std_msgs::msg::Float64MultiArray to the forward_velocity_controller/commands topic in order to move the robot. I found this exemple of solver usage. When I use the ChainIkSolverVel_pinv, it always computes zero velocities regardless of the twist input. I may be using the solver wrong and would be pleased to have some insight.

Regards,
Adrien

The code

Sending the twist through ros2 topic pub:

ros2 topic pub -r 1  twist_command geometry_msgs/msg/Twist "linear:
    x: 0.0
    y: 0.0
    z: -0.01
angular:
    x: 0.0
    y: 0.0
    z: 0.0"

Excerpt of C++ node:

// Construct KDL IK vel solver from URDF string
    void robotDescriptionCallback(const std_msgs::msg::String& msg)
    {
        RCLCPP_INFO(this->get_logger(), "Got robot description.");
        // Construct KDL tree from URDF
        const std::string urdf = msg.data;
        kdl_parser::treeFromString(urdf, tree_);
        number_of_joints_ = tree_.getNrOfJoints();
        // Print basic information about the tree
        std::cout << "nb joints:        " << number_of_joints_ << std::endl;
        std::cout << "nb segments:      " << tree_.getNrOfSegments() << std::endl;
        std::cout << "root segment:     " << tree_.getRootSegment()->first
                  << std::endl;
        // Get kinematic chain of the leg
        tree_.getChain("base_link", "foot", chain_);
        std::cout << "chain nb joints:  " << chain_.getNrOfJoints() << std::endl;
        // Create velocity IK solver
        solver_ = std::make_unique<KDL::ChainIkSolverVel_pinv>(chain_);
    }

// Receive twist commands and send them to the joint velocity controller
    void twistCallback(const geometry_msgs::msg::Twist& twist)
    {
        KDL::JntArray q_in;
        KDL::JntArray q_dot_out;
        std_msgs::msg::Float64MultiArray q_dot_pub;
        q_dot_pub.layout.dim.resize(1);
        q_dot_pub.layout.dim[0].label = 'q';
        q_dot_pub.layout.dim[0].size = number_of_joints_;
        q_dot_pub.layout.dim[0].stride = number_of_joints_;
        q_dot_pub.data.clear();
        KDL::Twist v_in;
        RCLCPP_INFO(this->get_logger(), "Geom msgs twist:");
        std::cout << twist.angular.x << std::endl;
        std::cout << twist.angular.y << std::endl;
        std::cout << twist.angular.z << std::endl;
        std::cout << twist.linear.x << std::endl;
        std::cout << twist.linear.y << std::endl;
        std::cout << twist.linear.z << std::endl;
        // Populate the twist from the topic
        twistGeomToKdl(twist, v_in);
        RCLCPP_INFO(this->get_logger(), "KDL twist:");
        std::cout << v_in.rot.x() << std::endl;
        std::cout << v_in.rot.y() << std::endl;
        std::cout << v_in.rot.z() << std::endl;
        std::cout << v_in.vel.x() << std::endl;
        std::cout << v_in.vel.y() << std::endl;
        std::cout << v_in.vel.z() << std::endl;
        RCLCPP_INFO(this->get_logger(), "Populate twist DONE.");
        // Populate the joint positions from the topic
        jointSensorToKdl(joint_, q_in);
        RCLCPP_INFO(this->get_logger(), "q_in JntArray:");
        std::cout << q_in.rows() << std::endl;
        for(unsigned int i=0; i<number_of_joints_; i++){
            std::cout << q_in(i) << std::endl;
        }
        RCLCPP_INFO(this->get_logger(), "Populate joint DONE.");
        // Run the IK solver
        q_dot_out.resize(number_of_joints_);
        solver_->CartToJnt(q_in, v_in, q_dot_out);
        RCLCPP_INFO(this->get_logger(), "IK solver DONE.");
        RCLCPP_INFO(this->get_logger(), "q_dot_out:");
        std::cout << q_dot_out.rows() << std::endl;
        for(unsigned int i=0; i<number_of_joints_; i++){
            std::cout << q_dot_out(i) << std::endl;
        }
        // Convert KDL joint velocities to an array
        jointKdlToArray(q_dot_out, q_dot_pub);
        RCLCPP_INFO(this->get_logger(), "Populate array DONE.");
        // Publish the joint velocity array to the controller
        jnt_vel_pub_->publish(q_dot_pub);
        RCLCPP_INFO(this->get_logger(), "Published joint velocity.");
    }

Corresponding logs:

[INFO] [1752225760.400575414] [joint_velocity_publisher]: Geom msgs twist:
0
0
0
0
0
-0.01
[INFO] [1752225760.400631356] [joint_velocity_publisher]: KDL twist:
0
0
0
0
0
-0.01
[INFO] [1752225760.400642891] [joint_velocity_publisher]: Populate twist DONE.
[INFO] [1752225760.400647611] [joint_velocity_publisher]: q_in JntArray:
6
1.19
-1.22
1.5
-2.04
-1.55
-5.5
[INFO] [1752225760.400661960] [joint_velocity_publisher]: Populate joint DONE.
[INFO] [1752225760.400666069] [joint_velocity_publisher]: IK solver DONE.
[INFO] [1752225760.400670093] [joint_velocity_publisher]: q_dot_out:
6
0
0
0
0
0
0
[INFO] [1752225760.400682894] [joint_velocity_publisher]: Populate array DONE.
[INFO] [1752225760.400700958] [joint_velocity_publisher]: Published joint velocity.

Robot view (it's not in a singular configuration):