diff --git a/CMakeLists.txt b/CMakeLists.txt index d0d3843..98a5b26 100644 --- a/CMakeLists.txt +++ b/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( diff --git a/README.md b/README.md index 0a73fc6..8989c86 100644 --- a/README.md +++ b/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 git@github.com: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:= load_gripper:= +``` +Replace 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:= load_gripper:= +``` +Here, you also need to replace 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 +``` \ No newline at end of file diff --git a/cfg/compliance_param.cfg b/cfg/compliance_param.cfg index 0c52c60..5e9df89 100755 --- a/cfg/compliance_param.cfg +++ b/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")) diff --git a/include/serl_franka_controllers/cartesian_impedance_controller.h b/include/serl_franka_controllers/cartesian_impedance_controller.h index db83e8d..ef24f31 100644 --- a/include/serl_franka_controllers/cartesian_impedance_controller.h +++ b/include/serl_franka_controllers/cartesian_impedance_controller.h @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include 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 publisher_franka_jacobian_; + realtime_tools::RealtimePublisher publisher_franka_jacobian_; void publishDebug(const ros::Time& time); - realtime_tools::RealtimePublisher publisher_franka_debug_; // Equilibrium pose subscriber ros::Subscriber sub_equilibrium_pose_; void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); diff --git a/msg/ZeroJacobian.msg b/msg/ZeroJacobian.msg new file mode 100644 index 0000000..e34c1d7 --- /dev/null +++ b/msg/ZeroJacobian.msg @@ -0,0 +1 @@ +float64[42] zero_jacobian diff --git a/package.xml b/package.xml index 7ae1da0..d2d66d7 100644 --- a/package.xml +++ b/package.xml @@ -1,15 +1,14 @@ serl_franka_controllers - 0.8.0 - serl_franka_controllers provides example code for controlling Franka Emika research robots with ros_control - Franka Emika GmbH + 0.1.0 + 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 + Charles Xu Apache 2.0 - http://wiki.ros.org/franka_example_controllers - https://github.com/frankaemika/franka_ros - https://github.com/frankaemika/franka_ros/issues - Franka Emika GmbH + https://serl-robot.github.io/ + https://github.com/rail-berkeley/serl_franka_controller/ + Jianlan Luo, Charles Xu catkin diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..74d5606 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,5 @@ +rospkg +scipy +requests +absl-py +defusedxml \ No newline at end of file diff --git a/src/cartesian_impedance_controller.cpp b/src/cartesian_impedance_controller.cpp index 346cc33..d6bec94 100644 --- a/src/cartesian_impedance_controller.cpp +++ b/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 #include @@ -20,7 +18,6 @@ bool CartesianImpedanceController::init(hardware_interface::RobotHW* robot_hw, std::vector cartesian_stiffness_vector; std::vector 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 coriolis_array = model_handle_->getCoriolis(); jacobian_array = model_handle_->getZeroJacobian(franka::Frame::kEndEffector); + publishZeroJacobian(time); Eigen::Map> coriolis(coriolis_array.data()); Eigen::Map> jacobian(jacobian_array.data()); Eigen::Map> 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(); diff --git a/src/joint_position_controller.cpp b/src/joint_position_controller.cpp index 7aedbaa..90ee18a 100644 --- a/src/joint_position_controller.cpp +++ b/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 #include diff --git a/test/test.py b/test/test.py new file mode 100644 index 0000000..8564b38 --- /dev/null +++ b/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)