pantor/frankx

High-Level Motion Library for Collaborative Robots

frankx's Issues

Brake state

Hello,

It is possible to lock and unlock brakes with Robot.lock_brakes and Robot.unlock_brakes.
But is it possible to simply read if the brakes are locked or unlocked?

Thank you in advance.

cartesian_motion_generator_joint_acceleration_discontinuity, cartesian_motion_generator_joint_velocity_discontinuity

Hello,

I have finished all the relevant configuration of Frankx and successfully run the joint motion generator. However, when I was trying to run the examples that use the cartesian motion generator, the exceptions were thrown by libfranka (e.g. cartesian_motion_generator_joint_acceleration_discontinuity, cartesian_motion_generator_joint_velocity_discontinuity,). So, could you please share the solutions if you have already solved the similar issues?

Best regards

Waypoint stop

Hello,

In the following example, the robot still stop at each waypoint while I added "Waypoint(False)" that should tell the robot to not stop at the next waypoint. I don't know if this problem is due to frankx or libfranka.

Thanks for your help.

m0 = WaypointMotion([
Waypoint(False),
Waypoint(Affine(0.45, 0.0, 0.2, 0., 0., 0.)),
Waypoint(False),
Waypoint(Affine(0.5, 0.0, 0.2, 0., 0., 0.)),
Waypoint(False),
Waypoint(Affine(0.55, 0.0, 0.2, 0., 0., 0.)),
])
robot = Robot(fci_ip, 0.05)
robot.move(m0)

How are you handling orientation singularity?

It looks like you are using euler angles for orientation interpolation with reflexxes (though feel free to correct me if I am wrong!). How are you dealing with instability close to singularity?

Implementation of JointImpedance

Hi @pantor , i would like to be able to set the impedance in a joint-by-joint case, in the c library this seems to be possible. In frankx there is a piece of code that says this is not been implemented yet. Can you please provide me with some guidance on how to implement such a feature?

Thanks in advance.
Miguel Tavares

could you provide interface about frankx with moveit

could you provide interface about frankx with moveit in the near future, it will be wonderful to use frankx in moveit environment,thanks

Reading force value

Hi @pantor , when i run the example file force_control.py it works perfectly, and it's visible where the force in Z in been measured:

linear_motion_data = MotionData().with_reaction(Reaction(Measure.ForceZ < -8.0, StopMotion())) # [N]

However when I try to see the value in the Measure.ForceZ it only returns <_frankx.Measure object at 0x7f47cbcb6ab0>, im I missing some definition in order to be able to get the force value? This is extremely important for my application.

Thanks.

_frankx.NetworkException: libfranka: UDP receive: Timeout

Hello,

I have installed libfranka 0.8.0 and the lattest version of frankx.

When I run a python script, I get the following error:

Traceback (most recent call last):
  File "read.py", line 24, in <module>
    robot = Robot(args.host)
  File "/home/user_name/.local/lib/python3.7/site-packages/frankx/robot.py", line 15, in __init__
    super().__init__(fci_ip, dynamic_rel=dynamic_rel, repeat_on_error=repeat_on_error, stop_at_python_signal=stop_at_python_signal)
_frankx.NetworkException: libfranka: UDP receive: Timeout

FCI feature is installed (I have checked on the admin interface) and ping on FCI IP is always bellow 1ms.

Do you have any idea to fix this issue?

Thank you in advance.

ValueError: libfranka: Attempt to set invalid transformation in motion generator. Has to be column major!

Hi @pantor

I know this is a libfranka issue, but maybe you can help fix it. When I run example/home.py i get:

# python3 examples/home.py 
initial position:   0.529378
  0.123311
   0.37398
   2.35987
-0.0296578
 0.0611505
 -0.643318
initial velocity: 0
0
0
0
0
0
0
initial acceleration: 0
0
0
0
0
0
0
target position: 0.4805
-0.229
 0.389
     0
    -0
     0
  1.75
target velocity: 0
0
0
0
0
0
0
tf: 4298.34
v_max_tfs: 71.8799
518.105
22.0886
1179.93
14.8289
30.5753
4298.34
a_max_tfs:  8.5064
22.8376
4.71548
30.1382
3.37865
4.85148
84.8339
j_max_tfs:  1.55498
 3.00373
 1.04933
 3.61387
0.840214
 1.06941
 7.20455
a: -1.99876e-19
-1.44069e-18
 6.14216e-20
-4.04976e-12
-6.29602e-12
-4.05035e-12
-6.29601e-12
b:  2.14785e-15
 1.54815e-14
-6.60028e-16
 3.48145e-08
  5.4125e-08
 3.48196e-08
 5.41248e-08
c: -6.15479e-12
-4.43632e-11
 1.89135e-12
-7.48225e-05
-0.000116324
-7.48333e-05
-0.000116324
d: 0
0
0
0
0
0
0
e: 0
0
0
0
0
0
0
f:   0.529378
  0.123311
   0.37398
   2.35987
-0.0296578
 0.0611505
 -0.643318
Traceback (most recent call last):
  File "examples/home.py", line 17, in <module>
    robot.move(camera, LinearMotion(home, 1.75))
ValueError: libfranka: Attempt to set invalid transformation in motion generator. Has to be column major!

To reproduce:

git clone https://github.com/dti-research/frankx.git
cd frankx
docker build -t pantor/frankx -f docker/Dockerfile .
docker run -it --rm --network=host --privileged -v $(pwd):/code/frankx -w /code/frankx pantor/frankx

And inside the container build the repo:

mkdir build && cd build
cmake ..
make -j$(nproc)
make install

And finally run the example:

python3 example/home.py

How to get the right libReflexxesTypeII.so

