├── requirements.txt ├── panda_simulator ├── CMakeLists.txt └── package.xml ├── panda_gazebo ├── setup.py ├── panda_robot_hw_sim_plugins.xml ├── worlds │ └── panda.world ├── startup_scripts │ └── force_neutral_pose.py ├── src │ ├── panda_gazebo_ros_control_plugin.cpp │ └── panda_robot_hw_sim.cpp ├── include │ └── panda_gazebo │ │ ├── panda_gazebo_ros_control_plugin.h │ │ ├── kdl_methods.h │ │ ├── panda_robot_hw_sim.h │ │ ├── arm_controller_interface.h │ │ └── arm_kinematics_interface.h ├── package.xml ├── CMakeLists.txt ├── config │ └── robot_details.yaml └── launch │ └── panda_world.launch ├── panda_simulator_examples ├── setup.py ├── launch │ ├── demo_sim_controller_test.launch │ ├── demo_task_space_control.launch │ └── demo_moveit.launch ├── CMakeLists.txt ├── package.xml └── scripts │ ├── move_robot.py │ ├── rviz_markers.py │ ├── config │ └── rviz_configs.rviz │ └── task_space_control_with_fri.py ├── docker ├── ros_entrypoint.sh └── Dockerfile ├── panda_sim_moveit ├── CMakeLists.txt ├── package.xml └── launch │ ├── moveit_planner_gui.launch │ └── sim_move_group.launch ├── panda_sim_custom_action_server ├── setup.py ├── src │ └── panda_sim_custom_action_server │ │ ├── __init__.py │ │ ├── pid.py │ │ └── gripper_action_server.py ├── CMakeLists.txt ├── package.xml ├── scripts │ ├── start_gripper_action_server.py │ └── start_joint_trajectory_server.py └── cfg │ └── PandaPositionFFJointTrajectoryActionServer.cfg ├── .gitignore ├── panda_sim_controllers ├── config │ ├── panda_gripper_controller.yaml │ ├── panda_sim_controllers.yaml │ └── panda_sim_controllers_plugins.xml ├── package.xml ├── CMakeLists.txt ├── include │ └── panda_sim_controllers │ │ ├── panda_effort_controller.h │ │ ├── panda_gravity_controller.h │ │ ├── panda_velocity_controller.h │ │ ├── panda_position_controller.h │ │ ├── panda_joint_effort_controller.h │ │ ├── panda_gripper_controller.h │ │ ├── joint_array_controller.h │ │ ├── panda_joint_velocity_controller.h │ │ └── panda_joint_position_controller.h ├── launch │ └── panda_sim_controllers.launch └── src │ ├── panda_effort_controller.cpp │ ├── panda_gravity_controller.cpp │ ├── panda_joint_effort_controller.cpp │ ├── panda_velocity_controller.cpp │ └── panda_joint_velocity_controller.cpp ├── panda_hardware_interface ├── CMakeLists.txt ├── package.xml └── include │ └── panda_hardware_interface │ ├── sum_command_interface.h │ └── shared_joint_interface.h ├── NOTICE ├── dependencies.rosinstall ├── run_docker.sh ├── versionLog.md └── .travis.yml /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | numpy-quaternion==2020.5.11.13.33.35 -------------------------------------------------------------------------------- /panda_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(panda_simulator) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /panda_gazebo/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup() 6 | 7 | setup(**d) 8 | -------------------------------------------------------------------------------- /panda_simulator_examples/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup() 6 | 7 | setup(**d) 8 | -------------------------------------------------------------------------------- /docker/ros_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # setup ros environment 5 | source "/opt/ros/$ROS_DISTRO/setup.bash" 6 | 7 | export ROS_HOSTNAME=localhost 8 | export ROS_MASTER_URI=http://localhost:11311 9 | 10 | exec "$@" 11 | -------------------------------------------------------------------------------- /panda_simulator_examples/launch/demo_sim_controller_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /panda_sim_moveit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(panda_sim_moveit) 3 | 4 | find_package(catkin 5 | REQUIRED 6 | COMPONENTS 7 | rospy 8 | panda_moveit_config 9 | ) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS 13 | rospy 14 | panda_moveit_config 15 | ) 16 | 17 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from distutils.core import setup 3 | from catkin_pkg.python_setup import generate_distutils_setup 4 | 5 | d = generate_distutils_setup() 6 | d['packages'] = ['panda_sim_custom_action_server'] 7 | d['package_dir'] = {'': 'src'} 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /panda_gazebo/panda_robot_hw_sim_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | The Panda robot simulation interface which constructs joint handles from an SDF/URDF. 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | *.py[cod] 8 | __pycache__ 9 | # Precompiled Headers 10 | *.gch 11 | *.pch 12 | 13 | # Compiled Dynamic libraries 14 | *.so 15 | *.dylib 16 | *.dll 17 | 18 | # Fortran module files 19 | *.mod 20 | 21 | # Compiled Static libraries 22 | *.lai 23 | *.la 24 | *.a 25 | *.lib 26 | 27 | # Executables 28 | *.exe 29 | *.out 30 | *.app 31 | 32 | # Linux 33 | *.swp 34 | -------------------------------------------------------------------------------- /panda_sim_controllers/config/panda_gripper_controller.yaml: -------------------------------------------------------------------------------- 1 | 2 | panda_gripper_controller: 3 | type: panda_sim_controllers/PandaGripperController 4 | pub_topic: /franka_gripper/joint_states 5 | joints: 6 | # main joint 7 | panda_finger_joint1_controller: 8 | joint: panda_finger_joint1 9 | pid: {p: 5000, i: 50, d: 0.5} 10 | # mimic joint 11 | panda_finger_joint2_controller: 12 | joint: panda_finger_joint2 13 | pid: {p: 5000, i: 50, d: 0.5} 14 | -------------------------------------------------------------------------------- /panda_gazebo/worlds/panda.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | 13 | 1000.0 14 | 15 | 16 | 17 | 0.0 0.0 0.0 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /panda_hardware_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(panda_hardware_interface) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | add_definitions(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | hardware_interface 9 | roscpp 10 | ) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include 14 | CATKIN_DEPENDS hardware_interface roscpp 15 | ) 16 | 17 | include_directories( 18 | include 19 | ${catkin_INCLUDE_DIRS} 20 | ) 21 | 22 | ## Mark cpp header files for installation 23 | install(DIRECTORY include/${PROJECT_NAME}/ 24 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 25 | ) 26 | 27 | -------------------------------------------------------------------------------- /NOTICE: -------------------------------------------------------------------------------- 1 | 2 | panda_simulator package 3 | 4 | Copyright 2019-2021 Saif Sidhik 5 | 6 | Licensed under the Apache License, Version 2.0 (the "License"); 7 | you may not use this file except in compliance with the License. 8 | You may obtain a copy of the License at 9 | 10 | http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | Unless required by applicable law or agreed to in writing, software 13 | distributed under the License is distributed on an "AS IS" BASIS, 14 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | See the License for the specific language governing permissions and 16 | limitations under the License. 17 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: franka_ros 3 | uri: https://github.com/frankaemika/franka_ros 4 | version: noetic-devel 5 | - git: 6 | local-name: panda_moveit_config 7 | uri: https://github.com/ros-planning/panda_moveit_config 8 | version: melodic-devel 9 | - git: 10 | local-name: franka_ros_interface 11 | uri: https://github.com/JustaGist/franka_ros_interface 12 | version: v0.7.1-dev 13 | - git: 14 | local-name: franka_panda_description 15 | uri: https://github.com/JustaGist/franka_panda_description 16 | version: master 17 | - git: 18 | local-name: orocos_kinematics_dynamics 19 | uri: https://github.com/orocos/orocos_kinematics_dynamics 20 | version: master -------------------------------------------------------------------------------- /panda_sim_moveit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_sim_moveit 4 | 1.0.0 5 | 6 | MoveIt support for Panda Simulator 7 | 8 | 9 | 10 | Saif Sidhik 11 | 12 | Apache 2.0 13 | 14 | https://github.com/Justagist/panda_simulator 15 | 16 | 17 | catkin 18 | rospy 19 | panda_moveit_config 20 | 21 | rospy 22 | panda_moveit_config 23 | xacro 24 | 25 | 26 | -------------------------------------------------------------------------------- /panda_hardware_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_hardware_interface 4 | 1.0.0 5 | The panda_hardware_interface package 6 | 7 | 8 | Saif Sidhik 9 | 10 | Apache 2.0 11 | 12 | https://github.com/Justagist/panda_simulator 13 | 14 | 15 | Saif Sidhik 16 | 17 | catkin 18 | hardware_interface 19 | roscpp 20 | hardware_interface 21 | roscpp 22 | 23 | 24 | -------------------------------------------------------------------------------- /panda_simulator_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(panda_simulator_examples) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | franka_core_msgs 7 | gazebo_msgs 8 | franka_moveit 9 | panda_sim_moveit 10 | panda_gazebo 11 | ) 12 | 13 | catkin_package( 14 | CATKIN_DEPENDS 15 | rospy 16 | franka_core_msgs 17 | gazebo_msgs 18 | franka_moveit 19 | panda_sim_moveit 20 | panda_gazebo 21 | ) 22 | 23 | catkin_python_setup() 24 | catkin_install_python(PROGRAMS scripts/move_robot.py 25 | scripts/task_space_control_using_sim_only.py 26 | scripts/task_space_control_with_fri.py 27 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 28 | -------------------------------------------------------------------------------- /panda_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_simulator 4 | 1.0.0 5 | 6 | Metapackage - Panda Robot Gazebo Simulation and Controllers 7 | 8 | 9 | 10 | Saif Sidhik 11 | 12 | Apache 2.0 13 | 14 | https://github.com/Justagist/panda_simulator 15 | 16 | 17 | Saif Sidhik 18 | 19 | catkin 20 | panda_gazebo 21 | panda_sim_controllers 22 | panda_simulator_examples 23 | panda_hardware_interface 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/src/panda_sim_custom_action_server/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | # /*************************************************************************** 3 | # Copyright (c) 2019-2021, Saif Sidhik 4 | 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # **************************************************************************/ 17 | 18 | from .joint_trajectory_action_server import JointTrajectoryActionServer 19 | from .gripper_action_server import GripperActionServer 20 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(panda_sim_custom_action_server) 3 | 4 | find_package(catkin 5 | REQUIRED 6 | COMPONENTS 7 | rospy 8 | actionlib 9 | sensor_msgs 10 | std_msgs 11 | control_msgs 12 | trajectory_msgs 13 | dynamic_reconfigure 14 | franka_core_msgs 15 | franka_interface 16 | panda_moveit_config 17 | ) 18 | 19 | catkin_python_setup() 20 | 21 | generate_dynamic_reconfigure_options( 22 | cfg/PandaPositionFFJointTrajectoryActionServer.cfg 23 | ) 24 | 25 | add_dependencies(${PROJECT_NAME}_gencfg franka_core_msgs_generate_messages_py) 26 | 27 | catkin_package( 28 | CATKIN_DEPENDS 29 | rospy 30 | actionlib 31 | sensor_msgs 32 | std_msgs 33 | control_msgs 34 | trajectory_msgs 35 | dynamic_reconfigure 36 | franka_core_msgs 37 | franka_interface 38 | panda_moveit_config 39 | ) 40 | 41 | catkin_install_python(PROGRAMS scripts/start_gripper_action_server.py 42 | scripts/start_joint_trajectory_server.py 43 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 44 | -------------------------------------------------------------------------------- /panda_simulator_examples/launch/demo_task_space_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /panda_gazebo/startup_scripts/force_neutral_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from franka_core_msgs.msg import JointCommand 5 | 6 | if __name__ == "__main__": 7 | 8 | rospy.init_node("force_neutral_pose_startup") 9 | 10 | pub = rospy.Publisher('panda_simulator/motion_controller/arm/joint_commands', 11 | JointCommand, 12 | tcp_nodelay=True, 13 | queue_size=10) 14 | 15 | command_msg = JointCommand() 16 | command_msg.names = ["panda_joint%d" % (idx) for idx in range(1, 8)] 17 | command_msg.position = [0.000,- 0.785,0.0,- 2.356,0.0,1.57,0.785] 18 | command_msg.mode = JointCommand.POSITION_MODE 19 | 20 | 21 | rospy.sleep(0.5) 22 | start = rospy.Time.now().to_sec() 23 | 24 | rospy.loginfo("Attempting to force robot to neutral pose...") 25 | rospy.sleep(0.5) 26 | 27 | while not rospy.is_shutdown() and (rospy.Time.now().to_sec() - start < 1.): 28 | # print rospy.Time.now() 29 | command_msg.header.stamp = rospy.Time.now() 30 | pub.publish(command_msg) 31 | 32 | rospy.loginfo("Robot forced to neutral pose. Complete!") 33 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | # FROM osrf/ros:kinetic-desktop-full-xenial 2 | FROM osrf/ros:noetic-desktop-full 3 | 4 | ENV ROS_DISTRO=${ROS_DISTRO} 5 | 6 | RUN apt-get update && apt-get install -q -y \ 7 | build-essential git swig sudo python3-future libcppunit-dev python3-pip 8 | 9 | RUN apt-get update && apt-get install -y \ 10 | ros-$ROS_DISTRO-libfranka \ 11 | python3-catkin-tools ros-$ROS_DISTRO-gazebo-ros-control \ 12 | ros-${ROS_DISTRO}-rospy-message-converter ros-${ROS_DISTRO}-effort-controllers \ 13 | ros-${ROS_DISTRO}-joint-state-controller \ 14 | ros-${ROS_DISTRO}-moveit ros-${ROS_DISTRO}-moveit-commander \ 15 | ros-${ROS_DISTRO}-moveit-visual-tools ros-${ROS_DISTRO}-combined-robot-hw 16 | 17 | RUN pip3 install --upgrade numpy numpy-quaternion==2020.5.11.13.33.35 osrf-pycommon 18 | 19 | RUN apt-get update && apt-get upgrade -y 20 | 21 | # nvidia-container-runtime 22 | ENV NVIDIA_VISIBLE_DEVICES \ 23 | ${NVIDIA_VISIBLE_DEVICES:-all} 24 | ENV NVIDIA_DRIVER_CAPABILITIES \ 25 | ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics 26 | 27 | LABEL PS_VERSION v1.0.0-noetic 28 | 29 | # setup entrypoint, need entrypoint.sh in the same folder with Dockerfile 30 | COPY ./ros_entrypoint.sh / 31 | ENTRYPOINT ["/ros_entrypoint.sh"] 32 | 33 | CMD ["bash"] -------------------------------------------------------------------------------- /panda_simulator_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_simulator_examples 4 | 1.0.0 5 | The panda_simulator_examples package 6 | 7 | 8 | Saif Sidhik 9 | 10 | Apache 2.0 11 | 12 | 13 | https://github.com/JustaGist/panda_simulator 14 | 15 | 16 | Saif Sidhik 17 | 18 | catkin 19 | 20 | rospy 21 | franka_core_msgs 22 | gazebo_msgs 23 | franka_moveit 24 | panda_sim_moveit 25 | panda_gazebo 26 | 27 | rospy 28 | rospack 29 | franka_core_msgs 30 | panda_gazebo 31 | gazebo_ros 32 | gazebo_msgs 33 | franka_moveit 34 | panda_sim_moveit 35 | panda_gazebo 36 | 37 | 38 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_sim_custom_action_server 4 | 1.0.0 5 | 6 | Custom action server for panda simulator 7 | 8 | 9 | 10 | Saif Sidhik 11 | 12 | Apache 2.0 13 | 14 | https://github.com/Justagist/panda_simulator 15 | 16 | 17 | catkin 18 | rospy 19 | actionlib 20 | franka_core_msgs 21 | control_msgs 22 | dynamic_reconfigure 23 | sensor_msgs 24 | std_msgs 25 | trajectory_msgs 26 | franka_interface 27 | panda_moveit_config 28 | 29 | rospy 30 | actionlib 31 | franka_core_msgs 32 | control_msgs 33 | dynamic_reconfigure 34 | sensor_msgs 35 | std_msgs 36 | trajectory_msgs 37 | rospy_message_converter 38 | franka_interface 39 | panda_moveit_config 40 | xacro 41 | 42 | 43 | -------------------------------------------------------------------------------- /panda_sim_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_sim_controllers 4 | 1.0.0 5 | 6 | Panda-specific controllers for Gazebo use 7 | 8 | 9 | 10 | Saif Sidhik 11 | 12 | Apache 2.0 13 | 14 | https://github.com/Justagist/panda_simulator 15 | 16 | 17 | Saif Sidhik 18 | 19 | catkin 20 | effort_controllers 21 | controller_interface 22 | roscpp 23 | realtime_tools 24 | franka_core_msgs 25 | roslint 26 | panda_hardware_interface 27 | franka_gripper 28 | control_msgs 29 | actionlib 30 | actionlib_msgs 31 | 32 | effort_controllers 33 | roscpp 34 | realtime_tools 35 | franka_gripper 36 | actionlib 37 | actionlib_msgs 38 | franka_core_msgs 39 | controller_interface 40 | panda_hardware_interface 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /panda_sim_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(panda_sim_controllers) 3 | 4 | ## Add support for C++14, supported in ROS Kinetic and newer 5 | add_definitions(-std=c++14) 6 | 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | effort_controllers 10 | roscpp 11 | roslint 12 | franka_core_msgs 13 | franka_gripper 14 | actionlib 15 | realtime_tools 16 | panda_hardware_interface 17 | actionlib_msgs 18 | ) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | LIBRARIES panda_sim_controllers 23 | CATKIN_DEPENDS panda_hardware_interface roscpp actionlib actionlib_msgs 24 | # DEPENDS Franka 25 | ) 26 | roslint_cpp() 27 | 28 | include_directories(include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | ## Declare a C++ library 33 | add_library(${PROJECT_NAME} 34 | src/panda_position_controller.cpp 35 | src/panda_velocity_controller.cpp 36 | src/panda_effort_controller.cpp 37 | src/panda_gravity_controller.cpp 38 | src/panda_joint_position_controller.cpp 39 | src/panda_joint_velocity_controller.cpp 40 | src/panda_joint_effort_controller.cpp 41 | src/panda_gripper_controller.cpp 42 | ) 43 | 44 | set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14) 45 | 46 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 47 | 48 | target_link_libraries(${PROJECT_NAME} 49 | ${catkin_LIBRARIES} 50 | ) 51 | 52 | install(TARGETS ${PROJECT_NAME} 53 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | ) 55 | install(DIRECTORY config launch 56 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 57 | ) 58 | install(DIRECTORY include/${PROJECT_NAME}/ 59 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 60 | ) 61 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/scripts/start_gripper_action_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # /*************************************************************************** 4 | 5 | # 6 | # @package: panda_joint_trajectory_action 7 | # @metapackage: panda_simulator 8 | # @author: Saif Sidhik 9 | # 10 | 11 | # **************************************************************************/ 12 | 13 | # /*************************************************************************** 14 | # Copyright (c) 2019-2021, Saif Sidhik 15 | 16 | # Licensed under the Apache License, Version 2.0 (the "License"); 17 | # you may not use this file except in compliance with the License. 18 | # You may obtain a copy of the License at 19 | 20 | # http://www.apache.org/licenses/LICENSE-2.0 21 | 22 | # Unless required by applicable law or agreed to in writing, software 23 | # distributed under the License is distributed on an "AS IS" BASIS, 24 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | # See the License for the specific language governing permissions and 26 | # limitations under the License. 27 | # **************************************************************************/ 28 | 29 | import rospy 30 | 31 | from panda_sim_custom_action_server import GripperActionServer 32 | 33 | def start_server(): 34 | rospy.loginfo("Initializing node... ") 35 | rospy.init_node("panda_gripper_action_server") 36 | rospy.loginfo("Initializing gripper action server...") 37 | 38 | GripperActionServer() 39 | rospy.spin() 40 | rospy.loginfo("Gripper action server running...") 41 | 42 | 43 | def main(): 44 | start_server() 45 | 46 | 47 | if __name__ == "__main__": 48 | main() 49 | -------------------------------------------------------------------------------- /run_docker.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | ROOT_DIR="$(cd $( dirname ${BASH_SOURCE[0]} ) && pwd)" 3 | 4 | command_exists () { 5 | type "$1" &> /dev/null ; 6 | } 7 | distro_type="noetic" 8 | 9 | # path where the catkin ws will be stored for the docker to use 10 | HOST_WS_PATH="$HOME/.panda_sim_${distro_type}_ws" 11 | 12 | if command_exists nvidia-docker; then 13 | extra_params="--runtime nvidia" 14 | xdocker="nvidia-docker" 15 | echo -e "\t[INFO] nvidia-docker exists" 16 | else 17 | xdocker="docker" 18 | extra_params="" 19 | echo -e "\t[INFO] nvidia-docker does not exist (falling back to docker). Rviz and Gazebo most likely will not work!" 20 | fi 21 | 22 | IMAGE_NAME=`docker images -f "label=PS_VERSION=v1.0.0-${distro_type}" -q` 23 | IMAGE_NAME=($IMAGE_NAME dummy) # make it into list for dealing with single and multiple entry from above output 24 | 25 | echo -e "\n\t[STATUS] Loading image: ${IMAGE_NAME[0]} ..." 26 | 27 | if ! [ -d "${HOST_WS_PATH}/src" ]; then 28 | mkdir -p ${HOST_WS_PATH}/src 29 | fi 30 | 31 | $xdocker run -it \ 32 | --user=$(id -u) \ 33 | --env="DISPLAY" \ 34 | --network="host" \ 35 | --privileged \ 36 | --env="QT_X11_NO_MITSHM=1" \ 37 | --volume="/home/$USER:/home/$USER" \ 38 | --volume="/etc/group:/etc/group:ro" \ 39 | --volume="/etc/passwd:/etc/passwd:ro" \ 40 | --volume="/etc/shadow:/etc/shadow:ro" \ 41 | --volume="/etc/sudoers.d:/etc/sudoers.d:ro" \ 42 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 43 | --volume="${HOST_WS_PATH}:/home/$USER/panda_sim_ws" \ 44 | --volume="${ROOT_DIR}:/home/$USER/panda_sim_ws/src/panda_simulator" ${extra_params} \ 45 | --workdir="/home/$USER/panda_sim_ws/" \ 46 | ${IMAGE_NAME[0]} \ 47 | bash 48 | 49 | rm -rf $HOME/panda_sim_ws 50 | echo -e "\nBye the byee!\n" 51 | -------------------------------------------------------------------------------- /panda_gazebo/src/panda_gazebo_ros_control_plugin.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_gazebo_ros_control_plugin.cpp (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_gazebo 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | // Overload the default plugin 30 | #include 31 | 32 | namespace panda_gazebo { 33 | 34 | void PandaGazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) { 35 | GazeboRosControlPlugin::Load(parent, sdf); 36 | // assembly_interface_.init(model_nh_); 37 | arm_controller_interface_.init(model_nh_, controller_manager_); 38 | arm_kinematics_interface_.init(model_nh_); 39 | // head_interface_.init(model_nh_, controller_manager_); 40 | } 41 | 42 | // register the plugin with gazebo 43 | GZ_REGISTER_MODEL_PLUGIN(PandaGazeboRosControlPlugin); 44 | } 45 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/cfg/PandaPositionFFJointTrajectoryActionServer.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | from dynamic_reconfigure.parameter_generator_catkin import ( 18 | ParameterGenerator, 19 | double_t, 20 | ) 21 | 22 | gen = ParameterGenerator() 23 | 24 | gen.add( 25 | 'goal_time', double_t, 0, 26 | "Amount of time (s) controller is permitted to be late achieving goal", 27 | 0.1, 0.0, 120.0, 28 | ) 29 | gen.add( 30 | 'stopped_velocity_tolerance', double_t, 0, 31 | "Maximum velocity (m/s) at end of trajectory to be considered stopped", 32 | 0.20, -1.0, 1.0, 33 | ) 34 | 35 | joints = ('panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5','panda_joint6', 'panda_joint7',) 36 | 37 | params = ('_goal', '_trajectory',) 38 | msg = ( 39 | " - maximum final error", 40 | " - maximum error during trajectory execution", 41 | ) 42 | min = (-0.5, -0.5,) 43 | default = (-0.2, 0.2,) 44 | max = (0.5, 0.5,) 45 | 46 | for idx, param in enumerate(params): 47 | for joint in joints: 48 | gen.add( 49 | joint + param, double_t, 0, joint + msg[idx], 50 | default[idx], min[idx], max[idx] 51 | ) 52 | 53 | exit(gen.generate('panda_joint_trajectory_action', '', 'PandaPositionFFJointTrajectoryActionServer')) 54 | -------------------------------------------------------------------------------- /panda_simulator_examples/launch/demo_moveit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /panda_gazebo/include/panda_gazebo/panda_gazebo_ros_control_plugin.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_gazebo_ros_interface.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_gazebo 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | #ifndef _PANDA_GAZEBO___PANDA_GAZEBO_ROS_CONTROL_PLUGIN_H_ 29 | #define _PANDA_GAZEBO___PANDA_GAZEBO_ROS_CONTROL_PLUGIN_H_ 30 | // Overload the default plugin 31 | #include 32 | #include 33 | #include 34 | 35 | 36 | namespace panda_gazebo { 37 | 38 | class PandaGazeboRosControlPlugin : public gazebo_ros_control::GazeboRosControlPlugin 39 | { 40 | private: 41 | // AssemblyInterface assembly_interface_; 42 | ArmControllerInterface arm_controller_interface_; 43 | ArmKinematicsInterface arm_kinematics_interface_; 44 | // HeadInterface head_interface_; 45 | 46 | protected: 47 | void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf); 48 | 49 | 50 | }; 51 | } 52 | #endif // #ifndef __PANDA_GAZEBO__PANDA_GAZEBO_ROS_CONTROL_PLUGIN_H_ 53 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_effort_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_effort_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #ifndef PANDA_EFFORT_CONTROLLER_H 30 | #define PANDA_EFFORT_CONTROLLER_H 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace panda_sim_controllers 40 | { 41 | class PandaEffortController : public panda_sim_controllers::JointArrayController 42 | { 43 | public: 44 | virtual ~PandaEffortController() {sub_joint_command_.shutdown();} 45 | virtual bool init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n); 46 | void setCommands(); 47 | 48 | private: 49 | ros::Subscriber sub_joint_command_; 50 | 51 | void jointCommandCB(const franka_core_msgs::JointCommandConstPtr& msg); 52 | }; 53 | } 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /panda_sim_controllers/launch/panda_sim_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 18 | 21 | 22 | 28 | 29 | 30 | 32 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /panda_sim_moveit/launch/moveit_planner_gui.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /panda_gazebo/include/panda_gazebo/kdl_methods.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Methods from orocos_kdl library. Isolated to avoid memory leak bugs. 3 | * Removing sns_ik_lib dependency 4 | 5 | * 6 | * @package: panda_gazebo 7 | * @metapackage: panda_simulator 8 | * @author: Saif Sidhik 9 | * 10 | 11 | **************************************************************************/ 12 | 13 | #ifndef PANDA_GAZEBO_KDL_METHODS_H 14 | #define PANDA_GAZEBO_KDL_METHODS_H 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace panda_gazebo 32 | { 33 | class KDLMethods{ 34 | 35 | public: 36 | 37 | bool initialise(KDL::Chain& chain); 38 | virtual int PosFKJntToCart(const KDL::JntArray& q_in, KDL::Frame& p_out, int seg_nr); 39 | virtual int RNECartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const KDL::Wrenches& f_ext,KDL::JntArray &torques, const KDL::Twist ag); 40 | 41 | virtual int JacobianJntToJac(const KDL::JntArray& q_in, KDL::Jacobian& jac, int seg_nr=-1); 42 | 43 | virtual int JntToCoriolis(const KDL::JntArray &q, const KDL::JntArray &q_dot, KDL::JntArray &coriolis); 44 | virtual int JntToMass(const KDL::JntArray &q, KDL::JntSpaceInertiaMatrix& H); 45 | virtual int JntToGravity(const KDL::JntArray &q,KDL::JntArray &gravity); 46 | 47 | private: 48 | KDL::Chain chain_; 49 | unsigned int nj; 50 | unsigned int ns; 51 | std::vector X; 52 | std::vector S; 53 | std::vector v; 54 | std::vector a; 55 | std::vector f; 56 | KDL::Twist g_ag; 57 | KDL::Twist c_ag; 58 | 59 | KDL::Twist t_tmp; 60 | KDL::Frame T_tmp; 61 | std::vector locked_joints_; 62 | 63 | int nr; 64 | KDL::Vector grav; 65 | KDL::Vector vectornull; 66 | KDL::JntArray jntarraynull; 67 | std::vector wrenchnull; 68 | std::vector > Ic; 69 | KDL::Wrench F; 70 | }; 71 | 72 | } // namespace panda_gazebo 73 | 74 | #endif // PANDA_GAZEBO_KDL_METHODS_H -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_gravity_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_gravity_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #ifndef PANDA_GRAVITY_CONTROLLER_H 30 | #define PANDA_GRAVITY_CONTROLLER_H 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | namespace panda_sim_controllers 43 | { 44 | class PandaGravityController : public panda_sim_controllers::JointArrayController 45 | { 46 | public: 47 | PandaGravityController() : panda_sim_controllers::JointArrayController() { }; 48 | 49 | virtual ~PandaGravityController() {/*sub_joint_command_.shutdown();*/} 50 | virtual bool init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n) override; 51 | void setCommands(); 52 | 53 | private: 54 | ros::Subscriber sub_joint_command_; 55 | 56 | std::vector joint_names_; 57 | 58 | void gravityCommandCB(const franka_core_msgs::RobotStateConstPtr& msg); 59 | void gravityDisableCB(const std_msgs::Empty& msg); 60 | }; 61 | } 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /panda_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | panda_gazebo 4 | 1.0.0 5 | The panda_gazebo package for simulating the Panda 6 | Robot from Franka Emika. 7 | 8 | 9 | 10 | Saif Sidhik 11 | 12 | Apache 2.0 13 | 14 | 15 | https://github.com/JustaGist/panda_simulator 16 | 17 | 18 | Saif Sidhik 19 | 20 | catkin 21 | eigen 22 | cv_bridge 23 | gazebo_ros 24 | gazebo_ros_control 25 | joint_state_controller 26 | franka_core_msgs 27 | franka_tools 28 | image_transport 29 | libgazebo11-dev 30 | realtime_tools 31 | roscpp 32 | std_msgs 33 | kdl_parser 34 | tf_conversions 35 | roslint 36 | orocos_kdl 37 | panda_hardware_interface 38 | panda_sim_controllers 39 | panda_sim_custom_action_server 40 | 41 | cv_bridge 42 | gazebo 43 | gazebo_ros 44 | gazebo_ros_control 45 | joint_state_controller 46 | franka_core_msgs 47 | franka_tools 48 | image_transport 49 | realtime_tools 50 | roscpp 51 | franka_panda_description 52 | std_msgs 53 | kdl_parser 54 | tf_conversions 55 | panda_hardware_interface 56 | panda_sim_controllers 57 | panda_sim_custom_action_server 58 | image_proc 59 | orocos_kdl 60 | nodelet 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/src/panda_sim_custom_action_server/pid.py: -------------------------------------------------------------------------------- 1 | # Modified example source code from the book 2 | # "Real-World Instrumentation with Python" 3 | # by J. M. Hughes, published by O'Reilly Media, December 2010, 4 | # ISBN 978-0-596-80956-0. 5 | 6 | import rospy 7 | 8 | 9 | class PID(object): 10 | """ 11 | PID control class 12 | 13 | This class implements a simplistic PID control algorithm. When first 14 | instantiated all the gain variables are set to zero, so calling 15 | the method compute_output will just return zero. 16 | """ 17 | 18 | def __init__(self, kp=0.0, ki=0.0, kd=0.0): 19 | # initialize gains 20 | self._kp = kp 21 | self._ki = ki 22 | self._kd = kd 23 | 24 | # initialize error, results, and time descriptors 25 | self._prev_err = 0.0 26 | self._cp = 0.0 27 | self._ci = 0.0 28 | self._cd = 0.0 29 | self._cur_time = 0.0 30 | self._prev_time = 0.0 31 | 32 | self.initialize() 33 | 34 | def initialize(self): 35 | """ 36 | Initialize pid controller. 37 | """ 38 | # reset delta t variables 39 | self._cur_time = rospy.get_time() 40 | self._prev_time = self._cur_time 41 | 42 | self._prev_err = 0.0 43 | 44 | # reset result variables 45 | self._cp = 0.0 46 | self._ci = 0.0 47 | self._cd = 0.0 48 | 49 | def set_kp(self, invar): 50 | """ 51 | Set proportional gain. 52 | """ 53 | self._kp = invar 54 | 55 | def set_ki(self, invar): 56 | """ 57 | Set integral gain. 58 | """ 59 | self._ki = invar 60 | 61 | def set_kd(self, invar): 62 | """ 63 | Set derivative gain. 64 | """ 65 | self._kd = invar 66 | 67 | def compute_output(self, error): 68 | """ 69 | Performs a PID computation and returns a control value based on 70 | the elapsed time (dt) and the error signal from a summing junction 71 | (the error parameter). 72 | """ 73 | self._cur_time = rospy.get_time() # get t 74 | dt = self._cur_time - self._prev_time # get delta t 75 | de = error - self._prev_err # get delta error 76 | 77 | self._cp = error # proportional term 78 | self._ci += error * dt # integral term 79 | 80 | self._cd = 0 81 | if dt > 0: # no div by zero 82 | self._cd = de / dt # derivative term 83 | 84 | self._prev_time = self._cur_time # save t for next pass 85 | self._prev_err = error # save t-1 error 86 | 87 | # sum the terms and return the result 88 | return ((self._kp * self._cp) + (self._ki * self._ci) + 89 | (self._kd * self._cd)) 90 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/scripts/start_joint_trajectory_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # /*************************************************************************** 4 | 5 | # 6 | # @package: panda_joint_trajectory_action 7 | # @metapackage: panda_simulator 8 | # @author: Saif Sidhik 9 | # 10 | 11 | # **************************************************************************/ 12 | 13 | # /*************************************************************************** 14 | # Copyright (c) 2019-2021, Saif Sidhik 15 | # Copyright (c) 2013-2018, Rethink Robotics Inc. 16 | # 17 | # Licensed under the Apache License, Version 2.0 (the "License"); 18 | # you may not use this file except in compliance with the License. 19 | # You may obtain a copy of the License at 20 | # 21 | # http://www.apache.org/licenses/LICENSE-2.0 22 | # 23 | # Unless required by applicable law or agreed to in writing, software 24 | # distributed under the License is distributed on an "AS IS" BASIS, 25 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 26 | # See the License for the specific language governing permissions and 27 | # limitations under the License. 28 | # **************************************************************************/ 29 | 30 | """ 31 | Emulating the joint trajectory controller server using the SDK. 32 | """ 33 | 34 | import argparse 35 | import importlib 36 | 37 | import rospy 38 | from dynamic_reconfigure.server import Server 39 | 40 | from panda_sim_custom_action_server import ( 41 | JointTrajectoryActionServer, 42 | ) 43 | 44 | import panda_sim_custom_action_server.cfg.PandaPositionFFJointTrajectoryActionServerConfig as ActionServerConfig 45 | 46 | def start_server(rate): 47 | rospy.loginfo("Initializing node... ") 48 | rospy.init_node("panda_joint_trajectory_action_server") 49 | 50 | rospy.loginfo("Initializing joint trajectory action server...") 51 | 52 | cfg = ActionServerConfig 53 | dyn_cfg_srv = Server(cfg, lambda config, level: config) 54 | jtas = [] 55 | 56 | jtas.append(JointTrajectoryActionServer(dyn_cfg_srv, rate)) 57 | 58 | 59 | def cleanup(): 60 | for j in jtas: 61 | j.clean_shutdown() 62 | 63 | rospy.on_shutdown(cleanup) 64 | rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit") 65 | rospy.spin() 66 | 67 | 68 | def main(): 69 | 70 | arg_fmt = argparse.ArgumentDefaultsHelpFormatter 71 | parser = argparse.ArgumentParser(formatter_class=arg_fmt) 72 | 73 | parser.add_argument( 74 | "-r", "--rate", dest="rate", default=100.0, 75 | type=float, help="trajectory control rate (Hz)" 76 | ) 77 | 78 | args = parser.parse_args(rospy.myargv()[1:]) 79 | start_server(args.rate) 80 | 81 | 82 | if __name__ == "__main__": 83 | main() 84 | -------------------------------------------------------------------------------- /panda_gazebo/include/panda_gazebo/panda_robot_hw_sim.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_robot_hw_sim.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_gazebo 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #ifndef _PANDA_GAZEBO___PANDA_ROBOT_HW_SIM_H_ 30 | #define _PANDA_GAZEBO___PANDA_ROBOT_HW_SIM_H_ 31 | 32 | // gazebo_ros_control 33 | #include 34 | #include 35 | #include 36 | 37 | 38 | namespace panda_gazebo 39 | { 40 | 41 | class PandaRobotHWSim : public gazebo_ros_control::DefaultRobotHWSim 42 | { 43 | public: 44 | 45 | virtual bool initSim( 46 | const std::string& robot_namespace, 47 | ros::NodeHandle model_nh, 48 | gazebo::physics::ModelPtr parent_model, 49 | const urdf::Model *const urdf_model, 50 | std::vector transmissions); 51 | 52 | virtual void writeSim(ros::Time time, ros::Duration period); 53 | 54 | virtual void eStopActive(const bool active); 55 | 56 | virtual void brakesActive(const bool active); 57 | 58 | protected: 59 | bool initCustomInterfaces(); 60 | void initBrakes(); 61 | 62 | panda_hardware_interface::SharedJointInterface sum_ej_interface_; 63 | // secondary list of refs to SumJointHandle containers, so we can call updateCommandSum() before write 64 | std::vector sum_ej_handles_refs_; 65 | 66 | std::vector joint_enable_; 67 | std::vector joint_disable_; 68 | static const double BRAKE_VALUE; 69 | }; 70 | 71 | typedef std::shared_ptr PandaRobotHWSimPtr; 72 | 73 | } // namespace 74 | 75 | #endif // #ifndef __PANDA_GAZEBO_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_ 76 | -------------------------------------------------------------------------------- /panda_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(panda_gazebo) 3 | 4 | find_package(catkin 5 | REQUIRED COMPONENTS 6 | panda_hardware_interface 7 | cv_bridge 8 | gazebo_ros 9 | gazebo_ros_control 10 | image_transport 11 | franka_core_msgs 12 | realtime_tools 13 | roscpp 14 | roslint 15 | std_msgs 16 | kdl_parser 17 | tf_conversions 18 | orocos_kdl 19 | panda_sim_controllers 20 | panda_sim_custom_action_server 21 | ) 22 | 23 | # Depend on system install of Gazebo 24 | find_package(GAZEBO REQUIRED) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 26 | find_package(Eigen3 REQUIRED) 27 | 28 | catkin_package( 29 | CATKIN_DEPENDS 30 | panda_hardware_interface 31 | gazebo_ros_control 32 | image_transport 33 | franka_core_msgs 34 | realtime_tools 35 | roscpp 36 | kdl_parser 37 | tf_conversions 38 | panda_sim_controllers 39 | panda_sim_custom_action_server 40 | INCLUDE_DIRS include 41 | LIBRARIES panda_robot_hw_sim 42 | ) 43 | 44 | roslint_cpp() 45 | link_directories( 46 | ${GAZEBO_LIBRARY_DIRS} 47 | ${catkin_LIBRARY_DIRS} 48 | ) 49 | 50 | include_directories(include 51 | ${catkin_INCLUDE_DIRS} 52 | ${GAZEBO_INCLUDE_DIRS} 53 | ${EIGEN3_INCLUDE_DIRS} 54 | ) 55 | 56 | add_library(panda_gazebo_ros_control 57 | src/panda_gazebo_ros_control_plugin.cpp 58 | src/arm_controller_interface.cpp 59 | src/arm_kinematics_interface.cpp 60 | src/kdl_methods.cpp 61 | ) 62 | set_property(TARGET panda_gazebo_ros_control PROPERTY CXX_STANDARD 14) 63 | 64 | target_link_libraries(panda_gazebo_ros_control 65 | ${catkin_LIBRARIES} 66 | ${GAZEBO_LIBRARIES} 67 | ) 68 | 69 | add_library(panda_robot_hw_sim src/panda_robot_hw_sim.cpp) 70 | target_link_libraries(panda_robot_hw_sim 71 | ${catkin_LIBRARIES} 72 | ${GAZEBO_LIBRARIES} 73 | ) 74 | add_dependencies( 75 | panda_gazebo_ros_control 76 | ${catkin_EXPORTED_TARGETS} 77 | ) 78 | 79 | install( 80 | TARGETS panda_gazebo_ros_control panda_robot_hw_sim 81 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 82 | ) 83 | 84 | install( 85 | FILES panda_robot_hw_sim_plugins.xml 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 87 | ) 88 | 89 | install( 90 | DIRECTORY config launch worlds share 91 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 92 | ) 93 | 94 | install( 95 | DIRECTORY include/${PROJECT_NAME}/ 96 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 97 | ) 98 | 99 | install( 100 | DIRECTORY scripts 101 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 102 | USE_SOURCE_PERMISSIONS 103 | ) 104 | 105 | catkin_python_setup() 106 | catkin_install_python(PROGRAMS startup_scripts/force_neutral_pose.py 107 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 108 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_velocity_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from panda_velocity_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #ifndef PANDA_VELOCITY_CONTROLLER_H 30 | #define PANDA_VELOCITY_CONTROLLER_H 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace panda_sim_controllers 43 | { 44 | class PandaVelocityController : public panda_sim_controllers::JointArrayController 45 | { 46 | public: 47 | virtual ~PandaVelocityController() {sub_joint_command_.shutdown(); ; t_.join(); } 48 | virtual bool init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n); 49 | void setCommands(); 50 | 51 | private: 52 | ros::Subscriber sub_joint_command_; 53 | ros::Subscriber sub_joint_ctrl_gains_; 54 | boost::thread t_; 55 | boost::scoped_ptr> controller_states_publisher_ ; 56 | void publishControllerState(); 57 | 58 | /** \brief The callback function to set joint commands 59 | * 60 | * \param msg joint commands (position). 61 | */ 62 | void jointCommandCB(const franka_core_msgs::JointCommandConstPtr& msg); 63 | 64 | /** \brief The callback function to set pid controller gain for each joint 65 | * 66 | * \param msg joint gains using the franka_core_msgs::JointControllerStates msg. 67 | */ 68 | void jointCtrlGainsCB(const franka_core_msgs::JointControllerStatesConstPtr& msg); 69 | }; 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /panda_gazebo/config/robot_details.yaml: -------------------------------------------------------------------------------- 1 | SIMULATOR_: true 2 | 3 | robot_config: 4 | 5 | joint_names: 6 | - panda_joint1 7 | - panda_joint2 8 | - panda_joint3 9 | - panda_joint4 10 | - panda_joint5 11 | - panda_joint6 12 | - panda_joint7 13 | arm_id: panda 14 | 15 | neutral_pose: 16 | panda_joint1: -0.017792060227770554 17 | panda_joint2: -0.7601235411041661 18 | panda_joint3: 0.019782607023391807 19 | panda_joint4: -2.342050140544315 20 | panda_joint5: 0.029840531355804868 21 | panda_joint6: 1.5411935298621688 22 | panda_joint7: 0.7534486589746342 23 | 24 | joint_config: 25 | joint_acceleration_limit: 26 | panda_joint1: 15.0 # rad / sec^2 27 | panda_joint2: 7.5 # rad / sec^2 28 | panda_joint3: 10.0 # rad / sec^2 29 | panda_joint4: 12.5 # rad / sec^2 30 | panda_joint5: 15.0 # rad / sec^2 31 | panda_joint6: 20.0 # rad / sec^2 32 | panda_joint7: 20.0 # rad / sec^2 33 | # panda_finger_joint1: 12.0 # rad / sec^2 34 | # panda_finger_joint2: 12.0 # rad / sec^2 35 | 36 | joint_velocity_limit: 37 | panda_joint1: 2.1750 # rad / sec 38 | panda_joint2: 2.1750 # rad / sec 39 | panda_joint3: 2.1750 # rad / sec 40 | panda_joint4: 2.1750 # rad / sec 41 | panda_joint5: 2.6100 # rad / sec 42 | panda_joint6: 2.6100 # rad / sec 43 | panda_joint7: 2.6100 # rad / sec 44 | # panda_finger_joint1: 2.0 # rad / sec 45 | # panda_finger_joint2: 2.0 # rad / sec 46 | 47 | joint_position_limit: 48 | lower: 49 | panda_joint1: -2.8973 # rad 50 | panda_joint2: -1.7628 # rad 51 | panda_joint3: -2.8973 # rad 52 | panda_joint4: -3.0718 # rad 53 | panda_joint5: -2.8973 # rad 54 | panda_joint6: -0.0175 # rad 55 | panda_joint7: -2.8973 # rad 56 | upper: 57 | panda_joint1: 2.8973 # rad 58 | panda_joint2: 1.7628 # rad 59 | panda_joint3: 2.8973 # rad 60 | panda_joint4: -0.0698 # rad 61 | panda_joint5: 2.8973 # rad 62 | panda_joint6: 3.7525 # rad 63 | panda_joint7: 2.8973 # rad 64 | 65 | joint_effort_limit: 66 | panda_joint1: 87 # Nm 67 | panda_joint2: 87 # Nm 68 | panda_joint3: 87 # Nm 69 | panda_joint4: 87 # Nm 70 | panda_joint5: 12 # Nm 71 | panda_joint6: 12 # Nm 72 | panda_joint7: 12 # Nm 73 | 74 | gripper_config: 75 | joint_names: 76 | - panda_finger_joint1 77 | - panda_finger_joint2 78 | default_speed: 0.1 # [m/s] 79 | default_grasp_epsilon: 80 | inner: 0.005 # [m] 81 | outer: 0.005 # [m] 82 | 83 | controllers_config: 84 | position_controller: "panda_simulator/position_joint_position_controller" 85 | torque_controller: "panda_simulator/effort_joint_torque_controller" 86 | velocity_controller: "panda_simulator/velocity_joint_velocity_controller" 87 | default_controller: "panda_simulator/position_joint_position_controller" -------------------------------------------------------------------------------- /panda_sim_controllers/src/panda_effort_controller.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_effort_controller.cpp (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #include 30 | #include 31 | 32 | namespace panda_sim_controllers { 33 | bool PandaEffortController::init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n){ 34 | if(!panda_sim_controllers::JointArrayController::init(hw, n)) { 35 | return false; 36 | } else { 37 | std::string topic_name; 38 | if (n.getParam("topic", topic_name)) { 39 | ros::NodeHandle nh("~"); 40 | sub_joint_command_ = nh.subscribe(topic_name, 1, &PandaEffortController::jointCommandCB, this); 41 | } else { 42 | sub_joint_command_ = n.subscribe("joint_command", 1, &PandaEffortController::jointCommandCB, this); 43 | } 44 | } 45 | return true; 46 | } 47 | 48 | void PandaEffortController::jointCommandCB(const franka_core_msgs::JointCommandConstPtr& msg) { 49 | if (msg->mode == franka_core_msgs::JointCommand::TORQUE_MODE) { 50 | std::vector commands; 51 | 52 | if (msg->names.size() != msg->effort.size()) { 53 | ROS_ERROR_STREAM_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "Effort commands size does not match joints size"); 54 | } 55 | 56 | for (int i = 0; i < msg->names.size(); i++) { 57 | Command cmd = Command(); 58 | cmd.name_ = msg->names[i]; 59 | cmd.effort_ = msg->effort[i]; 60 | commands.push_back(cmd); 61 | } 62 | command_buffer_.writeFromNonRT(commands); 63 | new_command_ = true; 64 | } 65 | } 66 | 67 | void PandaEffortController::setCommands() { 68 | // set the new commands for each controller 69 | std::vector command = *(command_buffer_.readFromRT()); 70 | 71 | for (auto it = command.begin(); it != command.end(); it++) { 72 | controllers_[it->name_]->command_buffer_.writeFromNonRT(it->effort_); 73 | } 74 | } 75 | } 76 | 77 | PLUGINLIB_EXPORT_CLASS(panda_sim_controllers::PandaEffortController, controller_interface::ControllerBase) 78 | -------------------------------------------------------------------------------- /panda_gazebo/include/panda_gazebo/arm_controller_interface.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from arm_controller_interface.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_gazebo 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | #ifndef _PANDA_GAZEBO___ARM_CONTROLLER_INTERFACE_H_ 29 | #define _PANDA_GAZEBO___ARM_CONTROLLER_INTERFACE_H_ 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | namespace panda_gazebo { 42 | class ArmControllerInterface 43 | { 44 | public: 45 | void init(ros::NodeHandle& nh, 46 | boost::shared_ptr controller_manager); 47 | 48 | private: 49 | // mutex for re-entrant calls to modeCommandCallback 50 | std::mutex mtx_; 51 | int current_mode_; 52 | std::string side_; 53 | 54 | realtime_tools::RealtimeBox< std::shared_ptr > box_timeout_length_; 55 | realtime_tools::RealtimeBox< std::shared_ptr > box_cmd_timeout_; 56 | 57 | ros::Timer cmd_timeout_timer_; 58 | 59 | ros::Subscriber joint_command_timeout_sub_; 60 | ros::Subscriber joint_command_sub_; 61 | boost::shared_ptr controller_manager_; 62 | 63 | std::string position_controller_name_; 64 | std::string velocity_controller_name_; 65 | std::string torque_controller_name_; 66 | std::string impedance_controller_name_; 67 | std::string trajectory_controller_name_; 68 | 69 | std::string default_controller_name_; 70 | std::string current_controller_name_; 71 | std::vector all_controllers_; 72 | 73 | std::map controller_name_to_mode_map_; 74 | 75 | protected: 76 | void jointCommandTimeoutCallback(const std_msgs::Float64 msg); 77 | void jointCommandCallback(const franka_core_msgs::JointCommandConstPtr& msg); 78 | std::string getControllerString(std::string mode_str); 79 | bool switchControllers(int control_mode); 80 | void commandTimeoutCheck(const ros::TimerEvent& e); 81 | bool switchToDefaultController(); 82 | 83 | 84 | }; 85 | } 86 | #endif // #ifndef __PANDA_GAZEBO__ARM_CONTROLLER_INTERFACE_H_ 87 | -------------------------------------------------------------------------------- /panda_sim_controllers/src/panda_gravity_controller.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_gravity_controller.cpp (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #include 30 | #include 31 | 32 | namespace panda_sim_controllers { 33 | 34 | bool PandaGravityController::init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n){ 35 | // TODO: use constant, don't hardcode ctrl_subtype ("GRAVITY_COMPENSATION") 36 | if(!panda_sim_controllers::JointArrayController::init(hw, n, "GRAVITY_COMPENSATION")) { 37 | return false; 38 | } 39 | 40 | sub_joint_command_ = n.subscribe("/panda_simulator/custom_franka_state_controller/robot_state", 1, &PandaGravityController::gravityCommandCB, this); 41 | 42 | if (!n.getParam("/robot_config/joint_names", joint_names_) ) { 43 | ROS_ERROR( 44 | "PandaPositionController: Invalid or no joint_names parameters provided, aborting " 45 | "controller init!"); 46 | return false; 47 | } 48 | return true; 49 | } 50 | 51 | 52 | void PandaGravityController::gravityCommandCB(const franka_core_msgs::RobotStateConstPtr& msg) { 53 | 54 | std::vector commands; 55 | if (joint_names_.size() != msg->gravity.size()) { 56 | ROS_ERROR_STREAM_NAMED("PandaGravityController", "Gravity commands size does not match joints size"); 57 | } 58 | for (int i = 0; i < msg->gravity.size(); i++) { 59 | Command cmd = Command(); 60 | cmd.name_ = joint_names_[i]; 61 | cmd.effort_ = msg->gravity[i]; 62 | commands.push_back(cmd); 63 | } 64 | command_buffer_.writeFromNonRT(commands); 65 | new_command_ = true; 66 | } 67 | 68 | void PandaGravityController::setCommands() { 69 | // set the new commands for each controller 70 | std::vector command = *(command_buffer_.readFromRT()); 71 | for (auto it = command.begin(); it != command.end(); it++) { 72 | controllers_[it->name_]->command_buffer_.writeFromNonRT(it->effort_); 73 | } 74 | } 75 | } 76 | 77 | PLUGINLIB_EXPORT_CLASS(panda_sim_controllers::PandaGravityController, controller_interface::ControllerBase) 78 | -------------------------------------------------------------------------------- /panda_sim_moveit/launch/sim_move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 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 | -------------------------------------------------------------------------------- /panda_sim_controllers/config/panda_sim_controllers.yaml: -------------------------------------------------------------------------------- 1 | panda_simulator: 2 | 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 100 6 | 7 | 8 | # Panda SDK Controllers: Position -------------------------- 9 | position_joint_position_controller: 10 | type: panda_sim_controllers/PandaPositionController 11 | topic_joint_command: /panda_simulator/motion_controller/arm/joint_commands 12 | topic_set_speed_ratio: /panda_simulator/arm/set_speed_ratio 13 | joints: 14 | panda_joint1_controller: 15 | joint: panda_joint1 16 | pid: {p: 3000, i: 10.0, d: 25.0} 17 | panda_joint2_controller: 18 | joint: panda_joint2 19 | pid: {p: 3000, i: 10.0, d: 30.0} 20 | panda_joint3_controller: 21 | joint: panda_joint3 22 | pid: {p: 3000, i: 10.0, d: 30.0} 23 | panda_joint4_controller: 24 | joint: panda_joint4 25 | pid: {p: 3000, i: 10.0, d: 50.0} 26 | panda_joint5_controller: 27 | joint: panda_joint5 28 | pid: {p: 3000, i: 10.0, d: 8.0} 29 | panda_joint6_controller: 30 | joint: panda_joint6 31 | pid: {p: 3000, i: 1.0, d: 5.0} 32 | panda_joint7_controller: 33 | joint: panda_joint7 34 | pid: {p: 3000, i: 10.0, d: 4.0} 35 | 36 | # Panda SDK Controllers: Velocity -------------------------- 37 | velocity_joint_velocity_controller: 38 | type: panda_sim_controllers/PandaVelocityController 39 | topic: /panda_simulator/motion_controller/arm/joint_commands 40 | joints: 41 | panda_joint1_controller: 42 | joint: panda_joint1 43 | pid: {p: 10, i: 0.0, d: 0.1} 44 | panda_joint2_controller: 45 | joint: panda_joint2 46 | pid: {p: 100, i: 1.0, d: 0.1} 47 | panda_joint3_controller: 48 | joint: panda_joint3 49 | pid: {p: 0.05, i: 0.0, d: 0.01} 50 | panda_joint4_controller: 51 | joint: panda_joint4 52 | pid: {p: 0.5, i: 0.01, d: 0.1} 53 | panda_joint5_controller: 54 | joint: panda_joint5 55 | pid: {p: 1.0, i: 0.0, d: 0.01} 56 | panda_joint6_controller: 57 | joint: panda_joint6 58 | pid: {p: 0.05, i: 0.0, d: 0.01} 59 | panda_joint7_controller: 60 | joint: panda_joint7 61 | pid: {p: 0.05, i: 0.0, d: 0.01} 62 | 63 | # Panda SDK Controllers: Effort -------------------------- 64 | effort_joint_torque_controller: 65 | type: panda_sim_controllers/PandaEffortController 66 | topic: /panda_simulator/motion_controller/arm/joint_commands 67 | joints: 68 | panda_joint1_controller: 69 | joint: panda_joint1 70 | panda_joint2_controller: 71 | joint: panda_joint2 72 | panda_joint3_controller: 73 | joint: panda_joint3 74 | panda_joint4_controller: 75 | joint: panda_joint4 76 | panda_joint5_controller: 77 | joint: panda_joint5 78 | panda_joint6_controller: 79 | joint: panda_joint6 80 | panda_joint7_controller: 81 | joint: panda_joint7 82 | 83 | # Panda SDK Controllers: Gravity Compensation ------------ 84 | effort_joint_gravity_controller: 85 | type: panda_sim_controllers/PandaGravityController 86 | joints: 87 | panda_joint1_controller: 88 | joint: panda_joint1 89 | panda_joint2_controller: 90 | joint: panda_joint2 91 | panda_joint3_controller: 92 | joint: panda_joint3 93 | panda_joint4_controller: 94 | joint: panda_joint4 95 | panda_joint5_controller: 96 | joint: panda_joint5 97 | panda_joint6_controller: 98 | joint: panda_joint6 99 | panda_joint7_controller: 100 | joint: panda_joint7 101 | 102 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_position_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from panda_position_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #ifndef PANDA_POSITION_CONTROLLER_H 30 | #define PANDA_POSITION_CONTROLLER_H 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | 46 | namespace panda_sim_controllers 47 | { 48 | class PandaPositionController : public panda_sim_controllers::JointArrayController 49 | { 50 | public: 51 | virtual ~PandaPositionController() { sub_joint_command_.shutdown(); t_.join(); } 52 | virtual bool init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n) override; 53 | void setCommands(); 54 | 55 | private: 56 | // mutex for re-entrant calls to modeCommandCallback 57 | std::mutex mtx_; 58 | typedef std::unique_ptr> CommandsPtr; 59 | ros::Subscriber sub_joint_command_; 60 | int lastMode; 61 | 62 | boost::thread t_; 63 | 64 | boost::scoped_ptr> controller_states_publisher_ ; 65 | 66 | ros::Subscriber sub_speed_ratio_; 67 | ros::Subscriber sub_joint_ctrl_gains_; 68 | 69 | realtime_tools::RealtimeBox< std::shared_ptr > speed_ratio_buffer_; 70 | 71 | /** \brief The callback function to set speed ratio 72 | * 73 | * \param msg Speed ratio value. 74 | */ 75 | void speedRatioCallback(const std_msgs::Float64 msg); 76 | 77 | /** \brief The callback function to set joint commands 78 | * 79 | * \param msg joint commands (position). 80 | */ 81 | void jointCommandCB(const franka_core_msgs::JointCommandConstPtr& msg); 82 | 83 | /** \brief The callback function to set pid controller gain for each joint 84 | * 85 | * \param msg joint gains using the franka_core_msgs::JointControllerStates msg. 86 | */ 87 | void jointCtrlGainsCB(const franka_core_msgs::JointControllerStatesConstPtr& msg); 88 | 89 | void publishControllerState(); 90 | 91 | CommandsPtr cmdTrajectoryMode(const franka_core_msgs::JointCommandConstPtr& msg); 92 | CommandsPtr cmdPositionMode(const franka_core_msgs::JointCommandConstPtr& msg); 93 | 94 | }; 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /versionLog.md: -------------------------------------------------------------------------------- 1 | # Version Log 2 | 3 | ## v1.0.0 4 | 5 | _Note: If you have an older version of the package, you may have to do a clean build of the package; several breaking changes in package._ 6 | 7 | - Added [MoveIt!](https://moveit.ros.org/) and [follow trajectory service](http://wiki.ros.org/joint_trajectory_controller) support. 8 | - Action client and MoveIt! support for Franka Gripper. 9 | - Now fully compatible with [PandaMoveGroupInterface](https://justagist.github.io/franka_ros_interface/DOC.html#pandamovegroupinterface) and [JointTrajectoryActionClient](https://justagist.github.io/franka_ros_interface/DOC.html#jointtrajectoryactionclient) interface classes of the [franka_ros_interface](https://github.com/justagist/franka_ros_interface) and [panda_robot](http://github.com/justagist/panda_robot) packages. 10 | - Several other methods from the [franka_ros_interface](https://github.com/justagist/franka_ros_interface) such as [`move_to_joint_positions`](https://justagist.github.io/franka_ros_interface/DOC.html?highlight=move_to_joint_positions#franka_interface.ArmInterface.move_to_joint_positions) are now available for the simulator (make sure to use the newest version of the interface package). 11 | - Now depends on franka_ros_interface's `franka_moveit` subpackage as well (included in `franka_ros_inteface`). 12 | 13 | - **MoveIt! and TrajectoryActionServer usage**: 14 | - The simulator launch file `panda_world.launch` now accepts two additional arguments: `use_custom_action_servers` and `start_moveit`. Both are set to `true` by default, and have to be enabled to be able to use MoveIt! commands and any trajectory action clients (eg. [JointTrajectoryActionClient](https://justagist.github.io/franka_ros_interface/DOC.html#jointtrajectoryactionclient)). To start the simulator without MoveIt! and trajectory action client support (quicker to load), pass as argument `use_custom_action_servers:=false` when launching `panda_world.launch`. 15 | (_Note: Disabling action service also disables MoveIt! support._) 16 | _Note: When using launching the simulator `panda_world.launch` with MoveIt!, wait till you see the message `You can start planning now!` before accessing the simulated robot. You can ignore the following error message: 'Exception while loading planning adapter plugin 'default_planner_request_adapters/ResolveConstraintFrames... '_ 17 | - A new demo file is added to `panda_simulator_examples`. This can be run using the command `roslaunch panda_simulator_examples demo_moveit.launch` (to disable gripper pass `load_gripper:=false` as argument). This loads the same demo as the `demo_moveit.launch` for the real robot from the franka_ros_interface's `franka_moveit` package. (_Note: If the planning fails, uncheck and check the box next to the display named 'MotionPlanning' in RViz. This seems to be an RViz bug._) 18 | 19 | - Other changes: 20 | 21 | - Controller switching is now quicker. Removed redundant controllers and ros parameter loading. 22 | - Impedance controller and Trajectory controller in the controller manager (switcher) are both 'position_joint_position_controller' in the simulator. 23 | - Joint position controller gains tuned (very high values; unrealistic) for better trajectory control and position control. 24 | - Renamed demo file for clarity. 25 | - Now requires version of 'orocos kdl' package from source. 26 | - Gravity is now disabled by default for better control when using 'torque controller' and 'velocity controller'. 27 | - Several other bug fixes. 28 | 29 | - Existing/Known Issues: 30 | - Gravity compensation control is not good with gripper enabled (Fix: disable gravity in simulator (now done by default)) 31 | 32 | ## v0.9 33 | 34 | - Joint position, velocity, torque, gravity, gripper controllers fixed 35 | - Controller names specified through parameter server and config file 36 | - Bug in controller switching interface fixed 37 | - Removed redundant impedance controller support 38 | - Compatible with updated [franka_ros_interface](https://github.com/justagist/franka_ros_interface) and [panda_robot](http://github.com/justagist/panda_robot) packages. 39 | -------------------------------------------------------------------------------- /panda_sim_controllers/config/panda_sim_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The PandaGripperController tracks position commands for the electric grippers. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 8 | 9 | 10 | 11 | 14 | 15 | The PandaPositionController tracks position commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 16 | 17 | 18 | 19 | 22 | 23 | The PandaPositionController tracks velocity commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 24 | 25 | 26 | 27 | 30 | 31 | The PandaPositionController tracks effort commands for the entire Panda arm. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 32 | 33 | 34 | 35 | 38 | 39 | The PandaGravityController adds an effort component to compensate for gravity. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 40 | 41 | 42 | 43 | 44 | 47 | 48 | The JointPositionController tracks position commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 49 | 50 | 51 | 52 | 55 | 56 | The JointVelocityController tracks velocity commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 57 | 58 | 59 | 60 | 63 | 64 | The JointEffortController tracks effort commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 65 | 66 | 67 | 68 | 71 | 72 | The JointEffortController tracks effort commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface. 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_joint_effort_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_joint_effort_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /********************************************************************* 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * 15 | * Licensed under the Apache License, Version 2.0 (the "License"); 16 | * you may not use this file except in compliance with the License. 17 | * You may obtain a copy of the License at 18 | * 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | **************************************************************************/ 27 | 28 | /********************************************************************* 29 | * Software License Agreement (BSD License) 30 | * 31 | * Copyright (c) 2008, Willow Garage, Inc. 32 | * Copyright (c) 2012, hiDOF, Inc. 33 | * Copyright (c) 2013, PAL Robotics, S.L. 34 | * All rights reserved. 35 | * 36 | * Redistribution and use in source and binary forms, with or without 37 | * modification, are permitted provided that the following conditions 38 | * are met: 39 | * 40 | * * Redistributions of source code must retain the above copyright 41 | * notice, this list of conditions and the following disclaimer. 42 | * * Redistributions in binary form must reproduce the above 43 | * copyright notice, this list of conditions and the following 44 | * disclaimer in the documentation and/or other materials provided 45 | * with the distribution. 46 | * * Neither the name of the Willow Garage nor the names of its 47 | * contributors may be used to endorse or promote products derived 48 | * from this software without specific prior written permission. 49 | * 50 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 51 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 52 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 53 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 54 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 55 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 56 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 57 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 58 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 59 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 60 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 61 | * POSSIBILITY OF SUCH DAMAGE. 62 | *********************************************************************/ 63 | 64 | #ifndef PANDA_EFFORT_CONTROLLERS__JOINT_EFFORT_CONTROLLER_H 65 | #define PANDA_EFFORT_CONTROLLERS__JOINT_EFFORT_CONTROLLER_H 66 | 67 | #include 68 | 69 | #include 70 | 71 | namespace panda_effort_controllers 72 | { 73 | 74 | /** 75 | * \brief Joint Effort Controller (torque or force) 76 | * 77 | * This class passes the commanded effort down to the joint. 78 | * 79 | * \section ROS interface 80 | * 81 | * \param type Must be "JointEffortController". 82 | * \param joint Name of the joint to control. 83 | * 84 | * Subscribes to: 85 | * - \b command (std_msgs::Float64) : The joint effort to apply. 86 | */ 87 | class JointEffortController : public forward_command_controller::ForwardCommandController 88 | { 89 | public: 90 | bool init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n); 91 | bool init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n, const std::string& ctrl_type); 92 | }; 93 | 94 | } // namespace 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /panda_sim_controllers/src/panda_joint_effort_controller.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_joint_effort_controller.cpp (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /********************************************************************* 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * 15 | * Licensed under the Apache License, Version 2.0 (the "License"); 16 | * you may not use this file except in compliance with the License. 17 | * You may obtain a copy of the License at 18 | * 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | **************************************************************************/ 27 | 28 | /********************************************************************* 29 | * Software License Agreement (BSD License) 30 | * 31 | * Copyright (c) 2008, Willow Garage, Inc. 32 | * Copyright (c) 2012, hiDOF, Inc. 33 | * Copyright (c) 2013, PAL Robotics, S.L. 34 | * All rights reserved. 35 | * 36 | * Redistribution and use in source and binary forms, with or without 37 | * modification, are permitted provided that the following conditions 38 | * are met: 39 | * 40 | * * Redistributions of source code must retain the above copyright 41 | * notice, this list of conditions and the following disclaimer. 42 | * * Redistributions in binary form must reproduce the above 43 | * copyright notice, this list of conditions and the following 44 | * disclaimer in the documentation and/or other materials provided 45 | * with the distribution. 46 | * * Neither the name of the Willow Garage nor the names of its 47 | * contributors may be used to endorse or promote products derived 48 | * from this software without specific prior written permission. 49 | * 50 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 51 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 52 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 53 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 54 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 55 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 56 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 57 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 58 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 59 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 60 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 61 | * POSSIBILITY OF SUCH DAMAGE. 62 | *********************************************************************/ 63 | 64 | #include 65 | #include 66 | 67 | template 68 | void forward_command_controller::ForwardCommandController::starting(const ros::Time& time) 69 | { 70 | // Start controller with 0.0 effort 71 | command_buffer_.writeFromNonRT(0.0); 72 | } 73 | 74 | 75 | namespace panda_effort_controllers 76 | { 77 | 78 | bool JointEffortController::init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n) 79 | { 80 | return forward_command_controller::ForwardCommandController::init(hw, n); 81 | } 82 | bool JointEffortController::init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n, const std::string& ctrl_type) 83 | { 84 | std::string joint_name; 85 | if (!n.getParam("joint", joint_name)) 86 | { 87 | ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); 88 | return false; 89 | } 90 | joint_ = hw->getHandle(joint_name, ctrl_type); 91 | // just got rid of ros Subscriber interface sub_command_; 92 | return true; 93 | } 94 | 95 | } // namespace 96 | 97 | 98 | PLUGINLIB_EXPORT_CLASS(panda_effort_controllers::JointEffortController,controller_interface::ControllerBase) 99 | -------------------------------------------------------------------------------- /panda_gazebo/launch/panda_world.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 | 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 | -------------------------------------------------------------------------------- /panda_simulator_examples/scripts/move_robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from sensor_msgs.msg import JointState 4 | from franka_core_msgs.msg import JointCommand, RobotState 5 | import numpy as np 6 | from copy import deepcopy 7 | 8 | vals = [] 9 | vels = [] 10 | names = ['panda_joint1','panda_joint2','panda_joint3','panda_joint4','panda_joint5','panda_joint6','panda_joint7'] 11 | 12 | neutral_pose = [-0.017792060227770554, -0.7601235411041661, 0.019782607023391807, -2.342050140544315, 0.029840531355804868, 1.5411935298621688, 0.7534486589746342] 13 | 14 | def callback(msg): 15 | 16 | global vals , vels 17 | temp_vals = [] 18 | temp_vels = [] 19 | for n in names: 20 | idx = msg.name.index(n) 21 | temp_vals.append(msg.position[idx]) 22 | temp_vels.append(msg.velocity[idx]) 23 | 24 | vals = deepcopy(temp_vals) 25 | vels = deepcopy(temp_vels) 26 | 27 | 28 | 29 | t = 0 30 | def state_callback(msg): 31 | global t 32 | if t%100 == 0: 33 | t = 1 34 | rospy.loginfo("============= Current robot state: ============\n" ) 35 | rospy.loginfo("Cartesian vel: \n{}\n".format(msg.O_dP_EE) ) 36 | rospy.loginfo("Gravity compensation torques: \n{}\n".format(msg.gravity) ) 37 | rospy.loginfo("Coriolis: \n{}\n".format(msg.coriolis) ) 38 | rospy.loginfo("Inertia matrix: \n{}\n".format(msg.mass_matrix) ) 39 | rospy.loginfo("Zero Jacobian: \n{}\n".format(msg.O_Jac_EE) ) 40 | 41 | 42 | rospy.loginfo("\n\n========\n\n") 43 | 44 | t+=1 45 | 46 | def send_to_neutral(): 47 | """ 48 | DON'T USE THIS ON REAL ROBOT!!! 49 | 50 | """ 51 | temp_pub = rospy.Publisher('/panda_simulator/motion_controller/arm/joint_commands',JointCommand, queue_size = 1, tcp_nodelay = True) 52 | # Create JointCommand message to publish commands 53 | pubmsg = JointCommand() 54 | pubmsg.names = names # names of joints (has to be 7) 55 | pubmsg.position = neutral_pose # JointCommand msg has other fields (velocities, efforts) for 56 | # when controlling in other control mode 57 | pubmsg.mode = pubmsg.POSITION_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE) 58 | curr_val = deepcopy(vals) 59 | 60 | while all(abs(neutral_pose[i]-curr_val[i]) > 0.01 for i in range(len(curr_val))): 61 | temp_pub.publish(pubmsg) 62 | curr_val = deepcopy(vals) 63 | 64 | if __name__ == '__main__': 65 | 66 | 67 | rospy.init_node("test_node") 68 | 69 | rospy.wait_for_service('/controller_manager/list_controllers') 70 | 71 | rospy.loginfo("Starting node...") 72 | rospy.sleep(5) 73 | 74 | pub = rospy.Publisher('/panda_simulator/motion_controller/arm/joint_commands',JointCommand, queue_size = 1, tcp_nodelay = True) 75 | 76 | # Subscribe to robot joint state 77 | rospy.Subscriber('/panda_simulator/custom_franka_state_controller/joint_states', JointState, callback) 78 | 79 | # Subscribe to robot state (Refer JointState.msg to find all available data. 80 | # Note: All msg fields are not populated when using the simulated environment) 81 | rospy.Subscriber('/panda_simulator/custom_franka_state_controller/robot_state', RobotState, state_callback) 82 | 83 | 84 | rate = rospy.Rate(1000) 85 | 86 | max_val = 1.1897 87 | min_val = 0.4723473991867569 88 | 89 | rospy.loginfo("Not recieved current joint state msg") 90 | 91 | while not rospy.is_shutdown() and len(vals) != 7: 92 | continue 93 | 94 | rospy.loginfo("Sending robot to neutral pose") 95 | send_to_neutral() # DON'T DO THIS ON REAL ROBOT!! (use move_to_neutral() method from ArmInterface of franka_ros_interface package) 96 | 97 | rospy.sleep(2.0) 98 | 99 | initial_pose = vals 100 | 101 | rospy.loginfo("Commanding...\n") 102 | elapsed_time_ = rospy.Duration(0.0) 103 | period = rospy.Duration(0.005) 104 | 105 | count = 0 106 | 107 | # Create JointCommand message to publish commands 108 | pubmsg = JointCommand() 109 | pubmsg.names = names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts)) 110 | while not rospy.is_shutdown(): 111 | 112 | elapsed_time_ += period 113 | 114 | delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2 115 | 116 | for j in range(len(vals)): 117 | if j == 4: 118 | vals[j] = initial_pose[j] - delta 119 | else: 120 | vals[j] = initial_pose[j] + delta 121 | 122 | pubmsg.position = vals # JointCommand msg has other fields (velocities, efforts) for 123 | # when controlling in other control mode 124 | # pubmsg.effort = [0.,0.,0.,0.,0.,0.,0.] 125 | pubmsg.mode = pubmsg.POSITION_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE) 126 | 127 | pub.publish(pubmsg) 128 | rate.sleep() 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /panda_sim_custom_action_server/src/panda_sim_custom_action_server/gripper_action_server.py: -------------------------------------------------------------------------------- 1 | # /*************************************************************************** 2 | 3 | # 4 | # @package: panda_sim_custom_action_server 5 | # @metapackage: panda_simulator 6 | # @author: Saif Sidhik 7 | # 8 | 9 | # **************************************************************************/ 10 | 11 | # /*************************************************************************** 12 | # Copyright (c) 2019-2021, Saif Sidhik 13 | 14 | # Licensed under the Apache License, Version 2.0 (the "License"); 15 | # you may not use this file except in compliance with the License. 16 | # You may obtain a copy of the License at 17 | 18 | # http://www.apache.org/licenses/LICENSE-2.0 19 | 20 | # Unless required by applicable law or agreed to in writing, software 21 | # distributed under the License is distributed on an "AS IS" BASIS, 22 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | # See the License for the specific language governing permissions and 24 | # limitations under the License. 25 | # **************************************************************************/ 26 | 27 | """ 28 | Franka Gripper Action Server Emulator using Franka ROS Interface SDK 29 | """ 30 | from math import fabs 31 | 32 | import rospy 33 | 34 | import actionlib 35 | 36 | from control_msgs.msg import ( 37 | GripperCommandAction, 38 | GripperCommandFeedback, 39 | GripperCommandResult, 40 | ) 41 | 42 | import franka_interface 43 | 44 | class GripperActionServer(object): 45 | def __init__(self): 46 | self._ns = 'franka_gripper/gripper_action' 47 | 48 | self._gripper = franka_interface.GripperInterface(gripper_joint_names = ['panda_finger_joint1', 'panda_finger_joint2']) 49 | 50 | # Action Server 51 | self._server = actionlib.SimpleActionServer( 52 | self._ns, 53 | GripperCommandAction, 54 | execute_cb=self._on_gripper_action, 55 | auto_start=False) 56 | self._action_name = "GripperActionServer" 57 | self._server.start() 58 | 59 | # Action Feedback/Result 60 | self._fdbk = GripperCommandFeedback() 61 | self._result = GripperCommandResult() 62 | 63 | # Initialize Parameters 64 | self._timeout = 5.0 65 | 66 | def _update_feedback(self, position): 67 | self._fdbk.position = self._gripper.joint_position('panda_finger_joint1') 68 | self._fdbk.effort = self._gripper.joint_effort('panda_finger_joint1') 69 | self._fdbk.stalled = False 70 | self._fdbk.reached_goal = (fabs(self._fdbk.position - 71 | position) <= 0.05) 72 | self._result = self._fdbk 73 | self._server.publish_feedback(self._fdbk) 74 | 75 | def _command_gripper(self, position, force = 0.001, speed = None, wait = False): 76 | return self._gripper.move_joints(2*position, speed = speed, wait_for_result = wait) 77 | 78 | def _check_state(self, position): 79 | return fabs(self._gripper.joint_position('panda_finger_joint1') - position) <= 0.01 80 | 81 | def _on_gripper_action(self, goal): 82 | # Store position and effort from call 83 | # Position to 0:100 == close:open 84 | position = goal.command.position 85 | effort = goal.command.max_effort 86 | 87 | # Reset feedback/result 88 | self._update_feedback(position) 89 | 90 | # 20 Hz gripper state rate 91 | control_rate = rospy.Rate(20.0) 92 | 93 | # Record start time 94 | start_time = rospy.get_time() 95 | 96 | # Set the moving_force/vacuum_threshold based on max_effort provided 97 | if fabs(effort) < 0.0001: 98 | effort = None 99 | 100 | def now_from_start(start): 101 | return rospy.get_time() - start 102 | 103 | # Continue commanding goal until success or timeout 104 | while ((now_from_start(start_time) < self._timeout or 105 | self._timeout < 0.0) and not rospy.is_shutdown()): 106 | if self._server.is_preempt_requested(): 107 | self._gripper.stop_action() 108 | rospy.loginfo("%s: Gripper Action Preempted" % 109 | (self._action_name,)) 110 | self._server.set_preempted(self._result) 111 | return 112 | self._update_feedback(position) 113 | if self._check_state(position): 114 | self._server.set_succeeded(self._result) 115 | return 116 | self._command_gripper(position = position, force = effort, speed = 0.) 117 | control_rate.sleep() 118 | 119 | # Gripper failed to achieve goal before timeout/shutdown 120 | self._update_feedback(position) 121 | result = self._command_gripper(position = position, force = effort, speed = 0., wait = True) 122 | 123 | self._gripper.stop_action() 124 | if not rospy.is_shutdown(): 125 | rospy.logerr("%s: Gripper Command Not Achieved in Allotted Time" % 126 | (self._action_name,)) 127 | self._update_feedback(position) 128 | self._server.set_aborted(self._result) 129 | -------------------------------------------------------------------------------- /panda_simulator_examples/scripts/rviz_markers.py: -------------------------------------------------------------------------------- 1 | from interactive_markers.interactive_marker_server import * 2 | from interactive_markers.menu_handler import * 3 | from visualization_msgs.msg import * 4 | from geometry_msgs.msg import Quaternion 5 | 6 | class RvizMarkers(): 7 | 8 | def makeBox( self, msg ): 9 | marker = Marker() 10 | 11 | marker.type = Marker.CUBE 12 | marker.scale.x = msg.scale * 0.05 13 | marker.scale.y = msg.scale * 0.05 14 | marker.scale.z = msg.scale * 0.05 15 | marker.color.r = 0.5 16 | marker.color.g = 0.5 17 | marker.color.b = 0.5 18 | marker.color.a = 1.0 19 | 20 | return marker 21 | 22 | def makeBoxControl( self, msg ): 23 | control = InteractiveMarkerControl() 24 | control.always_visible = True 25 | control.markers.append( self.makeBox(msg) ) 26 | msg.controls.append( control ) 27 | return control 28 | 29 | def makeMarker( self, fixed, interaction_mode, position, orientation, show_6dof = False): 30 | int_marker = InteractiveMarker() 31 | int_marker.header.frame_id = "base" 32 | int_marker.pose.position = position 33 | 34 | init_orientation = Quaternion(orientation[1], orientation[2], orientation[3], orientation[0]) 35 | 36 | int_marker.pose.orientation = init_orientation 37 | 38 | int_marker.scale = 0.5 39 | 40 | int_marker.name = "marker" 41 | int_marker.description = "Marker Control" 42 | 43 | # insert a box 44 | self.makeBoxControl(int_marker) 45 | int_marker.controls[0].interaction_mode = interaction_mode 46 | 47 | if fixed: 48 | int_marker.name += "_fixed" 49 | int_marker.description += "\n(fixed orientation)" 50 | 51 | if interaction_mode != InteractiveMarkerControl.NONE: 52 | control_modes_dict = { 53 | InteractiveMarkerControl.MOVE_3D : "MOVE_3D", 54 | InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D", 55 | InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" } 56 | int_marker.name += "_" + control_modes_dict[interaction_mode] 57 | int_marker.description = "3D Control" 58 | if show_6dof: 59 | int_marker.description += " + 6-DOF controls" 60 | int_marker.description += "\n" + control_modes_dict[interaction_mode] 61 | 62 | if show_6dof: 63 | control = InteractiveMarkerControl() 64 | control.orientation.w = 1 65 | control.orientation.x = 0 66 | control.orientation.y = 0 67 | control.orientation.z = 0 68 | 69 | control.name = "rotate_x" 70 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 71 | if fixed: 72 | control.orientation_mode = InteractiveMarkerControl.FIXED 73 | int_marker.controls.append(control) 74 | 75 | control = InteractiveMarkerControl() 76 | control.orientation = init_orientation 77 | 78 | control.name = "move_x" 79 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 80 | if fixed: 81 | control.orientation_mode = InteractiveMarkerControl.FIXED 82 | int_marker.controls.append(control) 83 | 84 | control = InteractiveMarkerControl() 85 | control.orientation.w = 1 86 | control.orientation.x = 0 87 | control.orientation.y = 1 88 | control.orientation.z = 0 89 | control.name = "rotate_z" 90 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 91 | if fixed: 92 | control.orientation_mode = InteractiveMarkerControl.FIXED 93 | int_marker.controls.append(control) 94 | 95 | control = InteractiveMarkerControl() 96 | control.orientation.w = 1 97 | control.orientation.x = 0 98 | control.orientation.y = 1 99 | control.orientation.z = 0 100 | control.name = "move_z" 101 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 102 | if fixed: 103 | control.orientation_mode = InteractiveMarkerControl.FIXED 104 | int_marker.controls.append(control) 105 | 106 | control = InteractiveMarkerControl() 107 | control.orientation.w = 1 108 | control.orientation.x = 0 109 | control.orientation.y = 0 110 | control.orientation.z = 1 111 | control.name = "rotate_y" 112 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 113 | if fixed: 114 | control.orientation_mode = InteractiveMarkerControl.FIXED 115 | int_marker.controls.append(control) 116 | 117 | control = InteractiveMarkerControl() 118 | control.orientation.w = 1 119 | control.orientation.x = 0 120 | control.orientation.y = 0 121 | control.orientation.z = 1 122 | control.name = "move_y" 123 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 124 | if fixed: 125 | control.orientation_mode = InteractiveMarkerControl.FIXED 126 | int_marker.controls.append(control) 127 | 128 | return int_marker -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Generic .travis.yml file for running continuous integration on Travis-CI for 2 | # any ROS package. 3 | # 4 | # Available here: 5 | # - https://github.com/felixduvallet/ros-travis-integration 6 | # 7 | # This installs ROS on a clean Travis-CI virtual machine, creates a ROS 8 | # workspace, resolves all listed dependencies, and sets environment variables 9 | # (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are 10 | # no compilation errors), and runs all the tests. If any of the compilation/test 11 | # phases fail, the build is marked as a failure. 12 | # 13 | # We handle two types of package dependencies specified in the package manifest: 14 | # - system dependencies that can be installed using `rosdep`, including other 15 | # ROS packages and system libraries. These dependencies must be known to 16 | # `rosdistro` and are installed using apt-get. 17 | # - package dependencies that must be checked out from source. These are handled by 18 | # `wstool`, and should be listed in a file named dependencies.rosinstall. 19 | # 20 | 21 | # There are envioronment variables you may want to change, such as ROS_DISTRO, 22 | # ROSINSTALL_FILE, and the CATKIN_OPTIONS file. See the README.md for more 23 | # information on these flags, and 24 | # https://docs.travis-ci.com/user/environment-variables/ for information about 25 | # Travis environment variables in general. 26 | # 27 | # Author: Felix Duvallet 28 | 29 | # NOTE: The build lifecycle on Travis.ci is something like this: 30 | # before_install 31 | # install 32 | # before_script 33 | # script 34 | # after_success or after_failure 35 | # after_script 36 | # OPTIONAL before_deploy 37 | # OPTIONAL deploy 38 | # OPTIONAL after_deploy 39 | 40 | ################################################################################ 41 | 42 | sudo: required 43 | cache: 44 | - apt 45 | 46 | # Build all valid Ubuntu/ROS combinations available on Travis VMs. 47 | language: generic 48 | matrix: 49 | include: 50 | - name: "focal noetic" 51 | dist: focal 52 | env: ROS_DISTRO=noetic 53 | 54 | # Configuration variables. All variables are global now, but this can be used to 55 | # trigger a build matrix for different ROS distributions if desired. 56 | env: 57 | global: 58 | - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [trusty|xenial|...] 59 | - CI_SOURCE_PATH=$(pwd) 60 | - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall 61 | - ROS_PARALLEL_JOBS='-j8 -l6' 62 | # Set the python path manually to include /usr/-/python2.7/dist-packages 63 | # as this is where apt-get installs python packages. 64 | - PYTHONPATH=$PYTHONPATH:/usr/lib/python3/dist-packages:/usr/local/lib/python3/dist-packages 65 | 66 | ################################################################################ 67 | 68 | # Install system dependencies, namely a very barebones ROS setup. 69 | before_install: 70 | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 71 | - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 72 | - sudo apt-get update -qq && sudo apt upgrade 73 | - sudo apt-get install dpkg 74 | - sudo apt-get install -y gcc python3-catkin-pkg python3-rosdep python3-wstool ros-$ROS_DISTRO-desktop-full ros-$ROS_DISTRO-combined-robot-hw 75 | - source /opt/ros/$ROS_DISTRO/setup.bash 76 | - sudo apt install ros-$ROS_DISTRO-libfranka 77 | - sudo apt install python3-catkin-tools python3-osrf-pycommon 78 | - sudo apt install ros-${ROS_DISTRO}-effort-controllers 79 | # Prepare rosdep to install dependencies. 80 | - sudo rosdep init 81 | - rosdep update --include-eol-distros # Support EOL distros. 82 | 83 | # Create a catkin workspace with the package under integration. 84 | install: 85 | - mkdir -p ~/catkin_ws/src 86 | - cd ~/catkin_ws/src 87 | - catkin_init_workspace 88 | # Create the devel/setup.bash (run catkin_make with an empty workspace) and 89 | # source it to set the path variables. 90 | - cd ~/catkin_ws 91 | - catkin build 92 | - source devel/setup.bash 93 | # Add the package under integration to the workspace using a symlink. 94 | - cd ~/catkin_ws/src 95 | - ln -s $CI_SOURCE_PATH . 96 | 97 | # Install all dependencies, using wstool first and rosdep second. 98 | # wstool looks for a ROSINSTALL_FILE defined in the environment variables. 99 | before_script: 100 | # source dependencies: install using wstool. 101 | - cd ~/catkin_ws/src 102 | - wstool init 103 | - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi 104 | - wstool up 105 | # package depdencies: install using rosdep. 106 | # - cd src/orocos_kinematics_dynamics && git checkout b35c424e77ebc5b7e6f1c5e5c34f8a4666fbf5bc 107 | - cd ~/catkin_ws/src/orocos_kinematics_dynamics/ && rm -rf * && git checkout b35c424e && git reset --hard 108 | # - cd ~/catkin_ws 109 | # - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 110 | 111 | # Compile and test (mark the build as failed if any step fails). If the 112 | # CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example 113 | # to blacklist certain packages. 114 | # 115 | # NOTE on testing: `catkin_make run_tests` will show the output of the tests 116 | # (gtest, nosetest, etc..) but always returns 0 (success) even if a test 117 | # fails. Running `catkin_test_results` aggregates all the results and returns 118 | # non-zero when a test fails (which notifies Travis the build failed). 119 | script: 120 | - source /opt/ros/$ROS_DISTRO/setup.bash 121 | - cd ~/catkin_ws 122 | - catkin build franka_msgs 123 | - source devel/setup.bash 124 | - catkin build franka_ros 125 | - source devel/setup.bash 126 | # Run the tests, ensuring the path is set correctly. 127 | - source devel/setup.bash 128 | - catkin build panda_simulator # --catkin-make-args run_tests && catkin_test_results -------------------------------------------------------------------------------- /panda_sim_controllers/src/panda_velocity_controller.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_velocity_controller.cpp (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #include 30 | #include 31 | 32 | namespace panda_sim_controllers { 33 | bool PandaVelocityController::init(panda_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n){ 34 | if(!panda_sim_controllers::JointArrayController::init(hw, n)) { 35 | return false; 36 | } else { 37 | std::string topic_name; 38 | if (n.getParam("topic", topic_name)) { 39 | ros::NodeHandle nh("~"); 40 | sub_joint_command_ = nh.subscribe(topic_name, 1, &PandaVelocityController::jointCommandCB, this); 41 | } else { 42 | sub_joint_command_ = n.subscribe("arm/joint_command", 1, &PandaVelocityController::jointCommandCB, this); 43 | } 44 | std::string topic_joint_controller_gains; 45 | if (n.getParam("topic_joint_controller_gains", topic_joint_controller_gains)) { 46 | ros::NodeHandle nh("~"); 47 | sub_joint_ctrl_gains_ = nh.subscribe(topic_joint_controller_gains, 1, &PandaVelocityController::jointCtrlGainsCB, this); 48 | } else { 49 | sub_joint_ctrl_gains_ = n.subscribe("arm/joint_velocity_control_gains", 1, &PandaVelocityController::jointCtrlGainsCB, this); 50 | } 51 | // Start realtime state publisher 52 | controller_states_publisher_.reset( 53 | new realtime_tools::RealtimePublisher(n, "/panda_simulator/motion_controller/arm/joint_controller_states", 1)); 54 | if (!n.getParam("/robot_config/joint_names", controller_states_publisher_->msg_.names) ) { 55 | ROS_ERROR( 56 | "PandaVelocityController: Invalid or no joint_names parameters provided, aborting " 57 | "controller init!"); 58 | return false; 59 | } 60 | controller_states_publisher_->msg_.controller_name = "velocity_joint_velocity_controller"; 61 | 62 | 63 | t_ = boost::thread(&PandaVelocityController::publishControllerState, this); 64 | } 65 | // start joint_controller_states publisher 66 | return true; 67 | } 68 | 69 | void PandaVelocityController::jointCtrlGainsCB(const franka_core_msgs::JointControllerStatesConstPtr& msg) { 70 | for (size_t i = 0; i < msg->names.size(); i++) 71 | { 72 | controllers_[msg->names[i]]->setGains(msg->joint_controller_states[i].p, msg->joint_controller_states[i].i, 73 | msg->joint_controller_states[i].d, msg->joint_controller_states[i].i_clamp, 74 | 0, msg->joint_controller_states[i].antiwindup); 75 | } 76 | } 77 | 78 | void PandaVelocityController::publishControllerState(){ 79 | 80 | ros::Rate loop_rate(50); 81 | 82 | // double _unused; 83 | while (ros::ok()) 84 | { 85 | if ((controller_states_publisher_->trylock()) && (controller_states_publisher_->msg_.names.size() <= controllers_.size())) { 86 | std::vector jcs_vec; 87 | for (size_t i = 0; i < controller_states_publisher_->msg_.names.size(); i++) 88 | { 89 | jcs_vec.push_back(controllers_[controller_states_publisher_->msg_.names[i]]->getCurrentControllerState()); 90 | } 91 | controller_states_publisher_->msg_.joint_controller_states = jcs_vec; 92 | controller_states_publisher_->msg_.header.stamp = ros::Time::now(); 93 | controller_states_publisher_->unlockAndPublish(); 94 | } 95 | 96 | loop_rate.sleep(); 97 | } 98 | } 99 | 100 | void PandaVelocityController::jointCommandCB(const franka_core_msgs::JointCommandConstPtr& msg) { 101 | if (msg->mode == franka_core_msgs::JointCommand::VELOCITY_MODE) { 102 | std::vector commands; 103 | 104 | if (msg->names.size() != msg->velocity.size()) { 105 | ROS_ERROR_STREAM_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "Velocity commands size does not match joints size"); 106 | } 107 | 108 | for (int i = 0; i < msg->names.size(); i++) { 109 | Command cmd = Command(); 110 | cmd.name_ = msg->names[i]; 111 | cmd.velocity_ = msg->velocity[i]; 112 | commands.push_back(cmd); 113 | } 114 | command_buffer_.writeFromNonRT(commands); 115 | new_command_ = true; 116 | } 117 | } 118 | 119 | 120 | void PandaVelocityController::setCommands() { 121 | // set the new commands for each controller 122 | 123 | std::vector command = *(command_buffer_.readFromRT()); 124 | for (auto it = command.begin(); it != command.end(); it++) { 125 | controllers_[it->name_]->setCommand(it->velocity_); 126 | } 127 | } 128 | } 129 | 130 | PLUGINLIB_EXPORT_CLASS(panda_sim_controllers::PandaVelocityController, controller_interface::ControllerBase) 131 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_gripper_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | 3 | * 4 | * @package: panda_sim_controllers 5 | * @metapackage: panda_simulator 6 | * @author: Saif Sidhik 7 | * 8 | 9 | **************************************************************************/ 10 | 11 | /********************************************************************* 12 | * Copyright (c) 2019-2021, Saif Sidhik 13 | * 14 | * Licensed under the Apache License, Version 2.0 (the "License"); 15 | * you may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at 17 | * 18 | * http://www.apache.org/licenses/LICENSE-2.0 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | **************************************************************************/ 26 | 27 | #ifndef PANDA_GRIPPER_CONTROLLER_H_ 28 | #define PANDA_GRIPPER_CONTROLLER_H_ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include // used for controlling individual joints 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | 49 | 50 | 51 | namespace panda_sim_controllers 52 | { 53 | class PandaGripperController : public controller_interface::Controller 54 | { 55 | 56 | 57 | private: 58 | 59 | ros::NodeHandle nh_; 60 | 61 | // Last Gripper Command Position 62 | realtime_tools::RealtimeBox< std::shared_ptr > gripper_pos_command_buffer_; 63 | realtime_tools::RealtimeBox< std::shared_ptr > gripper_speed_command_buffer_; 64 | 65 | actionlib::SimpleActionServer homing_action_server; 66 | actionlib::SimpleActionServer move_action_server; 67 | actionlib::SimpleActionServer grasp_action_server; 68 | actionlib::SimpleActionServer stop_action_server; 69 | 70 | // Indices for mimic and orignal joints 71 | int mimic_idx_; 72 | int main_idx_; 73 | 74 | size_t n_joints_; 75 | double start_position_; 76 | double last_position_; 77 | // An indicator to evalute to true when an unproccessed new command is in the realtime buffer 78 | bool new_command_; 79 | size_t update_counter_; 80 | // franka_core_msgs::IONodeConfiguration config_; 81 | 82 | // Command subscriber 83 | // ros::Subscriber gripper_command_sub_; 84 | // // State publisher 85 | ros::Publisher state_pub_; 86 | 87 | sensor_msgs::JointState joint_state_msg_; 88 | // // Config publisher 89 | // ros::Publisher gripper_config_pub_; 90 | // // Higher level end effector config publisher 91 | // ros::Publisher end_effector_config_pub_; 92 | // // Timer to update publishers 93 | ros::Timer pub_timer_; 94 | 95 | // franka::GripperState gripper_state_; 96 | std::mutex gripper_state_mutex_; 97 | /** 98 | * @brief Regular update for publishing config and state 99 | * @param event ROS Timer event triggered 100 | */ 101 | void timerUpdate(const ros::TimerEvent& event); 102 | 103 | /** 104 | * @brief Retrieves new command and updates both fingers controllers 105 | */ 106 | void updateCommands(); 107 | 108 | 109 | // Create an effort-based joint position controller for every joint 110 | std::vector< 111 | std::shared_ptr< 112 | panda_effort_controllers::JointPositionController> > gripper_controllers_; 113 | 114 | /** 115 | * Calls the libfranka move service of the gripper 116 | * 117 | * @param[in] gripper A gripper instance to execute the command 118 | * @param[in] goal A move goal with target width and velocity 119 | * 120 | * @return True if command was successful, false otherwise. 121 | */ 122 | void move(const franka_gripper::MoveGoalConstPtr& goal); 123 | 124 | /** 125 | * Calls the libfranka homing service of the gripper 126 | * 127 | * @param[in] gripper A gripper instance to execute the command 128 | * 129 | * @return True if command was successful, false otherwise. 130 | */ 131 | void homing(const franka_gripper::HomingGoalConstPtr& /*goal*/); 132 | 133 | /** 134 | * Calls the libfranka stop service of the gripper to stop applying force 135 | * 136 | * @param[in] gripper A gripper instance to execute the command 137 | * 138 | * @return True if command was successful, false otherwise. 139 | */ 140 | void stop(const franka_gripper::StopGoalConstPtr& /*goal*/); 141 | 142 | /** 143 | * Calls the libfranka grasp service of the gripper 144 | * 145 | * An object is considered grasped if the distance \f$d\f$ between the gripper fingers satisfies 146 | * \f$(\text{width} - \text{epsilon_inner}) < d < (\text{width} + \text{epsilon_outer})\f$. 147 | * 148 | * @param[in] gripper A gripper instance to execute the command 149 | * @param[in] goal A grasp goal with target width, epsilon_inner, epsilon_outer, velocity and effort 150 | * @return True if an object has been grasped, false otherwise. 151 | */ 152 | void grasp(const franka_gripper::GraspGoalConstPtr& goal); 153 | 154 | public: 155 | PandaGripperController(); 156 | virtual ~PandaGripperController(); 157 | 158 | bool init(panda_hardware_interface::SharedJointInterface* robot, ros::NodeHandle &n) ; 159 | void starting(const ros::Time& time){} ; 160 | void stopping(const ros::Time& time){} ; 161 | void update(const ros::Time& time, const ros::Duration& period) ; 162 | 163 | 164 | }; 165 | 166 | } // namespace 167 | 168 | #endif /* PANDA_GRIPPER_CONTROLLER_H_ */ 169 | -------------------------------------------------------------------------------- /panda_simulator_examples/scripts/config/rviz_configs.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /InteractiveMarkers1 8 | Splitter Ratio: 0.742560029 9 | Tree Height: 526 10 | - Class: rviz/Help 11 | Name: Help 12 | - Class: rviz/Views 13 | Expanded: 14 | - /Current View1 15 | Name: Views 16 | Splitter Ratio: 0.5 17 | - Class: rviz_visual_tools/RvizVisualToolsGui 18 | Name: RvizVisualToolsGui 19 | Toolbars: 20 | toolButtonStyle: 2 21 | Visualization Manager: 22 | Class: "" 23 | Displays: 24 | - Alpha: 0.5 25 | Cell Size: 1 26 | Class: rviz/Grid 27 | Color: 160; 160; 164 28 | Enabled: true 29 | Line Style: 30 | Line Width: 0.0299999993 31 | Value: Lines 32 | Name: Grid 33 | Normal Cell Count: 0 34 | Offset: 35 | X: 0 36 | Y: 0 37 | Z: 0 38 | Plane: XY 39 | Plane Cell Count: 10 40 | Reference Frame: 41 | Value: true 42 | - Alpha: 1 43 | Class: rviz/RobotModel 44 | Collision Enabled: false 45 | Enabled: true 46 | Links: 47 | All Links Enabled: true 48 | Expand Joint Details: false 49 | Expand Link Details: false 50 | Expand Tree: false 51 | Link Tree Style: Links in Alphabetic Order 52 | panda_hand: 53 | Alpha: 1 54 | Show Axes: false 55 | Show Trail: false 56 | Value: true 57 | panda_leftfinger: 58 | Alpha: 1 59 | Show Axes: false 60 | Show Trail: false 61 | Value: true 62 | panda_link0: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | panda_link1: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | panda_link2: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | panda_link3: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | panda_link4: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | panda_link5: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | panda_link6: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | panda_link7: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | panda_link8: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | panda_rightfinger: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | Value: true 111 | world: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Name: RobotModel 116 | Robot Description: robot_description 117 | TF Prefix: "" 118 | Update Interval: 0 119 | Value: true 120 | Visual Enabled: true 121 | - Class: rviz/InteractiveMarkers 122 | Enable Transparency: true 123 | Enabled: true 124 | Name: InteractiveMarkers 125 | Show Axes: false 126 | Show Descriptions: true 127 | Show Visual Aids: false 128 | Update Topic: /basic_control/update 129 | Value: true 130 | Enabled: true 131 | Global Options: 132 | Background Color: 48; 48; 48 133 | Default Light: true 134 | Fixed Frame: panda_link0 135 | Frame Rate: 30 136 | Name: root 137 | Tools: 138 | - Class: rviz/Interact 139 | Hide Inactive Objects: true 140 | - Class: rviz/MoveCamera 141 | - Class: rviz/Select 142 | - Class: rviz_visual_tools/KeyTool 143 | Value: true 144 | Views: 145 | Current: 146 | Class: rviz/XYOrbit 147 | Distance: 2.37703562 148 | Enable Stereo Rendering: 149 | Stereo Eye Separation: 0.0599999987 150 | Stereo Focal Distance: 1 151 | Swap Stereo Eyes: false 152 | Value: false 153 | Focal Point: 154 | X: 0.358186603 155 | Y: -0.0820539594 156 | Z: 2.23518001e-07 157 | Focal Shape Fixed Size: true 158 | Focal Shape Size: 0.0500000007 159 | Invert Z Axis: false 160 | Name: Current View 161 | Near Clip Distance: 0.00999999978 162 | Pitch: 0.415203065 163 | Target Frame: panda_link0 164 | Value: XYOrbit (rviz) 165 | Yaw: 5.2799654 166 | Saved: ~ 167 | Window Geometry: 168 | Displays: 169 | collapsed: false 170 | Height: 752 171 | Help: 172 | collapsed: false 173 | Hide Left Dock: false 174 | Hide Right Dock: false 175 | QMainWindow State: 000000ff00000000fd0000000100000000000001560000029cfc020000000dfb000000100044006900730070006c006100790073010000003a0000029c000000c600fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b50000009e00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b6000002280000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006900000004bd0000008d0000003e00fffffffb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000308000001760000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000004440000029c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 176 | RvizVisualToolsGui: 177 | collapsed: false 178 | Views: 179 | collapsed: false 180 | Width: 1440 181 | X: 213 182 | Y: 117 183 | -------------------------------------------------------------------------------- /panda_hardware_interface/include/panda_hardware_interface/sum_command_interface.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sum_command_interface.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_hardware_interface 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 14 | * 15 | * Licensed under the Apache License, Version 2.0 (the "License"); 16 | * you may not use this file except in compliance with the License. 17 | * You may obtain a copy of the License at 18 | * 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | **************************************************************************/ 27 | 28 | #ifndef _PANDA_HARDWARE_INTERFACE___SUM_COMMAND_INTERFACE_H_ 29 | #define _PANDA_HARDWARE_INTERFACE___SUM_COMMAND_INTERFACE_H_ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace panda_hardware_interface 38 | { 39 | 40 | 41 | /** \brief Container handle used to manage and sum all the sub-JointHandles that 42 | * command a single joint. 43 | * 44 | * Manages multiple "sub" JointHandles for a single joint. Controllers can ask for 45 | * either a unique subHandle by "subType" or get the main, shared subHandle by default. 46 | * 47 | * The SumJointHandle will store the individual cmd values for each sub JointHandle and 48 | * stores the reference to the actual output &cmd_[] from the robotHwSim ej_interface. 49 | * To the individual Controllers, the JointHandle's behave the same as normal. 50 | * 51 | * Then SumJointHandle.updateCommandSum() can be called to sum the sub-Commands and 52 | * write the sum to the output &cmd_[]. 53 | */ 54 | class SumJointHandle 55 | { 56 | public: 57 | 58 | /** 59 | * \param js The JointState handle for this joint 60 | * (used for read; from robotHWSim::js_interface_.getHandle()) 61 | * \param cmd A pointer to the storage for this joint's output command effort 62 | * (write target of summed sub-handles; same one used by normal ej_interface) 63 | */ 64 | SumJointHandle(const hardware_interface::JointStateHandle& js, double* cmd) 65 | : js_(js), 66 | cmd_(cmd), 67 | name_(js.getName()) 68 | { 69 | if (!cmd) 70 | { 71 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + "'. Command pointer is null."); 72 | } 73 | } 74 | 75 | hardware_interface::JointHandle getDefaultSubHandle(const std::string& jointName) 76 | { 77 | return getSubHandle(jointName, "MAIN_SETPOINT_COMMAND"); 78 | } 79 | 80 | std::string getDefaultSubType() { 81 | return "MAIN_SETPOINT_COMMAND"; 82 | } 83 | 84 | hardware_interface::JointHandle getSubHandle(const std::string& jointName) 85 | { 86 | return getSubHandle(jointName, getDefaultSubType()); 87 | } 88 | 89 | // TODO: make create/GetSubHandle() that is for init, and may or may not create new one, 90 | // and separate getSubHandle() that will only get existing (safe for update) 91 | 92 | hardware_interface::JointHandle getSubHandle(const std::string& jointName, const std::string& subType) 93 | { 94 | SubJointHandleMap::const_iterator it = sub_handles_.find(subType); 95 | if (it == sub_handles_.end()) 96 | { 97 | // Create and register subHandle 98 | sub_commands_[subType] = 0.0; 99 | sub_handles_[subType] = hardware_interface::JointHandle(js_, &sub_commands_[subType]); 100 | 101 | ROS_DEBUG_STREAM("Creating SubHandle (" << howManySubs() << "): '" << sub_handles_[subType].getName() 102 | << "'/'" << subType << "'."); 103 | } 104 | else 105 | { 106 | // SubHandle already exists 107 | // NB: For better or worse, all the USER cmds will be using same subHandle... 108 | ROS_DEBUG_STREAM("Reusing SubHandle (" << howManySubs() << "): '" << sub_handles_[subType].getName() 109 | << "'/'" << subType << "'."); 110 | } 111 | return sub_handles_[subType]; 112 | } 113 | 114 | std::string getName() const { 115 | return name_; 116 | } 117 | 118 | void updateCommandSum() 119 | { 120 | // get and sum the individual commands for each sub interface 121 | assert(cmd_); 122 | *cmd_ = getCommand(); 123 | } 124 | 125 | /** 126 | * \brief Get sum total command 127 | */ 128 | double getCommand() 129 | { 130 | // get and sum the individual commands for each sub interface 131 | double total = 0.0; 132 | 133 | // Just iterate over the numerical cmd array until we can figure out what's going on w/ "deactivating" cmds 134 | for (std::map::const_iterator it = sub_commands_.begin(); it != sub_commands_.end(); ++it) 135 | { 136 | total += it->second; 137 | } 138 | 139 | return total; 140 | } 141 | 142 | 143 | // like getNames() for the subHandles, but add to an existing list (b/c that's how I need it atm) 144 | void appendSubNames(std::vector& namesList) 145 | { 146 | for(SubJointHandleMap::const_iterator it = sub_handles_.begin(); it != sub_handles_.end(); ++it) 147 | { 148 | namesList.push_back(it->first); 149 | } 150 | } 151 | 152 | int howManySubs() 153 | { 154 | return sub_handles_.size(); 155 | } 156 | 157 | private: 158 | typedef std::map SubJointHandleMap; 159 | 160 | std::string name_; 161 | double* cmd_; // pointer to storage for output command to ej_interface 162 | std::map sub_commands_; // storage for sub-command values 163 | 164 | SubJointHandleMap sub_handles_; 165 | 166 | hardware_interface::JointStateHandle js_; 167 | 168 | // TODO: replace use of *cmd_ w/ writing to a JointHandle from ej_interface 169 | // (pass in & store ej_interface.getHandle(j), like jsHandle is done) 170 | }; 171 | 172 | 173 | typedef std::shared_ptr SumJointHandlePtr; 174 | 175 | 176 | } // namespace 177 | 178 | 179 | #endif /*_PANDA_HARDWARE_INTERFACE___SUM_COMMAND_INTERFACE_H_*/ 180 | -------------------------------------------------------------------------------- /panda_gazebo/src/panda_robot_hw_sim.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_robot_hw_sim.cpp (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_gazebo 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #include 30 | 31 | namespace panda_gazebo 32 | { 33 | 34 | const double PandaRobotHWSim::BRAKE_VALUE = 10000.0; 35 | 36 | bool PandaRobotHWSim::initSim( 37 | const std::string& robot_namespace, 38 | ros::NodeHandle model_nh, 39 | gazebo::physics::ModelPtr parent_model, 40 | const urdf::Model *const urdf_model, 41 | std::vector transmissions) 42 | { 43 | 44 | bool ret = gazebo_ros_control::DefaultRobotHWSim::initSim(robot_namespace, 45 | model_nh, parent_model, urdf_model, transmissions); 46 | 47 | // add custom (sum) interfaces 48 | // std::cout << std::endl << "INIT " << std::endl; 49 | ret &= PandaRobotHWSim::initCustomInterfaces(); 50 | // std::cout << std::endl << "INIT CUSTOM" << std::endl; 51 | 52 | PandaRobotHWSim::initBrakes(); 53 | // std::cout << std::endl << "INIT BRAKES" << std::endl; 54 | return ret; 55 | } 56 | 57 | 58 | 59 | bool PandaRobotHWSim::initCustomInterfaces() 60 | { 61 | 62 | // Hard-code apply SumEffortInterfaces to all joints in Panda 63 | 64 | // Initialize values 65 | for(unsigned int j=0; j < n_dof_; j++) 66 | { 67 | // ROS_WARN_STREAM("Hey!! "); 68 | // ROS_WARN_STREAM("CONTROL METHOD: " << std::to_string(joint_control_methods_[j])); 69 | // ROS_WARN_STREAM("DESIRED METHOD: " << std::to_string(EFFORT)); 70 | if (joint_control_methods_[j] == EFFORT) 71 | { 72 | ROS_DEBUG_STREAM_NAMED("panda_robot_hw_sim","Loading joint '" << joint_names_[j] 73 | << "' of type '" << "panda_hardware_interface/SharedJointInterface" << "'"); 74 | 75 | panda_hardware_interface::SumJointHandlePtr sumHandle(new panda_hardware_interface::SumJointHandle( 76 | js_interface_.getHandle(joint_names_[j]), 77 | &joint_effort_command_[j])); 78 | 79 | sum_ej_handles_refs_.push_back(sumHandle); 80 | sum_ej_interface_.registerContainer(sumHandle); 81 | } 82 | else 83 | { 84 | ROS_FATAL_STREAM_NAMED("panda_robot_hw_sim","No matching hardware interface found for '" 85 | << "panda_hardware_interface/SharedJointInterface" << "' while loading interfaces for " << joint_names_[j] ); 86 | return false; 87 | } 88 | 89 | } 90 | 91 | // Register interfaces 92 | gazebo_ros_control::DefaultRobotHWSim::registerInterface(&sum_ej_interface_); 93 | 94 | return true; 95 | } 96 | 97 | void PandaRobotHWSim::initBrakes() 98 | { 99 | 100 | joint_enable_.resize(gazebo_ros_control::DefaultRobotHWSim::n_dof_); 101 | joint_disable_.resize(gazebo_ros_control::DefaultRobotHWSim::n_dof_); 102 | for(std::size_t i = 0; i != gazebo_ros_control::DefaultRobotHWSim::sim_joints_.size(); ++i) 103 | { 104 | joint_enable_[i] = gazebo_ros_control::DefaultRobotHWSim::sim_joints_[i]->GetDamping(0); 105 | if (joint_names_[i] == "panda_joint1" || 106 | joint_names_[i] == "panda_joint2" || 107 | joint_names_[i] == "panda_joint3"){ 108 | // Add brakes to j1 and j2 109 | joint_disable_[i] = BRAKE_VALUE; 110 | } 111 | else{ 112 | joint_disable_[i] = joint_enable_[i]; 113 | } 114 | } 115 | } 116 | 117 | 118 | void PandaRobotHWSim::writeSim(ros::Time time, ros::Duration period) 119 | { 120 | // NB: Majority of this function is copy paste of inherited writeSim 121 | 122 | // If the E-stop is active, joints controlled by position commands will maintain their positions. 123 | // ROS_WARN_STREAM("writeSim j_p_ " << std::to_string(joint_position_[0])); 124 | if (e_stop_active_) 125 | { 126 | if (!last_e_stop_active_) 127 | { 128 | last_joint_position_command_ = joint_position_; 129 | last_e_stop_active_ = true; 130 | } 131 | joint_position_command_ = last_joint_position_command_; 132 | } 133 | else 134 | { 135 | last_e_stop_active_ = false; 136 | } 137 | 138 | ej_sat_interface_.enforceLimits(period); 139 | ej_limits_interface_.enforceLimits(period); 140 | pj_sat_interface_.enforceLimits(period); 141 | pj_limits_interface_.enforceLimits(period); 142 | vj_sat_interface_.enforceLimits(period); 143 | vj_limits_interface_.enforceLimits(period); 144 | 145 | // for each handle in sum vector store, call 'updateCommandSum()' 146 | for (std::vector::iterator it = sum_ej_handles_refs_.begin(); it != sum_ej_handles_refs_.end(); ++it) 147 | { 148 | (*it)->updateCommandSum(); 149 | } 150 | ROS_DEBUG_STREAM_THROTTLE_NAMED(20, "panda_robot_hw_sim", "SumJoint '" << sum_ej_handles_refs_[0]->getName() << "' has (" 151 | << sum_ej_handles_refs_[0]->howManySubs() << ") subs"); 152 | 153 | 154 | for(unsigned int j=0; j < n_dof_; j++) 155 | { 156 | switch (joint_control_methods_[j]) 157 | { 158 | case EFFORT: 159 | { 160 | const double effort = e_stop_active_ ? 0 : joint_effort_command_[j]; 161 | sim_joints_[j]->SetForce(0, effort); 162 | } 163 | break; 164 | default: 165 | { 166 | ROS_WARN_STREAM_NAMED("panda_robot_hw_sim", "Non-EFFORT controlled joint on panda: '" 167 | << joint_names_[j] << "'!"); 168 | break; 169 | } 170 | } 171 | } 172 | } 173 | 174 | 175 | void PandaRobotHWSim::brakesActive(const bool active) 176 | { 177 | for(std::size_t i = 0; i != gazebo_ros_control::DefaultRobotHWSim::sim_joints_.size(); ++i) 178 | { 179 | gazebo_ros_control::DefaultRobotHWSim::sim_joints_[i]->SetDamping(0, (active ? joint_disable_[i] : joint_enable_[i])); 180 | } 181 | } 182 | 183 | void PandaRobotHWSim::eStopActive(const bool active) 184 | { 185 | PandaRobotHWSim::brakesActive(active); 186 | gazebo_ros_control::DefaultRobotHWSim::eStopActive(active); 187 | } 188 | 189 | } 190 | 191 | PLUGINLIB_EXPORT_CLASS(panda_gazebo::PandaRobotHWSim, gazebo_ros_control::RobotHWSim) 192 | -------------------------------------------------------------------------------- /panda_hardware_interface/include/panda_hardware_interface/shared_joint_interface.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from shared_joint_interface.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_hardware_interface 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 14 | * 15 | * Licensed under the Apache License, Version 2.0 (the "License"); 16 | * you may not use this file except in compliance with the License. 17 | * You may obtain a copy of the License at 18 | * 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | **************************************************************************/ 27 | 28 | #ifndef _PANDA_HARDWARE_INTERFACE___SHARED_JOINT_INTERFACE_H_ 29 | #define _PANDA_HARDWARE_INTERFACE___SHARED_JOINT_INTERFACE_H_ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace panda_hardware_interface 39 | { 40 | 41 | /** \brief Replacement for EffortJointInterface that sums cmds from multiple 42 | * handles for the same joint. 43 | * 44 | * getHandle() 45 | * Gives out multiple JointHandles (subHandles) for the same joint. 46 | * Controllers can ask for either a unique subHandle by "subType" or get the 47 | * shared main subHandle by default. 48 | * 49 | * For each joint, a SumJointHandle container can be created/registered, which 50 | * will manage the multiple (sub) JointHandles for that joint. 51 | * 52 | */ 53 | class SharedJointInterface : public hardware_interface::EffortJointInterface 54 | { 55 | public: 56 | // todo: should we make a createContainer(&js, cmd*) here so it's more RAII? 57 | 58 | void registerContainer(const SumJointHandlePtr& ctrHandle) 59 | { 60 | // using simple std container (not HWResourceMgr) 61 | _registerContainer(ctrHandle); 62 | } 63 | 64 | /** 65 | * \brief Register a new resource. 66 | * If the resource name already exists, the previously stored resource value will be replaced with \e val. 67 | * \param handle Resource value. Its type should implement a std::string getName() method. 68 | */ 69 | void _registerContainer(const SumJointHandlePtr& handle) 70 | { 71 | ContainerHandlesMap::iterator it = containers_.find(handle->getName()); 72 | if (it == containers_.end()) 73 | { 74 | containers_.insert(std::make_pair(handle->getName(), handle)); 75 | ROS_DEBUG_STREAM("Added Container '" << handle->getName() << "'"); 76 | } 77 | else 78 | { 79 | ROS_WARN_STREAM("Replacing previously registered handle '" << handle->getName() << "' in '" + 80 | hardware_interface::internal::demangledTypeName(*this) + "'."); 81 | it->second = handle; 82 | } 83 | } 84 | 85 | /** 86 | * \brief Get a resource handle by name. 87 | * \param name Resource name. 88 | * \return Resource associated to \e name. If the resource name is not found, an exception is thrown. 89 | */ 90 | SumJointHandlePtr getContainer(const std::string& name) 91 | { 92 | // using simple std container (not HWResourceMgr) 93 | return _getContainerHandle(name); 94 | } 95 | 96 | SumJointHandlePtr _getContainerHandle(const std::string& name) 97 | { 98 | ContainerHandlesMap::const_iterator it = containers_.find(name); 99 | 100 | if (it == containers_.end()) 101 | { 102 | ROS_ERROR_STREAM("Could not find Container '" << name << "' in (" << containers_.size() << ") containers. " 103 | << "Including: "); 104 | std::vector names = getNames(); 105 | for(unsigned int i=0; i < names.size(); i++) { 106 | ROS_ERROR_STREAM(" * " << names[i]); 107 | } 108 | 109 | throw hardware_interface::HardwareInterfaceException("HEY LISTEN! Could not find resource '" + name + "' in '" + 110 | hardware_interface::internal::demangledTypeName(*this) + "'." + 111 | "Out of (" + std::to_string(containers_.size()) + ") Containers."); 112 | } 113 | std::vector names = getNames(); 114 | 115 | // ROS_WARN_STREAM("HERE!! "); 116 | // ROS_WARN_STREAM("CONTAINERS SIZE: " << std::to_string(containers_.size())); 117 | return it->second; 118 | } 119 | 120 | // ------ override ej-interface : HwRsrcMgr implementation ------ 121 | 122 | hardware_interface::JointHandle getHandle(const std::string& name) // name=jointName 123 | { 124 | return _getHandle(name); 125 | } 126 | 127 | hardware_interface::JointHandle _getHandle(const std::string& name) // name=jointName 128 | { 129 | try 130 | { 131 | SumJointHandlePtr ctrHandle = getContainer(name); 132 | 133 | return getHandle(name, ctrHandle->getDefaultSubType()); 134 | } 135 | catch(const std::logic_error& e) 136 | { 137 | throw hardware_interface::HardwareInterfaceException(e.what()); 138 | } 139 | } 140 | 141 | hardware_interface::JointHandle getHandle(const std::string& name, const std::string& subType) 142 | { 143 | try 144 | { 145 | SumJointHandlePtr ctrHandle = getContainer(name); 146 | 147 | hardware_interface::JointHandle out = ctrHandle->getSubHandle(name, subType); 148 | 149 | // If ClaimPolicy type is ClaimResources, the below method claims resources, for DontClaimResources it's a no-op 150 | hardware_interface::ClaimResources::claim(this, name + '/' + subType); 151 | 152 | return out; 153 | } 154 | catch(const std::logic_error& e) 155 | { 156 | throw hardware_interface::HardwareInterfaceException(e.what()); 157 | } 158 | } 159 | 160 | // ------ override ej-interface : ResourceManager impl ------- 161 | 162 | void registerHandle(const ResourceHandleType & handle) 163 | { 164 | // FIXME: Would be nice if there was a better fitting class relationship 165 | ROS_ERROR_STREAM("Tried calling underlying registerHandle() on SumCmdsInterface!"); 166 | } 167 | 168 | std::vector getNames() const 169 | { 170 | // Hmmm.... not sure what the 'proper' behavior here should be - include both types (Containers, CmdHandles)? 171 | 172 | std::vector out; 173 | for(ContainerHandlesMap::const_iterator it = containers_.begin(); it != containers_.end(); ++it) 174 | { 175 | out.push_back(it->first); 176 | it->second->appendSubNames(out); 177 | } 178 | return out; 179 | } 180 | 181 | protected: 182 | typedef std::map ContainerHandlesMap; 183 | 184 | ContainerHandlesMap containers_; 185 | }; 186 | 187 | 188 | } // namespace 189 | 190 | 191 | #endif /* _PANDA_HARDWARE_INTERFACE___SHARED_JOINT_INTERFACE_H_ */ 192 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/joint_array_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from joint_array_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2013-2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #define JOINT_ARRAY_CONTROLLER_NAME "joint_array_controller" 37 | namespace panda_sim_controllers { 38 | template 39 | using JointControllerMap = typename std::map >; 40 | 41 | template // T = Joint(_)Controller 42 | class JointArrayController : public controller_interface::Controller 43 | { 44 | public: 45 | JointArrayController() {}; 46 | ~JointArrayController() {}; 47 | 48 | /** 49 | * Store joint commands in a generic format that is compatible with JointPositionController 50 | */ 51 | struct Command { 52 | double position_; 53 | double velocity_; 54 | double effort_; 55 | bool has_velocity_; 56 | std::string name_; 57 | }; 58 | 59 | // gazebo plugin functions 60 | // bool init(hardware_interface::EffortJointInterface* robot, ros::NodeHandle& nh); 61 | // void starting(const ros::Time& time); 62 | // void stopping(const ros::Time& time); 63 | // void update(const ros::Time& time, const ros::Duration& period); 64 | 65 | size_t n_joints_; 66 | bool new_command_; 67 | std::string control_mode_name_; 68 | 69 | // virtual void setCommands(std::map cmds); 70 | 71 | realtime_tools::RealtimeBuffer > command_buffer_; 72 | JointControllerMap controllers_; 73 | 74 | 75 | //template // T = Joint(_)Controller 76 | virtual bool init(panda_hardware_interface::SharedJointInterface* robot, ros::NodeHandle& nh) 77 | { 78 | return init(robot, nh, ""); 79 | } 80 | 81 | virtual bool init(panda_hardware_interface::SharedJointInterface* robot, ros::NodeHandle& nh, const std::string& ctrl_type) 82 | { 83 | return _initJointsControls(robot, nh, ctrl_type); 84 | } 85 | 86 | bool _initJointsControls(panda_hardware_interface::SharedJointInterface* robot, ros::NodeHandle& nh, const std::string& ctrl_type) 87 | { 88 | // Get joint sub-controllers 89 | XmlRpc::XmlRpcValue xml_struct; 90 | if (!nh.getParam("joints", xml_struct)) 91 | { 92 | ROS_ERROR_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "No 'joints' parameter in controller (namespace '%s')", nh.getNamespace().c_str()); 93 | return false; 94 | } 95 | 96 | // Make sure it's a struct 97 | if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) 98 | { 99 | ROS_ERROR_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "The 'joints' parameter is not a struct (namespace '%s')", nh.getNamespace().c_str()); 100 | return false; 101 | } 102 | 103 | // Get number of joints 104 | n_joints_ = xml_struct.size(); 105 | control_mode_name_ = nh.getNamespace(); 106 | ROS_INFO_STREAM_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "Initializing JointArrayController(type:" << control_mode_name_ <<") with " << n_joints_ << " joints."); 107 | 108 | int i = 0; // track the joint id 109 | for (XmlRpc::XmlRpcValue::iterator joint_it = xml_struct.begin(); joint_it != xml_struct.end(); ++joint_it) 110 | { 111 | // Get joint controller 112 | if (joint_it->second.getType() != XmlRpc::XmlRpcValue::TypeStruct) 113 | { 114 | ROS_ERROR_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", 115 | nh.getNamespace().c_str()); 116 | return false; 117 | } 118 | 119 | // Get joint controller name 120 | std::string joint_controller_name = joint_it->first; 121 | 122 | // Get the joint name 123 | std::string joint_name = joint_it->second["joint"]; 124 | 125 | // Get the joint-namespace nodehandle 126 | { 127 | ros::NodeHandle joint_nh(nh, "joints/" + joint_controller_name); 128 | ROS_INFO_STREAM_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "Loading sub-controller '" << joint_controller_name 129 | << "', Namespace: " << joint_nh.getNamespace()); 130 | if (ctrl_type.length() <= 0) 131 | { 132 | // This is a main ctrl command 133 | controllers_[joint_name].reset(new T()); 134 | controllers_[joint_name]->init(robot, joint_nh); 135 | } 136 | else 137 | { 138 | // Acquire a compounded sub-command handle - we're going to Add (+) our command to the main ctrl cmd 139 | controllers_[joint_name].reset(new T()); 140 | controllers_[joint_name]->init(robot, joint_nh, ctrl_type); 141 | } 142 | 143 | } // end of joint-namespaces 144 | 145 | // increment joint i 146 | ++i; 147 | } 148 | 149 | return true; 150 | } 151 | 152 | //template 153 | void starting(const ros::Time& time) { 154 | ROS_DEBUG_STREAM_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "STARTING - JointArrayController for '" << control_mode_name_ << "' mode."); 155 | 156 | for (auto it = controllers_.begin(); it != controllers_.end(); it++) { 157 | it->second->starting(time); 158 | } 159 | } 160 | 161 | //template 162 | void stopping(const ros::Time& time) { 163 | ROS_DEBUG_STREAM_NAMED(JOINT_ARRAY_CONTROLLER_NAME, "Stopping - JointArrayController for '" << control_mode_name_ << "' mode."); 164 | 165 | } 166 | 167 | virtual void setCommands() {}; 168 | 169 | //template 170 | void update(const ros::Time& time, const ros::Duration& period) { 171 | // first check if there are new commands 172 | if (new_command_) { 173 | // assume we will succeed at this command 174 | new_command_ = false; 175 | 176 | // set the new commands for all controllers 177 | setCommands(); 178 | } 179 | 180 | // always update all controllers 181 | for (auto it = controllers_.begin(); it != controllers_.end(); it++) { 182 | it->second->update(time, period); 183 | } 184 | } 185 | }; 186 | } 187 | -------------------------------------------------------------------------------- /panda_sim_controllers/src/panda_joint_velocity_controller.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_joint_velocity_controller.cpp (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /********************************************************************* 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * 15 | * Licensed under the Apache License, Version 2.0 (the "License"); 16 | * you may not use this file except in compliance with the License. 17 | * You may obtain a copy of the License at 18 | * 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | **************************************************************************/ 27 | 28 | /********************************************************************* 29 | * Software License Agreement (BSD License) 30 | * 31 | * Copyright (c) 2008, Willow Garage, Inc. 32 | * Copyright (c) 2012, hiDOF, Inc. 33 | * All rights reserved. 34 | * 35 | * Redistribution and use in source and binary forms, with or without 36 | * modification, are permitted provided that the following conditions 37 | * are met: 38 | * 39 | * * Redistributions of source code must retain the above copyright 40 | * notice, this list of conditions and the following disclaimer. 41 | * * Redistributions in binary form must reproduce the above 42 | * copyright notice, this list of conditions and the following 43 | * disclaimer in the documentation and/or other materials provided 44 | * with the distribution. 45 | * * Neither the name of the Willow Garage nor the names of its 46 | * contributors may be used to endorse or promote products derived 47 | * from this software without specific prior written permission. 48 | * 49 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 50 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 51 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 52 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 53 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 54 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 55 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 56 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 57 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 58 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 59 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 60 | * POSSIBILITY OF SUCH DAMAGE. 61 | *********************************************************************/ 62 | 63 | #include 64 | #include 65 | 66 | 67 | namespace panda_effort_controllers { 68 | 69 | JointVelocityController::JointVelocityController() 70 | : command_(0), loop_count_(0) 71 | {} 72 | 73 | JointVelocityController::~JointVelocityController() 74 | { 75 | sub_command_.shutdown(); 76 | } 77 | 78 | bool JointVelocityController::init(panda_hardware_interface::SharedJointInterface *robot, 79 | const std::string &joint_name, const control_toolbox::Pid &pid) 80 | { 81 | pid_controller_ = pid; 82 | 83 | // Get joint handle from hardware interface 84 | joint_ = robot->getHandle(joint_name); 85 | 86 | return true; 87 | } 88 | 89 | bool JointVelocityController::init(panda_hardware_interface::SharedJointInterface *robot, ros::NodeHandle &n) 90 | { 91 | // Get joint name from parameter server 92 | std::string joint_name; 93 | if (!n.getParam("joint", joint_name)) { 94 | ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str()); 95 | return false; 96 | } 97 | 98 | // Get joint handle from hardware interface 99 | joint_ = robot->getHandle(joint_name); 100 | 101 | // Load PID Controller using gains set on parameter server 102 | if (!pid_controller_.init(ros::NodeHandle(n, "pid"))) 103 | return false; 104 | 105 | // Start realtime state publisher 106 | // controller_state_publisher_.reset( 107 | // new realtime_tools::RealtimePublisher 108 | // (n, "state", 1)); 109 | 110 | // Start command subscriber 111 | // sub_command_ = n.subscribe("command", 1, &JointVelocityController::setCommandCB, this); 112 | 113 | return true; 114 | } 115 | 116 | void JointVelocityController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup) 117 | { 118 | pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup); 119 | } 120 | 121 | void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) 122 | { 123 | pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup); 124 | } 125 | 126 | void JointVelocityController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) 127 | { 128 | bool dummy; 129 | pid_controller_.getGains(p,i,d,i_max,i_min,dummy); 130 | } 131 | 132 | void JointVelocityController::printDebug() 133 | { 134 | pid_controller_.printValues(); 135 | } 136 | 137 | control_msgs::JointControllerState JointVelocityController::getCurrentControllerState() 138 | { 139 | return curr_state_; 140 | } 141 | 142 | std::string JointVelocityController::getJointName() 143 | { 144 | return joint_.getName(); 145 | } 146 | 147 | // Set the joint velocity command 148 | void JointVelocityController::setCommand(double cmd) 149 | { 150 | command_ = cmd; 151 | } 152 | 153 | // Return the current velocity command 154 | void JointVelocityController::getCommand(double& cmd) 155 | { 156 | cmd = command_; 157 | } 158 | 159 | void JointVelocityController::starting(const ros::Time& time) 160 | { 161 | command_ = 0.0; 162 | pid_controller_.reset(); 163 | } 164 | 165 | void JointVelocityController::update(const ros::Time& time, const ros::Duration& period) 166 | { 167 | double error = command_ - joint_.getVelocity(); 168 | 169 | // Set the PID error and compute the PID command with nonuniform time 170 | // step size. The derivative error is computed from the change in the error 171 | // and the timestep dt. 172 | double commanded_effort = pid_controller_.computeCommand(error, period); 173 | 174 | joint_.setCommand(commanded_effort); 175 | 176 | { 177 | double dummy; 178 | bool antiwindup; 179 | curr_state_.header.stamp = time; 180 | curr_state_.set_point = command_; 181 | curr_state_.process_value = joint_.getVelocity(); 182 | curr_state_.error = error; 183 | curr_state_.time_step = period.toSec(); 184 | curr_state_.command = commanded_effort; 185 | 186 | getGains(curr_state_.p, curr_state_.i, curr_state_.d, curr_state_.i_clamp, dummy, antiwindup); 187 | curr_state_.antiwindup = static_cast(antiwindup); 188 | } 189 | } 190 | 191 | void JointVelocityController::setCommandCB(const std_msgs::Float64ConstPtr& msg) 192 | { 193 | command_ = msg->data; 194 | } 195 | 196 | } // namespace 197 | 198 | PLUGINLIB_EXPORT_CLASS( panda_effort_controllers::JointVelocityController, controller_interface::ControllerBase) 199 | -------------------------------------------------------------------------------- /panda_simulator_examples/scripts/task_space_control_with_fri.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # /*************************************************************************** 4 | 5 | # 6 | # @package: panda_siimulator_examples 7 | # @metapackage: panda_simulator 8 | # @author: Saif Sidhik 9 | # 10 | 11 | # **************************************************************************/ 12 | 13 | # /*************************************************************************** 14 | # Copyright (c) 2019-2021, Saif Sidhik 15 | 16 | # Licensed under the Apache License, Version 2.0 (the "License"); 17 | # you may not use this file except in compliance with the License. 18 | # You may obtain a copy of the License at 19 | 20 | # http://www.apache.org/licenses/LICENSE-2.0 21 | 22 | # Unless required by applicable law or agreed to in writing, software 23 | # distributed under the License is distributed on an "AS IS" BASIS, 24 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | # See the License for the specific language governing permissions and 26 | # limitations under the License. 27 | # **************************************************************************/ 28 | 29 | """ 30 | This is a demo showing task-space control on the 31 | simulator robot using the Franka ROS Interface 32 | (https://github.com/justagist/franka_ros_interface). 33 | 34 | ( A more unified package called panda_robot is 35 | available which simplifies and combines all the 36 | functionalities of Franka ROS Interface and 37 | provides a simpler and intuitive python API. 38 | https://github.com/justagist/panda_robot ) 39 | 40 | The task-space force for the desired pose is computed 41 | using a simple PD law, and the corresponding 42 | joint torques are computed and sent to the robot. 43 | 44 | USAGE: 45 | After launching the simulator (panda_world.launch), 46 | run this demo using the command: 47 | 48 | roslaunch panda_simulator_examples demo_task_space_control.launch --use_fri:=true 49 | 50 | """ 51 | 52 | import copy 53 | import rospy 54 | import threading 55 | import quaternion 56 | import numpy as np 57 | from geometry_msgs.msg import Point 58 | from visualization_msgs.msg import * 59 | from interactive_markers.interactive_marker_server import * 60 | from franka_interface import ArmInterface 61 | 62 | # -- add to pythonpath for finding rviz_markers.py 63 | import sys, os 64 | sys.path.append(os.path.dirname(os.path.abspath(__file__))) 65 | # ------------------------------------------------- 66 | 67 | from rviz_markers import RvizMarkers 68 | 69 | # --------- Modify as required ------------ 70 | # Task-space controller parameters 71 | # stiffness gains 72 | P_pos = 50. 73 | P_ori = 25. 74 | # damping gains 75 | D_pos = 10. 76 | D_ori = 1. 77 | # ----------------------------------------- 78 | publish_rate = 100 79 | 80 | JACOBIAN = None 81 | CARTESIAN_POSE = None 82 | CARTESIAN_VEL = None 83 | 84 | destination_marker = RvizMarkers() 85 | 86 | 87 | def quatdiff_in_euler(quat_curr, quat_des): 88 | """ 89 | Compute difference between quaternions and return 90 | Euler angles as difference 91 | """ 92 | curr_mat = quaternion.as_rotation_matrix(quat_curr) 93 | des_mat = quaternion.as_rotation_matrix(quat_des) 94 | rel_mat = des_mat.T.dot(curr_mat) 95 | rel_quat = quaternion.from_rotation_matrix(rel_mat) 96 | vec = quaternion.as_float_array(rel_quat)[1:] 97 | if rel_quat.w < 0.0: 98 | vec = -vec 99 | 100 | return -des_mat.dot(vec) 101 | 102 | def control_thread(rate): 103 | """ 104 | Actual control loop. Uses goal pose from the feedback thread 105 | and current robot states from the subscribed messages to compute 106 | task-space force, and then the corresponding joint torques. 107 | """ 108 | while not rospy.is_shutdown(): 109 | error = 100. 110 | while error > 0.005: 111 | # when using the panda_robot interface, the next 2 lines can be simplified 112 | # to: `curr_pos, curr_ori = panda.ee_pose()` 113 | curr_pos = robot.endpoint_pose()['position'] 114 | curr_ori = np.asarray(robot.endpoint_pose()['orientation']) 115 | 116 | delta_pos = (goal_pos - curr_pos).reshape([3,1]) 117 | delta_ori = quatdiff_in_euler(curr_ori, goal_ori).reshape([3,1]) 118 | 119 | # when using the panda_robot interface, the next 2 lines can be simplified 120 | # to: `curr_vel, curr_omg = panda.ee_velocity()` 121 | curr_vel = robot.endpoint_velocity()['linear'].reshape([3,1]) 122 | curr_omg = robot.endpoint_velocity()['angular'].reshape([3,1]) 123 | 124 | # Desired task-space force using PD law 125 | F = np.vstack([P_pos*(delta_pos), P_ori*(delta_ori)]) - \ 126 | np.vstack([D_pos*(curr_vel), D_ori*(curr_omg)]) 127 | 128 | error = np.linalg.norm(delta_pos) + np.linalg.norm(delta_ori) 129 | 130 | # panda_robot equivalent: panda.jacobian(angles[optional]) or panda.zero_jacobian() 131 | J = robot.zero_jacobian() 132 | 133 | # joint torques to be commanded 134 | tau = np.dot(J.T,F) 135 | 136 | # command robot using joint torques 137 | # panda_robot equivalent: panda.exec_torque_cmd(tau) 138 | robot.set_joint_torques(dict(list(zip(robot.joint_names(), tau)))) 139 | 140 | rate.sleep() 141 | 142 | def process_feedback(feedback): 143 | """ 144 | InteractiveMarker callback function. Update target pose. 145 | """ 146 | global goal_pos, goal_ori 147 | 148 | if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: 149 | p = feedback.pose.position 150 | q = feedback.pose.orientation 151 | goal_pos = np.array([p.x,p.y,p.z]) 152 | goal_ori = np.quaternion(q.w, q.x,q.y,q.z) 153 | 154 | def _on_shutdown(): 155 | """ 156 | Clean shutdown controller thread when rosnode dies. 157 | """ 158 | global ctrl_thread 159 | if ctrl_thread.is_alive(): 160 | ctrl_thread.join() 161 | 162 | if __name__ == "__main__": 163 | 164 | # global goal_pos, goal_ori, ctrl_thread 165 | 166 | rospy.init_node("ts_control_sim_only") 167 | 168 | # when using franka_ros_interface, the robot can be controlled through and 169 | # the robot state is directly accessible with the API 170 | # If using the panda_robot API, this will be 171 | # panda = PandaArm() 172 | robot = ArmInterface() 173 | 174 | # when using the panda_robot interface, the next 2 lines can be simplified 175 | # to: `start_pos, start_ori = panda.ee_pose()` 176 | ee_pose = robot.endpoint_pose() 177 | start_pos, start_ori = ee_pose['position'], ee_pose['orientation'] 178 | 179 | goal_pos, goal_ori = start_pos, start_ori 180 | 181 | # start controller thread 182 | rospy.on_shutdown(_on_shutdown) 183 | rate = rospy.Rate(publish_rate) 184 | ctrl_thread = threading.Thread(target=control_thread, args = [rate]) 185 | ctrl_thread.start() 186 | 187 | # ------------------------------------------------------------------------------------ 188 | server = InteractiveMarkerServer("basic_control") 189 | 190 | position = Point( start_pos[0], start_pos[1], start_pos[2]) 191 | marker = destination_marker.makeMarker( False, InteractiveMarkerControl.MOVE_ROTATE_3D, \ 192 | position, quaternion.as_float_array(start_ori), True) 193 | server.insert(marker, process_feedback) 194 | 195 | server.applyChanges() 196 | 197 | rospy.spin() 198 | # ------------------------------------------------------------------------------------ -------------------------------------------------------------------------------- /panda_gazebo/include/panda_gazebo/arm_kinematics_interface.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from arm_kinematics_interface.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_gazebo 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /*************************************************************************** 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * Copyright (c) 2018, Rethink Robotics Inc. 15 | * 16 | * Licensed under the Apache License, Version 2.0 (the "License"); 17 | * you may not use this file except in compliance with the License. 18 | * You may obtain a copy of the License at 19 | * 20 | * http://www.apache.org/licenses/LICENSE-2.0 21 | * 22 | * Unless required by applicable law or agreed to in writing, software 23 | * distributed under the License is distributed on an "AS IS" BASIS, 24 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 | * See the License for the specific language governing permissions and 26 | * limitations under the License. 27 | **************************************************************************/ 28 | #ifndef PANDA_GAZEBO_ARM_KINEMATICS_INTERFACE_H 29 | #define PANDA_GAZEBO_ARM_KINEMATICS_INTERFACE_H 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | #include "kdl/chainfksolver.hpp" 53 | 54 | #include 55 | 56 | namespace panda_gazebo 57 | { 58 | class ArmKinematicsInterface 59 | { 60 | public: 61 | /** Method to initialize and run the Kinematic Interface 62 | * \param nh ROS node handle to use 63 | * \param side string dictating which arm interface to construct 64 | * \return bool true indicating successful initialization 65 | */ 66 | bool init(ros::NodeHandle& nh); 67 | 68 | struct Kinematics 69 | { 70 | KDL::Chain chain; 71 | std::vector joint_names; 72 | 73 | }; 74 | private: 75 | std::string root_name_, tip_name_, gravity_tip_name_; 76 | urdf::Model robot_model_; 77 | franka_core_msgs::JointLimits joint_limits_; 78 | KDL::Tree tree_; 79 | std::map acceleration_map_; 80 | std::map kinematic_chain_map_; 81 | 82 | realtime_tools::RealtimeBox< std::shared_ptr > joint_command_buffer_; 83 | realtime_tools::RealtimeBox< std::shared_ptr > joint_state_buffer_; 84 | realtime_tools::RealtimeBox> ft_msg_buffer_; 85 | 86 | ros::Subscriber joint_command_sub_; 87 | ros::Subscriber joint_state_sub_; 88 | ros::Subscriber ft_sensor_sub_; 89 | 90 | ros::Publisher joint_limits_pub_; 91 | ros::Publisher endpoint_state_pub_; 92 | ros::Publisher robot_state_publisher_; 93 | long endpoint_state_seq_; 94 | long gravity_torques_seq_; 95 | 96 | ros::Timer update_timer_; 97 | 98 | KDLMethods* kdl_; 99 | 100 | 101 | // std::unique_ptr& joint_names, 196 | const franka_core_msgs::JointCommand& command_msg, 197 | franka_core_msgs::RobotState& robot_state, std::array &acc); 198 | 199 | }; 200 | } // namespace panda_gazebo 201 | #endif // PANDA_GAZEBO_ARM_KINEMATICS_INTERFACE_H 202 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_joint_velocity_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_joint_velocity_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /********************************************************************* 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * 15 | * Licensed under the Apache License, Version 2.0 (the "License"); 16 | * you may not use this file except in compliance with the License. 17 | * You may obtain a copy of the License at 18 | * 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | **************************************************************************/ 27 | 28 | /********************************************************************* 29 | * Software License Agreement (BSD License) 30 | * 31 | * Copyright (c) 2008, Willow Garage, Inc. 32 | * Copyright (c) 2012, hiDOF, Inc. 33 | * All rights reserved. 34 | * 35 | * Redistribution and use in source and binary forms, with or without 36 | * modification, are permitted provided that the following conditions 37 | * are met: 38 | * 39 | * * Redistributions of source code must retain the above copyright 40 | * notice, this list of conditions and the following disclaimer. 41 | * * Redistributions in binary form must reproduce the above 42 | * copyright notice, this list of conditions and the following 43 | * disclaimer in the documentation and/or other materials provided 44 | * with the distribution. 45 | * * Neither the name of the Willow Garage nor the names of its 46 | * contributors may be used to endorse or promote products derived 47 | * from this software without specific prior written permission. 48 | * 49 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 50 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 51 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 52 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 53 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 54 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 55 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 56 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 57 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 58 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 59 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 60 | * POSSIBILITY OF SUCH DAMAGE. 61 | *********************************************************************/ 62 | 63 | #ifndef PANDA_EFFORT_CONTROLLERS__JOINT_VELOCITY_CONTROLLER_H 64 | #define PANDA_EFFORT_CONTROLLERS__JOINT_VELOCITY_CONTROLLER_H 65 | 66 | /** 67 | @class effort_controllers::JointVelocityController 68 | @brief Joint Velocity Controller 69 | 70 | This class controls velocity using a pid loop. 71 | 72 | @section ROS ROS interface 73 | 74 | @param type Must be "effort_controllers::JointVelocityController" 75 | @param joint Name of the joint to control. 76 | @param pid Contains the gains for the PID loop around velocity. See: control_toolbox::Pid 77 | 78 | Subscribes to: 79 | 80 | - @b command (std_msgs::Float64) : The joint velocity to achieve. 81 | 82 | Publishes: 83 | 84 | - @b state (control_msgs::JointControllerState) : 85 | Current state of the controller, including pid error and gains. 86 | 87 | */ 88 | 89 | #include 90 | #include 91 | #include 92 | #include 93 | #include 94 | #include 95 | #include 96 | #include 97 | #include 98 | 99 | #include 100 | 101 | 102 | namespace panda_effort_controllers 103 | { 104 | 105 | class JointVelocityController: public controller_interface::Controller 106 | { 107 | public: 108 | 109 | JointVelocityController(); 110 | ~JointVelocityController(); 111 | 112 | bool init(panda_hardware_interface::SharedJointInterface *robot, const std::string &joint_name, const control_toolbox::Pid &pid); 113 | 114 | /** \brief The init function is called to initialize the controller from a 115 | * non-realtime thread with a pointer to the hardware interface, itself, 116 | * instead of a pointer to a RobotHW. 117 | * 118 | * \param robot The specific hardware interface used by this controller. 119 | * 120 | * \param n A NodeHandle in the namespace from which the controller 121 | * should read its configuration, and where it should set up its ROS 122 | * interface. 123 | * 124 | * \returns True if initialization was successful and the controller 125 | * is ready to be started. 126 | */ 127 | bool init(panda_hardware_interface::SharedJointInterface *robot, ros::NodeHandle &n); 128 | bool init(panda_hardware_interface::SharedJointInterface *robot, ros::NodeHandle &n, const std::string& ctrl_type) 129 | { 130 | // ctrl_type is no-op for non-effort control 131 | return init(robot, n); 132 | } 133 | 134 | /*! 135 | * \brief Give set velocity of the joint for next update: revolute (angle) and prismatic (velocity) 136 | * 137 | * \param double pos Velocity command to issue 138 | */ 139 | void setCommand(double cmd); 140 | 141 | /*! 142 | * \brief Get latest velocity command to the joint: revolute (angle) and prismatic (velocity). 143 | */ 144 | void getCommand(double & cmd); 145 | 146 | /** \brief This is called from within the realtime thread just before the 147 | * first call to \ref update 148 | * 149 | * \param time The current time 150 | */ 151 | void starting(const ros::Time& time); 152 | 153 | /*! 154 | * \brief Issues commands to the joint. Should be called at regular intervals 155 | */ 156 | void update(const ros::Time& time, const ros::Duration& period); 157 | 158 | /** 159 | * \brief Get the PID parameters 160 | */ 161 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 162 | 163 | /** 164 | * \brief Get the PID parameters 165 | */ 166 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup); 167 | 168 | /** 169 | * \brief Print debug info to console 170 | */ 171 | void printDebug(); 172 | control_msgs::JointControllerState getCurrentControllerState(); 173 | /** 174 | * \brief Get the PID parameters 175 | */ 176 | void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false); 177 | 178 | /** 179 | * \brief Get the name of the joint this controller uses 180 | */ 181 | std::string getJointName(); 182 | 183 | hardware_interface::JointHandle joint_; 184 | double command_; /**< Last commanded velocity. */ 185 | 186 | private: 187 | int loop_count_; 188 | control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ 189 | 190 | //friend class JointVelocityControllerNode; // what is this for?? 191 | 192 | // boost::scoped_ptr< 193 | // realtime_tools::RealtimePublisher< 194 | // control_msgs::JointControllerState> > controller_state_publisher_ ; 195 | 196 | ros::Subscriber sub_command_; 197 | control_msgs::JointControllerState curr_state_; 198 | /** 199 | * \brief Callback from /command subscriber for setpoint 200 | */ 201 | void setCommandCB(const std_msgs::Float64ConstPtr& msg); 202 | }; 203 | 204 | } // namespace 205 | 206 | #endif 207 | -------------------------------------------------------------------------------- /panda_sim_controllers/include/panda_sim_controllers/panda_joint_position_controller.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | * Adapted from sawyer_joint_position_controller.h (sawyer_simulator package) 3 | 4 | * 5 | * @package: panda_sim_controllers 6 | * @metapackage: panda_simulator 7 | * @author: Saif Sidhik 8 | * 9 | 10 | **************************************************************************/ 11 | 12 | /********************************************************************* 13 | * Copyright (c) 2019-2021, Saif Sidhik 14 | * 15 | * Licensed under the Apache License, Version 2.0 (the "License"); 16 | * you may not use this file except in compliance with the License. 17 | * You may obtain a copy of the License at 18 | * 19 | * http://www.apache.org/licenses/LICENSE-2.0 20 | * 21 | * Unless required by applicable law or agreed to in writing, software 22 | * distributed under the License is distributed on an "AS IS" BASIS, 23 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 24 | * See the License for the specific language governing permissions and 25 | * limitations under the License. 26 | **************************************************************************/ 27 | 28 | /********************************************************************* 29 | * Software License Agreement (BSD License) 30 | * 31 | * Copyright (c) 2008, Willow Garage, Inc. 32 | * Copyright (c) 2012, hiDOF, Inc. 33 | * All rights reserved. 34 | * 35 | * Redistribution and use in source and binary forms, with or without 36 | * modification, are permitted provided that the following conditions 37 | * are met: 38 | * 39 | * * Redistributions of source code must retain the above copyright 40 | * notice, this list of conditions and the following disclaimer. 41 | * * Redistributions in binary form must reproduce the above 42 | * copyright notice, this list of conditions and the following 43 | * disclaimer in the documentation and/or other materials provided 44 | * with the distribution. 45 | * * Neither the name of the Willow Garage nor the names of its 46 | * contributors may be used to endorse or promote products derived 47 | * from this software without specific prior written permission. 48 | * 49 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 50 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 51 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 52 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 53 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 54 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 55 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 56 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 57 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 58 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 59 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 60 | * POSSIBILITY OF SUCH DAMAGE. 61 | *********************************************************************/ 62 | 63 | #ifndef PANDA_EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H 64 | #define PANDA_EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H 65 | 66 | /** 67 | @class effort_controllers::JointPositionController 68 | @brief Joint Position Controller 69 | 70 | This class controls positon using a pid loop. 71 | 72 | @section ROS ROS interface 73 | 74 | @param type Must be "effort_controllers::JointPositionController" 75 | @param joint Name of the joint to control. 76 | @param pid Contains the gains for the PID loop around position. See: control_toolbox::Pid 77 | 78 | Subscribes to: 79 | 80 | - @b command (std_msgs::Float64) : The joint position to achieve. 81 | 82 | Publishes: 83 | 84 | - @b state (control_msgs::JointControllerState) : 85 | Current state of the controller, including pid error and gains. 86 | 87 | */ 88 | 89 | #include 90 | #include 91 | #include 92 | #include 93 | #include 94 | #include 95 | #include 96 | #include 97 | #include 98 | #include 99 | #include 100 | #include 101 | 102 | #include 103 | 104 | 105 | namespace panda_effort_controllers 106 | { 107 | 108 | class JointPositionController: public controller_interface::Controller 109 | { 110 | public: 111 | 112 | /** 113 | * \brief Store position and velocity command in struct to allow easier realtime buffer usage 114 | */ 115 | struct Commands 116 | { 117 | double position_; // Last commanded position 118 | double velocity_; // Last commanded velocity 119 | bool has_velocity_; // false if no velocity command has been specified 120 | }; 121 | 122 | JointPositionController(); 123 | ~JointPositionController(); 124 | 125 | /** \brief The init function is called to initialize the controller from a 126 | * non-realtime thread with a pointer to the hardware interface, itself, 127 | * instead of a pointer to a RobotHW. 128 | * 129 | * \param robot The specific hardware interface used by this controller. 130 | * 131 | * \param n A NodeHandle in the namespace from which the controller 132 | * should read its configuration, and where it should set up its ROS 133 | * interface. 134 | * 135 | * \returns True if initialization was successful and the controller 136 | * is ready to be started. 137 | */ 138 | bool init(panda_hardware_interface::SharedJointInterface *robot, ros::NodeHandle &n); 139 | bool init(panda_hardware_interface::SharedJointInterface *robot, ros::NodeHandle &n, const std::string& ctrl_type) 140 | { 141 | // ctrl_type is no-op for non-effort control 142 | return init(robot, n); 143 | } 144 | 145 | /*! 146 | * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) 147 | * 148 | * \param command 149 | */ 150 | void setCommand(double pos_target); 151 | 152 | /*! 153 | * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) 154 | * Also supports a target velocity 155 | * 156 | * \param pos_target - position setpoint 157 | * \param vel_target - velocity setpoint 158 | */ 159 | void setCommand(double pos_target, double vel_target); 160 | 161 | /** \brief This is called from within the realtime thread just before the 162 | * first call to \ref update 163 | * 164 | * \param time The current time 165 | */ 166 | void starting(const ros::Time& time); 167 | 168 | /*! 169 | * \brief Issues commands to the joint. Should be called at regular intervals 170 | */ 171 | void update(const ros::Time& time, const ros::Duration& period); 172 | 173 | /** 174 | * \brief Get the PID parameters 175 | */ 176 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 177 | 178 | /** 179 | * \brief Get the PID parameters 180 | */ 181 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup); 182 | 183 | /** 184 | * \brief Print debug info to console 185 | */ 186 | void printDebug(); 187 | 188 | /** 189 | * \brief Get the PID parameters 190 | */ 191 | void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false); 192 | 193 | /** 194 | * \brief Get the name of the joint this controller uses 195 | */ 196 | std::string getJointName(); 197 | 198 | control_msgs::JointControllerState getCurrentControllerState(); 199 | /** 200 | * \brief Get the current position of the joint 201 | * \return current position 202 | */ 203 | double getPosition(); 204 | 205 | hardware_interface::JointHandle joint_; 206 | urdf::JointConstSharedPtr joint_urdf_; 207 | realtime_tools::RealtimeBuffer command_; 208 | Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer 209 | 210 | private: 211 | int loop_count_; 212 | control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ 213 | 214 | // boost::scoped_ptr< 215 | // realtime_tools::RealtimePublisher< 216 | // control_msgs::JointControllerState> > controller_state_publisher_ ; 217 | 218 | ros::Subscriber sub_command_; 219 | 220 | control_msgs::JointControllerState curr_state_; 221 | 222 | 223 | /** 224 | * \brief Callback from /command subscriber for setpoint 225 | */ 226 | void setCommandCB(const std_msgs::Float64ConstPtr& msg); 227 | 228 | /** 229 | * \brief Check that the command is within the hard limits of the joint. Checks for joint 230 | * type first. Sets command to limit if out of bounds. 231 | * \param command - the input to test 232 | */ 233 | void enforceJointLimits(double &command); 234 | 235 | }; 236 | 237 | } // namespace 238 | 239 | #endif 240 | --------------------------------------------------------------------------------