add readme, edit package info, add jacobian message, add test

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.4)
 project(serl_franka_controllers)
 
 set(CMAKE_BUILD_TYPE Release)
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 20)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
 find_package(catkin REQUIRED COMPONENTS
@@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(Eigen3 REQUIRED)
 find_package(Franka 0.7.0 REQUIRED)
 
+add_message_files(FILES ZeroJacobian.msg)
 generate_messages()
 
 generate_dynamic_reconfigure_options(

README.md

@@ -1,2 +1,51 @@
 # serl_franka_controller
-Compliant carteasian impedance controller for Franka Emika Robot
+SERL controller package for Franka Emika Robot.
+
+serl_franka_controller is a ROS package designed to control Franka Emika Robot through `libfranka` and `franka_ros`. This package provides a compliant yet accurate Cartesian Impedence Controller for safe online reinforcement learning algorithms, as well as a Joint Position Controller for resetting arm.
+
+## Installation
+
+### Prerequisites
+- ROS Noetic
+- Installation of `libfranka>=0.8.0` and `franka_ros>0.8.0` according to the [Franka FCI Documentation](https://frankaemika.github.io/docs/installation_linux.html)
+
+### Installing from Source
+
+1. Clone the repository into your catkin workspace:
+   ```bash
+   cd ~/catkin_ws/src
+   git clone [email protected]:rail-berkeley/serl_franka_controller.git
+   cd ~/catkin_ws
+   catkin_make -DFranka_DIR:PATH=/path/to/libfranka/build
+   source ~/catkin_ws/devel/setup.bash
+   ```
+
+## Usage
+### Cartesian Impedance Controller
+To launch the Cartesian Impedance Controller, use:
+```bash
+roslaunch serl_franka_controllers impedance.launch robot_ip:=<RobotIP> load_gripper:=<true/false>
+```
+Replace <RobotIP> with the IP address of your Franka robot. The load_gripper argument is a boolean value (true or false) depending on whether you have a gripper attached.
+
+### Interacting with the controller
+Compliance parameters for the controller can be adjusted in an interactive GUI by running `rosrun rqt_reconfigure rqt_reconfigure`. This can also be achieved in Python code as demonstrated in the example section.
+
+### Joint Position Controller
+For resetting or moving the robot to a specific joint position, launch the joint position controller:
+
+```bash
+rosparam set /target_joint_positions '[q1, q2, q3, q4, q5, q6, q7]'
+roslaunch serl_franka_controllers joint.launch robot_ip:=<RobotIP> load_gripper:=<true/false>
+```
+Here, you also need to replace <RobotIP> with the actual IP address and specify the load_gripper option. Then replace [q1, q2, q3, q4, q5, q6, q7] with the desired joint positions.
+
+
+## rospy Example
+We include a `requirements.txt` and python script to show one way of interacting with the controller. This script shows how to adjust the reference limiting values and how to send robot commands through ROS Topics and `dynamic_reconfigure`. To use this, run
+```bash
+conda create -n franka_controller python=3.8
+conda activate franka_controller
+pip install -r requirements.txt
+python test/test.py --robot_ip=ROBOT_IP
+```

cfg/compliance_param.cfg

@@ -11,19 +11,19 @@ gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 1
 gen.add("rotational_damping", double_t, 0, "Cartesian rotational damping", 7, 0, 30)
 gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0.2, 0, 100)
 gen.add("joint1_nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 100, 0, 100)
-gen.add("translational_clip_neg_x", double_t, 0, "Value to clip error to in realtime control loop", 0.05, 0, 0.1)
-gen.add("translational_clip_neg_y", double_t, 0, "Value to clip error to in realtime control loop", 0.002, 0, 0.1)
-gen.add("translational_clip_neg_z", double_t, 0, "Value to clip error to in realtime control loop", 0.002, 0, 0.1)
-gen.add("translational_clip_x", double_t, 0, "Value to clip error to in realtime control loop", 0.003, 0, 0.1)
-gen.add("translational_clip_y", double_t, 0, "Value to clip error to in realtime control loop", 0.002, 0, 0.1)
-gen.add("translational_clip_z", double_t, 0, "Value to clip error to in realtime control loop", 0.0015, 0, 0.1)
-gen.add("rotational_clip_neg_x", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
-gen.add("rotational_clip_neg_y", double_t, 0, "Value to clip error to in realtime control loop", 0.005, 0, 0.1)
-gen.add("rotational_clip_neg_z", double_t, 0, "Value to clip error to in realtime control loop", 0.005, 0, 0.1)
-gen.add("rotational_clip_x", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
-gen.add("rotational_clip_y", double_t, 0, "Value to clip error to in realtime control loop", 0.005, 0, 0.1)
-gen.add("rotational_clip_z", double_t, 0, "Value to clip error to in realtime control loop", 0.005, 0, 0.1)
-gen.add("translational_Ki", double_t, 0, "Cartesian translational intergral gain", 30, 0, 100)
+gen.add("translational_clip_neg_x", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
+gen.add("translational_clip_neg_y", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
+gen.add("translational_clip_neg_z", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
+gen.add("translational_clip_x", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
+gen.add("translational_clip_y", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
+gen.add("translational_clip_z", double_t, 0, "Value to clip error to in realtime control loop", 0.01, 0, 0.1)
+gen.add("rotational_clip_neg_x", double_t, 0, "Value to clip error to in realtime control loop", 0.05, 0, 0.1)
+gen.add("rotational_clip_neg_y", double_t, 0, "Value to clip error to in realtime control loop", 0.05, 0, 0.1)
+gen.add("rotational_clip_neg_z", double_t, 0, "Value to clip error to in realtime control loop", 0.05, 0, 0.1)
+gen.add("rotational_clip_x", double_t, 0, "Value to clip error to in realtime control loop", 0.05, 0, 0.1)
+gen.add("rotational_clip_y", double_t, 0, "Value to clip error to in realtime control loop", 0.05, 0, 0.1)
+gen.add("rotational_clip_z", double_t, 0, "Value to clip error to in realtime control loop", 0.05, 0, 0.1)
+gen.add("translational_Ki", double_t, 0, "Cartesian translational intergral gain", 0, 0, 100)
 gen.add("rotational_Ki", double_t, 0, "Cartesian rotational intergral gain", 0, 0, 100)
 
 exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param"))

include/serl_franka_controllers/cartesian_impedance_controller.h

@@ -20,7 +20,7 @@
 #include <franka_hw/franka_model_interface.h>
 #include <franka_hw/franka_state_interface.h>
 #include <realtime_tools/realtime_publisher.h>
-#include <franka_msgs/ZeroJacobian.h>
+#include <serl_franka_controllers/ZeroJacobian.h>
 #include <franka_msgs/FrankaDebug.h>
 
 namespace serl_franka_controllers {
@@ -93,9 +93,8 @@ class CartesianImpedanceController : public controller_interface::MultiInterface
   void complianceParamCallback(serl_franka_controllers::compliance_paramConfig& config,
                                uint32_t level);
   void publishZeroJacobian(const ros::Time& time);
-  realtime_tools::RealtimePublisher<franka_msgs::ZeroJacobian> publisher_franka_jacobian_;
+  realtime_tools::RealtimePublisher<serl_franka_controllers::ZeroJacobian> publisher_franka_jacobian_;
   void publishDebug(const ros::Time& time);
-  realtime_tools::RealtimePublisher<franka_msgs::FrankaDebug> publisher_franka_debug_;
   // Equilibrium pose subscriber
   ros::Subscriber sub_equilibrium_pose_;
   void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);

msg/ZeroJacobian.msg

@@ -0,0 +1 @@
+float64[42] zero_jacobian

package.xml

@@ -1,15 +1,14 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>serl_franka_controllers</name>
-  <version>0.8.0</version>
-  <description>serl_franka_controllers provides example code for controlling Franka Emika research robots with ros_control</description>
-  <maintainer email="[email protected]">Franka Emika GmbH</maintainer>
+  <version>0.1.0</version>
+  <description>serl_franka_controllers provides a compliant yet accurate Cartesian impedence controller for controlling Franka Emika research robots that can be used in online reinforcement learning applications </description>
+  <maintainer email="[email protected]">Charles Xu</maintainer>
   <license>Apache 2.0</license>
 
-  <url type="website">http://wiki.ros.org/franka_example_controllers</url>
-  <url type="repository">https://github.com/frankaemika/franka_ros</url>
-  <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
-  <author>Franka Emika GmbH</author>
+  <url type="website">https://serl-robot.github.io/</url>
+  <url type="repository">https://github.com/rail-berkeley/serl_franka_controller/</url>
+  <author>Jianlan Luo, Charles Xu</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 

requirements.txt

@@ -0,0 +1,5 @@
+rospkg
+scipy
+requests
+absl-py
+defusedxml

src/cartesian_impedance_controller.cpp

@@ -1,5 +1,3 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
 #include <serl_franka_controllers/cartesian_impedance_controller.h>
 
 #include <cmath>
@@ -20,7 +18,6 @@ bool CartesianImpedanceController::init(hardware_interface::RobotHW* robot_hw,
   std::vector<double> cartesian_stiffness_vector;
   std::vector<double> cartesian_damping_vector;
   publisher_franka_jacobian_.init(node_handle, "franka_jacobian", 1);
-  publisher_franka_debug_.init(node_handle, "franka_debug", 1);
 
   sub_equilibrium_pose_ = node_handle.subscribe(
       "equilibrium_pose", 20, &CartesianImpedanceController::equilibriumPoseCallback, this,
@@ -136,6 +133,7 @@ void CartesianImpedanceController::update(const ros::Time& time,
   std::array<double, 7> coriolis_array = model_handle_->getCoriolis();
   jacobian_array =
       model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+  publishZeroJacobian(time);
   Eigen::Map<Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
   Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
   Eigen::Map<Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
@@ -153,7 +151,6 @@ void CartesianImpedanceController::update(const ros::Time& time,
     error(i) = std::min(std::max(error(i), translational_clip_min(i)), translational_clip_max(i));
   }
 
-
   // orientation error
   if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) {
     orientation.coeffs() << -orientation.coeffs();

src/joint_position_controller.cpp

@@ -1,5 +1,3 @@
-// Copyright (c) 2017 Franka Emika GmbH
-// Use of this source code is governed by the Apache-2.0 license, see LICENSE
 #include <serl_franka_controllers/joint_position_controller.h>
 
 #include <cmath>

test/test.py

@@ -0,0 +1,92 @@
+import sys
+import rospy
+import numpy as np
+import geometry_msgs.msg as geom_msg
+import time
+import subprocess
+from dynamic_reconfigure.client import Client
+from absl import app, flags, logging
+from scipy.spatial.transform import Rotation as R
+import os
+
+FLAGS = flags.FLAGS
+flags.DEFINE_string("robot_ip", None, "IP address of the robot.", required=True)
+flags.DEFINE_string("load_gripper", 'false', "Whether or not to load the gripper.")
+
+def main(_):
+    try:
+
+        input("\033[33mPress enter to start roscore and the impedence controller.\033[0m")
+        try:
+            roscore = subprocess.Popen('roscore')
+            time.sleep(1)
+        except:
+            pass
+
+        impedence_controller = subprocess.Popen(['roslaunch', 'serl_franka_controllers', 'impedence.launch', 
+                                                f'robot_ip:={FLAGS.robot_ip}', f'load_gripper:={FLAGS.load_gripper}'],
+                                                stdout=subprocess.PIPE)
+
+        eepub = rospy.Publisher('/cartesian_impedance_controller/equilibrium_pose', geom_msg.PoseStamped ,queue_size=10)
+        rospy.init_node('franka_control_api' )
+        client = Client("/cartesian_impedance_controllerdynamic_reconfigure_compliance_param_node")
+
+
+        # Reset the arm
+        msg = geom_msg.PoseStamped()
+        msg.header.frame_id = "0"
+        msg.header.stamp = rospy.Time.now()
+        msg.pose.position = geom_msg.Point(0.5, 0, 0.2)
+        quat = R.from_euler('xyz', [np.pi, 0, np.pi/2]).as_quat()
+        msg.pose.orientation = geom_msg.Quaternion(quat[0], quat[1], quat[2], quat[3])
+        input("\033[33m\nObserve the surroundings. Press enter to move the robot to the initial position.\033[0m")
+        eepub.publish(msg)
+        time.sleep(1)
+
+
+        time.sleep(1)
+        # Setting the reference limiting values through ros dynamic reconfigure
+        for direction in ['x', 'y', 'z', 'neg_x', 'neg_y', 'neg_z']:
+            client.update_configuration({"translational_clip_" + direction: 0.005})
+            client.update_configuration({"rotational_clip_"+ direction: 0.04})
+        time.sleep(1)
+        print("\nNew reference limiting values has been set")
+
+        time.sleep(1)
+        input("\033[33mPress enter to move the robot up with the reference limiting engaged. Notice that the arm motion should be slower this time because the maximum force is effectively limited. \033[0m")
+        for i in range(10):
+            msg = geom_msg.PoseStamped()
+            msg.header.frame_id = "0"
+            msg.header.stamp = rospy.Time.now()
+            msg.pose.position = geom_msg.Point(0.5, 0, 0.2+i*0.02)
+            quat = R.from_euler('xyz', [np.pi, 0, np.pi/2]).as_quat()
+            msg.pose.orientation = geom_msg.Quaternion(quat[0], quat[1], quat[2], quat[3])
+            eepub.publish(msg)
+            time.sleep(0.2)
+        time.sleep(1)
+
+        time.sleep(1)
+        input("\033[33m\nPress enter to reset the robot arm back to the initial pose. \033[0m")
+        for i in range(10):
+            msg = geom_msg.PoseStamped()
+            msg.header.frame_id = "0"
+            msg.header.stamp = rospy.Time.now()
+            msg.pose.position = geom_msg.Point(0.5, 0, 0.4-i*0.02)
+            quat = R.from_euler('xyz', [np.pi, 0, np.pi/2]).as_quat()
+            msg.pose.orientation = geom_msg.Quaternion(quat[0], quat[1], quat[2], quat[3])
+            eepub.publish(msg)
+            time.sleep(0.1)
+
+
+        input("\033[33m\n \nPress enter to exit the test and stop the controller.\033[0m")
+        impedence_controller.terminate()
+        roscore.terminate()
+        sys.exit()
+    except:
+        rospy.logerr("Error occured. Terminating the controller.")
+        impedence_controller.terminate()
+        roscore.terminate()
+        sys.exit()
+
+if __name__ == "__main__":
+    app.run(main)