Hi,
I downloaded ReflexxesTypeII.zip and compiled linux64 .so file from it.
When I used g++7.5, the compiling failed. So I used g++4.8 to compile it and succeeded.
But when I import frankx, it showed libReflexxesTypeII.so: invalid ELF header.
So how to get the right libReflexxesTypeII.so?

My os is ubuntu18.04

Thanks for you help!

ImportError occur

Thanks for you brilliant job!
I have successfully installed all the libraries according to the file ci.yml.
But I get following error when i import frankx:
from frankx import Affine, LinearRelativeMotion, Robot
error:
ImportError: libfrankx.so: cannot open shared object file: No such file or directory

Can you give some suggestions for fixing this?

Missing Waypoint Python bindings

Hi @pantor,

I'd like to know if its possible to have WaypointMotion or PathMotion with waypoints that differ in all of these parameters: affine, elbow, blend_max_distance, and velocity_rel within the same motion.

Is it by design that the Python bindings are not exposing enough variables to allow this?

For example, via Python, you can only create a PathMotion where all waypoints have the same blend_max_distance. There is a Waypoint constructor in C++ that that accepts blend_max_distance, but its not there in Python.

There's also no Waypoint constructor that accepts: affine, elbow, blend_max_distance, and velocity_rel. But velocity_rel is a mutable field that can be changed after creating a waypoint.

reaction implementation

Can you please add a similar reaction handling as in "motion_waypoint_generator.hpp" to all other motion generators?

A full implementation with reaction movements would be optimal,
transitionally a stop command would also do:

        for (auto& reaction : data.reactions) {
            if (reaction.has_fired) {
                continue;
            }

            if (reaction.condition(MotionGenerator::convertState(robot_state), time)) {
                std::cout << "[frankx] reaction fired." << std::endl;
                reaction.has_fired = true;

                bool new_motion = false;
                robot->stop();
                return franka::MotionFinished(CartesianPose(trajectory.path.q(s_current, frame), use_elbow));
                //return franka::MotionFinished(MotionGenerator::CartesianPose(input_para.current_position, waypoint_has_elbow));
            }
        }

Segmentation fault (core dumped)

Hi! Thanks to your great project. I meet some difficult while I try to build the project on docker:
When I try to run the tutorial lines

from frankx import Affine, LinearRelativeMotion, Robot

robot = Robot("172.16.0.2")
robot.set_dynamic_rel(0.05)

motion = LinearRelativeMotion(Affine(0.2, 0.0, 0.0))
robot.move(motion)

Nothing output except the:
Segmentation fault (core dumped)
I tried the robot.current_pose(), which can return the pose quite well, so the problem seems showed on the robot.move(motion). Do you have any idea ?

Get state while moving

Hello,

Is there any way to get information like the pose or the "active waypoint" while the robot is moving? It would be very useful.

I tried to do it with Robot.move_async and then Robot.read_once() but it's not permitted.

Thank you for your help.

Required version of Eigen

I noticed that in

frankx/CMakeLists.txt

Line 17 in 6953ca9

find_package(Eigen3 3.3.7 EXACT REQUIRED NO_MODULE)

you are requesting a specific version of Eigen.

Is there any specific reason behind this choice? Is it possible to use other versions of the library?

Thank you.

Cannot install

Hi

I'm trying to install your library but I'm stuck in a make error:

RUN cd frankx && mkdir build && cd build     && cmake -DREFLEXXES_TYPE=ReflexxesTypeII -DReflexxes_INCLUDE_DIR=RMLTypeII/include/RMLTypeII/ -DReflexxes_LIB_DIR=RMLTypeII/build ..     && make -j8
 ---> Running in 9568e9979c92
-- The CXX compiler identification is GNU 7.4.0
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Looking for C++ include pthread.h
-- Looking for C++ include pthread.h - found
-- Looking for pthread_create
-- Looking for pthread_create - not found
-- Check if compiler accepts -pthread
-- Check if compiler accepts -pthread - yes
-- Found Threads: TRUE  
CMake Error at cmake/FindReflexxes.cmake:10 (target_include_directories):
  Cannot specify include directories for imported target
  "Reflexxes::Reflexxes".
Call Stack (most recent call first):
  CMakeLists.txt:19 (find_package)


CMake Error at cmake/FindReflexxes.cmake:11 (target_link_libraries):
  Cannot specify link libraries for target "Reflexxes::Reflexxes" which is
  not built by this project.
Call Stack (most recent call first):
  CMakeLists.txt:19 (find_package)


-- Configuring incomplete, errors occurred!
See also "/code/frankx/build/CMakeFiles/CMakeOutput.log".
See also "/code/frankx/build/CMakeFiles/CMakeError.log".
The command '/bin/sh -c cd frankx && mkdir build && cd build     && cmake -DREFLEXXES_TYPE=ReflexxesTypeII -DReflexxes_INCLUDE_DIR=RMLTypeII/include/RMLTypeII/ -DReflexxes_LIB_DIR=RMLTypeII/build ..     && make -j8' returned a non-zero code: 1

The Docker file to reproduce this is here: https://github.com/dti-research/frankx/blob/master/docker/Dockerfile

I've tried to change the FindReflexxes.cmake file to avoid this error, but my changes just gives an error at compile time instead:

Step 10/10 : RUN cd frankx && mkdir build && cd build     && cmake -DREFLEXXES_TYPE=ReflexxesTypeII -DReflexxes_INCLUDE_DIR=../RMLTypeII/include/RMLTypeII/ -DReflexxes_LIB_DIR=../RMLTypeII/build -DBUILD_TESTS=OFF ..     && make -j8
 ---> Running in 6f9b8a815891
