├── kuka_push ├── srv │ ├── object_reset.srv │ └── rl_env.srv ├── msg │ ├── object_pose_vel.msg │ └── rlplugin_msg.msg ├── scripts │ ├── __pycache__ │ │ ├── core.cpython-36.pyc │ │ └── ReplayBuffer.cpython-36.pyc │ ├── logs │ │ ├── events.out.tfevents.1587123195.tomas-MS7B18 │ │ └── events.out.tfevents.1587123797.tomas-MS7B18 │ ├── .idea │ │ ├── misc.xml │ │ ├── libraries │ │ │ ├── ROS.xml │ │ │ └── workspace.xml │ │ ├── inspectionProfiles │ │ │ └── profiles_settings.xml │ │ ├── modules.xml │ │ ├── scripts.iml │ │ ├── ros.xml │ │ └── workspace.xml │ ├── ReplayBuffer.py │ ├── core.py │ ├── kuka_trained.py │ └── kuka_agent.py ├── saved_model │ ├── sac_KUKAPush_trained_model.index │ ├── sac_KUKAPush_trained_model.meta │ ├── sac_KUKAPush_trained_model.data-00000-of-00001 │ └── checkpoint ├── launch │ ├── environment.launch │ └── agent_training.launch ├── include │ └── kuka_push │ │ ├── marker.h │ │ ├── include.h │ │ └── kukapush_env_lib.h ├── src │ ├── kukapush_env.cpp │ ├── marker.cpp │ └── kukapush_env_lib.cpp ├── plugin │ ├── goalgazebo.cpp │ ├── goalgazebo.h │ ├── pushobject.h │ └── pushobject.cpp ├── CMakeLists.txt └── package.xml ├── img ├── scheme.png ├── kukapush.png └── kukapush_performance.png ├── kuka_moveit_config ├── launch │ ├── kuka_kr6r900sixx_moveit_sensor_manager.launch.xml │ ├── planning_pipeline.launch.xml │ ├── ros_controllers.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── chomp_planning_pipeline.launch.xml │ ├── warehouse.launch │ ├── setup_assistant.launch │ ├── joystick_control.launch │ ├── warehouse_settings.launch.xml │ ├── kuka_kr6r900sixx_moveit_controller_manager.launch.xml │ ├── default_warehouse_db.launch │ ├── moveit_rviz.launch │ ├── sensor_manager.launch.xml │ ├── run_benchmark_ompl.launch │ ├── gazebo.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── kuka_planning_execution.launch │ ├── planning_context.launch │ ├── trajectory_execution.launch.xml │ ├── demo.launch │ ├── demo_gazebo.launch │ ├── move_group.launch │ └── moveit.rviz ├── config │ ├── joint_names.yaml │ ├── sensors_3d.yaml │ ├── kinematics.yaml │ ├── fake_controllers.yaml │ ├── controllers.yaml │ ├── controllers_gazebo.yaml │ ├── chomp_planning.yaml │ ├── ros_controllers.yaml │ ├── joint_limits.yaml │ ├── kuka_kr6r900sixx.srdf │ └── ompl_planning.yaml ├── .setup_assistant ├── CMakeLists.txt └── package.xml ├── kuka_experimental ├── kuka_kr6_support │ ├── config │ │ ├── joint_names_kr6r700sixx.yaml │ │ └── joint_names_kr6r900sixx.yaml │ ├── meshes │ │ ├── kr6r900sixx │ │ │ ├── visual │ │ │ │ ├── priruba.stl │ │ │ │ └── gripper_body.stl │ │ │ └── collision │ │ │ │ ├── link_2.stl │ │ │ │ └── link_4.stl │ │ ├── kr6_agilus │ │ │ └── collision │ │ │ │ ├── link_1.stl │ │ │ │ ├── link_3.stl │ │ │ │ ├── link_5.stl │ │ │ │ ├── link_6.stl │ │ │ │ └── base_link.stl │ │ └── kr6r700sixx │ │ │ └── collision │ │ │ ├── link_2.stl │ │ │ └── link_4.stl │ ├── launch │ │ ├── load_kr6r700sixx.launch │ │ ├── load_kr6r900sixx.launch │ │ ├── test_kr6r700sixx.launch │ │ └── test_kr6r900sixx.launch │ ├── urdf │ │ ├── kr6r900sixx.xacro │ │ ├── kr6r700sixx.xacro │ │ ├── kr6r700sixx_macro.xacro │ │ └── kr6r900sixx_macro.xacro │ ├── test │ │ ├── roslaunch_test_kr6r700sixx.xml │ │ └── roslaunch_test_kr6r900sixx.xml │ ├── CMakeLists.txt │ └── package.xml ├── kuka_experimental │ ├── CMakeLists.txt │ └── package.xml └── kuka_resources │ ├── urdf │ ├── common_constants.xacro │ ├── common_colours.xacro │ └── common_materials.xacro │ ├── CMakeLists.txt │ └── package.xml ├── gazebo_objects ├── goal │ ├── model.config │ └── model.sdf └── unit_box │ ├── model.config │ └── model.sdf ├── kuka_gazebo ├── launch │ ├── gazebo_basic.launch │ ├── kuka_gazebo_controllers.launch │ ├── kuka_world.launch │ └── gazebo_moveit.launch ├── config │ └── gazebo_controllers.yaml ├── package.xml ├── CMakeLists.txt └── worlds │ └── kuka.world ├── rl_rviz_plugin ├── plugin_description.xml ├── include │ └── rl_rviz_plugin │ │ └── rl_rviz_plugin.h ├── CMakeLists.txt ├── src │ └── rl_rviz_plugin.cpp └── package.xml └── README.md /kuka_push/srv/object_reset.srv: -------------------------------------------------------------------------------- 1 | bool reset 2 | --- 3 | bool confirm 4 | -------------------------------------------------------------------------------- /img/scheme.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/img/scheme.png -------------------------------------------------------------------------------- /img/kukapush.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/img/kukapush.png -------------------------------------------------------------------------------- /kuka_push/msg/object_pose_vel.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose 2 | geometry_msgs/Twist twist 3 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/kuka_kr6r900sixx_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /img/kukapush_performance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/img/kukapush_performance.png -------------------------------------------------------------------------------- /kuka_moveit_config/config/joint_names.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"] 2 | -------------------------------------------------------------------------------- /kuka_push/msg/rlplugin_msg.msg: -------------------------------------------------------------------------------- 1 | int16 epoch 2 | int16 episode 3 | float32 prev_score 4 | float32 max_score 5 | float32 act_score 6 | bool training 7 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/sensors_3d.yaml: -------------------------------------------------------------------------------- 1 | # The name of this file shouldn't be changed, or else the Setup Assistant won't detect it 2 | sensors: 3 | - {} -------------------------------------------------------------------------------- /kuka_push/scripts/__pycache__/core.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_push/scripts/__pycache__/core.cpython-36.pyc -------------------------------------------------------------------------------- /kuka_push/saved_model/sac_KUKAPush_trained_model.index: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_push/saved_model/sac_KUKAPush_trained_model.index -------------------------------------------------------------------------------- /kuka_push/saved_model/sac_KUKAPush_trained_model.meta: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_push/saved_model/sac_KUKAPush_trained_model.meta -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/config/joint_names_kr6r700sixx.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] 2 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/config/joint_names_kr6r900sixx.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: ['joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6'] 2 | -------------------------------------------------------------------------------- /kuka_push/scripts/__pycache__/ReplayBuffer.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_push/scripts/__pycache__/ReplayBuffer.cpython-36.pyc -------------------------------------------------------------------------------- /kuka_experimental/kuka_experimental/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kuka_experimental) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /kuka_push/srv/rl_env.srv: -------------------------------------------------------------------------------- 1 | float32[3] action 2 | bool reset 3 | --- 4 | float32[26] state 5 | float32[3] desired_goal 6 | float32[3] achieved_goal 7 | float32 reward 8 | bool done 9 | -------------------------------------------------------------------------------- /kuka_push/scripts/logs/events.out.tfevents.1587123195.tomas-MS7B18: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_push/scripts/logs/events.out.tfevents.1587123195.tomas-MS7B18 -------------------------------------------------------------------------------- /kuka_push/scripts/logs/events.out.tfevents.1587123797.tomas-MS7B18: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_push/scripts/logs/events.out.tfevents.1587123797.tomas-MS7B18 -------------------------------------------------------------------------------- /kuka_push/launch/environment.launch: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | -------------------------------------------------------------------------------- /kuka_push/saved_model/sac_KUKAPush_trained_model.data-00000-of-00001: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_push/saved_model/sac_KUKAPush_trained_model.data-00000-of-00001 -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/visual/priruba.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/visual/priruba.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_1.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_3.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_5.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/link_6.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6r700sixx/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6r700sixx/collision/link_2.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6r700sixx/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6r700sixx/collision/link_4.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/collision/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/collision/link_2.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/collision/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/collision/link_4.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6_agilus/collision/base_link.stl -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/visual/gripper_body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/HEAD/kuka_experimental/kuka_kr6_support/meshes/kr6r900sixx/visual/gripper_body.stl -------------------------------------------------------------------------------- /kuka_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | manipulator: 2 | kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.04 5 | -------------------------------------------------------------------------------- /kuka_push/launch/agent_training.launch: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_resources/urdf/common_constants.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/libraries/ROS.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/launch/load_kr6r700sixx.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/launch/load_kr6r900sixx.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_manipulator_controller 3 | joints: 4 | - joint_a1 5 | - joint_a2 6 | - joint_a3 7 | - joint_a4 8 | - joint_a5 9 | - joint_a6 -------------------------------------------------------------------------------- /kuka_push/saved_model/checkpoint: -------------------------------------------------------------------------------- 1 | model_checkpoint_path: "/home/tomas/catkin_ws/src/kuka_sac/saved_model/sac_KUKAReach_trained_model" 2 | all_model_checkpoint_paths: "/home/tomas/catkin_ws/src/kuka_sac/saved_model/sac_KUKAReach_trained_model" 3 | -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/libraries/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: "" 3 | action_ns: position_trajectory_controller/follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | joints: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"] 6 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_resources/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kuka_resources) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | 9 | install(DIRECTORY urdf 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/controllers_gazebo.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: "" 3 | action_ns: position_trajectory_controller/follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | joints: ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"] 6 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/urdf/kr6r900sixx.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/urdf/kr6r700sixx.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /gazebo_objects/goal/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | goal 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gazebo_objects/unit_box/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | unit_box 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /kuka_gazebo/launch/gazebo_basic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rl_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel widget allowing simple diff-drive style robot base control. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/test/roslaunch_test_kr6r700sixx.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/test/roslaunch_test_kr6r900sixx.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /kuka_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: kuka_kr6_support 4 | relative_path: urdf/kr6.urdf 5 | xacro_args: "--inorder " 6 | SRDF: 7 | relative_path: config/kuka_kr6r900sixx.srdf 8 | CONFIG: 9 | author_name: Tomas Merva 10 | author_email: tmerva7@gmail.com 11 | generated_timestamp: 1586525044 -------------------------------------------------------------------------------- /kuka_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kuka_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/ros_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(kuka_kr6_support) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | 10 | if (CATKIN_ENABLE_TESTING) 11 | find_package(roslaunch REQUIRED) 12 | roslaunch_add_file_check(test/roslaunch_test_kr6r700sixx.xml) 13 | roslaunch_add_file_check(test/roslaunch_test_kr6r900sixx.xml) 14 | endif() 15 | 16 | install(DIRECTORY config launch meshes urdf 17 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 18 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/chomp_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/launch/test_kr6r700sixx.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/launch/test_kr6r900sixx.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/chomp_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_time_limit: 10.0 2 | max_iterations: 200 3 | max_iterations_after_collision_free: 5 4 | smoothness_cost_weight: 0.1 5 | obstacle_cost_weight: 1.0 6 | learning_rate: 0.01 7 | smoothness_cost_velocity: 0.0 8 | smoothness_cost_acceleration: 1.0 9 | smoothness_cost_jerk: 0.0 10 | ridge_factor: 0.01 11 | use_pseudo_inverse: false 12 | pseudo_inverse_ridge_factor: 1e-4 13 | joint_update_limit: 0.1 14 | collision_clearence: 0.2 15 | collision_threshold: 0.07 16 | use_stochastic_descent: true 17 | enable_failure_recovery: true 18 | max_recovery_attempts: 5 -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/scripts.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/ros.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | -------------------------------------------------------------------------------- /kuka_push/include/kuka_push/marker.h: -------------------------------------------------------------------------------- 1 | #ifndef MARKER_H 2 | #define MARKER_H 3 | 4 | #include 5 | 6 | // Class for generating marker (goal position) in RViz 7 | class RvizMarker 8 | { 9 | public: 10 | RvizMarker(ros::NodeHandle* nh); 11 | void AddMarker(); // generate new position of the marker (goal position) 12 | 13 | visualization_msgs::Marker marker; 14 | 15 | private: 16 | // Publisher for RViz a Gazebo 17 | ros::Publisher marker_pub; 18 | 19 | // Generator of random variables 20 | std::random_device rd; 21 | std::mt19937 mt; 22 | std::uniform_int_distribution x_cord; 23 | std::uniform_int_distribution y_cord; 24 | }; 25 | #endif // MARKER_H 26 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /kuka_gazebo/launch/kuka_gazebo_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/kuka_kr6r900sixx_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /kuka_push/src/kukapush_env.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Initialize KUKA_push environment 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "kukapush_env"); 7 | ros::NodeHandle nh; 8 | 9 | ros::AsyncSpinner spinner(3); 10 | spinner.start(); 11 | 12 | // Creating service server for interaction between an agent and the environment. 13 | // The server represents the environment. 14 | kukapush_env env(&nh); 15 | ros::ServiceServer service = nh.advertiseService("kukapush_server", 16 | &kukapush_env::env_step, &env); 17 | 18 | // Creating the Subscriber for getting the pose of the block from Gazebo 19 | ros::Subscriber sub = nh.subscribe("object_cord", 1, 20 | &kukapush_env::objectCallback, &env); 21 | 22 | ros::waitForShutdown(); 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /kuka_gazebo/config/gazebo_controllers.yaml: -------------------------------------------------------------------------------- 1 | #Publish all joint states 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 100 5 | 6 | # Joint trajectory controller 7 | position_trajectory_controller: 8 | type: position_controllers/JointTrajectoryController 9 | joints: 10 | - joint_a1 11 | - joint_a2 12 | - joint_a3 13 | - joint_a4 14 | - joint_a5 15 | - joint_a6 16 | gains: 17 | joint_1: {p: 80, i: 0, d: 0, i_clamp_min: 0, i_clamp_max: 0} 18 | joint_2: {p: 80, i: 0, d: 0, i_clamp_min: 0, i_clamp_max: 0} 19 | joint_3: {p: 80, i: 0, d: 0, i_clamp_min: 0, i_clamp_max: 0} 20 | joint_4: {p: 60, i: 0, d: 0, i_clamp_min: 0, i_clamp_max: 0} 21 | joint_5: {p: 60, i: 0, d: 0, i_clamp_min: 0, i_clamp_max: 0} 22 | joint_6: {p: 60, i: 0, d: 0, i_clamp_min: 0, i_clamp_max: 0} 23 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/ros_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Simulation settings for using moveit_sim_controllers 2 | moveit_sim_hw_interface: 3 | joint_model_group: arm 4 | joint_model_group_pose: home 5 | # Settings for ros_control_boilerplate control loop 6 | generic_hw_control_loop: 7 | loop_hz: 300 8 | cycle_time_error_threshold: 0.01 9 | # Settings for ros_control hardware interface 10 | hardware_interface: 11 | joints: 12 | - joint_a1 13 | - joint_a2 14 | - joint_a3 15 | - joint_a4 16 | - joint_a5 17 | - joint_a6 18 | sim_control_mode: 1 # 0: position, 1: velocity 19 | # Publish all joint states 20 | # Creates the /joint_states topic necessary in ROS 21 | joint_state_controller: 22 | type: joint_state_controller/JointStateController 23 | publish_rate: 50 24 | controller_list: 25 | [] 26 | position_trajectory_controller: 27 | type: position_controllers/JointTrajectoryController 28 | joints: 29 | [] 30 | gains: 31 | {} -------------------------------------------------------------------------------- /kuka_push/include/kuka_push/include.h: -------------------------------------------------------------------------------- 1 | /** 2 | * \file include.h 3 | * \brief Included libraries and header files 4 | * 5 | * \author Tomas Merva 6 | * \date 07.08.2020 7 | */ 8 | #ifndef INCLUDE_H 9 | #define INCLUDE_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #define NUM_OF_JOINTS 6 34 | 35 | 36 | #endif // INCLUDE_H 37 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_resources/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kuka_resources 4 | 0.1.0 5 | 6 |

7 | Shared resources for KUKA manipulators within ROS-Industrial. 8 |

9 |

10 | This package contains common urdf / xacro resources used by KUKA robot 11 | support packages within the ROS-Industrial program. 12 |

13 | 14 | G.A. vd. Hoorn (TU Delft Robotics Institute) 15 | G.A. vd. Hoorn (TU Delft Robotics Institute) 16 | Apache2 17 | 18 | http://wiki.ros.org/kuka_resources 19 | https://github.com/ros-industrial/kuka_experimental/issues 20 | https://github.com/ros-industrial/kuka_experimental 21 | 22 | catkin 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /kuka_gazebo/launch/kuka_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 | 29 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/kuka_planning_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /kuka_push/plugin/goalgazebo.cpp: -------------------------------------------------------------------------------- 1 | #include "goalgazebo.h" 2 | 3 | using namespace gazebo; 4 | 5 | 6 | // Gazebo plugin for showing a goal position 7 | GoalGazebo::GoalGazebo() 8 | { 9 | nh = new ros::NodeHandle; 10 | // subcriber for getting position of the marker (goal) 11 | sub = nh->subscribe("visualization_marker", 1, &GoalGazebo::Goal, this); 12 | 13 | // Fixed rotation of the marker (does not matter) 14 | goal_pose.Rot().X() = 0; 15 | goal_pose.Rot().Y() = 0; 16 | goal_pose.Rot().Z() = 0; 17 | goal_pose.Rot().W() = 1; 18 | } 19 | 20 | void GoalGazebo::Load(physics::ModelPtr _parent, sdf::ElementPtr) 21 | { 22 | this->model = _parent; 23 | world = this->model->GetWorld(); 24 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 25 | std::bind(&GoalGazebo::OnUpdate, this)); 26 | } 27 | 28 | void GoalGazebo::OnUpdate() 29 | { 30 | } 31 | 32 | // Setting a position of the object based on the position of the marker 33 | void GoalGazebo::Goal(const visualization_msgs::Marker& msg) 34 | { 35 | goal_pose.Pos().X() = msg.pose.position.x; 36 | goal_pose.Pos().Y() = msg.pose.position.y; 37 | goal_pose.Pos().Z() = msg.pose.position.z; 38 | this->model->SetWorldPose(goal_pose); 39 | } 40 | -------------------------------------------------------------------------------- /kuka_gazebo/launch/gazebo_moveit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/planning_context.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 | -------------------------------------------------------------------------------- /rl_rviz_plugin/include/rl_rviz_plugin/rl_rviz_plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef RlPlugin_H 2 | #define RlPlugin_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace rl_rviz_plugin 21 | { 22 | 23 | class RlPlugin : public rviz::Panel 24 | { 25 | Q_OBJECT 26 | public: 27 | RlPlugin( QWidget* parent = 0 ); 28 | 29 | protected: 30 | QLabel *epoch_label; 31 | QLabel *episode_label; 32 | QLabel *prevscore_label; 33 | QLabel *maxscore_label; 34 | 35 | QLineEdit *epoch_value; 36 | QLineEdit *episode_value; 37 | QLineEdit *prevscore_value; 38 | QLineEdit *maxscore_value; 39 | 40 | QGridLayout *grid; 41 | QGroupBox *box; 42 | 43 | QFormLayout *layout; 44 | 45 | private: 46 | ros::NodeHandle nh; 47 | ros::Subscriber sub_epoch; 48 | void epochCallback(const kuka_push::rlplugin_msg &msg); 49 | 50 | 51 | public Q_SLOTS: 52 | 53 | Q_SIGNALS: 54 | 55 | }; 56 | 57 | } 58 | 59 | #endif // RL_RVIZ_PLUGIN_H 60 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_experimental/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kuka_experimental 4 | 0.1.0 5 | Experimental packages for KUKA manipulators within ROS-Industrial. 6 | Shaun Edwards 7 | G.A. vd. Hoorn (TU Delft Robotics Institute) 8 | BSD 9 | 10 | http://wiki.ros.org/kuka_experimental 11 | https://github.com/ros-industrial/kuka_experimental/issues 12 | https://github.com/ros-industrial/kuka_experimental 13 | 14 | catkin 15 | 16 | kuka_kr10_support 17 | kuka_kr120_support 18 | kuka_kr150_support 19 | kuka_kr16_support 20 | kuka_kr210_support 21 | kuka_kr5_support 22 | kuka_kr6_support 23 | kuka_lbr_iiwa_support 24 | kuka_resources 25 | kuka_eki_hw_interface 26 | kuka_rsi_hw_interface 27 | kuka_rsi_simulator 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /kuka_push/plugin/goalgazebo.h: -------------------------------------------------------------------------------- 1 | #ifndef GOAL_PLUGIN_H 2 | #define GOAL_PLUGIN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | namespace gazebo 21 | { 22 | 23 | class GoalGazebo : public ModelPlugin 24 | { 25 | public: 26 | GoalGazebo(); // create subscriber and set fixed orientation of the object 27 | void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/); 28 | void OnUpdate(); 29 | 30 | void Goal(const visualization_msgs::Marker& msg); //callback subcribing function 31 | 32 | private: 33 | //Gazebo objects 34 | physics::ModelPtr model; 35 | gazebo::physics::WorldPtr world; 36 | 37 | //Ros objects 38 | ros::NodeHandle *nh; 39 | ros::Subscriber sub; 40 | visualization_msgs::Marker marker; 41 | 42 | //variables for poses of the object 43 | ignition::math::Pose3d goal_pose; 44 | 45 | // Pointer to the update event connection 46 | event::ConnectionPtr updateConnection; 47 | }; 48 | GZ_REGISTER_MODEL_PLUGIN(GoalGazebo) 49 | } 50 | 51 | 52 | #endif // GOAL_PLUGIN_H 53 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /kuka_push/src/marker.cpp: -------------------------------------------------------------------------------- 1 | #include "kuka_push/marker.h" 2 | 3 | 4 | RvizMarker::RvizMarker(ros::NodeHandle* nh) 5 | : mt(rd()), 6 | x_cord(700, 850), 7 | y_cord(-160, 160) 8 | { 9 | // Publisher for RViz and Gazebo 10 | marker_pub = nh->advertise("visualization_marker", 1); 11 | 12 | marker.header.frame_id = "/world"; 13 | marker.ns = "basic_shapes"; 14 | marker.id = 0; 15 | 16 | // Set the marker type 17 | marker.type = visualization_msgs::Marker::SPHERE; 18 | 19 | // Set the marker action. 20 | marker.action = visualization_msgs::Marker::ADD; 21 | 22 | // Set the scale of the marker -- 1x1x1 here means 1m on a side 23 | marker.scale.x = 0.04; 24 | marker.scale.y = 0.04; 25 | marker.scale.z = 0.04; 26 | 27 | // Set the color 28 | marker.color.r = 0.0f; 29 | marker.color.g = 1.0f; 30 | marker.color.b = 0.0f; 31 | marker.color.a = 1.0; 32 | 33 | // Set fixed marker orientation 34 | marker.pose.orientation.x = 0.0; 35 | marker.pose.orientation.y = 0.0; 36 | marker.pose.orientation.z = 0.0; 37 | marker.pose.orientation.w = 1.0; 38 | 39 | // Set fixed height of the marker 40 | marker.pose.position.z = 0.42; 41 | 42 | marker.lifetime = ros::Duration(); 43 | } 44 | 45 | void RvizMarker::AddMarker() 46 | { 47 | // publishing new goal position for RViz and Gazebo 48 | marker.header.stamp = ros::Time::now(); 49 | marker.pose.position.x = x_cord(mt)/1000.0; // generate random x_coordinate 50 | marker.pose.position.y = y_cord(mt)/1000.0; // generate random y_coordinate 51 | marker_pub.publish(marker); 52 | } 53 | -------------------------------------------------------------------------------- /kuka_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kuka_moveit_config 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the kuka_kr6r900sixx with the MoveIt! Motion Planning Framework 7 | 8 | Tomas Merva 9 | Tomas Merva 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit/issues 15 | https://github.com/ros-planning/moveit 16 | 17 | catkin 18 | 19 | moveit_ros_move_group 20 | moveit_fake_controller_manager 21 | moveit_kinematics 22 | moveit_planners_ompl 23 | moveit_ros_visualization 24 | moveit_setup_assistant 25 | joint_state_publisher 26 | robot_state_publisher 27 | tf2_ros 28 | xacro 29 | 30 | 31 | kuka_kr6_support 32 | kuka_kr6_support 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /gazebo_objects/goal/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 7 | 8 | 0.1 9 | 0 10 | 0 11 | 0.1 12 | 0 13 | 0.1 14 | 15 | 0 0 0 0 -0 0 16 | 17 | 0 18 | 0 19 | 0 20 | 0 0 0 0 -0 0 21 | 0 22 | 23 | 24 | 25 | 0.025 26 | 27 | 28 | 29 | 33 | 34 | __default__ 35 | 36 | 0 1 0 1 37 | 0 1 0 1 38 | 0 1 0 1 39 | 0 1 0 1 40 | 41 | 0 0 0 0 -0 0 42 | 0 43 | 1 44 | 45 | 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | joint_a1: 6 | has_velocity_limits: true 7 | max_velocity: 6.28318530718 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | max_position: 1.0471975512 11 | min_position: -1.0471975512 12 | joint_a2: 13 | has_velocity_limits: true 14 | max_velocity: 5.23598775598 15 | has_acceleration_limits: false 16 | max_acceleration: 0 17 | max_position: 0.1745329252 18 | min_position: -1.5707963268 19 | joint_a3: 20 | has_velocity_limits: true 21 | max_velocity: 6.28318530718 22 | has_acceleration_limits: false 23 | max_acceleration: 0 24 | max_position: 2.3561944902 25 | min_position: 0 26 | joint_a4: 27 | has_velocity_limits: true 28 | max_velocity: 6.6497044501 29 | has_acceleration_limits: false 30 | max_acceleration: 0 31 | max_position: 1.5707963268 32 | min_position: -1.5707963268 33 | joint_a5: 34 | has_velocity_limits: true 35 | max_velocity: 6.77187749774 36 | has_acceleration_limits: false 37 | max_acceleration: 0 38 | max_position: 1.9198621772 39 | min_position: -1.5707963268 40 | joint_a6: 41 | has_velocity_limits: true 42 | max_velocity: 10.7337748998 43 | has_acceleration_limits: false 44 | max_acceleration: 0 45 | max_position: 3.8397243544 46 | min_position: -3.8397243544 47 | -------------------------------------------------------------------------------- /rl_rviz_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rl_rviz_plugin) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | set(CMAKE_AUTOMOC ON) 8 | 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | message_generation 12 | roscpp 13 | rviz 14 | moveit_core 15 | moveit_ros_planning 16 | moveit_ros_planning_interface 17 | trajectory_msgs 18 | std_msgs 19 | geometry_msgs 20 | kuka_push 21 | ) 22 | 23 | 24 | find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets) 25 | find_package(Boost REQUIRED system filesystem date_time thread) 26 | 27 | set(QT_LIBRARIES Qt5::Widgets) 28 | add_definitions(-DQT_NO_KEYWORDS) 29 | 30 | 31 | catkin_package( 32 | #INCLUDE_DIRS include/rl_rviz_plugin 33 | #LIBRARIES rl_rviz_plugin 34 | CATKIN_DEPENDS 35 | message_generation 36 | roscpp 37 | rviz 38 | moveit_core 39 | moveit_ros_planning 40 | moveit_ros_planning_interface 41 | trajectory_msgs 42 | std_msgs 43 | geometry_msgs 44 | kuka_push 45 | # DEPENDS system_lib 46 | ) 47 | 48 | ########### 49 | ## Build ## 50 | ########### 51 | 52 | 53 | include_directories( 54 | include 55 | ${catkin_INCLUDE_DIRS} 56 | ${Boost_INCLUDE_DIR} 57 | ${EIGEN3_INCLUDE_DIRS} 58 | ) 59 | 60 | link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARIES} ${EIGEN3_INCLUDE_DIRS}) 61 | #add_dependencies(rl_rviz_plugin ) 62 | 63 | add_library(rl_rviz_plugin src/rl_rviz_plugin.cpp 64 | include/rl_rviz_plugin/rl_rviz_plugin.h 65 | ) 66 | target_link_libraries(rl_rviz_plugin ${QT_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${EIGEN3_INCLUDE_DIRS}) 67 | 68 | -------------------------------------------------------------------------------- /kuka_push/scripts/ReplayBuffer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | 5 | ## A simple FIFO experience replay buffer for SAC agent 6 | # 7 | class ReplayBuffer(): 8 | ## The constructor of the class 9 | # Initialize arrays 10 | # @param state_dim The dimension of state 11 | # @param act_dim The dimension of action 12 | # @param size The maximum size of the buffer 13 | # 14 | def __init__(self, state_dim, act_dim, size): 15 | self.obs1_buf = np.zeros([size, state_dim], dtype=np.float32) 16 | self.obs2_buf = np.zeros([size, state_dim], dtype=np.float32) 17 | self.acts_buf = np.zeros([size, act_dim], dtype=np.float32) 18 | self.rews_buf = np.zeros(size, dtype=np.float32) 19 | self.done_buf = np.zeros(size, dtype=np.float32) 20 | self.ptr, self.size, self.max_size = 0, 0, size 21 | 22 | ## @brief The method for storing data after each timestep 23 | # 24 | def store(self, obs, act, rew, next_obs, done): 25 | self.obs1_buf[self.ptr] = obs 26 | self.obs2_buf[self.ptr] = next_obs 27 | self.acts_buf[self.ptr] = act 28 | self.rews_buf[self.ptr] = rew 29 | self.done_buf[self.ptr] = done 30 | self.ptr = (self.ptr + 1) % self.max_size 31 | self.size = min(self.size + 1, self.max_size) 32 | 33 | ## @brief The method for random sampling a batch of transitions, 34 | # @details B={(s,a,r,s',d)} from D 35 | def sample_batch(self, batch_size=32): 36 | idxs = np.random.randint(0, self.size, size=batch_size) 37 | return dict(obs1=self.obs1_buf[idxs], 38 | obs2=self.obs2_buf[idxs], 39 | acts=self.acts_buf[idxs], 40 | rews=self.rews_buf[idxs], 41 | done=self.done_buf[idxs]) 42 | -------------------------------------------------------------------------------- /kuka_push/scripts/.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 1586618929591 27 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /rl_rviz_plugin/src/rl_rviz_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include "rl_rviz_plugin/rl_rviz_plugin.h" 2 | 3 | namespace rl_rviz_plugin 4 | { 5 | 6 | RlPlugin::RlPlugin( QWidget* parent) 7 | : rviz::Panel( parent ) 8 | { 9 | sub_epoch = nh.subscribe("/rl_states", 1, &RlPlugin::epochCallback, this); 10 | 11 | grid = new QGridLayout(this); 12 | this->setLayout(grid); 13 | box = new QGroupBox("RL state" ,this); 14 | grid->addWidget(box, 0, 0); 15 | 16 | layout = new QFormLayout(box); 17 | 18 | epoch_label = new QLabel("Epoch:"); 19 | episode_label = new QLabel("Episode:"); 20 | prevscore_label = new QLabel("Prev. Score:"); 21 | maxscore_label = new QLabel("Max Score:"); 22 | 23 | epoch_value = new QLineEdit(); 24 | episode_value = new QLineEdit(); 25 | prevscore_value = new QLineEdit(); 26 | maxscore_value = new QLineEdit(); 27 | 28 | epoch_value->setReadOnly(true); 29 | episode_value->setReadOnly(true); 30 | prevscore_value->setReadOnly(true); 31 | maxscore_value->setReadOnly(true); 32 | 33 | layout->addRow(epoch_label, epoch_value); 34 | layout->addRow(episode_label, episode_value); 35 | layout->addRow(prevscore_label, prevscore_value); 36 | layout->addRow(maxscore_label, maxscore_value); 37 | 38 | this->setLayout(layout); 39 | } 40 | 41 | 42 | 43 | void RlPlugin::epochCallback(const kuka_push::rlplugin_msg &msg) 44 | { 45 | QString epoch_str; 46 | QString episode_str; 47 | QString prevscore_str; 48 | QString maxscore_str; 49 | 50 | 51 | epoch_str = QString::number(msg.epoch, 'f', 1); 52 | epoch_value->setText(epoch_str); 53 | 54 | episode_str = QString::number(msg.episode, 'f', 1); 55 | episode_value->setText(episode_str); 56 | 57 | prevscore_str = QString::number((double)msg.prev_score, 'g', 1); 58 | prevscore_value->setText(prevscore_str); 59 | 60 | maxscore_str = QString::number((double)msg.max_score, 'g', 1); 61 | maxscore_value->setText(maxscore_str); 62 | 63 | } 64 | 65 | 66 | 67 | }//namespace 68 | 69 | #include 70 | PLUGINLIB_EXPORT_CLASS(rl_rviz_plugin::RlPlugin,rviz::Panel) 71 | -------------------------------------------------------------------------------- /kuka_push/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kuka_push) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | gazebo_ros 10 | message_generation 11 | moveit_core 12 | moveit_ros_planning 13 | moveit_ros_planning_interface 14 | roscpp 15 | rospy 16 | std_msgs 17 | trajectory_msgs 18 | visualization_msgs 19 | tf2 20 | ) 21 | 22 | find_package(gazebo 9 REQUIRED) 23 | find_package(Boost REQUIRED system filesystem date_time thread) 24 | 25 | 26 | 27 | 28 | ### Generate messages in the 'msg' folder 29 | add_message_files( 30 | FILES 31 | rlplugin_msg.msg 32 | object_pose_vel.msg 33 | ) 34 | 35 | ### Generate services in the 'srv' folder 36 | add_service_files( 37 | FILES 38 | rl_env.srv 39 | object_reset.srv 40 | ) 41 | 42 | ### Generate added messages and services with any dependencies listed here 43 | generate_messages( 44 | DEPENDENCIES 45 | std_msgs 46 | trajectory_msgs 47 | ) 48 | 49 | 50 | include_directories( 51 | include 52 | ${catkin_INCLUDE_DIRS} 53 | ${Boost_INCLUDE_DIR} 54 | ${GAZEBO_INCLUDE_DIRS} 55 | ) 56 | 57 | link_directories(${GAZEBO_LIBRARY_DIRS}) 58 | 59 | catkin_package( 60 | INCLUDE_DIRS include/kuka_push 61 | LIBRARIES 62 | CATKIN_DEPENDS 63 | gazebo_ros 64 | message_generation 65 | moveit_core 66 | moveit_ros_planning 67 | moveit_ros_planning_interface 68 | roscpp 69 | rospy 70 | std_msgs 71 | trajectory_msgs 72 | visualization_msgs 73 | tf2 74 | # DEPENDS system_lib 75 | ) 76 | 77 | add_library(pushobject_plugin plugin/pushobject.cpp) 78 | target_link_libraries(pushobject_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 79 | 80 | add_library(goalgazebo_plugin plugin/goalgazebo.cpp) 81 | target_link_libraries(goalgazebo_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 82 | 83 | 84 | add_library(kukapush_env_lib src/kukapush_env_lib.cpp) 85 | add_library(goal src/marker.cpp) 86 | 87 | add_executable(kukapush_env src/kukapush_env.cpp ) 88 | target_link_libraries(kukapush_env 89 | kukapush_env_lib 90 | goal 91 | ${catkin_LIBRARIES} 92 | ${Boost_LIBRARIES} 93 | ${EIGEN3_INCLUDE_DIRS}) 94 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kuka_kr6_support 4 | 0.1.0 5 | 6 |

7 | ROS-Industrial support for the KUKA KR 6 (and variants). 8 |

9 |

10 | This package contains configuration data, 3D models and launch files 11 | for KUKA KR 6 manipulators. This currently includes the R700 sixx and 12 | the R900 sixx. 13 |

14 |

Specifications:

15 |
    16 |
  • KR 6 R700 sixx - Default
    • 17 |
  • KR 6 R900 sixx - Default
    • 18 |
19 |

20 | Joint limits and maximum joint velocities are based on the information 21 | in the KUKA Roboter GmbH - KR AGILUS sixx - With W and C Variants - 22 | Specification version Spez KR AGILUS sixx V12, 26.03.2015. 23 | All urdfs are based on the default motion and joint velocity limits, 24 | unless noted otherwise (ie: no support for high speed joints, 25 | extended / limited motion ranges or other options). 26 |

27 |

28 | Before using any of the configuration files and / or meshes included 29 | in this package, be sure to check they are correct for the particular 30 | robot model and configuration you intend to use them with. 31 |

32 | 33 | G.A. vd. Hoorn (TU Delft Robotics Institute) 34 | Brett Hemes (3M) 35 | G.A. vd. Hoorn (TU Delft Robotics Institute) 36 | BSD 37 | 38 | http://wiki.ros.org/kuka_kr6_support 39 | https://github.com/ros-industrial/kuka_experimental/issues 40 | https://github.com/ros-industrial/kuka_experimental 41 | 42 | catkin 43 | 44 | roslaunch 45 | 46 | industrial_robot_client 47 | joint_state_publisher 48 | kuka_resources 49 | robot_state_publisher 50 | rviz 51 | xacro 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /kuka_push/plugin/pushobject.h: -------------------------------------------------------------------------------- 1 | #ifndef PUSHOBJECT_H 2 | #define PUSHOBJECT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | namespace gazebo 25 | { 26 | 27 | // Gazebo plugin for the block: getting its position and setting new random position of it 28 | class PushObject : public ModelPlugin 29 | { 30 | public: 31 | PushObject(); 32 | void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/); 33 | 34 | void OnUpdate(); 35 | 36 | //service function for setting random position of the block 37 | bool ObjectReset(kuka_push::object_reset::Request &req, 38 | kuka_push::object_reset::Response &resp); 39 | 40 | 41 | private: 42 | //Gazebo objects 43 | physics::ModelPtr model; 44 | gazebo::physics::WorldPtr world; 45 | 46 | //Ros objects 47 | ros::NodeHandle *nh; 48 | ros::ServiceServer service_reset; 49 | ros::Publisher object_cord_pub; 50 | 51 | //Custom msg 52 | kuka_push::object_pose_vel object_msg; 53 | 54 | geometry_msgs::Pose object_pose_msg; 55 | geometry_msgs::Twist object_vel_msg; 56 | geometry_msgs::TransformStamped transformStamped; 57 | 58 | //variables for poses of the object 59 | ignition::math::Pose3d new_pose; 60 | ignition::math::Pose3d object_pose; 61 | ignition::math::Vector3d object_linear_vel; 62 | ignition::math::Vector3d object_angular_vel; 63 | 64 | // Pointer to the update event connection 65 | event::ConnectionPtr updateConnection; 66 | 67 | // Generator of random variables 68 | std::random_device rd; 69 | std::mt19937 mt; 70 | std::uniform_int_distribution x_cord; 71 | std::uniform_int_distribution y_cord; 72 | 73 | }; 74 | GZ_REGISTER_MODEL_PLUGIN(PushObject) 75 | } 76 | 77 | 78 | #endif // PUSHOBJECT_H 79 | -------------------------------------------------------------------------------- /kuka_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kuka_gazebo 4 | 0.0.0 5 | The kuka_gazebo package 6 | 7 | 8 | 9 | 10 | robolab 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /kuka_push/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kuka_push 4 | 0.0.0 5 | The kuka_push package 6 | 7 | 8 | 9 | 10 | Tomas Merva 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | catkin 20 | gazebo_ros 21 | message_generation 22 | moveit_core 23 | moveit_ros_planning 24 | moveit_ros_planning_interface 25 | roscpp 26 | rospy 27 | std_msgs 28 | trajectory_msgs 29 | visualization_msgs 30 | tf2 31 | 32 | 33 | gazebo_ros 34 | message_generation 35 | moveit_core 36 | moveit_ros_planning 37 | moveit_ros_planning_interface 38 | roscpp 39 | rospy 40 | std_msgs 41 | trajectory_msgs 42 | visualization_msgs 43 | tf2 44 | 45 | 46 | gazebo_ros 47 | message_generation 48 | moveit_core 49 | moveit_ros_planning 50 | moveit_ros_planning_interface 51 | roscpp 52 | rospy 53 | std_msgs 54 | trajectory_msgs 55 | visualization_msgs 56 | tf2 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | [move_group/fake_controller_joint_states] 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 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/demo_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 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 | [/joint_states] 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 | -------------------------------------------------------------------------------- /kuka_push/plugin/pushobject.cpp: -------------------------------------------------------------------------------- 1 | #include "pushobject.h" 2 | 3 | using namespace gazebo; 4 | 5 | 6 | PushObject::PushObject() 7 | : mt(rd()), 8 | x_cord(650, 900), 9 | y_cord(-140, 140) 10 | { 11 | nh = new ros::NodeHandle; 12 | 13 | // service for reseting 14 | service_reset = nh->advertiseService("object_server", &PushObject::ObjectReset, this); 15 | 16 | // publisher for object's velocity and pose 17 | object_cord_pub = nh->advertise("object_cord", 1); 18 | 19 | //set fixed orientation and Z coordinate of the block 20 | new_pose.Rot().X() = 0; 21 | new_pose.Rot().Y() = 0; 22 | new_pose.Rot().Z() = 0; 23 | new_pose.Rot().W() = 1; 24 | new_pose.Pos().Z() = 0.42; 25 | 26 | transformStamped.header.frame_id = "world"; 27 | transformStamped.child_frame_id = "object"; 28 | } 29 | 30 | void PushObject::Load(physics::ModelPtr _parent, sdf::ElementPtr) 31 | { 32 | this->model = _parent; 33 | world = this->model->GetWorld(); 34 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 35 | std::bind(&PushObject::OnUpdate, this)); 36 | } 37 | 38 | void PushObject::OnUpdate() 39 | { 40 | //reading actual pose of the object 41 | object_pose = this->model->WorldPose(); 42 | 43 | object_msg.pose.position.x = object_pose.Pos().X(); 44 | object_msg.pose.position.y = object_pose.Pos().Y(); 45 | object_msg.pose.position.z = object_pose.Pos().Z(); 46 | 47 | object_msg.pose.orientation.x = object_pose.Rot().X(); 48 | object_msg.pose.orientation.y = object_pose.Rot().Y(); 49 | object_msg.pose.orientation.z = object_pose.Rot().Z(); 50 | object_msg.pose.orientation.w = object_pose.Rot().W(); 51 | 52 | 53 | object_linear_vel = this->model->RelativeLinearVel(); 54 | object_msg.twist.linear.x = object_linear_vel.X(); 55 | object_msg.twist.linear.y = object_linear_vel.Y(); 56 | object_msg.twist.linear.z = object_linear_vel.Z(); 57 | 58 | object_angular_vel = this->model->RelativeAngularVel(); 59 | object_msg.twist.angular.x = object_angular_vel.X(); 60 | object_msg.twist.angular.y = object_angular_vel.Y(); 61 | object_msg.twist.angular.z = object_angular_vel.Z(); 62 | 63 | //publishing the pose and velocity of the block 64 | object_cord_pub.publish(object_msg); 65 | 66 | // publish tf coordinates for RViz 67 | static tf2_ros::TransformBroadcaster br; 68 | transformStamped.header.stamp = ros::Time::now(); 69 | transformStamped.transform.translation.x = object_msg.pose.position.x; 70 | transformStamped.transform.translation.y = object_msg.pose.position.y; 71 | transformStamped.transform.translation.z = object_msg.pose.position.z; 72 | 73 | transformStamped.transform.rotation.x = object_msg.pose.orientation.x; 74 | transformStamped.transform.rotation.y = object_msg.pose.orientation.y; 75 | transformStamped.transform.rotation.z = object_msg.pose.orientation.z; 76 | transformStamped.transform.rotation.w = object_msg.pose.orientation.w; 77 | 78 | br.sendTransform(transformStamped); 79 | } 80 | 81 | 82 | bool PushObject::ObjectReset(kuka_push::object_reset::Request &req, 83 | kuka_push::object_reset::Response &resp) 84 | { 85 | //Generating random numbers for X and Y, Z will remain the same 86 | new_pose.Pos().X() = x_cord(mt)/1000.0; 87 | new_pose.Pos().Y() = y_cord(mt)/1000.0; 88 | //setting the new random position for object in Gazebo 89 | this->model->SetWorldPose(new_pose); 90 | resp.confirm = true; 91 | return true; 92 | } 93 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS_KUKA_env 2 | 3 | ## Overview 4 | This repository contains the implementation of the **Soft Actor-Critic** and **Hindsight Experience Replay** algorithms alongside with the RL robotics environment **KUKA_Push** within ROS. The goal of the environment is to push the block towards a goal position using the KUKA end-effector. [video](https://youtu.be/GN2U0PE8QBk) 5 | 6 | ![KUKA_Push environment](https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/master/img/kukapush.png?raw=true "KUKA_Push environment") 7 | 8 | ## Dependency 9 | - **ROS Melodic** [link](http://wiki.ros.org/melodic/Installation/Ubuntu) 10 | - **ros_control** [link](http://wiki.ros.org/ros_control) 11 | - **MoveIt** [link](https://moveit.ros.org/install/) 12 | - **Trac-IK** [link](https://ros-planning.github.io/moveit_tutorials/doc/trac_ik/trac_ik_tutorial.html) 13 | - **TensorFlow (Version 1.14.0)** [link](https://www.tensorflow.org/install/pip) 14 | 15 | ### Overview of ROS mechanism for KUKA_Push 16 | The agent and environment are two separate ROS nodes that communicate through ROS Services. The agent fulfils the role of the client, whereas the environment functions as the server. Before each episode, the agent sends a request with reset flag set to 1. As a result, the environment ignores the received actions and prepares itself for new episode. After resetting the environment, the agent chooses actions [X,Y,Z] based on a current state of the environment and sends them in the form of a request to the service server (the environment). Consequently, the desired [X,Y,Z] coordinates are assigned to the Trac-IK solver in order to compute a set of position waypoints that are applied to the robot for the time of 30ms. The new state is recorded after those 30 ms and thereafter sent back with other information to the agent. The interaction loop lasts 40ms. 17 | 18 | In order to get a state of the block, the object gazebo plugin has been created. The plugin functions as a publisher and a service server as well. The publisher is responsible for reading and sending object's pose and velocities each 1ms, wheareas the service server responds to the environment's request for resetting the object's position. To change a goal position in Gazebo, the other plugin has been created that subscribes the marker (representing a goal in RViz) topic. For better overview of the learning process, the RViz plugin has been created so an user can check the learning process of the agent. 19 | ![KUKA_Push mechanism](https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/master/img/scheme.png?raw=true "KUKA_Push mechanism") 20 | 21 | ## Start guide 22 | 1. Launch gazebo simulation: `roslaunch kuka_gazebo gazebo_basic.launch` 23 | 2. Start MoveIt and RViz: `roslaunch kuka_gazebo gazebo_moveit.launch` 24 | 3. Start the environment: `roslaunch kuka_push environment.launch` 25 | 4. Start the training process: `roslaunch kuka_push agent_training.launch` 26 | --- 27 | 5. Check your path for saving and reloading the neural network model 28 | - `kuka_push/scripts/kuka_agent.py` -> path where you want to save the model 29 | - `kuka_push/scripts/kuka_trained.py` -> path from where you want to load the model 30 | 31 | ## Result 32 | The learning performance of SAC+HER in the KUKA_Push environment for 30 epochs (every epoch = 500 episodes = 500*100 timesteps): 33 | 34 | ![KUKA_Push performance](https://raw.githubusercontent.com/TomasMerva/ROS_KUKA_env/master/img/kukapush_performance.png?raw=true "KUKA_Push performance") 35 | 36 | ## Acknowledgement 37 | I would like to thank **OpenAI SpinningUp** and **kuka_experimental** for their tutorials, code and packages. 38 | https://spinningup.openai.com/en/latest/ 39 | 40 | http://wiki.ros.org/kuka_experimental 41 | -------------------------------------------------------------------------------- /gazebo_objects/unit_box/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 7 | 8 | 0.166667 9 | 0 10 | 0 11 | 0.166667 12 | 0 13 | 0.166667 14 | 15 | 0 0 0 0 -0 0 16 | 17 | 0 18 | 0 19 | 0 20 | 0 0 0 0 -0 0 21 | 1 22 | 23 | 24 | 25 | 0.04 0.04 0.04 26 | 27 | 28 | 29 | 33 | 34 | __default__ 35 | 36 | 0.3 0.3 0.7 1 37 | 0.7 0.7 0.7 1 38 | 0.01 0.01 0.7 1 39 | 0 0 0 1 40 | 41 | 0 0 0 0 -0 0 42 | 0 43 | 1 44 | 45 | 46 | 0 47 | 10 48 | 0 0 0 0 -0 0 49 | 50 | 51 | 0.04 0.04 0.04 52 | 53 | 54 | 55 | 56 | 57 | 1 58 | 1 59 | 0 0 0 60 | 0 61 | 0 62 | 63 | 64 | 1 65 | 0 66 | 0 67 | 1 68 | 69 | 0 70 | 71 | 72 | 73 | 74 | 0 75 | 1e+06 76 | 77 | 78 | 0 79 | 1 80 | 1 81 | 82 | 0 83 | 0.2 84 | 1e+13 85 | 1 86 | 0.01 87 | 0 88 | 89 | 90 | 1 91 | -0.01 92 | 0 93 | 0.2 94 | 1e+13 95 | 1 96 | 97 | 98 | 99 | 100 | 101 | 0 102 | 1 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /kuka_push/include/kuka_push/kukapush_env_lib.h: -------------------------------------------------------------------------------- 1 | #ifndef KUKAPUSH_ENV_LIB_H 2 | #define KUKAPUSH_ENV_LIB_H 3 | 4 | #include 5 | #include 6 | 7 | //Class for creating KUKA_Push environment. 8 | class kukapush_env 9 | { 10 | public: 11 | /* The constructor sets fixed orientation of the gripper, initializes MoveIt 12 | * and creates the trajectory publisher and the service client 13 | * for reseting object in Gazebo 14 | */ 15 | kukapush_env(ros::NodeHandle *nh); 16 | 17 | /* The method for receiving an action from the client 18 | * and sending a current state of the environment to the client 19 | */ 20 | bool env_step(kuka_push::rl_env::Request &req, 21 | kuka_push::rl_env::Response &resp); 22 | 23 | /* The method for resetting the environment and sending 24 | * a current state of the environment thereafter to the client 25 | */ 26 | void env_reset(kuka_push::rl_env::Response &resp); 27 | 28 | // The callback method for getting the pose and velocity of the object. 29 | void objectCallback(const kuka_push::object_pose_vel::ConstPtr &msg); 30 | 31 | private: 32 | void ExecuteAction(geometry_msgs::Pose &action_pose); //Execute an action 33 | void RewardFunction(); // Binary reward function 34 | void GetObservation(); // Read and compute poses and velocities 35 | void SetObservation(kuka_push::rl_env::Response &resp); // prepare a state for sending to client 36 | 37 | //class for generating goal 38 | RvizMarker marker; 39 | 40 | //Trajectory publisher for JointTrajectoryController 41 | ros::Publisher pub_joint_publisher; 42 | 43 | //Gazebo 44 | ros::ServiceClient gazebo_client; // Gazebo client for generating new initial position of the object 45 | kuka_push::object_reset object_service; // service message for reseting pose of the object 46 | 47 | //Moveit 48 | moveit::planning_interface::MoveGroupInterface group; 49 | moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 50 | robot_model_loader::RobotModelLoader robot_model_loader; 51 | robot_model::RobotModelPtr robot_model_ptr; 52 | robot_state::RobotStatePtr current_state_ptr; 53 | robot_state::RobotStatePtr goal_state_ptr; 54 | const robot_state::JointModelGroup* joint_model_group; 55 | 56 | //ROS formats 57 | trajectory_msgs::JointTrajectory arm_command; 58 | trajectory_msgs::JointTrajectoryPoint desired_configuration; 59 | std::vector goal_joint_values; 60 | 61 | // State of the environment 62 | geometry_msgs::PoseStamped gripper_pose; // current pose of the robot 63 | geometry_msgs::Pose action_pose; // desired pose of the robot 64 | geometry_msgs::Pose object_pose; // current pose of the object 65 | geometry_msgs::Point goal_position; // goal position 66 | geometry_msgs::Point object_gripper; // object position relative to gripper 67 | geometry_msgs::Point object_goal; // object position relative to goal 68 | 69 | geometry_msgs::Twist object_vel; // object linear and angular velocity 70 | 71 | geometry_msgs::Point gripper_prev; // previous gripper's position 72 | geometry_msgs::Point gripper_speed; // linear velocity of the gripper 73 | 74 | double tolerance = 0.01; // tolerance for computing RewardFunction 75 | bool found_approach_ik; // flag for successfully finding IK solution 76 | double time_move = 0.5; // desired trajectory time between two points 77 | 78 | /* variables needed for transforming agent's actions <-1.0, 1.0> 79 | * into physical gripper's coordinates 80 | */ 81 | double delta_x = 0.19; 82 | double delta_y = 0.34; 83 | double delta_z = 0.15; 84 | double half_range_x = 0.6; 85 | double half_range_y = -0.34; 86 | double half_range_z = 0.24; 87 | 88 | // reinforcement learning variables 89 | float reward = -1.0; 90 | bool done = false; 91 | }; 92 | 93 | #endif // KUKAPUSH_ENV_LIB_H 94 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 34 | 35 | 36 | 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 | -------------------------------------------------------------------------------- /kuka_push/scripts/core.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | 4 | import numpy as np 5 | import tensorflow as tf 6 | 7 | EPS = 1e-8 8 | 9 | def placeholder(dim=None): 10 | return tf.placeholder(dtype=tf.float32, shape=(None,dim) if dim else (None,)) 11 | 12 | def placeholders(*args): 13 | return [placeholder(dim) for dim in args] 14 | 15 | def mlp(x, hidden_sizes=(32,), activation=tf.nn.relu, output_activation=None): 16 | for h in hidden_sizes[:-1]: 17 | x = tf.layers.dense(x, units=h, activation=activation) 18 | return tf.layers.dense(x, units=hidden_sizes[-1], activation=output_activation) 19 | 20 | def get_vars(scope): 21 | return [x for x in tf.global_variables() if scope in x.name] 22 | 23 | def count_vars(scope): 24 | v = get_vars(scope) 25 | return sum([np.prod(var.shape.as_list()) for var in v]) 26 | 27 | def gaussian_likelihood(x, mu, log_std): 28 | pre_sum = -0.5 * (((x-mu)/(tf.exp(log_std)+EPS))**2 + 2*log_std + np.log(2*np.pi)) 29 | return tf.reduce_sum(pre_sum, axis=1) 30 | 31 | def clip_but_pass_gradient(x, l=-1., u=1.): 32 | clip_up = tf.cast(x > u, tf.float32) 33 | clip_low = tf.cast(x < l, tf.float32) 34 | return x + tf.stop_gradient((u - x)*clip_up + (l - x)*clip_low) 35 | 36 | 37 | """ 38 | Policies 39 | """ 40 | 41 | LOG_STD_MAX = 2 42 | LOG_STD_MIN = -20 43 | 44 | 45 | def mlp_gaussian_policy(x, a, hidden_sizes, activation, output_activation): 46 | act_dim = 3 47 | net = mlp(x, hidden_sizes, activation, activation) 48 | mu = tf.layers.dense(net, act_dim, activation=output_activation) 49 | 50 | """ 51 | Because algorithm maximizes trade-off of reward and entropy, 52 | entropy must be unique to state---and therefore log_stds need 53 | to be a neural network output instead of a shared-across-states 54 | learnable parameter vector. But for deep Relu and other nets, 55 | simply sticking an activationless dense layer at the end would 56 | be quite bad---at the beginning of training, a randomly initialized 57 | net could produce extremely large values for the log_stds, which 58 | would result in some actions being either entirely deterministic 59 | or too random to come back to earth. Either of these introduces 60 | numerical instability which could break the algorithm. To 61 | protect against that, we'll constrain the output range of the 62 | log_stds, to lie within [LOG_STD_MIN, LOG_STD_MAX]. This is 63 | slightly different from the trick used by the original authors of 64 | SAC---they used tf.clip_by_value instead of squashing and rescaling. 65 | I prefer this approach because it allows gradient propagation 66 | through log_std where clipping wouldn't, but I don't know if 67 | it makes much of a difference. 68 | """ 69 | log_std = tf.layers.dense(net, act_dim, activation=tf.tanh) 70 | log_std = LOG_STD_MIN + 0.5 * (LOG_STD_MAX - LOG_STD_MIN) * (log_std + 1) 71 | 72 | std = tf.exp(log_std) 73 | pi = mu + tf.random_normal(tf.shape(mu)) * std 74 | logp_pi = gaussian_likelihood(pi, mu, log_std) 75 | return mu, pi, logp_pi 76 | 77 | def apply_squashing_func(mu, pi, logp_pi): 78 | mu = tf.tanh(mu) 79 | pi = tf.tanh(pi) 80 | # To avoid evil machine precision error, strictly clip 1-pi**2 to [0,1] range. 81 | logp_pi -= tf.reduce_sum(tf.log(clip_but_pass_gradient(1 - pi**2, l=0, u=1) + 1e-6), axis=1) 82 | return mu, pi, logp_pi 83 | 84 | 85 | """ 86 | Actor-Critics 87 | """ 88 | def mlp_actor_critic(x, a, hidden_sizes, activation=tf.nn.relu, 89 | output_activation=None, policy=mlp_gaussian_policy, action_space=None): 90 | # policy 91 | with tf.variable_scope('pi'): 92 | mu, pi, logp_pi = policy(x, a, hidden_sizes, activation, output_activation) 93 | mu, pi, logp_pi = apply_squashing_func(mu, pi, logp_pi) 94 | 95 | # make sure actions are in correct range 96 | # action_scale = 1 97 | # mu *= action_scale 98 | # pi *= action_scale 99 | 100 | # vfs 101 | vf_mlp = lambda x : tf.squeeze(mlp(x, hidden_sizes+[1], activation, None), axis=1) 102 | with tf.variable_scope('q1'): 103 | q1 = vf_mlp(tf.concat([x,a], axis=-1)) 104 | with tf.variable_scope('q1', reuse=True): 105 | q1_pi = vf_mlp(tf.concat([x,pi], axis=-1)) 106 | with tf.variable_scope('q2'): 107 | q2 = vf_mlp(tf.concat([x,a], axis=-1)) 108 | with tf.variable_scope('q2', reuse=True): 109 | q2_pi = vf_mlp(tf.concat([x,pi], axis=-1)) 110 | with tf.variable_scope('v'): 111 | v = vf_mlp(x) 112 | return mu, pi, logp_pi, q1, q2, q1_pi, q2_pi, v -------------------------------------------------------------------------------- /rl_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rl_rviz_plugin 4 | 0.0.0 5 | The rl_plugin package 6 | 7 | 8 | 9 | 10 | tomas 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | message_generation 53 | roscpp 54 | rviz 55 | moveit_core 56 | moveit_ros_planning 57 | moveit_ros_planning_interface 58 | trajectory_msgs 59 | visualization_msgs 60 | std_msgs 61 | geometry_msgs 62 | kuka_push 63 | 64 | 65 | 66 | message_generation 67 | roscpp 68 | rviz 69 | moveit_core 70 | moveit_ros_planning 71 | moveit_ros_planning_interface 72 | trajectory_msgs 73 | visualization_msgs 74 | message_generation 75 | std_msgs 76 | geometry_msgs 77 | kuka_push 78 | 79 | 80 | 81 | message_generation 82 | roscpp 83 | rviz 84 | moveit_core 85 | moveit_ros_planning 86 | moveit_ros_planning_interface 87 | trajectory_msgs 88 | visualization_msgs 89 | message_generation 90 | std_msgs 91 | geometry_msgs 92 | message_runtime 93 | kuka_push 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/kuka_kr6r900sixx.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_resources/urdf/common_colours.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_resources/urdf/common_materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | -------------------------------------------------------------------------------- /kuka_push/scripts/kuka_trained.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | from kuka_push.srv import * 5 | from kuka_push.msg import rlplugin_msg 6 | from std_msgs.msg import Bool 7 | 8 | import numpy as np 9 | from ReplayBuffer import ReplayBuffer 10 | import random 11 | import time 12 | import tensorflow as tf 13 | import core 14 | from core import get_vars 15 | 16 | 17 | class trainedAgent(): 18 | def __init__(self): 19 | self.sess = tf.Session() 20 | #this needs to be changed !!!!!!!!!!!!!!!!!!!!!!! 21 | self.model = tf.train.import_meta_graph\ 22 | ("/home/tomas/catkin_ws/src/kuka_push/saved_model/sac_KUKAPush_trained_model.meta") 23 | self.model.restore(self.sess, tf.train.latest_checkpoint("/home/tomas/catkin_ws/src/kuka_push/saved_model/")) 24 | 25 | self.graph = tf.get_default_graph() 26 | 27 | """ Placeholders """ 28 | self.state_t_ph = self.graph.get_tensor_by_name("State_t_ph:0") 29 | self.state_t1_ph = self.graph.get_tensor_by_name("State_t1_ph:0") 30 | self.act_ph = self.graph.get_tensor_by_name("Actions_ph:0") 31 | self.ret_ph = self.graph.get_tensor_by_name("Rewards_ph:0") 32 | self.done_ph = self.graph.get_tensor_by_name("Done_ph:0") 33 | 34 | """ Tensors for policy updating """ 35 | self.pi_loss = self.graph.get_tensor_by_name("pi_loss:0") 36 | self.q1_loss = self.graph.get_tensor_by_name("q1_loss:0") 37 | self.q2_loss = self.graph.get_tensor_by_name("q2_loss:0") 38 | self.v_loss = self.graph.get_tensor_by_name("v_loss:0") 39 | self.q1 = self.graph.get_tensor_by_name("q1:0") 40 | self.q2 = self.graph.get_tensor_by_name("q2:0") 41 | self.v = self.graph.get_tensor_by_name("v:0") 42 | self.logp_pi = self.graph.get_tensor_by_name("logp_pi:0") 43 | 44 | """ Ops """ 45 | self.train_pi_op = self.graph.get_operation_by_name("train_pi_op") 46 | self.train_value_op = self.graph.get_operation_by_name("train_value_op") 47 | self.target_update = self.graph.get_operation_by_name("target_update") 48 | 49 | """Action""" 50 | self.mean = self.graph.get_tensor_by_name("mean:0") 51 | self.pi = self.graph.get_tensor_by_name("pi:0") 52 | 53 | # All ops to call during one training step 54 | self.step_ops = [self.pi_loss, self.q1_loss, self.q2_loss, self.v_loss, self.q1, self.q2, self.v, self.logp_pi, 55 | self.train_pi_op, self.train_value_op, self.target_update] 56 | 57 | #self.file_writer = tf.summary.FileWriter("/home/tomas/catkin_ws/src/kuka_sac/scripts/obnovenie", self.sess.graph) 58 | 59 | self.reset = False 60 | 61 | # def update(self, obs1, obs2, acts, rews, done): 62 | # feed_dict = {self.state_t_ph: obs1, 63 | # self.state_t1_ph: obs2, 64 | # self.act_ph: acts, 65 | # self.ret_ph: rews, 66 | # self.done_ph: done, 67 | # } 68 | # outs = self.sess.run(self.step_ops, feed_dict) 69 | 70 | def getAction(self, state, deterministic=False): 71 | act_op = self.mean if deterministic else self.pi 72 | return self.sess.run(act_op, feed_dict={self.state_t_ph: state.reshape(1, -1)})[0] 73 | 74 | def resetEnv(self, msg): 75 | self.reset = msg.data 76 | 77 | 78 | if __name__ == "__main__": 79 | # intialization 80 | rospy.init_node("kuka_agent") 81 | rospy.wait_for_service("kukapush_server") 82 | step = rospy.ServiceProxy("kukapush_server", rl_env) 83 | 84 | pub = rospy.Publisher('/rl_states', rlplugin_msg, queue_size=1) 85 | msg = rlplugin_msg() 86 | 87 | tf.set_random_seed(0) 88 | np.random.seed(0) 89 | 90 | # properties of the environment 91 | state_dim = 29 92 | act_dim = 3 93 | timesteps_per_episode = 50 94 | 95 | # hyperparameters 96 | buffer_maxsize = 1000000 97 | batchSize = 256 98 | hidden_sizes = [256, 256] 99 | 100 | timestep = 0 101 | ep_len = 0 102 | 103 | # Results 104 | mean_ret = [] 105 | 106 | agent = trainedAgent() 107 | replay_buffer = ReplayBuffer(state_dim, act_dim, buffer_maxsize) 108 | 109 | rospy.Subscriber("reset_env", Bool, agent.resetEnv) 110 | 111 | 112 | def reset_env(): 113 | reset = True 114 | action = [0, 0, 0] 115 | resp = step(action, reset) 116 | obs = np.round(resp.state, decimals=6) 117 | state = np.concatenate((resp.state, resp.desired_goal), axis=None) 118 | state = np.round(state, decimals=6) 119 | return state, obs 120 | 121 | 122 | def policy_test(): 123 | global mean_ret, ep_len 124 | state, obs = reset_env() 125 | reward, ep_len, done, reset = 0, 0, False, False 126 | 127 | 128 | while not done: 129 | action = agent.getAction(state, True) 130 | 131 | """ Send request to server """ 132 | resp = step(action, reset, ep_len) 133 | 134 | """ Handle response from server """ 135 | state = np.round(np.concatenate((resp.state, resp.desired_goal), axis=None), decimals=6) 136 | reward = resp.reward 137 | done = resp.done 138 | 139 | """ Cas """ 140 | ep_len += 1 141 | 142 | if reward == 0.0: 143 | done = True 144 | if ep_len == 50: 145 | done = True 146 | 147 | if reward == 0.0: 148 | mean_ret.append(1) 149 | elif reward == -1.0: 150 | mean_ret.append(0) 151 | 152 | 153 | msg.training = False 154 | state, obs = reset_env() 155 | reset = False 156 | while True: 157 | if agent.reset: 158 | state, obs = reset_env() 159 | reset, agent.reset = False, False 160 | 161 | action = agent.getAction(state, True) 162 | 163 | """ Send request to server """ 164 | resp = step(action, reset) 165 | """ Handle response from server """ 166 | state = np.round(np.concatenate((resp.state, resp.desired_goal), axis=None), decimals=6) 167 | 168 | if resp.reward == 0.0: 169 | msg.act_score = 1 170 | elif resp.reward== -1.0: 171 | msg.act_score = 0 172 | pub.publish(msg) 173 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/urdf/kr6r700sixx_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | -------------------------------------------------------------------------------- /kuka_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kuka_gazebo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES kuka_gazebo 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/kuka_gazebo.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/kuka_gazebo_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_kuka_gazebo.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /kuka_experimental/kuka_kr6_support/urdf/kr6r900sixx_macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | -------------------------------------------------------------------------------- /kuka_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBL: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | EST: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECE: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECE: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECE: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRT: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnect: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstar: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRT: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRM: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstar: 54 | type: geometric::PRMstar 55 | FMT: 56 | type: geometric::FMT 57 | num_samples: 1000 # number of states that the planner should sample. default: 1000 58 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 59 | nearest_k: 1 # use Knearest strategy. default: 1 60 | cache_cc: 1 # use collision checking cache. default: 1 61 | heuristics: 0 # activate cost to go heuristics. default: 0 62 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 63 | BFMT: 64 | type: geometric::BFMT 65 | num_samples: 1000 # number of states that the planner should sample. default: 1000 66 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 67 | nearest_k: 1 # use the Knearest strategy. default: 1 68 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 69 | optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 70 | heuristics: 1 # activates cost to go heuristics. default: 1 71 | cache_cc: 1 # use the collision checking cache. default: 1 72 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 73 | PDST: 74 | type: geometric::PDST 75 | STRIDE: 76 | type: geometric::STRIDE 77 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 78 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 79 | use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 80 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 81 | max_degree: 18 # max degree of a node in the GNAT. default: 12 82 | min_degree: 12 # min degree of a node in the GNAT. default: 12 83 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 84 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 85 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 86 | BiTRRT: 87 | type: geometric::BiTRRT 88 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 89 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 90 | init_temperature: 100 # initial temperature. default: 100 91 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 92 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 93 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 94 | LBTRRT: 95 | type: geometric::LBTRRT 96 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 97 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 98 | epsilon: 0.4 # optimality approximation factor. default: 0.4 99 | BiEST: 100 | type: geometric::BiEST 101 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 102 | ProjEST: 103 | type: geometric::ProjEST 104 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 105 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 106 | LazyPRM: 107 | type: geometric::LazyPRM 108 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 109 | LazyPRMstar: 110 | type: geometric::LazyPRMstar 111 | SPARS: 112 | type: geometric::SPARS 113 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 114 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 115 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 116 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 117 | SPARStwo: 118 | type: geometric::SPARStwo 119 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 123 | manipulator: 124 | default_planner_config: None 125 | planner_configs: 126 | - SBL 127 | - EST 128 | - LBKPIECE 129 | - BKPIECE 130 | - KPIECE 131 | - RRT 132 | - RRTConnect 133 | - RRTstar 134 | - TRRT 135 | - PRM 136 | - PRMstar 137 | - FMT 138 | - BFMT 139 | - PDST 140 | - STRIDE 141 | - BiTRRT 142 | - LBTRRT 143 | - BiEST 144 | - ProjEST 145 | - LazyPRM 146 | - LazyPRMstar 147 | - SPARS 148 | - SPARStwo 149 | projection_evaluator: joints(joint_a1,joint_a2) 150 | longest_valid_segment_fraction: 0.005 -------------------------------------------------------------------------------- /kuka_push/src/kukapush_env_lib.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Random goal position 5 | std::random_device rd; 6 | std::mt19937 mt(rd()); 7 | std::uniform_int_distribution x_cord(600, 980); //range of random x_coordinate 8 | std::uniform_int_distribution y_cord(-340, 340); // range of random y_coordinate 9 | 10 | // Constructor 11 | kukapush_env::kukapush_env(ros::NodeHandle *nh) 12 | : marker(nh), 13 | group("manipulator"), 14 | robot_model_loader("robot_description"), 15 | current_state_ptr(group.getCurrentState()), 16 | goal_state_ptr(group.getCurrentState()) 17 | { 18 | //trajectory publisher for JointTrajectoryController 19 | pub_joint_publisher = nh->advertise("position_trajectory_controller/command",1); 20 | 21 | //Gazebo client for reseting the object's pose and generating new goal 22 | gazebo_client = nh->serviceClient("object_server"); 23 | 24 | //Moveit 25 | robot_model_ptr = robot_model_loader.getModel(); 26 | joint_model_group = robot_model_ptr->getJointModelGroup(group.getName()); 27 | 28 | desired_configuration.positions.resize(NUM_OF_JOINTS); 29 | arm_command.joint_names.resize(NUM_OF_JOINTS); 30 | 31 | //set fixed orientation of gripper 32 | action_pose.orientation.x = 0.706846; 33 | action_pose.orientation.y = 0.000587617; 34 | action_pose.orientation.z = 0.707367; 35 | action_pose.orientation.w = 0.000508517; 36 | } 37 | 38 | 39 | bool kukapush_env::env_step(kuka_push::rl_env::Request &req, 40 | kuka_push::rl_env::Response &resp) 41 | { 42 | // check if agent requires resetting the environment or interacting with it 43 | if(req.reset) 44 | { 45 | env_reset(resp); //reset the environment 46 | } 47 | else // interact with the environment 48 | { 49 | // transforming desired agent's actions into physical coordinates 50 | action_pose.position.x = (static_cast(req.action[0])*delta_x + delta_x) + half_range_x; 51 | action_pose.position.y = (static_cast(req.action[1])*delta_y + delta_y) + half_range_y; 52 | action_pose.position.z = (static_cast(req.action[1])*delta_z + delta_z) + half_range_z; 53 | if(action_pose.position.z < 0.43) 54 | { 55 | action_pose.position.z = 0.43; 56 | } 57 | ExecuteAction(action_pose); // execute client's action 58 | ros::Duration(0.03).sleep(); // wait 30ms for action having an effect 59 | GetObservation(); // get and compute state 60 | RewardFunction(); // compute reward 61 | SetObservation(resp); // prepare state for sending 62 | } 63 | return true; 64 | } 65 | 66 | // ------------------------------------------------ 67 | void kukapush_env::env_reset(kuka_push::rl_env::Response &resp) 68 | { 69 | //generate new goal 70 | marker.AddMarker(); 71 | 72 | action_pose.position.x = 0.62; 73 | action_pose.position.y = 0.0; 74 | action_pose.position.z = 0.43; 75 | //go to home pose 76 | ExecuteAction(action_pose); 77 | ros::Duration(0.8).sleep(); 78 | 79 | //generate new initial position of the object 80 | object_service.request.reset = true; 81 | gazebo_client.call(object_service); 82 | //ros::Duration(0.1).sleep(); //just to be sure, probably useless 83 | gripper_pose = group.getCurrentPose(); 84 | gripper_prev.x = gripper_pose.pose.position.x; 85 | gripper_prev.y = gripper_pose.pose.position.y; 86 | gripper_prev.z = gripper_pose.pose.position.z; 87 | 88 | GetObservation(); // get and compute state 89 | RewardFunction(); // compute reward 90 | SetObservation(resp); // prepare state for sending 91 | } 92 | 93 | // ------------------------------------------------ 94 | void kukapush_env::ExecuteAction(geometry_msgs::Pose &action_pose) 95 | { 96 | //Calculate joint values based on action_pose 97 | found_approach_ik = goal_state_ptr->setFromIK(joint_model_group, action_pose, 0.04); 98 | //If the solution has been found, continuou 99 | if(found_approach_ik) 100 | { 101 | goal_state_ptr->copyJointGroupPositions("manipulator", goal_joint_values); 102 | 103 | // Assign joint_values to joint_names 104 | std::stringstream joint_name; 105 | for(int i = 0; i< NUM_OF_JOINTS; ++i){ 106 | joint_name.str(""); 107 | joint_name << "joint_a" << (i + 1); 108 | desired_configuration.positions[i] = goal_joint_values[i]; 109 | arm_command.joint_names[i] = joint_name.str(); 110 | } 111 | 112 | // Setting message for controller 113 | arm_command.header.stamp = ros::Time::now(); 114 | arm_command.header.frame_id = "base_link"; 115 | arm_command.points.resize(1); 116 | arm_command.points[0] = desired_configuration; 117 | arm_command.points[0].time_from_start = ros::Duration(time_move); 118 | 119 | // Publish trajectory into controller 120 | pub_joint_publisher.publish(arm_command); 121 | } 122 | 123 | else 124 | { 125 | ROS_WARN("No IK found"); 126 | std::cout << "actions are: \n" << action_pose.position << std::endl; 127 | } 128 | } 129 | 130 | // ------------------------------------------------ 131 | void kukapush_env::RewardFunction() 132 | { 133 | // Checking if the object is within tolerance at the goal position 134 | if(((abs(object_pose.position.x - goal_position.x)) <= tolerance) && 135 | ((abs(object_pose.position.y - goal_position.y)) <= tolerance) && 136 | ((abs(object_pose.position.z - goal_position.z)) <= tolerance)) 137 | { 138 | reward = 0.0; // positive learning signal 139 | done = true; // flag that the goal is accomplished 140 | } 141 | else 142 | { 143 | reward = -1.0; //negative learning signal 144 | done = false; // flag that the goal is NOT accomplished 145 | } 146 | } 147 | 148 | // ------------------------------------------------ 149 | void kukapush_env::GetObservation() 150 | { 151 | //get current pose of the gripper 152 | gripper_pose = group.getCurrentPose(); 153 | 154 | //get goal position 155 | goal_position = marker.marker.pose.position; 156 | 157 | //object position relative to gripper 158 | object_gripper.x = gripper_pose.pose.position.x - object_pose.position.x; 159 | object_gripper.y = gripper_pose.pose.position.y - object_pose.position.y; 160 | object_gripper.z = gripper_pose.pose.position.z - object_pose.position.z; 161 | 162 | gripper_speed.x = ((abs(gripper_pose.pose.position.x - gripper_prev.x))/0.04)/10; 163 | gripper_speed.y = ((abs(gripper_pose.pose.position.y - gripper_prev.y))/0.04)/10; 164 | gripper_speed.z = ((abs(gripper_pose.pose.position.z - gripper_prev.z))/0.04)/10; 165 | 166 | gripper_prev.x = gripper_pose.pose.position.x; 167 | gripper_prev.y = gripper_pose.pose.position.y; 168 | gripper_prev.z = gripper_pose.pose.position.z; 169 | } 170 | 171 | // ------------------------------------------------ 172 | void kukapush_env::SetObservation(kuka_push::rl_env::Response &resp) 173 | { 174 | //assign gripper's pose 175 | resp.state[0] = gripper_pose.pose.position.x; 176 | resp.state[1] = gripper_pose.pose.position.y; 177 | resp.state[2] = gripper_pose.pose.position.z; 178 | resp.state[3] = gripper_pose.pose.orientation.x; 179 | resp.state[4] = gripper_pose.pose.orientation.y; 180 | resp.state[5] = gripper_pose.pose.orientation.z; 181 | resp.state[6] = gripper_pose.pose.orientation.w; 182 | 183 | //assign object's pose 184 | resp.state[7] = object_pose.position.x; 185 | resp.state[8] = object_pose.position.y; 186 | resp.state[9] = object_pose.position.z; 187 | resp.state[10] = object_pose.orientation.x; 188 | resp.state[11] = object_pose.orientation.y; 189 | resp.state[12] = object_pose.orientation.z; 190 | resp.state[13] = object_pose.orientation.w; 191 | 192 | //assign object's position relative to gripper's position 193 | resp.state[14] = object_gripper.x; 194 | resp.state[15] = object_gripper.y; 195 | resp.state[16] = object_gripper.z; 196 | 197 | // assign linear velocities of the object 198 | resp.state[17] = object_vel.linear.x; 199 | resp.state[18] = object_vel.linear.y; 200 | resp.state[19] = object_vel.linear.z; 201 | 202 | // assign angular velocities of the object 203 | resp.state[20] = object_vel.angular.x; 204 | resp.state[21] = object_vel.angular.y; 205 | resp.state[22] = object_vel.angular.z; 206 | 207 | // assign linear velocities of the gripper 208 | resp.state[23] = gripper_speed.x; 209 | resp.state[24] = gripper_speed.y; 210 | resp.state[25] = gripper_speed.z; 211 | 212 | //Asign desired_goal 213 | resp.desired_goal[0] = goal_position.x; 214 | resp.desired_goal[1] = goal_position.y; 215 | resp.desired_goal[2] = goal_position.z; 216 | 217 | //Assign achieved_goal 218 | resp.achieved_goal[0] = object_pose.position.x; 219 | resp.achieved_goal[1] = object_pose.position.y; 220 | resp.achieved_goal[2] = object_pose.position.z; 221 | 222 | resp.reward = reward; 223 | resp.done = done; 224 | } 225 | 226 | // ------------------------------------------------ 227 | void kukapush_env::objectCallback(const kuka_push::object_pose_vel::ConstPtr &msg) 228 | { 229 | // subscribe object's pose and velocities from Gazebo object's plugins 230 | object_pose.position = msg->pose.position; 231 | object_pose.orientation = msg->pose.orientation; 232 | 233 | object_vel.linear = msg->twist.linear; 234 | object_vel.angular = msg->twist.angular; 235 | } 236 | -------------------------------------------------------------------------------- /kuka_moveit_config/launch/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /TF1/Tree1 11 | Splitter Ratio: 0.5 12 | Tree Height: 520 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | - Class: rl_rviz_plugin/RlPlugin 33 | Name: RlPlugin 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Class: moveit_rviz_plugin/PlanningScene 60 | Enabled: true 61 | Move Group Namespace: "" 62 | Name: PlanningScene 63 | Planning Scene Topic: move_group/monitored_planning_scene 64 | Robot Description: robot_description 65 | Scene Geometry: 66 | Scene Alpha: 0.8999999761581421 67 | Scene Color: 50; 230; 50 68 | Scene Display Time: 0.20000000298023224 69 | Show Scene Geometry: true 70 | Voxel Coloring: Z-Axis 71 | Voxel Rendering: Occupied Voxels 72 | Scene Robot: 73 | Attached Body Color: 150; 50; 150 74 | Links: 75 | All Links Enabled: true 76 | Expand Joint Details: false 77 | Expand Link Details: false 78 | Expand Tree: false 79 | Link Tree Style: Links in Alphabetic Order 80 | base: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | base_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | fingers: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | Value: true 94 | link_1: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | Value: true 99 | link_2: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | link_3: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | link_4: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | Value: true 114 | link_5: 115 | Alpha: 1 116 | Show Axes: false 117 | Show Trail: false 118 | Value: true 119 | link_6: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | table: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Value: true 129 | tool0: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Value: true 134 | world: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Robot Alpha: 1 139 | Show Robot Collision: false 140 | Show Robot Visual: true 141 | Value: true 142 | - Class: rviz/Marker 143 | Enabled: true 144 | Marker Topic: visualization_marker 145 | Name: Marker 146 | Namespaces: 147 | basic_shapes: true 148 | Queue Size: 100 149 | Value: true 150 | - Class: rviz/TF 151 | Enabled: true 152 | Frame Timeout: 15 153 | Frames: 154 | All Enabled: false 155 | base: 156 | Value: false 157 | base_link: 158 | Value: false 159 | fingers: 160 | Value: false 161 | link_1: 162 | Value: false 163 | link_2: 164 | Value: false 165 | link_3: 166 | Value: false 167 | link_4: 168 | Value: false 169 | link_5: 170 | Value: false 171 | link_6: 172 | Value: false 173 | marker: 174 | Value: false 175 | object: 176 | Value: true 177 | table: 178 | Value: false 179 | tool0: 180 | Value: false 181 | world: 182 | Value: false 183 | Marker Scale: 1 184 | Name: TF 185 | Show Arrows: true 186 | Show Axes: true 187 | Show Names: false 188 | Tree: 189 | world: 190 | marker: 191 | {} 192 | object: 193 | {} 194 | table: 195 | base_link: 196 | base: 197 | {} 198 | link_1: 199 | link_2: 200 | link_3: 201 | link_4: 202 | link_5: 203 | link_6: 204 | tool0: 205 | fingers: 206 | {} 207 | Update Interval: 0 208 | Value: true 209 | Enabled: true 210 | Global Options: 211 | Background Color: 48; 48; 48 212 | Default Light: true 213 | Fixed Frame: world 214 | Frame Rate: 30 215 | Name: root 216 | Tools: 217 | - Class: rviz/Interact 218 | Hide Inactive Objects: true 219 | - Class: rviz/MoveCamera 220 | - Class: rviz/Select 221 | - Class: rviz/FocusCamera 222 | - Class: rviz/Measure 223 | - Class: rviz/SetInitialPose 224 | Theta std deviation: 0.2617993950843811 225 | Topic: /initialpose 226 | X std deviation: 0.5 227 | Y std deviation: 0.5 228 | - Class: rviz/SetGoal 229 | Topic: /move_base_simple/goal 230 | - Class: rviz/PublishPoint 231 | Single click: true 232 | Topic: /clicked_point 233 | Value: true 234 | Views: 235 | Current: 236 | Class: rviz/Orbit 237 | Distance: 5.417956829071045 238 | Enable Stereo Rendering: 239 | Stereo Eye Separation: 0.05999999865889549 240 | Stereo Focal Distance: 1 241 | Swap Stereo Eyes: false 242 | Value: false 243 | Focal Point: 244 | X: 0.168140709400177 245 | Y: 0.34994566440582275 246 | Z: 0.12612515687942505 247 | Focal Shape Fixed Size: true 248 | Focal Shape Size: 0.05000000074505806 249 | Invert Z Axis: false 250 | Name: Current View 251 | Near Clip Distance: 0.009999999776482582 252 | Pitch: 0.4353983402252197 253 | Target Frame: 254 | Value: Orbit (rviz) 255 | Yaw: 5.6835808753967285 256 | Saved: ~ 257 | Window Geometry: 258 | Displays: 259 | collapsed: false 260 | Height: 1025 261 | Hide Left Dock: false 262 | Hide Right Dock: true 263 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000293000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100052006c0050006c007500670069006e01000002d6000000ca000000ca00ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f0000003efc0100000002fb0000000800540069006d006501000000000000039f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002430000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 264 | RlPlugin: 265 | collapsed: false 266 | Selection: 267 | collapsed: false 268 | Time: 269 | collapsed: false 270 | Tool Properties: 271 | collapsed: false 272 | Views: 273 | collapsed: true 274 | Width: 927 275 | X: 993 276 | Y: 27 277 | -------------------------------------------------------------------------------- /kuka_gazebo/worlds/kuka.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://ground_plane 7 | 8 | 9 | 10 | model://sun 11 | 12 | 13 | 14 | 0.001 15 | 1 16 | 1000 17 | 0 0 -9.8 18 | 19 | 20 | 21 | 22 | 23 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 24 | orbit 25 | 26 | 27 | 28 | 29 | 30 | 31 | 1 32 | 33 | 0.166667 34 | 0 35 | 0 36 | 0.166667 37 | 0 38 | 0.166667 39 | 40 | 0 0 0 0 -0 0 41 | 42 | 0 43 | 0 44 | 0 45 | 0 0 0 0 -0 0 46 | 1 47 | 48 | 49 | 50 | 0.04 0.04 0.04 51 | 52 | 53 | 54 | 58 | 59 | __default__ 60 | 61 | 0.3 0.3 0.7 1 62 | 0.7 0.7 0.7 1 63 | 0.01 0.01 0.7 1 64 | 0 0 0 1 65 | 66 | 0 0 0 0 -0 0 67 | 0 68 | 1 69 | 70 | 71 | 0 72 | 10 73 | 0 0 0 0 -0 0 74 | 75 | 76 | 0.04 0.04 0.04 77 | 78 | 79 | 80 | 81 | 82 | 1 83 | 1 84 | 0 0 0 85 | 0 86 | 0 87 | 88 | 89 | 1 90 | 0 91 | 0 92 | 1 93 | 94 | 0 95 | 96 | 97 | 98 | 99 | 0 100 | 1e+06 101 | 102 | 103 | 0 104 | 1 105 | 1 106 | 107 | 0 108 | 0.2 109 | 1e+13 110 | 1 111 | 0.01 112 | 0 113 | 114 | 115 | 1 116 | -0.01 117 | 0 118 | 0.2 119 | 1e+13 120 | 1 121 | 122 | 123 | 124 | 125 | 126 | 0 127 | 1 128 | 129 | 1.08452 0.735173 0 0 -0 0 130 | 131 | 132 | 133 | 134 | 1 135 | 136 | 0.1 137 | 0 138 | 0 139 | 0.1 140 | 0 141 | 0.1 142 | 143 | 0 0 0 0 -0 0 144 | 145 | 0 146 | 0 147 | 0 148 | 0 0 0 0 -0 0 149 | 0 150 | 151 | 152 | 153 | 0.025 154 | 155 | 156 | 157 | 161 | 162 | __default__ 163 | 164 | 0 1 0 1 165 | 0 1 0 1 166 | 0 1 0 1 167 | 0 1 0 1 168 | 169 | 0 0 0 0 -0 0 170 | 0 171 | 1 172 | 173 | 174 | 0 175 | 1 176 | 177 | 1.26268 0.764758 0 0 -0 0 178 | 179 | 180 | 13 236000000 181 | 13 307146538 182 | 1596973931 88567997 183 | 13236 184 | 185 | 1.26268 0.764758 0 0 -0 0 186 | 1 1 1 187 | 188 | 1.26268 0.764758 0 0 -0 0 189 | 0 0 0 0 -0 0 190 | 0 0 0 0 -0 0 191 | 0 0 0 0 -0 0 192 | 193 | 194 | 195 | 0 0 0 0 -0 0 196 | 1 1 1 197 | 198 | 0 0 0 0 -0 0 199 | 0 0 0 0 -0 0 200 | 0 0 0 0 -0 0 201 | 0 0 0 0 -0 0 202 | 203 | 204 | 205 | 0 -0 -0 -0 -0 -0 206 | 1 1 1 207 | 208 | 0.2 0 0.8 0 -1e-06 -1e-06 209 | -0 1e-06 -0 -1e-06 -0 0 210 | -0.000328 0.000701 -0.000294 -0.001388 -0.000643 0.000148 211 | -0.004097 0.008757 -0.003675 0 -0 0 212 | 213 | 214 | 0.225 -0 0.8 0 -1.57 0 215 | -0 1e-06 -0 -1e-06 0.000817 -0 216 | -0.000328 0.000705 -0.00028 -0.000203 -0.000324 0.000198 217 | -0.001639 0.003525 -0.001399 0 -0 0 218 | 219 | 220 | 0.225362 -0 1.255 0 -2e-06 -1e-06 221 | 0.000372 1e-06 -0 -1e-06 -0.000329 0 222 | -0.000398 0.001043 -0.000282 -0.000797 -0.000146 8.6e-05 223 | -0.001988 0.005216 -0.001408 0 -0 0 224 | 225 | 226 | 0.225362 -0 1.29 -1.3e-05 2e-05 -1e-06 227 | 0.00036 1e-06 -0 0 -0.000329 -1e-06 228 | -0.000378 0.001036 -0.000282 -0.000416 -0.000131 -0.000792 229 | -0.001892 0.005179 -0.001412 0 -0 0 230 | 231 | 232 | 0.645362 -0 1.28999 -0.016498 1.57002 -0.016499 233 | 0.00036 0 0.000138 -0 0.001394 -2e-06 234 | -0.000367 0.000574 -0.000215 -0.000166 -0.000142 -0.001179 235 | -0.000733 0.001148 -0.000431 0 -0 0 236 | 237 | 238 | 0.645424 -1e-06 1.20999 -0.016559 1.57003 -0.016558 239 | 0.000249 0 0.000138 -2e-06 0.001393 -3e-06 240 | -0.000321 0.000429 -0.000213 -0.001425 -0.000425 -0.001296 241 | -0.000706 0.000944 -0.000469 0 -0 0 242 | 243 | 244 | 0.6 0 0.2 -0 -0 -0 245 | -0 0 -0 -1e-06 -0 -0 246 | -9.1e-05 9.2e-05 -0.000131 -0.000624 -0.000401 -5.5e-05 247 | -0.011832 0.01194 -0.017094 0 -0 0 248 | 249 | 250 | 251 | 1.08452 0.735173 0.020001 -1.6e-05 -5.3e-05 -5e-06 252 | 1 1 1 253 | 254 | 1.08452 0.735173 0.020001 -1.6e-05 -5.3e-05 -5e-06 255 | 0 0 0 0 -0 0 256 | 0.028505 -0.028524 2.10749 1.42545 1.42544 2e-06 257 | 0.028505 -0.028524 2.10749 0 -0 0 258 | 259 | 260 | 261 | 262 | 263 | -------------------------------------------------------------------------------- /kuka_push/scripts/kuka_agent.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | from kuka_push.srv import * 5 | from kuka_push.msg import rlplugin_msg 6 | 7 | import numpy as np 8 | from ReplayBuffer import ReplayBuffer 9 | import random 10 | import time 11 | import tensorflow as tf 12 | import core 13 | from core import get_vars 14 | 15 | 16 | class SAC(): 17 | def __init__(self, state_dim, act_dim, hidden_sizes, gamma=0.99, polyak=0.995, 18 | lr=1e-3, alpha=0.2): 19 | 20 | """ 21 | Placeholders initialization 22 | """ 23 | self.state_t_ph = tf.placeholder(shape=(None, state_dim), dtype=tf.float32, name="State_t_ph") 24 | self.state_t1_ph = tf.placeholder(shape=(None, state_dim), dtype=tf.float32, name="State_t1_ph") 25 | self.act_ph = tf.placeholder(shape=(None, act_dim), dtype=tf.float32, name="Actions_ph") 26 | self.ret_ph = tf.placeholder(shape=(None,), dtype=tf.float32, name="Rewards_ph") 27 | self.done_ph = tf.placeholder(shape=(None,), dtype=tf.float32, name="Done_ph") 28 | 29 | self.gamma = gamma 30 | self.alpha = alpha 31 | self.lr = lr 32 | self.polyak = polyak 33 | 34 | with tf.variable_scope("main"): 35 | self.mean, self.pi, self.logp_pi, self.q1, \ 36 | self.q2, self.q1_pi, self.q2_pi, self.v = core.mlp_actor_critic(self.state_t_ph, self.act_ph, 37 | hidden_sizes) 38 | with tf.variable_scope("target"): 39 | _, _, _, _, _, _, _, self.v_targ = core.mlp_actor_critic(self.state_t1_ph, self.act_ph, hidden_sizes) 40 | 41 | # Min Double-Q: 42 | self.min_q_pi = tf.minimum(self.q1_pi, self.q2_pi) 43 | 44 | # Targets for Q and V regression 45 | self.q_backup = tf.stop_gradient(self.ret_ph + self.gamma * (1 - self.done_ph) * self.v_targ) 46 | self.v_backup = tf.stop_gradient(self.min_q_pi - self.alpha * self.logp_pi) 47 | 48 | # Soft actor-critic losses 49 | self.pi_loss = tf.reduce_mean(self.alpha * self.logp_pi - self.min_q_pi, name="pi_loss") 50 | self.q1_loss = 0.5 * tf.reduce_mean((self.q_backup - self.q1) ** 2, name="q1_loss") 51 | self.q2_loss = 0.5 * tf.reduce_mean((self.q_backup - self.q2) ** 2, name="q2_loss") 52 | self.v_loss = 0.5 * tf.reduce_mean((self.v_backup - self.v) ** 2, name="v_loss") 53 | self.value_loss = self.q1_loss + self.q2_loss + self.v_loss 54 | 55 | # Policy train op 56 | # (has to be separate from value train op, because q1_pi appears in pi_loss) 57 | self.pi_optimizer = tf.train.AdamOptimizer(learning_rate=self.lr) 58 | self.train_pi_op = self.pi_optimizer.minimize(self.pi_loss, var_list=get_vars('main/pi'), name="train_pi_op") 59 | 60 | # Value train op 61 | # (control dep of train_pi_op because sess.run otherwise evaluates in nondeterministic order) 62 | self.value_optimizer = tf.train.AdamOptimizer(learning_rate=self.lr) 63 | self.value_params = get_vars('main/q') + get_vars('main/v') 64 | with tf.control_dependencies([self.train_pi_op]): 65 | self.train_value_op = self.value_optimizer.minimize(self.value_loss, var_list=self.value_params, name="train_value_op") 66 | 67 | # Polyak averaging for target variables 68 | # (control flow because sess.run otherwise evaluates in nondeterministic order) 69 | with tf.control_dependencies([self.train_value_op]): 70 | self.target_update = tf.group([tf.assign(self.v_targ, self.polyak * self.v_targ + (1 - self.polyak) * self.v_main) 71 | for self.v_main, self.v_targ in zip(get_vars('main'), get_vars('target'))], name="target_update") 72 | 73 | """ alpha tuning""" 74 | self.target_entropy = -4 75 | self.log_alpha = tf.compat.v1.get_variable('log_alpha', dtype=tf.float32, initializer=0.0) 76 | self.alpha = tf.exp(self.log_alpha, name="alpha") 77 | self.alpha_loss = -tf.reduce_mean(self.log_alpha * tf.stop_gradient(self.logp_pi + self.target_entropy), name="alpha_loss") 78 | 79 | self.alpha_optimizer = tf.compat.v1.train.AdamOptimizer(self.lr, name='alpha_optimizer') 80 | self.alpha_train_op = self.alpha_optimizer.minimize(loss=self.alpha_loss, var_list=[self.log_alpha], name="alpha_train_op") 81 | """-------------""" 82 | 83 | # All ops to call during one training step 84 | self.step_ops = [self.pi_loss, self.q1_loss, self.q2_loss, self.v_loss, self.q1, self.q2, self.v, 85 | self.logp_pi, self.train_pi_op, self.train_value_op, self.target_update, 86 | self.alpha_loss, self.alpha_train_op] 87 | 88 | # Initializing targets to match main variables 89 | self.target_init = tf.group([tf.assign(self.v_targ, self.v_main) 90 | for self.v_main, self.v_targ in zip(get_vars('main'), get_vars('target'))]) 91 | 92 | """For saving purposes""" 93 | tf.identity(self.pi, name="pi") 94 | tf.identity(self.mean, name="mean") 95 | tf.identity(self.q1, name="q1") 96 | tf.identity(self.q2, name="q2") 97 | tf.identity(self.v, name="v") 98 | tf.identity(self.logp_pi, name="logp_pi") 99 | 100 | """ Tensorboard summaries """ 101 | self.summary_reward_ph = tf.placeholder(tf.float32, name="reward_scalar_ph") 102 | self.summary_reward = tf.summary.scalar("Learning performance", self.summary_reward_ph) 103 | 104 | """ For saving trained model """ 105 | self.saver = tf.train.Saver() 106 | 107 | self.sess = tf.Session() 108 | self.sess.run(tf.global_variables_initializer()) 109 | self.sess.run(self.target_init) 110 | 111 | self.file_writer = tf.summary.FileWriter('/home/tomas/catkin_ws/src/kuka_push/scripts/logs', self.sess.graph) 112 | 113 | def SAC_getAction(self, state, deterministic=False): 114 | act_op = self.mean if deterministic else self.pi 115 | return self.sess.run(act_op, feed_dict={self.state_t_ph: state.reshape(1, -1)})[0] 116 | 117 | def SAC_update(self, obs1, obs2, acts, rews, done): 118 | feed_dict = {self.state_t_ph: obs1, 119 | self.state_t1_ph: obs2, 120 | self.act_ph: acts, 121 | self.ret_ph: rews, 122 | self.done_ph: done, 123 | } 124 | outs = self.sess.run(self.step_ops, feed_dict) 125 | 126 | 127 | 128 | 129 | 130 | if __name__ == "__main__": 131 | # Initialization 132 | rospy.init_node("kuka_agent") 133 | rospy.wait_for_service("kukapush_server") 134 | #service client for interacting with the environment 135 | step = rospy.ServiceProxy("kukapush_server", rl_env) 136 | #publisher for RViz plugin 137 | pub = rospy.Publisher('/rl_states', rlplugin_msg, queue_size=1) 138 | msg = rlplugin_msg() 139 | 140 | 141 | # Setting seed 142 | tf.set_random_seed(0) 143 | np.random.seed(0) 144 | 145 | # Properties of the environment 146 | state_dim = 29 147 | act_dim = 3 148 | timesteps_per_episode = 100 149 | start_steps = 10000 150 | 151 | # Hyperparameters 152 | buffer_maxsize = 1000000 153 | batchSize = 256 154 | hidden_sizes = [256, 256] 155 | epochs = 30 156 | iteration_per_epoch = 500 157 | 158 | timestep = 0 159 | ep_len = 0 160 | 161 | # Recording results 162 | mean_ret = [] 163 | prev_ret = 0.0 164 | 165 | # lists for Hindsight Experience Replay 166 | list_achievedGoals = [] 167 | list_states = [] 168 | list_new_states = [] 169 | list_actions = [] 170 | 171 | agent = SAC(state_dim, act_dim, hidden_sizes) 172 | replay_buffer = ReplayBuffer(state_dim, act_dim, buffer_maxsize) 173 | 174 | # request for resetting the environment 175 | def reset_env(): 176 | reset = True 177 | action = [0, 0, 0] 178 | resp = step(action, reset) 179 | obs = np.round(resp.state, decimals=6) 180 | state = np.concatenate((resp.state, resp.desired_goal), axis=None) 181 | state = np.round(state, decimals=6) 182 | return state, obs 183 | 184 | # generating random actions 185 | def randomAction(): 186 | action_x = random.uniform(-1.01, 1.01) 187 | action_y = random.uniform(-1.01, 1.01) 188 | action_z = random.uniform(-1.01, 1.01) 189 | action = [action_x, action_y, action_z] 190 | return action 191 | 192 | # Hindsight Experience Replay algorithm 193 | def her(): 194 | global ep_len 195 | global list_states, list_actions, list_new_states, list_achievedGoals 196 | #check if episode lasted less than 4 timesteps 197 | if ep_len < 4: 198 | # create additional goal only from the last timestep 199 | her_state = np.concatenate((np.array(list_states[0]), np.array(list_achievedGoals[0]))) 200 | her_next_state = np.concatenate((np.array(list_new_states[0]), np.array(list_achievedGoals[0]))) 201 | her_action = list_actions[0] 202 | her_reward = 0 203 | her_done = True 204 | replay_buffer.store(her_state, her_action, her_reward, her_next_state, her_done) 205 | else: 206 | # divide an episode into 4 groups/parts, which gives us 4 additional goals 207 | indexes = np.sort(np.random.randint(0, ep_len, size=3)) 208 | """ first group """ 209 | for i in range(indexes[0] + 1): 210 | her_state = np.concatenate((np.array(list_states[i]), np.array(list_achievedGoals[indexes[0]]))) 211 | her_next_state = np.concatenate( 212 | (np.array(list_new_states[i]), np.array(list_achievedGoals[indexes[0]]))) 213 | her_action = list_actions[i] 214 | if i == indexes[0]: 215 | her_done = True 216 | her_reward = 0.0 217 | else: 218 | her_done = False 219 | her_reward = -1.0 220 | replay_buffer.store(her_state, her_action, her_reward, her_next_state, her_done) 221 | """ second group """ 222 | for i in range(indexes[0] + 1, indexes[1] + 1): 223 | her_state = np.concatenate((np.array(list_states[i]), np.array(list_achievedGoals[indexes[1]]))) 224 | her_next_state = np.concatenate( 225 | (np.array(list_new_states[i]), np.array(list_achievedGoals[indexes[1]]))) 226 | her_action = list_actions[i] 227 | if i == indexes[1]: 228 | her_done = True 229 | her_reward = 0.0 230 | else: 231 | her_done = False 232 | her_reward = -1.0 233 | replay_buffer.store(her_state, her_action, her_reward, her_next_state, her_done) 234 | """ third group """ 235 | for i in range(indexes[1] + 1, indexes[2] + 1): 236 | her_state = np.concatenate((np.array(list_states[i]), np.array(list_achievedGoals[indexes[2]]))) 237 | her_next_state = np.concatenate( 238 | (np.array(list_new_states[i]), np.array(list_achievedGoals[indexes[2]]))) 239 | her_action = list_actions[i] 240 | if i == indexes[2]: 241 | her_done = True 242 | her_reward = 0.0 243 | else: 244 | her_done = False 245 | her_reward = -1.0 246 | replay_buffer.store(her_state, her_action, her_reward, her_next_state, her_done) 247 | """ fourth group """ 248 | for i in range(indexes[2] + 1, ep_len): 249 | her_state = np.concatenate((np.array(list_states[i]), np.array(list_achievedGoals[ep_len - 1]))) 250 | her_next_state = np.concatenate( 251 | (np.array(list_new_states[i]), np.array(list_achievedGoals[ep_len - 1]))) 252 | her_action = list_actions[i] 253 | if i == ep_len - 1: 254 | her_done = True 255 | her_reward = 0.0 256 | else: 257 | her_done = False 258 | her_reward = -1.0 259 | replay_buffer.store(her_state, her_action, her_reward, her_next_state, her_done) 260 | 261 | # Run one episode 262 | def episode(): 263 | state, obs = reset_env() 264 | 265 | global timestep, timesteps_per_episode, mean_ret 266 | global list_states, list_actions, list_new_states, list_achievedGoals 267 | global ep_len 268 | 269 | reward, ep_len, done, reset = 0, 0, False, False 270 | 271 | for _ in range(timesteps_per_episode): 272 | # record state for HER 273 | list_states.append(obs) 274 | 275 | #first X timesteps do only random actions -> it supports exploration 276 | if timestep > start_steps: 277 | action = agent.SAC_getAction(state) 278 | else: 279 | action = randomAction() 280 | 281 | # record action for HER 282 | list_actions.append(action) 283 | 284 | # do an interaction with the environment 285 | resp = step(action, reset) 286 | 287 | # new state, reward and done flag for the current timestep 288 | new_state = np.round(np.concatenate((resp.state, resp.desired_goal), axis=None), decimals=6) 289 | reward = resp.reward 290 | done = resp.done 291 | 292 | # record new_state and achieved_goal for HER 293 | achieved_goal = np.round(resp.achieved_goal, decimals=6) 294 | obs = np.round(resp.state, decimals=6) 295 | list_new_states.append(obs) 296 | list_achievedGoals.append(achieved_goal) 297 | 298 | # increment time 299 | ep_len += 1 300 | timestep += 1 301 | 302 | # save trajectory into the buffer 303 | replay_buffer.store(state, action, reward, new_state, done) 304 | 305 | state = new_state 306 | 307 | # at the end of the episode use HER 308 | if ep_len == (timesteps_per_episode - 1): 309 | her() 310 | list_states, list_actions, list_achievedGoals, list_new_states = [], [], [], [] 311 | break 312 | 313 | 314 | def policy_test(): 315 | global mean_ret, ep_len 316 | state, obs = reset_env() 317 | reward, ep_len, done, reset = 0, 0, False, False 318 | 319 | while not done: 320 | action = agent.SAC_getAction(state, True) 321 | 322 | # Send request to server 323 | resp = step(action, reset) 324 | 325 | #Handle response from server 326 | state = np.round(np.concatenate((resp.state, resp.desired_goal), axis=None), decimals=6) 327 | reward = resp.reward 328 | done = resp.done 329 | 330 | ep_len += 1 331 | 332 | if reward == 0.0: 333 | done = True 334 | if ep_len == 50: 335 | done = True 336 | 337 | if reward == 0.0: 338 | mean_ret.append(1) 339 | elif reward == -1.0: 340 | mean_ret.append(0) 341 | 342 | # main loop 343 | for epoch in range(epochs): 344 | start_time = time.time() 345 | for i in range(iteration_per_epoch): 346 | # data for RViz plugin 347 | msg.training = True 348 | msg.epoch = epoch 349 | msg.episode = i 350 | msg.max_score = prev_ret 351 | pub.publish(msg) 352 | # run one episode 353 | episode() 354 | # Update policy 355 | for j in range(2*ep_len): 356 | batch = replay_buffer.sample_batch(batchSize) 357 | agent.SAC_update(batch['obs1'], batch['obs2'], batch['acts'], batch['rews'], batch['done']) 358 | # test policy after each epoch 359 | for j in range(10): 360 | policy_test() 361 | print('epoch: %3d \t ep_ret_avg: %.3f \t elapsed_time: %3d' % (epoch, np.mean(mean_ret), (time.time() - start_time))) 362 | # Tensorboard charts 363 | summary = agent.sess.run(agent.summary_reward, feed_dict={agent.summary_reward_ph: np.mean(mean_ret)}) 364 | agent.file_writer.add_summary(summary, (epoch + 1)) 365 | 366 | msg.prev_score = np.mean(mean_ret) 367 | 368 | # save the best policy 369 | if prev_ret < np.mean(mean_ret): 370 | agent.saver.save(agent.sess, "/home/tomas/catkin_ws/src/kuka_push/saved_model/sac_KUKAPush_trained_model") 371 | print("model is saved") 372 | prev_ret = np.mean(mean_ret) 373 | mean_ret.clear() 374 | 375 | msg.max_score = prev_ret 376 | msg.training = False 377 | pub.publish(msg) 378 | --------------------------------------------------------------------------------