franky-control 1.0.2__cp310-cp310-manylinux_2_28_x86_64.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,881 @@
1
+ Metadata-Version: 2.4
2
+ Name: franky-control
3
+ Version: 1.0.2
4
+ Summary: High-level control library for Franka robots.
5
+ Home-page: https://github.com/TimSchneider42/franky
6
+ Author: Tim Schneider
7
+ Author-email: tim@robot-learning.de
8
+ License: LGPL-3.0-or-later
9
+ Keywords: robot,robotics,trajectory-generation,motion-control
10
+ Classifier: Development Status :: 4 - Beta
11
+ Classifier: Intended Audience :: Science/Research
12
+ Classifier: Topic :: Scientific/Engineering
13
+ Classifier: Programming Language :: C++
14
+ Requires-Python: >=3.7
15
+ Description-Content-Type: text/markdown
16
+ License-File: LICENSE
17
+ Requires-Dist: numpy
18
+ Dynamic: author
19
+ Dynamic: author-email
20
+ Dynamic: classifier
21
+ Dynamic: description
22
+ Dynamic: description-content-type
23
+ Dynamic: home-page
24
+ Dynamic: keywords
25
+ Dynamic: license
26
+ Dynamic: license-file
27
+ Dynamic: requires-dist
28
+ Dynamic: requires-python
29
+ Dynamic: summary
30
+
31
+ <div align="center">
32
+ <img width="340" src="https://raw.githubusercontent.com/timschneider42/franky/master/doc/logo.svg?sanitize=true">
33
+ <h3 align="center">
34
+ High-Level Control Library for Franka Robots with Python and C++ Support
35
+ </h3>
36
+ </div>
37
+ <p align="center">
38
+ <a href="https://github.com/timschneider42/franky/actions">
39
+ <img src="https://github.com/timschneider42/franky/workflows/CI/badge.svg" alt="CI">
40
+ </a>
41
+
42
+ <a href="https://github.com/timschneider42/franky/actions">
43
+ <img src="https://github.com/timschneider42/franky/workflows/Publish/badge.svg" alt="Publish">
44
+ </a>
45
+
46
+ <a href="https://github.com/timschneider42/franky/issues">
47
+ <img src="https://img.shields.io/github/issues/timschneider42/franky.svg" alt="Issues">
48
+ </a>
49
+
50
+ <a href="https://github.com/timschneider42/franky/releases">
51
+ <img src="https://img.shields.io/github/v/release/timschneider42/franky.svg?include_prereleases&sort=semver" alt="Releases">
52
+ </a>
53
+
54
+ <a href="https://github.com/timschneider42/franky/blob/master/LICENSE">
55
+ <img src="https://img.shields.io/badge/license-LGPL-green.svg" alt="LGPL">
56
+ </a>
57
+ </p>
58
+
59
+ Franky is a high-level control library for Franka robots offering Python and C++ support.
60
+ By providing a high-level control interface, Franky eliminates the need for strict real-time programming at 1 kHz,
61
+ making control from non-real-time environments, such as Python programs, feasible.
62
+ Instead of relying on low-level control commands, Franky expects high-level position or velocity targets and
63
+ uses [Ruckig](https://github.com/pantor/ruckig) to plan time-optimal trajectories in real-time.
64
+
65
+ Although Python does not provide real-time guarantees, Franky strives to maintain as much real-time control as possible.
66
+ Motions can be preempted at any moment, prompting Franky to re-plan trajectories on the fly.
67
+ To handle unforeseen situations—such as unexpected contact with the environment — Franky includes a reaction system that
68
+ allows to update motion commands dynamically.
69
+ Furthermore, most non-real-time functionality of [libfranka](https://frankaemika.github.io/docs/libfranka.html), such as
70
+ Gripper control is made directly available in Python.
71
+
72
+ Check out the [tutorial](#-tutorial) and the [examples](https://github.com/TimSchneider42/franky/tree/master/examples)
73
+ for an introduction.
74
+ The full documentation can be found
75
+ at [https://timschneider42.github.io/franky/](https://timschneider42.github.io/franky/).
76
+
77
+ ## 🚀 Features
78
+
79
+ - **Control your Franka robot directly from Python in just a few lines!**
80
+ No more endless hours setting up ROS, juggling packages, or untangling dependencies. Just `pip install` — no ROS at all.
81
+
82
+ - **[Four control modes](#motion-types)**: [Cartesian position](#cartesian-position-control), [Cartesian velocity](#cartesian-velocity-control), [Joint position](#joint-position-control), [Joint velocity](#joint-velocity-control)
83
+ Franky uses [Ruckig](https://github.com/pantor/ruckig) to generate smooth, time-optimal trajectories while respecting velocity, acceleration, and jerk limits.
84
+
85
+ - **[Real-time control from Python and C++](#real-time-motions)**
86
+ Need to change the target while the robot’s moving? No problem. Franky re-plans trajectories on the fly so that you can preempt motions anytime.
87
+
88
+ - **[Reactive behavior](#-real-time-reactions)**
89
+ Robots don’t always go according to plan. Franky lets you define reactions to unexpected events—like contact with the environment — so you can change course in real-time.
90
+
91
+ - **[Motion and reaction callbacks](#motion-callbacks)**
92
+ Want to monitor what’s happening under the hood? Add callbacks to your motions and reactions. They won’t block the control thread and are super handy for debugging or logging.
93
+
94
+ - **Things are moving too fast? [Tune the robot's dynamics to your needs](#-robot)**
95
+ Adjust max velocity, acceleration, and jerk to match your setup or task. Fine control for smooth, safe operation.
96
+
97
+ - **Full Python access to the libfranka API**
98
+ Want to tweak impedance, read the robot state, set force thresholds, or mess with the Jacobian? Go for it. If libfranka supports it, chances are Franky does, too.
99
+
100
+ ## 📖 Python Quickstart Guide
101
+
102
+ Real-time kernel already installed and real-time permissions granted? Just install Franky via
103
+
104
+ ```bash
105
+ pip install franky-control
106
+ ```
107
+
108
+ otherwise, follow the [setup instructions](#setup) first.
109
+
110
+ Now we are already ready to go!
111
+ Unlock the brakes in the web interface, activate FCI, and start coding:
112
+ ```python
113
+ from franky import *
114
+
115
+ robot = Robot("172.16.0.2") # Replace this with your robot's IP
116
+
117
+ # Let's start slow (this lets the robot use a maximum of 5% of its velocity, acceleration, and jerk limits)
118
+ robot.relative_dynamics_factor = 0.05
119
+
120
+ # Move the robot 20cm along the relative X-axis of its end-effector
121
+ motion = CartesianMotion(Affine([0.2, 0.0, 0.0]), ReferenceType.Relative)
122
+ robot.move(motion)
123
+ ```
124
+
125
+ If you are seeing server version mismatch errors, such as
126
+ ```
127
+ franky.IncompatibleVersionException: libfranka: Incompatible library version (server version: 5, library version: 9)
128
+ ```
129
+ then your Franka robot is either not on the most recent firmware version or you are using the older Franka Panda model.
130
+ In any case, it's no big deal; just check [here](https://frankaemika.github.io/docs/compatibility.html) which libfranka version you need and follow our [instructions](installing-frankly) to install the appropriate Franky wheels.
131
+
132
+ ## <a id="setup" /> ⚙️ Setup
133
+
134
+ To install Franky, you have to follow three steps:
135
+
136
+ 1. Ensure that you are using a realtime kernel
137
+ 2. Ensure that the executing user has permission to run real-time applications
138
+ 3. Install Franky via pip or build it from source
139
+
140
+ ### Installing a real-time kernel
141
+
142
+ In order for Franky to function properly, it requires the underlying OS to use a realtime kernel.
143
+ Otherwise, you might see `communication_constrains_violation` errors.
144
+
145
+ To check whether your system is currently using a real-time kernel, type `uname -a`.
146
+ You should see something like this:
147
+
148
+ ```
149
+ $ uname -a
150
+ Linux [PCNAME] 5.15.0-1056-realtime #63-Ubuntu SMP PREEMPT_RT ...
151
+ ```
152
+
153
+ If it does not say PREEMPT_RT, you are not currently running a real-time kernel.
154
+
155
+ There are multiple ways of installing a real-time kernel.
156
+ You
157
+ can [build it from source](https://frankaemika.github.io/docs/installation_linux.html#setting-up-the-real-time-kernel)
158
+ or, if you are using Ubuntu, it can be [enabled through Ubuntu Pro](https://ubuntu.com/real-time).
159
+
160
+ ### Allowing the executing user to run real-time applications
161
+
162
+ First, create a group `realtime` and add your user (or whoever is running Franky) to this group:
163
+
164
+ ```bash
165
+ sudo addgroup realtime
166
+ sudo usermod -a -G realtime $(whoami)
167
+ ```
168
+
169
+ Afterward, add the following limits to the real-time group in /etc/security/limits.conf:
170
+
171
+ ```
172
+ @realtime soft rtprio 99
173
+ @realtime soft priority 99
174
+ @realtime soft memlock 102400
175
+ @realtime hard rtprio 99
176
+ @realtime hard priority 99
177
+ @realtime hard memlock 102400
178
+ ```
179
+
180
+ Log out and log in again to let the changes take effect.
181
+
182
+ To verify that the changes were applied, check if your user is in the `realtime` group:
183
+
184
+ ```bash
185
+ $ groups
186
+ ... realtime
187
+ ```
188
+
189
+ If realtime is not listed in your groups, try rebooting.
190
+
191
+ ### Installing Franky
192
+
193
+ To start using Franky with Python and libfranka *0.15.0*, just install it via
194
+
195
+ ```bash
196
+ pip install franky-control
197
+ ```
198
+
199
+ We also provide wheels for libfranka versions *0.7.1*, *0.8.0*, *0.9.2*, *0.10.0*, *0.11.0*, *0.12.1*, *0.13.3*,
200
+ *0.14.2*, and *0.15.0*.
201
+ They can be installed via
202
+
203
+ ```bash
204
+ VERSION=0-9-2
205
+ wget https://github.com/TimSchneider42/franky/releases/latest/download/libfranka_${VERSION}_wheels.zip
206
+ unzip libfranka_${VERSION}_wheels.zip
207
+ pip install numpy
208
+ pip install --no-index --find-links=./dist franky-control
209
+ ```
210
+
211
+ Franky is based on [libfranka](https://github.com/frankaemika/libfranka), [Eigen](https://eigen.tuxfamily.org) for
212
+ transformation calculations and [pybind11](https://github.com/pybind/pybind11) for the Python bindings.
213
+ As the Franka is sensitive to acceleration discontinuities, it requires jerk-constrained motion generation, for which
214
+ Franky uses the [Ruckig](https://ruckig.com) community version for Online Trajectory Generation (OTG).
215
+
216
+ After installing the dependencies (the exact versions can be found [here](#-development)), you can build and install
217
+ Franky via
218
+
219
+ ```bash
220
+ git clone --recurse-submodules git@github.com:timschneider42/franky.git
221
+ cd franky
222
+ mkdir -p build
223
+ cd build
224
+ cmake -DCMAKE_BUILD_TYPE=Release ..
225
+ make
226
+ make install
227
+ ```
228
+
229
+ To use Franky, you can also include it as a subproject in your parent CMake via `add_subdirectory(franky)` and then
230
+ `target_link_libraries(<target> franky)`.
231
+
232
+ If you need only the Python module, you can install Franky via
233
+
234
+ ```bash
235
+ pip install .
236
+ ```
237
+
238
+ Make sure that the built library `_franky.cpython-3**-****-linux-gnu.so` is in the Python path, e.g. by adjusting
239
+ `PYTHONPATH` accordingly.
240
+
241
+ #### Using Docker
242
+
243
+ To use Franky within Docker we provide a [Dockerfile](docker/run/Dockerfile) and
244
+ accompanying [docker-compose](docker-compose.yml) file.
245
+
246
+ ```bash
247
+ git clone --recurse-submodules https://github.com/timschneider42/franky.git
248
+ cd franky/
249
+ docker compose build franky-run
250
+ ```
251
+
252
+ To use another version of libfranka than the default (0.15.0) add a build argument:
253
+
254
+ ```bash
255
+ docker compose build franky-run --build-arg LIBFRANKA_VERSION=0.9.2
256
+ ```
257
+
258
+ To run the container:
259
+
260
+ ```bash
261
+ docker compose run franky-run bash
262
+ ```
263
+
264
+ The container requires access to the host machines network *and* elevated user rights to allow the docker user to set RT
265
+ capabilities of the processes run from within it.
266
+
267
+ #### Building Franky with Docker
268
+
269
+ For building Franky and its wheels, we provide another Docker container that can also be launched using docker-compose:
270
+
271
+ ```bash
272
+ docker compose build franky-build
273
+ docker compose run --rm franky-build run-tests # To run the tests
274
+ docker compose run --rm franky-build build-wheels # To build wheels for all supported python versions
275
+ ```
276
+
277
+ ## 📚 Tutorial
278
+
279
+ Franky comes with both a C++ and Python API that differ only regarding real-time capability.
280
+ We will introduce both languages next to each other.
281
+ In your C++ project, just include `include <franky.hpp>` and link the library.
282
+ For Python, just `import franky`.
283
+ As a first example, only four lines of code are needed for simple robotic motions.
284
+
285
+ ```c++
286
+ #include <franky.hpp>
287
+ using namespace franky;
288
+
289
+ // Connect to the robot with the FCI IP address
290
+ Robot robot("172.16.0.2");
291
+
292
+ // Reduce velocity and acceleration of the robot
293
+ robot.setRelativeDynamicsFactor(0.05);
294
+
295
+ // Move the end-effector 20cm in positive x-direction
296
+ auto motion = std::make_shared<CartesianMotion>(RobotPose(Affine({0.2, 0.0, 0.0}), 0.0), ReferenceType::Relative);
297
+
298
+ // Finally move the robot
299
+ robot.move(motion);
300
+ ```
301
+
302
+ The corresponding program in Python is
303
+
304
+ ```python
305
+ from franky import Affine, CartesianMotion, Robot, ReferenceType
306
+
307
+ robot = Robot("172.16.0.2")
308
+ robot.relative_dynamics_factor = 0.05
309
+
310
+ motion = CartesianMotion(Affine([0.2, 0.0, 0.0]), ReferenceType.Relative)
311
+ robot.move(motion)
312
+ ```
313
+
314
+ Furthermore, we will introduce methods for geometric calculations, for moving the robot according to different motion
315
+ types, how to implement real-time reactions and changing waypoints in real time as well as controlling the gripper.
316
+
317
+ ### 🧮 Geometry
318
+
319
+ `franky.Affine` is a python wrapper for [Eigen::Affine3d](https://eigen.tuxfamily.org/dox/group__TutorialGeometry.html).
320
+ It is used for Cartesian poses, frames and transformation.
321
+ franky adds its own constructor, which takes a position and a quaternion as inputs:
322
+
323
+ ```python
324
+ import math
325
+ from scipy.spatial.transform import Rotation
326
+ from franky import Affine
327
+
328
+ z_translation = Affine([0.0, 0.0, 0.5])
329
+
330
+ quat = Rotation.from_euler("xyz", [0, 0, math.pi / 2]).as_quat()
331
+ z_rotation = Affine([0.0, 0.0, 0.0], quat)
332
+
333
+ combined_transformation = z_translation * z_rotation
334
+ ```
335
+
336
+ In all cases, distances are in [m] and rotations in [rad].
337
+
338
+ ### 🤖 Robot
339
+
340
+ Franky exposes most of the libfanka API for Python.
341
+ Moreover, we added methods to adapt the dynamics limits of the robot for all motions.
342
+
343
+ ```python
344
+ from franky import *
345
+
346
+ robot = Robot("172.16.0.2")
347
+
348
+ # Recover from errors
349
+ robot.recover_from_errors()
350
+
351
+ # Set velocity, acceleration and jerk to 5% of the maximum
352
+ robot.relative_dynamics_factor = 0.05
353
+
354
+ # Alternatively, you can define each constraint individually
355
+ robot.relative_dynamics_factor = RelativeDynamicsFactor(velocity=0.1, acceleration=0.05, jerk=0.1)
356
+
357
+ # Or, for more finegrained access, set individual limits
358
+ robot.translation_velocity_limit.set(3.0)
359
+ robot.rotation_velocity_limit.set(2.5)
360
+ robot.elbow_velocity_limit.set(2.62)
361
+ robot.translation_acceleration_limit.set(9.0)
362
+ robot.rotation_acceleration_limit.set(17.0)
363
+ robot.elbow_acceleration_limit.set(10.0)
364
+ robot.translation_jerk_limit.set(4500.0)
365
+ robot.rotation_jerk_limit.set(8500.0)
366
+ robot.elbow_jerk_limit.set(5000.0)
367
+ robot.joint_velocity_limit.set([2.62, 2.62, 2.62, 2.62, 5.26, 4.18, 5.26])
368
+ robot.joint_acceleration_limit.set([10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0])
369
+ robot.joint_jerk_limit.set([5000.0, 5000.0, 5000.0, 5000.0, 5000.0, 5000.0, 5000.0])
370
+ # By default, these limits are set to their respective maxima (the values shown here)
371
+
372
+ # Get the max of each limit (as provided by Franka) with the max function, e.g.:
373
+ print(robot.joint_jerk_limit.max)
374
+ ```
375
+
376
+ #### Robot State
377
+
378
+ The robot state can be retrieved by accessing the following properties:
379
+
380
+ * `state`: Object of type `franky.RobotState`, which extends the
381
+ libfranka [franka::RobotState](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html) structure by
382
+ additional state elements.
383
+ * `current_cartesian_state`: Object of type `franky.CartesianState`, which contains the end-effector pose and velocity
384
+ obtained
385
+ from [franka::RobotState::O_T_EE](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html#a193781d47722b32925e0ea7ac415f442)
386
+ and [franka::RobotState::O_dP_EE_c](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html#a4be112bd1a9a7d777a67aea4a18a8dcc).
387
+ * `current_joint_position`: Object of type `franky.JointState`, which contains the joint positions and velocities
388
+ obtained
389
+ from [franka::RobotState::q](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html#ade3335d1ac2f6c44741a916d565f7091)
390
+ and [franka::RobotState::dq](https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html#a706045af1b176049e9e56df755325bd2).
391
+
392
+ ```python
393
+ from franky import *
394
+
395
+ robot = Robot("172.16.0.2")
396
+
397
+ # Get the current state as `franky.RobotState`. See the documentation for a list of fields.
398
+ state = robot.state
399
+
400
+ # Get the robot's cartesian state
401
+ cartesian_state = robot.current_cartesian_state
402
+ robot_pose = cartesian_state.pose # Contains end-effector pose and elbow position
403
+ ee_pose = robot_pose.end_effector_pose
404
+ elbow_pos = robot_pose.elbow_state
405
+ robot_velocity = cartesian_state.velocity # Contains end-effector twist and elbow velocity
406
+ ee_twist = robot_velocity.end_effector_twist
407
+ elbow_vel = robot_velocity.elbow_velocity
408
+
409
+ # Get the robot's joint state
410
+ joint_state = robot.current_joint_state
411
+ joint_pos = joint_state.position
412
+ joint_vel = joint_state.velocity
413
+
414
+ # Use the robot model to compute kinematics
415
+ q = [-0.3, 0.1, 0.3, -1.4, 0.1, 1.8, 0.7]
416
+ f_t_ee = Affine()
417
+ ee_t_k = Affine()
418
+ ee_pose_kin = robot.model.pose(Frame.EndEffector, q, f_t_ee, ee_t_k)
419
+
420
+ # Get the jacobian of the current robot state
421
+ jacobian = robot.model.body_jacobian(Frame.EndEffector, state)
422
+
423
+ # Alternatively, just get the URDF as string and do the kinematics computation yourself (only for libfranka >= 0.15.0)
424
+ urdf_model = robot.model_urdf
425
+ ```
426
+
427
+ For a full list of state-related features, check
428
+ the [Robot](https://timschneider42.github.io/franky/classfranky_1_1_robot.html)
429
+ and [Model](https://timschneider42.github.io/franky/classfranky_1_1_model.html) sections of the documentation.
430
+
431
+ ### <a id="motion-types" /> 🏃‍♂️ Motion Types
432
+
433
+ Franky currently supports four different impedance control modes: **joint position control**, **joint velocity control
434
+ **, **cartesian position control**, and **cartesian velocity control**.
435
+ Each of these control modes is invoked by passing the robot an appropriate _Motion_ object.
436
+
437
+ In the following, we provide a brief example for each motion type implemented by Franky in Python.
438
+ The C++ interface is generally analogous, though some variable and method names are different because we
439
+ follow [PEP 8](https://peps.python.org/pep-0008/) naming conventions in Python
440
+ and [Google naming conventions](https://google.github.io/styleguide/cppguide.html) in C++.
441
+
442
+ All units are in $m$, $\frac{m}{s}$, $\textit{rad}$, or $\frac{\textit{rad}}{s}$.
443
+
444
+ #### Joint Position Control
445
+
446
+ ```python
447
+ from franky import *
448
+
449
+ # A point-to-point motion in the joint space
450
+ m_jp1 = JointMotion([-0.3, 0.1, 0.3, -1.4, 0.1, 1.8, 0.7])
451
+
452
+ # A motion in joint space with multiple waypoints
453
+ m_jp2 = JointWaypointMotion([
454
+ JointWaypoint([-0.3, 0.1, 0.3, -1.4, 0.1, 1.8, 0.7]),
455
+ JointWaypoint([0.0, 0.3, 0.3, -1.5, -0.2, 1.5, 0.8]),
456
+ JointWaypoint([0.1, 0.4, 0.3, -1.4, -0.3, 1.7, 0.9])
457
+ ])
458
+
459
+ # Intermediate waypoints also permit to specify target velocities. The default target velocity is 0, meaning that the
460
+ # robot will stop at every waypoint.
461
+ m_jp3 = JointWaypointMotion([
462
+ JointWaypoint([-0.3, 0.1, 0.3, -1.4, 0.1, 1.8, 0.7]),
463
+ JointWaypoint(
464
+ JointState(
465
+ position=[0.0, 0.3, 0.3, -1.5, -0.2, 1.5, 0.8],
466
+ velocity=[0.1, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0])),
467
+ JointWaypoint([0.1, 0.4, 0.3, -1.4, -0.3, 1.7, 0.9])
468
+ ])
469
+
470
+ # Stop the robot in joint position control mode. The difference of JointStopMotion to other stop motions such as
471
+ # CartesianStopMotion is that # JointStopMotion # stops the robot in joint position control mode while
472
+ # CartesianStopMotion stops it in cartesian pose control mode. The difference becomes relevant when asynchronous move
473
+ # commands are being sent or reactions are being used(see below).
474
+ m_jp4 = JointStopMotion()
475
+ ```
476
+
477
+ #### Joint Velocity Control
478
+
479
+ ```python
480
+ from franky import *
481
+
482
+ # Accelerate to the given joint velocity and hold it. After 1000ms stop the robot again.
483
+ m_jv1 = JointVelocityMotion([0.1, 0.3, -0.1, 0.0, 0.1, -0.2, 0.4], duration=Duration(1000))
484
+
485
+ # Joint velocity motions also support waypoints. Unlike in joint position control, a joint velocity waypoint is a
486
+ # target velocity to be reached. This particular example first accelerates the joints, holds the velocity for 1s, then
487
+ # reverses direction for 2s, reverses direction again for 1s, and finally stops. It is important not to forget to stop
488
+ # the robot at the end of such a sequence, as it will otherwise throw an error.
489
+ m_jv2 = JointVelocityWaypointMotion([
490
+ JointVelocityWaypoint([0.1, 0.3, -0.1, 0.0, 0.1, -0.2, 0.4], hold_target_duration=Duration(1000)),
491
+ JointVelocityWaypoint([-0.1, -0.3, 0.1, -0.0, -0.1, 0.2, -0.4], hold_target_duration=Duration(2000)),
492
+ JointVelocityWaypoint([0.1, 0.3, -0.1, 0.0, 0.1, -0.2, 0.4], hold_target_duration=Duration(1000)),
493
+ JointVelocityWaypoint([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
494
+ ])
495
+
496
+ # Stop the robot in joint velocity control mode.
497
+ m_jv3 = JointVelocityStopMotion()
498
+ ```
499
+
500
+ #### Cartesian Position Control
501
+
502
+ ```python
503
+ import math
504
+ from scipy.spatial.transform import Rotation
505
+ from franky import *
506
+
507
+ # Move to the given target pose
508
+ quat = Rotation.from_euler("xyz", [0, 0, math.pi / 2]).as_quat()
509
+ m_cp1 = CartesianMotion(Affine([0.4, -0.2, 0.3], quat))
510
+
511
+ # With target elbow angle (otherwise, the Franka firmware will choose by itself)
512
+ m_cp2 = CartesianMotion(RobotPose(Affine([0.4, -0.2, 0.3], quat), elbow_state=ElbowState(0.3)))
513
+
514
+ # A linear motion in cartesian space relative to the initial position
515
+ # (Note that this motion is relative both in position and orientation. Hence, when the robot's end-effector is oriented
516
+ # differently, it will move in a different direction)
517
+ m_cp3 = CartesianMotion(Affine([0.2, 0.0, 0.0]), ReferenceType.Relative)
518
+
519
+ # Generalization of CartesianMotion that allows for multiple waypoints
520
+ m_cp4 = CartesianWaypointMotion([
521
+ CartesianWaypoint(RobotPose(Affine([0.4, -0.2, 0.3], quat), elbow_state=ElbowState(0.3))),
522
+ # The following waypoint is relative to the prior one and 50% slower
523
+ CartesianWaypoint(Affine([0.2, 0.0, 0.0]), ReferenceType.Relative, RelativeDynamicsFactor(0.5, 1.0, 1.0))
524
+ ])
525
+
526
+ # Cartesian waypoints also permit to specify target velocities
527
+ m_cp5 = CartesianWaypointMotion([
528
+ CartesianWaypoint(Affine([0.5, -0.2, 0.3], quat)),
529
+ CartesianWaypoint(
530
+ CartesianState(
531
+ pose=Affine([0.4, -0.1, 0.3], quat),
532
+ velocity=Twist([-0.01, 0.01, 0.0]))),
533
+ CartesianWaypoint(Affine([0.3, 0.0, 0.3], quat))
534
+ ])
535
+
536
+ # Stop the robot in cartesian position control mode.
537
+ m_cp6 = CartesianStopMotion()
538
+ ```
539
+
540
+ #### Cartesian Velocity Control
541
+
542
+ ```python
543
+ from franky import *
544
+
545
+ # A cartesian velocity motion with linear (first argument) and angular (second argument) components
546
+ m_cv1 = CartesianVelocityMotion(Twist([0.2, -0.1, 0.1], [0.1, -0.1, 0.2]))
547
+
548
+ # With target elbow velocity
549
+ m_cv2 = CartesianVelocityMotion(RobotVelocity(Twist([0.2, -0.1, 0.1], [0.1, -0.1, 0.2]), elbow_velocity=-0.2))
550
+
551
+ # Cartesian velocity motions also support multiple waypoints. Unlike in cartesian position control, a cartesian velocity
552
+ # waypoint is a target velocity to be reached. This particular example first accelerates the end-effector, holds the
553
+ # velocity for 1s, then # reverses direction for 2s, reverses direction again for 1s, and finally stops. It is important
554
+ # not to forget to stop # the robot at the end of such a sequence, as it will otherwise throw an error.
555
+ m_cv4 = CartesianVelocityWaypointMotion([
556
+ CartesianVelocityWaypoint(Twist([0.2, -0.1, 0.1], [0.1, -0.1, 0.2]), hold_target_duration=Duration(1000)),
557
+ CartesianVelocityWaypoint(Twist([-0.2, 0.1, -0.1], [-0.1, 0.1, -0.2]), hold_target_duration=Duration(2000)),
558
+ CartesianVelocityWaypoint(Twist([0.2, -0.1, 0.1], [0.1, -0.1, 0.2]), hold_target_duration=Duration(1000)),
559
+ CartesianVelocityWaypoint(Twist()),
560
+ ])
561
+
562
+ # Stop the robot in cartesian velocity control mode.
563
+ m_cv6 = CartesianVelocityStopMotion()
564
+ ```
565
+
566
+ #### Relative Dynamics Factors
567
+
568
+ Every motion and waypoint type allows to adapt the dynamics (velocity, acceleration and jerk) by setting the respective
569
+ `relative_dynamics_factor` parameter.
570
+ This parameter can also be set for the robot globally as shown below or in the `robot.move` command.
571
+ Crucially, relative dynamics factors on different layers (robot, move command, and motion) do not override each other
572
+ but rather get multiplied.
573
+ Hence, a relative dynamics factor on a motion can only reduce the dynamics of the robot and never increase them.
574
+
575
+ There is one exception to this rule and that is if any layer sets the relative dynamics factor to
576
+ `RelativeDynamicsFactor.MAX_DYNAMICS`.
577
+ This will cause the motion to be executed with maximum velocity, acceleration, and jerk limits, independently of the
578
+ relative dynamics factors of the other layers.
579
+ This feature should only be used to abruptly stop the robot in case of an unexpected environment contact as executing
580
+ other motions with it is likely to lead to a discontinuity error and might be dangerous.
581
+
582
+ #### Executing Motions
583
+
584
+ The real robot can be moved by applying a motion to the robot using `move`:
585
+
586
+ ```python
587
+ # Before moving the robot, set an appropriate dynamics factor. We start small:
588
+ robot.relative_dynamics_factor = 0.05
589
+ # or alternatively, to control the scaling of velocity, acceleration, and jerk limits separately:
590
+ robot.relative_dynamics_factor = RelativeDynamicsFactor(0.05, 0.1, 0.15)
591
+ # If these values are set too high, you will see discontinuity errors
592
+
593
+ robot.move(m_jp1)
594
+
595
+ # We can also set a relative dynamics factor in the move command. It will be multiplied with the other relative
596
+ # dynamics factors (robot and motion if present).
597
+ robot.move(m_jp2, relative_dynamics_factor=0.8)
598
+ ```
599
+
600
+ #### Motion Callbacks
601
+
602
+ All motions support callbacks, which will be invoked in every control step at 1kHz.
603
+ Callbacks can be attached as follows:
604
+
605
+ ```python
606
+ def cb(
607
+ robot_state: RobotState,
608
+ time_step: Duration,
609
+ rel_time: Duration,
610
+ abs_time: Duration,
611
+ control_signal: JointPositions):
612
+ print(f"At time {abs_time}, the target joint positions were {control_signal.q}")
613
+
614
+
615
+ m_jp1.register_callback(cb)
616
+ robot.move(m_jp1)
617
+ ```
618
+
619
+ Note that in Python, these callbacks are not executed in the control thread since they would otherwise block it.
620
+ Instead, they are put in a queue and executed by another thread.
621
+ While this scheme ensures that the control thread can always run, it cannot prevent that the queue grows indefinitely
622
+ when the callbacks take more time to execute than it takes for new callbacks to be queued.
623
+ Hence, callbacks might be executed significantly after they were queued if they take a long time to execute.
624
+
625
+ ### ⚡ Real-Time Reactions
626
+
627
+ By adding reactions to the motion data, the robot can react to unforeseen events.
628
+ In the Python API, you can define conditions by using a comparison between a robot's value and a given threshold.
629
+ If the threshold is exceeded, the reaction fires.
630
+
631
+ ```python
632
+ from franky import CartesianMotion, Affine, ReferenceType, Measure, Reaction
633
+
634
+ motion = CartesianMotion(Affine([0.0, 0.0, 0.1]), ReferenceType.Relative) # Move down 10cm
635
+
636
+ # It is important that the reaction motion uses the same control mode as the original motion. Hence, we cannot register
637
+ # a JointMotion as a reaction motion to a CartesianMotion.
638
+ reaction_motion = CartesianMotion(Affine([0.0, 0.0, 0.01]), ReferenceType.Relative) # Move up for 1cm
639
+
640
+ # Trigger reaction if the Z force is greater than 30N
641
+ reaction = Reaction(Measure.FORCE_Z > 30.0, reaction_motion)
642
+ motion.add_reaction(reaction)
643
+
644
+ robot.move(motion)
645
+ ```
646
+
647
+ Possible values to measure are
648
+
649
+ * `Measure.FORCE_X,` `Measure.FORCE_Y,` `Measure.FORCE_Z`: Force in X, Y and Z direction
650
+ * `Measure.REL_TIME`: Time in seconds since the current motion started
651
+ * `Measure.ABS_TIME`: Time in seconds since the initial motion started
652
+
653
+ The difference between `Measure.REL_TIME` and `Measure.ABS_TIME` is that `Measure.REL_TIME` is reset to zero whenever a
654
+ new motion starts (either by calling `Robot.move` or as a result of a triggered `Reaction`).
655
+ `Measure.ABS_TIME`, on the other hand, is only reset to zero when a motion terminates regularly without being
656
+ interrupted and the robot stops moving.
657
+ Hence, `Measure.ABS_TIME` measures the total time in which the robot has moved without interruption.
658
+
659
+ `Measure` values support all classical arithmetic operations, like addition, subtraction, multiplication, division, and
660
+ exponentiation (both as base and exponent).
661
+
662
+ ```python
663
+ normal_force = (Measure.FORCE_X ** 2 + Measure.FORCE_Y ** 2 + Measure.FORCE_Z ** 2) ** 0.5
664
+ ```
665
+
666
+ With arithmetic comparisons, conditions can be generated.
667
+
668
+ ```python
669
+ normal_force_within_bounds = normal_force < 30.0
670
+ time_up = Measure.ABS_TIME > 10.0
671
+ ```
672
+
673
+ Conditions support negation, conjunction (and), and disjunction (or):
674
+
675
+ ```python
676
+ abort = ~normal_force_within_bounds | time_up
677
+ fast_abort = ~normal_force_within_bounds | time_up
678
+ ```
679
+
680
+ To check whether a reaction has fired, a callback can be attached:
681
+
682
+ ```python
683
+ from franky import RobotState
684
+
685
+
686
+ def reaction_callback(robot_state: RobotState, rel_time: float, abs_time: float):
687
+ print(f"Reaction fired at {abs_time}.")
688
+
689
+
690
+ reaction.register_callback(reaction_callback)
691
+ ```
692
+
693
+ Similar to the motion callbacks, in Python, reaction callbacks are not executed in real-time but in a regular thread
694
+ with lower priority to ensure that the control thread does not get blocked.
695
+ Thus, the callbacks might fire substantially after the reaction has fired, depending on the time it takes to execute
696
+ them.
697
+
698
+ In C++ you can additionally use lambdas to define more complex behaviours:
699
+
700
+ ```c++
701
+ auto motion = CartesianMotion(RobotPose(Affine({0.0, 0.0, 0.2}), 0.0), ReferenceType::Relative);
702
+
703
+ // Stop motion if force is over 10N
704
+ auto stop_motion = StopMotion<franka::CartesianPose>()
705
+
706
+ motion
707
+ .addReaction(
708
+ Reaction(
709
+ Measure::ForceZ() > 10.0, // [N],
710
+ stop_motion))
711
+ .addReaction(
712
+ Reaction(
713
+ Condition(
714
+ [](const franka::RobotState& state, double rel_time, double abs_time) {
715
+ // Lambda condition
716
+ return state.current_errors.self_collision_avoidance_violation;
717
+ }),
718
+ [](const franka::RobotState& state, double rel_time, double abs_time) {
719
+ // Lambda reaction motion generator
720
+ // (we are just returning a stop motion, but there could be arbitrary
721
+ // logic here for generating reaction motions)
722
+ return StopMotion<franka::CartesianPose>();
723
+ })
724
+ ));
725
+
726
+ robot.move(motion)
727
+ ```
728
+
729
+ ### <a id="real-time-motions" /> ⏱️ Real-Time Motions
730
+
731
+ By setting the `asynchronous` parameter of `Robot.move` to `True`, the function does not block until the motion
732
+ finishes.
733
+ Instead, it returns immediately and, thus, allows the main thread to set new motions asynchronously.
734
+
735
+ ```python
736
+ import time
737
+ from franky import Affine, CartesianMotion, Robot, ReferenceType
738
+
739
+ robot = Robot("172.16.0.2")
740
+ robot.relative_dynamics_factor = 0.05
741
+
742
+ motion1 = CartesianMotion(Affine([0.2, 0.0, 0.0]), ReferenceType.Relative)
743
+ robot.move(motion1, asynchronous=True)
744
+
745
+ time.sleep(0.5)
746
+ # Note that similar to reactions, when preempting active motions with new motions, the control mode cannot change.
747
+ # Hence, we cannot use, e.g., a JointMotion here.
748
+ motion2 = CartesianMotion(Affine([0.2, 0.0, 0.0]), ReferenceType.Relative)
749
+ robot.move(motion2, asynchronous=True)
750
+ ```
751
+
752
+ By calling `Robot.join_motion` the main thread can be synchronized with the motion thread, as it will block until the
753
+ robot finishes its motion.
754
+
755
+ ```python
756
+ robot.join_motion()
757
+ ```
758
+
759
+ Note that when exceptions occur during the asynchronous execution of a motion, they will not be thrown immediately.
760
+ Instead, the control thread stores the exception and terminates.
761
+ The next time `Robot.join_motion` or `Robot.move` are called, they will throw the stored exception in the main thread.
762
+ Hence, after an asynchronous motion has finished, make sure to call `Robot.join_motion` to ensure being notified of any
763
+ exceptions that occurred during the motion.
764
+
765
+ ### Gripper
766
+
767
+ In the `franky::Gripper` class, the default gripper force and gripper speed can be set.
768
+ Then, additionally to the libfranka commands, the following helper methods can be used:
769
+
770
+ ```c++
771
+ #include <franky.hpp>
772
+ #include <chrono>
773
+ #include <future>
774
+
775
+ auto gripper = franky::Gripper("172.16.0.2");
776
+
777
+ double speed = 0.02; // [m/s]
778
+ double force = 20.0; // [N]
779
+
780
+ // Move the fingers to a specific width (5cm)
781
+ bool success = gripper.move(0.05, speed);
782
+
783
+ // Grasp an object of unknown width
784
+ success &= gripper.grasp(0.0, speed, force, epsilon_outer=1.0);
785
+
786
+ // Get the width of the grasped object
787
+ double width = gripper.width();
788
+
789
+ // Release the object
790
+ gripper.open(speed);
791
+
792
+ // There are also asynchronous versions of the methods
793
+ std::future<bool> success_future = gripper.moveAsync(0.05, speed);
794
+
795
+ // Wait for 1s
796
+ if (!success_future.wait_for(std::chrono::seconds(1)) == std::future_status::ready) {
797
+ // Get the result
798
+ std::cout << "Success: " << success_future.get() << std::endl;
799
+ } else {
800
+ gripper.stop();
801
+ success_future.wait();
802
+ std::cout << "Gripper motion timed out." << std::endl;
803
+ }
804
+ ```
805
+
806
+ The Python API follows the c++ API closely:
807
+
808
+ ```python
809
+ import franky
810
+
811
+ gripper = franky.Gripper("172.16.0.2")
812
+
813
+ speed = 0.02 # [m/s]
814
+ force = 20.0 # [N]
815
+
816
+ # Move the fingers to a specific width (5cm)
817
+ success = gripper.move(0.05, speed)
818
+
819
+ # Grasp an object of unknown width
820
+ success &= gripper.grasp(0.0, speed, force, epsilon_outer=1.0)
821
+
822
+ # Get the width of the grasped object
823
+ width = gripper.width
824
+
825
+ # Release the object
826
+ gripper.open(speed)
827
+
828
+ # There are also asynchronous versions of the methods
829
+ success_future = gripper.move_async(0.05, speed)
830
+
831
+ # Wait for 1s
832
+ if success_future.wait(1):
833
+ print(f"Success: {success_future.get()}")
834
+ else:
835
+ gripper.stop()
836
+ success_future.wait()
837
+ print("Gripper motion timed out.")
838
+ ```
839
+
840
+ ## 🛠️ Development
841
+
842
+ Franky is currently tested against following versions
843
+
844
+ - libfranka 0.7.1, 0.8.0, 0.9.2, 0.10.0, 0.11.0, 0.12.1, 0.13.3, 0.14.2, 0.15.0
845
+ - Eigen 3.4.0
846
+ - Pybind11 2.13.6
847
+ - POCO 1.12.5p2
848
+ - Pinocchio 3.4.0
849
+ - Python 3.7, 3.8, 3.9, 3.10, 3.11, 3.12, 3.13
850
+ - Catch2 2.13.8 (for testing only)
851
+
852
+ ## 📜 License
853
+
854
+ For non-commercial applications, this software is licensed under the LGPL v3.0.
855
+ If you want to use Franky within commercial applications or under a different license, please contact us for individual
856
+ agreements.
857
+
858
+ ## 🔍 Differences to frankx
859
+
860
+ Franky started originally as a fork of [frankx](https://github.com/pantor/frankx), though both codebase and
861
+ functionality differ substantially from frankx by now.
862
+ Aside of bug fixes and general performance improvements, Franky provides the following new features/improvements:
863
+
864
+ * [Motions can be updated asynchronously.](#-real-time-motions)
865
+ * [Reactions allow for the registration of callbacks instead of just printing to stdout when fired.](#-real-time-reactions)
866
+ * [Motions allow for the registration of callbacks for profiling.](#motion-callbacks)
867
+ * [The robot state is also available during control.](#robot-state)
868
+ * A larger part of the libfranka API is exposed to python (e.g.,`setCollisionBehavior`, `setJoinImpedance`, and
869
+ `setCartesianImpedance`).
870
+ * Cartesian motion generation handles boundaries in Euler angles properly.
871
+ * [There is a new joint motion type that supports waypoints.](#-motion-types)
872
+ * [The signature of `Affine` changed.](#-geometry) `Affine` does not handle elbow positions anymore.
873
+ Instead, a new class `RobotPose` stores both the end-effector pose and optionally the elbow position.
874
+ * The `MotionData` class does not exist anymore.
875
+ Instead, reactions and other settings moved to `Motion`.
876
+ * [The `Measure` class allows for arithmetic operations.](#-real-time-reactions)
877
+ * Exceptions caused by libfranka are raised properly instead of being printed to stdout.
878
+ * [We provide wheels for both Franka Research 3 and the older Franka Panda](#-setup)
879
+ * Franky supports [joint velocity control](#joint-velocity-control)
880
+ and [cartesian velocity control](#cartesian-velocity-control)
881
+ * The dynamics limits are not hard-coded anymore but can be [set for each robot instance](#-robot).