-- The CXX compiler identification is GNU 7.4.0
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Looking for C++ include pthread.h
-- Looking for C++ include pthread.h - found
-- Looking for pthread_create
-- Looking for pthread_create - not found
-- Check if compiler accepts -pthread
-- Check if compiler accepts -pthread - yes
-- Found Threads: TRUE  
-- Found PythonInterp: /usr/bin/python3.6 (found version "3.6.9") 
-- Found PythonLibs: /usr/lib/x86_64-linux-gnu/libpython3.6m.so
-- Performing Test HAS_CPP14_FLAG
-- Performing Test HAS_CPP14_FLAG - Success
-- Performing Test HAS_FLTO
-- Performing Test HAS_FLTO - Success
-- LTO enabled
-- Configuring done
-- Generating done
-- Build files have been written to: /code/frankx/build
Scanning dependencies of target libfrankx
[  8%] Building CXX object CMakeFiles/libfrankx.dir/src/gripper.cpp.o
[ 16%] Building CXX object CMakeFiles/libfrankx.dir/src/robot.cpp.o
[ 25%] Building CXX object CMakeFiles/libfrankx.dir/src/motion_joint.cpp.o
[ 33%] Building CXX object CMakeFiles/libfrankx.dir/src/affine.cpp.o
[ 41%] Building CXX object CMakeFiles/libfrankx.dir/src/reaction.cpp.o
[ 50%] Linking CXX shared library liblibfrankx.so
/usr/bin/ld: cannot find -lReflexxes-NOTFOUND
collect2: error: ld returned 1 exit status
CMakeFiles/libfrankx.dir/build.make:200: recipe for target 'liblibfrankx.so' failed
make[2]: *** [liblibfrankx.so] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/libfrankx.dir/all' failed
make[1]: *** [CMakeFiles/libfrankx.dir/all] Error 2
Makefile:129: recipe for target 'all' failed
make: *** [all] Error 2
The command '/bin/sh -c cd frankx && mkdir build && cd build     && cmake -DREFLEXXES_TYPE=ReflexxesTypeII -DReflexxes_INCLUDE_DIR=../RMLTypeII/include/RMLTypeII/ -DReflexxes_LIB_DIR=../RMLTypeII/build -DBUILD_TESTS=OFF ..     && make -j8' returned a non-zero code: 2

ImportError: No module named build._frankx

I imported the frankx module in a simple file, but the following problem occurred.

Traceback (most recent call last):
File "/home/ajian/catkin_ur/src/universal_robot-melodic-devel/ur_driver/frankx/frankx_test.py", line 4, in
from frankx import Affine, LinearRelativeMotion, Robot
File "/home/ajian/catkin_ur/src/universal_robot-melodic-devel/ur_driver/frankx/frankx/init.py", line 1, in
from build._frankx import Affine, Comparison, Duration, Errors, GripperState, JointMotion, LinearMotion, LinearRelativeMotion, Measure, MotionData, Reaction, PositionHold, RobotMode, RobotState, Waypoint, WaypointMotion
ImportError: No module named build._frankx

I can't find any files or folders about build. _frankx in the frankx folder.
I don't know how to solve this problem, does anyone know what is the reason and how to solve it?

The following is my python file.

#!/usr/bin/env python

-- coding: utf-8 --

from frankx import Affine, LinearRelativeMotion, Robot

robot = Robot("172.16.0.2")
robot.set_dynamic_rel(0.05)

motion = LinearRelativeMotion(Affine(0.2, 0.0, 0.0))
robot.move(motion)

Cloning issue

Hello,
When cloning frankx I get the following messages and then I can't build it.
Thanks in advance for your help.

$ git clone --recursive https://github.com/pantor/frankx
Cloning into 'frankx'...
remote: Enumerating objects: 1325, done.
remote: Counting objects: 100% (1325/1325), done.
remote: Compressing objects: 100% (463/463), done.
remote: Total 5555 (delta 1085), reused 1007 (delta 857), pack-reused 4230
Receiving objects: 100% (5555/5555), 2.73 MiB | 5.49 MiB/s, done.
Resolving deltas: 100% (4242/4242), done.
Submodule 'affx' ([email protected]:pantor/affx.git) registered for path 'affx'
Submodule 'ruckig' ([email protected]:pantor/ruckig.git) registered for path 'ruckig'
Cloning into '/home/user/frankx/affx'...
Warning: Permanently added the RSA host key for IP address '140.82.121.3' to the list of known hosts.
[email protected]: Permission denied (publickey).
fatal: Could not read from remote repository.

Please make sure you have the correct access rights
and the repository exists.
fatal: clone of '[email protected]:pantor/affx.git' into submodule path '/home/user/frankx/affx' failed
Failed to clone 'affx'. Retry scheduled
Cloning into '/home/user/frankx/ruckig'...
[email protected]: Permission denied (publickey).
fatal: Could not read from remote repository.

Please make sure you have the correct access rights
and the repository exists.
fatal: clone of '[email protected]:pantor/ruckig.git' into submodule path '/home/user/frankx/ruckig' failed
Failed to clone 'ruckig'. Retry scheduled
Cloning into '/home/user/frankx/affx'...
[email protected]: Permission denied (publickey).
fatal: Could not read from remote repository.

Please make sure you have the correct access rights
and the repository exists.
fatal: clone of '[email protected]:pantor/affx.git' into submodule path '/home/user/frankx/affx' failed
Failed to clone 'affx' a second time, aborting
Cloning into '/home/user/frankx/ruckig'...
[email protected]: Permission denied (publickey).
fatal: Could not read from remote repository.

Please make sure you have the correct access rights
and the repository exists.
fatal: clone of '[email protected]:pantor/ruckig.git' into submodule path '/home/user/frankx/ruckig' failed
Failed to clone 'ruckig' a second time, aborting

pip install . not working

Hi,

