feat: Added the Cartesian pose interface to pylibfranka

Author: AndreasKuhner

pylibfranka/README.md

@@ -6,7 +6,6 @@ This document provides comprehensive instructions for installing and using pylib
 - [Prerequisites](#prerequisites)
 - [Installation](#installation)
   - [Installation from Source](#installation-from-source)
-  - [Installation from Artifactory](#installation-from-artifactory)
 - [Examples](#examples)
   - [Joint Position Example](#joint-position-example)
   - [Print Robot State Example](#print-robot-state-example)

pylibfranka/__init__.py

@@ -2,6 +2,7 @@
 
 from ._pylibfranka import (
     ActiveControlBase,
+    CartesianPose,
     ControllerMode,
     Errors,
     Gripper,
@@ -18,6 +19,7 @@
 __all__ = [
     "ActiveControlBase",
     "ControllerMode",
+    "CartesianPose",
     "Errors",
     "JointPositions",
     "JointVelocities",

pylibfranka/examples/cartesian_pose_example.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2025 Franka Robotics GmbH
+# Use of this source code is governed by the Apache-2.0 license, see LICENSE
+
+import argparse
+import time
+
+import numpy as np
+
+from pylibfranka import ControllerMode, CartesianPose, Robot
+
+
+def main():
+    # Parse command line arguments
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--ip", type=str, default="localhost", help="Robot IP address")
+    args = parser.parse_args()
+
+    # Connect to robot
+    robot = Robot(args.ip)
+
+    try:
+        # Set collision behavior
+        lower_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]
+        upper_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]
+        lower_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]
+        upper_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]
+
+        robot.set_collision_behavior(
+            lower_torque_thresholds,
+            upper_torque_thresholds,
+            lower_force_thresholds,
+            upper_force_thresholds,
+        )
+
+        # First move the robot to a suitable joint configuration
+        print("WARNING: This example will move the robot!")
+        print("Please make sure to have the user stop button at hand!")
+        input("Press Enter to continue...")
+
+        # Start cartesian pose control with external control loop
+        active_control = robot.start_cartesian_pose_control(ControllerMode.JointImpedance)
+
+        time_elapsed = 0.0
+        motion_finished = False
+
+        robot_state, duration = active_control.readOnce()
+        initial_cartesian_pose = robot_state.O_T_EE
+
+        # External control loop
+        while not motion_finished:
+            # Read robot state and duration
+            robot_state, duration = active_control.readOnce()
+
+            kRadius = 0.3
+            angle = np.pi / 4 * (1 - np.cos(np.pi / 5.0 * time_elapsed))
+            delta_x = kRadius * np.sin(angle)
+            delta_z = kRadius * (np.cos(angle) - 1)
+
+            # Update time
+            time_elapsed += duration.to_sec()
+
+
+            # Update joint positions
+            new_cartesian_pose = initial_cartesian_pose.copy()
+            new_cartesian_pose[12] += delta_x  # x position
+            new_cartesian_pose[14] += delta_z  # z position
+
+            # Set joint positions
+            cartesian_pose = CartesianPose(new_cartesian_pose)
+
+            # Set motion_finished flag to True on the last update
+            if time_elapsed >= 5.0:
+                cartesian_pose.motion_finished = True
+                motion_finished = True
+                print("Finished motion, shutting down example")
+
+            # Send command to robot
+            active_control.writeOnce(cartesian_pose)
+
+    except Exception as e:
+        print(f"Error occurred: {e}")
+        if robot is not None:
+            robot.stop()
+        return -1
+
+
+if __name__ == "__main__":
+    main()

pylibfranka/include/pylibfranka.h

@@ -7,8 +7,7 @@
  *
  * This header file provides C++ classes that wrap the Franka Robotics Robot Control Library
  * for use in Python through pybind11. It offers:
- *
- * 1. PyRobot: A wrapper for the Franka robot control functionality, providing:
+ * PyRobot: A wrapper for the Franka robot control functionality, providing:
  *    - Active control methods (torque, joint position, joint velocity control)
  *    - Configuration methods (collision behavior, impedance settings, etc.)
  *    - State reading and control methods
@@ -44,11 +43,31 @@ class PyRobot {
   ~PyRobot() = default;
 
   // Active control methods
-  std::unique_ptr<franka::ActiveControlBase> startTorqueControl();
-  std::unique_ptr<franka::ActiveControlBase> startJointPositionControl(
-      const franka::ControllerMode&);
-  std::unique_ptr<franka::ActiveControlBase> startJointVelocityControl(
-      const franka::ControllerMode&);
+  /**
+   * Starts torque control mode.
+   */
+  auto startTorqueControl() -> std::unique_ptr<franka::ActiveControlBase>;
+
+  /**
+   * Starts the joint position control mode.
+   * @param control_type The type of controller to use (JointImpedance or CartesianImpedance).
+   */
+  auto startJointPositionControl(franka::ControllerMode controller_mode)
+      -> std::unique_ptr<franka::ActiveControlBase>;
+
+  /**
+   * Starts the joint velocity control mode.
+   * @param control_type The type of controller to use (JointImpedance or CartesianImpedance).
+   */
+  auto startJointVelocityControl(franka::ControllerMode controller_mode)
+      -> std::unique_ptr<franka::ActiveControlBase>;
+
+  /**
+   * Starts the Cartesian pose control mode.
+   * @param control_type The type of controller to use (JointImpedance or CartesianImpedance).
+   */
+  auto startCartesianPoseControl(franka::ControllerMode controller_mode)
+      -> std::unique_ptr<franka::ActiveControlBase>;
 
   // Configuration methods
   void setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds,

pylibfranka/src/pylibfranka.cpp

@@ -16,6 +16,26 @@
 
 namespace py = pybind11;
 
+namespace {
+
+/**
+ * Convert franka::ControllerMode to research_interface::robot::Move::ControllerMode
+ * @param mode franka::ControllerMode the mode to convert
+ * @return research_interface::robot::Move::ControllerMode
+ */
+auto convertControllerMode(franka::ControllerMode mode)
+    -> research_interface::robot::Move::ControllerMode {
+  switch (mode) {
+    case franka::ControllerMode::kJointImpedance:
+      return research_interface::robot::Move::ControllerMode::kJointImpedance;
+    case franka::ControllerMode::kCartesianImpedance:
+      return research_interface::robot::Move::ControllerMode::kCartesianImpedance;
+    default:
+      throw std::invalid_argument("Invalid franka::ControllerMode");
+  }
+}
+}  // namespace
+
 namespace pylibfranka {
 
 PyGripper::PyGripper(const std::string& franka_address)
@@ -51,30 +71,23 @@ franka::Gripper::ServerVersion PyGripper::serverVersion() {
 PyRobot::PyRobot(const std::string& franka_address, franka::RealtimeConfig realtime_config)
     : robot_(std::make_unique<franka::Robot>(franka_address, realtime_config)) {}
 
-std::unique_ptr<franka::ActiveControlBase> PyRobot::startTorqueControl() {
+auto PyRobot::startTorqueControl() -> std::unique_ptr<franka::ActiveControlBase> {
   return robot_->startTorqueControl();
 }
 
-std::unique_ptr<franka::ActiveControlBase> PyRobot::startJointPositionControl(
-    const franka::ControllerMode& control_type) {
-  research_interface::robot::Move::ControllerMode mode;
-  if (control_type == franka::ControllerMode::kJointImpedance) {
-    mode = research_interface::robot::Move::ControllerMode::kJointImpedance;
-  } else {
-    mode = research_interface::robot::Move::ControllerMode::kCartesianImpedance;
-  }
-  return robot_->startJointPositionControl(mode);
+auto PyRobot::startJointPositionControl(franka::ControllerMode controller_mode)
+    -> std::unique_ptr<franka::ActiveControlBase> {
+  return robot_->startJointPositionControl(convertControllerMode(controller_mode));
 }
 
-std::unique_ptr<franka::ActiveControlBase> PyRobot::startJointVelocityControl(
-    const franka::ControllerMode& control_type) {
-  research_interface::robot::Move::ControllerMode mode;
-  if (control_type == franka::ControllerMode::kJointImpedance) {
-    mode = research_interface::robot::Move::ControllerMode::kJointImpedance;
-  } else {
-    mode = research_interface::robot::Move::ControllerMode::kCartesianImpedance;
-  }
-  return robot_->startJointVelocityControl(mode);
+auto PyRobot::startJointVelocityControl(franka::ControllerMode controller_mode)
+    -> std::unique_ptr<franka::ActiveControlBase> {
+  return robot_->startJointVelocityControl(convertControllerMode(controller_mode));
+}
+
+auto PyRobot::startCartesianPoseControl(franka::ControllerMode controller_mode)
+    -> std::unique_ptr<franka::ActiveControlBase> {
+  return robot_->startCartesianPoseControl(convertControllerMode(controller_mode));
 }
 
 void PyRobot::setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds,
@@ -430,6 +443,7 @@ PYBIND11_MODULE(_pylibfranka, m) {
       .def("start_torque_control", &PyRobot::startTorqueControl)
       .def("start_joint_position_control", &PyRobot::startJointPositionControl)
       .def("start_joint_velocity_control", &PyRobot::startJointVelocityControl)
+      .def("start_cartesian_pose_control", &PyRobot::startCartesianPoseControl)
       .def("set_collision_behavior", &PyRobot::setCollisionBehavior)
       .def("set_joint_impedance", &PyRobot::setJointImpedance)
       .def("set_cartesian_impedance", &PyRobot::setCartesianImpedance)

pyproject.toml

@@ -13,7 +13,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "pylibfranka"
-version = "0.1.0"
+version = "0.2.0"
 description = "Python bindings for libfranka with automatic dependency bundling"
 authors = [{name = "Franka Robotics GmbH", email = "[email protected]"}]
 license = {text = "Apache-2.0"}