Replacing pykdl with custom transformations library

[DhruvKoolRajamani]

This PR is a replacement of pykdl geometric primitives - Vector, Frame, Rotation, Twist, and Wrench. The library needs extensive testing but for the short term, testing for matrix multiplication and inverse should be done.

[adnanmunawar]

Why is this called HD? Should it not be called DH?

[adnanmunawar]

This doesn't seem O.K. Is mat_44 an attribute of Frame class?

[adnanmunawar]

Any reason for these two attributes are declared as static? Wouldn't it be better to make them instance attributes?

[adnanmunawar]

Thanks for the effort into the PR. This is going to be really useful.
I went over the python files from the original fork and I believe we should make the following changes.

1. the rotation.py can be improved. Not sure why the __new__ operator is being overloaded rather than simply using the __init__ method.
2. Let's overload the __repr__ and __str__ methods in all the kinematic classes.
3. Remove the CMakeLists.txt and package.xml file as this package does not contain any ROS specific code and thus should not include these files.
4. The contents of the __init__.py files in this PR and the last one seems redundant. Not sure if this is the best way to include the contents of the package.
5. As you have already noted in your comments, it's better to test the kinematics before integration rather than afterward. So let's create a script where we can test for the most commonly used operations for vectors, rotation matrices, and transforms.

I can work alongside you on these changes but you may need to add me as a collaborator in your fork.

[DhruvKoolRajamani]

Thanks for the effort into the PR. This is going to be really useful.
I went over the python files from the original fork and I believe we should make the following changes.

1. the rotation.py can be improved. Not sure why the `__new__` operator is being overloaded rather than simply using the `__init__` method.

2. Let's overload the `__repr__` and `__str__` methods in all the kinematic classes.

3. Remove the CMakeLists.txt and package.xml file as this package does not contain any ROS specific code and thus should not include these files.

4. The contents of the `__init__.py` files in this PR and the last one seems redundant. Not sure if this is the best way to include the contents of the package.

5. As you have already noted in your comments, it's better to test the kinematics before integration rather than afterward. So let's create a script where we can test for the most commonly used operations for vectors, rotation matrices, and transforms.

I can work alongside you on these changes but you may need to add me as a collaborator in your fork.

1. From the numpy ndarray documentation it is suggested to use __new__() instead of __init__(). The documentation would do more justice in explaining the reason behind this, TLDR; np ndarray only implement __new__() since it allows returning different datatypes depending on the input arguments. While I was following the standards, specific to our approach, we could use__init__() instead.
2. Yes I agree, we should overload the __repr__ and __str__ methods in all the kinematic classes.
3. Yes, I agree with removing the CMakeLists as well as the multiple layers of __init__.py. I had added it there because my interpreter was unable to add the library to path on it's own. I believe the correct method would be to have the setup.py file get build and installed into the ambf/build folder? I was trying to find ways to that with the main CMakeLists file but that wasn't working for some odd reason. We could either investigate appending the folders to path in the CMakeLists file itself? Or allow catkin (Not necessarily ros) to source it?
4. As mentioned in the previous item, it definitely needs a refactoring.
5. Yes, I have been working with just testpsmIK as of now, but we should ideally add tests to the respective geometric primitives itself.

Yes! I have added you as a collaborator as well!

[DhruvKoolRajamani]

I'm also bringing the parallel discussion we had over slack to this thread.

1. The library transformations.py should be merged with this library.
2. To avoid confusion with ROS TF, we can rename this library to kinematics or math or transform_math or tansform_lib
3. We could also possibly merge these two and other python utils for ambf into a new package called ambf_python_utils

[DhruvKoolRajamani]

I was also investigating the actual kdl library. It seems that they have updated their python bindings and now use pybind11 instead of sip. I feel we can try building the library itself -> add it to external/kdl and build both c++ and python libraries in python2 and python3. 2 lib files will be generated for both versions of python, and based on the python interpreter version, the correct one can be sourced?

orocos_kdl

I was simultaneously working on creating a parent CMakeLists file to initiate both repository builds at the same time. The only issue I was facing was finding a way to install files into the build folder itself, instead of a sudo make install command, so that the python_kdl library can find the required libs and headers. I guess this is a more CMakeLists related problem and can be fixed?