I tried to install from source, using pip install .. But it is not working. Here is the error message:
`
Preparing metadata (setup.py) ... done
Building wheels for collected packages: frankx
Building wheel for frankx (setup.py) ... error
error: subprocess-exited-with-error

× python setup.py bdist_wheel did not run successfully.
│ exit code: 1
╰─> [65 lines of output]
running bdist_wheel
running build
running build_py
creating build/lib.linux-x86_64-3.8
creating build/lib.linux-x86_64-3.8/frankx
copying frankx/robot.py -> build/lib.linux-x86_64-3.8/frankx
copying frankx/gripper.py -> build/lib.linux-x86_64-3.8/frankx
copying frankx/init.py -> build/lib.linux-x86_64-3.8/frankx
running build_ext
-- The CXX compiler identification is GNU 7.5.0
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Check for working CXX compiler: /usr/bin/c++ - skipped
-- Detecting CXX compile features
-- Detecting CXX compile features - done
CMake Error at CMakeLists.txt:17 (find_package):
By not providing "FindFranka.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "Franka", but
CMake did not find one.

    Could not find a package configuration file provided by "Franka" (requested
    version 0.8) with any of the following names:
  
      FrankaConfig.cmake
      franka-config.cmake
  
    Add the installation prefix of "Franka" to CMAKE_PREFIX_PATH or set
    "Franka_DIR" to a directory containing one of the above files.  If "Franka"
    provides a separate development package or SDK, be sure it has been
    installed.
  
  
  -- Configuring incomplete, errors occurred!
  Traceback (most recent call last):
    File "<string>", line 2, in <module>
    File "<pip-setuptools-caller>", line 34, in <module>
    File "/home/bwu/original_frankx/frankx/setup.py", line 69, in <module>
      setup(
    File "/home/bwu/original_frankx/original_frankx_env/lib/python3.8/site-packages/setuptools/__init__.py", line 153, in setup
      return distutils.core.setup(**attrs)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/core.py", line 148, in setup
      dist.run_commands()
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 966, in run_commands
      self.run_command(cmd)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/home/bwu/original_frankx/original_frankx_env/lib/python3.8/site-packages/wheel/bdist_wheel.py", line 343, in run
      self.run_command("build")
    File "/home/bwu/miniconda3/lib/python3.8/distutils/cmd.py", line 313, in run_command
      self.distribution.run_command(command)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/home/bwu/miniconda3/lib/python3.8/distutils/command/build.py", line 135, in run
      self.run_command(cmd_name)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/cmd.py", line 313, in run_command
      self.distribution.run_command(command)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/home/bwu/original_frankx/frankx/setup.py", line 36, in run
      self.build_extension(ext)
    File "/home/bwu/original_frankx/frankx/setup.py", line 65, in build_extension
      subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp)
    File "/home/bwu/miniconda3/lib/python3.8/subprocess.py", line 364, in check_call
      raise CalledProcessError(retcode, cmd)
  subprocess.CalledProcessError: Command '['cmake', '/home/bwu/original_frankx/frankx', '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=/home/bwu/original_frankx/frankx/build/lib.linux-x86_64-3.8/', '-DPYTHON_EXECUTABLE=/home/bwu/original_frankx/original_frankx_env/bin/python3', '-DEXAMPLE_VERSION_INFO=0.2.0', '-DCMAKE_BUILD_TYPE=Release', '-DUSE_PYTHON_EXTENSION=OFF', '-DBUILD_EXAMPLES=OFF', '-DBUILD_TESTS=OFF', '-DBUILD_SHARED_LIBS=OFF', '-DCMAKE_BUILD_WITH_INSTALL_RPATH=TRUE', '-DCMAKE_INSTALL_RPATH=$ORIGIN', '-DCMAKE_POSITION_INDEPENDENT_CODE=ON']' returned non-zero exit status 1.
  [end of output]

note: This error originates from a subprocess, and is likely not a problem with pip.
ERROR: Failed building wheel for frankx
Running setup.py clean for frankx
Failed to build frankx
Installing collected packages: frankx
Running setup.py install for frankx ... error
error: subprocess-exited-with-error

× Running setup.py install for frankx did not run successfully.
│ exit code: 1
╰─> [67 lines of output]
running install
running build
running build_py
creating build/lib.linux-x86_64-3.8
creating build/lib.linux-x86_64-3.8/frankx
copying frankx/robot.py -> build/lib.linux-x86_64-3.8/frankx
copying frankx/gripper.py -> build/lib.linux-x86_64-3.8/frankx
copying frankx/init.py -> build/lib.linux-x86_64-3.8/frankx
running build_ext
-- The CXX compiler identification is GNU 7.5.0
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Check for working CXX compiler: /usr/bin/c++ - skipped
-- Detecting CXX compile features
-- Detecting CXX compile features - done
CMake Error at CMakeLists.txt:17 (find_package):
By not providing "FindFranka.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "Franka", but
CMake did not find one.

    Could not find a package configuration file provided by "Franka" (requested
    version 0.8) with any of the following names:
  
      FrankaConfig.cmake
      franka-config.cmake
  
    Add the installation prefix of "Franka" to CMAKE_PREFIX_PATH or set
    "Franka_DIR" to a directory containing one of the above files.  If "Franka"
    provides a separate development package or SDK, be sure it has been
    installed.
  
  
  -- Configuring incomplete, errors occurred!
  Traceback (most recent call last):
    File "<string>", line 2, in <module>
    File "<pip-setuptools-caller>", line 34, in <module>
    File "/home/bwu/original_frankx/frankx/setup.py", line 69, in <module>
      setup(
    File "/home/bwu/original_frankx/original_frankx_env/lib/python3.8/site-packages/setuptools/__init__.py", line 153, in setup
      return distutils.core.setup(**attrs)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/core.py", line 148, in setup
      dist.run_commands()
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 966, in run_commands
      self.run_command(cmd)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/home/bwu/original_frankx/original_frankx_env/lib/python3.8/site-packages/setuptools/command/install.py", line 61, in run
      return orig.install.run(self)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/command/install.py", line 545, in run
      self.run_command('build')
    File "/home/bwu/miniconda3/lib/python3.8/distutils/cmd.py", line 313, in run_command
      self.distribution.run_command(command)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/home/bwu/miniconda3/lib/python3.8/distutils/command/build.py", line 135, in run
      self.run_command(cmd_name)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/cmd.py", line 313, in run_command
      self.distribution.run_command(command)
    File "/home/bwu/miniconda3/lib/python3.8/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/home/bwu/original_frankx/frankx/setup.py", line 36, in run
      self.build_extension(ext)
    File "/home/bwu/original_frankx/frankx/setup.py", line 65, in build_extension
      subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp)
    File "/home/bwu/miniconda3/lib/python3.8/subprocess.py", line 364, in check_call
      raise CalledProcessError(retcode, cmd)
  subprocess.CalledProcessError: Command '['cmake', '/home/bwu/original_frankx/frankx', '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=/home/bwu/original_frankx/frankx/build/lib.linux-x86_64-3.8/', '-DPYTHON_EXECUTABLE=/home/bwu/original_frankx/original_frankx_env/bin/python3', '-DEXAMPLE_VERSION_INFO=0.2.0', '-DCMAKE_BUILD_TYPE=Release', '-DUSE_PYTHON_EXTENSION=OFF', '-DBUILD_EXAMPLES=OFF', '-DBUILD_TESTS=OFF', '-DBUILD_SHARED_LIBS=OFF', '-DCMAKE_BUILD_WITH_INSTALL_RPATH=TRUE', '-DCMAKE_INSTALL_RPATH=$ORIGIN', '-DCMAKE_POSITION_INDEPENDENT_CODE=ON']' returned non-zero exit status 1.
  [end of output]

note: This error originates from a subprocess, and is likely not a problem with pip.
error: legacy-install-failure

× Encountered error while trying to install package.
╰─> frankx

note: This is an issue with the package mentioned above, not pip.
hint: See above for output from the failure.

`

