Eli H. Houlton
Eli H. Houlton

Reputation: 1

Inverse Kinematic solves using ik_fast fail to return values after first program run

After powering on the ur3e robotic arm and starting the external control program, I run a python program using the .rclpy library to control the arm. I use ik_fast for kinematics. The program inverse solves a global position for joint angles roughly every .1 seconds for 10 seconds and the solver has no issues, but when I run the same python code subsequent times the solver will often return no solution. I have narrowed the issue down to this line of code in the ik_fast pyx file: return self.thisptr.inverse(ee_pose)

Specifically, this line will return nothing at seemingly random times. There must be some kind of cache or data that UR or cpp is holding onto, since the solver always works flawlessly on the first program run after booting, but after weeks of trouble-shooting I cannot track down the issue. Note in the commented out code below I already tried creating a new pointer on each solve and that didn't fix the issue. Any help greatly appreciated.

Below is the full ur3e_ikfast.pyx file (bottom line the one from above):

#This is a Cython file and extracts the relevant classes from the C++ header file.
from libc.stdio cimport printf
# distutils: language = c++
# distutils: sources = ur3e/ikfast_wrapper.cpp

cdef extern from "<vector>" namespace "std":
    cdef cppclass vector[T]:
        cppclass iterator:
            T operator*()
            iterator operator++()
            bint operator==(iterator)
            bint operator!=(iterator)
        vector()
        void push_back(T&)
        T& operator[](int)
        T& at(int)
        iterator begin()
        iterator end()

cdef extern from "../include/Kinematics.hpp" namespace "robots":
    cdef cppclass Kinematics:
        Kinematics()
        int num_of_joints, num_free_parameters
        vector[float] forward(vector[float] joint_config);
        vector[float] inverse(vector[float] ee_pose);

cdef class PyKinematics:
    cdef Kinematics *thisptr      # hold a C++ instance which we're wrapping
    def __cinit__(self):
        self.thisptr = new Kinematics()
    def __dealloc__(self):
        del self.thisptr
    def getDOF(self):
        return self.thisptr.num_of_joints
    def forward(self,vector[float] joint_config):
        return self.thisptr.forward(joint_config)
    def inverse(self,vector[float] ee_pose):
        # del self.thisptr
        # self.thisptr = new Kinematics() # try reinitialize to get rid of any cache?
        # print("thisptr.inverse: " + str(self.thisptr.inverse(ee_pose)))
        # printf("%f\s", "thisptr.inverse: ")
        # printf("%f\s", str(self.thisptr.inverse(ee_pose)))
        # cdef vector[float] ee_pose_copy = ee_pose
        return self.thisptr.inverse(ee_pose)

Kinematics.hpp file:

namespace robots { 
    class Kinematics { 
        public: int num_of_joints, num_free_parameters; 
        Kinematics(); 
        ~Kinematics(); 
        std::vector<float> forward(std::vector<float> joint_config);
        std::vector<float> inverse(std::vector<float> ee_pose);
    }; 
}

The cpp files are lengthy and auto generated (I believe) but I can share them as well if that would be helpful

-I tried retrying the solve 10 times if no solution was returned, but it seems like if no solution was returned on the first attempt, the same would happen at each other attempt. -I also tried turning on and off the controllers (I'm using a velocity controller, joint_state_controller, and some others) -creating an entirely new pointer on each solve to ensure something whacky wasn't happening with that spot in memory. -It seems like completely powering off the ur3e arm and then back on the arm is the only fix to get the solver to solve every time again?

Upvotes: 0

Views: 46

Answers (0)

Related Questions