├── .gitignore ├── CMakeLists.txt ├── README.md ├── cfg ├── compliance_joint_param.cfg ├── compliance_param.cfg └── desired_mass_param.cfg ├── config ├── franka_control_node.yaml ├── franka_human_friendly_controllers.yaml └── latest_config.json ├── franka_human_friendly_controllers_plugin.xml ├── include └── franka_human_friendly_controllers │ ├── cartesian_variable_impedance_controller.h │ ├── cartesian_variable_impedance_external_model_controller.h │ ├── fk_panda.c │ ├── franka_model.h │ ├── joint_variable_impedance_controller.h │ └── pseudo_inversion.h ├── launch ├── cartesian_variable_impedance_controller.launch ├── franka_control.launch ├── joint_variable_impedance_controller.launch ├── robot.rviz └── rviz │ └── franka_description_with_marker.rviz ├── mainpage.dox ├── msg ├── JointTorqueComparison.msg └── JointTorqueState.msg ├── package.xml ├── python ├── .gitignore ├── LfD │ ├── .vscode │ │ └── settings.json │ ├── Learning_from_demonstration.py │ ├── Learning_from_demonstration_joint.py │ ├── __pycache__ │ │ ├── Learning_from_demonstration.cpython-38.pyc │ │ ├── panda.cpython-38.pyc │ │ └── pose_transform_functions.cpython-38.pyc │ ├── data │ │ └── last.npz │ ├── franka_gripper │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ └── __init__.cpython-38.pyc │ │ └── msg │ │ │ ├── _GraspAction.py │ │ │ ├── _GraspActionFeedback.py │ │ │ ├── _GraspActionGoal.py │ │ │ ├── _GraspActionResult.py │ │ │ ├── _GraspEpsilon.py │ │ │ ├── _GraspFeedback.py │ │ │ ├── _GraspGoal.py │ │ │ ├── _GraspResult.py │ │ │ ├── _HomingAction.py │ │ │ ├── _HomingActionFeedback.py │ │ │ ├── _HomingActionGoal.py │ │ │ ├── _HomingActionResult.py │ │ │ ├── _HomingFeedback.py │ │ │ ├── _HomingGoal.py │ │ │ ├── _HomingResult.py │ │ │ ├── _MoveAction.py │ │ │ ├── _MoveActionFeedback.py │ │ │ ├── _MoveActionGoal.py │ │ │ ├── _MoveActionResult.py │ │ │ ├── _MoveFeedback.py │ │ │ ├── _MoveGoal.py │ │ │ ├── _MoveResult.py │ │ │ ├── _StopAction.py │ │ │ ├── _StopActionFeedback.py │ │ │ ├── _StopActionGoal.py │ │ │ ├── _StopActionResult.py │ │ │ ├── _StopFeedback.py │ │ │ ├── _StopGoal.py │ │ │ ├── _StopResult.py │ │ │ ├── __init__.py │ │ │ └── __pycache__ │ │ │ ├── _GraspAction.cpython-38.pyc │ │ │ ├── _GraspActionFeedback.cpython-38.pyc │ │ │ ├── _GraspActionGoal.cpython-38.pyc │ │ │ ├── _GraspActionResult.cpython-38.pyc │ │ │ ├── _GraspEpsilon.cpython-38.pyc │ │ │ ├── _GraspFeedback.cpython-38.pyc │ │ │ ├── _GraspGoal.cpython-38.pyc │ │ │ ├── _GraspResult.cpython-38.pyc │ │ │ ├── _HomingAction.cpython-38.pyc │ │ │ ├── _HomingActionFeedback.cpython-38.pyc │ │ │ ├── _HomingActionGoal.cpython-38.pyc │ │ │ ├── _HomingActionResult.cpython-38.pyc │ │ │ ├── _HomingFeedback.cpython-38.pyc │ │ │ ├── _HomingGoal.cpython-38.pyc │ │ │ ├── _HomingResult.cpython-38.pyc │ │ │ ├── _MoveAction.cpython-38.pyc │ │ │ ├── _MoveActionFeedback.cpython-38.pyc │ │ │ ├── _MoveActionGoal.cpython-38.pyc │ │ │ ├── _MoveActionResult.cpython-38.pyc │ │ │ ├── _MoveFeedback.cpython-38.pyc │ │ │ ├── _MoveGoal.cpython-38.pyc │ │ │ ├── _MoveResult.cpython-38.pyc │ │ │ ├── _StopAction.cpython-38.pyc │ │ │ ├── _StopActionFeedback.cpython-38.pyc │ │ │ ├── _StopActionGoal.cpython-38.pyc │ │ │ ├── _StopActionResult.cpython-38.pyc │ │ │ ├── _StopFeedback.cpython-38.pyc │ │ │ ├── _StopGoal.cpython-38.pyc │ │ │ ├── _StopResult.cpython-38.pyc │ │ │ └── __init__.cpython-38.pyc │ ├── franka_msgs │ │ ├── __init__.py │ │ ├── msg │ │ │ ├── _ErrorRecoveryAction.py │ │ │ ├── _ErrorRecoveryActionFeedback.py │ │ │ ├── _ErrorRecoveryActionGoal.py │ │ │ ├── _ErrorRecoveryActionResult.py │ │ │ ├── _ErrorRecoveryFeedback.py │ │ │ ├── _ErrorRecoveryGoal.py │ │ │ ├── _ErrorRecoveryResult.py │ │ │ ├── _Errors.py │ │ │ ├── _FrankaState.py │ │ │ └── __init__.py │ │ └── srv │ │ │ ├── _SetCartesianImpedance.py │ │ │ ├── _SetEEFrame.py │ │ │ ├── _SetForceTorqueCollisionBehavior.py │ │ │ ├── _SetFullCollisionBehavior.py │ │ │ ├── _SetJointImpedance.py │ │ │ ├── _SetKFrame.py │ │ │ ├── _SetLoad.py │ │ │ └── __init__.py │ ├── main_lfd.py │ ├── panda.py │ ├── pose_transform_functions.py │ └── teleoperation.py └── Teleop_Keyboard │ ├── README.md │ ├── __pycache__ │ ├── panda.cpython-38.pyc │ ├── pose_transform_functions.cpython-38.pyc │ ├── teleop_keyboard.cpython-38.pyc │ └── telep_keyboard.cpython-38.pyc │ ├── main_teleop.py │ ├── panda.py │ ├── pose_transform_functions.py │ └── teleop_keyboard.py ├── rosdoc.yaml ├── setup_gazebo.py ├── src ├── cartesian_variable_impedance_controller.cpp ├── cartesian_variable_impedance_external_model_controller.cpp └── joint_variable_impedance_controller.cpp └── urdf └── panda_calibrated.urdf /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | project(franka_human_friendly_controllers) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(CMAKE_CXX_STANDARD 14) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | controller_interface 10 | dynamic_reconfigure 11 | eigen_conversions 12 | franka_hw 13 | franka_gripper 14 | geometry_msgs 15 | hardware_interface 16 | tf 17 | tf_conversions 18 | message_generation 19 | pluginlib 20 | realtime_tools 21 | roscpp 22 | rospy 23 | urdf 24 | ) 25 | 26 | find_package(Eigen3 REQUIRED) 27 | find_package(Franka 0.7.0 REQUIRED) 28 | find_package(pinocchio REQUIRED) 29 | 30 | add_message_files(FILES 31 | JointTorqueComparison.msg 32 | ) 33 | 34 | generate_messages() 35 | 36 | generate_dynamic_reconfigure_options( 37 | cfg/compliance_param.cfg 38 | cfg/desired_mass_param.cfg 39 | cfg/compliance_joint_param.cfg 40 | ) 41 | 42 | catkin_package( 43 | INCLUDE_DIRS include 44 | LIBRARIES franka_human_friendly_controllers 45 | CATKIN_DEPENDS 46 | controller_interface 47 | dynamic_reconfigure 48 | eigen_conversions 49 | franka_hw 50 | franka_gripper 51 | geometry_msgs 52 | hardware_interface 53 | tf 54 | tf_conversions 55 | message_runtime 56 | pluginlib 57 | realtime_tools 58 | roscpp 59 | DEPENDS Franka 60 | ) 61 | 62 | add_library(franka_human_friendly_controllers 63 | src/cartesian_variable_impedance_controller.cpp 64 | src/joint_variable_impedance_controller.cpp 65 | src/cartesian_variable_impedance_external_model_controller.cpp 66 | ) 67 | 68 | add_dependencies(franka_human_friendly_controllers 69 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 70 | ${catkin_EXPORTED_TARGETS} 71 | ${PROJECT_NAME}_generate_messages_cpp 72 | ${PROJECT_NAME}_gencpp 73 | ${PROJECT_NAME}_gencfg 74 | ) 75 | 76 | target_link_libraries(franka_human_friendly_controllers PUBLIC 77 | ${Franka_LIBRARIES} 78 | ${catkin_LIBRARIES} 79 | pinocchio::pinocchio 80 | ) 81 | 82 | target_include_directories(franka_human_friendly_controllers SYSTEM PUBLIC 83 | ${Franka_INCLUDE_DIRS} 84 | ${EIGEN3_INCLUDE_DIRS} 85 | ${catkin_INCLUDE_DIRS} 86 | ${PINOCCHIO_INCLUDE_DIRS} 87 | ) 88 | target_include_directories(franka_human_friendly_controllers PUBLIC 89 | include 90 | ) 91 | 92 | 93 | 94 | ## Installation 95 | install(TARGETS franka_human_friendly_controllers 96 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 97 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 98 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 99 | ) 100 | install(DIRECTORY include/${PROJECT_NAME}/ 101 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 102 | ) 103 | install(DIRECTORY launch 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 105 | ) 106 | install(DIRECTORY config 107 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 108 | ) 109 | install(FILES franka_human_friendly_controllers_plugin.xml 110 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 111 | ) 112 | 113 | ## Tools 114 | include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL 115 | RESULT_VARIABLE CLANG_TOOLS 116 | ) 117 | if(CLANG_TOOLS) 118 | file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) 119 | file(GLOB_RECURSE HEADERS 120 | ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h 121 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h 122 | ) 123 | add_format_target(franka_human_friendly_controllers FILES ${SOURCES} ${HEADERS}) 124 | add_tidy_target(franka_human_friendly_controllers 125 | FILES ${SOURCES} 126 | DEPENDS franka_human_friendly_controllers 127 | ) 128 | endif() 129 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Franka Human Friendly Controllers 2 | 3 | ### What is this repository useful for? 4 | This code is build on top of [Franka Cartesian Control](https://github.com/frankaemika/franka_ros/blob/develop/franka_example_controllers/src/cartesian_impedance_example_controller.cpp). 5 | This code has some other desirable features for human-robot interaction. 6 | 7 | Features: 8 | 9 | - **Joint limit repulsion**: The controller has a joint limit repulsion feature that allows the robot to avoid joint limits. This is useful when the robot is controlled by a human operator. 10 | 11 | - **Anisotropic stiffness**: The controller allows the user to set different stiffness values for each axis, both linear and angular. 12 | 13 | - **Publish the desired attractor**: the toipc /equilibrium_pose is the topic where you can publish the deried pose of the robot. The message type is geometry_msgs/PoseStamped. 14 | 15 | - **Safety feature**: The attractor distance are clipped inside the controller. You can set the clipping value in the rqt_reconfigure. This enusre that the robot will not move too fast when a too far attractor is published. 16 | 17 | - **Haptic feedback**: You can publish the haptic feedback in the topic /haptic_feedback. The message type is std_msgs/Float32. The value that is published is the time that the last joint will vibrate. For example 18 | ` 19 | rostopic pub /haptic_feedback std_msgs/Float32 "data: 0.5" 20 | ` 21 | will make the last joint vibrate for 0.5 seconds. 22 | - **Read external forces**: The controller reads the external forces and torques from the robot and publish them in the topic /force_torque_ext. The message type is geometry_msgs/WrenchStamped. This value is already filtered as it is also compensating for the gravity, Coriolis and friction forces. 23 | 24 | ### Installation 25 | - Install Franka ROS from [here](https://frankaemika.github.io/docs/installation_linux.html) 26 | 27 | - Go the the catkin_ws and clone franka_ros from source 28 | ``` 29 | cd catkin_ws/src 30 | git clone https://github.com/frankaemika/franka_ros.git 31 | ``` 32 | - Clone the human-friendly controller and the install: 33 | 34 | ``` 35 | git clone https://github.com/franzesegiovanni/franka_human_friendly_controllers.git 36 | cd .. 37 | catkin build -DMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=~/libfranka/build 38 | ``` 39 | To check the compatibility of the franka ros and the libfranka, please give a look [here](https://frankaemika.github.io/docs/compatibility.html). 40 | 41 | To run the controller: 42 | - Switch on your Panda robot (make sure the gripper is initialized correctly), unlock its joints (and activate the FCI). 43 | - Open a terminal and source the evnironment: 44 | ``` 45 | source devel/setup.bash 46 | ``` 47 | If you have a Panda, run the controller as: 48 | ``` 49 | roslaunch franka_human_friendly_controllers cartesian_variable_impedance_controller.launch robot_ip:=ROBOT_IP load_gripper:=True arm_id:=panda 50 | ``` 51 | 52 | If you have an FR3 then run 53 | 54 | ``` 55 | roslaunch franka_human_friendly_controllers cartesian_variable_impedance_controller.launch robot_ip:=ROBOT_IP load_gripper:=True arm_id:=fr3 56 | ``` 57 | 58 | selecting the right arm_id is important for set correctly the joint limit repulsion. 59 | 60 | ## Run with calibrated kinematic model 61 | This repo allows to use an external model for the urdf that is stored in the folder urdf with the name 'panda_calibrated.urdf'. This calibrated model is generated in using this [other repository]( https://github.com/platonics-delft/kinematics_calibration). You can copy the generated file from that repo in the urdf folder of this repo. A calibrated kinematic model will ensure that the robot correctly predicts the real position of the end effector. This is particularly important when doing fine manipulation tasks. The nominal model that is used by franka is not valid for every robot and the small manufacturing imperfaction can give a robot that does not track correctly its Cartesian position. 62 | 63 | To run the calibrated Cartesian impedance controller, you can doing so by simply launching: 64 | ``` 65 | roslaunch franka_human_friendly_controllers cartesian_variable_impedance_controller.launch robot_ip:=ROBOT_IP use_external_model:=True 66 | ``` 67 | 68 | 69 | # Run this in Gazebo simulation 70 | After building the catkin_ws and sourced the environment, you can run the following python code to set up the files such that to be able to run the code in simulation. 71 | ``` python3 setup_gazebo.py ```. 72 | 73 | Compile again: 74 | ``` catkin build ``` 75 | 76 | To lunch the cartesian impedance controller in simulation: 77 | 78 | ``` roslaunch franka_gazebo panda.launch x:=-0.5 world:=$(rospack find franka_gazebo)/world/stone.sdf controller:=cartesian_variable_impedance_controller rviz:=true ``` 79 | 80 | To kill gazebo run: 81 | ``` killall -9 gazebo & killall -9 gzserver & killall -9 gzclient & killall -9 rosmaster & killall -9 roscore killall -9 rviz``` 82 | 83 | 84 | # Learning from Demonstration in Python 85 | 86 | In a terminal with the workspace sourced, open vscode, be sure that you have the extension Jupyter installed and then run one by one the cells in python/LfD/main_lfd.py. You can record kinesthetic demonstration, save them, load them and then play them back. Every time you execute a skill, the robot goes back to the starting position of the demonstration. 87 | 88 | 89 | # More cool stuff 90 | - Do you want to control two arms at the same time using similar controllers? Check out [franka_bimanual_controllers](https://github.com/franzesegiovanni/franka_bimanual_controllers). 91 | 92 | - Do you want to read the franka buttons in ROS? Check out [franka_buttons](https://github.com/franzesegiovanni/franka_buttons). 93 | 94 | - Do you want to do learning from demonstration? Check out [ILoSA](https://github.com/franzesegiovanni/ILoSA), [SIMPLe](https://github.com/franzesegiovanni/SIMPLe) and [franka_learning_from_demonstration](https://github.com/platonics-delft/franka_learning_from_demonstrations). 95 | 96 | # Cite us! 97 | If you found this repo useful for your research, please cite it as: 98 | 99 | ``` 100 | @inproceedings{franzese2021ilosa, 101 | title={ILoSA: Interactive learning of stiffness and attractors}, 102 | author={Franzese, Giovanni and M{\'e}sz{\'a}ros, Anna and Peternel, Luka and Kober, Jens}, 103 | booktitle={2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 104 | pages={7778--7785}, 105 | year={2021}, 106 | organization={IEEE} 107 | } 108 | ``` 109 | # Acknowledgements 110 | 111 | This work has received funding from the European Union’s ERC starting grant TERI "Teaching Robots Interactively", number 804907. -------------------------------------------------------------------------------- /cfg/compliance_joint_param.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "franka_human_friendly_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("joint_1", double_t, 0, "Stiffness_joint_1", 600, 0, 600) 9 | gen.add("joint_2", double_t, 0, "Stiffness_joint_2", 600, 0, 600) 10 | gen.add("joint_3", double_t, 0, "Stiffness_joint_3", 600, 0, 600) 11 | gen.add("joint_4", double_t, 0, "Stiffness_joint_4", 600, 0, 600) 12 | gen.add("joint_5", double_t, 0, "Stiffness_joint_5", 250, 0, 250) 13 | gen.add("joint_6", double_t, 0, "Stiffness_joint_6", 150, 0, 150) 14 | gen.add("joint_7", double_t, 0, "Stiffness_joint_7", 50, 0, 50) 15 | gen.add("damping_ratio", double_t, 0, "Damping_ratio", 1, 0, 2) 16 | 17 | exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_joint_param")) 18 | -------------------------------------------------------------------------------- /cfg/compliance_param.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "franka_human_friendly_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("translational_stiffness_X", double_t, 0, "Cartesian translational stiffness", 400, 0, 4000) 9 | gen.add("translational_stiffness_Y", double_t, 0, "Cartesian translational stiffness", 400, 0, 4000) 10 | gen.add("translational_stiffness_Z", double_t, 0, "Cartesian translational stiffness", 400, 0, 4000) 11 | gen.add("rotational_stiffness_X", double_t, 0, "Cartesian rotational stiffness", 30, 0, 40) 12 | gen.add("rotational_stiffness_Y", double_t, 0, "Cartesian rotational stiffness", 30, 0, 40) 13 | gen.add("rotational_stiffness_Z", double_t, 0, "Cartesian rotational stiffness", 30, 0, 40) 14 | 15 | gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100) 16 | 17 | gen.add("joint_default_damping", double_t, 0, "Default damping value", 0, 0, 2) 18 | 19 | gen.add("max_delta_lin", double_t, 0, "Maximum delta linear displacement [m]", 0.05, 0.01, 0.2) 20 | 21 | gen.add("max_delta_ori", double_t, 0, "Maximum delta angular displacement [rad]", 0.15, 0.01, 0.5) 22 | 23 | exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param")) 24 | -------------------------------------------------------------------------------- /cfg/desired_mass_param.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "franka_human_friendly_controllers" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0, 0.0, 2.0) 9 | gen.add("k_p", double_t, 0, "force P gain", 0.0, 0.0, 2.0) 10 | gen.add("k_i", double_t, 0, "force I gain", 0.0, 0.0, 2.0) 11 | 12 | exit(gen.generate(PACKAGE, "dynamic_mass", "desired_mass_param")) 13 | -------------------------------------------------------------------------------- /config/franka_control_node.yaml: -------------------------------------------------------------------------------- 1 | arm_id: $(arg arm_id) 2 | joint_names: 3 | - $(arg arm_id)_joint1 4 | - $(arg arm_id)_joint2 5 | - $(arg arm_id)_joint3 6 | - $(arg arm_id)_joint4 7 | - $(arg arm_id)_joint5 8 | - $(arg arm_id)_joint6 9 | - $(arg arm_id)_joint7 10 | 11 | # Configure the threshold angle for printing joint limit warnings. 12 | joint_limit_warning_threshold: 0.1 # [rad] 13 | # Activate rate limiter? [true|false] 14 | rate_limiting: true 15 | # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate. 16 | cutoff_frequency: 100 17 | # Internal controller for motion generators [joint_impedance|cartesian_impedance] 18 | internal_controller: joint_impedance 19 | # Used to decide whether to enforce realtime mode [enforce|ignore] 20 | realtime_config: enforce 21 | # Configure the initial defaults for the collision behavior reflexes. 22 | collision_config: 23 | lower_torque_thresholds_acceleration: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [Nm] 24 | upper_torque_thresholds_acceleration: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [Nm] 25 | lower_torque_thresholds_nominal: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [Nm] 26 | upper_torque_thresholds_nominal: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [Nm] 27 | lower_force_thresholds_acceleration: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [N, N, N, Nm, Nm, Nm] 28 | upper_force_thresholds_acceleration: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [N, N, N, Nm, Nm, Nm] 29 | lower_force_thresholds_nominal: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [N, N, N, Nm, Nm, Nm] 30 | upper_force_thresholds_nominal: [100.0, 100.0, 100.0, 100.0, 100.0, 100.0] # [N, N, N, Nm, Nm, Nm] 31 | -------------------------------------------------------------------------------- /config/franka_human_friendly_controllers.yaml: -------------------------------------------------------------------------------- 1 | cartesian_variable_impedance_controller: 2 | type: franka_human_friendly_controllers/CartesianVariableImpedanceController 3 | arm_id: panda 4 | joint_names: 5 | - panda_joint1 6 | - panda_joint2 7 | - panda_joint3 8 | - panda_joint4 9 | - panda_joint5 10 | - panda_joint6 11 | - panda_joint7 12 | joint_variable_impedance_controller: 13 | type: franka_human_friendly_controllers/JointVariableImpedanceController 14 | arm_id: panda 15 | joint_names: 16 | - panda_joint1 17 | - panda_joint2 18 | - panda_joint3 19 | - panda_joint4 20 | - panda_joint5 21 | - panda_joint6 22 | - panda_joint7 23 | cartesian_variable_impedance_external_model_controller: 24 | type: franka_human_friendly_controllers/CartesianVariableImpedanceExternalModelController 25 | arm_id: panda 26 | joint_names: 27 | - panda_joint1 28 | - panda_joint2 29 | - panda_joint3 30 | - panda_joint4 31 | - panda_joint5 32 | - panda_joint6 33 | - panda_joint7 34 | -------------------------------------------------------------------------------- /config/latest_config.json: -------------------------------------------------------------------------------- 1 | {"mass":0.9,"centerOfMass":[-0.002,0,0.034],"transformation":[0.7071,-0.7071,0,0,0.7071,0.7071,0,0,0,0,1,0,0,0,0.1034,1],"inertia":[0.001,0,0,0,0.0025,0,0,0,0.0017],"collisionModel":{"pointA":[0.03535533905932738,0.03535533905932738,0.04,0.03535533905932738,0.03535533905932738,0.1,0,0,0],"pointB":[-0.03535533905932738,-0.03535533905932738,0.04,-0.03535533905932738,-0.03535533905932738,0.1,0,0,0],"radius":[0.04,0.02,0]}} -------------------------------------------------------------------------------- /franka_human_friendly_controllers_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A controller that renders a spring damper system in cartesian space. 5 | 6 | 7 | 8 | 9 | A controller that renders a spring damper system in joint space. 10 | 11 | 12 | 13 | 14 | A controller that renders a spring damper system in cartesian space using an external urdf file as a model for forward kinematics and jacobian computation. 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017 Franka Emika GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "std_msgs/MultiArrayLayout.h" 14 | #include "std_msgs/MultiArrayDimension.h" 15 | #include "std_msgs/Float32MultiArray.h" 16 | #include "std_msgs/Float32.h" 17 | #include "std_msgs/Bool.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace franka_human_friendly_controllers { 31 | 32 | class CartesianVariableImpedanceController : public controller_interface::MultiInterfaceController< 33 | franka_hw::FrankaModelInterface, 34 | hardware_interface::EffortJointInterface, 35 | franka_hw::FrankaStateInterface> { 36 | public: 37 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; 38 | void starting(const ros::Time&) override; 39 | void update(const ros::Time&, const ros::Duration& period) override; 40 | virtual void loadModel(); 41 | 42 | private: 43 | // Saturation 44 | Eigen::Matrix saturateTorqueRate( 45 | const Eigen::Matrix& tau_d_calculated, 46 | const Eigen::Matrix& tau_J_d); // NOLINT (readability-identifier-naming) 47 | 48 | double calculateTauJointLimit(double q_value, double threshold, double magnitude, double upper_bound, double lower_bound); 49 | virtual std::array get_jacobian(franka::RobotState robot_state); 50 | virtual double* get_fk(franka::RobotState robot_state); 51 | 52 | 53 | std::unique_ptr state_handle_; 54 | std::unique_ptr model_handle_; 55 | std::vector joint_handles_; 56 | 57 | double nullspace_stiffness_{0.0}; 58 | double nullspace_stiffness_target_{0.0}; 59 | double dt{0.001}; 60 | double time_start; 61 | int alpha; 62 | int filter_step{0}; 63 | int filter_step_; 64 | const double delta_tau_max_{1.0}; 65 | double delta_lim_lin; 66 | double delta_lim_ori; 67 | Eigen::Matrix cartesian_stiffness_; 68 | Eigen::Matrix cartesian_damping_; 69 | Eigen::Matrix q_d_nullspace_; 70 | Eigen::Matrix cartesian_stiffness_target_; 71 | Eigen::Matrix cartesian_damping_target_; 72 | double joint_default_damping_; 73 | double last_joint_default_damping_; 74 | Eigen::Matrix force_torque; 75 | Eigen::Matrix force_torque_old; 76 | Eigen::Matrix stiff_; 77 | Eigen::Vector3d position_d_; 78 | Eigen::Quaterniond orientation_d_; 79 | 80 | double count_vibration{10000.0}; 81 | double duration_vibration; 82 | bool vibrate= false; 83 | 84 | double joint_limits[7][2]; 85 | 86 | // Dynamic reconfigure 87 | std::unique_ptr> 88 | dynamic_server_compliance_param_; 89 | ros::NodeHandle dynamic_reconfigure_compliance_param_node_; 90 | void complianceParamCallback(franka_human_friendly_controllers::compliance_paramConfig& config, 91 | uint32_t level); 92 | 93 | // Equilibrium pose subscriber 94 | ros::Subscriber sub_equilibrium_pose_; 95 | void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); 96 | 97 | // Configuration pose subscriber 98 | ros::Subscriber sub_equilibrium_config_; 99 | void equilibriumConfigurationCallback( const std_msgs::Float32MultiArray::ConstPtr& joint); 100 | 101 | // Make the robot to vibrate in the end effector 102 | ros::Subscriber sub_vibration_; 103 | void equilibriumVibrationCallback(const std_msgs::Float32::ConstPtr& vibration_); 104 | 105 | ros::Publisher pub_stiff_update_; 106 | 107 | ros::Publisher pub_cartesian_pose_; 108 | ros::Publisher pub_force_torque_; 109 | 110 | }; 111 | 112 | 113 | } // namespace franka_human_friendly_controllers 114 | -------------------------------------------------------------------------------- /include/franka_human_friendly_controllers/cartesian_variable_impedance_external_model_controller.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017 Franka Emika GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace franka_human_friendly_controllers { 8 | 9 | class CartesianVariableImpedanceExternalModelController : public CartesianVariableImpedanceController { 10 | private: 11 | std::string urdf_path_; 12 | pinocchio::Model model_pin_; 13 | pinocchio::Data* data_pin_; 14 | std::string frame_name_; 15 | int frame_id_; 16 | 17 | public: 18 | double* get_fk(franka::RobotState robot_state) override; 19 | std::array get_jacobian(franka::RobotState robot_state) override; 20 | void loadModel() override; 21 | }; 22 | 23 | } // namespace franka_human_friendly_controllers 24 | -------------------------------------------------------------------------------- /include/franka_human_friendly_controllers/franka_model.h: -------------------------------------------------------------------------------- 1 | /* 2 | * franka_model.h 3 | * 4 | * Created on: 24 feb 2019 5 | * Authors: Oliva Alexander, Gaz Claudio, Cognetti Marco 6 | * 7 | * C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano, A. De Luca, 'Dynamic 8 | * Identification of the Franka Emika Panda Robot With Retrieval of Feasible 9 | * Parameters Using Penalty-Based Optimization'. IEEE RA-L, 2019. 10 | * 11 | */ 12 | 13 | 14 | #include 15 | 16 | #ifndef INCLUDE_FRANKA_MODEL_H_ 17 | #define INCLUDE_FRANKA_MODEL_H_ 18 | 19 | #define NUMBER_OF_JOINTS 7 20 | 21 | namespace Eigen { 22 | 23 | typedef Eigen::Matrix< double , NUMBER_OF_JOINTS , NUMBER_OF_JOINTS > Matrix7d; 24 | typedef Eigen::Matrix< double , NUMBER_OF_JOINTS , 1 > Vector7d; 25 | } 26 | 27 | using namespace std; 28 | 29 | /* 30 | * In order to speed up the code execution, we pre-compute the constant part of the 31 | * friction model of each joint i (second term of the right member of the equation). 32 | * 33 | * tau_f(i) = FI_1(i)/(1+exp(-FI_2(i)*(dq(i)+FI_3(i)))) -FI_1(i)/(1+exp(-FI_2(i)*FI_3(i))) 34 | * 35 | * For further information refer to our paper and relative Supplementary Material: 36 | * C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano, A. De Luca, 37 | * 'Dynamic Identification of the Franka Emika Panda Robot With Retrieval of 38 | * Feasible Parameters Using Penalty-Based Optimization'. IEEE RA-L, 2019. 39 | */ 40 | 41 | const double FI_11 = 0.54615; 42 | const double FI_12 = 0.87224; 43 | const double FI_13 = 0.64068; 44 | const double FI_14 = 1.2794; 45 | const double FI_15 = 0.83904; 46 | const double FI_16 = 0.30301; 47 | const double FI_17 = 0.56489; 48 | 49 | const double FI_21 = 5.1181; 50 | const double FI_22 = 9.0657; 51 | const double FI_23 = 10.136; 52 | const double FI_24 = 5.5903; 53 | const double FI_25 = 8.3469; 54 | const double FI_26 = 17.133; 55 | const double FI_27 = 10.336; 56 | 57 | const double FI_31 = 0.039533; 58 | const double FI_32 = 0.025882; 59 | const double FI_33 = -0.04607; 60 | const double FI_34 = 0.036194; 61 | const double FI_35 = 0.026226; 62 | const double FI_36 = -0.021047; 63 | const double FI_37 = 0.0035526; 64 | 65 | const double TAU_F_CONST_1 = FI_11/(1+exp(-FI_21*FI_31)); 66 | const double TAU_F_CONST_2 = FI_12/(1+exp(-FI_22*FI_32)); 67 | const double TAU_F_CONST_3 = FI_13/(1+exp(-FI_23*FI_33)); 68 | const double TAU_F_CONST_4 = FI_14/(1+exp(-FI_24*FI_34)); 69 | const double TAU_F_CONST_5 = FI_15/(1+exp(-FI_25*FI_35)); 70 | const double TAU_F_CONST_6 = FI_16/(1+exp(-FI_26*FI_36)); 71 | const double TAU_F_CONST_7 = FI_17/(1+exp(-FI_27*FI_37)); 72 | 73 | 74 | Eigen::Matrix7d MassMatrix(const Eigen::Vector7d &q); 75 | 76 | Eigen::Matrix7d CoriolisMatrix(const Eigen::Vector7d &q,const Eigen::Vector7d &dq); 77 | 78 | Eigen::Vector7d GravityVector(const Eigen::Vector7d &q); 79 | 80 | Eigen::Vector7d Friction(const Eigen::Vector7d &dq); 81 | 82 | 83 | #endif /* INCLUDE_FRANKA_MODEL_H_ */ 84 | -------------------------------------------------------------------------------- /include/franka_human_friendly_controllers/joint_variable_impedance_controller.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017 Franka Emika GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "std_msgs/MultiArrayLayout.h" 14 | #include "std_msgs/MultiArrayDimension.h" 15 | #include "std_msgs/Float32MultiArray.h" 16 | #include "sensor_msgs/JointState.h" 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | // #include 30 | 31 | 32 | namespace franka_human_friendly_controllers { 33 | 34 | class JointVariableImpedanceController : public controller_interface::MultiInterfaceController< 35 | franka_hw::FrankaModelInterface, 36 | hardware_interface::EffortJointInterface, 37 | franka_hw::FrankaStateInterface> { 38 | public: 39 | bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; 40 | void starting(const ros::Time&) override; 41 | void update(const ros::Time&, const ros::Duration& period) override; 42 | 43 | private: 44 | // Saturation 45 | Eigen::Matrix saturateTorqueRate( 46 | const Eigen::Matrix& tau_d_calculated, 47 | const Eigen::Matrix& tau_J_d); // NOLINT (readability-identifier-naming) 48 | std::unique_ptr state_handle_; 49 | std::unique_ptr model_handle_; 50 | std::vector joint_handles_; 51 | 52 | double dt{0.001}; 53 | int alpha; 54 | int filter_step{0}; 55 | int filter_step_; 56 | const double delta_tau_max_{1.0}; 57 | double damping_ratio{1}; 58 | Eigen::Matrix q_d_; 59 | Eigen::Matrix joint_stiffness_target_; 60 | Eigen::Matrix joint_damping_target_; 61 | 62 | // Dynamic reconfigure of stiffness 63 | std::unique_ptr> 64 | dynamic_server_compliance_joint_param_; 65 | 66 | ros::NodeHandle dynamic_reconfigure_compliance_joint_param_node_; 67 | void complianceJointParamCallback(franka_human_friendly_controllers::compliance_joint_paramConfig& config, 68 | uint32_t level); 69 | 70 | // Configuration joint configuration subscriber 71 | ros::Subscriber sub_equilibrium_config_; 72 | void equilibriumConfigurationCallback( const sensor_msgs::JointState::ConstPtr& joint); 73 | 74 | 75 | Eigen::Matrix force_torque; 76 | Eigen::Matrix force_torque_old; 77 | 78 | ros::Publisher pub_stiff_update_; 79 | ros::Publisher pub_cartesian_pose_; 80 | ros::Publisher pub_force_torque_; 81 | 82 | hardware_interface::PositionJointInterface *_position_joint_interface; 83 | std::vector _position_joint_handles; 84 | 85 | }; 86 | 87 | 88 | } // namespace franka_advanced_controllers 89 | -------------------------------------------------------------------------------- /include/franka_human_friendly_controllers/pseudo_inversion.h: -------------------------------------------------------------------------------- 1 | // Author: Enrico Corvaglia 2 | // https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_controllers/include/utils/pseudo_inversion.h 3 | // File provided under public domain 4 | // pseudo_inverse() computes the pseudo inverse of matrix M_ using SVD decomposition (can choose 5 | // between damped and not) 6 | // returns the pseudo inverted matrix M_pinv_ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | namespace franka_human_friendly_controllers { 15 | 16 | inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, bool damped = true) { 17 | double lambda_ = damped ? 0.2 : 0.0; 18 | 19 | Eigen::JacobiSVD svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV); 20 | Eigen::JacobiSVD::SingularValuesType sing_vals_ = svd.singularValues(); 21 | Eigen::MatrixXd S_ = M_; // copying the dimensions of M_, its content is not needed. 22 | S_.setZero(); 23 | 24 | for (int i = 0; i < sing_vals_.size(); i++) 25 | S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_); 26 | 27 | M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose()); 28 | } 29 | 30 | } // namespace franka_human_friendly_controllers 31 | -------------------------------------------------------------------------------- /launch/cartesian_variable_impedance_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /launch/franka_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | [franka_state_controller/joint_states, franka_gripper/joint_states] 32 | [franka_state_controller/joint_states] 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /launch/joint_variable_impedance_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 89 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 746 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | panda_hand: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | panda_leftfinger: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | panda_link0: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | panda_link1: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | panda_link2: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | panda_link3: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | panda_link4: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | panda_link5: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | panda_link6: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | panda_link7: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | Value: true 111 | panda_link8: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | panda_rightfinger: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | Name: RobotModel 121 | Robot Description: robot_description 122 | TF Prefix: "" 123 | Update Interval: 0 124 | Value: true 125 | Visual Enabled: true 126 | Enabled: true 127 | Global Options: 128 | Background Color: 48; 48; 48 129 | Fixed Frame: panda_link0 130 | Frame Rate: 30 131 | Name: root 132 | Tools: 133 | - Class: rviz/Interact 134 | Hide Inactive Objects: true 135 | - Class: rviz/MoveCamera 136 | - Class: rviz/Select 137 | - Class: rviz/FocusCamera 138 | - Class: rviz/Measure 139 | - Class: rviz/SetInitialPose 140 | Topic: /initialpose 141 | - Class: rviz/SetGoal 142 | Topic: /move_base_simple/goal 143 | - Class: rviz/PublishPoint 144 | Single click: true 145 | Topic: /clicked_point 146 | Value: true 147 | Views: 148 | Current: 149 | Class: rviz/Orbit 150 | Distance: 3.39362764 151 | Enable Stereo Rendering: 152 | Stereo Eye Separation: 0.0599999987 153 | Stereo Focal Distance: 1 154 | Swap Stereo Eyes: false 155 | Value: false 156 | Focal Point: 157 | X: -0.139062241 158 | Y: 0.151414081 159 | Z: 0.0437288694 160 | Focal Shape Fixed Size: true 161 | Focal Shape Size: 0.0500000007 162 | Invert Z Axis: false 163 | Name: Current View 164 | Near Clip Distance: 0.00999999978 165 | Pitch: 0.485397696 166 | Target Frame: 167 | Value: Orbit (rviz) 168 | Yaw: 0.500398219 169 | Saved: ~ 170 | Window Geometry: 171 | Displays: 172 | collapsed: false 173 | Height: 1059 174 | Hide Left Dock: false 175 | Hide Right Dock: false 176 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000384fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000384000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000384fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000384000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000027600fffffffb0000000800540069006d00650100000000000004500000000000000000000004f90000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 177 | Selection: 178 | collapsed: false 179 | Time: 180 | collapsed: false 181 | Tool Properties: 182 | collapsed: false 183 | Views: 184 | collapsed: false 185 | Width: 1918 186 | X: 1920 187 | Y: 19 188 | -------------------------------------------------------------------------------- /launch/rviz/franka_description_with_marker.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 573 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | panda_link0: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | panda_link1: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | panda_link2: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | panda_link3: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | panda_link4: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | panda_link5: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | panda_link6: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | panda_link7: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | panda_link8: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Name: RobotModel 106 | Robot Description: robot_description 107 | TF Prefix: "" 108 | Update Interval: 0 109 | Value: true 110 | Visual Enabled: true 111 | - Class: rviz/InteractiveMarkers 112 | Enable Transparency: true 113 | Enabled: true 114 | Name: InteractiveMarkers 115 | Show Axes: false 116 | Show Descriptions: true 117 | Show Visual Aids: false 118 | Update Topic: /equilibrium_pose_marker/update 119 | Value: true 120 | Enabled: true 121 | Global Options: 122 | Background Color: 48; 48; 48 123 | Fixed Frame: panda_link0 124 | Frame Rate: 30 125 | Name: root 126 | Tools: 127 | - Class: rviz/Interact 128 | Hide Inactive Objects: true 129 | - Class: rviz/MoveCamera 130 | - Class: rviz/Select 131 | - Class: rviz/FocusCamera 132 | - Class: rviz/Measure 133 | - Class: rviz/SetInitialPose 134 | Topic: /initialpose 135 | - Class: rviz/SetGoal 136 | Topic: /move_base_simple/goal 137 | - Class: rviz/PublishPoint 138 | Single click: true 139 | Topic: /clicked_point 140 | Value: true 141 | Views: 142 | Current: 143 | Class: rviz/Orbit 144 | Distance: 4.644041061401367 145 | Enable Stereo Rendering: 146 | Stereo Eye Separation: 0.05999999865889549 147 | Stereo Focal Distance: 1 148 | Swap Stereo Eyes: false 149 | Value: false 150 | Focal Point: 151 | X: 0 152 | Y: 0 153 | Z: 0 154 | Focal Shape Fixed Size: true 155 | Focal Shape Size: 0.05000000074505806 156 | Invert Z Axis: false 157 | Name: Current View 158 | Near Clip Distance: 0.009999999776482582 159 | Pitch: 0.785398006439209 160 | Target Frame: 161 | Value: Orbit (rviz) 162 | Yaw: 0.785398006439209 163 | Saved: ~ 164 | Window Geometry: 165 | Displays: 166 | collapsed: false 167 | Height: 846 168 | Hide Left Dock: false 169 | Hide Right Dock: false 170 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000002c5000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000027000002c50000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000042fc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 171 | Selection: 172 | collapsed: false 173 | Time: 174 | collapsed: false 175 | Tool Properties: 176 | collapsed: false 177 | Views: 178 | collapsed: false 179 | Width: 1200 180 | X: 295 181 | Y: 39 182 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | * @mainpage 3 | * @htmlinclude "manifest.html" 4 | * 5 | * Overview page for Franka Emika research robots: https://frankaemika.github.io 6 | */ 7 | -------------------------------------------------------------------------------- /msg/JointTorqueComparison.msg: -------------------------------------------------------------------------------- 1 | float64[7] tau_error 2 | float64[7] tau_commanded 3 | float64[7] tau_measured 4 | float64 root_mean_square_error 5 | -------------------------------------------------------------------------------- /msg/JointTorqueState.msg: -------------------------------------------------------------------------------- 1 | float64[7] joint_torques 2 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | franka_human_friendly_controllers 4 | 0.8.1 5 | This is part of human friendly controllers 6 | Giovanni Franzese 7 | Apache 2.0 8 | 9 | Franka Emika GmbH 10 | 11 | catkin 12 | 13 | message_generation 14 | eigen 15 | 16 | message_runtime 17 | 18 | controller_interface 19 | dynamic_reconfigure 20 | eigen_conversions 21 | franka_hw 22 | franka_gripper 23 | geometry_msgs 24 | hardware_interface 25 | tf 26 | tf_conversions 27 | libfranka 28 | pluginlib 29 | realtime_tools 30 | roscpp 31 | 32 | franka_control 33 | franka_description 34 | message_runtime 35 | panda_moveit_config 36 | moveit_commander 37 | rospy 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /python/.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/* 2 | -------------------------------------------------------------------------------- /python/LfD/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "ros.distro": "noetic" 3 | } -------------------------------------------------------------------------------- /python/LfD/Learning_from_demonstration.py: -------------------------------------------------------------------------------- 1 | #%% 2 | #!/usr/bin/env python 3 | import rospy 4 | import math 5 | import numpy as np 6 | import time 7 | import os 8 | from geometry_msgs.msg import PoseStamped, Pose 9 | from pynput.keyboard import Listener, KeyCode, Key 10 | from pose_transform_functions import array_quat_2_pose, list_2_quaternion, position_2_array, transform_pose 11 | from panda import Panda 12 | 13 | class LfD(Panda): 14 | def __init__(self): 15 | rospy.init_node("learning_node") 16 | super(LfD, self).__init__() 17 | self.r=rospy.Rate(20) 18 | self.pose = Pose() 19 | self.recorded_traj = None 20 | self.recorded_ori=None 21 | self.recorded_gripper= None 22 | self.end=False 23 | self.attractor_distance_threshold=0.05 # If the attractor is larger than the treshold, the robot will wait 24 | self.max_gripper_force= 2 25 | 26 | self.gripper_close_width = 0 27 | self.gripper_open_width = 0.06 28 | self.gripper_sensitivity= 0.03 29 | 30 | self.listener = Listener(on_press=self._on_press) 31 | self.listener.start() 32 | 33 | def _on_press(self, key): 34 | # This function runs on the background and checks if a keyboard key was pressed 35 | if key == Key.esc: 36 | self.end = True 37 | 38 | def traj_rec(self, trigger=0.005, rec_position=True, rec_orientation=True): 39 | # trigger for starting the recording 40 | if rec_position: 41 | self.set_K.update_configuration({"translational_stiffness_X": 0}) 42 | self.set_K.update_configuration({"translational_stiffness_Y": 0}) 43 | self.set_K.update_configuration({"translational_stiffness_Z": 0}) 44 | if rec_orientation: 45 | self.set_K.update_configuration({"rotational_stiffness_X": 0}) 46 | self.set_K.update_configuration({"rotational_stiffness_Y": 0}) 47 | self.set_K.update_configuration({"rotational_stiffness_Z": 0}) 48 | self.set_K.update_configuration({"nullspace_stiffness": 0}) 49 | self.end=False 50 | init_pos = self.curr_pos 51 | robot_perturbation = 0 52 | print("Move robot to start recording.") 53 | while robot_perturbation < trigger: 54 | robot_perturbation = math.sqrt((self.curr_pos[0]-init_pos[0])**2 + (self.curr_pos[1]-init_pos[1])**2 + (self.curr_pos[2]-init_pos[2])**2) 55 | 56 | self.recorded_traj = self.curr_pos 57 | self.recorded_ori = self.curr_ori 58 | self.recorded_gripper = self.gripper_open_width 59 | 60 | 61 | print("Recording started. Press ESC to stop.") 62 | while not self.end: 63 | if self.gripper_width < (self.gripper_open_width - self.gripper_sensitivity): 64 | # print("Close gripper") 65 | self.grip_value = 0 #Close the gripper 66 | else: 67 | # print("Open gripper") 68 | self.grip_value = self.gripper_open_width #Open the gripper 69 | 70 | self.recorded_traj = np.c_[self.recorded_traj, self.curr_pos] 71 | self.recorded_ori = np.c_[self.recorded_ori, self.curr_ori] 72 | self.recorded_gripper = np.c_[self.recorded_gripper, self.grip_value] 73 | 74 | 75 | self.r.sleep() 76 | 77 | quat_goal = list_2_quaternion(self.curr_ori) 78 | goal = array_quat_2_pose(self.curr_pos, quat_goal) 79 | self.goal_pub.publish(goal) 80 | self.set_stiffness(100, 100, 100, 5, 5, 5, 0) 81 | rospy.loginfo("Ending trajectory recording") 82 | 83 | def execute(self): 84 | self.set_stiffness(1000, 1000, 1000, 30, 30, 30, 0) 85 | start = PoseStamped() 86 | self.grasp_gripper(self.recorded_gripper[0][0], self.max_gripper_force) 87 | quat_start = list_2_quaternion(self.recorded_ori[:, 0]) 88 | start = array_quat_2_pose(self.recorded_traj[:, 0], quat_start) 89 | 90 | self.go_to_pose(start) 91 | 92 | self.time_index=0 93 | while self.time_index <( self.recorded_traj.shape[1]): 94 | 95 | quat_goal = list_2_quaternion(self.recorded_ori[:, self.time_index]) 96 | goal = array_quat_2_pose(self.recorded_traj[:, self.time_index], quat_goal) 97 | goal.header.seq = 1 98 | goal.header.stamp = rospy.Time.now() 99 | 100 | 101 | if (self.recorded_gripper[0][self.time_index]-self.recorded_gripper[0][max([0,self.time_index-1])]) < -self.gripper_sensitivity: 102 | print("closing gripper") 103 | self.grasp_gripper(self.recorded_gripper[0][self.time_index], self.max_gripper_force) 104 | time.sleep(0.1) 105 | 106 | if (self.recorded_gripper[0][self.time_index]-self.recorded_gripper[0][max([0,self.time_index-1])]) > self.gripper_sensitivity: 107 | print("open gripper") 108 | self.move_gripper(self.recorded_gripper[0][self.time_index]) 109 | time.sleep(0.1) 110 | 111 | self.goal_pub.publish(goal) 112 | 113 | # Safety feature in case somebody is touching the robot during execution 114 | goal_pos_array = position_2_array(goal.pose.position) 115 | if np.linalg.norm(self.curr_pos-goal_pos_array) <= self.attractor_distance_threshold: 116 | self.time_index=self.time_index+1 117 | self.r.sleep() 118 | 119 | def save(self, name='demo'): 120 | curr_dir=os.getcwd() 121 | np.savez(curr_dir+ '/data/' + str(name) + '.npz', 122 | traj=self.recorded_traj, 123 | ori=self.recorded_ori, 124 | grip=self.recorded_gripper) 125 | 126 | def load(self, name='demo'): 127 | curr_dir=os.getcwd() 128 | data = np.load(curr_dir+ '/data/' + str(name) + '.npz') 129 | self.recorded_traj = data['traj'] 130 | self.recorded_ori = data['ori'] 131 | self.recorded_gripper = data['grip'] 132 | self.filename=str(name) 133 | -------------------------------------------------------------------------------- /python/LfD/Learning_from_demonstration_joint.py: -------------------------------------------------------------------------------- 1 | #%% 2 | #!/usr/bin/env python 3 | #from winreg import REG_EXPAND_SZ 4 | import rospy 5 | # import rosbag 6 | import math 7 | import numpy as np 8 | import time 9 | 10 | #from zmq import RECONNECT_IVL_MAX 11 | from sensor_msgs.msg import JointState 12 | from geometry_msgs.msg import PoseStamped 13 | from std_msgs.msg import Float32MultiArray, Float32 14 | import dynamic_reconfigure.client 15 | from sys import exit 16 | from pynput.keyboard import Listener, KeyCode 17 | import pathlib 18 | class LfD(): 19 | def __init__(self): 20 | rospy.init_node('LfD', anonymous=True) 21 | self.r=rospy.Rate(10) 22 | self.curr_pos=None 23 | self.curr_joint = None 24 | self.width=None 25 | self.recorded_traj = None 26 | self.recorded_experiment = None 27 | self.recorded_gripper= None 28 | self.recorded_joint = None 29 | self.step_change = 0.1 30 | self.end = False 31 | self.save_joint_position = False 32 | # bag_name = 'test_results-' + time.strftime("%H-%M-%S") + '.bag' 33 | # self.bag = rosbag.Bag(bag_name, 'w') 34 | self.recording_state = False 35 | self.gripper_sub=rospy.Subscriber("/joint_states", JointState, self.gripper_callback) 36 | #self.results_sub = rospy.Subscriber("/joint_states", JointState, self.rec_results) 37 | self.joints=rospy.Subscriber("/joint_states", JointState, self.joint_callback) 38 | self.grip_pub = rospy.Publisher('/gripper_online', Float32, queue_size=0) 39 | # self.stiff_mat_pub_ = rospy.Publisher('/stiffness', Float32MultiArray, queue_size=0) #TODO check the name of this topic 40 | self.joint_pub = rospy.Publisher('/equilibrium_configuration', JointState , queue_size=0) 41 | self.listener = Listener(on_press=self._on_press) 42 | self.listener.start() 43 | def gripper_callback(self, data): 44 | self.width =data.position[7]+data.position[8] 45 | def joint_callback(self,data): 46 | self.curr_joint =data.position[0:7] 47 | if self.recording_state == True: 48 | self.recorded_experiment = np.c_[self.recorded_experiment, self.curr_joint] 49 | self.recorded_time = np.c_[self.recorded_time, time.time()] 50 | def _on_press(self, key): 51 | # This function runs on the background and checks if a keyboard key was pressed 52 | if key == KeyCode.from_char('e'): 53 | self.end = True 54 | 55 | def set_stiffness_joint(self, k_1, k_2, k_3, k_4, k_5, k_6, k_7): 56 | 57 | set_K = dynamic_reconfigure.client.Client('/dynamic_reconfigure_compliance_param_node', config_callback=None) 58 | set_K.update_configuration({"joint_1": k_1}) 59 | set_K.update_configuration({"joint_2": k_2}) 60 | set_K.update_configuration({"joint_3": k_3}) 61 | set_K.update_configuration({"joint_4": k_4}) 62 | set_K.update_configuration({"joint_5": k_5}) 63 | set_K.update_configuration({"joint_6": k_6}) 64 | set_K.update_configuration({"joint_7": k_7}) 65 | 66 | 67 | def joint_rec(self): 68 | self.set_stiffness_joint(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) 69 | trigger = 0.05 70 | init_joint = self.curr_joint 71 | vel = 0 72 | while not(vel > trigger): 73 | vel= math.sqrt((self.curr_joint[0]-init_joint[0])**2 + (self.curr_joint[1]-init_joint[1])**2 + (self.curr_joint[2]-init_joint[2])**2+ (self.curr_joint[3]-init_joint[3])**2+ (self.curr_joint[4]-init_joint[4])**2+ (self.curr_joint[5]-init_joint[5])**2+(self.curr_joint[6]-init_joint[6])**2) 74 | print("Recording Trajectory") 75 | self.recorded_joint = self.curr_joint 76 | self.recorded_gripper= self.width 77 | # recorded_joint = joint_pos 78 | self.end=False 79 | while not(self.end): 80 | now = time.time() 81 | 82 | self.recorded_gripper = np.c_[self.recorded_gripper, self.width] 83 | self.recorded_joint = np.c_[self.recorded_joint, self.curr_joint] 84 | self.r.sleep() 85 | 86 | self.end=False 87 | 88 | 89 | def save(self, data='recorded_demo'): 90 | np.savez(str(pathlib.Path().resolve())+'/data/'+str(data)+'.npz', 91 | recorded_joint=self.recorded_joint, 92 | recorded_gripper=self.recorded_gripper) 93 | 94 | def go_to_start_joint(self): 95 | start = self.curr_joint 96 | goal_=np.array([self.recorded_joint[0][0], self.recorded_joint[1][0], self.recorded_joint[2][0], self.recorded_joint[3][0], self.recorded_joint[4][0], self.recorded_joint[5][0], self.recorded_joint[6][0]]) 97 | print("goal:", goal_) 98 | squared_dist = np.sum(np.subtract(start, goal_)**2, axis=0) 99 | dist = np.sqrt(squared_dist) 100 | print("dist", dist) 101 | interp_dist = 0.05 102 | step_num = math.floor(dist / interp_dist) 103 | print("num of steps", step_num) 104 | q1 = np.linspace(start[0], goal_[0], step_num) 105 | q2 = np.linspace(start[1], goal_[1], step_num) 106 | q3 = np.linspace(start[2], goal_[2], step_num) 107 | q4 = np.linspace(start[3], goal_[3], step_num) 108 | q5 = np.linspace(start[4], goal_[4], step_num) 109 | q6 = np.linspace(start[5], goal_[5], step_num) 110 | q7 = np.linspace(start[6], goal_[6], step_num) 111 | goal=JointState() 112 | goal.position=[q1[0],q2[0],q3[0],q4[0],q5[0],q6[0],q7[0]] 113 | self.joint_pub.publish(goal) 114 | self.set_stiffness_joint(10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 5.0) 115 | 116 | for i in range(step_num): 117 | goal=JointState() 118 | goal.position=[q1[i],q2[i],q3[i],q4[i],q5[i],q6[i],q7[i]] 119 | self.joint_pub.publish(goal) 120 | self.r.sleep() 121 | 122 | def execute_joints(self): 123 | self.set_stiffness_joint(30.0, 30.0, 30.0, 30.0, 30.0, 30.0, 8.0) 124 | 125 | for i in range(np.shape(self.recorded_joint)[1]): 126 | goal=JointState() 127 | goal.position=self.recorded_joint[:,i] 128 | self.joint_pub.publish(goal) 129 | grip_command = Float32() 130 | grip_command.data = self.recorded_gripper[0,i] 131 | self.grip_pub.publish(grip_command) 132 | self.r.sleep() 133 | 134 | def load_file(self, file="recorded_demo"): 135 | data=np.load(str(pathlib.Path().resolve())+'/data/'+str(file)+'.npz') 136 | self.recorded_joint = data['recorded_joint'] 137 | self.recorded_gripper = data['recorded_gripper'] 138 | 139 | 140 | #%% 141 | LfD=LfD() 142 | #%% 143 | LfD.joint_rec() 144 | 145 | #%% 146 | LfD.go_to_start_joint() 147 | 148 | #%% 149 | LfD.execute_joints() 150 | 151 | # %% 152 | -------------------------------------------------------------------------------- /python/LfD/__pycache__/Learning_from_demonstration.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/franzesegiovanni/franka_human_friendly_controllers/05f13bfab3b3ac7ad0c08123be272e3d8bcb7fe0/python/LfD/__pycache__/Learning_from_demonstration.cpython-38.pyc -------------------------------------------------------------------------------- /python/LfD/__pycache__/panda.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/franzesegiovanni/franka_human_friendly_controllers/05f13bfab3b3ac7ad0c08123be272e3d8bcb7fe0/python/LfD/__pycache__/panda.cpython-38.pyc -------------------------------------------------------------------------------- /python/LfD/__pycache__/pose_transform_functions.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/franzesegiovanni/franka_human_friendly_controllers/05f13bfab3b3ac7ad0c08123be272e3d8bcb7fe0/python/LfD/__pycache__/pose_transform_functions.cpython-38.pyc -------------------------------------------------------------------------------- /python/LfD/data/last.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/franzesegiovanni/franka_human_friendly_controllers/05f13bfab3b3ac7ad0c08123be272e3d8bcb7fe0/python/LfD/data/last.npz -------------------------------------------------------------------------------- /python/LfD/franka_gripper/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/franzesegiovanni/franka_human_friendly_controllers/05f13bfab3b3ac7ad0c08123be272e3d8bcb7fe0/python/LfD/franka_gripper/__init__.py -------------------------------------------------------------------------------- /python/LfD/franka_gripper/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/franzesegiovanni/franka_human_friendly_controllers/05f13bfab3b3ac7ad0c08123be272e3d8bcb7fe0/python/LfD/franka_gripper/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_GraspEpsilon.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/GraspEpsilon.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class GraspEpsilon(genpy.Message): 11 | _md5sum = "95b2c5464a6f679bd1dacaf86414f095" 12 | _type = "franka_gripper/GraspEpsilon" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """float64 inner # [m] 15 | float64 outer # [m] 16 | """ 17 | __slots__ = ['inner','outer'] 18 | _slot_types = ['float64','float64'] 19 | 20 | def __init__(self, *args, **kwds): 21 | """ 22 | Constructor. Any message fields that are implicitly/explicitly 23 | set to None will be assigned a default value. The recommend 24 | use is keyword arguments as this is more robust to future message 25 | changes. You cannot mix in-order arguments and keyword arguments. 26 | 27 | The available fields are: 28 | inner,outer 29 | 30 | :param args: complete set of field values, in .msg order 31 | :param kwds: use keyword arguments corresponding to message field names 32 | to set specific fields. 33 | """ 34 | if args or kwds: 35 | super(GraspEpsilon, self).__init__(*args, **kwds) 36 | # message fields cannot be None, assign default values for those that are 37 | if self.inner is None: 38 | self.inner = 0. 39 | if self.outer is None: 40 | self.outer = 0. 41 | else: 42 | self.inner = 0. 43 | self.outer = 0. 44 | 45 | def _get_types(self): 46 | """ 47 | internal API method 48 | """ 49 | return self._slot_types 50 | 51 | def serialize(self, buff): 52 | """ 53 | serialize message into buffer 54 | :param buff: buffer, ``StringIO`` 55 | """ 56 | try: 57 | _x = self 58 | buff.write(_get_struct_2d().pack(_x.inner, _x.outer)) 59 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 60 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 61 | 62 | def deserialize(self, str): 63 | """ 64 | unpack serialized message in str into this message instance 65 | :param str: byte array of serialized message, ``str`` 66 | """ 67 | if python3: 68 | codecs.lookup_error("rosmsg").msg_type = self._type 69 | try: 70 | end = 0 71 | _x = self 72 | start = end 73 | end += 16 74 | (_x.inner, _x.outer,) = _get_struct_2d().unpack(str[start:end]) 75 | return self 76 | except struct.error as e: 77 | raise genpy.DeserializationError(e) # most likely buffer underfill 78 | 79 | 80 | def serialize_numpy(self, buff, numpy): 81 | """ 82 | serialize message with numpy array types into buffer 83 | :param buff: buffer, ``StringIO`` 84 | :param numpy: numpy python module 85 | """ 86 | try: 87 | _x = self 88 | buff.write(_get_struct_2d().pack(_x.inner, _x.outer)) 89 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 90 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 91 | 92 | def deserialize_numpy(self, str, numpy): 93 | """ 94 | unpack serialized message in str into this message instance using numpy for array types 95 | :param str: byte array of serialized message, ``str`` 96 | :param numpy: numpy python module 97 | """ 98 | if python3: 99 | codecs.lookup_error("rosmsg").msg_type = self._type 100 | try: 101 | end = 0 102 | _x = self 103 | start = end 104 | end += 16 105 | (_x.inner, _x.outer,) = _get_struct_2d().unpack(str[start:end]) 106 | return self 107 | except struct.error as e: 108 | raise genpy.DeserializationError(e) # most likely buffer underfill 109 | 110 | _struct_I = genpy.struct_I 111 | def _get_struct_I(): 112 | global _struct_I 113 | return _struct_I 114 | _struct_2d = None 115 | def _get_struct_2d(): 116 | global _struct_2d 117 | if _struct_2d is None: 118 | _struct_2d = struct.Struct("<2d") 119 | return _struct_2d 120 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_GraspFeedback.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/GraspFeedback.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class GraspFeedback(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_gripper/GraspFeedback" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | 16 | """ 17 | __slots__ = [] 18 | _slot_types = [] 19 | 20 | def __init__(self, *args, **kwds): 21 | """ 22 | Constructor. Any message fields that are implicitly/explicitly 23 | set to None will be assigned a default value. The recommend 24 | use is keyword arguments as this is more robust to future message 25 | changes. You cannot mix in-order arguments and keyword arguments. 26 | 27 | The available fields are: 28 | 29 | 30 | :param args: complete set of field values, in .msg order 31 | :param kwds: use keyword arguments corresponding to message field names 32 | to set specific fields. 33 | """ 34 | if args or kwds: 35 | super(GraspFeedback, self).__init__(*args, **kwds) 36 | 37 | def _get_types(self): 38 | """ 39 | internal API method 40 | """ 41 | return self._slot_types 42 | 43 | def serialize(self, buff): 44 | """ 45 | serialize message into buffer 46 | :param buff: buffer, ``StringIO`` 47 | """ 48 | try: 49 | pass 50 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 51 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 52 | 53 | def deserialize(self, str): 54 | """ 55 | unpack serialized message in str into this message instance 56 | :param str: byte array of serialized message, ``str`` 57 | """ 58 | if python3: 59 | codecs.lookup_error("rosmsg").msg_type = self._type 60 | try: 61 | end = 0 62 | return self 63 | except struct.error as e: 64 | raise genpy.DeserializationError(e) # most likely buffer underfill 65 | 66 | 67 | def serialize_numpy(self, buff, numpy): 68 | """ 69 | serialize message with numpy array types into buffer 70 | :param buff: buffer, ``StringIO`` 71 | :param numpy: numpy python module 72 | """ 73 | try: 74 | pass 75 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 76 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 77 | 78 | def deserialize_numpy(self, str, numpy): 79 | """ 80 | unpack serialized message in str into this message instance using numpy for array types 81 | :param str: byte array of serialized message, ``str`` 82 | :param numpy: numpy python module 83 | """ 84 | if python3: 85 | codecs.lookup_error("rosmsg").msg_type = self._type 86 | try: 87 | end = 0 88 | return self 89 | except struct.error as e: 90 | raise genpy.DeserializationError(e) # most likely buffer underfill 91 | 92 | _struct_I = genpy.struct_I 93 | def _get_struct_I(): 94 | global _struct_I 95 | return _struct_I 96 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_GraspGoal.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/GraspGoal.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import franka_gripper.msg 10 | 11 | class GraspGoal(genpy.Message): 12 | _md5sum = "627a0f0b10ad0c919fbd62b0b3427e63" 13 | _type = "franka_gripper/GraspGoal" 14 | _has_header = False # flag to mark the presence of a Header object 15 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 16 | float64 width # [m] 17 | GraspEpsilon epsilon 18 | float64 speed # [m/s] 19 | float64 force # [N] 20 | 21 | ================================================================================ 22 | MSG: franka_gripper/GraspEpsilon 23 | float64 inner # [m] 24 | float64 outer # [m] 25 | """ 26 | __slots__ = ['width','epsilon','speed','force'] 27 | _slot_types = ['float64','franka_gripper/GraspEpsilon','float64','float64'] 28 | 29 | def __init__(self, *args, **kwds): 30 | """ 31 | Constructor. Any message fields that are implicitly/explicitly 32 | set to None will be assigned a default value. The recommend 33 | use is keyword arguments as this is more robust to future message 34 | changes. You cannot mix in-order arguments and keyword arguments. 35 | 36 | The available fields are: 37 | width,epsilon,speed,force 38 | 39 | :param args: complete set of field values, in .msg order 40 | :param kwds: use keyword arguments corresponding to message field names 41 | to set specific fields. 42 | """ 43 | if args or kwds: 44 | super(GraspGoal, self).__init__(*args, **kwds) 45 | # message fields cannot be None, assign default values for those that are 46 | if self.width is None: 47 | self.width = 0. 48 | if self.epsilon is None: 49 | self.epsilon = franka_gripper.msg.GraspEpsilon() 50 | if self.speed is None: 51 | self.speed = 0. 52 | if self.force is None: 53 | self.force = 0. 54 | else: 55 | self.width = 0. 56 | self.epsilon = franka_gripper.msg.GraspEpsilon() 57 | self.speed = 0. 58 | self.force = 0. 59 | 60 | def _get_types(self): 61 | """ 62 | internal API method 63 | """ 64 | return self._slot_types 65 | 66 | def serialize(self, buff): 67 | """ 68 | serialize message into buffer 69 | :param buff: buffer, ``StringIO`` 70 | """ 71 | try: 72 | _x = self 73 | buff.write(_get_struct_5d().pack(_x.width, _x.epsilon.inner, _x.epsilon.outer, _x.speed, _x.force)) 74 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 75 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 76 | 77 | def deserialize(self, str): 78 | """ 79 | unpack serialized message in str into this message instance 80 | :param str: byte array of serialized message, ``str`` 81 | """ 82 | if python3: 83 | codecs.lookup_error("rosmsg").msg_type = self._type 84 | try: 85 | if self.epsilon is None: 86 | self.epsilon = franka_gripper.msg.GraspEpsilon() 87 | end = 0 88 | _x = self 89 | start = end 90 | end += 40 91 | (_x.width, _x.epsilon.inner, _x.epsilon.outer, _x.speed, _x.force,) = _get_struct_5d().unpack(str[start:end]) 92 | return self 93 | except struct.error as e: 94 | raise genpy.DeserializationError(e) # most likely buffer underfill 95 | 96 | 97 | def serialize_numpy(self, buff, numpy): 98 | """ 99 | serialize message with numpy array types into buffer 100 | :param buff: buffer, ``StringIO`` 101 | :param numpy: numpy python module 102 | """ 103 | try: 104 | _x = self 105 | buff.write(_get_struct_5d().pack(_x.width, _x.epsilon.inner, _x.epsilon.outer, _x.speed, _x.force)) 106 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 107 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 108 | 109 | def deserialize_numpy(self, str, numpy): 110 | """ 111 | unpack serialized message in str into this message instance using numpy for array types 112 | :param str: byte array of serialized message, ``str`` 113 | :param numpy: numpy python module 114 | """ 115 | if python3: 116 | codecs.lookup_error("rosmsg").msg_type = self._type 117 | try: 118 | if self.epsilon is None: 119 | self.epsilon = franka_gripper.msg.GraspEpsilon() 120 | end = 0 121 | _x = self 122 | start = end 123 | end += 40 124 | (_x.width, _x.epsilon.inner, _x.epsilon.outer, _x.speed, _x.force,) = _get_struct_5d().unpack(str[start:end]) 125 | return self 126 | except struct.error as e: 127 | raise genpy.DeserializationError(e) # most likely buffer underfill 128 | 129 | _struct_I = genpy.struct_I 130 | def _get_struct_I(): 131 | global _struct_I 132 | return _struct_I 133 | _struct_5d = None 134 | def _get_struct_5d(): 135 | global _struct_5d 136 | if _struct_5d is None: 137 | _struct_5d = struct.Struct("<5d") 138 | return _struct_5d 139 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_GraspResult.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/GraspResult.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class GraspResult(genpy.Message): 11 | _md5sum = "45872d25d65c97743cc71afc6d4e884d" 12 | _type = "franka_gripper/GraspResult" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | bool success 16 | string error 17 | """ 18 | __slots__ = ['success','error'] 19 | _slot_types = ['bool','string'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | success,error 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(GraspResult, self).__init__(*args, **kwds) 37 | # message fields cannot be None, assign default values for those that are 38 | if self.success is None: 39 | self.success = False 40 | if self.error is None: 41 | self.error = '' 42 | else: 43 | self.success = False 44 | self.error = '' 45 | 46 | def _get_types(self): 47 | """ 48 | internal API method 49 | """ 50 | return self._slot_types 51 | 52 | def serialize(self, buff): 53 | """ 54 | serialize message into buffer 55 | :param buff: buffer, ``StringIO`` 56 | """ 57 | try: 58 | _x = self.success 59 | buff.write(_get_struct_B().pack(_x)) 60 | _x = self.error 61 | length = len(_x) 62 | if python3 or type(_x) == unicode: 63 | _x = _x.encode('utf-8') 64 | length = len(_x) 65 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class HomingFeedback(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_gripper/HomingFeedback" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | 16 | """ 17 | __slots__ = [] 18 | _slot_types = [] 19 | 20 | def __init__(self, *args, **kwds): 21 | """ 22 | Constructor. Any message fields that are implicitly/explicitly 23 | set to None will be assigned a default value. The recommend 24 | use is keyword arguments as this is more robust to future message 25 | changes. You cannot mix in-order arguments and keyword arguments. 26 | 27 | The available fields are: 28 | 29 | 30 | :param args: complete set of field values, in .msg order 31 | :param kwds: use keyword arguments corresponding to message field names 32 | to set specific fields. 33 | """ 34 | if args or kwds: 35 | super(HomingFeedback, self).__init__(*args, **kwds) 36 | 37 | def _get_types(self): 38 | """ 39 | internal API method 40 | """ 41 | return self._slot_types 42 | 43 | def serialize(self, buff): 44 | """ 45 | serialize message into buffer 46 | :param buff: buffer, ``StringIO`` 47 | """ 48 | try: 49 | pass 50 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 51 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 52 | 53 | def deserialize(self, str): 54 | """ 55 | unpack serialized message in str into this message instance 56 | :param str: byte array of serialized message, ``str`` 57 | """ 58 | if python3: 59 | codecs.lookup_error("rosmsg").msg_type = self._type 60 | try: 61 | end = 0 62 | return self 63 | except struct.error as e: 64 | raise genpy.DeserializationError(e) # most likely buffer underfill 65 | 66 | 67 | def serialize_numpy(self, buff, numpy): 68 | """ 69 | serialize message with numpy array types into buffer 70 | :param buff: buffer, ``StringIO`` 71 | :param numpy: numpy python module 72 | """ 73 | try: 74 | pass 75 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 76 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 77 | 78 | def deserialize_numpy(self, str, numpy): 79 | """ 80 | unpack serialized message in str into this message instance using numpy for array types 81 | :param str: byte array of serialized message, ``str`` 82 | :param numpy: numpy python module 83 | """ 84 | if python3: 85 | codecs.lookup_error("rosmsg").msg_type = self._type 86 | try: 87 | end = 0 88 | return self 89 | except struct.error as e: 90 | raise genpy.DeserializationError(e) # most likely buffer underfill 91 | 92 | _struct_I = genpy.struct_I 93 | def _get_struct_I(): 94 | global _struct_I 95 | return _struct_I 96 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_HomingGoal.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/HomingGoal.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class HomingGoal(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_gripper/HomingGoal" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | """ 16 | __slots__ = [] 17 | _slot_types = [] 18 | 19 | def __init__(self, *args, **kwds): 20 | """ 21 | Constructor. Any message fields that are implicitly/explicitly 22 | set to None will be assigned a default value. The recommend 23 | use is keyword arguments as this is more robust to future message 24 | changes. You cannot mix in-order arguments and keyword arguments. 25 | 26 | The available fields are: 27 | 28 | 29 | :param args: complete set of field values, in .msg order 30 | :param kwds: use keyword arguments corresponding to message field names 31 | to set specific fields. 32 | """ 33 | if args or kwds: 34 | super(HomingGoal, self).__init__(*args, **kwds) 35 | 36 | def _get_types(self): 37 | """ 38 | internal API method 39 | """ 40 | return self._slot_types 41 | 42 | def serialize(self, buff): 43 | """ 44 | serialize message into buffer 45 | :param buff: buffer, ``StringIO`` 46 | """ 47 | try: 48 | pass 49 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 50 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 51 | 52 | def deserialize(self, str): 53 | """ 54 | unpack serialized message in str into this message instance 55 | :param str: byte array of serialized message, ``str`` 56 | """ 57 | if python3: 58 | codecs.lookup_error("rosmsg").msg_type = self._type 59 | try: 60 | end = 0 61 | return self 62 | except struct.error as e: 63 | raise genpy.DeserializationError(e) # most likely buffer underfill 64 | 65 | 66 | def serialize_numpy(self, buff, numpy): 67 | """ 68 | serialize message with numpy array types into buffer 69 | :param buff: buffer, ``StringIO`` 70 | :param numpy: numpy python module 71 | """ 72 | try: 73 | pass 74 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 75 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 76 | 77 | def deserialize_numpy(self, str, numpy): 78 | """ 79 | unpack serialized message in str into this message instance using numpy for array types 80 | :param str: byte array of serialized message, ``str`` 81 | :param numpy: numpy python module 82 | """ 83 | if python3: 84 | codecs.lookup_error("rosmsg").msg_type = self._type 85 | try: 86 | end = 0 87 | return self 88 | except struct.error as e: 89 | raise genpy.DeserializationError(e) # most likely buffer underfill 90 | 91 | _struct_I = genpy.struct_I 92 | def _get_struct_I(): 93 | global _struct_I 94 | return _struct_I 95 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_HomingResult.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/HomingResult.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class HomingResult(genpy.Message): 11 | _md5sum = "45872d25d65c97743cc71afc6d4e884d" 12 | _type = "franka_gripper/HomingResult" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | bool success 16 | string error 17 | """ 18 | __slots__ = ['success','error'] 19 | _slot_types = ['bool','string'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | success,error 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(HomingResult, self).__init__(*args, **kwds) 37 | # message fields cannot be None, assign default values for those that are 38 | if self.success is None: 39 | self.success = False 40 | if self.error is None: 41 | self.error = '' 42 | else: 43 | self.success = False 44 | self.error = '' 45 | 46 | def _get_types(self): 47 | """ 48 | internal API method 49 | """ 50 | return self._slot_types 51 | 52 | def serialize(self, buff): 53 | """ 54 | serialize message into buffer 55 | :param buff: buffer, ``StringIO`` 56 | """ 57 | try: 58 | _x = self.success 59 | buff.write(_get_struct_B().pack(_x)) 60 | _x = self.error 61 | length = len(_x) 62 | if python3 or type(_x) == unicode: 63 | _x = _x.encode('utf-8') 64 | length = len(_x) 65 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class MoveFeedback(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_gripper/MoveFeedback" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | 16 | """ 17 | __slots__ = [] 18 | _slot_types = [] 19 | 20 | def __init__(self, *args, **kwds): 21 | """ 22 | Constructor. Any message fields that are implicitly/explicitly 23 | set to None will be assigned a default value. The recommend 24 | use is keyword arguments as this is more robust to future message 25 | changes. You cannot mix in-order arguments and keyword arguments. 26 | 27 | The available fields are: 28 | 29 | 30 | :param args: complete set of field values, in .msg order 31 | :param kwds: use keyword arguments corresponding to message field names 32 | to set specific fields. 33 | """ 34 | if args or kwds: 35 | super(MoveFeedback, self).__init__(*args, **kwds) 36 | 37 | def _get_types(self): 38 | """ 39 | internal API method 40 | """ 41 | return self._slot_types 42 | 43 | def serialize(self, buff): 44 | """ 45 | serialize message into buffer 46 | :param buff: buffer, ``StringIO`` 47 | """ 48 | try: 49 | pass 50 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 51 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 52 | 53 | def deserialize(self, str): 54 | """ 55 | unpack serialized message in str into this message instance 56 | :param str: byte array of serialized message, ``str`` 57 | """ 58 | if python3: 59 | codecs.lookup_error("rosmsg").msg_type = self._type 60 | try: 61 | end = 0 62 | return self 63 | except struct.error as e: 64 | raise genpy.DeserializationError(e) # most likely buffer underfill 65 | 66 | 67 | def serialize_numpy(self, buff, numpy): 68 | """ 69 | serialize message with numpy array types into buffer 70 | :param buff: buffer, ``StringIO`` 71 | :param numpy: numpy python module 72 | """ 73 | try: 74 | pass 75 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 76 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 77 | 78 | def deserialize_numpy(self, str, numpy): 79 | """ 80 | unpack serialized message in str into this message instance using numpy for array types 81 | :param str: byte array of serialized message, ``str`` 82 | :param numpy: numpy python module 83 | """ 84 | if python3: 85 | codecs.lookup_error("rosmsg").msg_type = self._type 86 | try: 87 | end = 0 88 | return self 89 | except struct.error as e: 90 | raise genpy.DeserializationError(e) # most likely buffer underfill 91 | 92 | _struct_I = genpy.struct_I 93 | def _get_struct_I(): 94 | global _struct_I 95 | return _struct_I 96 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_MoveGoal.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/MoveGoal.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class MoveGoal(genpy.Message): 11 | _md5sum = "d16050a0fe85f1c2cb8347c99678362e" 12 | _type = "franka_gripper/MoveGoal" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | float64 width # [m] 16 | float64 speed # [m/s] 17 | """ 18 | __slots__ = ['width','speed'] 19 | _slot_types = ['float64','float64'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | width,speed 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(MoveGoal, self).__init__(*args, **kwds) 37 | # message fields cannot be None, assign default values for those that are 38 | if self.width is None: 39 | self.width = 0. 40 | if self.speed is None: 41 | self.speed = 0. 42 | else: 43 | self.width = 0. 44 | self.speed = 0. 45 | 46 | def _get_types(self): 47 | """ 48 | internal API method 49 | """ 50 | return self._slot_types 51 | 52 | def serialize(self, buff): 53 | """ 54 | serialize message into buffer 55 | :param buff: buffer, ``StringIO`` 56 | """ 57 | try: 58 | _x = self 59 | buff.write(_get_struct_2d().pack(_x.width, _x.speed)) 60 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 61 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 62 | 63 | def deserialize(self, str): 64 | """ 65 | unpack serialized message in str into this message instance 66 | :param str: byte array of serialized message, ``str`` 67 | """ 68 | if python3: 69 | codecs.lookup_error("rosmsg").msg_type = self._type 70 | try: 71 | end = 0 72 | _x = self 73 | start = end 74 | end += 16 75 | (_x.width, _x.speed,) = _get_struct_2d().unpack(str[start:end]) 76 | return self 77 | except struct.error as e: 78 | raise genpy.DeserializationError(e) # most likely buffer underfill 79 | 80 | 81 | def serialize_numpy(self, buff, numpy): 82 | """ 83 | serialize message with numpy array types into buffer 84 | :param buff: buffer, ``StringIO`` 85 | :param numpy: numpy python module 86 | """ 87 | try: 88 | _x = self 89 | buff.write(_get_struct_2d().pack(_x.width, _x.speed)) 90 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 91 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 92 | 93 | def deserialize_numpy(self, str, numpy): 94 | """ 95 | unpack serialized message in str into this message instance using numpy for array types 96 | :param str: byte array of serialized message, ``str`` 97 | :param numpy: numpy python module 98 | """ 99 | if python3: 100 | codecs.lookup_error("rosmsg").msg_type = self._type 101 | try: 102 | end = 0 103 | _x = self 104 | start = end 105 | end += 16 106 | (_x.width, _x.speed,) = _get_struct_2d().unpack(str[start:end]) 107 | return self 108 | except struct.error as e: 109 | raise genpy.DeserializationError(e) # most likely buffer underfill 110 | 111 | _struct_I = genpy.struct_I 112 | def _get_struct_I(): 113 | global _struct_I 114 | return _struct_I 115 | _struct_2d = None 116 | def _get_struct_2d(): 117 | global _struct_2d 118 | if _struct_2d is None: 119 | _struct_2d = struct.Struct("<2d") 120 | return _struct_2d 121 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_MoveResult.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/MoveResult.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class MoveResult(genpy.Message): 11 | _md5sum = "45872d25d65c97743cc71afc6d4e884d" 12 | _type = "franka_gripper/MoveResult" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | bool success 16 | string error 17 | """ 18 | __slots__ = ['success','error'] 19 | _slot_types = ['bool','string'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | success,error 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(MoveResult, self).__init__(*args, **kwds) 37 | # message fields cannot be None, assign default values for those that are 38 | if self.success is None: 39 | self.success = False 40 | if self.error is None: 41 | self.error = '' 42 | else: 43 | self.success = False 44 | self.error = '' 45 | 46 | def _get_types(self): 47 | """ 48 | internal API method 49 | """ 50 | return self._slot_types 51 | 52 | def serialize(self, buff): 53 | """ 54 | serialize message into buffer 55 | :param buff: buffer, ``StringIO`` 56 | """ 57 | try: 58 | _x = self.success 59 | buff.write(_get_struct_B().pack(_x)) 60 | _x = self.error 61 | length = len(_x) 62 | if python3 or type(_x) == unicode: 63 | _x = _x.encode('utf-8') 64 | length = len(_x) 65 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | import actionlib_msgs.msg 10 | import franka_gripper.msg 11 | import genpy 12 | import std_msgs.msg 13 | 14 | class StopActionGoal(genpy.Message): 15 | _md5sum = "4b30be6cd12b9e72826df56b481f40e0" 16 | _type = "franka_gripper/StopActionGoal" 17 | _has_header = True # flag to mark the presence of a Header object 18 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 19 | 20 | Header header 21 | actionlib_msgs/GoalID goal_id 22 | StopGoal goal 23 | 24 | ================================================================================ 25 | MSG: std_msgs/Header 26 | # Standard metadata for higher-level stamped data types. 27 | # This is generally used to communicate timestamped data 28 | # in a particular coordinate frame. 29 | # 30 | # sequence ID: consecutively increasing ID 31 | uint32 seq 32 | #Two-integer timestamp that is expressed as: 33 | # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') 34 | # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') 35 | # time-handling sugar is provided by the client library 36 | time stamp 37 | #Frame this data is associated with 38 | string frame_id 39 | 40 | ================================================================================ 41 | MSG: actionlib_msgs/GoalID 42 | # The stamp should store the time at which this goal was requested. 43 | # It is used by an action server when it tries to preempt all 44 | # goals that were requested before a certain time 45 | time stamp 46 | 47 | # The id provides a way to associate feedback and 48 | # result message with specific goal requests. The id 49 | # specified must be unique. 50 | string id 51 | 52 | 53 | ================================================================================ 54 | MSG: franka_gripper/StopGoal 55 | # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 56 | """ 57 | __slots__ = ['header','goal_id','goal'] 58 | _slot_types = ['std_msgs/Header','actionlib_msgs/GoalID','franka_gripper/StopGoal'] 59 | 60 | def __init__(self, *args, **kwds): 61 | """ 62 | Constructor. Any message fields that are implicitly/explicitly 63 | set to None will be assigned a default value. The recommend 64 | use is keyword arguments as this is more robust to future message 65 | changes. You cannot mix in-order arguments and keyword arguments. 66 | 67 | The available fields are: 68 | header,goal_id,goal 69 | 70 | :param args: complete set of field values, in .msg order 71 | :param kwds: use keyword arguments corresponding to message field names 72 | to set specific fields. 73 | """ 74 | if args or kwds: 75 | super(StopActionGoal, self).__init__(*args, **kwds) 76 | # message fields cannot be None, assign default values for those that are 77 | if self.header is None: 78 | self.header = std_msgs.msg.Header() 79 | if self.goal_id is None: 80 | self.goal_id = actionlib_msgs.msg.GoalID() 81 | if self.goal is None: 82 | self.goal = franka_gripper.msg.StopGoal() 83 | else: 84 | self.header = std_msgs.msg.Header() 85 | self.goal_id = actionlib_msgs.msg.GoalID() 86 | self.goal = franka_gripper.msg.StopGoal() 87 | 88 | def _get_types(self): 89 | """ 90 | internal API method 91 | """ 92 | return self._slot_types 93 | 94 | def serialize(self, buff): 95 | """ 96 | serialize message into buffer 97 | :param buff: buffer, ``StringIO`` 98 | """ 99 | try: 100 | _x = self 101 | buff.write(_get_struct_3I().pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 102 | _x = self.header.frame_id 103 | length = len(_x) 104 | if python3 or type(_x) == unicode: 105 | _x = _x.encode('utf-8') 106 | length = len(_x) 107 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class StopFeedback(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_gripper/StopFeedback" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | 16 | """ 17 | __slots__ = [] 18 | _slot_types = [] 19 | 20 | def __init__(self, *args, **kwds): 21 | """ 22 | Constructor. Any message fields that are implicitly/explicitly 23 | set to None will be assigned a default value. The recommend 24 | use is keyword arguments as this is more robust to future message 25 | changes. You cannot mix in-order arguments and keyword arguments. 26 | 27 | The available fields are: 28 | 29 | 30 | :param args: complete set of field values, in .msg order 31 | :param kwds: use keyword arguments corresponding to message field names 32 | to set specific fields. 33 | """ 34 | if args or kwds: 35 | super(StopFeedback, self).__init__(*args, **kwds) 36 | 37 | def _get_types(self): 38 | """ 39 | internal API method 40 | """ 41 | return self._slot_types 42 | 43 | def serialize(self, buff): 44 | """ 45 | serialize message into buffer 46 | :param buff: buffer, ``StringIO`` 47 | """ 48 | try: 49 | pass 50 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 51 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 52 | 53 | def deserialize(self, str): 54 | """ 55 | unpack serialized message in str into this message instance 56 | :param str: byte array of serialized message, ``str`` 57 | """ 58 | if python3: 59 | codecs.lookup_error("rosmsg").msg_type = self._type 60 | try: 61 | end = 0 62 | return self 63 | except struct.error as e: 64 | raise genpy.DeserializationError(e) # most likely buffer underfill 65 | 66 | 67 | def serialize_numpy(self, buff, numpy): 68 | """ 69 | serialize message with numpy array types into buffer 70 | :param buff: buffer, ``StringIO`` 71 | :param numpy: numpy python module 72 | """ 73 | try: 74 | pass 75 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 76 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 77 | 78 | def deserialize_numpy(self, str, numpy): 79 | """ 80 | unpack serialized message in str into this message instance using numpy for array types 81 | :param str: byte array of serialized message, ``str`` 82 | :param numpy: numpy python module 83 | """ 84 | if python3: 85 | codecs.lookup_error("rosmsg").msg_type = self._type 86 | try: 87 | end = 0 88 | return self 89 | except struct.error as e: 90 | raise genpy.DeserializationError(e) # most likely buffer underfill 91 | 92 | _struct_I = genpy.struct_I 93 | def _get_struct_I(): 94 | global _struct_I 95 | return _struct_I 96 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_StopGoal.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/StopGoal.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class StopGoal(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_gripper/StopGoal" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | """ 16 | __slots__ = [] 17 | _slot_types = [] 18 | 19 | def __init__(self, *args, **kwds): 20 | """ 21 | Constructor. Any message fields that are implicitly/explicitly 22 | set to None will be assigned a default value. The recommend 23 | use is keyword arguments as this is more robust to future message 24 | changes. You cannot mix in-order arguments and keyword arguments. 25 | 26 | The available fields are: 27 | 28 | 29 | :param args: complete set of field values, in .msg order 30 | :param kwds: use keyword arguments corresponding to message field names 31 | to set specific fields. 32 | """ 33 | if args or kwds: 34 | super(StopGoal, self).__init__(*args, **kwds) 35 | 36 | def _get_types(self): 37 | """ 38 | internal API method 39 | """ 40 | return self._slot_types 41 | 42 | def serialize(self, buff): 43 | """ 44 | serialize message into buffer 45 | :param buff: buffer, ``StringIO`` 46 | """ 47 | try: 48 | pass 49 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 50 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 51 | 52 | def deserialize(self, str): 53 | """ 54 | unpack serialized message in str into this message instance 55 | :param str: byte array of serialized message, ``str`` 56 | """ 57 | if python3: 58 | codecs.lookup_error("rosmsg").msg_type = self._type 59 | try: 60 | end = 0 61 | return self 62 | except struct.error as e: 63 | raise genpy.DeserializationError(e) # most likely buffer underfill 64 | 65 | 66 | def serialize_numpy(self, buff, numpy): 67 | """ 68 | serialize message with numpy array types into buffer 69 | :param buff: buffer, ``StringIO`` 70 | :param numpy: numpy python module 71 | """ 72 | try: 73 | pass 74 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 75 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 76 | 77 | def deserialize_numpy(self, str, numpy): 78 | """ 79 | unpack serialized message in str into this message instance using numpy for array types 80 | :param str: byte array of serialized message, ``str`` 81 | :param numpy: numpy python module 82 | """ 83 | if python3: 84 | codecs.lookup_error("rosmsg").msg_type = self._type 85 | try: 86 | end = 0 87 | return self 88 | except struct.error as e: 89 | raise genpy.DeserializationError(e) # most likely buffer underfill 90 | 91 | _struct_I = genpy.struct_I 92 | def _get_struct_I(): 93 | global _struct_I 94 | return _struct_I 95 | -------------------------------------------------------------------------------- /python/LfD/franka_gripper/msg/_StopResult.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_gripper/StopResult.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class StopResult(genpy.Message): 11 | _md5sum = "45872d25d65c97743cc71afc6d4e884d" 12 | _type = "franka_gripper/StopResult" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | bool success 16 | string error 17 | """ 18 | __slots__ = ['success','error'] 19 | _slot_types = ['bool','string'] 20 | 21 | def __init__(self, *args, **kwds): 22 | """ 23 | Constructor. Any message fields that are implicitly/explicitly 24 | set to None will be assigned a default value. The recommend 25 | use is keyword arguments as this is more robust to future message 26 | changes. You cannot mix in-order arguments and keyword arguments. 27 | 28 | The available fields are: 29 | success,error 30 | 31 | :param args: complete set of field values, in .msg order 32 | :param kwds: use keyword arguments corresponding to message field names 33 | to set specific fields. 34 | """ 35 | if args or kwds: 36 | super(StopResult, self).__init__(*args, **kwds) 37 | # message fields cannot be None, assign default values for those that are 38 | if self.success is None: 39 | self.success = False 40 | if self.error is None: 41 | self.error = '' 42 | else: 43 | self.success = False 44 | self.error = '' 45 | 46 | def _get_types(self): 47 | """ 48 | internal API method 49 | """ 50 | return self._slot_types 51 | 52 | def serialize(self, buff): 53 | """ 54 | serialize message into buffer 55 | :param buff: buffer, ``StringIO`` 56 | """ 57 | try: 58 | _x = self.success 59 | buff.write(_get_struct_B().pack(_x)) 60 | _x = self.error 61 | length = len(_x) 62 | if python3 or type(_x) == unicode: 63 | _x = _x.encode('utf-8') 64 | length = len(_x) 65 | buff.write(struct.Struct(' 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class ErrorRecoveryFeedback(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_msgs/ErrorRecoveryFeedback" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | 16 | """ 17 | __slots__ = [] 18 | _slot_types = [] 19 | 20 | def __init__(self, *args, **kwds): 21 | """ 22 | Constructor. Any message fields that are implicitly/explicitly 23 | set to None will be assigned a default value. The recommend 24 | use is keyword arguments as this is more robust to future message 25 | changes. You cannot mix in-order arguments and keyword arguments. 26 | 27 | The available fields are: 28 | 29 | 30 | :param args: complete set of field values, in .msg order 31 | :param kwds: use keyword arguments corresponding to message field names 32 | to set specific fields. 33 | """ 34 | if args or kwds: 35 | super(ErrorRecoveryFeedback, self).__init__(*args, **kwds) 36 | 37 | def _get_types(self): 38 | """ 39 | internal API method 40 | """ 41 | return self._slot_types 42 | 43 | def serialize(self, buff): 44 | """ 45 | serialize message into buffer 46 | :param buff: buffer, ``StringIO`` 47 | """ 48 | try: 49 | pass 50 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 51 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 52 | 53 | def deserialize(self, str): 54 | """ 55 | unpack serialized message in str into this message instance 56 | :param str: byte array of serialized message, ``str`` 57 | """ 58 | if python3: 59 | codecs.lookup_error("rosmsg").msg_type = self._type 60 | try: 61 | end = 0 62 | return self 63 | except struct.error as e: 64 | raise genpy.DeserializationError(e) # most likely buffer underfill 65 | 66 | 67 | def serialize_numpy(self, buff, numpy): 68 | """ 69 | serialize message with numpy array types into buffer 70 | :param buff: buffer, ``StringIO`` 71 | :param numpy: numpy python module 72 | """ 73 | try: 74 | pass 75 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 76 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 77 | 78 | def deserialize_numpy(self, str, numpy): 79 | """ 80 | unpack serialized message in str into this message instance using numpy for array types 81 | :param str: byte array of serialized message, ``str`` 82 | :param numpy: numpy python module 83 | """ 84 | if python3: 85 | codecs.lookup_error("rosmsg").msg_type = self._type 86 | try: 87 | end = 0 88 | return self 89 | except struct.error as e: 90 | raise genpy.DeserializationError(e) # most likely buffer underfill 91 | 92 | _struct_I = genpy.struct_I 93 | def _get_struct_I(): 94 | global _struct_I 95 | return _struct_I 96 | -------------------------------------------------------------------------------- /python/LfD/franka_msgs/msg/_ErrorRecoveryGoal.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_msgs/ErrorRecoveryGoal.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class ErrorRecoveryGoal(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_msgs/ErrorRecoveryGoal" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | """ 16 | __slots__ = [] 17 | _slot_types = [] 18 | 19 | def __init__(self, *args, **kwds): 20 | """ 21 | Constructor. Any message fields that are implicitly/explicitly 22 | set to None will be assigned a default value. The recommend 23 | use is keyword arguments as this is more robust to future message 24 | changes. You cannot mix in-order arguments and keyword arguments. 25 | 26 | The available fields are: 27 | 28 | 29 | :param args: complete set of field values, in .msg order 30 | :param kwds: use keyword arguments corresponding to message field names 31 | to set specific fields. 32 | """ 33 | if args or kwds: 34 | super(ErrorRecoveryGoal, self).__init__(*args, **kwds) 35 | 36 | def _get_types(self): 37 | """ 38 | internal API method 39 | """ 40 | return self._slot_types 41 | 42 | def serialize(self, buff): 43 | """ 44 | serialize message into buffer 45 | :param buff: buffer, ``StringIO`` 46 | """ 47 | try: 48 | pass 49 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 50 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 51 | 52 | def deserialize(self, str): 53 | """ 54 | unpack serialized message in str into this message instance 55 | :param str: byte array of serialized message, ``str`` 56 | """ 57 | if python3: 58 | codecs.lookup_error("rosmsg").msg_type = self._type 59 | try: 60 | end = 0 61 | return self 62 | except struct.error as e: 63 | raise genpy.DeserializationError(e) # most likely buffer underfill 64 | 65 | 66 | def serialize_numpy(self, buff, numpy): 67 | """ 68 | serialize message with numpy array types into buffer 69 | :param buff: buffer, ``StringIO`` 70 | :param numpy: numpy python module 71 | """ 72 | try: 73 | pass 74 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 75 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 76 | 77 | def deserialize_numpy(self, str, numpy): 78 | """ 79 | unpack serialized message in str into this message instance using numpy for array types 80 | :param str: byte array of serialized message, ``str`` 81 | :param numpy: numpy python module 82 | """ 83 | if python3: 84 | codecs.lookup_error("rosmsg").msg_type = self._type 85 | try: 86 | end = 0 87 | return self 88 | except struct.error as e: 89 | raise genpy.DeserializationError(e) # most likely buffer underfill 90 | 91 | _struct_I = genpy.struct_I 92 | def _get_struct_I(): 93 | global _struct_I 94 | return _struct_I 95 | -------------------------------------------------------------------------------- /python/LfD/franka_msgs/msg/_ErrorRecoveryResult.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_msgs/ErrorRecoveryResult.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class ErrorRecoveryResult(genpy.Message): 11 | _md5sum = "d41d8cd98f00b204e9800998ecf8427e" 12 | _type = "franka_msgs/ErrorRecoveryResult" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 15 | """ 16 | __slots__ = [] 17 | _slot_types = [] 18 | 19 | def __init__(self, *args, **kwds): 20 | """ 21 | Constructor. Any message fields that are implicitly/explicitly 22 | set to None will be assigned a default value. The recommend 23 | use is keyword arguments as this is more robust to future message 24 | changes. You cannot mix in-order arguments and keyword arguments. 25 | 26 | The available fields are: 27 | 28 | 29 | :param args: complete set of field values, in .msg order 30 | :param kwds: use keyword arguments corresponding to message field names 31 | to set specific fields. 32 | """ 33 | if args or kwds: 34 | super(ErrorRecoveryResult, self).__init__(*args, **kwds) 35 | 36 | def _get_types(self): 37 | """ 38 | internal API method 39 | """ 40 | return self._slot_types 41 | 42 | def serialize(self, buff): 43 | """ 44 | serialize message into buffer 45 | :param buff: buffer, ``StringIO`` 46 | """ 47 | try: 48 | pass 49 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 50 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 51 | 52 | def deserialize(self, str): 53 | """ 54 | unpack serialized message in str into this message instance 55 | :param str: byte array of serialized message, ``str`` 56 | """ 57 | if python3: 58 | codecs.lookup_error("rosmsg").msg_type = self._type 59 | try: 60 | end = 0 61 | return self 62 | except struct.error as e: 63 | raise genpy.DeserializationError(e) # most likely buffer underfill 64 | 65 | 66 | def serialize_numpy(self, buff, numpy): 67 | """ 68 | serialize message with numpy array types into buffer 69 | :param buff: buffer, ``StringIO`` 70 | :param numpy: numpy python module 71 | """ 72 | try: 73 | pass 74 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 75 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 76 | 77 | def deserialize_numpy(self, str, numpy): 78 | """ 79 | unpack serialized message in str into this message instance using numpy for array types 80 | :param str: byte array of serialized message, ``str`` 81 | :param numpy: numpy python module 82 | """ 83 | if python3: 84 | codecs.lookup_error("rosmsg").msg_type = self._type 85 | try: 86 | end = 0 87 | return self 88 | except struct.error as e: 89 | raise genpy.DeserializationError(e) # most likely buffer underfill 90 | 91 | _struct_I = genpy.struct_I 92 | def _get_struct_I(): 93 | global _struct_I 94 | return _struct_I 95 | -------------------------------------------------------------------------------- /python/LfD/franka_msgs/msg/__init__.py: -------------------------------------------------------------------------------- 1 | from ._ErrorRecoveryAction import * 2 | from ._ErrorRecoveryActionFeedback import * 3 | from ._ErrorRecoveryActionGoal import * 4 | from ._ErrorRecoveryActionResult import * 5 | from ._ErrorRecoveryFeedback import * 6 | from ._ErrorRecoveryGoal import * 7 | from ._ErrorRecoveryResult import * 8 | from ._Errors import * 9 | from ._FrankaState import * 10 | -------------------------------------------------------------------------------- /python/LfD/franka_msgs/srv/_SetKFrame.py: -------------------------------------------------------------------------------- 1 | # This Python file uses the following encoding: utf-8 2 | """autogenerated by genpy from franka_msgs/SetKFrameRequest.msg. Do not edit.""" 3 | import codecs 4 | import sys 5 | python3 = True if sys.hexversion > 0x03000000 else False 6 | import genpy 7 | import struct 8 | 9 | 10 | class SetKFrameRequest(genpy.Message): 11 | _md5sum = "f8e38719bdb98c0e8ddafd6da2db480f" 12 | _type = "franka_msgs/SetKFrameRequest" 13 | _has_header = False # flag to mark the presence of a Header object 14 | _full_text = """float64[16] EE_T_K 15 | """ 16 | __slots__ = ['EE_T_K'] 17 | _slot_types = ['float64[16]'] 18 | 19 | def __init__(self, *args, **kwds): 20 | """ 21 | Constructor. Any message fields that are implicitly/explicitly 22 | set to None will be assigned a default value. The recommend 23 | use is keyword arguments as this is more robust to future message 24 | changes. You cannot mix in-order arguments and keyword arguments. 25 | 26 | The available fields are: 27 | EE_T_K 28 | 29 | :param args: complete set of field values, in .msg order 30 | :param kwds: use keyword arguments corresponding to message field names 31 | to set specific fields. 32 | """ 33 | if args or kwds: 34 | super(SetKFrameRequest, self).__init__(*args, **kwds) 35 | # message fields cannot be None, assign default values for those that are 36 | if self.EE_T_K is None: 37 | self.EE_T_K = [0.] * 16 38 | else: 39 | self.EE_T_K = [0.] * 16 40 | 41 | def _get_types(self): 42 | """ 43 | internal API method 44 | """ 45 | return self._slot_types 46 | 47 | def serialize(self, buff): 48 | """ 49 | serialize message into buffer 50 | :param buff: buffer, ``StringIO`` 51 | """ 52 | try: 53 | buff.write(_get_struct_16d().pack(*self.EE_T_K)) 54 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 55 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 56 | 57 | def deserialize(self, str): 58 | """ 59 | unpack serialized message in str into this message instance 60 | :param str: byte array of serialized message, ``str`` 61 | """ 62 | if python3: 63 | codecs.lookup_error("rosmsg").msg_type = self._type 64 | try: 65 | end = 0 66 | start = end 67 | end += 128 68 | self.EE_T_K = _get_struct_16d().unpack(str[start:end]) 69 | return self 70 | except struct.error as e: 71 | raise genpy.DeserializationError(e) # most likely buffer underfill 72 | 73 | 74 | def serialize_numpy(self, buff, numpy): 75 | """ 76 | serialize message with numpy array types into buffer 77 | :param buff: buffer, ``StringIO`` 78 | :param numpy: numpy python module 79 | """ 80 | try: 81 | buff.write(self.EE_T_K.tostring()) 82 | except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self))))) 83 | except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self))))) 84 | 85 | def deserialize_numpy(self, str, numpy): 86 | """ 87 | unpack serialized message in str into this message instance using numpy for array types 88 | :param str: byte array of serialized message, ``str`` 89 | :param numpy: numpy python module 90 | """ 91 | if python3: 92 | codecs.lookup_error("rosmsg").msg_type = self._type 93 | try: 94 | end = 0 95 | start = end 96 | end += 128 97 | self.EE_T_K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=16) 98 | return self 99 | except struct.error as e: 100 | raise genpy.DeserializationError(e) # most likely buffer underfill 101 | 102 | _struct_I = genpy.struct_I 103 | def _get_struct_I(): 104 | global _struct_I 105 | return _struct_I 106 | _struct_16d = None 107 | def _get_struct_16d(): 108 | global _struct_16d 109 | if _struct_16d is None: 110 | _struct_16d = struct.Struct("<16d") 111 | return _struct_16d 112 | # This Python file uses the following encoding: utf-8 113 | """autogenerated by genpy from franka_msgs/SetKFrameResponse.msg. Do not edit.""" 114 | import codecs 115 | import sys 116 | python3 = True if sys.hexversion > 0x03000000 else False 117 | import genpy 118 | import struct 119 | 120 | 121 | class SetKFrameResponse(genpy.Message): 122 | _md5sum = "45872d25d65c97743cc71afc6d4e884d" 123 | _type = "franka_msgs/SetKFrameResponse" 124 | _has_header = False # flag to mark the presence of a Header object 125 | _full_text = """bool success 126 | string error 127 | 128 | 129 | """ 130 | __slots__ = ['success','error'] 131 | _slot_types = ['bool','string'] 132 | 133 | def __init__(self, *args, **kwds): 134 | """ 135 | Constructor. Any message fields that are implicitly/explicitly 136 | set to None will be assigned a default value. The recommend 137 | use is keyword arguments as this is more robust to future message 138 | changes. You cannot mix in-order arguments and keyword arguments. 139 | 140 | The available fields are: 141 | success,error 142 | 143 | :param args: complete set of field values, in .msg order 144 | :param kwds: use keyword arguments corresponding to message field names 145 | to set specific fields. 146 | """ 147 | if args or kwds: 148 | super(SetKFrameResponse, self).__init__(*args, **kwds) 149 | # message fields cannot be None, assign default values for those that are 150 | if self.success is None: 151 | self.success = False 152 | if self.error is None: 153 | self.error = '' 154 | else: 155 | self.success = False 156 | self.error = '' 157 | 158 | def _get_types(self): 159 | """ 160 | internal API method 161 | """ 162 | return self._slot_types 163 | 164 | def serialize(self, buff): 165 | """ 166 | serialize message into buffer 167 | :param buff: buffer, ``StringIO`` 168 | """ 169 | try: 170 | _x = self.success 171 | buff.write(_get_struct_B().pack(_x)) 172 | _x = self.error 173 | length = len(_x) 174 | if python3 or type(_x) == unicode: 175 | _x = _x.encode('utf-8') 176 | length = len(_x) 177 | buff.write(struct.Struct(' 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace franka_human_friendly_controllers { 13 | 14 | 15 | void CartesianVariableImpedanceExternalModelController::loadModel() { 16 | std::string package_path = ros::package::getPath("franka_human_friendly_controllers"); 17 | urdf_path_ = package_path + "/urdf/panda_calibrated.urdf"; 18 | ros::param::get("frame_name", frame_name_); 19 | 20 | std::cout << "Loading urdf into pinocchio as we are using the urdf model" << std::endl; 21 | pinocchio::urdf::buildModel(urdf_path_, model_pin_); 22 | frame_id_ = model_pin_.getFrameId(frame_name_); 23 | data_pin_ = new pinocchio::Data(model_pin_); 24 | std::cout << "Succesfully loaded the model and created the data pointer." << std::endl; 25 | } 26 | 27 | double* CartesianVariableImpedanceExternalModelController::get_fk(franka::RobotState robot_state) 28 | { 29 | Eigen::Map> q(robot_state.q.data()); 30 | Eigen::VectorXd q_vector = Eigen::VectorXd::Map(q.data(), q.size()); 31 | 32 | 33 | pinocchio::forwardKinematics(model_pin_, *data_pin_, q_vector); 34 | pinocchio::updateFramePlacement(model_pin_, *data_pin_, frame_id_); 35 | const auto& transformation = data_pin_->oMf[frame_id_]; // Get the transformation of the frame 36 | 37 | // Allocate memory for the result 38 | double* result = new double[16]; 39 | std::memcpy(result, transformation.toHomogeneousMatrix().data(), 16 * sizeof(double)); 40 | return result; // Caller is responsible for deleting the allocated memory 41 | } 42 | 43 | std::array CartesianVariableImpedanceExternalModelController::get_jacobian(franka::RobotState robot_state) 44 | { 45 | Eigen::Map> q(robot_state.q.data()); 46 | Eigen::VectorXd q_vector = Eigen::VectorXd::Map(q.data(), q.size()); 47 | Eigen::MatrixXd jacobian(6, model_pin_.nv); // 6xnv matrix for spatial Jacobian 48 | jacobian.fill(0); // Initialize to zero 49 | 50 | 51 | pinocchio::forwardKinematics(model_pin_, *data_pin_, q_vector); 52 | pinocchio::computeJointJacobians(model_pin_, *data_pin_, q_vector); 53 | //pinocchio::updateFramePlacements(model_pin_, *data_pin_); 54 | pinocchio::getFrameJacobian(model_pin_, *data_pin_, frame_id_, pinocchio::LOCAL_WORLD_ALIGNED, jacobian); 55 | std::array result; 56 | std::memcpy(result.data(), jacobian.data(), 42 * sizeof(double)); 57 | return result; 58 | } 59 | 60 | } // namespace franka_human_friendly_controllers 61 | 62 | PLUGINLIB_EXPORT_CLASS( 63 | franka_human_friendly_controllers::CartesianVariableImpedanceExternalModelController, 64 | controller_interface::ControllerBase 65 | ) 66 | -------------------------------------------------------------------------------- /urdf/panda_calibrated.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | --------------------------------------------------------------------------------