forwardKinematics is not working consistent?

While adding some tests to my library I came across strange behaviors of the function Robot::forwardKinematics. So I added tests #26 to frankx. The first test simply checks if Robot::forwardKinematics with Robot::currentJointPositions results in the same Affine as Robot::currentPose. As I see in the implementation these functions read (readOnce) from libfranka. So I ask myself, if the implementation of forwardKinematics is incorrect.
However I suspect my self to have a misconception as this function is one of the central functions.

Maybe you want to add the tests anyway to the library.
Thanks

Kinematics example not working

Hello,

I wrote a short example of Kinematics.forward and Kinematics.inverse used successively so that the output should be the same as the input but it is not always the case:

from frankx import Kinematics, Affine

q = [-1.45549, 1.15401, 1.50061, -2.30909, -1.3141, 1.9391, 0.02815]
x = Affine(Kinematics.forward(q))
q_new = Kinematics.inverse(x.vector(), q)
print('Inital joints: ', q)
print('New joints: ', q_new)

q = [-0.012, 0.215, 0.097, -2.26, -0.388, 2.04, 1.45]
x = Affine(Kinematics.forward(q))
q_new = Kinematics.inverse(x.vector(), q)
print('Inital joints: ', q)
print('New joints: ', q_new)

Here is the output I get:

Inital joints: [-1.45549, 1.15401, 1.50061, -2.30909, -1.3141, 1.9391, 0.02815]
New joints: [-1.45543159 1.15399955 1.50062745 -2.30901084 -1.31383838 1.93888225 0.02806562]
Inital joints: [-0.012, 0.215, 0.097, -2.26, -0.388, 2.04, 1.45]
New joints: [-0.2064247 0.31382151 -0.01503988 -2.10968927 1.29631794 1.56807399 2.97976908]

Support for Python 3.8-3.10 on pypi?

Hi, thanks for this great package!

Do we have any support plan for higher version of python?
Is there any changes need to do to the pybind interface to get it supported?

Update to libfranka 0.9.0

Hy, i'm working with a emika panda robot and the it requires the 0.9.0 version of libfrank. I would like to use frankx, do you have prediction for when libfranka version 0.9.0 will be supported?

pip3 install fail

Hello,

