├── .github
└── workflows
│ ├── ci.yml
│ ├── documentation.yml
│ └── publish.yml
├── .gitignore
├── .gitmodules
├── CMakeLists.txt
├── LICENSE
├── README.md
├── doc
├── Doxyfile
└── logo.svg
├── docker
└── Dockerfile
├── examples
├── async_target.py
├── brakes.py
├── force_control.py
├── grasping.cpp
├── grasping.py
├── home.py
├── impedance.py
├── kinematics.py
├── linear.cpp
├── linear.py
├── path.py
├── reaction.py
├── read.py
└── waypoints.py
├── frankx
├── __init__.py
├── gripper.py
└── robot.py
├── include
├── frankx
│ ├── frankx.hpp
│ ├── gripper.hpp
│ ├── kinematics.hpp
│ ├── motion_generator.hpp
│ ├── motion_impedance_generator.hpp
│ ├── motion_joint_generator.hpp
│ ├── motion_path_generator.hpp
│ ├── motion_waypoint_generator.hpp
│ └── robot.hpp
└── movex
│ ├── motion
│ ├── motion_impedance.hpp
│ ├── motion_joint.hpp
│ ├── motion_path.hpp
│ └── motion_waypoint.hpp
│ ├── path
│ ├── path.hpp
│ ├── segment.hpp
│ ├── time_parametrization.hpp
│ └── trajectory.hpp
│ ├── robot
│ ├── kinematics.hpp
│ ├── measure.hpp
│ ├── motion_data.hpp
│ ├── reaction.hpp
│ └── robot_state.hpp
│ └── waypoint.hpp
├── notebooks
├── kinematics.nb
└── polynomial-blending.nb
├── setup.py
├── src
├── frankx
│ ├── gripper.cpp
│ ├── kinematics.cpp
│ ├── python.cpp
│ └── robot.cpp
└── movex
│ ├── path.cpp
│ └── python.cpp
└── test
├── joint_plot.py
├── kinematics-test.cpp
├── null-space.py
├── path-test.cpp
├── path_plot.py
├── trajectory_plot.py
└── unit-test.cpp
/.github/workflows/ci.yml:
--------------------------------------------------------------------------------
1 | name: CI
2 |
3 | on: [push, pull_request]
4 |
5 | jobs:
6 | build:
7 | runs-on: ubuntu-latest
8 |
9 | steps:
10 | - name: Checkout repository and submodules
11 | uses: actions/checkout@v2
12 | with:
13 | submodules: recursive
14 |
15 | - name: Install apt dependencies
16 | run: |
17 | sudo apt-get update
18 | sudo apt-get -y install build-essential cmake git libpoco-dev
19 |
20 | - name: Install Eigen3
21 | run: |
22 | git clone https://github.com/eigenteam/eigen-git-mirror.git
23 | cd eigen-git-mirror
24 | git checkout 3.3.7
25 | mkdir build && cd build
26 | cmake ..
27 | sudo make install
28 |
29 | - name: Install libfranka
30 | run: |
31 | git clone --recursive https://github.com/frankaemika/libfranka.git
32 | cd libfranka
33 | git checkout 0.9.0
34 | git submodule update
35 | mkdir build && cd build
36 | cmake -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF ..
37 | make -j2
38 | sudo make install
39 |
40 | - name: Install pybind11
41 | run: |
42 | git clone https://github.com/pybind/pybind11.git
43 | cd pybind11
44 | git checkout v2.9.1
45 | mkdir build && cd build
46 | cmake -DPYBIND11_TEST=OFF ..
47 | make -j2
48 | sudo make install
49 |
50 | - name: Install Catch2
51 | run: |
52 | git clone https://github.com/catchorg/Catch2.git
53 | cd Catch2
54 | git checkout v2.13.0
55 | mkdir build && cd build
56 | cmake -DCATCH_BUILD_TESTING=OFF -DCATCH_ENABLE_WERROR=OFF -DCATCH_INSTALL_DOCS=OFF -DCATCH_INSTALL_HELPERS=OFF ..
57 | sudo make install
58 |
59 | - name: Configure & make
60 | run: |
61 | mkdir build && cd build
62 | cmake ..
63 | make -j2
64 |
65 | - name: Test
66 | run: make test
67 |
--------------------------------------------------------------------------------
/.github/workflows/documentation.yml:
--------------------------------------------------------------------------------
1 | name: Documentation
2 |
3 | on:
4 | push:
5 | branches:
6 | - master
7 |
8 | jobs:
9 | build-deploy:
10 | runs-on: ubuntu-latest
11 | steps:
12 | - uses: actions/checkout@master
13 |
14 | - name: dependencies
15 | env:
16 | dependency_packages: doxygen
17 | run: sudo apt-get update && sudo apt-get -y install ${dependency_packages}
18 |
19 | - name: build
20 | run: cd doc && doxygen ./Doxyfile
21 |
22 | - name: deploy
23 | uses: peaceiris/actions-gh-pages@v3
24 | with:
25 | deploy_key: ${{ secrets.ACTIONS_DEPLOY_KEY }}
26 | publish_dir: ./doc/html
27 |
--------------------------------------------------------------------------------
/.github/workflows/publish.yml:
--------------------------------------------------------------------------------
1 | name: Publish
2 |
3 | on:
4 | release:
5 | types: [created]
6 |
7 | jobs:
8 | publish:
9 | runs-on: ubuntu-latest
10 |
11 | container:
12 | image: quay.io/pypa/manylinux2014_x86_64
13 |
14 | steps:
15 | - name: Checkout repository and submodules
16 | uses: actions/checkout@v2
17 | with:
18 | submodules: recursive
19 |
20 | - name: Install build essentials
21 | run: |
22 | yum -y install poco-devel*
23 |
24 | /opt/python/cp36-cp36m/bin/pip install cmake setuptools wheel --user
25 | /opt/python/cp37-cp37m/bin/pip install cmake setuptools wheel --user
26 | /opt/python/cp38-cp38/bin/pip install cmake setuptools wheel --user
27 | /opt/python/cp39-cp39/bin/pip install cmake setuptools wheel --user
28 |
29 | ln -f -s $HOME/.local/bin/cmake /usr/bin/cmake
30 |
31 | - name: Install Eigen3
32 | run: |
33 | echo $PATH
34 | git clone https://github.com/eigenteam/eigen-git-mirror.git
35 | cd eigen-git-mirror
36 | git checkout 3.3.7
37 | mkdir build && cd build
38 | cmake ..
39 | make install -j2
40 |
41 | - name: Install libfranka
42 | run: |
43 | git clone --recursive https://github.com/frankaemika/libfranka.git
44 | cd libfranka
45 | git checkout 0.9.0
46 | git submodule update
47 | mkdir build && cd build
48 | cmake -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF ..
49 | make -j2
50 | make install
51 |
52 | - name: Install pybind11
53 | run: |
54 | git clone https://github.com/pybind/pybind11.git
55 | cd pybind11
56 | git checkout v2.9.1
57 | mkdir build && cd build
58 | cmake -DPYBIND11_TEST=OFF ..
59 | make -j2
60 | make install
61 |
62 | - name: Install Catch2
63 | run: |
64 | git clone https://github.com/catchorg/Catch2.git
65 | cd Catch2
66 | git checkout v2.13.0
67 | mkdir build && cd build
68 | cmake -DCATCH_BUILD_TESTING=OFF -DCATCH_ENABLE_WERROR=OFF -DCATCH_INSTALL_DOCS=OFF -DCATCH_INSTALL_HELPERS=OFF ..
69 | make install
70 |
71 | - name: Python package
72 | run: |
73 | mkdir -p wheels
74 | mkdir -p dist
75 |
76 | # Compile wheels
77 | for PYBIN in /opt/python/*/bin; do
78 | if [[ "${PYBIN}" == *"cp36m"* ]] || [[ "${PYBIN}" == *"cp37m"* ]] || [[ "${PYBIN}" == *"cp38"* ]] || [[ "${PYBIN}" == *"cp39"* ]]; then
79 | "${PYBIN}/pip" wheel . -w wheels/
80 | fi
81 | done
82 |
83 | # Bundle external shared libraries into the wheels
84 | for whl in wheels/*.whl; do
85 | auditwheel repair "$whl" -w dist/
86 | done
87 |
88 | # Install packages and test
89 | for PYBIN in /opt/python/*/bin/; do
90 | if [[ "${PYBIN}" == *"cp36m"* ]] || [[ "${PYBIN}" == *"cp37m"* ]] || [[ "${PYBIN}" == *"cp38"* ]] || [[ "${PYBIN}" == *"cp39"* ]]; then
91 | "${PYBIN}/pip" install frankx --no-index -f dist
92 | # (cd "$HOME"; "${PYBIN}/nosetests" -w /io/tests)
93 | fi
94 | done
95 |
96 | ls dist
97 |
98 | - name: Publish python package
99 | uses: pypa/gh-action-pypi-publish@master
100 | with:
101 | user: keNB2
102 | password: ${{ secrets.pypi_password }}
103 |
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | build
2 | .vscode
3 | .mypy_cache
4 | cmake-local.sh
5 | dist
6 | *.egg-info
7 | __pycache__
8 | geckodriver.log
9 | doc/html
10 | tmp
11 | *.pyc
--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "ruckig"]
2 | path = ruckig
3 | url = ../ruckig.git
4 | [submodule "affx"]
5 | path = affx
6 | url = ../affx.git
7 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.11)
2 |
3 |
4 | project(frankx VERSION 0.2.0 LANGUAGES CXX)
5 |
6 |
7 | include(GNUInstallDirs)
8 |
9 |
10 | option(BUILD_EXAMPLES "Build example programs" ON)
11 | option(BUILD_PYTHON_MODULE "Build python module" ON)
12 | option(BUILD_TESTS "Build tests" ON)
13 | option(USE_PYTHON_EXTENSION "Use python in frankx library" ON)
14 |
15 |
16 | find_package(Eigen3 3.3.7 REQUIRED NO_MODULE)
17 | find_package(Franka 0.8 REQUIRED)
18 |
19 | message("Found Eigen Version: ${Eigen3_VERSION}")
20 | message("Found Franka Version: ${Franka_VERSION}")
21 |
22 |
23 | add_subdirectory(affx)
24 | add_subdirectory(ruckig)
25 |
26 |
27 | add_library(movex src/movex/path.cpp)
28 | target_compile_features(movex PUBLIC cxx_std_17)
29 | target_include_directories(movex PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
30 | target_link_libraries(movex PUBLIC affx ruckig)
31 |
32 |
33 | add_library(frankx
34 | src/frankx/gripper.cpp
35 | src/frankx/kinematics.cpp
36 | src/frankx/robot.cpp
37 | )
38 | target_compile_features(frankx PUBLIC cxx_std_17)
39 | target_include_directories(frankx PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
40 | target_link_libraries(frankx PUBLIC Franka::Franka Eigen3::Eigen movex)
41 |
42 |
43 | if(BUILD_PYTHON_MODULE)
44 | # Check if pybind11 exists as a subdirectory
45 | if(EXISTS pybind11)
46 | add_subdirectory(pybind11)
47 | else()
48 | find_package(Python COMPONENTS Interpreter Development)
49 | find_package(pybind11 2.6 REQUIRED)
50 | endif()
51 |
52 | if(USE_PYTHON_EXTENSION)
53 | target_compile_definitions(frankx PUBLIC WITH_PYTHON)
54 | target_link_libraries(frankx PUBLIC pybind11::embed)
55 | endif()
56 |
57 | pybind11_add_module(_frankx src/frankx/python.cpp)
58 | target_link_libraries(_frankx PUBLIC frankx)
59 |
60 | # Movex python package for development
61 | pybind11_add_module(_movex src/movex/python.cpp)
62 | target_compile_features(_movex PUBLIC cxx_std_17)
63 | target_include_directories(_movex PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include pybind11::embed)
64 | target_link_libraries(_movex PUBLIC Eigen3::Eigen movex)
65 | endif()
66 |
67 |
68 | if(BUILD_EXAMPLES)
69 | foreach(example IN ITEMS linear grasping)
70 | add_executable(${example} "examples/${example}.cpp")
71 | target_link_libraries(${example} PRIVATE frankx)
72 | endforeach()
73 | endif()
74 |
75 |
76 | if(BUILD_TESTS)
77 | enable_testing()
78 |
79 | find_package(Catch2 REQUIRED)
80 |
81 | foreach(test IN ITEMS kinematics-test unit-test path-test)
82 | add_executable(${test} "test/${test}.cpp")
83 | target_link_libraries(${test} PRIVATE frankx Catch2::Catch2)
84 | add_test(NAME ${test} COMMAND ${test})
85 | endforeach()
86 | endif()
87 |
88 |
89 | install(TARGETS frankx
90 | ARCHIVE DESTINATION lib
91 | LIBRARY DESTINATION lib
92 | RUNTIME DESTINATION bin
93 | INCLUDES DESTINATION include
94 | )
95 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | GNU LESSER GENERAL PUBLIC LICENSE
2 | Version 3, 29 June 2007
3 |
4 | Copyright (C) 2007 Free Software Foundation, Inc.
2 |
3 |
4 | High-Level Motion Library for the Franka Emika Robot
5 |
6 |
8 |
9 |
11 |
12 |
13 |
15 |
16 |
17 |
19 |
20 |
21 |
23 |
24 |
25 |
27 |
28 |
29 |
30 | Frankx is a high-level motion library (both C++ and Python) for the Franka Emika robot. It adds a Python wrapper around [libfranka](https://frankaemika.github.io/docs/libfranka.html), while replacing necessary real-time programming with higher-level motion commands. As frankx focuses on making real-time trajectory generation easy, it allows the robot to react to unforeseen events.
31 |
32 |
33 | ## Installation
34 |
35 | To start using frankx with Python and libfranka *0.9.0*, just install it via
36 | ```bash
37 | pip install frankx
38 | ```
39 |
40 | Frankx is based on [libfranka](https://github.com/frankaemika/libfranka), [Eigen](https://eigen.tuxfamily.org) for transformation calculations and [pybind11](https://github.com/pybind/pybind11) for the Python bindings. Frankx uses the [Ruckig](https://ruckig.com) Community Version for Online Trajectory Generation (OTG). As the Franka is quite sensitive to acceleration discontinuities, it requires constrained jerk for all motions. After installing the dependencies (the exact versions can be found below), you can build and install frankx via
41 |
42 | ```bash
43 | git clone --recurse-submodules git@github.com:pantor/frankx.git
44 | cd frankx
45 | mkdir -p build
46 | cd build
47 | cmake -DBUILD_TYPE=Release ..
48 | make
49 | make install
50 | ```
51 |
52 | To use frankx, you can also include it as a subproject in your parent CMake via `add_subdirectory(frankx)` and then `target_link_libraries( libfrankx)`. If you need only the Python module, you can install frankx via
53 |
54 | ```bash
55 | pip install .
56 | ```
57 |
58 | Make sure that the built library can be found from Python by adapting your Python Path.
59 |
60 |
61 | #### Using Docker
62 |
63 | To use frankx within Docker we have supplied a [Dockerfile](docker/Dockerfile) which you currently need to build yourself:
64 |
65 | ```bash
66 | git clone https://github.com/pantor/frankx.git
67 | cd frankx/
68 | docker build -t pantor/frankx --build-arg libfranka_version=0.7.0 -f docker/Dockerfile .
69 | ```
70 |
71 | To use another version of libfranka than the default (v.0.7.0) simply change the build argument. Then, to run the container simply:
72 |
73 | ```bash
74 | docker run -it --rm --network=host --privileged pantor/frankx
75 | ```
76 |
77 | The container requires access to the host machines network *and* elevated user rights to allow the docker user to set RT capabilities of the processes run from within it.
78 |
79 |
80 | ## Tutorial
81 |
82 | Frankx comes with both a C++ and Python API that differ only regarding real-time capability. We will introduce both languages next to each other. In your C++ project, just include `include ` and link the library. For Python, just `import frankx`. As a first example, only four lines of code are needed for simple robotic motions.
83 |
84 | ```.cpp
85 | #include
86 | using namespace frankx;
87 |
88 | // Connect to the robot with the FCI IP address
89 | Robot robot("172.16.0.2");
90 |
91 | // Reduce velocity and acceleration of the robot
92 | robot.setDynamicRel(0.05);
93 |
94 | // Move the end-effector 20cm in positive x-direction
95 | auto motion = LinearRelativeMotion(Affine(0.2, 0.0, 0.0));
96 |
97 | // Finally move the robot
98 | robot.move(motion);
99 | ```
100 |
101 | The corresponding program in Python is
102 | ```.py
103 | from frankx import Affine, LinearRelativeMotion, Robot
104 |
105 | robot = Robot("172.16.0.2")
106 | robot.set_dynamic_rel(0.05)
107 |
108 | motion = LinearRelativeMotion(Affine(0.2, 0.0, 0.0))
109 | robot.move(motion)
110 | ```
111 |
112 | Furthermore, we will introduce methods for geometric calculations, for moving the robot according to different motion types, how to implement real-time reactions and changing waypoints in real time as well as controlling the gripper.
113 |
114 |
115 | ### Geometry
116 |
117 | `frankx::Affine` is a thin wrapper around [Eigen::Affine3d](https://eigen.tuxfamily.org/dox/group__TutorialGeometry.html). It is used for Cartesian poses, frames and transformation. Frankx simplifies the usage of Euler angles (default ZYX-convention).
118 | ```.cpp
119 | // Initiliaze a transformation with an (x, y, z, a=0.0, b=0.0, c=0.0) translation
120 | Affine z_translation = Affine(0.0, 0.0, 0.5);
121 |
122 | // Define a rotation transformation using the (x, y, z, a, b, c) parameter list
123 | Affine z_rotation = Affine(0.0, 0.0, 0.0, M_PI / 3, 0.0, 0.0);
124 |
125 | // Make use of the wonderful Eigen library
126 | auto combined_transformation = z_translation * z_rotation;
127 |
128 | // Get the Euler angles (a, b, c) in a vector representation
129 | Eigen::Vector3d euler_angles = combined_transformation.angles();
130 |
131 | // Get the vector representation (x, y, z, a, b, c) of an affine transformation
132 | frankx::Vector6d pose = combined_transformation.vector();
133 | ```
134 |
135 | In all cases, distances are in [m] and rotations in [rad]. Additionally, there are several helper functions for conversion between Eigen and libfranka's std::array objects. In python, this translates into
136 | ```.py
137 | z_translation = Affine(0.0, 0.0, 0.5)
138 | z_rotation = Affine(0.0, 0.0, 0.0, math.pi / 3, 0.0, 0.0)
139 | combined_transformation = z_translation * z_rotation
140 |
141 | # These two are now numpy arrays
142 | euler_angles = combined_transformation.angles()
143 | pose = combined_transformation.vector()
144 | ```
145 |
146 | As the trajectory generation works in the Euler space, please make sure to have continuous Euler angles around your working point. You can adapt this by setting the flange to end-effector transformation via `setEE(...)`.
147 |
148 |
149 | ### Robot
150 |
151 | We wrapped most of the libfanka API (including the RobotState or ErrorMessage) for Python. Moreover, we added methods to adapt the dynamics of the robot for all motions. The `rel` name denotes that this a factor of the maximum constraints of the robot.
152 | ```.py
153 | robot = Robot("172.16.0.2")
154 |
155 | # Recover from errors
156 | robot.recover_from_errors()
157 |
158 | # Set velocity, acceleration and jerk to 5% of the maximum
159 | robot.set_dynamic_rel(0.05)
160 |
161 | # Alternatively, you can define each constraint individually
162 | robot.velocity_rel = 0.2
163 | robot.acceleration_rel = 0.1
164 | robot.jerk_rel = 0.01
165 |
166 | # Get the current pose
167 | current_pose = robot.current_pose()
168 | ```
169 |
170 |
171 | ### Motion Types
172 |
173 | Frankx defines five different motion types. In python, you can use them as follows:
174 | ```.py
175 | # A point-to-point motion in the joint space
176 | m1 = JointMotion([-1.81194, 1.17910, 1.75710, -2.1416, -1.14336, 1.63304, -0.43217])
177 |
178 | # A linear motion in cartesian space
179 | m2 = LinearMotion(Affine(0.2, -0.4, 0.3, math.pi / 2, 0.0, 0.0))
180 | m3 = LinearMotion(Affine(0.2, -0.4, 0.3, math.pi / 2, 0.0, 0.0), elbow=1.7) # With target elbow angle
181 |
182 | # A linear motion in cartesian space relative to the initial position
183 | m4 = LinearRelativeMotion(Affine(0.0, 0.1, 0.0))
184 |
185 | # A more complex motion by defining multiple waypoints
186 | m5 = WaypointMotion([
187 | Waypoint(Affine(0.2, -0.4, 0.2, 0.3, 0.2, 0.1)),
188 | # The following waypoint is relative to the prior one
189 | Waypoint(Affine(0.0, 0.1, 0.0), Waypoint.ReferenceType.Relative)
190 | ])
191 |
192 | # Hold the position for [s]
193 | m6 = PositionHold(5.0)
194 | ```
195 |
196 | The real robot can be moved by applying a motion to the robot using `move`:
197 | ```.py
198 | robot.move(m1)
199 | robot.move(m2)
200 |
201 | # To use a given frame relative to the end effector
202 | camera_frame = Affine(0.1, 0.0, 0.1)
203 | robot.move(camera_frame, m3)
204 |
205 | # To change the dynamics of the motion, use MotionData
206 | data = MotionData(0.2) # Using a dynamic_rel of 0.2 (eventually multiplied with robot.dynamic_rel)
207 | robot.move(m4, data)
208 | ```
209 |
210 | Using MotionData, you can adapt the dynamics (velocity, acceleration and jerk) of a specific motion.
211 | ```.py
212 | data.velocity_rel = 1.0
213 | data.jerk_rel = 0.2
214 | ```
215 |
216 |
217 | ### Real-Time Reactions
218 |
219 | By adding reactions to the motion data, the robot can react to unforeseen events. In the Python API, you can define conditions by using a comparison between a robot's value and a given threshold. If the threshold is exceeded, the reaction fires. Following comparisons are currently implemented
220 | ```.py
221 | reaction_motion = LinearRelativeMotion(Affine(0.0, 0.0, 0.01)) # Move up for 1cm
222 |
223 | # Stop motion if the overall force is greater than 30N
224 | d1 = MotionData().with_reaction(Reaction(Measure.ForceXYZNorm() > 30.0))
225 |
226 | # Apply reaction motion if the force in negative z-direction is greater than 10N
227 | d2 = MotionData().with_reaction(Reaction(Measure.ForceZ() < -10.0), reaction_motion)
228 |
229 | # Stop motion if its duration is above 30s
230 | d3 = MotionData().with_reaction(Reaction(Measure.Time() >= 30.0))
231 |
232 | robot.move(m2, d2)
233 |
234 | # Check if the reaction was triggered
235 | if d2.has_fired:
236 | robot.recover_from_errors()
237 | print('Force exceeded 10N!')
238 | ```
239 |
240 | Once a reaction has fired, it will be neglected furthermore. In C++ you can additionally use lambdas to define more complex behaviours:
241 | ```.cpp
242 | // Stop motion if force is over 10N
243 | auto data = MotionData()
244 | .withReaction({
245 | Measure::ForceXYZNorm() > 10.0 // [N]
246 | })
247 | .withReaction({
248 | [](const franka::RobotState& state, double time) {
249 | return (state.current_errors.self_collision_avoidance_violation);
250 | }
251 | });
252 |
253 | // Hold position for 5s
254 | robot.move(PositionHold(5.0), data); // [s]
255 | // e.g. combined with a PositionHold, the robot continues its program after pushing the end effector.
256 | ```
257 |
258 |
259 | ### Real-Time Waypoint Motion
260 |
261 | While the robot moves in a background thread, you can change the waypoints in real-time.
262 | ```.cpp
263 | robot.moveAsync(motion_hold);
264 |
265 | // Wait for key input from user
266 | std::cin.get();
267 |
268 | motion_hold.setNextWaypoint(Waypoint(Affine(0.0, 0.0, 0.1), Waypoint::ReferenceType::Relative);
269 | ```
270 |
271 |
272 | ### Gripper
273 |
274 | In the `frankx::Gripper` class, the default gripper force and gripper speed can be set. Then, additionally to the libfranka commands, the following helper methods can be used:
275 |
276 | ```.cpp
277 | auto gripper = Gripper("172.16.0.2");
278 |
279 | // These are the default values
280 | gripper.gripper_speed = 0.02; // [m/s]
281 | gripper.gripper_force = 20.0; // [N]
282 |
283 | // Preshape gripper before grasp, use the given speed
284 | gripper.move(50.0); // [mm]
285 |
286 | // Grasp an object of unknown width
287 | is_grasping = gripper.clamp();
288 |
289 | // Do something
290 | is_grasping &= gripper.isGrasping();
291 |
292 | // Release an object and move to a given distance
293 | if (is_grasping) {
294 | gripper.release(50.0);
295 | }
296 | ```
297 |
298 | The Python API should be very straight-forward for the Gripper class.
299 |
300 |
301 | ### Kinematics
302 |
303 | Frankx includes a rudimentary, non-realtime-capable forward and inverse kinematics.
304 |
305 | ```.py
306 | # Some initial joint configuration
307 | q = [-1.45549, 1.15401, 1.50061, -2.30909, -1.3141, 1.9391, 0.02815]
308 |
309 | # Calculate the forward kinematics
310 | x = Affine(Kinematics.forward(q))
311 | print('Current end effector position: ', x)
312 |
313 | # Define new target position
314 | x_new = Affine(x=0.1, y=0.0, z=0.0) * x
315 |
316 | # Franka has 7 DoFs, so what to do with the remaining Null space?
317 | null_space = NullSpaceHandling(2, 1.4) # Set elbow joint to 1.4
318 |
319 | # Inverse kinematic with target, initial joint angles, and Null space configuration
320 | q_new = Kinematics.inverse(x_new.vector(), q, null_space)
321 |
322 | print('New position: ', x_new)
323 | print('New joints: ', q_new)
324 | ```
325 |
326 |
327 | ## Documentation
328 |
329 | An auto-generated documentation can be found at [https://pantor.github.io/frankx/](https://pantor.github.io/frankx/). Moreover, there are multiple examples for both C++ and Python in the [examples](https://github.com/pantor/frankx/tree/master/examples) directory. We will add a more detailed documentation once frankx reaches v1.0.
330 |
331 |
332 | ## Development
333 |
334 | Frankx is written in C++17 and Python3.7. It is currently tested against following versions
335 |
336 | - Eigen v3.3.7
337 | - Libfranka v0.9.0
338 | - Pybind11 v2.9.1
339 | - Catch2 v2.13 (only for testing)
340 |
341 |
342 | ## License
343 |
344 | For non-commercial applications, this software is licensed under the LGPL v3.0. If you want to use frankx within commercial applications or under a different license, please contact us for individual agreements.
345 |
--------------------------------------------------------------------------------
/doc/logo.svg:
--------------------------------------------------------------------------------
1 |
2 |
4 |
5 |
6 |
7 |
10 |
11 |
12 |
25 |
26 |
37 |
38 |
39 |
40 |
42 |
43 |
--------------------------------------------------------------------------------
/docker/Dockerfile:
--------------------------------------------------------------------------------
1 | # Copyright (c) 2020, Danish Technological Institute.
2 | # All rights reserved.
3 | #
4 | # This source code is licensed under the GNU-style license found in the
5 | # LICENSE file in the root directory of this source tree.
6 | #
7 | # Original author: Nicolai Anton Lynnerup
8 |
9 | FROM ubuntu:20.04
10 |
11 | LABEL maintainer="Nicolai Anton Lynnerup & Lars Berscheid "
12 |
13 | # Set working directory
14 |
15 | RUN mkdir -p /code
16 | WORKDIR /code
17 |
18 | # ------------ #
19 | # Install deps #
20 | # ------------ #
21 |
22 | ENV DEBIAN_FRONTEND=noninteractive
23 |
24 | RUN apt-get update && apt-get install -y --no-install-recommends \
25 | git \
26 | build-essential \
27 | cmake \
28 | wget \
29 | iputils-ping \
30 | python3-dev \
31 | python3-pip \
32 | python3-setuptools \
33 | # Poco for libfranka \
34 | libpoco-dev \
35 | # Clear cache
36 | && rm -rf /var/lib/apt/lists/*
37 |
38 | RUN pip3 install numpy
39 |
40 | # -------------- #
41 | # Install Eigen3 #
42 | # -------------- #
43 |
44 | RUN git clone https://github.com/eigenteam/eigen-git-mirror.git \
45 | && cd eigen-git-mirror \
46 | && git checkout 3.3.7 \
47 | && mkdir build && cd build \
48 | && cmake .. \
49 | && make install
50 |
51 | # -------------- #
52 | # Install Franka #
53 | # -------------- #
54 |
55 | ARG libfranka_version=0.7.0
56 |
57 | RUN git clone --recursive https://github.com/frankaemika/libfranka.git \
58 | && cd libfranka \
59 | && git checkout $libfranka_version \
60 | && git submodule update \
61 | && mkdir build && cd build \
62 | && cmake -DBUILD_EXAMPLES=ON .. \
63 | && make -j$(nproc) \
64 | && make install
65 |
66 | # ---------------- #
67 | # Install PyBind11 #
68 | # ---------------- #
69 |
70 | RUN git clone https://github.com/pybind/pybind11.git \
71 | && cd pybind11 \
72 | && git checkout v2.6 \
73 | && mkdir build && cd build \
74 | && cmake -DPYBIND11_TEST=OFF .. \
75 | && make -j$(nproc) \
76 | && make install
77 |
78 | # ---------------- #
79 | # Install Catch2 #
80 | # ---------------- #
81 |
82 | RUN git clone https://github.com/catchorg/Catch2.git \
83 | && cd Catch2 \
84 | && git checkout v2.5.0 \
85 | && mkdir build && cd build \
86 | && cmake -DCATCH_BUILD_TESTING=OFF -DCATCH_ENABLE_WERROR=OFF -DCATCH_INSTALL_DOCS=OFF -DCATCH_INSTALL_HELPERS=OFF .. \
87 | && make install
88 |
89 | # ---------------- #
90 | # Build frankx #
91 | # ---------------- #
92 |
93 | RUN git clone --recursive https://github.com/pantor/frankx.git \
94 | && mkdir -p frankx/build && cd frankx/build \
95 | && cmake .. \
96 | && make -j$(nproc) \
97 | && make install \
98 | && ./unit-test
99 |
100 | ENV PYTHONPATH=$PYTHONPATH:/code/frankx/build/
101 |
--------------------------------------------------------------------------------
/examples/async_target.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 |
3 | from frankx import Affine, Robot, WaypointMotion, Waypoint
4 |
5 |
6 | if __name__ == '__main__':
7 | parser = ArgumentParser()
8 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
9 | args = parser.parse_args()
10 |
11 | robot = Robot(args.host)
12 | gripper = robot.get_gripper()
13 |
14 | robot.set_default_behavior()
15 | robot.recover_from_errors()
16 | robot.set_dynamic_rel(0.2)
17 |
18 |
19 | home_left = Waypoint(Affine(0.480, -0.15, 0.40), 1.65)
20 | home_right = Waypoint(Affine(0.450, 0.05, 0.35), 1.65)
21 |
22 | motion = WaypointMotion([home_left], return_when_finished=False)
23 | thread = robot.move_async(motion)
24 |
25 | gripper_thread = gripper.move_unsafe_async(0.05)
26 |
27 | for new_affine, new_width in [(home_right, 0.02), (home_left, 0.07), (home_right, 0.0)]:
28 | input('Press enter for new affine (also while in motion!)\n')
29 | motion.set_next_waypoint(new_affine)
30 | gripper_thread = gripper.move_unsafe_async(new_width)
31 |
32 | input('Press enter to stop\n')
33 | motion.finish()
34 | thread.join()
35 | gripper_thread.join()
36 |
--------------------------------------------------------------------------------
/examples/brakes.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 | from time import sleep
3 |
4 | from frankx import Robot
5 |
6 |
7 | if __name__ == '__main__':
8 | parser = ArgumentParser(description='Control Franka Emika Panda Desk interface remotely.')
9 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
10 | parser.add_argument('--user', default='admin', help='user name to login into Franka Desk')
11 | parser.add_argument('--password', help='password')
12 |
13 | args = parser.parse_args()
14 |
15 |
16 | with Robot(args.host, args.user, args.password) as api:
17 | # api.lock_brakes()
18 | api.unlock_brakes()
19 |
--------------------------------------------------------------------------------
/examples/force_control.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 |
3 | from time import sleep
4 | from frankx import Affine, JointMotion, LinearRelativeMotion, ImpedanceMotion, Robot, Measure, MotionData, Reaction, StopMotion
5 |
6 |
7 | if __name__ == '__main__':
8 | parser = ArgumentParser()
9 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
10 | args = parser.parse_args()
11 |
12 | robot = Robot(args.host)
13 | robot.set_default_behavior()
14 | robot.recover_from_errors()
15 |
16 | # Reduce the acceleration and velocity dynamic
17 | robot.set_dynamic_rel(0.2)
18 |
19 | joint_motion = JointMotion([-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171])
20 | robot.move(joint_motion)
21 |
22 | linear_motion = LinearRelativeMotion(Affine(z=-0.19), -0.3)
23 | linear_motion_data = MotionData().with_reaction(Reaction(Measure.ForceZ < -8.0, StopMotion())) # [N]
24 | robot.move(linear_motion, linear_motion_data)
25 |
26 | # Define and move forwards
27 | impedance_motion = ImpedanceMotion(800.0, 80.0)
28 |
29 | # BE CAREFUL HERE!
30 | # Move forward in y-direction while applying a force in z direction
31 | impedance_motion.add_force_constraint(z=-8.0) # [N]
32 | impedance_motion.set_linear_relative_target_motion(Affine(y=0.1), 2.0) # [m], [s]
33 | robot.move(impedance_motion)
34 |
--------------------------------------------------------------------------------
/examples/grasping.cpp:
--------------------------------------------------------------------------------
1 | #include
2 |
3 |
4 | using namespace frankx;
5 |
6 | inline Affine getBase(double x = 0.0, double y = 0.0, double z = 0.0, double a = 0.0, double b = 0.0, double c = 0.0) {
7 | return Affine(0.48 + x, -0.204 + y, 0.267 + z, a, b, c);
8 | }
9 |
10 |
11 | int main(int argc, char *argv[]) {
12 | if (argc != 2) {
13 | std::cerr << "Usage: " << argv[0] << " " << std::endl;
14 | return -1;
15 | }
16 |
17 | Robot robot(argv[1]);
18 | robot.automaticErrorRecovery();
19 | robot.setDefaultBehavior();
20 |
21 | auto joint_motion = JointMotion({-1.8119446041, 1.1791089121, 1.7571002245, -2.141621800, -1.1433693913, 1.6330460616, -0.4321716643});
22 | auto joint_data = MotionData().withDynamicRel(0.2);
23 | robot.move(joint_motion, joint_data);
24 |
25 | auto data = MotionData().withDynamicRel(0.5).withReaction(Reaction(
26 | Measure::ForceZ() < -7.0,
27 | std::make_shared(Affine(0.0, 0.0, 0.001))
28 | ));
29 |
30 | WaypointMotion waypoint_motion({
31 | Waypoint {getBase(0.05, 0.05, 0.0, M_PI_2)},
32 | Waypoint {getBase(0.05, 0.05, -0.185, M_PI_2)}
33 | });
34 | robot.move(waypoint_motion, data);
35 |
36 | WaypointMotion waypoint_motion2({
37 | Waypoint {getBase(0.05, 0.05, 0.0, M_PI_2)},
38 | Waypoint {getBase(0.0, 0.0, 0.0), 1.75}
39 | });
40 | robot.move(waypoint_motion2);
41 |
42 | return 0;
43 | }
44 |
--------------------------------------------------------------------------------
/examples/grasping.py:
--------------------------------------------------------------------------------
1 | from dataclasses import dataclass
2 |
3 | from frankx import *
4 |
5 |
6 | @dataclass
7 | class Grasp:
8 | """Planar grasp with stroke d"""
9 | x: float # [m]
10 | y: float # [m]
11 | z: float # [m]
12 | a: float # [m]
13 | d: float # [m]
14 |
15 |
16 | class Grasping:
17 | def __init__(self, host):
18 | self.robot = Robot(host)
19 | self.gripper = self.robot.get_gripper()
20 |
21 | self.robot.set_default_behavior()
22 | self.robot.set_dynamic_rel(0.2)
23 | self.robot.recover_from_errors()
24 |
25 | joint_motion = JointMotion([-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171])
26 | self.robot.move(joint_motion)
27 |
28 | # self.gripper.move(self.gripper.max_width)
29 |
30 | self.robot.move(LinearMotion(self.get_base(0, 0, 0), 1.75))
31 |
32 | @staticmethod
33 | def get_base(x, y, z, a=0.0, b=0.0, c=0.0):
34 | return Affine(0.48 + x, -0.204 + y, 0.37 + z, a, b, c)
35 |
36 | def grasp(self, grasp: Grasp):
37 | data_down = MotionData(0.6).with_reaction(Reaction(Measure.ForceZ < -7.0, LinearRelativeMotion(Affine(0, 0, 0))))
38 | data_up = MotionData(0.8)
39 |
40 | # self.gripper.move_async(grasp.d)
41 |
42 | motion_down = WaypointMotion([
43 | Waypoint(self.get_base(grasp.x, grasp.y, -0.04, grasp.a), 1.6),
44 | Waypoint(self.get_base(grasp.x, grasp.y, grasp.z, grasp.a), 1.3),
45 | ])
46 |
47 | move_success = self.robot.move(motion_down, data_down)
48 |
49 | if not move_success:
50 | self.robot.recover_from_errors()
51 |
52 | self.gripper.clamp()
53 |
54 | motion_up = WaypointMotion([
55 | Waypoint(self.get_base(grasp.x, grasp.y, -0.04, grasp.a), 1.6),
56 | Waypoint(self.get_base(0.0, 0.0, 0.0), 1.75),
57 | ])
58 | self.robot.move(motion_up, data_up)
59 |
60 | self.gripper.release()
61 |
62 |
63 | if __name__ == '__main__':
64 | grasping = Grasping('172.16.0.2')
65 | grasping.grasp(Grasp(x=0.05, y=0.03, z=-0.185, a=0.6, d=0.06)) # [m]
66 |
--------------------------------------------------------------------------------
/examples/home.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 |
3 | from frankx import Affine, JointMotion, LinearMotion, Robot
4 |
5 |
6 | if __name__ == '__main__':
7 | parser = ArgumentParser()
8 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
9 | args = parser.parse_args()
10 |
11 | # Connect to the robot
12 | robot = Robot(args.host)
13 | robot.set_default_behavior()
14 | robot.recover_from_errors()
15 | robot.set_dynamic_rel(0.15)
16 |
17 | # Joint motion
18 | robot.move(JointMotion([-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171]))
19 |
20 | # Define and move forwards
21 | camera_frame = Affine(y=0.05)
22 | home_pose = Affine(0.480, 0.0, 0.40)
23 |
24 | robot.move(camera_frame, LinearMotion(home_pose, 1.75))
25 |
--------------------------------------------------------------------------------
/examples/impedance.py:
--------------------------------------------------------------------------------
1 | from time import sleep
2 | from frankx import Affine, JointMotion, ImpedanceMotion, Robot
3 |
4 |
5 | if __name__ == '__main__':
6 | # Connect to the robot
7 | robot = Robot('172.16.0.2')
8 | robot.set_default_behavior()
9 | robot.recover_from_errors()
10 |
11 | # Reduce the acceleration and velocity dynamic
12 | robot.set_dynamic_rel(0.2)
13 |
14 | joint_motion = JointMotion([-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171])
15 | robot.move(joint_motion)
16 |
17 | # Define and move forwards
18 | impedance_motion = ImpedanceMotion(200.0, 20.0)
19 | robot_thread = robot.move_async(impedance_motion)
20 |
21 | sleep(0.05)
22 |
23 | initial_target = impedance_motion.target
24 | print('initial target: ', initial_target)
25 |
26 | sleep(0.5)
27 |
28 | impedance_motion.target = Affine(y=0.15) * initial_target
29 | print('set new target: ', impedance_motion.target)
30 |
31 | sleep(2.0)
32 |
33 | impedance_motion.finish()
34 | robot_thread.join()
35 | print('motion finished')
36 |
--------------------------------------------------------------------------------
/examples/kinematics.py:
--------------------------------------------------------------------------------
1 | from frankx import Affine, Kinematics, NullSpaceHandling
2 |
3 |
4 | if __name__ == '__main__':
5 | # Joint configuration
6 | q = [-1.45549, 1.15401, 1.50061, -2.30909, -1.3141, 1.9391, 0.02815]
7 |
8 | # Forward kinematic
9 | x = Affine(Kinematics.forward(q))
10 | print('Current end effector position: ', x)
11 |
12 | # Define new target position
13 | x_new = Affine(x=0.1, y=0.0, z=0.0) * x
14 |
15 | # Franka has 7 DoFs, so what to do with the remaining Null space?
16 | null_space = NullSpaceHandling(2, 1.4) # Set elbow joint to 1.4
17 |
18 | # Inverse kinematic with target, initial joint angles, and Null space configuration
19 | q_new = Kinematics.inverse(x_new.vector(), q, null_space)
20 |
21 | print('New position: ', x_new)
22 | print('New joints: ', q_new)
23 |
--------------------------------------------------------------------------------
/examples/linear.cpp:
--------------------------------------------------------------------------------
1 | #include
2 |
3 |
4 | using namespace frankx;
5 |
6 |
7 | int main(int argc, char *argv[]) {
8 | if (argc != 2) {
9 | std::cerr << "Usage: " << argv[0] << " " << std::endl;
10 | return -1;
11 | }
12 |
13 | // Connect to the robot
14 | Robot robot(argv[1]);
15 | robot.automaticErrorRecovery();
16 |
17 | // Reduce the acceleration and velocity dynamic
18 | robot.setDynamicRel(0.15);
19 |
20 | // Define and move forwards
21 | auto way = Affine(0.0, 0.2, 0.0);
22 | auto motion_forward = LinearRelativeMotion(way);
23 | robot.move(motion_forward);
24 |
25 | // And move backwards using the inverse motion
26 | auto motion_backward = LinearRelativeMotion(way.inverse());
27 | robot.move(motion_backward);
28 |
29 | return 0;
30 | }
31 |
--------------------------------------------------------------------------------
/examples/linear.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 |
3 | from frankx import Affine, LinearRelativeMotion, Robot
4 |
5 |
6 | if __name__ == '__main__':
7 | parser = ArgumentParser()
8 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
9 | args = parser.parse_args()
10 |
11 | # Connect to the robot
12 | robot = Robot(args.host)
13 | robot.set_default_behavior()
14 | robot.recover_from_errors()
15 |
16 | # Reduce the acceleration and velocity dynamic
17 | robot.set_dynamic_rel(0.15)
18 |
19 | # Define and move forwards
20 | way = Affine(0.0, 0.2, 0.0)
21 | motion_forward = LinearRelativeMotion(way)
22 | robot.move(motion_forward)
23 |
24 | # And move backwards using the inverse motion
25 | motion_backward = LinearRelativeMotion(way.inverse())
26 | robot.move(motion_backward)
27 |
--------------------------------------------------------------------------------
/examples/path.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 |
3 | from frankx import Affine, PathMotion, Robot
4 |
5 |
6 | if __name__ == '__main__':
7 | parser = ArgumentParser()
8 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
9 | args = parser.parse_args()
10 |
11 | # Connect to the robot
12 | robot = Robot(args.host)
13 | robot.set_default_behavior()
14 | robot.recover_from_errors()
15 |
16 | # Reduce the acceleration and velocity dynamic
17 | robot.set_dynamic_rel(0.15)
18 |
19 | # Define and move forwards
20 | motion = PathMotion([
21 | Affine(0.5, 0.0, 0.35),
22 | Affine(0.5, 0.0, 0.24, -0.3),
23 | Affine(0.5, -0.2, 0.35),
24 | ], blend_max_distance=0.05)
25 | robot.move(motion)
26 |
--------------------------------------------------------------------------------
/examples/reaction.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 |
3 | from frankx import Affine, JointMotion, LinearRelativeMotion, Measure, MotionData, Reaction, Robot, StopMotion
4 |
5 |
6 | if __name__ == '__main__':
7 | parser = ArgumentParser()
8 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
9 | args = parser.parse_args()
10 |
11 | # Connect to the robot
12 | robot = Robot(args.host, repeat_on_error=False)
13 | robot.set_default_behavior()
14 | robot.recover_from_errors()
15 |
16 | # Reduce the acceleration and velocity dynamic
17 | robot.set_dynamic_rel(0.2)
18 |
19 | joint_motion = JointMotion([-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171])
20 | robot.move(joint_motion)
21 |
22 | # Define and move forwards
23 | motion_down = LinearRelativeMotion(Affine(0.0, 0.0, -0.12), -0.2)
24 | motion_down_data = MotionData().with_reaction(Reaction(Measure.ForceZ < -5.0, StopMotion(Affine(0.0, 0.0, 0.002), 0.0)))
25 |
26 | # You can try to block the robot now.
27 | robot.move(motion_down, motion_down_data)
28 |
29 | if motion_down_data.did_break:
30 | print('Robot stopped at: ', robot.current_pose())
31 |
--------------------------------------------------------------------------------
/examples/read.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 | from time import sleep
3 |
4 | from frankx import Affine, Robot
5 |
6 |
7 | if __name__ == '__main__':
8 | parser = ArgumentParser()
9 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
10 | args = parser.parse_args()
11 |
12 | robot = Robot(args.host)
13 | robot.set_default_behavior()
14 |
15 | while True:
16 | state = robot.read_once()
17 | print('\nPose: ', robot.current_pose())
18 | print('O_TT_E: ', state.O_T_EE)
19 | print('Joints: ', state.q)
20 | print('Elbow: ', state.elbow)
21 | sleep(0.05)
22 |
--------------------------------------------------------------------------------
/examples/waypoints.py:
--------------------------------------------------------------------------------
1 | from argparse import ArgumentParser
2 |
3 | from frankx import Affine, JointMotion, Robot, Waypoint, WaypointMotion
4 |
5 |
6 | if __name__ == '__main__':
7 | parser = ArgumentParser()
8 | parser.add_argument('--host', default='172.16.0.2', help='FCI IP of the robot')
9 | args = parser.parse_args()
10 |
11 | # Connect to the robot
12 | robot = Robot(args.host, repeat_on_error=False)
13 | robot.set_default_behavior()
14 | robot.recover_from_errors()
15 |
16 | # Reduce the acceleration and velocity dynamic
17 | robot.set_dynamic_rel(0.2)
18 |
19 | joint_motion = JointMotion([-1.811944, 1.179108, 1.757100, -2.14162, -1.143369, 1.633046, -0.432171])
20 | robot.move(joint_motion)
21 |
22 | # Define and move forwards
23 | motion_down = WaypointMotion([
24 | Waypoint(Affine(0.0, 0.0, -0.12), -0.2, Waypoint.Relative),
25 | Waypoint(Affine(0.08, 0.0, 0.0), 0.0, Waypoint.Relative),
26 | Waypoint(Affine(0.0, 0.1, 0.0, 0.0), 0.0, Waypoint.Relative),
27 | ])
28 |
29 | # You can try to block the robot now.
30 | robot.move(motion_down)
31 |
--------------------------------------------------------------------------------
/frankx/__init__.py:
--------------------------------------------------------------------------------
1 | from pyaffx import Affine
2 |
3 | from _frankx import (
4 | Duration,
5 | Errors,
6 | GripperState,
7 | ImpedanceMotion,
8 | JointMotion,
9 | LinearMotion,
10 | LinearRelativeMotion,
11 | Kinematics,
12 | Measure,
13 | MotionData,
14 | NullSpaceHandling,
15 | PathMotion,
16 | PositionHold,
17 | Reaction,
18 | RobotMode,
19 | RobotState,
20 | StopMotion,
21 | Waypoint,
22 | WaypointMotion,
23 | )
24 |
25 | from .gripper import Gripper
26 | from .robot import Robot
--------------------------------------------------------------------------------
/frankx/gripper.py:
--------------------------------------------------------------------------------
1 | from threading import Thread
2 |
3 | from _frankx import Gripper as _Gripper
4 |
5 |
6 | class Gripper(_Gripper):
7 | def move_async(self, width) -> Thread:
8 | p = Thread(target=self.move, args=(width, ), daemon=True)
9 | p.start()
10 | return p
11 |
12 | def move_unsafe_async(self, width) -> Thread:
13 | p = Thread(target=self.move_unsafe, args=(width, ), daemon=True)
14 | p.start()
15 | return p
16 |
--------------------------------------------------------------------------------
/frankx/robot.py:
--------------------------------------------------------------------------------
1 | import base64
2 | import json
3 | import hashlib
4 | from http.client import HTTPSConnection
5 | import ssl
6 | from time import sleep
7 | from threading import Thread
8 |
9 | from _frankx import Robot as _Robot
10 | from .gripper import Gripper as _Gripper
11 |
12 |
13 | class Robot(_Robot):
14 | def __init__(self, fci_ip, dynamic_rel=1.0, user=None, password=None, repeat_on_error=True, stop_at_python_signal=True):
15 | super().__init__(fci_ip, dynamic_rel=dynamic_rel, repeat_on_error=repeat_on_error, stop_at_python_signal=stop_at_python_signal)
16 | self.hostname = fci_ip
17 | self.user = user
18 | self.password = password
19 |
20 | self.client = None
21 | self.token = None
22 |
23 | @staticmethod
24 | def _encode_password(user, password):
25 | bs = ','.join([str(b) for b in hashlib.sha256((password + '#' + user + '@franka').encode('utf-8')).digest()])
26 | return base64.encodebytes(bs.encode('utf-8')).decode('utf-8')
27 |
28 | def __enter__(self):
29 | self.client = HTTPSConnection(self.hostname, timeout=12, context=ssl._create_unverified_context()) # [s]
30 | self.client.connect()
31 | self.client.request(
32 | 'POST', '/admin/api/login',
33 | body=json.dumps({'login': self.user, 'password': self._encode_password(self.user, self.password)}),
34 | headers={'content-type': 'application/json'}
35 | )
36 | self.token = self.client.getresponse().read().decode('utf8')
37 | return self
38 |
39 | def __exit__(self, type, value, traceback):
40 | self.client.close()
41 |
42 | def start_task(self, task):
43 | self.client.request(
44 | 'POST', '/desk/api/execution',
45 | body=f'id={task}',
46 | headers={'content-type': 'application/x-www-form-urlencoded', 'Cookie': f'authorization={self.token}'}
47 | )
48 | return self.client.getresponse().read()
49 |
50 | def unlock_brakes(self):
51 | self.client.request(
52 | 'POST', '/desk/api/robot/open-brakes',
53 | headers={'content-type': 'application/x-www-form-urlencoded', 'Cookie': f'authorization={self.token}'}
54 | )
55 | return self.client.getresponse().read()
56 |
57 | def lock_brakes(self):
58 | self.client.request(
59 | 'POST', '/desk/api/robot/close-brakes',
60 | headers={'content-type': 'application/x-www-form-urlencoded', 'Cookie': f'authorization={self.token}'}
61 | )
62 | return self.client.getresponse().read()
63 |
64 | def move_async(self, *args) -> Thread:
65 | p = Thread(target=self.move, args=tuple(args), daemon=True)
66 | p.start()
67 | sleep(0.001) # Sleep one control cycle
68 | return p
69 |
70 | def get_gripper(self):
71 | return _Gripper(self.fci_ip)
72 |
--------------------------------------------------------------------------------
/include/frankx/frankx.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 |
12 | #include
13 | #include
14 |
--------------------------------------------------------------------------------
/include/frankx/gripper.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 | #include
6 |
7 | #include
8 | #include
9 |
10 |
11 | namespace frankx {
12 |
13 | class Gripper: public franka::Gripper {
14 | const double width_calibration {0.004}; // [m], Difference from gripper jaws
15 | const double min_width {0.002}; // [m]
16 |
17 | /**
18 | * Save clamp width and compare it in the is_grasping method. If it's smaller,
19 | * the gripper moves and the object is missing.
20 | */
21 | double last_clamp_width; // [m]
22 |
23 | public:
24 | static constexpr double max_speed {0.1}; // [m/s]
25 |
26 | //! Connects to a gripper at the given FCI IP address.
27 | explicit Gripper(const std::string& fci_ip, double speed = 0.04, double force = 20.0);
28 |
29 | double gripper_force {20.0}; // [N]
30 | double gripper_speed {0.04}; // [m/s]
31 | bool has_error {false};
32 |
33 | const double max_width {0.081 + width_calibration}; // [m]
34 |
35 | double width() const;
36 | bool isGrasping() const;
37 |
38 | bool move(double width); // [m]
39 | bool move_unsafe(double width); // [m]
40 | std::future moveAsync(double width); // [m]
41 |
42 | bool open();
43 | bool clamp();
44 | bool clamp(double min_clamping_width);
45 |
46 | bool release();
47 | bool release(double width); // [m]
48 | bool releaseRelative(double width); // [m]
49 |
50 |
51 | ::franka::GripperState get_state();
52 | };
53 |
54 | } // namepace frankx
55 |
--------------------------------------------------------------------------------
/include/frankx/kinematics.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | #include
6 | #include
7 |
8 |
9 | namespace frankx {
10 |
11 | struct Kinematics {
12 | template
13 | static Eigen::Matrix pseudoinverse(const MatT &mat, typename MatT::Scalar tolerance = typename MatT::Scalar{1e-4}) {
14 | typedef typename MatT::Scalar Scalar;
15 | auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
16 | const auto &singularValues = svd.singularValues();
17 | Eigen::Matrix singularValuesInv(mat.cols(), mat.rows());
18 | singularValuesInv.setZero();
19 | for (unsigned int i = 0; i < singularValues.size(); ++i) {
20 | if (singularValues(i) > tolerance) {
21 | singularValuesInv(i, i) = Scalar{1} / singularValues(i);
22 |
23 | } else {
24 | singularValuesInv(i, i) = Scalar{0};
25 | }
26 | }
27 | return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
28 | }
29 |
30 | struct NullSpaceHandling {
31 | size_t joint_index;
32 | double value;
33 |
34 | explicit NullSpaceHandling(size_t joint_index, double value): joint_index(joint_index), value(value) {}
35 | };
36 |
37 | static std::array forward(const Eigen::Matrix& q);
38 | static std::array forwardElbow(const Eigen::Matrix& q);
39 | static Eigen::Matrix forwardEuler(const Eigen::Matrix& q);
40 | static Eigen::Matrix jacobian(const Eigen::Matrix& q);
41 | static Eigen::Matrix inverse(const Eigen::Matrix& x_target, const Eigen::Matrix& q0, std::optional null_space = std::nullopt);
42 | };
43 |
44 | }
45 |
--------------------------------------------------------------------------------
/include/frankx/motion_generator.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 | #include
6 |
7 | #include
8 | #include
9 | #include
10 | #include
11 |
12 |
13 | namespace frankx {
14 | using namespace movex;
15 | using Affine = affx::Affine;
16 | using Vector6d = Eigen::Matrix;
17 | using Vector7d = Eigen::Matrix;
18 |
19 | struct MotionGenerator {
20 | static inline franka::CartesianPose CartesianPose(const Vector7d& vector, bool include_elbow = true) {
21 | auto affine = Affine(vector);
22 | if (include_elbow) {
23 | return franka::CartesianPose(affine.array(), {vector[6], -1});
24 | }
25 | return franka::CartesianPose(affine.array());
26 | }
27 |
28 | static inline franka::CartesianPose CartesianPose(const std::array& vector, bool include_elbow = true) {
29 | auto affine = Affine(vector);
30 | if (include_elbow) {
31 | return franka::CartesianPose(affine.array(), {vector[6], -1});
32 | }
33 | return franka::CartesianPose(affine.array());
34 | }
35 |
36 | template
37 | static inline std::array VectorCartRotElbow(T cart, T rot, T elbow) {
38 | return {cart, cart, cart, rot, rot, rot, elbow};
39 | }
40 |
41 | static inline void setCartRotElbowVector(Vector7d& vector, double cart, double rot, double elbow) {
42 | const std::array data = VectorCartRotElbow(cart, rot, elbow);
43 | vector = Eigen::Map(data.data(), data.size());
44 | }
45 |
46 | static inline std::array toStd(const Vector7d& vector) {
47 | std::array result;
48 | Vector7d::Map(result.data()) = vector;
49 | return result;
50 | }
51 |
52 | static inline movex::RobotState<7> convertState(const franka::RobotState& franka) {
53 | movex::RobotState<7> movex;
54 | movex.q = franka.q;
55 | movex.q_d = franka.q_d;
56 | movex.dq = franka.dq;
57 |
58 | movex.O_T_EE = franka.O_T_EE;
59 | movex.O_T_EE_c = franka.O_T_EE_c;
60 | movex.O_dP_EE_c = franka.O_dP_EE_c;
61 |
62 | movex.elbow = franka.elbow;
63 | movex.elbow_c = franka.elbow_c;
64 | movex.elbow_d = franka.elbow_d;
65 |
66 | movex.O_F_ext_hat_K = franka.O_F_ext_hat_K;
67 | return movex;
68 | }
69 |
70 | template
71 | static std::tuple, std::array, std::array> getInputLimits(RobotType* robot, const MotionData& data) {
72 | return getInputLimits(robot, Waypoint(), data);
73 | }
74 |
75 | template
76 | static std::tuple, std::array, std::array> getInputLimits(RobotType* robot, const Waypoint& waypoint, const MotionData& data) {
77 | constexpr double translation_factor {0.4};
78 | constexpr double derivative_factor {0.4};
79 |
80 | if (waypoint.max_dynamics || data.max_dynamics) {
81 | auto max_velocity = MotionGenerator::VectorCartRotElbow(
82 | 0.8 * translation_factor * robot->max_translation_velocity,
83 | 0.8 * robot->max_rotation_velocity,
84 | 0.8 * robot->max_elbow_velocity
85 | );
86 | auto max_acceleration = MotionGenerator::VectorCartRotElbow(
87 | 0.8 * translation_factor * derivative_factor * robot->max_translation_acceleration,
88 | 0.8 * derivative_factor * robot->max_rotation_acceleration,
89 | 0.8 * derivative_factor * robot->max_elbow_acceleration
90 | );
91 | auto max_jerk = MotionGenerator::VectorCartRotElbow(
92 | 0.8 * translation_factor * std::pow(derivative_factor, 2) * robot->max_translation_jerk,
93 | 0.8 * std::pow(derivative_factor, 2) * robot->max_rotation_jerk,
94 | 0.8 * std::pow(derivative_factor, 2) * robot->max_elbow_jerk
95 | );
96 | return {max_velocity, max_acceleration, max_jerk};
97 | }
98 |
99 | auto max_velocity = MotionGenerator::VectorCartRotElbow(
100 | translation_factor * waypoint.velocity_rel * data.velocity_rel * robot->velocity_rel * robot->max_translation_velocity,
101 | waypoint.velocity_rel * data.velocity_rel * robot->velocity_rel * robot->max_rotation_velocity,
102 | waypoint.velocity_rel * data.velocity_rel * robot->velocity_rel * robot->max_elbow_velocity
103 | );
104 | auto max_acceleration = MotionGenerator::VectorCartRotElbow(
105 | translation_factor * derivative_factor * data.acceleration_rel * robot->acceleration_rel * robot->max_translation_acceleration,
106 | derivative_factor * data.acceleration_rel * robot->acceleration_rel * robot->max_rotation_acceleration,
107 | derivative_factor * data.acceleration_rel * robot->acceleration_rel * robot->max_elbow_acceleration
108 | );
109 | auto max_jerk = MotionGenerator::VectorCartRotElbow(
110 | translation_factor * std::pow(derivative_factor, 2) * data.jerk_rel * robot->jerk_rel * robot->max_translation_jerk,
111 | std::pow(derivative_factor, 2) * data.jerk_rel * robot->jerk_rel * robot->max_rotation_jerk,
112 | std::pow(derivative_factor, 2) * data.jerk_rel * robot->jerk_rel * robot->max_elbow_jerk
113 | );
114 | return {max_velocity, max_acceleration, max_jerk};
115 | }
116 |
117 | template
118 | static void setInputLimits(ruckig::InputParameter<7>& input_parameters, RobotType* robot, const MotionData& data) {
119 | setInputLimits(input_parameters, robot, Waypoint(), data);
120 | }
121 |
122 | template
123 | static void setInputLimits(ruckig::InputParameter<7>& input_parameters, RobotType* robot, const Waypoint& waypoint, const MotionData& data) {
124 | std::tie(input_parameters.max_velocity, input_parameters.max_acceleration, input_parameters.max_jerk) = getInputLimits(robot, waypoint, data);
125 |
126 | if (!(waypoint.max_dynamics || data.max_dynamics) && waypoint.minimum_time.has_value()) {
127 | input_parameters.minimum_duration = waypoint.minimum_time.value();
128 | }
129 |
130 | if (waypoint.max_dynamics) {
131 | input_parameters.synchronization = ruckig::Synchronization::TimeIfNecessary;
132 | } else {
133 | input_parameters.synchronization = ruckig::Synchronization::Time;
134 | }
135 | }
136 | };
137 |
138 |
139 | // Stateful functor to get values after the control function
140 | template
141 | class StatefulFunctor {
142 | OriginalFunctor &fun;
143 |
144 | public:
145 | StatefulFunctor() = delete;
146 | StatefulFunctor(OriginalFunctor &orig) : fun(orig) {}
147 | StatefulFunctor(StatefulFunctor const &other) : fun(other.fun) {}
148 | StatefulFunctor(StatefulFunctor &&other) : fun(other.fun) {}
149 |
150 | template
151 | RType operator() (Args&&... args) {
152 | return fun(std::forward(args)...);
153 | }
154 | };
155 |
156 |
157 | template
158 | StatefulFunctor stateful(OF &fun) {
159 | return StatefulFunctor(fun);
160 | }
161 |
162 | } // namespace frankx
163 |
--------------------------------------------------------------------------------
/include/frankx/motion_impedance_generator.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | #include
7 | #include
8 | #include
9 |
10 |
11 | namespace frankx {
12 | using namespace movex;
13 |
14 | template
15 | struct ImpedanceMotionGenerator: public MotionGenerator {
16 | double time {0.0}, motion_init_time {0.0};
17 | RobotType* robot;
18 |
19 | Eigen::Matrix stiffness, damping;
20 | Affine initial_affine;
21 | Eigen::Vector3d position_d;
22 | Eigen::Quaterniond orientation_d;
23 |
24 | franka::RobotState initial_state;
25 | franka::Model* model;
26 |
27 | Affine frame;
28 | ImpedanceMotion& motion;
29 | MotionData& data;
30 |
31 | explicit ImpedanceMotionGenerator(RobotType* robot, const Affine& frame, ImpedanceMotion& motion, MotionData& data): robot(robot), frame(frame), motion(motion), data(data) {
32 | if (motion.type == ImpedanceMotion::Type::Joint) {
33 | throw std::runtime_error("joint impedance is not implemented yet.");
34 | }
35 |
36 | stiffness.setZero();
37 | stiffness.topLeftCorner(3, 3) << motion.translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
38 | stiffness.bottomRightCorner(3, 3) << motion.rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
39 | damping.setZero();
40 | damping.topLeftCorner(3, 3) << 2.0 * sqrt(motion.translational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
41 | damping.bottomRightCorner(3, 3) << 2.0 * sqrt(motion.rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
42 |
43 | initial_state = robot->readOnce();
44 | model = new franka::Model(robot->loadModel());
45 |
46 | initial_affine = Affine(initial_state.O_T_EE);
47 | position_d = Eigen::Vector3d(initial_affine.translation());
48 | orientation_d = Eigen::Quaterniond(initial_affine.quaternion());
49 | }
50 |
51 | void init(const franka::RobotState& robot_state, franka::Duration period) {
52 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
53 | motion.target = Affine(transform);
54 | motion.is_active = true;
55 | }
56 |
57 | franka::Torques operator()(const franka::RobotState& robot_state, franka::Duration period) {
58 | time += period.toSec();
59 | if (time == 0.0) {
60 | init(robot_state, period);
61 | }
62 |
63 | std::array coriolis_array = model->coriolis(robot_state);
64 | std::array jacobian_array = model->zeroJacobian(franka::Frame::kEndEffector, robot_state);
65 |
66 | Eigen::Map> coriolis(coriolis_array.data());
67 | Eigen::Map> jacobian(jacobian_array.data());
68 | Eigen::Map> q(robot_state.q.data());
69 | Eigen::Map> dq(robot_state.dq.data());
70 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
71 | Eigen::Vector3d position(transform.translation());
72 | Eigen::Quaterniond orientation(transform.linear());
73 |
74 | Eigen::Matrix error;
75 | error.head(3) << position - position_d;
76 |
77 | if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
78 | orientation.coeffs() << -orientation.coeffs();
79 | }
80 |
81 | Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
82 | error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
83 | error.tail(3) << -transform.linear() * error.tail(3);
84 |
85 | Eigen::VectorXd wrench_cartesian(6), tau_task(7), tau_d(7);
86 | wrench_cartesian = -stiffness * error - damping * (jacobian * dq);
87 |
88 | // Force constraints
89 | for (const auto force_constraint : motion.force_constraints) {
90 | switch (force_constraint.first) {
91 | case ImpedanceMotion::Axis::X: {
92 | wrench_cartesian(0) = force_constraint.second;
93 | } break;
94 | case ImpedanceMotion::Axis::Y: {
95 | wrench_cartesian(1) = force_constraint.second;
96 | } break;
97 | case ImpedanceMotion::Axis::Z: {
98 | wrench_cartesian(2) = force_constraint.second;
99 | } break;
100 | }
101 | }
102 |
103 | tau_task << jacobian.transpose() * wrench_cartesian;
104 | tau_d << tau_task + coriolis;
105 |
106 | std::array tau_d_array{};
107 | Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
108 |
109 | #ifdef WITH_PYTHON
110 | if (robot->stop_at_python_signal && Py_IsInitialized() && PyErr_CheckSignals() == -1) {
111 | motion.is_active = false;
112 | return franka::MotionFinished(franka::Torques(tau_d_array));
113 | }
114 | #endif
115 |
116 | if (motion.should_finish) {
117 | motion.is_active = false;
118 | return franka::MotionFinished(franka::Torques(tau_d_array));
119 | }
120 |
121 | // Update target with target motion
122 | switch (motion.target_motion) {
123 | case ImpedanceMotion::TargetMotion::Exponential: {
124 | position_d = motion.exponential_decay * motion.target.translation() + (1.0 - motion.exponential_decay) * position_d;
125 | orientation_d = orientation_d.slerp(motion.exponential_decay, motion.target.quaternion());
126 | } break;
127 | case ImpedanceMotion::TargetMotion::Linear: {
128 | if (!motion.linear_motion.initialized) {
129 | initial_affine = Affine(Eigen::Affine3d(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())));
130 | motion_init_time = time;
131 | motion.linear_motion.initialized = true;
132 | }
133 |
134 | double transition_parameter = (time - motion_init_time) / (motion.linear_motion.duration);
135 | if (transition_parameter <= 1.0) { // [ms] to [s]
136 | motion.target = initial_affine.slerp(motion.linear_motion.relative_target * initial_affine, transition_parameter);
137 | position_d = motion.target.translation();
138 | orientation_d = motion.target.quaternion();
139 | } else if (motion.linear_motion.finish_after && transition_parameter > motion.linear_motion.finish_wait_factor) { // Wait a bit longer to stop
140 | motion.should_finish = true;
141 | }
142 | } break;
143 | case ImpedanceMotion::TargetMotion::Spiral: {
144 | if (!motion.spiral_motion.initialized) {
145 | motion_init_time = time;
146 | motion.spiral_motion.initialized = true;
147 | }
148 |
149 | double time_diff = motion_init_time - time;
150 | // target = spiral_motion.center * Affine(0, 0, 0, 2 * pi * spiral_motion.revolutions_per_second * time) * Affine(spiral_motion.radius_increment_per_revolution * time);
151 | // position_d = target.translation();
152 | // orientation_d = target.quaternion();
153 | } break;
154 | }
155 |
156 | return franka::Torques(tau_d_array);
157 | }
158 | };
159 |
160 | } // namespace frankx
161 |
--------------------------------------------------------------------------------
/include/frankx/motion_joint_generator.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | #include
7 | #include
8 | #include
9 | #include
10 |
11 |
12 | namespace frankx {
13 | using namespace movex;
14 |
15 | template
16 | struct JointMotionGenerator: public MotionGenerator {
17 | ruckig::Ruckig trajectory_generator {RobotType::control_rate};
18 |
19 | ruckig::InputParameter input_para;
20 | ruckig::OutputParameter output_para;
21 | ruckig::Result result;
22 |
23 | std::array joint_positions;
24 | double time {0.0};
25 | RobotType* robot;
26 |
27 | JointMotion motion;
28 | MotionData& data;
29 |
30 | explicit JointMotionGenerator(RobotType* robot, JointMotion motion, MotionData& data): robot(robot), motion(motion), data(data) { }
31 |
32 | void init(const franka::RobotState& robot_state, franka::Duration period) {
33 | input_para.current_position = robot_state.q_d;
34 | input_para.current_velocity = toStd(Vector7d::Zero());
35 | input_para.current_acceleration = toStd(Vector7d::Zero());
36 |
37 | input_para.target_position = toStd(motion.target);
38 | input_para.target_velocity = toStd(Vector7d::Zero());
39 | input_para.target_acceleration = toStd(Vector7d::Zero());
40 |
41 | for (size_t dof = 0; dof < RobotType::degrees_of_freedoms; dof += 1) {
42 | input_para.max_velocity[dof] = RobotType::max_joint_velocity[dof] * robot->velocity_rel * data.velocity_rel;
43 | input_para.max_acceleration[dof] = 0.3 * RobotType::max_joint_acceleration[dof] * robot->acceleration_rel * data.acceleration_rel;
44 | input_para.max_jerk[dof] = 0.3 * RobotType::max_joint_jerk[dof] * robot->jerk_rel * data.jerk_rel;
45 | }
46 | }
47 |
48 | franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period) {
49 | time += period.toSec();
50 | if (time == 0.0) {
51 | init(robot_state, period);
52 | }
53 |
54 | #ifdef WITH_PYTHON
55 | if (robot->stop_at_python_signal && Py_IsInitialized() && PyErr_CheckSignals() == -1) {
56 | robot->stop();
57 | }
58 | #endif
59 |
60 | const int steps = std::max(period.toMSec(), 1);
61 | for (int i = 0; i < steps; i++) {
62 | result = trajectory_generator.update(input_para, output_para);
63 | joint_positions = output_para.new_position;
64 |
65 | if (result == ruckig::Result::Finished) {
66 | joint_positions = input_para.target_position;
67 | return franka::MotionFinished(franka::JointPositions(joint_positions));
68 |
69 | } else if (result == ruckig::Result::Error) {
70 | std::cout << "[frankx robot] Invalid inputs:" << std::endl;
71 | return franka::MotionFinished(franka::JointPositions(joint_positions));
72 | }
73 |
74 | output_para.pass_to_input(input_para);
75 | }
76 |
77 | return franka::JointPositions(joint_positions);
78 | }
79 | };
80 |
81 | } // namespace frankx
82 |
--------------------------------------------------------------------------------
/include/frankx/motion_path_generator.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 |
14 | namespace frankx {
15 | using namespace movex;
16 |
17 | template
18 | struct PathMotionGenerator: public MotionGenerator {
19 | size_t trajectory_index {0};
20 | double s_current {0.0};
21 | const bool use_elbow {false};
22 | double time {0.0};
23 |
24 | Trajectory trajectory {};
25 |
26 | RobotType* robot;
27 | Affine frame;
28 | PathMotion motion;
29 | MotionData& data;
30 |
31 | explicit PathMotionGenerator(RobotType* robot, const Affine& frame, PathMotion motion, MotionData& data): robot(robot), frame(frame), motion(motion), data(data) {
32 | // Insert current pose into beginning of path
33 | auto initial_state = robot->readOnce();
34 | franka::CartesianPose initial_cartesian_pose(initial_state.O_T_EE_c, initial_state.elbow_c);
35 | Affine initial_pose(initial_cartesian_pose.O_T_EE);
36 |
37 | Waypoint start_waypoint {initial_pose * frame, initial_cartesian_pose.elbow[0]};
38 | auto all_waypoints = motion.waypoints;
39 | all_waypoints.insert(all_waypoints.begin(), start_waypoint);
40 |
41 | // Create path
42 | const Path path {all_waypoints};
43 |
44 | // Get time parametrization
45 | TimeParametrization time_parametrization {RobotType::control_rate};
46 | const auto [max_velocity, max_acceleration, max_jerk] = getInputLimits(robot, data);
47 | trajectory = time_parametrization.parametrize(path, max_velocity, max_acceleration, max_jerk);
48 | }
49 |
50 | franka::CartesianPose operator()(const franka::RobotState& robot_state, franka::Duration period) {
51 | time += period.toSec();
52 |
53 | #ifdef WITH_PYTHON
54 | if (robot->stop_at_python_signal && Py_IsInitialized() && PyErr_CheckSignals() == -1) {
55 | robot->stop();
56 | }
57 | #endif
58 |
59 | const int steps = std::max(period.toMSec(), 1);
60 | trajectory_index += steps;
61 | if (trajectory_index >= trajectory.states.size()) {
62 | s_current = trajectory.path.get_length();
63 | return franka::MotionFinished(CartesianPose(trajectory.path.q(s_current, frame), use_elbow));
64 | }
65 |
66 | s_current = trajectory.states[trajectory_index].s;
67 | return CartesianPose(trajectory.path.q(s_current, frame), use_elbow);
68 | }
69 | };
70 |
71 | } // namespace frankx
72 |
--------------------------------------------------------------------------------
/include/frankx/motion_waypoint_generator.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | #include
7 | #include
8 | #include
9 | #include
10 |
11 |
12 | namespace frankx {
13 | using namespace movex;
14 | using Affine = affx::Affine;
15 |
16 | template
17 | struct WaypointMotionGenerator: public MotionGenerator {
18 | ruckig::Ruckig trajectory_generator {RobotType::control_rate};
19 | ruckig::InputParameter input_para;
20 | ruckig::OutputParameter output_para;
21 | ruckig::Result result;
22 |
23 | WaypointMotion current_motion;
24 | std::vector::iterator waypoint_iterator;
25 | bool waypoint_has_elbow {false};
26 |
27 | Vector7d old_vector;
28 | Affine old_affine;
29 | double old_elbow;
30 | double time {0.0};
31 | RobotType* robot;
32 |
33 | Affine frame;
34 | WaypointMotion& motion;
35 | MotionData& data;
36 |
37 | bool set_target_at_zero_time {true};
38 |
39 | const size_t cooldown_iterations {5};
40 | size_t current_cooldown_iteration {0};
41 |
42 | explicit WaypointMotionGenerator(RobotType* robot, const Affine& frame, WaypointMotion& motion, MotionData& data): robot(robot), frame(frame), motion(motion), current_motion(motion), data(data) { }
43 |
44 | void reset() {
45 | time = 0.0;
46 | current_cooldown_iteration = 0;
47 | set_target_at_zero_time = false;
48 | }
49 |
50 | void init(const franka::RobotState& robot_state, franka::Duration period) {
51 | franka::CartesianPose initial_cartesian_pose(robot_state.O_T_EE_c, robot_state.elbow_c);
52 | Affine initial_pose(initial_cartesian_pose.O_T_EE);
53 |
54 | Vector7d initial_vector = initial_pose.vector_with_elbow(initial_cartesian_pose.elbow[0]);
55 | Vector7d initial_velocity = (Vector7d() << robot_state.O_dP_EE_c[0], robot_state.O_dP_EE_c[1], robot_state.O_dP_EE_c[2], robot_state.O_dP_EE_c[3], robot_state.O_dP_EE_c[4], robot_state.O_dP_EE_c[5], robot_state.delbow_c[0]).finished();
56 |
57 | old_affine = initial_pose;
58 | old_vector = initial_vector;
59 | old_elbow = old_vector(6);
60 |
61 | input_para.current_position = toStd(initial_vector);
62 | input_para.current_velocity = toStd(initial_velocity);
63 | input_para.current_acceleration = toStd(Vector7d::Zero());
64 |
65 | input_para.enabled = MotionGenerator::VectorCartRotElbow(true, true, true);
66 | setInputLimits(input_para, robot, data);
67 |
68 | if (set_target_at_zero_time) {
69 | waypoint_iterator = current_motion.waypoints.begin();
70 |
71 | const auto current_waypoint = *waypoint_iterator;
72 | waypoint_has_elbow = current_waypoint.elbow.has_value();
73 | auto target_position_vector = current_waypoint.getTargetVector(frame, old_affine, old_elbow);
74 |
75 | input_para.enabled = {true, true, true, true, true, true, waypoint_has_elbow};
76 | input_para.target_position = toStd(target_position_vector);
77 | input_para.target_velocity = toStd(Vector7d::Zero());
78 | setInputLimits(input_para, robot, current_waypoint, data);
79 |
80 | old_affine = current_waypoint.getTargetAffine(frame, old_affine);
81 | old_vector = target_position_vector;
82 | old_elbow = old_vector(6);
83 | }
84 | }
85 |
86 | franka::CartesianPose operator()(const franka::RobotState& robot_state, franka::Duration period) {
87 | time += period.toSec();
88 | if (time == 0.0) {
89 | init(robot_state, period);
90 | }
91 |
92 | for (auto& reaction : data.reactions) {
93 | if (reaction.has_fired) {
94 | continue;
95 | }
96 |
97 | if (reaction.condition(MotionGenerator::convertState(robot_state), time)) {
98 | std::cout << "[frankx] reaction fired." << std::endl;
99 | reaction.has_fired = true;
100 |
101 | bool new_motion = false;
102 |
103 | if (reaction.waypoint_action.has_value()) {
104 | new_motion = true;
105 | current_motion = reaction.waypoint_action.value()(MotionGenerator::convertState(robot_state), time);
106 | } else if (reaction.waypoint_motion.has_value()) {
107 | new_motion = true;
108 | current_motion = *(reaction.waypoint_motion.value());
109 | } else {
110 | robot->stop();
111 | }
112 |
113 | if (new_motion) {
114 | waypoint_iterator = current_motion.waypoints.begin();
115 |
116 | franka::CartesianPose current_cartesian_pose(robot_state.O_T_EE_c, robot_state.elbow_c);
117 | Affine current_pose(current_cartesian_pose.O_T_EE);
118 | auto current_vector = current_pose.vector_with_elbow(current_cartesian_pose.elbow[0]);
119 | old_affine = current_pose;
120 | old_vector = current_vector;
121 | old_elbow = old_vector(6);
122 |
123 | const auto current_waypoint = *waypoint_iterator;
124 | waypoint_has_elbow = current_waypoint.elbow.has_value();
125 | auto target_position_vector = current_waypoint.getTargetVector(Affine(), old_affine, old_elbow);
126 |
127 | input_para.enabled = {true, true, true, true, true, true, waypoint_has_elbow};
128 | input_para.target_position = toStd(target_position_vector);
129 | input_para.target_velocity = toStd(Vector7d::Zero());
130 | setInputLimits(input_para, robot, current_waypoint, data);
131 |
132 | old_affine = current_waypoint.getTargetAffine(Affine(), old_affine);
133 | old_vector = target_position_vector;
134 | old_elbow = old_vector(6);
135 | } else {
136 | return franka::MotionFinished(MotionGenerator::CartesianPose(input_para.current_position, waypoint_has_elbow));
137 | }
138 | }
139 | }
140 |
141 | #ifdef WITH_PYTHON
142 | if (robot->stop_at_python_signal && Py_IsInitialized() && PyErr_CheckSignals() == -1) {
143 | robot->stop();
144 | }
145 | #endif
146 |
147 | const int steps = std::max(period.toMSec(), 1);
148 | for (int i = 0; i < steps; i++) {
149 | result = trajectory_generator.update(input_para, output_para);
150 |
151 | if (motion.reload || result == ruckig::Result::Finished) {
152 | bool has_new_waypoint {false};
153 |
154 | if (waypoint_iterator != current_motion.waypoints.end()) {
155 | ++waypoint_iterator;
156 | has_new_waypoint = (waypoint_iterator != current_motion.waypoints.end());
157 | }
158 |
159 | if (motion.return_when_finished && waypoint_iterator >= current_motion.waypoints.end()) {
160 | // Allow cooldown of motion, so that the low-pass filter has time to adjust to target values
161 | if (current_cooldown_iteration < cooldown_iterations) {
162 | current_cooldown_iteration += 1;
163 | return MotionGenerator::CartesianPose(input_para.target_position, waypoint_has_elbow);
164 | }
165 |
166 | return franka::MotionFinished(MotionGenerator::CartesianPose(input_para.target_position, waypoint_has_elbow));
167 |
168 | } else if (motion.reload) {
169 | current_motion = motion;
170 | waypoint_iterator = current_motion.waypoints.begin();
171 | motion.reload = false;
172 | current_motion.reload = false;
173 | has_new_waypoint = true;
174 | }
175 |
176 | if (has_new_waypoint) {
177 | const auto current_waypoint = *waypoint_iterator;
178 | waypoint_has_elbow = current_waypoint.elbow.has_value();
179 | auto target_position_vector = current_waypoint.getTargetVector(frame, old_affine, old_elbow);
180 |
181 | input_para.enabled = {true, true, true, true, true, true, waypoint_has_elbow};
182 | input_para.target_position = toStd(target_position_vector);
183 | input_para.target_velocity = toStd(Vector7d::Zero());
184 | setInputLimits(input_para, robot, current_waypoint, data);
185 |
186 | old_affine = current_waypoint.getTargetAffine(frame, old_affine);
187 | old_vector = target_position_vector;
188 | old_elbow = old_vector(6);
189 | }
190 |
191 | } else if (result == ruckig::Result::Error) {
192 | std::cout << "[frankx robot] Invalid inputs:" << std::endl;
193 | return franka::MotionFinished(MotionGenerator::CartesianPose(input_para.current_position, waypoint_has_elbow));
194 | }
195 |
196 | output_para.pass_to_input(input_para);
197 | }
198 |
199 | return MotionGenerator::CartesianPose(output_para.new_position, waypoint_has_elbow);
200 | }
201 | };
202 |
203 | } // namespace frankx
204 |
--------------------------------------------------------------------------------
/include/frankx/robot.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #ifdef WITH_PYTHON
4 | #include
5 | #endif
6 |
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 |
14 | #include
15 | #include
16 | #include
17 | #include
18 |
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 |
26 |
27 | namespace frankx {
28 | using namespace movex;
29 | using Affine = affx::Affine;
30 |
31 | class Robot: public franka::Robot {
32 | // Modified DH-parameters: alpha, d, a
33 | const KinematicChain<7> kinematics = KinematicChain<7>(
34 | {{{0.0, 0.333, 0.0}, {-M_PI/2, 0.0, 0.0}, {M_PI/2, 0.316, 0.0}, {M_PI/2, 0.0, 0.0825}, {-M_PI/2, 0.384, -0.0825}, {M_PI/2, 0.0, 0.0}, {M_PI/2, 0.0, 0.088}}},
35 | Affine(0, 0, 0.107, M_PI/4, 0, M_PI)
36 | );
37 |
38 | public:
39 | //! The robot's hostname / IP address
40 | std::string fci_ip;
41 |
42 | // Cartesian constraints
43 | static constexpr double max_translation_velocity {1.7}; // [m/s]
44 | static constexpr double max_rotation_velocity {2.5}; // [rad/s]
45 | static constexpr double max_elbow_velocity {2.175}; // [rad/s]
46 | static constexpr double max_translation_acceleration {13.0}; // [m/s²]
47 | static constexpr double max_rotation_acceleration {25.0}; // [rad/s²]
48 | static constexpr double max_elbow_acceleration {10.0}; // [rad/s²]
49 | static constexpr double max_translation_jerk {6500.0}; // [m/s³]
50 | static constexpr double max_rotation_jerk {12500.0}; // [rad/s³]
51 | static constexpr double max_elbow_jerk {5000.0}; // [rad/s³]
52 |
53 | // Joint constraints
54 | static constexpr std::array max_joint_velocity {{2.175, 2.175, 2.175, 2.175, 2.610, 2.610, 2.610}}; // [rad/s]
55 | static constexpr std::array max_joint_acceleration {{15.0, 7.5, 10.0, 12.5, 15.0, 20.0, 20.0}}; // [rad/s²]
56 | static constexpr std::array max_joint_jerk {{7500.0, 3750.0, 5000.0, 6250.0, 7500.0, 10000.0, 10000.0}}; // [rad/s^3]
57 |
58 | static constexpr size_t degrees_of_freedoms {7};
59 | static constexpr double control_rate {0.001}; // [s]
60 |
61 | double velocity_rel {1.0}, acceleration_rel {1.0}, jerk_rel {1.0};
62 |
63 | franka::ControllerMode controller_mode {franka::ControllerMode::kJointImpedance}; // kCartesianImpedance wobbles -> setK?
64 |
65 | //! Whether the robots try to continue an interrupted motion due to a libfranka position/velocity/acceleration discontinuity with reduced dynamics.
66 | bool repeat_on_error {true};
67 |
68 | //! Whether the robot stops if a python error signal is detected.
69 | bool stop_at_python_signal {true};
70 |
71 | //! Connects to a robot at the given FCI IP address.
72 | explicit Robot(std::string fci_ip, double dynamic_rel = 1.0, bool repeat_on_error = true, bool stop_at_python_signal = true);
73 |
74 | void setDefaultBehavior();
75 | void setDynamicRel(double dynamic_rel);
76 |
77 | bool hasErrors();
78 | bool recoverFromErrors();
79 |
80 | Affine currentPose(const Affine& frame = Affine());
81 | std::array currentJointPositions();
82 | Affine forwardKinematics(const std::array& q);
83 | std::array inverseKinematics(const Affine& target, const std::array& q0);
84 |
85 | ::franka::RobotState get_state();
86 |
87 | bool move(ImpedanceMotion& motion);
88 | bool move(ImpedanceMotion& motion, MotionData& data);
89 | bool move(const Affine& frame, ImpedanceMotion& motion);
90 | bool move(const Affine& frame, ImpedanceMotion& motion, MotionData& data);
91 |
92 | bool move(JointMotion motion);
93 | bool move(JointMotion motion, MotionData& data);
94 |
95 | bool move(PathMotion motion);
96 | bool move(PathMotion motion, MotionData& data);
97 | bool move(const Affine& frame, PathMotion motion);
98 | bool move(const Affine& frame, PathMotion motion, MotionData& data);
99 |
100 | bool move(WaypointMotion& motion);
101 | bool move(WaypointMotion& motion, MotionData& data);
102 | bool move(const Affine& frame, WaypointMotion& motion);
103 | bool move(const Affine& frame, WaypointMotion& motion, MotionData& data);
104 | };
105 |
106 | } // namespace frankx
107 |
--------------------------------------------------------------------------------
/include/movex/motion/motion_impedance.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | #include
7 | #include
8 |
9 | #include
10 | #include
11 |
12 |
13 | namespace movex {
14 |
15 | struct ImpedanceMotion {
16 | using Affine = affx::Affine;
17 |
18 | enum class Axis { X, Y, Z };
19 |
20 | enum class Type {
21 | Cartesian,
22 | Joint
23 | };
24 |
25 | enum class TargetMotion {
26 | Exponential,
27 | Linear,
28 | Spiral,
29 | };
30 |
31 | struct LinearTargetMotion {
32 | Affine relative_target;
33 | double duration;
34 | bool initialized {false};
35 | bool finish_after {true};
36 | double finish_wait_factor {1.2};
37 | };
38 |
39 | struct SpiralTargetMotion {
40 | Affine center; // In XY-plane
41 | double revolutions_per_second;
42 | double radius_per_revolution;
43 | bool initialized {false};
44 | };
45 |
46 | Type type;
47 |
48 | TargetMotion target_motion {TargetMotion::Exponential};
49 | double exponential_decay {0.005};
50 | LinearTargetMotion linear_motion;
51 | SpiralTargetMotion spiral_motion;
52 |
53 | std::map force_constraints;
54 |
55 | public:
56 | const double translational_stiffness {2000.0}; // in [10, 3000] N/m
57 | const double rotational_stiffness {200.0}; // in [1, 300] Nm/rad
58 | const double joint_stiffness {200.0}; // ?
59 |
60 | Affine target;
61 | bool is_active {false};
62 | bool should_finish {false};
63 |
64 | explicit ImpedanceMotion() { }
65 | explicit ImpedanceMotion(double joint_stiffness): joint_stiffness(joint_stiffness), type(Type::Joint) { }
66 | explicit ImpedanceMotion(double translational_stiffness, double rotational_stiffness): translational_stiffness(translational_stiffness), rotational_stiffness(rotational_stiffness), type(Type::Cartesian) { }
67 |
68 |
69 | Affine getTarget() const {
70 | return target;
71 | }
72 |
73 | void setTarget(const Affine& new_target) {
74 | if (is_active) {
75 | target = new_target;
76 | }
77 | target_motion = ImpedanceMotion::TargetMotion::Exponential;
78 | }
79 |
80 | void setLinearRelativeTargetMotion(const Affine& relative_target, double duration) {
81 | linear_motion = {relative_target, duration};
82 | target_motion = ImpedanceMotion::TargetMotion::Linear;
83 | }
84 |
85 | void setSpiralTargetMotion(const Affine& center, double revolutions_per_second, double radius_per_revolution) {
86 | spiral_motion = {center, revolutions_per_second, radius_per_revolution};
87 | target_motion = ImpedanceMotion::TargetMotion::Spiral;
88 | }
89 |
90 | void addForceConstraint(Axis axis, double value) {
91 | if (is_active) {
92 | return;
93 | }
94 |
95 | force_constraints[axis] = value;
96 | }
97 |
98 | void addForceConstraint(std::optional x = std::nullopt, std::optional y = std::nullopt, std::optional z = std::nullopt) {
99 | if (is_active) {
100 | return;
101 | }
102 |
103 | if (x) {
104 | force_constraints[Axis::X] = x.value();
105 | }
106 | if (y) {
107 | force_constraints[Axis::Y] = y.value();
108 | }
109 | if (z) {
110 | force_constraints[Axis::Z] = z.value();
111 | }
112 | }
113 |
114 | bool isActive() const {
115 | return is_active;
116 | }
117 |
118 | void finish() {
119 | should_finish = true;
120 | }
121 | };
122 |
123 | } // namespace movex
124 |
--------------------------------------------------------------------------------
/include/movex/motion/motion_joint.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 |
6 | namespace movex {
7 |
8 | /**
9 | * A motion in the joint space
10 | */
11 | struct JointMotion {
12 | using Vector7d = Eigen::Matrix;
13 |
14 | const Vector7d target;
15 |
16 | explicit JointMotion(const std::array target): target(target.data()) { }
17 | };
18 |
19 | } // namespace movex
20 |
--------------------------------------------------------------------------------
/include/movex/motion/motion_path.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | #include
6 | #include
7 |
8 |
9 | namespace movex {
10 |
11 | /**
12 | * A motion following a pre-defined path.
13 | * Needs zero velocity and acceleration at start time.
14 | */
15 | struct PathMotion {
16 | using Affine = affx::Affine;
17 |
18 | std::vector waypoints;
19 |
20 | explicit PathMotion(const std::vector& waypoints): waypoints(waypoints) { }
21 | explicit PathMotion(const std::vector& waypoints, double blend_max_distance = 0.0) {
22 | this->waypoints.resize(waypoints.size());
23 | for (size_t i = 0; i < waypoints.size(); i += 1) {
24 | this->waypoints[i] = Waypoint(waypoints[i], std::nullopt, blend_max_distance);
25 | }
26 | }
27 | };
28 |
29 |
30 | struct LinearPathMotion: public PathMotion {
31 | explicit LinearPathMotion(const Affine& target): PathMotion({ Waypoint(target) }) { }
32 | explicit LinearPathMotion(const Affine& target, double elbow): PathMotion({ Waypoint(target, elbow) }) { }
33 | };
34 |
35 |
36 | struct LinearRelativePathMotion: public PathMotion {
37 | explicit LinearRelativePathMotion(const Affine& affine): PathMotion({ Waypoint(affine, Waypoint::ReferenceType::Relative) }) { }
38 | explicit LinearRelativePathMotion(const Affine& affine, double elbow): PathMotion({ Waypoint(affine, elbow, Waypoint::ReferenceType::Relative) }) { }
39 | };
40 |
41 | } // namespace movex
42 |
--------------------------------------------------------------------------------
/include/movex/motion/motion_waypoint.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | #include
7 | #include
8 |
9 |
10 | namespace movex {
11 |
12 | /**
13 | * A motion following multiple waypoints (with intermediate zero velocity) in a time-optimal way.
14 | * Works with aribtrary initial conditions.
15 | */
16 | struct WaypointMotion {
17 | using Affine = affx::Affine;
18 |
19 | bool reload {false};
20 | bool return_when_finished {true};
21 |
22 | std::vector waypoints;
23 |
24 | explicit WaypointMotion() {}
25 | explicit WaypointMotion(const std::vector& waypoints): waypoints(waypoints) {}
26 | explicit WaypointMotion(const std::vector& waypoints, bool return_when_finished): waypoints(waypoints), return_when_finished(return_when_finished) {}
27 |
28 | void setNextWaypoint(const Waypoint& waypoint) {
29 | waypoints = { waypoint };
30 | reload = true;
31 | }
32 |
33 | void setNextWaypoints(const std::vector& waypoints) {
34 | this->waypoints = waypoints;
35 | reload = true;
36 | }
37 |
38 | void finish() {
39 | return_when_finished = true;
40 | reload = true;
41 | }
42 | };
43 |
44 |
45 | struct LinearMotion: public WaypointMotion {
46 | explicit LinearMotion(const Affine& target): WaypointMotion({ Waypoint(target) }) { }
47 | explicit LinearMotion(const Affine& target, double elbow): WaypointMotion({ Waypoint(target, elbow) }) { }
48 | };
49 |
50 |
51 | struct LinearRelativeMotion: public WaypointMotion {
52 | explicit LinearRelativeMotion(const Affine& affine): WaypointMotion({ Waypoint(affine, Waypoint::ReferenceType::Relative) }) { }
53 | explicit LinearRelativeMotion(const Affine& affine, double elbow): WaypointMotion({ Waypoint(affine, elbow, Waypoint::ReferenceType::Relative) }) { }
54 | explicit LinearRelativeMotion(const Affine& affine, double elbow, double dynamic_rel): WaypointMotion({ Waypoint(affine, elbow, Waypoint::ReferenceType::Relative, dynamic_rel) }) { }
55 | };
56 |
57 |
58 | struct StopMotion: public WaypointMotion {
59 | explicit StopMotion(): WaypointMotion() {
60 | Waypoint stop_waypoint(Affine(), 0.0, Waypoint::ReferenceType::Relative);
61 | stop_waypoint.max_dynamics = true;
62 | waypoints = { stop_waypoint };
63 | }
64 |
65 | explicit StopMotion(const Affine& affine): WaypointMotion() {
66 | Waypoint stop_waypoint(affine, Waypoint::ReferenceType::Relative);
67 | stop_waypoint.max_dynamics = true;
68 | waypoints = { stop_waypoint };
69 | }
70 |
71 | explicit StopMotion(const Affine& affine, double elbow): WaypointMotion() {
72 | Waypoint stop_waypoint(affine, elbow, Waypoint::ReferenceType::Relative);
73 | stop_waypoint.max_dynamics = true;
74 | waypoints = { stop_waypoint };
75 | }
76 | };
77 |
78 |
79 | struct PositionHold: public WaypointMotion {
80 | explicit PositionHold(double duration): WaypointMotion({ Waypoint(duration) }) { }
81 | };
82 |
83 | } // namespace movex
84 |
--------------------------------------------------------------------------------
/include/movex/path/path.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 | #include
7 |
8 | #include
9 | #include
10 | #include
11 |
12 |
13 | namespace movex {
14 |
15 | class Path {
16 | using Affine = affx::Affine;
17 |
18 | std::vector cumulative_lengths;
19 |
20 | double length {0.0};
21 |
22 | void init_path_points(const std::vector& waypoints);
23 |
24 | public:
25 | constexpr static size_t degrees_of_freedom {7};
26 |
27 | std::vector> segments;
28 | size_t get_index(double s) const;
29 | std::tuple, double> get_local(double s) const;
30 |
31 | explicit Path() { }
32 | explicit Path(const std::vector& waypoints);
33 | explicit Path(const std::vector& waypoints, double blend_max_distance = 0.0);
34 |
35 | double get_length() const;
36 |
37 | Vector7d q(double s) const;
38 | Vector7d q(double s, const Affine& frame) const;
39 | Vector7d pdq(double s) const;
40 | Vector7d pddq(double s) const;
41 | Vector7d pdddq(double s) const;
42 |
43 | Vector7d dq(double s, double ds) const;
44 | Vector7d ddq(double s, double ds, double dds) const;
45 | Vector7d dddq(double s, double ds, double dds, double ddds) const;
46 |
47 | Vector7d max_pddq() const;
48 | Vector7d max_pdddq() const;
49 | };
50 |
51 | } // namespace movex
52 |
--------------------------------------------------------------------------------
/include/movex/path/segment.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | #include
6 |
7 |
8 | namespace movex {
9 |
10 | using Vector7d = Eigen::Matrix;
11 |
12 | struct Segment {
13 | double length;
14 |
15 | virtual double get_length() const = 0;
16 | virtual Vector7d q(double s) const = 0;
17 | virtual Vector7d pdq(double s) const = 0;
18 | virtual Vector7d pddq(double s) const = 0;
19 | virtual Vector7d pdddq(double s) const = 0;
20 |
21 | Vector7d dq(double s, double ds) const {
22 | return pdq(s) * ds;
23 | }
24 |
25 | Vector7d ddq(double s, double ds, double dds) const {
26 | return pddq(s) * std::pow(ds, 2) + pdq(s) * dds;
27 | }
28 |
29 | Vector7d dddq(double s, double ds, double dds, double ddds) const {
30 | return 3 * ds * pddq(s) * dds + std::pow(ds, 3) * pdddq(s) + pdq(s) * ddds;
31 | }
32 |
33 | virtual Vector7d max_pddq() const = 0;
34 | virtual Vector7d max_pdddq() const = 0;
35 | };
36 |
37 |
38 | class LineSegment: public Segment {
39 | public:
40 | Vector7d start, end;
41 |
42 | explicit LineSegment(const Vector7d& start, const Vector7d&end): start(start), end(end) {
43 | Vector7d diff = end - start;
44 |
45 | length = diff.norm();
46 | }
47 |
48 | double get_length() const {
49 | return length;
50 | }
51 |
52 | Vector7d q(double s) const {
53 | return start + s / length * (end - start);
54 | }
55 |
56 | Vector7d pdq(double s) const {
57 | return (end - start) / length;
58 | }
59 |
60 | Vector7d pddq(double s) const {
61 | return Vector7d::Zero();
62 | }
63 |
64 | Vector7d pdddq(double s) const {
65 | return Vector7d::Zero();
66 | }
67 |
68 | Vector7d max_pddq() const {
69 | return Vector7d::Zero();
70 | }
71 |
72 | Vector7d max_pdddq() const {
73 | return Vector7d::Zero();
74 | }
75 | };
76 |
77 |
78 | class CircleSegment: public Segment {
79 |
80 | };
81 |
82 |
83 | class QuinticSegment: public Segment {
84 |
85 | };
86 |
87 |
88 | class QuarticBlendSegment: public Segment {
89 | void integrate_path_length() {
90 | length = 0.0;
91 |
92 | Vector7d f_s = q(0.0), f_s_new;
93 | const size_t steps {5000};
94 | const double step_length = s_length / steps;
95 | const double step_length_squared = std::pow(step_length, 2);
96 | for (int i = 1; i < steps; i += 1) {
97 | f_s_new = q(i * step_length);
98 | length += std::sqrt((f_s_new - f_s).squaredNorm() + step_length_squared);
99 | std::swap(f_s_new, f_s);
100 | }
101 | }
102 |
103 | public:
104 | double s_length;
105 | Vector7d b, c, e, f;
106 | Vector7d lb, lm, rb, rm;
107 |
108 | explicit QuarticBlendSegment(const Vector7d& lb, const Vector7d& lm, const Vector7d& rb, const Vector7d& rm, double s_mid, double max_diff, double s_abs_max): lb(lb), lm(lm), rb(rb), rm(rm) {
109 | Vector7d sAbs_ = ((-16*max_diff)/(3.*(lm - rm).array())).abs();
110 | double s_abs_min = std::min({sAbs_.minCoeff(), s_abs_max});
111 | s_length = 2 * s_abs_min;
112 |
113 | b = (lm - rm).array() / (16.*std::pow(s_abs_min, 3));
114 | c = (-lm + rm).array() / (4.*std::pow(s_abs_min, 2));
115 | e = lm;
116 | f = lb.array() + lm.array()*(-s_abs_min + s_mid);
117 | }
118 |
119 | double get_length() const {
120 | return s_length;
121 | }
122 |
123 | Vector7d q(double s) const {
124 | return f + s * (e + s * (s * (c + s * b)));
125 | }
126 |
127 | Vector7d pdq(double s) const {
128 | return e + s * (s * (3 * c + s * 4 * b));
129 | }
130 |
131 | Vector7d pddq(double s) const {
132 | return s * (6 * c + s * 12 * b);
133 | }
134 |
135 | Vector7d pdddq(double s) const {
136 | return 6 * c + s * 24 * b;
137 | }
138 |
139 | Vector7d max_pddq() const {
140 | double s_abs = s_length / 2;
141 | return (-3*(lm - rm))/(4.*s_abs);
142 | }
143 |
144 | Vector7d max_pdddq() const {
145 | double s_abs = s_length / 2;
146 | return (3*(lm - rm))/(2.*std::pow(s_abs,2));
147 | }
148 | };
149 |
150 | } // namespace movex
151 |
--------------------------------------------------------------------------------
/include/movex/path/time_parametrization.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 |
6 |
7 | namespace movex {
8 |
9 | class TimeParametrization {
10 | std::unique_ptr> otg;
11 |
12 | //! Time step between updates (cycle time) in [s]
13 | const double delta_time;
14 |
15 | double intersection(double s1, double ds1, double s2, double ds2, double max_ds, double max_dds, double max_ddds) {
16 |
17 | }
18 |
19 | std::tuple max_dynamics(double s_start, double ds_start, double max_ds, double max_dds, double max_ddds) {
20 |
21 | }
22 |
23 | public:
24 | TimeParametrization(double delta_time): delta_time(delta_time) {
25 | otg = std::make_unique>(delta_time);
26 | }
27 |
28 | //! Returns list of path positions s at delta time
29 | Trajectory parametrize(const Path& path, const std::array& max_velocity, const std::array& max_acceleration, const std::array& max_jerk) {
30 | Trajectory trajectory {path};
31 |
32 | // For linear segments: accelerate as fast as possible
33 | // For blend segments: Constant path velocity ds
34 | // Check continuous, and go back to zero velocity otherwise
35 |
36 | Vector7d max_velocity_v = Eigen::Map(max_velocity.data(), max_velocity.size());
37 | Vector7d max_accleration_v = Eigen::Map(max_acceleration.data(), max_acceleration.size());
38 | Vector7d max_jerk_v = Eigen::Map(max_jerk.data(), max_jerk.size());
39 |
40 | std::vector> max_path_dynamics;
41 | for (auto segment: path.segments) {
42 | auto max_pddq = segment->max_pddq();
43 | auto max_pdddq = segment->max_pdddq();
44 |
45 | double max_ds, max_dds, max_ddds;
46 |
47 | // Linear segments
48 | if ((max_pddq.array().abs() < 1e-16).any() && (max_pdddq.array().abs() < 1e-16).any()) {
49 | auto constant_pdq = segment->pdq(0.0);
50 |
51 | max_ds = (max_velocity_v.array() / constant_pdq.array().abs()).minCoeff();
52 | max_dds = (max_accleration_v.array() / constant_pdq.array().abs()).minCoeff();
53 | max_ddds = (max_jerk_v.array() / constant_pdq.array().abs()).minCoeff();
54 |
55 | // Other segments
56 | } else {
57 | // ds = max_velocity_v.array() / pdq(s) // pdq will always be between two linear segments...
58 | double ds_acc = (max_accleration_v.array() / max_pddq.array().abs()).sqrt().minCoeff();
59 | double ds_jerk = (max_jerk_v.array() / max_pdddq.array().abs()).pow(1./3).minCoeff();
60 | max_ds = std::min(ds_acc, ds_jerk);
61 | max_dds = 0.0;
62 | max_ddds = 0.0;
63 | }
64 |
65 | max_path_dynamics.push_back({max_ds, max_dds, max_ddds});
66 | }
67 |
68 | double current_s, current_ds, current_dds;
69 | // Integrate forward and (if necessary) backward
70 | // Get maximal possible velocity, acceleration
71 |
72 | // Calculate path at time steps
73 | ruckig::InputParameter<1> input;
74 | ruckig::OutputParameter<1> output;
75 | ruckig::Result otg_result {ruckig::Result::Working};
76 |
77 | double time {0.0};
78 | double s_new {0.0}, ds_new {0.0}, dds_new {0.0};
79 | size_t index_current = path.get_index(s_new);
80 | // auto [segment_new, s_local_new] = path.get_local(s_new);
81 |
82 | Trajectory::State current_state {time, s_new, ds_new, dds_new, 0.0};
83 | trajectory.states.push_back(current_state);
84 |
85 | input.current_position[0] = s_new;
86 | input.current_velocity[0] = ds_new;
87 | input.current_acceleration[0] = dds_new;
88 | input.target_position[0] = path.get_length();
89 | input.target_velocity[0] = 0.0;
90 | std::tie(input.max_velocity[0], input.max_acceleration[0], input.max_jerk[0]) = max_path_dynamics[index_current];
91 |
92 | while (otg_result == ruckig::Result::Working) {
93 | time += delta_time;
94 | otg_result = otg->update(input, output);
95 |
96 | s_new = output.new_position[0];
97 | ds_new = output.new_velocity[0];
98 | dds_new = output.new_acceleration[0];
99 |
100 | size_t index_new = path.get_index(s_new);
101 |
102 | // New segment
103 | if (index_new > index_current) {
104 | index_current = index_new;
105 |
106 | // std::tie(segment_new, s_local_new) = path.get_local(s_new);
107 | // std::tie(input.max_velocity(0), input.max_acceleration(0), input.max_jerk(0)) = max_path_dynamics[index_current];
108 | }
109 |
110 | current_state = {time, s_new, ds_new, dds_new, 0.0};
111 | trajectory.states.push_back(current_state);
112 |
113 | input.current_position = output.new_position;
114 | input.current_velocity = output.new_velocity;
115 | input.current_acceleration = output.new_acceleration;
116 | }
117 |
118 | return trajectory;
119 | }
120 | };
121 |
122 | } // namespace movex
123 |
--------------------------------------------------------------------------------
/include/movex/path/trajectory.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 |
6 | namespace movex {
7 |
8 | struct Trajectory {
9 | struct State {
10 | //! The time i n[s]
11 | double t;
12 |
13 | //! The path position (between 0 and the path length) and its derivatives
14 | double s, ds, dds, ddds;
15 | };
16 |
17 | Path path;
18 |
19 | //! The trajectory state for each consecutive time step (with delta_time difference of the time parametrization)
20 | std::vector states;
21 |
22 | explicit Trajectory() { }
23 | explicit Trajectory(const Path& path): path(path) { }
24 | };
25 |
26 | } // namespace movex
27 |
--------------------------------------------------------------------------------
/include/movex/robot/kinematics.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 |
6 | namespace movex {
7 |
8 | template
9 | class KinematicChain {
10 | using Affine = affx::Affine;
11 |
12 | struct DenavitHartenbergParameter {
13 | double alpha; // [rad]
14 | double d, a; // [m]
15 |
16 | Affine get_transformation(double theta) const {
17 | Affine rot1 {0, 0, 0, theta, 0, 0};
18 | Affine trans1 {0, 0, d, 0, 0, 0};
19 | Affine trans2 {a, 0, 0, 0, 0, 0};
20 | Affine rot2 {0, 0, 0, 0, 0, alpha};
21 | return rot2 * trans2 * rot1 * trans1;
22 | }
23 | };
24 |
25 | std::array parameters;
26 | Affine base;
27 |
28 | public:
29 | explicit KinematicChain(const std::array& parameters, const Affine& base): parameters(parameters), base(base) { }
30 |
31 | Affine forward_chain(const std::array& q) const {
32 | Affine result;
33 | for (size_t i = 0; i < DoFs; ++i) {
34 | result = result * parameters[i].get_transformation(q[i]);
35 | }
36 | return result * base;
37 | }
38 | };
39 |
40 | } // namespace movex
41 |
--------------------------------------------------------------------------------
/include/movex/robot/measure.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | #include
6 |
7 |
8 | namespace movex {
9 |
10 | class Condition {
11 | using RobotState_ = RobotState<7>;
12 | public:
13 | using CallbackType = std::function;
14 |
15 | explicit Condition(CallbackType callback): callback(callback) { }
16 |
17 | Condition& operator&&(const Condition& rhs) {
18 | callback = [this, rhs](const RobotState_& robot_state, double time) {
19 | return callback(robot_state, time) && rhs.callback(robot_state, time);
20 | };
21 | return *this;
22 | }
23 |
24 | Condition& operator||(const Condition& rhs) {
25 | callback = [this, rhs](const RobotState_& robot_state, double time) {
26 | return callback(robot_state, time) || rhs.callback(robot_state, time);
27 | };
28 | return *this;
29 | }
30 |
31 | //! Check if the condition is fulfilled
32 | bool operator()(const RobotState_& robot_state, double time) {
33 | return callback(robot_state, time);
34 | }
35 |
36 | private:
37 | CallbackType callback;
38 | };
39 |
40 |
41 | class Measure {
42 | using RobotState_ = RobotState<7>;
43 | using MeasureCallback = std::function;
44 |
45 | MeasureCallback callback;
46 | explicit Measure(MeasureCallback callback): callback(callback) { }
47 |
48 | public:
49 | static Measure ForceX() {
50 | return Measure([](const RobotState_& robot_state, double time) {
51 | return robot_state.O_F_ext_hat_K[0];
52 | });
53 | }
54 |
55 | static Measure ForceY() {
56 | return Measure([](const RobotState_& robot_state, double time) {
57 | return robot_state.O_F_ext_hat_K[1];
58 | });
59 | }
60 |
61 | static Measure ForceZ() {
62 | return Measure([](const RobotState_& robot_state, double time) {
63 | return robot_state.O_F_ext_hat_K[2];
64 | });
65 | }
66 |
67 | static Measure ForceXYNorm() {
68 | return Measure([](const RobotState_& robot_state, double time) {
69 | return std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2);
70 | });
71 | }
72 |
73 | static Measure ForceXYZNorm() {
74 | return Measure([](const RobotState_& robot_state, double time) {
75 | return std::pow(robot_state.O_F_ext_hat_K[0], 2) + std::pow(robot_state.O_F_ext_hat_K[1], 2) + std::pow(robot_state.O_F_ext_hat_K[2], 2);
76 | });
77 | }
78 |
79 | static Measure Time() {
80 | return Measure([](const RobotState_& robot_state, double time) {
81 | return time;
82 | });
83 | }
84 |
85 | Condition operator==(double threshold) {
86 | return Condition([*this, threshold](const RobotState_& robot_state, double time) {
87 | return callback(robot_state, time) == threshold;
88 | });
89 | }
90 |
91 | Condition operator!=(double threshold) {
92 | return Condition([*this, threshold](const RobotState_& robot_state, double time) {
93 | return callback(robot_state, time) != threshold;
94 | });
95 | }
96 |
97 | Condition operator<(double threshold) {
98 | return Condition([*this, threshold](const RobotState_& robot_state, double time) {
99 | return callback(robot_state, time) < threshold;
100 | });
101 | }
102 |
103 | Condition operator<=(double threshold) {
104 | return Condition([*this, threshold](const RobotState_& robot_state, double time) {
105 | return callback(robot_state, time) <= threshold;
106 | });
107 | }
108 |
109 | Condition operator>(double threshold) {
110 | return Condition([*this, threshold](const RobotState_& robot_state, double time) {
111 | return callback(robot_state, time) > threshold;
112 | });
113 | }
114 |
115 | Condition operator>=(double threshold) {
116 | return Condition([*this, threshold](const RobotState_& robot_state, double time) {
117 | return callback(robot_state, time) >= threshold;
118 | });
119 | }
120 | };
121 |
122 | } // namespace movex
123 |
--------------------------------------------------------------------------------
/include/movex/robot/motion_data.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 |
6 | namespace movex {
7 |
8 | struct MotionData {
9 | double velocity_rel {1.0}, acceleration_rel {1.0}, jerk_rel {1.0};
10 | bool max_dynamics {false};
11 |
12 | std::vector reactions {};
13 |
14 | explicit MotionData(double dynamic_rel = 1.0): velocity_rel(dynamic_rel), acceleration_rel(dynamic_rel), jerk_rel(dynamic_rel) { }
15 |
16 | MotionData& withDynamicRel(double dynamic_rel) {
17 | velocity_rel = dynamic_rel;
18 | acceleration_rel = dynamic_rel;
19 | jerk_rel = dynamic_rel;
20 | return *this;
21 | }
22 |
23 | //! Use maximal possible dynamic of the robot
24 | MotionData& withMaxDynamics() {
25 | max_dynamics = true;
26 | return *this;
27 | }
28 |
29 | //! Add a reaction to the motion
30 | MotionData& withReaction(const Reaction& reaction) {
31 | reactions.push_back(reaction);
32 | return *this;
33 | }
34 |
35 | //! Whether any of the reactions did occur (fire) during the motion
36 | bool didBreak() {
37 | return std::any_of(reactions.begin(), reactions.end(), [](auto r) { return r.has_fired; });
38 | }
39 | };
40 |
41 | } // namespace movex
42 |
--------------------------------------------------------------------------------
/include/movex/robot/reaction.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 |
9 | #include
10 |
11 |
12 | namespace movex {
13 |
14 | class WaypointMotion;
15 |
16 | struct Reaction {
17 | using WaypointAction = std::function&, double)>;
18 | std::optional waypoint_action;
19 | std::optional> waypoint_motion;
20 |
21 | Condition condition;
22 | bool has_fired {false};
23 |
24 | explicit Reaction(Condition::CallbackType callback): condition(callback) { }
25 | explicit Reaction(Condition::CallbackType callback, std::optional> waypoint_motion): condition(callback), waypoint_motion(waypoint_motion) { }
26 | explicit Reaction(Condition::CallbackType callback, std::optional waypoint_action): condition(callback), waypoint_action(waypoint_action) { }
27 |
28 | explicit Reaction(Condition condition): condition(condition) { }
29 | explicit Reaction(Condition condition, std::optional> waypoint_motion): condition(condition), waypoint_motion(waypoint_motion) { }
30 | explicit Reaction(Condition condition, std::optional waypoint_action): condition(condition), waypoint_action(waypoint_action) { }
31 | };
32 |
33 | } // namespace movex
34 |
--------------------------------------------------------------------------------
/include/movex/robot/robot_state.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 |
4 | namespace movex {
5 |
6 | //! The overall state of the robot.
7 | template
8 | struct RobotState {
9 | std::array q;
10 | std::array q_d;
11 | std::array dq;
12 |
13 | std::array O_T_EE;
14 | std::array O_T_EE_c;
15 | std::array O_dP_EE_c;
16 |
17 | std::array elbow;
18 | std::array elbow_c;
19 | std::array elbow_d;
20 |
21 | std::array O_F_ext_hat_K;
22 | };
23 |
24 | } // namespace movex
25 |
--------------------------------------------------------------------------------
/include/movex/waypoint.hpp:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | #include
6 |
7 | #include
8 |
9 |
10 | namespace movex {
11 |
12 | struct Waypoint {
13 | using Affine = affx::Affine;
14 | using Vector6d = Eigen::Matrix;
15 | using Vector7d = Eigen::Matrix;
16 |
17 | enum class ReferenceType {
18 | Absolute,
19 | Relative
20 | };
21 |
22 | Affine affine;
23 | std::optional elbow;
24 | ReferenceType reference_type;
25 |
26 |
27 | //! Dynamic Waypoint: Relative velocity factor
28 | double velocity_rel {1.0};
29 |
30 | //! Dynamic Waypoint: Use maximal dynamics of the robot independent on other parameters
31 | bool max_dynamics {false};
32 |
33 | //! Zero velocity Waypoint: Stop to zero velocity
34 | bool zero_velocity {false};
35 |
36 | //! Dynamic Waypoint: Minimum time to get to next waypoint
37 | std::optional minimum_time;
38 |
39 | //! Path Waypoint: Maximum distance for blending.
40 | double blend_max_distance {0.0};
41 |
42 |
43 | explicit Waypoint(): affine(Affine()), reference_type(ReferenceType::Absolute) {}
44 | explicit Waypoint(const Affine& affine, ReferenceType reference_type = ReferenceType::Absolute): affine(affine), reference_type(reference_type) {}
45 | explicit Waypoint(const Affine& affine, double elbow, ReferenceType reference_type = ReferenceType::Absolute): affine(affine), reference_type(reference_type), elbow(elbow) {}
46 |
47 | explicit Waypoint(bool zero_velocity): affine(Affine()), reference_type(ReferenceType::Relative), zero_velocity(zero_velocity) {}
48 | explicit Waypoint(double minimum_time): affine(Affine()), reference_type(ReferenceType::Relative), minimum_time(minimum_time) {}
49 | explicit Waypoint(const Affine& affine, ReferenceType reference_type, double velocity_rel): affine(affine), reference_type(reference_type), velocity_rel(velocity_rel) {}
50 | explicit Waypoint(const Affine& affine, double elbow, ReferenceType reference_type, double velocity_rel): affine(affine), elbow(elbow), reference_type(reference_type), velocity_rel(velocity_rel) {}
51 |
52 | // explicit Waypoint(const Affine& affine, double blend_max_distance): affine(affine), blend_max_distance(blend_max_distance) {}
53 | explicit Waypoint(const Affine& affine, std::optional elbow, double blend_max_distance): affine(affine), elbow(elbow), blend_max_distance(blend_max_distance), reference_type(ReferenceType::Absolute) {}
54 |
55 |
56 | Affine getTargetAffine(const Affine& frame, const Affine& old_affine) const {
57 | switch (reference_type) {
58 | case ReferenceType::Absolute:
59 | return affine * frame.inverse();
60 | case ReferenceType::Relative:
61 | return old_affine * affine * frame.inverse();
62 | }
63 | }
64 |
65 | Vector7d getTargetVector(const Affine& old_affine, double old_elbow) const {
66 | return getTargetVector(Affine(), old_affine, old_elbow);
67 | }
68 |
69 | Vector7d getTargetVector(const Affine& frame, const Affine& old_affine, double old_elbow) const {
70 | double new_elbow;
71 | if (reference_type == ReferenceType::Relative) {
72 | new_elbow = elbow.value_or(0.0) + old_elbow;
73 | } else {
74 | new_elbow = elbow.value_or(old_elbow);
75 | }
76 | return getTargetAffine(frame, old_affine).vector_with_elbow(new_elbow);
77 | }
78 | };
79 |
80 | } // namespace movex
81 |
--------------------------------------------------------------------------------
/notebooks/kinematics.nb:
--------------------------------------------------------------------------------
1 | (* Content-type: application/vnd.wolfram.mathematica *)
2 |
3 | (*** Wolfram Notebook File ***)
4 | (* http://www.wolfram.com/nb *)
5 |
6 | (* CreatedBy='Mathematica 12.0' *)
7 |
8 | (*CacheID: 234*)
9 | (* Internal cache information:
10 | NotebookFileLineBreakTest
11 | NotebookFileLineBreakTest
12 | NotebookDataPosition[ 158, 7]
13 | NotebookDataLength[ 21966, 590]
14 | NotebookOptionsPosition[ 20530, 560]
15 | NotebookOutlinePosition[ 20867, 575]
16 | CellTagsIndexPosition[ 20824, 572]
17 | WindowFrame->Normal*)
18 |
19 | (* Beginning of Notebook Content *)
20 | Notebook[{
21 |
22 | Cell[CellGroupData[{
23 | Cell["Kinematics", "Subsection",
24 | CellChangeTimes->{{3.823582071786221*^9,
25 | 3.823582076986455*^9}},ExpressionUUID->"2638955b-d1e8-4e28-ba05-\
26 | db1765a28fd7"],
27 |
28 | Cell[BoxData[
29 | RowBox[{
30 | RowBox[{"T", "[",
31 | RowBox[{"\[Alpha]_", ",", "a_", ",", "\[Theta]_", ",", "d_"}], "]"}], ":=",
32 | RowBox[{"{", "\[IndentingNewLine]",
33 | RowBox[{
34 | RowBox[{"{",
35 | RowBox[{
36 | RowBox[{"Cos", "[", "\[Theta]", "]"}], ",",
37 | RowBox[{"-",
38 | RowBox[{"Sin", "[", "\[Theta]", "]"}]}], ",", "0", ",", "a"}], "}"}],
39 | ",", "\[IndentingNewLine]",
40 | RowBox[{"{",
41 | RowBox[{
42 | RowBox[{
43 | RowBox[{"Sin", "[", "\[Theta]", "]"}],
44 | RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ",",
45 | RowBox[{
46 | RowBox[{"Cos", "[", "\[Theta]", "]"}],
47 | RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ",",
48 | RowBox[{"-",
49 | RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], ",",
50 | RowBox[{
51 | RowBox[{"-", "d"}], " ",
52 | RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], "}"}], ",",
53 | "\[IndentingNewLine]",
54 | RowBox[{"{",
55 | RowBox[{
56 | RowBox[{
57 | RowBox[{"Sin", "[", "\[Theta]", "]"}], " ",
58 | RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], ",",
59 | RowBox[{
60 | RowBox[{"Cos", "[", "\[Theta]", "]"}],
61 | RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], ",",
62 | RowBox[{"Cos", "[", "\[Alpha]", "]"}], ",",
63 | RowBox[{"d", " ",
64 | RowBox[{"Cos", "[", "\[Alpha]", "]"}]}]}], "}"}], ",",
65 | "\[IndentingNewLine]",
66 | RowBox[{"{",
67 | RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}],
68 | "\[IndentingNewLine]", "}"}]}]], "Input",
69 | CellChangeTimes->{{3.823582093563463*^9, 3.823582186507723*^9}},
70 | CellLabel->"In[1]:=",ExpressionUUID->"1293ae6c-31e5-4370-8907-caad95c252d0"],
71 |
72 | Cell[BoxData[
73 | RowBox[{
74 | RowBox[{"p", "[",
75 | RowBox[{
76 | "\[Theta]1_", ",", "\[Theta]2_", ",", "\[Theta]3_", ",", "\[Theta]4_", ",",
77 | "\[Theta]5_", ",", "\[Theta]6_", ",", "\[Theta]7_"}], "]"}], ":=",
78 | RowBox[{
79 | RowBox[{"T", "[",
80 | RowBox[{"0", ",", "0", ",", "\[Theta]1", ",", "0.333"}], "]"}], ".",
81 | RowBox[{"T", "[",
82 | RowBox[{
83 | RowBox[{"-",
84 | FractionBox["\[Pi]", "2"]}], ",", "0", ",", "\[Theta]2", ",", "0"}],
85 | "]"}], ".",
86 | RowBox[{"T", "[",
87 | RowBox[{
88 | FractionBox["\[Pi]", "2"], ",", "0", ",", "\[Theta]3", ",", "0.316"}],
89 | "]"}], ".",
90 | RowBox[{"T", "[",
91 | RowBox[{
92 | FractionBox["\[Pi]", "2"], ",", "0.0825", ",", "\[Theta]4", ",", "0"}],
93 | "]"}], ".",
94 | RowBox[{"T", "[",
95 | RowBox[{
96 | RowBox[{"-",
97 | FractionBox["\[Pi]", "2"]}], ",",
98 | RowBox[{"-", "0.0825"}], ",", "\[Theta]5", ",", "0.384"}], "]"}], ".",
99 | RowBox[{"T", "[",
100 | RowBox[{
101 | FractionBox["\[Pi]", "2"], ",", "0", ",", "\[Theta]6", ",", "0"}], "]"}],
102 | ".",
103 | RowBox[{"T", "[",
104 | RowBox[{
105 | FractionBox["\[Pi]", "2"], ",", "0.088", ",", "\[Theta]7", ",", "0"}],
106 | "]"}], ".",
107 | RowBox[{"T", "[",
108 | RowBox[{"\[Pi]", ",", "0", ",",
109 | RowBox[{"-",
110 | FractionBox["\[Pi]", "4"]}], ",",
111 | RowBox[{"-", "0.107"}]}], "]"}]}]}]], "Input",
112 | CellChangeTimes->{{3.823582198644116*^9, 3.823582337516674*^9}, {
113 | 3.823582386285802*^9, 3.823582419461158*^9}, {3.823582453054434*^9,
114 | 3.82358249544624*^9}},
115 | CellLabel->"In[2]:=",ExpressionUUID->"d8b24c1a-4cdd-43ed-9ee4-98459c265c06"],
116 |
117 | Cell[BoxData[
118 | RowBox[{
119 | RowBox[{"pM", "=",
120 | RowBox[{"Simplify", "[",
121 | RowBox[{
122 | RowBox[{"p", "[",
123 | RowBox[{
124 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4", ",",
125 | "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "/.",
126 | RowBox[{"{",
127 | RowBox[{
128 | RowBox[{"0.", "\[Rule]", "0"}], ",",
129 | RowBox[{"1.", "\[Rule]", "1"}]}], "}"}]}], "]"}]}], ";"}]], "Input",
130 | CellChangeTimes->{{3.823582623495618*^9, 3.823582655774441*^9}, {
131 | 3.823582693975222*^9, 3.823582697862906*^9}},
132 | CellLabel->"In[3]:=",ExpressionUUID->"5eced034-3e4c-4a28-956f-542ef9fac943"],
133 |
134 | Cell[BoxData[
135 | RowBox[{
136 | RowBox[{"euler", "[",
137 | RowBox[{
138 | "\[Theta]1_", ",", "\[Theta]2_", ",", "\[Theta]3_", ",", "\[Theta]4_", ",",
139 | "\[Theta]5_", ",", "\[Theta]6_", ",", "\[Theta]7_"}], "]"}], ":=",
140 | RowBox[{"{", "\[IndentingNewLine]",
141 | RowBox[{
142 | RowBox[{
143 | RowBox[{"p", "[",
144 | RowBox[{
145 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4", ",",
146 | "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
147 | RowBox[{"[",
148 | RowBox[{"1", ",", "4"}], "]"}], "]"}], ",", "\[IndentingNewLine]",
149 | RowBox[{
150 | RowBox[{"p", "[",
151 | RowBox[{
152 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4", ",",
153 | "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
154 | RowBox[{"[",
155 | RowBox[{"2", ",", "4"}], "]"}], "]"}], ",", "\[IndentingNewLine]",
156 | RowBox[{
157 | RowBox[{"p", "[",
158 | RowBox[{
159 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4", ",",
160 | "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
161 | RowBox[{"[",
162 | RowBox[{"3", ",", "4"}], "]"}], "]"}], ",", "\[IndentingNewLine]",
163 | RowBox[{"ArcTan", "[",
164 | RowBox[{
165 | RowBox[{
166 | RowBox[{"p", "[",
167 | RowBox[{
168 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4",
169 | ",", "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
170 | RowBox[{"[",
171 | RowBox[{"2", ",", "1"}], "]"}], "]"}], "/",
172 | RowBox[{
173 | RowBox[{"p", "[",
174 | RowBox[{
175 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4",
176 | ",", "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
177 | RowBox[{"[",
178 | RowBox[{"1", ",", "1"}], "]"}], "]"}]}], "]"}], ",",
179 | "\[IndentingNewLine]",
180 | RowBox[{"-",
181 | RowBox[{"ArcSin", "[",
182 | RowBox[{
183 | RowBox[{"p", "[",
184 | RowBox[{
185 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4",
186 | ",", "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
187 | RowBox[{"[",
188 | RowBox[{"3", ",", "1"}], "]"}], "]"}], "]"}]}], ",",
189 | "\[IndentingNewLine]",
190 | RowBox[{"ArcTan", "[",
191 | RowBox[{
192 | RowBox[{
193 | RowBox[{"p", "[",
194 | RowBox[{
195 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4",
196 | ",", "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
197 | RowBox[{"[",
198 | RowBox[{"3", ",", "2"}], "]"}], "]"}], "/",
199 | RowBox[{
200 | RowBox[{"p", "[",
201 | RowBox[{
202 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4",
203 | ",", "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "[",
204 | RowBox[{"[",
205 | RowBox[{"3", ",", "3"}], "]"}], "]"}]}], "]"}]}],
206 | "\[IndentingNewLine]", "}"}]}]], "Input",
207 | CellChangeTimes->{{3.823594209828718*^9, 3.823594305244712*^9}},
208 | CellLabel->"In[4]:=",ExpressionUUID->"25bca131-2606-4f40-956f-9b4b520b9c99"],
209 |
210 | Cell[BoxData[
211 | RowBox[{
212 | RowBox[{"eM", "=",
213 | RowBox[{"Simplify", "[",
214 | RowBox[{
215 | RowBox[{"euler", "[",
216 | RowBox[{
217 | "\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3", ",", "\[Theta]4", ",",
218 | "\[Theta]5", ",", "\[Theta]6", ",", "\[Theta]7"}], "]"}], "/.",
219 | RowBox[{"{",
220 | RowBox[{
221 | RowBox[{"0.", "\[Rule]", "0"}], ",",
222 | RowBox[{"1.", "\[Rule]", "1"}]}], "}"}]}], "]"}]}], ";"}]], "Input",
223 | CellChangeTimes->{{3.823594332044726*^9, 3.823594334756621*^9}},
224 | CellLabel->"In[6]:=",ExpressionUUID->"31153068-cde7-4919-97b6-1af27bb17782"],
225 |
226 | Cell[CellGroupData[{
227 |
228 | Cell[BoxData[
229 | RowBox[{
230 | RowBox[{"{",
231 | RowBox[{
232 | RowBox[{"D", "[",
233 | RowBox[{"eM", ",", "\[Theta]1"}], "]"}], ",",
234 | RowBox[{"D", "[",
235 | RowBox[{"eM", ",", "\[Theta]2"}], "]"}], ",",
236 | RowBox[{"D", "[",
237 | RowBox[{"eM", ",", "\[Theta]3"}], "]"}], ",",
238 | RowBox[{"D", "[",
239 | RowBox[{"eM", ",", "\[Theta]4"}], "]"}], ",",
240 | RowBox[{"D", "[",
241 | RowBox[{"eM", ",", "\[Theta]5"}], "]"}], ",",
242 | RowBox[{"D", "[",
243 | RowBox[{"eM", ",", "\[Theta]6"}], "]"}], ",",
244 | RowBox[{"D", "[",
245 | RowBox[{"eM", ",", "\[Theta]7"}], "]"}]}], "}"}], ";"}]], "Input",
246 | CellChangeTimes->{{3.8235956031590548`*^9, 3.8235957116785507`*^9}, {
247 | 3.823602846632757*^9, 3.8236028864799213`*^9},
248 | 3.823751768880197*^9},ExpressionUUID->"9db25a01-77cb-4265-b9c7-\
249 | eac93a89fd5b"],
250 |
251 | Cell[BoxData[
252 | InterpretationBox[
253 | TagBox[
254 | FrameBox[GridBox[{
255 | {
256 | ItemBox[
257 | TagBox[
258 | RowBox[{"{",
259 | TemplateBox[{"1"},
260 | "OutputSizeLimit`Skeleton"], "}"}],
261 | Short[#, 5]& ],
262 | BaseStyle->{Deployed -> False},
263 | StripOnInput->False]},
264 | {GridBox[{
265 | {
266 | PaneBox[
267 | TagBox[
268 | TooltipBox[
269 | StyleBox[
270 | StyleBox[
271 | DynamicBox[ToBoxes[
272 | FEPrivate`FrontEndResource[
273 | "FEStrings", "sizeBriefExplanation"], StandardForm],
274 | ImageSizeCache->{61., {2., 8.}}],
275 | StripOnInput->False,
276 | DynamicUpdating->True,
277 | LineSpacing->{1, 2},
278 | LineIndent->0,
279 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLText",
280 | StripOnInput->False],
281 | StyleBox[
282 | DynamicBox[
283 | ToBoxes[
284 | FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"],
285 | StandardForm]], DynamicUpdating -> True, LineIndent -> 0,
286 | LinebreakAdjustments -> {1., 100, 0, 0, 0},
287 | LineSpacing -> {1, 2}, StripOnInput -> False]],
288 | Annotation[#,
289 | Style[
290 | Dynamic[
291 | FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"]],
292 | DynamicUpdating -> True, LineIndent -> 0,
293 | LinebreakAdjustments -> {1., 100, 0, 0, 0},
294 | LineSpacing -> {1, 2}], "Tooltip"]& ],
295 | Alignment->Center,
296 | BaselinePosition->Baseline,
297 | ImageSize->{Automatic, {25, Full}}],
298 | ButtonBox[
299 | PaneSelectorBox[{False->
300 | StyleBox[
301 | StyleBox[
302 | DynamicBox[ToBoxes[
303 | FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"],
304 | StandardForm],
305 | ImageSizeCache->{54., {0., 8.}}],
306 | StripOnInput->False,
307 | DynamicUpdating->True,
308 | LineSpacing->{1, 2},
309 | LineIndent->0,
310 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl",
311 | StripOnInput->False], True->
312 | StyleBox[
313 | StyleBox[
314 | DynamicBox[ToBoxes[
315 | FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"],
316 | StandardForm],
317 | ImageSizeCache->{54., {0., 8.}}],
318 | StripOnInput->False,
319 | DynamicUpdating->True,
320 | LineSpacing->{1, 2},
321 | LineIndent->0,
322 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive",
323 | StripOnInput->False]}, Dynamic[
324 | CurrentValue["MouseOver"]],
325 | Alignment->Center,
326 | FrameMargins->0,
327 | ImageSize->{Automatic, {25, Full}}],
328 | Appearance->None,
329 | BaselinePosition->Baseline,
330 |
331 | ButtonFunction:>OutputSizeLimit`ButtonFunction[
332 | OutputSizeLimit`Defer, 16, 29561006755129039129, 5/2],
333 | Enabled->True,
334 | Evaluator->Automatic,
335 | Method->"Queued"],
336 | ButtonBox[
337 | PaneSelectorBox[{False->
338 | StyleBox[
339 | StyleBox[
340 | DynamicBox[ToBoxes[
341 | FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"],
342 | StandardForm],
343 | ImageSizeCache->{61., {0., 8.}}],
344 | StripOnInput->False,
345 | DynamicUpdating->True,
346 | LineSpacing->{1, 2},
347 | LineIndent->0,
348 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl",
349 | StripOnInput->False], True->
350 | StyleBox[
351 | StyleBox[
352 | DynamicBox[ToBoxes[
353 | FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"],
354 | StandardForm]],
355 | StripOnInput->False,
356 | DynamicUpdating->True,
357 | LineSpacing->{1, 2},
358 | LineIndent->0,
359 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive",
360 | StripOnInput->False]}, Dynamic[
361 | CurrentValue["MouseOver"]],
362 | Alignment->Center,
363 | FrameMargins->0,
364 | ImageSize->{Automatic, {25, Full}}],
365 | Appearance->None,
366 | BaselinePosition->Baseline,
367 |
368 | ButtonFunction:>OutputSizeLimit`ButtonFunction[
369 | OutputSizeLimit`Defer, 16, 29561006755129039129, 5 2],
370 | Enabled->True,
371 | Evaluator->Automatic,
372 | Method->"Queued"],
373 | ButtonBox[
374 | PaneSelectorBox[{False->
375 | StyleBox[
376 | StyleBox[
377 | DynamicBox[ToBoxes[
378 | FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"],
379 | StandardForm],
380 | ImageSizeCache->{46., {0., 8.}}],
381 | StripOnInput->False,
382 | DynamicUpdating->True,
383 | LineSpacing->{1, 2},
384 | LineIndent->0,
385 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl",
386 | StripOnInput->False], True->
387 | StyleBox[
388 | StyleBox[
389 | DynamicBox[ToBoxes[
390 | FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"],
391 | StandardForm],
392 | ImageSizeCache->{46., {0., 8.}}],
393 | StripOnInput->False,
394 | DynamicUpdating->True,
395 | LineSpacing->{1, 2},
396 | LineIndent->0,
397 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive",
398 | StripOnInput->False]}, Dynamic[
399 | CurrentValue["MouseOver"]],
400 | Alignment->Center,
401 | FrameMargins->0,
402 | ImageSize->{Automatic, {25, Full}}],
403 | Appearance->None,
404 | BaselinePosition->Baseline,
405 |
406 | ButtonFunction:>OutputSizeLimit`ButtonFunction[
407 | OutputSizeLimit`Defer, 16, 29561006755129039129, Infinity],
408 | Enabled->True,
409 | Evaluator->Automatic,
410 | Method->"Queued"],
411 | ButtonBox[
412 | PaneSelectorBox[{False->
413 | StyleBox[
414 | StyleBox[
415 | DynamicBox[ToBoxes[
416 | FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"],
417 | StandardForm],
418 | ImageSizeCache->{83., {0., 8.}}],
419 | StripOnInput->False,
420 | DynamicUpdating->True,
421 | LineSpacing->{1, 2},
422 | LineIndent->0,
423 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl",
424 | StripOnInput->False], True->
425 | StyleBox[
426 | StyleBox[
427 | DynamicBox[ToBoxes[
428 | FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"],
429 | StandardForm]],
430 | StripOnInput->False,
431 | DynamicUpdating->True,
432 | LineSpacing->{1, 2},
433 | LineIndent->0,
434 | LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive",
435 | StripOnInput->False]}, Dynamic[
436 | CurrentValue["MouseOver"]],
437 | Alignment->Center,
438 | FrameMargins->0,
439 | ImageSize->{Automatic, {25, Full}}],
440 | Appearance->None,
441 | BaselinePosition->Baseline,
442 | ButtonFunction:>FrontEndExecute[{
443 | FrontEnd`SetOptions[
444 | FrontEnd`$FrontEnd,
445 | FrontEnd`PreferencesSettings -> {"Page" -> "Advanced"}],
446 | FrontEnd`FrontEndToken["PreferencesDialog"]}],
447 | Evaluator->None,
448 | Method->"Preemptive"]}
449 | },
450 | AutoDelete->False,
451 | FrameStyle->GrayLevel[0.85],
452 | GridBoxDividers->{"Columns" -> {False, {True}}},
453 | GridBoxItemSize->{"Columns" -> {{Automatic}}, "Rows" -> {{Automatic}}},
454 | GridBoxSpacings->{"Columns" -> {{2}}}]}
455 | },
456 | DefaultBaseStyle->"Column",
457 | GridBoxAlignment->{"Columns" -> {{Left}}, "Rows" -> {{Baseline}}},
458 | GridBoxDividers->{"Columns" -> {{False}}, "Rows" -> {{False}}},
459 | GridBoxItemSize->{"Columns" -> {{Automatic}}, "Rows" -> {{1.}}},
460 | GridBoxSpacings->{"Columns" -> {
461 | Offset[0.27999999999999997`], {
462 | Offset[0.5599999999999999]},
463 | Offset[0.27999999999999997`]}, "Rows" -> {
464 | Offset[0.2],
465 | Offset[1.2], {
466 | Offset[0.4]},
467 | Offset[0.2]}}],
468 | BaseStyle->"OutputSizeLimit",
469 | FrameMargins->{{12, 12}, {0, 15}},
470 | FrameStyle->GrayLevel[0.85],
471 | RoundingRadius->5,
472 | StripOnInput->False],
473 | Deploy,
474 | DefaultBaseStyle->"Deploy"],
475 | If[29561006755129039129 === $SessionID,
476 | Out[16], Message[
477 | MessageName[Syntax, "noinfoker"]]; Missing["NotAvailable"];
478 | Null]]], "Output",
479 | CellChangeTimes->{
480 | 3.823595603762179*^9, {3.823595635713426*^9, 3.8235956777038507`*^9},
481 | 3.823595712209647*^9, 3.823599409865849*^9, {3.823599459668364*^9,
482 | 3.823599464038105*^9}, {3.823602848043659*^9, 3.823602886956541*^9}},
483 | CellLabel->"Out[16]=",ExpressionUUID->"38574d08-ae8e-4fc2-be09-08955360fdff"]
484 | }, Open ]],
485 |
486 | Cell[CellGroupData[{
487 |
488 | Cell[BoxData[{
489 | RowBox[{
490 | RowBox[{"tmp", "=",
491 | RowBox[{"D", "[",
492 | RowBox[{
493 | RowBox[{"eM", "[",
494 | RowBox[{"[", "6", "]"}], "]"}], ",", "\[Theta]7"}], "]"}]}],
495 | ";"}], "\[IndentingNewLine]",
496 | RowBox[{
497 | RowBox[{"ToString", "[",
498 | RowBox[{"tmp", ",", "CForm"}], "]"}], ";"}], "\[IndentingNewLine]",
499 | RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input",
500 | CellChangeTimes->{{3.823594395534802*^9, 3.823594412717284*^9}, {
501 | 3.823594895697111*^9, 3.823594908600604*^9}, {3.823594963617971*^9,
502 | 3.823595138170384*^9}, {3.823595177571471*^9, 3.8235953003878927`*^9},
503 | 3.8237517626082907`*^9},
504 | CellLabel->"In[23]:=",ExpressionUUID->"f934a522-8b1f-4ba2-941b-f8a1252d3712"],
505 |
506 | Cell[BoxData[
507 | TemplateBox[{
508 | "Part","partd",
509 | "\"Part specification \\!\\(\\*RowBox[{\\\"eM\\\", \
510 | \\\"\[LeftDoubleBracket]\\\", \\\"6\\\", \\\"\[RightDoubleBracket]\\\"}]\\) \
511 | is longer than depth of object.\"",2,23,1,29561611951775162962,"Local"},
512 | "MessageTemplate"]], "Message", "MSG",
513 | CellChangeTimes->{3.8237517631418056`*^9},
514 | CellLabel->
515 | "During evaluation of \
516 | In[23]:=",ExpressionUUID->"00ec474b-f02f-45f7-861f-2e1395d96af0"]
517 | }, Open ]],
518 |
519 | Cell[BoxData[
520 | RowBox[{
521 | RowBox[{"pelbow", "[",
522 | RowBox[{"\[Theta]1_", ",", "\[Theta]2_", ",", "\[Theta]3_"}], "]"}], ":=",
523 |
524 | RowBox[{
525 | RowBox[{"T", "[",
526 | RowBox[{"0", ",", "0", ",", "\[Theta]1", ",", "0.333"}], "]"}], ".",
527 | RowBox[{"T", "[",
528 | RowBox[{
529 | RowBox[{"-",
530 | FractionBox["\[Pi]", "2"]}], ",", "0", ",", "\[Theta]2", ",", "0"}],
531 | "]"}], ".",
532 | RowBox[{"T", "[",
533 | RowBox[{
534 | FractionBox["\[Pi]", "2"], ",", "0", ",", "\[Theta]3", ",", "0.316"}],
535 | "]"}], ".",
536 | RowBox[{"T", "[",
537 | RowBox[{
538 | FractionBox["\[Pi]", "2"], ",", "0.0825", ",", "0", ",", "0"}],
539 | "]"}]}]}]], "Input",
540 | CellChangeTimes->{{3.823692303381714*^9, 3.823692313093701*^9}},
541 | CellLabel->"In[9]:=",ExpressionUUID->"945a2b31-754d-4fdb-8276-9e69341b497a"],
542 |
543 | Cell[BoxData[{
544 | RowBox[{
545 | RowBox[{"tmp", "=",
546 | RowBox[{"pelbow", "[",
547 | RowBox[{"\[Theta]1", ",", "\[Theta]2", ",", "\[Theta]3"}], "]"}]}],
548 | ";"}], "\[IndentingNewLine]",
549 | RowBox[{
550 | RowBox[{"ToString", "[",
551 | RowBox[{
552 | RowBox[{"Simplify", "[", "tmp", "]"}], ",", "CForm"}], "]"}],
553 | ";"}], "\[IndentingNewLine]",
554 | RowBox[{"CopyToClipboard", "[", "%", "]"}]}], "Input",
555 | CellChangeTimes->{{3.82369231492756*^9, 3.823692322197592*^9}, {
556 | 3.8236923671989527`*^9, 3.823692382966064*^9}, {3.823751749664494*^9,
557 | 3.823751764784334*^9}},
558 | CellLabel->"In[26]:=",ExpressionUUID->"b631774c-2d04-493b-8a99-35a0466935c8"]
559 | }, Open ]]
560 | },
561 | WindowSize->{1519, 1198},
562 | WindowMargins->{{Automatic, 264}, {66, Automatic}},
563 | FrontEndVersion->"12.0 for Linux x86 (64-bit) (April 8, 2019)",
564 | StyleDefinitions->"Default.nb"
565 | ]
566 | (* End of Notebook Content *)
567 |
568 | (* Internal cache information *)
569 | (*CellTagsOutline
570 | CellTagsIndex->{}
571 | *)
572 | (*CellTagsIndex
573 | CellTagsIndex->{}
574 | *)
575 | (*NotebookFileOutline
576 | Notebook[{
577 | Cell[CellGroupData[{
578 | Cell[580, 22, 158, 3, 55, "Subsection",ExpressionUUID->"2638955b-d1e8-4e28-ba05-db1765a28fd7"],
579 | Cell[741, 27, 1569, 42, 147, "Input",ExpressionUUID->"1293ae6c-31e5-4370-8907-caad95c252d0"],
580 | Cell[2313, 71, 1558, 43, 83, "Input",ExpressionUUID->"d8b24c1a-4cdd-43ed-9ee4-98459c265c06"],
581 | Cell[3874, 116, 625, 15, 31, "Input",ExpressionUUID->"5eced034-3e4c-4a28-956f-542ef9fac943"],
582 | Cell[4502, 133, 2978, 74, 193, "Input",ExpressionUUID->"25bca131-2606-4f40-956f-9b4b520b9c99"],
583 | Cell[7483, 209, 580, 14, 31, "Input",ExpressionUUID->"31153068-cde7-4919-97b6-1af27bb17782"],
584 | Cell[CellGroupData[{
585 | Cell[8088, 227, 801, 21, 31, "Input",ExpressionUUID->"9db25a01-77cb-4265-b9c7-eac93a89fd5b"],
586 | Cell[8892, 250, 9008, 232, 95, "Output",ExpressionUUID->"38574d08-ae8e-4fc2-be09-08955360fdff"]
587 | }, Open ]],
588 | Cell[CellGroupData[{
589 | Cell[17937, 487, 695, 16, 78, "Input",ExpressionUUID->"f934a522-8b1f-4ba2-941b-f8a1252d3712"],
590 | Cell[18635, 505, 443, 10, 23, "Message",ExpressionUUID->"00ec474b-f02f-45f7-861f-2e1395d96af0"]
591 | }, Open ]],
592 | Cell[19093, 518, 788, 22, 46, "Input",ExpressionUUID->"945a2b31-754d-4fdb-8276-9e69341b497a"],
593 | Cell[19884, 542, 630, 15, 78, "Input",ExpressionUUID->"b631774c-2d04-493b-8a99-35a0466935c8"]
594 | }, Open ]]
595 | }
596 | ]
597 | *)
598 |
599 |
--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
1 | import os
2 | import re
3 | import subprocess
4 | import sys
5 |
6 | from setuptools import setup, Extension, find_packages
7 | from setuptools.command.build_ext import build_ext
8 | from distutils.version import LooseVersion
9 |
10 |
11 | with open('README.md', 'r') as readme_file:
12 | long_description = readme_file.read()
13 |
14 |
15 | class CMakeExtension(Extension):
16 | def __init__(self, name, sourcedir=''):
17 | Extension.__init__(self, name, sources=[])
18 | self.sourcedir = os.path.abspath(sourcedir)
19 |
20 |
21 | class CMakeBuild(build_ext):
22 | def run(self):
23 | try:
24 | out = subprocess.check_output(['cmake', '--version'])
25 | except OSError:
26 | raise RuntimeError(
27 | 'CMake must be installed to build the following extensions: ' +
28 | ', '.join(e.name for e in self.extensions)
29 | )
30 |
31 | cmake_version = LooseVersion(re.search(r'version\s*([\d.]+)', out.decode()).group(1))
32 | if cmake_version < LooseVersion('3.10.0'):
33 | raise RuntimeError('CMake >= 3.10.0 is required')
34 |
35 | for ext in self.extensions:
36 | self.build_extension(ext)
37 |
38 | def build_extension(self, ext):
39 | extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name)))
40 |
41 | # required for auto-detection of auxiliary "native" libs
42 | if not extdir.endswith(os.path.sep):
43 | extdir += os.path.sep
44 |
45 | build_type = os.environ.get('BUILD_TYPE', 'Release')
46 | build_args = ['--config', build_type]
47 |
48 | cmake_args = [
49 | '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=' + extdir,
50 | '-DPYTHON_EXECUTABLE={}'.format(sys.executable),
51 | '-DEXAMPLE_VERSION_INFO={}'.format(self.distribution.get_version()),
52 | '-DCMAKE_BUILD_TYPE=' + build_type,
53 | '-DUSE_PYTHON_EXTENSION=OFF',
54 | '-DBUILD_EXAMPLES=OFF',
55 | '-DBUILD_TESTS=OFF',
56 | '-DBUILD_SHARED_LIBS=OFF',
57 | '-DCMAKE_BUILD_WITH_INSTALL_RPATH=TRUE',
58 | '-DCMAKE_INSTALL_RPATH={}'.format('$ORIGIN'),
59 | '-DCMAKE_POSITION_INDEPENDENT_CODE=ON',
60 | ]
61 |
62 | if not os.path.exists(self.build_temp):
63 | os.makedirs(self.build_temp)
64 |
65 | subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp)
66 | subprocess.check_call(['cmake', '--build', '.', '--target', ext.name] + build_args, cwd=self.build_temp)
67 |
68 |
69 | setup(
70 | name='frankx',
71 | version='0.2.0',
72 | description='High-Level Motion Library for the Franka Panda Robot',
73 | long_description=long_description,
74 | long_description_content_type='text/markdown',
75 | author='Lars Berscheid',
76 | author_email='lars.berscheid@kit.edu',
77 | url='https://github.com/pantor/frankx',
78 | packages=find_packages(),
79 | license='LGPL',
80 | ext_modules=[CMakeExtension('_frankx'), CMakeExtension('pyaffx')],
81 | cmdclass=dict(build_ext=CMakeBuild),
82 | keywords=['robot', 'robotics', 'trajectory-generation', 'motion-control'],
83 | classifiers=[
84 | 'Development Status :: 4 - Beta',
85 | 'Intended Audience :: Science/Research',
86 | 'Topic :: Scientific/Engineering',
87 | 'License :: OSI Approved :: GNU Lesser General Public License v3 (LGPLv3)',
88 | 'Programming Language :: C++',
89 | ],
90 | python_requires='>=3.6',
91 | zip_safe=False,
92 | )
93 |
--------------------------------------------------------------------------------
/src/frankx/gripper.cpp:
--------------------------------------------------------------------------------
1 | #include
2 |
3 |
4 | namespace frankx {
5 |
6 | Gripper::Gripper(const std::string& fci_ip, double speed, double force): franka::Gripper(fci_ip), gripper_speed(speed), gripper_force(force) { }
7 |
8 | double Gripper::width() const {
9 | auto state = readOnce();
10 | return state.width + width_calibration;
11 | }
12 |
13 | bool Gripper::isGrasping() const {
14 | const double current_width = width();
15 | const bool libfranka_is_grasped = readOnce().is_grasped;
16 | const bool width_is_grasped = std::abs(current_width - last_clamp_width) < 0.003; // [m], magic number
17 | const bool width_larger_than_threshold = current_width > 0.005; // [m]
18 | return libfranka_is_grasped && width_is_grasped && width_larger_than_threshold;
19 | }
20 |
21 | bool Gripper::move(double width) { // [m]
22 | try {
23 | const bool result = ((franka::Gripper*) this)->move(width - width_calibration, gripper_speed); // [m] [m/s]
24 | const double current_width = this->width();
25 | if (current_width > 0.01 && std::abs(current_width - width) > 0.01) {
26 | has_error = true;
27 | throw std::runtime_error("Gripper does (" + std::to_string(current_width) + ") not do what it should (" + std::to_string(width) + ").");
28 | return false;
29 | }
30 | if (result) {
31 | has_error = false;
32 | }
33 | return result;
34 | } catch (franka::Exception const& e) {
35 | has_error = true;
36 | std::cout << e.what() << std::endl;
37 | this->stop();
38 | this->homing();
39 | return ((franka::Gripper*) this)->move(width - width_calibration, gripper_speed); // [m] [m/s]
40 | }
41 | }
42 |
43 | bool Gripper::move_unsafe(double width) { // [m]
44 | const bool result_stop = ((franka::Gripper*) this)->stop();
45 | return ((franka::Gripper*) this)->move(width - width_calibration, gripper_speed); // [m] [m/s]
46 | }
47 |
48 | std::future 
3 | 
10 |
11 |
12 |
13 | 
14 |
15 |
16 |
17 | 
18 |
19 |
20 |
21 | 
22 |
23 |
24 |
25 | 
26 |
27 |