I have an issue at the last step when running the command : pip3 install .
Previously I used the command cmake with the option -DBUILD_TESTS=OFF (as I didn't installed catch2).
I don't know if this can be the reason why I got the following output:

~/frankx$ pip3 install .
Processing /home/user/frankx
Building wheels for collected packages: frankx
  Running setup.py bdist_wheel for frankx ... error
  Complete output from command /usr/bin/python3 -u -c "import setuptools, tokenize;__file__='/tmp/pip-req-build-or0bn859/setup.py';f=getattr(tokenize, 'open', open)(__file__);code=f.read().replace('\r\n', '\n');f.close();exec(compile(code, __file__, 'exec'))" bdist_wheel -d /tmp/pip-wheel-yfenl37p --python-tag cp37:
  running bdist_wheel
  running build
  running build_py
  creating build/lib.linux-x86_64-3.7
  creating build/lib.linux-x86_64-3.7/frankx
  copying frankx/__init__.py -> build/lib.linux-x86_64-3.7/frankx
  copying frankx/gripper.py -> build/lib.linux-x86_64-3.7/frankx
  copying frankx/robot.py -> build/lib.linux-x86_64-3.7/frankx
  running build_ext
  CMake Error: The current CMakeCache.txt directory /tmp/pip-req-build-or0bn859/build/temp.linux-x86_64-3.7/CMakeCache.txt is different than the directory /home/user/frankx/build/temp.linux-x86_64-3.7 where CMakeCache.txt was created. This may result in binaries being created in the wrong place. If you are not sure, reedit the CMakeCache.txt
  CMake Error: The source "/tmp/pip-req-build-or0bn859/CMakeLists.txt" does not match the source "/home/user/frankx/CMakeLists.txt" used to generate cache.  Re-run cmake with a different source directory.
  Traceback (most recent call last):
    File "<string>", line 1, in <module>
    File "/tmp/pip-req-build-or0bn859/setup.py", line 91, in <module>
      zip_safe=False,
    File "/usr/lib/python3/dist-packages/setuptools/__init__.py", line 145, in setup
      return distutils.core.setup(**attrs)
    File "/usr/lib/python3.7/distutils/core.py", line 148, in setup
      dist.run_commands()
    File "/usr/lib/python3.7/distutils/dist.py", line 966, in run_commands
      self.run_command(cmd)
    File "/usr/lib/python3.7/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/usr/lib/python3/dist-packages/wheel/bdist_wheel.py", line 188, in run
      self.run_command('build')
    File "/usr/lib/python3.7/distutils/cmd.py", line 313, in run_command
      self.distribution.run_command(command)
    File "/usr/lib/python3.7/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/usr/lib/python3.7/distutils/command/build.py", line 135, in run
      self.run_command(cmd_name)
    File "/usr/lib/python3.7/distutils/cmd.py", line 313, in run_command
      self.distribution.run_command(command)
    File "/usr/lib/python3.7/distutils/dist.py", line 985, in run_command
      cmd_obj.run()
    File "/tmp/pip-req-build-or0bn859/setup.py", line 36, in run
      self.build_extension(ext)
    File "/tmp/pip-req-build-or0bn859/setup.py", line 65, in build_extension
      subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp)
    File "/usr/lib/python3.7/subprocess.py", line 347, in check_call
      raise CalledProcessError(retcode, cmd)
  subprocess.CalledProcessError: Command '['cmake', '/tmp/pip-req-build-or0bn859', '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=/tmp/pip-req-build-or0bn859/build/lib.linux-x86_64-3.7/', '-DPYTHON_EXECUTABLE=/usr/bin/python3', '-DEXAMPLE_VERSION_INFO=0.1.1', '-DCMAKE_BUILD_TYPE=Release', '-DUSE_PYTHON_EXTENSION=OFF', '-DBUILD_EXAMPLES=OFF', '-DBUILD_TESTS=OFF', '-DBUILD_SHARED_LIBS=OFF', '-DCMAKE_BUILD_WITH_INSTALL_RPATH=TRUE', '-DCMAKE_INSTALL_RPATH=$ORIGIN', '-DCMAKE_POSITION_INDEPENDENT_CODE=ON']' returned non-zero exit status 1.
  
  ----------------------------------------
  Failed building wheel for frankx
  Running setup.py clean for frankx
Failed to build frankx
Installing collected packages: frankx
  Running setup.py install for frankx ... error
    Complete output from command /usr/bin/python3 -u -c "import setuptools, tokenize;__file__='/tmp/pip-req-build-or0bn859/setup.py';f=getattr(tokenize, 'open', open)(__file__);code=f.read().replace('\r\n', '\n');f.close();exec(compile(code, __file__, 'exec'))" install --record /tmp/pip-record-8yf5jer6/install-record.txt --single-version-externally-managed --compile --user --prefix=:
    running install
    running build
    running build_py
    creating build/lib.linux-x86_64-3.7
    creating build/lib.linux-x86_64-3.7/frankx
    copying frankx/__init__.py -> build/lib.linux-x86_64-3.7/frankx
    copying frankx/gripper.py -> build/lib.linux-x86_64-3.7/frankx
    copying frankx/robot.py -> build/lib.linux-x86_64-3.7/frankx
    running build_ext
    -- The CXX compiler identification is GNU 8.3.0
    -- Check for working CXX compiler: /usr/bin/c++
    -- Check for working CXX compiler: /usr/bin/c++ -- works
    -- Detecting CXX compiler ABI info
    -- Detecting CXX compiler ABI info - done
    -- Detecting CXX compile features
    -- Detecting CXX compile features - done
    CMake Error at CMakeLists.txt:17 (find_package):
      By not providing "FindFranka.cmake" in CMAKE_MODULE_PATH this project has
      asked CMake to find a package configuration file provided by "Franka", but
      CMake did not find one.
    
      Could not find a package configuration file provided by "Franka" (requested
      version 0.7) with any of the following names:
    
        FrankaConfig.cmake
        franka-config.cmake
    
      Add the installation prefix of "Franka" to CMAKE_PREFIX_PATH or set
      "Franka_DIR" to a directory containing one of the above files.  If "Franka"
      provides a separate development package or SDK, be sure it has been
      installed.
    
    
    -- Configuring incomplete, errors occurred!
    See also "/tmp/pip-req-build-or0bn859/build/temp.linux-x86_64-3.7/CMakeFiles/CMakeOutput.log".
    Traceback (most recent call last):
      File "<string>", line 1, in <module>
      File "/tmp/pip-req-build-or0bn859/setup.py", line 91, in <module>
        zip_safe=False,
      File "/usr/lib/python3/dist-packages/setuptools/__init__.py", line 145, in setup
        return distutils.core.setup(**attrs)
      File "/usr/lib/python3.7/distutils/core.py", line 148, in setup
        dist.run_commands()
      File "/usr/lib/python3.7/distutils/dist.py", line 966, in run_commands
        self.run_command(cmd)
      File "/usr/lib/python3.7/distutils/dist.py", line 985, in run_command
        cmd_obj.run()
      File "/usr/lib/python3/dist-packages/setuptools/command/install.py", line 61, in run
        return orig.install.run(self)
      File "/usr/lib/python3.7/distutils/command/install.py", line 589, in run
        self.run_command('build')
      File "/usr/lib/python3.7/distutils/cmd.py", line 313, in run_command
        self.distribution.run_command(command)
      File "/usr/lib/python3.7/distutils/dist.py", line 985, in run_command
        cmd_obj.run()
      File "/usr/lib/python3.7/distutils/command/build.py", line 135, in run
        self.run_command(cmd_name)
      File "/usr/lib/python3.7/distutils/cmd.py", line 313, in run_command
        self.distribution.run_command(command)
      File "/usr/lib/python3.7/distutils/dist.py", line 985, in run_command
        cmd_obj.run()
      File "/tmp/pip-req-build-or0bn859/setup.py", line 36, in run
        self.build_extension(ext)
      File "/tmp/pip-req-build-or0bn859/setup.py", line 65, in build_extension
        subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp)
      File "/usr/lib/python3.7/subprocess.py", line 347, in check_call
        raise CalledProcessError(retcode, cmd)
    subprocess.CalledProcessError: Command '['cmake', '/tmp/pip-req-build-or0bn859', '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=/tmp/pip-req-build-or0bn859/build/lib.linux-x86_64-3.7/', '-DPYTHON_EXECUTABLE=/usr/bin/python3', '-DEXAMPLE_VERSION_INFO=0.1.1', '-DCMAKE_BUILD_TYPE=Release', '-DUSE_PYTHON_EXTENSION=OFF', '-DBUILD_EXAMPLES=OFF', '-DBUILD_TESTS=OFF', '-DBUILD_SHARED_LIBS=OFF', '-DCMAKE_BUILD_WITH_INSTALL_RPATH=TRUE', '-DCMAKE_INSTALL_RPATH=$ORIGIN', '-DCMAKE_POSITION_INDEPENDENT_CODE=ON']' returned non-zero exit status 1.
    
    ----------------------------------------
Command "/usr/bin/python3 -u -c "import setuptools, tokenize;__file__='/tmp/pip-req-build-or0bn859/setup.py';f=getattr(tokenize, 'open', open)(__file__);code=f.read().replace('\r\n', '\n');f.close();exec(compile(code, __file__, 'exec'))" install --record /tmp/pip-record-8yf5jer6/install-record.txt --single-version-externally-managed --compile --user --prefix=" failed with error code 1 in /tmp/pip-req-build-or0bn859/

Can't import frankx in python3

Hello,

im stuck with using frankx in python3.7. I successfully build frankx with libfranka v0.7.1 and also got _frankx.cpython-37m-aarch64-linux-gnu.so and libfrankx.so in the build directory. I tried both adding the build directory to pythons path and also copying the files to diverse path folders (site-packages, dist-packages,dyn-load..) but i always get the following error when triying to import frankx:

>>> import frankx Traceback (most recent call last): File "<stdin>", line 1, in <module> ModuleNotFoundError: No module named 'frankx'

When renaming the files to "frankx.so" inside site-packages, i get:
ImportError: dynamic module does not define module export function (PyInit_frankx)

Do you have any hints for fixing this? Thank you in Advance!

ImpedanceMotion Segfault and other issues on system version 4 and libfranka 0.8.0

I observed the following errors on a clean Ubuntu 21.04 install with linux kernel version 5.10 patched with rt support. Frankx is compiled against libfranka 0.8.0. The franka panda arm firmware version is shown as 1.3.4-F-AX and 1.3.4-F-A7, system version 4.1.1

• Running ImpedanceMotion as the example shows gives segfault. Upon closer inspection, the line "*model = robot->loadModel()" in include/frankx/motion_impedance_generator.hpp is the source of the error. This line should be "model = new franka::Model(robot->loadModel())" or something like that, as no memory was allocated for *model.
• Impedance control also does not work the intended way, as changing motion.target after async_move in the example code has no effect at all. I believe this is because the constructor of motion_impedance_generator makes a copy of the ImpedanceMotion object, instead of making a reference to the original object.
• The robot will refuse any cartesian motion target after the setEE() function in the set_default_behavior() is run. After a closer inspection, the F_T_EE matrix is set differently from the default one in the desk settings:
  0.7071, 0.7071, 0, 0
  -0.7071, 0.7071, 0, 0
  0, 0, 1, 0.1034,
  0, 0, 0, 1
  This seems to be a libfranka 0.8.0 issue where we have to set F_T_EE matrix in desk from now on.
• Without setting F_T_EE matrix in desk results in LinearMotion/WaypointMotion/PathMotion twists the robot around its axis of "roll" until it stops due to self-collision most of the time. Setting the F_T_EE matrix in desk manually to the value specified in setDefaultBehavior solves the issue. However, the robot runs into cartesian_motion_generator_joint_acceleration_discontinuity very often despite command success rate close to 100% and dynamic rel set to 0.2, so moving the robot through these motion types are not practical.
• After setting the desired F_T_EE matrix in desk, the example force_control breaks even with set_default_behavior now commented out.

"cartesian_motion_generator_joint_acceleration_discontinuity"

When using async command, very often I run into this error:
libfranka: Move command aborted: motion aborted by reflex! ["cartesian_motion_generator_joint_acceleration_discontinuity"] control_command_success_rate: 0.94

The first time this error appears, the robot can keep working after it appears, but when it happens more than twice, it blocks the robot.

Could you help me undersatnd why it is happening and how can I avoid it? Thank you very much!

Here is the test code, I am using the "robot-state" branch of the repo of Toni-SM:robot-state only for measuring the open-loop error:

from argparse import ArgumentParser
from frankx import Affine, Robot, WaypointMotion, Waypoint
import frankx
import time
import numpy as np
import numpy.linalg as LA

def get_robot_state(robot):
    # get robot state
    try:
        robot_state = robot.get_state(read_once=True)
    except frankx.InvalidOperationException:
        robot_state = robot.get_state(read_once=False)
    return np.array(robot_state.O_T_EE[-4:-1])

if __name__ == '__main__':
    parser = ArgumentParser()
    parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
    args = parser.parse_args()

    robot = Robot(args.host)
    gripper = robot.get_gripper()

    robot.set_default_behavior()
    robot.recover_from_errors()
    robot.set_dynamic_rel(0.2)


    home = Waypoint(Affine(0.5, 0.0, 0.5), 1.65)
    motion = WaypointMotion([home], return_when_finished=False)
    thread = robot.move_async(motion)

    i = 0
    go_left = True
    last_time = time.time()
    while True:
        try:
            if i > 0.4:
                go_left = False
            if i < -0.4: 
                go_left = True
            if go_left:
                i += 0.01
            else:
                i -= 0.01
            print("y (cm): ", round(i*100))
            new_affine = Waypoint(Affine(0.5, i, 0.5), 1.65)

            # print("dt: ", time.time()-last_time)
            last_time = time.time()

            motion.set_next_waypoint(new_affine)

            # measure the open loop error
            cmd = np.array([0.5, i, 0.5])
            time.sleep(0.2) 
            meas = get_robot_state(robot)
            ol_error = LA.norm(cmd-meas)

            print("cmd :", cmd)
            print("meas:", meas)
            # print("ol_error:", ol_error)
            # print("vel:", robot.velocity_rel)
            # print("acc:", robot.acceleration_rel)
            # print("jer:", robot.jerk_rel)
        except KeyboardInterrupt:
            print("ctrl+c pressed")
            motion.finish()
            thread.join()

Confusion with degree of freedoms

Hey Pantor,

Same as I mentioned in the elbow issue I opened recently for libfranka here, I probably missout a concept for robotics in general or some specifics of Franka Emika.
However I specially had a hard time grabbing the term Degree of Freedom you use in ruckig and frankx.
As the const definition of degree of freedom it must either reference to jointspace (7 joints) or cartesian space (x,y,z, pitch, yaw roll + elbow value). In setting up the motion generators you just use it for both. Maybe even though degree of freedom for cartesian space is same it would be nice if it's stored in separate const values. And add some explenations as a comment.
I'm also not right sure why elbow besides other joints can be seen as extra DOF?

Best regards,
Peter

Moving the robot while in ImpedanceMotion mode

Hi, I would like to know if it is possible to send a JointMotion command while the robot is in ImpedanceMotion mode.
I see that you have in the example (impedance.py), the robot moving to a new target while maintaining the ImpedandeMotion, I want to do something similar but using the JointMotion command.

I try using the combination of:

q = [-1.53,0.508, -0.051, -1.796, -0.094, 3.386, -0.646]
x = Affine(Kinematics.forward(q))
impedance_motion.target = x

but when a new movement is initiated it doesn't move all the joints to the desired value, it only moves the end effector to the final position. Also, I can't seem to be able to control the velocity between movements (Always move at full speed), even using the robot.set_dynamic_rel(0.2) or data = MotionData(0.1) follow by robot.move_async(impedance, data)

Thanks in advance

Is there any requirement or environment which could be installed well by pypi

i want to install this package in pypi release but it can't be installed because of error

frankx-0.0.1-cp36-cp36m-manylinux2010_x86_64.whl is not a supported wheel on this platform

I think this come out because of dismatching environment. But the python version is 3.6.10

is there any environment we have to fit?

or

is there any guide for install this package without pypi packaging whl file?

Segmentation fault (core dumped)

When running cpp example code where the robot needs to move a segfault is encountered.

root@ThinkPad-P50:/code/frankx/build# ./home 10.224.60.160
Segmentation fault (core dumped)

I've outcommented/removed the PyErr_CheckSignals to resolve the segfault, see: dti-research@0195a1e

It seems that the WITH_PYTHON is set even-though the code is run from cpp.

libfranka aborted due to discontinuities

Hey everyone,

I was trying to run the examples that use the cartesian motion generator (e.g. linear.py, path.py) and I get exceptions thrown by libfranka. I am currently using the default frankx installation without Reflexxes. The errors that I get are mostly related to the jerk (cartesian_motion_generator_joint_acceleration_discontinuity) and acceleration (cartesian_motion_generator_joint_velocity_discontinuity). I assumed, since you can set them to a percentage of the maximum, that the limits can't be violated. Do you tune your constraints in a specific way or do you have another solution for that?

Best regards
Max

Request updated builds for frankx on pypi

It would be great to get the latest frankx when running pip install frankx
This, I believe, is a manual process, so a PR won't help.

Is there an active maintainer for this package who would be able to do this?
Another option is to have an automated process such as this workflow shared by @krzentner.

Setting a rotation in LinearRelativeMotion and LinearMotion

Hi,
I am experiencing strange behavior when I am setting a target rotation in LinearRelativeMotion or LinearMotion.

E.g. by calling

robot.move(LinearRelativeMotion(Affine(0.0, 0.0, -0.1, 0.0, 0.0, 0.2)))

I expect the robot to move 10cm and rotate its gripper by around 11.5 degrees. However, what I am experiencing in around 25% of all cases is a continuous rotation of the gripper way above 90°, which I have to abort to prevent the robot from crashing into itself. Do you have any idea how to debug this problem?

My Franka is on version 4.2.1 and I am running the latest frankx version in PyPI (0.2.0).

Best,
Tim