From 509f244dc291499dece705de941f80b35320a2bf Mon Sep 17 00:00:00 2001 From: Michalis Logothetis Date: Sat, 12 May 2018 19:19:37 +0300 Subject: [PATCH] gazebo and ros control interface --- README.md | 20 +- README.md~ | 7 - pa10_description/README.md~ | 5 - pa10_description/launch/test.launch~ | 15 - pa10_description/launch/upload_pa10.launch~ | 8 - pa10_description/urdf/arm.transmission.xacro | 6 +- pa10_description/urdf/arm.transmission.xacro~ | 21 - pa10_description/urdf/materials.xacro~ | 40 -- pa10_description/urdf/pa10.gazebo~ | 60 -- pa10_description/urdf/pa10_csl.urdf.xacro | 94 +-- pa10_description/urdf/pa10_csl.urdf.xacro~ | 503 ---------------- pa10_description/urdf/pa10_csl.urdf~ | 551 ------------------ pa10_description/urdf/pa10csl.gazebo | 11 +- pa10_description/urdf/pa10csl.gazebo~ | 84 --- pa10_gazebo/launch/pa10.launch | 75 +-- pa10_gazebo/launch/pa10.launch~ | 38 -- pa10_gazebo/launch/pa10_ccsl_gazebo.launch | 29 - pa10_gazebo/launch/pa10_gazebo.launch | 29 - pa10_gazebo/launch/pa10_gazebo.launch~ | 34 -- pa10_gazebo/launch/pa10_moveit.launch | 27 + pa10_gazebo/launch/pa10_spawn.launch | 6 +- pa10_gazebo/launch/pa10_spawn.launch~ | 24 - .../launch/simulation_pa10_bringup.launch | 19 - .../launch/simulation_pa10_bringup.launch~ | 20 - pa10_hardware_interface/CMakeLists.txt~ | 218 ------- ...s.yaml~ => gazebo_effort_controllers.yaml} | 26 +- ...yaml~ => gazebo_position_controllers.yaml} | 22 +- .../config/joint_names.yaml~ | 1 - .../config/mycontroller.yaml~ | 37 -- pa10_hardware_interface/config/pids.yaml~ | 14 - pa10_hardware_interface/package.xml~ | 62 -- .../scripts/add_collision_object.py | 43 ++ .../scripts/add_collision_object.pyc | Bin 0 -> 2061 bytes .../scripts/joint_position_interface.cpp~ | 131 ----- .../scripts/p10_hardware_interface.cpp~ | 388 ------------ .../scripts/pa10_hardware_interface.cpp~ | 547 ----------------- .../scripts/pa10_ms1_vrep_scene.py | 46 ++ pa10_hardware_interface/src/home_arm.py | 67 +++ pa10_hardware_interface/src/home_gripper.py | 64 -- pa10_hardware_interface/src/home_gripper.py~ | 64 -- .../src/joint_position_interface.cpp~ | 205 ------- .../src/joint_publisher.py~ | 34 -- .../src/motion_planning_scene_tutorial.cpp~ | 169 ------ .../src/pa10_fake_hardware_interface.cpp~ | 345 ----------- .../src/pa10_vrep_ms1_planning_scene.cpp | 21 + .../src/simple_trajectoryry.cpp~ | 129 ---- pa10_hardware_interface/src/tuninig_script.py | 73 +++ pa10_hardware_interface/src/tutorial.py~ | 35 -- .../config/sensors_kinect2.yaml | 2 +- .../config/sensors_kinect2.yaml~ | 2 +- .../launch/demo_velocity.launch | 10 +- .../launch/demo_velocity.launch~ | 2 +- pa10_moveit_config/launch/gazebo_demo.launch | 37 ++ pa10_moveit_config/launch/move_group.launch | 1 - pa10_moveit_config/launch/move_group.launch~ | 19 +- 55 files changed, 494 insertions(+), 4046 deletions(-) delete mode 100644 README.md~ delete mode 100644 pa10_description/README.md~ delete mode 100644 pa10_description/launch/test.launch~ delete mode 100644 pa10_description/launch/upload_pa10.launch~ delete mode 100644 pa10_description/urdf/arm.transmission.xacro~ delete mode 100644 pa10_description/urdf/materials.xacro~ delete mode 100644 pa10_description/urdf/pa10.gazebo~ delete mode 100644 pa10_description/urdf/pa10_csl.urdf.xacro~ delete mode 100644 pa10_description/urdf/pa10_csl.urdf~ delete mode 100644 pa10_description/urdf/pa10csl.gazebo~ delete mode 100644 pa10_gazebo/launch/pa10.launch~ delete mode 100644 pa10_gazebo/launch/pa10_ccsl_gazebo.launch delete mode 100644 pa10_gazebo/launch/pa10_gazebo.launch delete mode 100644 pa10_gazebo/launch/pa10_gazebo.launch~ create mode 100644 pa10_gazebo/launch/pa10_moveit.launch delete mode 100644 pa10_gazebo/launch/pa10_spawn.launch~ delete mode 100644 pa10_gazebo/launch/simulation_pa10_bringup.launch delete mode 100644 pa10_gazebo/launch/simulation_pa10_bringup.launch~ delete mode 100644 pa10_hardware_interface/CMakeLists.txt~ rename pa10_hardware_interface/config/{fake_controllers.yaml~ => gazebo_effort_controllers.yaml} (56%) rename pa10_hardware_interface/config/{gazebo_controllers.yaml~ => gazebo_position_controllers.yaml} (62%) delete mode 100644 pa10_hardware_interface/config/joint_names.yaml~ delete mode 100644 pa10_hardware_interface/config/mycontroller.yaml~ delete mode 100644 pa10_hardware_interface/config/pids.yaml~ delete mode 100644 pa10_hardware_interface/package.xml~ create mode 100644 pa10_hardware_interface/scripts/add_collision_object.py create mode 100644 pa10_hardware_interface/scripts/add_collision_object.pyc delete mode 100644 pa10_hardware_interface/scripts/joint_position_interface.cpp~ delete mode 100644 pa10_hardware_interface/scripts/p10_hardware_interface.cpp~ delete mode 100644 pa10_hardware_interface/scripts/pa10_hardware_interface.cpp~ create mode 100755 pa10_hardware_interface/scripts/pa10_ms1_vrep_scene.py create mode 100755 pa10_hardware_interface/src/home_arm.py delete mode 100755 pa10_hardware_interface/src/home_gripper.py delete mode 100755 pa10_hardware_interface/src/home_gripper.py~ delete mode 100644 pa10_hardware_interface/src/joint_position_interface.cpp~ delete mode 100755 pa10_hardware_interface/src/joint_publisher.py~ delete mode 100644 pa10_hardware_interface/src/motion_planning_scene_tutorial.cpp~ delete mode 100644 pa10_hardware_interface/src/pa10_fake_hardware_interface.cpp~ create mode 100644 pa10_hardware_interface/src/pa10_vrep_ms1_planning_scene.cpp delete mode 100644 pa10_hardware_interface/src/simple_trajectoryry.cpp~ create mode 100755 pa10_hardware_interface/src/tuninig_script.py delete mode 100644 pa10_hardware_interface/src/tutorial.py~ create mode 100644 pa10_moveit_config/launch/gazebo_demo.launch diff --git a/README.md b/README.md index d26c6db..04c1ca4 100644 --- a/README.md +++ b/README.md @@ -60,6 +60,22 @@ roslaunch pa10_moveit_config demo_velocity.launch 3) Move the robot end effector using MoveIt framework. ## Gazebo -:bangbang: +You can launch Gazebo using the launch file- -** Not ready Yet ** +```sh +roslaunch pa10_gazebo pa10.launch +``` + +The launch file : +1. Launches Gazebo +2. Loads the robot model in Gazebo +3. Launched ros_control controller for the robot + +The controller is set to launch a joint trajectory controller. + +## Gazebo + MoveIt +To run the Gazebo + Pa10 Controllers + MoveIt : +```sh +roslaunch pa10_gazebo pa10_moveit.launch rviz:=true +``` +Set the argument ```rviz := true``` to launch RViz. diff --git a/README.md~ b/README.md~ deleted file mode 100644 index e68a883..0000000 --- a/README.md~ +++ /dev/null @@ -1,7 +0,0 @@ -## Synopsis - -These packages are an integration of MoveIt framework, ros_control and Gazebo for -Mitsubishi PA10 7-DOF Arm. - - - diff --git a/pa10_description/README.md~ b/pa10_description/README.md~ deleted file mode 100644 index cbcf987..0000000 --- a/pa10_description/README.md~ +++ /dev/null @@ -1,5 +0,0 @@ -#Synopsis -This package provides the urdf and mesh files of the PA10. - -An underactuated gripper is mounted on the final robot link - diff --git a/pa10_description/launch/test.launch~ b/pa10_description/launch/test.launch~ deleted file mode 100644 index fafe3ed..0000000 --- a/pa10_description/launch/test.launch~ +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/pa10_description/launch/upload_pa10.launch~ b/pa10_description/launch/upload_pa10.launch~ deleted file mode 100644 index fde94e2..0000000 --- a/pa10_description/launch/upload_pa10.launch~ +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/pa10_description/urdf/arm.transmission.xacro b/pa10_description/urdf/arm.transmission.xacro index 94db30c..728a057 100644 --- a/pa10_description/urdf/arm.transmission.xacro +++ b/pa10_description/urdf/arm.transmission.xacro @@ -8,12 +8,14 @@ transmission_interface/SimpleTransmission + + PositionJointInterface ${reduction} - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface + + PositionJointInterface diff --git a/pa10_description/urdf/arm.transmission.xacro~ b/pa10_description/urdf/arm.transmission.xacro~ deleted file mode 100644 index 78bc7a7..0000000 --- a/pa10_description/urdf/arm.transmission.xacro~ +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - transmission_interface/SimpleTransmission - - ${reduction} - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - - - diff --git a/pa10_description/urdf/materials.xacro~ b/pa10_description/urdf/materials.xacro~ deleted file mode 100644 index 0d0ccb9..0000000 --- a/pa10_description/urdf/materials.xacro~ +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_description/urdf/pa10.gazebo~ b/pa10_description/urdf/pa10.gazebo~ deleted file mode 100644 index d0fc482..0000000 --- a/pa10_description/urdf/pa10.gazebo~ +++ /dev/null @@ -1,60 +0,0 @@ - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - - - Gazebo/Green - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - - 0.5 - 0.5 - Gazebo/Green - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - - 0.5 - 0.5 - Gazebo/Green - - - - - 0.5 - 0.5 - Gazebo/Grey - - - diff --git a/pa10_description/urdf/pa10_csl.urdf.xacro b/pa10_description/urdf/pa10_csl.urdf.xacro index 5c1452c..f78a9cd 100644 --- a/pa10_description/urdf/pa10_csl.urdf.xacro +++ b/pa10_description/urdf/pa10_csl.urdf.xacro @@ -6,18 +6,18 @@ - - - - - - - + + + + + + + - + @@ -34,6 +34,24 @@ + + + + + + + + + + + + + + + + + + @@ -66,7 +84,7 @@ - + @@ -92,7 +110,7 @@ - + @@ -115,7 +133,7 @@ - + @@ -140,7 +158,7 @@ - + @@ -149,7 +167,7 @@ - + @@ -174,7 +192,7 @@ - + @@ -198,7 +216,7 @@ - + @@ -222,7 +240,7 @@ - + @@ -245,7 +263,7 @@ - + @@ -281,7 +299,7 @@ - + @@ -340,15 +358,18 @@ - - - + + + + + - + + @@ -369,10 +390,11 @@ - + @@ -385,6 +407,7 @@ k_velocity="20" soft_lower_limit="${-joint_2_max_position + arm_eps}" soft_upper_limit="${ joint_2_max_position - arm_eps}" /> + @@ -404,6 +427,7 @@ k_velocity="20" soft_lower_limit="${-joint_3_max_position + arm_eps}" soft_upper_limit="${ joint_3_max_position - arm_eps}" /> + @@ -416,6 +440,7 @@ k_velocity="20" soft_lower_limit="${-joint_4_max_position + arm_eps}" soft_upper_limit="${ joint_4_max_position - arm_eps}" /> + @@ -428,6 +453,7 @@ k_velocity="20" soft_lower_limit="${-joint_5_max_position + arm_eps}" soft_upper_limit="${ joint_5_max_position - arm_eps}" /> + @@ -440,6 +466,7 @@ k_velocity="20" soft_lower_limit="${-joint_6_max_position + arm_eps}" soft_upper_limit="${ joint_6_max_position - arm_eps}" /> + @@ -452,6 +479,7 @@ k_velocity="20" soft_lower_limit="${-joint_7_max_position + arm_eps}" soft_upper_limit="${ joint_7_max_position - arm_eps}" /> + @@ -499,15 +527,15 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/pa10_description/urdf/pa10_csl.urdf.xacro~ b/pa10_description/urdf/pa10_csl.urdf.xacro~ deleted file mode 100644 index 02c2235..0000000 --- a/pa10_description/urdf/pa10_csl.urdf.xacro~ +++ /dev/null @@ -1,503 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_description/urdf/pa10_csl.urdf~ b/pa10_description/urdf/pa10_csl.urdf~ deleted file mode 100644 index d03cb43..0000000 --- a/pa10_description/urdf/pa10_csl.urdf~ +++ /dev/null @@ -1,551 +0,0 @@ - - - - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - Gazebo/Blue - - - - 0.5 - 0.5 - Gazebo/Grey - - - - 0.5 - 0.5 - Gazebo/Blue - - - - 0.5 - 0.5 - Gazebo/Grey - - - - 0.5 - 0.5 - Gazebo/Blue - - - - 0.5 - 0.5 - Gazebo/Grey - - - 0.5 - 0.5 - Gazebo/Grey - - - 0.5 - 0.5 - Gazebo/Grey - - - 0.5 - 0.5 - Gazebo/Orange - - - 0.5 - 0.5 - Gazebo/Orange - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/PositionJointInterface - hardware_interface/EffortJointInterface - - - diff --git a/pa10_description/urdf/pa10csl.gazebo b/pa10_description/urdf/pa10csl.gazebo index 3fe8a89..5b60f42 100644 --- a/pa10_description/urdf/pa10csl.gazebo +++ b/pa10_description/urdf/pa10csl.gazebo @@ -1,7 +1,7 @@ - + @@ -70,15 +70,16 @@ - 0.5 - 0.5 + 1000.5 + 1000.5 Gazebo/Orange - 0.5 - 0.5 + 1000.5 + 1000.5 Gazebo/Orange + diff --git a/pa10_description/urdf/pa10csl.gazebo~ b/pa10_description/urdf/pa10csl.gazebo~ deleted file mode 100644 index 9c4da7f..0000000 --- a/pa10_description/urdf/pa10csl.gazebo~ +++ /dev/null @@ -1,84 +0,0 @@ - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - - - Gazebo/Blue - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - - 0.5 - 0.5 - Gazebo/Blue - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - - 0.5 - 0.5 - Gazebo/Blue - - - - - 0.5 - 0.5 - Gazebo/Grey - - - - 0.5 - 0.5 - Gazebo/Grey - - - - 0.5 - 0.5 - Gazebo/Grey - - - - 0.5 - 0.5 - Gazebo/Orange - - - - 0.5 - 0.5 - Gazebo/Orange - - - diff --git a/pa10_gazebo/launch/pa10.launch b/pa10_gazebo/launch/pa10.launch index 6c141b9..3ed53c3 100644 --- a/pa10_gazebo/launch/pa10.launch +++ b/pa10_gazebo/launch/pa10.launch @@ -1,38 +1,47 @@ + + + + + + - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pa10_gazebo/launch/pa10.launch~ b/pa10_gazebo/launch/pa10.launch~ deleted file mode 100644 index 1f1e198..0000000 --- a/pa10_gazebo/launch/pa10.launch~ +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_gazebo/launch/pa10_ccsl_gazebo.launch b/pa10_gazebo/launch/pa10_ccsl_gazebo.launch deleted file mode 100644 index 402387f..0000000 --- a/pa10_gazebo/launch/pa10_ccsl_gazebo.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_gazebo/launch/pa10_gazebo.launch b/pa10_gazebo/launch/pa10_gazebo.launch deleted file mode 100644 index 402387f..0000000 --- a/pa10_gazebo/launch/pa10_gazebo.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_gazebo/launch/pa10_gazebo.launch~ b/pa10_gazebo/launch/pa10_gazebo.launch~ deleted file mode 100644 index 5bd7824..0000000 --- a/pa10_gazebo/launch/pa10_gazebo.launch~ +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_gazebo/launch/pa10_moveit.launch b/pa10_gazebo/launch/pa10_moveit.launch new file mode 100644 index 0000000..2869e4c --- /dev/null +++ b/pa10_gazebo/launch/pa10_moveit.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pa10_gazebo/launch/pa10_spawn.launch b/pa10_gazebo/launch/pa10_spawn.launch index 9b1cee9..f6857f3 100644 --- a/pa10_gazebo/launch/pa10_spawn.launch +++ b/pa10_gazebo/launch/pa10_spawn.launch @@ -5,12 +5,12 @@ - + - + - + diff --git a/pa10_gazebo/launch/pa10_spawn.launch~ b/pa10_gazebo/launch/pa10_spawn.launch~ deleted file mode 100644 index 6f97f3c..0000000 --- a/pa10_gazebo/launch/pa10_spawn.launch~ +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_gazebo/launch/simulation_pa10_bringup.launch b/pa10_gazebo/launch/simulation_pa10_bringup.launch deleted file mode 100644 index 8df163f..0000000 --- a/pa10_gazebo/launch/simulation_pa10_bringup.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/pa10_gazebo/launch/simulation_pa10_bringup.launch~ b/pa10_gazebo/launch/simulation_pa10_bringup.launch~ deleted file mode 100644 index 7b44658..0000000 --- a/pa10_gazebo/launch/simulation_pa10_bringup.launch~ +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/pa10_hardware_interface/CMakeLists.txt~ b/pa10_hardware_interface/CMakeLists.txt~ deleted file mode 100644 index 160fa97..0000000 --- a/pa10_hardware_interface/CMakeLists.txt~ +++ /dev/null @@ -1,218 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(pa10_hardware_interface) - -## Add support for C++11, supported in ROS Kinetic and newer -# add_definitions(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - cmake_modules - control_toolbox - hardware_interface - joint_limits_interface - roscpp - sensor_msgs - controller_manager -) - -find_package(Eigen REQUIRED) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES pa10_hardware_interface -# CATKIN_DEPENDS cmake_modules control_toolbox eigen hardware_interface joint_limits_interface roscpp -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - include - ${catkin_INCLUDE_DIRS} - SYSTEM ${Boost_INCLUDE_DIR} ${EIGEN_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/pa10_hardware_interface.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(joint_position_interface src/joint_position_interface.cpp) -add_dependencies(joint_position_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(joint_position_interface ${catkin_LIBRARIES}) - -add_executable(pa10_fake_hardware_interface src/pa10_fake_hardware_interface.cpp) -add_dependencies(pa10_fake_hardware_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(pa10_fake_hardware_interface ${catkin_LIBRARIES}) - -add_executable(pa10_hardware_interface_node scripts/pa10_hardware_interface.cpp) -add_dependencies(pa10_hardware_interface_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(pa10_hardware_interface_node ${catkin_LIBRARIES}) - - -add_executable(motion_planning_scene_tutorial src/motion_planning_scene_tutorial.cpp) -add_dependencies(motion_planning_scene_tutorial ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(motion_planning_scene_tutorial ${catkin_LIBRARIES}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - - - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_pa10_hardware_interface.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/pa10_hardware_interface/config/fake_controllers.yaml~ b/pa10_hardware_interface/config/gazebo_effort_controllers.yaml similarity index 56% rename from pa10_hardware_interface/config/fake_controllers.yaml~ rename to pa10_hardware_interface/config/gazebo_effort_controllers.yaml index 0b7f516..66f620d 100644 --- a/pa10_hardware_interface/config/fake_controllers.yaml~ +++ b/pa10_hardware_interface/config/gazebo_effort_controllers.yaml @@ -3,14 +3,14 @@ joint_names: [S1,S2,E1,E2,W1,W2,W3,finger_joint_1,finger_joint_2] # Publish all joint states ----------------------------------- joint_state_controller: type: joint_state_controller/JointStateController - publish_rate: 25 + publish_rate: 50 # Position Controllers --------------------------------------- arm_controller: #type: position_controllers/JointGroupPositionController - type: "velocity_controllers/JointTrajectoryController" + #type: "velocity_controllers/JointTrajectoryController" #type: "position_controllers/JointTrajectoryController" - #type: "effort_controllers/JointTrajectoryController" + type: "effort_controllers/JointTrajectoryController" joints: - S1 @@ -21,23 +21,23 @@ arm_controller: - W2 - W3 gains: - S1: {p: 2.0, i: 0.05, d: 0.2, i_clamp: 0.5} - S2: {p: 2.0, i: 0.05, d: 0.2, i_clamp: 0.5} - E1: {p: 2.0, i: 0.05, d: 0.2, i_clamp: 0.5} - E2: {p: 2.0, i: 0.05, d: 0.2, i_clamp: 0.5} - W1: {p: 2.0, i: 0.05, d: 0.2, i_clamp: 0.5} - W2: {p: 2.0, i: 0.05, d: 0.2, i_clamp: 0.5} - W3: {p: 2.0, i: 0.05, d: 0.2, i_clamp: 0.5} + S1: {p: 130.0, i: 20.2, d: 80.05, i_clamp: 0.5} + S2: {p: 130.0, i: 20.2, d: 80.05, i_clamp: 0.5} + E1: {p: 130.0, i: 20.2, d: 80.05, i_clamp: 0.5} + E2: {p: 130.0, i: 20.2, d: 80.05, i_clamp: 0.5} + W1: {p: 130.0, i: 20.2, d: 80.05, i_clamp: 0.5} + W2: {p: 130.0, i: 20.2, d: 80.05, i_clamp: 0.5} + W3: {p: 130.0, i: 20.2, d: 80.05, i_clamp: 0.5} constraints: goal_time: 0.5 # Override default - state_publish_rate: 25 # Override default + state_publish_rate: 50 # Override default action_monitor_rate: 20 # Override default stop_trajectory_duration: 0 # Override default gripper_controller: - #type: "position_controllers/JointTrajectoryController" - type: "velocity_controllers/JointTrajectoryController" + type: "effort_controllers/JointTrajectoryController" + #type: "velocity_controllers/JointTrajectoryController" joints: - finger_joint_1 - finger_joint_2 diff --git a/pa10_hardware_interface/config/gazebo_controllers.yaml~ b/pa10_hardware_interface/config/gazebo_position_controllers.yaml similarity index 62% rename from pa10_hardware_interface/config/gazebo_controllers.yaml~ rename to pa10_hardware_interface/config/gazebo_position_controllers.yaml index 47e1ec7..30611ae 100644 --- a/pa10_hardware_interface/config/gazebo_controllers.yaml~ +++ b/pa10_hardware_interface/config/gazebo_position_controllers.yaml @@ -8,9 +8,9 @@ joint_state_controller: # Position Controllers --------------------------------------- arm_controller: #type: position_controllers/JointGroupPositionController - type: "velocity_controllers/JointTrajectoryController" + #type: "velocity_controllers/JointTrajectoryController" #type: "position_controllers/JointTrajectoryController" - #type: "effort_controllers/JointTrajectoryController" + type: "position_controllers/JointTrajectoryController" joints: - S1 @@ -21,13 +21,13 @@ arm_controller: - W2 - W3 gains: - S1: {p: 10.0, i: 0.2, d: 0.05, i_clamp: 2.5} - S2: {p: 10.0, i: 0.2, d: 0.05, i_clamp: 2.5} - E1: {p: 10.0, i: 0.2, d: 0.05, i_clamp: 2.5} - E2: {p: 10.0, i: 0.2, d: 0.05, i_clamp: 2.5} - W1: {p: 10.0, i: 0.2, d: 0.05, i_clamp: 2.5} - W2: {p: 10.0, i: 0.2, d: 0.05, i_clamp: 2.5} - W3: {p: 10.0, i: 0.2, d: 0.05, i_clamp: 2.5} + S1: {p: 10.0, i: 2.2, d: 8.05, i_clamp: 0.5} + S2: {p: 10.0, i: 2.2, d: 8.05, i_clamp: 0.5} + E1: {p: 10.0, i: 2.2, d: 8.05, i_clamp: 0.5} + E2: {p: 10.0, i: 2.2, d: 8.05, i_clamp: 0.5} + W1: {p: 10.0, i: 2.2, d: 8.05, i_clamp: 0.5} + W2: {p: 10.0, i: 2.2, d: 8.05, i_clamp: 0.5} + W3: {p: 10.0, i: 2.2, d: 8.05, i_clamp: 0.5} constraints: goal_time: 0.5 # Override default @@ -36,8 +36,8 @@ arm_controller: stop_trajectory_duration: 0 # Override default gripper_controller: - #type: "position_controllers/JointTrajectoryController" - type: "velocity_controllers/JointTrajectoryController" + type: "position_controllers/JointTrajectoryController" + #type: "velocity_controllers/JointTrajectoryController" joints: - finger_joint_1 - finger_joint_2 diff --git a/pa10_hardware_interface/config/joint_names.yaml~ b/pa10_hardware_interface/config/joint_names.yaml~ deleted file mode 100644 index 33be887..0000000 --- a/pa10_hardware_interface/config/joint_names.yaml~ +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: [soldier_joint_1,soldier_joint_2,elbow_joint_1,elbow_joint_2,wrist_joint_1,wrist_joint_2,wrist_joint_3,finger_joint_1,finger_joint_2] diff --git a/pa10_hardware_interface/config/mycontroller.yaml~ b/pa10_hardware_interface/config/mycontroller.yaml~ deleted file mode 100644 index c78ac7a..0000000 --- a/pa10_hardware_interface/config/mycontroller.yaml~ +++ /dev/null @@ -1,37 +0,0 @@ -joint_names: [soldier_joint_1,soldier_joint_2,elbow_joint_1,elbow_joint_2,wrist_joint_1,wrist_joint_2,wrist_joint_3,finger_joint_1,finger_joint_2] - -# Publish all joint states ----------------------------------- -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - -# Position Controllers --------------------------------------- -arm_controller: - type: position_controllers/JointGroupPositionController - joints: - - soldier_joint_1 - - soldier_joint_2 - - elbow_joint_1 - - elbow_joint_2 - - wrist_joint_1 - - wrist_joint_2 - - wrist_joint_3 - gains: - soldier_joint_1: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - soldier_joint_2: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - elbow_joint_1: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - elbow_joint_2: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - wrist_joint_1: {p: 500, d: 1, i: 0.1, i_clamp: 3, torque_clamp: 60} - wrist_joint_2: {p: 500, d: 1, i: 0.1, i_clamp: 3, torque_clamp: 60} - wrist_joint_3: {p: 500, d: 1, i: 0.1, i_clamp: 3, torque_clamp: 60} - - -gripper_controller: - type: position_controllers/JointGroupPositionController - joints: - - finger_joint_1 - - finger_joint_2 - - gains: - finger_joint_1: &gripper_finger_gains {p: 2000, d: 0, i: 0, p_gain: 2000} - finger_joint_2: *gripper_finger_gains diff --git a/pa10_hardware_interface/config/pids.yaml~ b/pa10_hardware_interface/config/pids.yaml~ deleted file mode 100644 index 00dbb09..0000000 --- a/pa10_hardware_interface/config/pids.yaml~ +++ /dev/null @@ -1,14 +0,0 @@ -gains: - soldier_joint_1: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - soldier_joint_2: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - elbow_joint_1: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - elbow_joint_2: {p: 3000, d: 5, i: 1, i_clamp: 3, torque_clamp: 60} - wrist_joint_1: {p: 500, d: 1, i: 0.1, i_clamp: 3, torque_clamp: 60} - wrist_joint_2: {p: 500, d: 1, i: 0.1, i_clamp: 3, torque_clamp: 60} - wrist_joint_3: {p: 500, d: 1, i: 0.1, i_clamp: 3, torque_clamp: 60} - - finger_joint_1: &gripper_finger_gains {p: 2000, d: 0, i: 0, p_gain: 2000} - finger_joint_2: *gripper_finger_gains - - - diff --git a/pa10_hardware_interface/package.xml~ b/pa10_hardware_interface/package.xml~ deleted file mode 100644 index 8323595..0000000 --- a/pa10_hardware_interface/package.xml~ +++ /dev/null @@ -1,62 +0,0 @@ - - - pa10_hardware_interface - 0.0.0 - The pa10_hardware_interface package - - - - - michalis - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - cmake_modules - control_toolbox - eigen - hardware_interface - joint_limits_interface - roscpp - cmake_modules - control_toolbox - eigen - hardware_interface - joint_limits_interface - roscpp - - - - - - - - diff --git a/pa10_hardware_interface/scripts/add_collision_object.py b/pa10_hardware_interface/scripts/add_collision_object.py new file mode 100644 index 0000000..a1acba3 --- /dev/null +++ b/pa10_hardware_interface/scripts/add_collision_object.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +""" +Add a collision Object In the Planning Scene +For Visualization and collision check with environment + +Description: +This class adds a collision object in the planning scene in order to +avoid it while planning using MoveIt and visualize it in RVIZ. +""" + +import numpy as np +# Ros Libs +import rospy +import tf + +# MoveIt Labraries +import moveit_commander + +# ROS Messages +from geometry_msgs.msg import Pose, PoseStamped + +class add_collision_object: + + def __init__(self): + """ Initialize Collision Object Class """ + self.scene = moveit_commander.PlanningSceneInterface() + self.robot = moveit_commander.RobotCommander() + rospy.sleep(2) + + + def add_object(self, shape = "box", frame = "base_footprint", pose = Pose(), dimensions = [0.1,0.1,0.1]): + """ Add Collision Object to the World """ + p = PoseStamped() + p.header.frame_id = self.robot.get_planning_frame() + p.pose = pose + if (shape == "box"): + self.scene.add_box(frame, p , dimensions) + + def remove(self): + """ Remove All Object from Collision World """ + self.scene.remove_world_object() + diff --git a/pa10_hardware_interface/scripts/add_collision_object.pyc b/pa10_hardware_interface/scripts/add_collision_object.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5c897ee71b427bb579fa993d28360c79bf6ca2cf GIT binary patch literal 2061 zcmchX-;diw5XZ-Meq9I@3Ka3Sh?hKF+SgK*9;uK{g5%_%qWWd!vp4bPY_H|ryHJ!q zUH=J?_%o2;@8X3wnD06X$B)<6iYK${ot@eF%xv`g+v8t;{x!*H_eA{vgkOu9oG4)# zP##hlP!jI-h>~cp_bBP@^*$wiuSZl4s2Wl-q-sRTh=QCK4Jhx^E20~s=gARvT}+2? z_6LjcNuG;TWTq^&)utBTT&z{*#Az*Dp~QKa*0ruzVv(s@jlVRlc&6=DTIwHChvjLV z-!je$m0gOBc7;&&wQfyasoIUlPnFGD-N54E_}fBTk(H^nBF%Faw>B`Hcink!_TDYL z3!XJ?u3F)Y7^l}p=R!NNDRimCVd<4cxL=!Vb?SU{*SmPx-eHUQ*=ce-)_?MIHVrui zuE*lsSmoZ@DGQfYjmq5zJmwJPAv>~*50v~fVEF~V_A?fWxiK_Ct`E>uguLg>&Hq*` zd-STK=czEdcrnFBS-=7>f|~PdXDl4iI=~)VA{qLj-x9+fxjwC#FE0ofHpoUW{FDVF z)J}tx67zrKo%{4_csldN;NE4m;;d=M$E_;ns#&yq*szKW5z`=M>%v?gS1= zyD;DTqgbHuv4Gk9b%Md4BKVQ>^ofK2AB@!h@AW$b;nYrUD=fXXf$U*tY}Z!8}m+(iPH7QIcO zr?{*An(IJH4^7I-H+{wUy)Mg zCgaFahxJv(%?z8miAPug-7RDEumj+g;w!|pFJxs`_LxO?I^Q8!`$kXaQ~#(-_G28# RqG<==B%B13XcFBE{{n9s2nGNE literal 0 HcmV?d00001 diff --git a/pa10_hardware_interface/scripts/joint_position_interface.cpp~ b/pa10_hardware_interface/scripts/joint_position_interface.cpp~ deleted file mode 100644 index 4428f54..0000000 --- a/pa10_hardware_interface/scripts/joint_position_interface.cpp~ +++ /dev/null @@ -1,131 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -#include -#include -#include "sensor_msgs/JointState.h" - -class pa10 : public hardware_interface::RobotHW -{ -public: - - ros::NodeHandle n_; - - //bool nlyPositionFeedback = true; - - pa10() - { - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_1("soldier_joint_1", &pos[0], &vel[0], &eff[0]); - jnt_state_interface.registerHandle(state_handle_1); - - hardware_interface::JointStateHandle state_handle_2("soldier_joint_2", &pos[1], &vel[1], &eff[1]); - jnt_state_interface.registerHandle(state_handle_2); - - hardware_interface::JointStateHandle state_handle_3("elbow_joint_1", &pos[2], &vel[2], &eff[2]); - jnt_state_interface.registerHandle(state_handle_3); - - hardware_interface::JointStateHandle state_handle_4("elbow_joint_2", &pos[3], &vel[3], &eff[3]); - jnt_state_interface.registerHandle(state_handle_4); - - hardware_interface::JointStateHandle state_handle_5("wrist_joint_1", &pos[4], &vel[4], &eff[4]); - jnt_state_interface.registerHandle(state_handle_5); - - hardware_interface::JointStateHandle state_handle_6("wrist_joint_2", &pos[5], &vel[5], &eff[5]); - jnt_state_interface.registerHandle(state_handle_6); - - hardware_interface::JointStateHandle state_handle_7("wrist_joint_3", &pos[6], &vel[6], &eff[6]); - jnt_state_interface.registerHandle(state_handle_7); - - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle vel_handle_1(jnt_state_interface.getHandle("soldier_joint_1"), &cmd[0]); - jnt_vel_interface.registerHandle(vel_handle_1); - - hardware_interface::JointHandle vel_handle_2(jnt_state_interface.getHandle("soldier_joint_2"), &cmd[1]); - jnt_vel_interface.registerHandle(vel_handle_2); - - hardware_interface::JointHandle vel_handle_3(jnt_state_interface.getHandle("elbow_joint_1"), &cmd[2]); - jnt_vel_interface.registerHandle(vel_handle_3); - - hardware_interface::JointHandle vel_handle_4(jnt_state_interface.getHandle("elbow_joint_2"), &cmd[3]); - jnt_vel_interface.registerHandle(vel_handle_4); - - hardware_interface::JointHandle vel_handle_5(jnt_state_interface.getHandle("wrist_joint_1"), &cmd[4]); - jnt_vel_interface.registerHandle(vel_handle_5); - - hardware_interface::JointHandle vel_handle_6(jnt_state_interface.getHandle("wrist_joint_2"), &cmd[5]); - jnt_vel_interface.registerHandle(vel_handle_6); - - hardware_interface::JointHandle vel_handle_7(jnt_state_interface.getHandle("wrist_joint_3"), &cmd[6]); - jnt_vel_interface.registerHandle(vel_handle_7); - - registerInterface(&jnt_vel_interface); - } - - n_ = ros::NodeHandle(); - //subscriber to joint states - ros::Subscriber sub = n_.subscribe("joint_states", 1, &pa10::updateJointState, this); - - //call back function - void updateJointState(sensor_msgs::JointState msg) - { - - pos = msg.position; - vel = msg.velocity; - eff = msg.>effort; - - std::cout<<"Actual Joint States: \nPosition:"< -#include - -#include -#include -#include -#include - -#include -#include -#include "sensor_msgs/JointState.h" - - -#include "std_msgs/Float64MultiArray.h" -#include -#include - -#define PI acos(-1) -using namespace std; -using namespace Eigen; - -//global var -double qdot[7]; - -class pa10 : public hardware_interface::RobotHW -{ -public: - ros::Publisher pub; //publisher to pub the f/t mesurements - ros::NodeHandle n_; - -public: - - pa10() - { - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_1("soldier_joint_1", &pos[0], &vel[0], &eff[0]); - jnt_state_interface.registerHandle(state_handle_1); - - hardware_interface::JointStateHandle state_handle_2("soldier_joint_2", &pos[1], &vel[1], &eff[1]); - jnt_state_interface.registerHandle(state_handle_2); - - hardware_interface::JointStateHandle state_handle_3("elbow_joint_1", &pos[2], &vel[2], &eff[2]); - jnt_state_interface.registerHandle(state_handle_3); - - hardware_interface::JointStateHandle state_handle_4("elbow_joint_2", &pos[3], &vel[3], &eff[3]); - jnt_state_interface.registerHandle(state_handle_4); - - hardware_interface::JointStateHandle state_handle_5("wrist_joint_1", &pos[4], &vel[4], &eff[4]); - jnt_state_interface.registerHandle(state_handle_5); - - hardware_interface::JointStateHandle state_handle_6("wrist_joint_2", &pos[5], &vel[5], &eff[5]); - jnt_state_interface.registerHandle(state_handle_6); - - hardware_interface::JointStateHandle state_handle_7("wrist_joint_3", &pos[6], &vel[6], &eff[6]); - jnt_state_interface.registerHandle(state_handle_7); - - - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle vel_handle_1(jnt_state_interface.getHandle("soldier_joint_1"), &cmd[0]); - jnt_vel_interface.registerHandle(vel_handle_1); - - hardware_interface::JointHandle vel_handle_2(jnt_state_interface.getHandle("soldier_joint_2"), &cmd[1]); - jnt_vel_interface.registerHandle(vel_handle_2); - - hardware_interface::JointHandle vel_handle_3(jnt_state_interface.getHandle("elbow_joint_1"), &cmd[2]); - jnt_vel_interface.registerHandle(vel_handle_3); - - hardware_interface::JointHandle vel_handle_4(jnt_state_interface.getHandle("elbow_joint_2"), &cmd[3]); - jnt_vel_interface.registerHandle(vel_handle_4); - - hardware_interface::JointHandle vel_handle_5(jnt_state_interface.getHandle("wrist_joint_1"), &cmd[4]); - jnt_vel_interface.registerHandle(vel_handle_5); - - hardware_interface::JointHandle vel_handle_6(jnt_state_interface.getHandle("wrist_joint_2"), &cmd[5]); - jnt_vel_interface.registerHandle(vel_handle_6); - - hardware_interface::JointHandle vel_handle_7(jnt_state_interface.getHandle("wrist_joint_3"), &cmd[6]); - jnt_vel_interface.registerHandle(vel_handle_7); - - - registerInterface(&jnt_vel_interface); - - n_ = ros::NodeHandle(); - - } - - //////////////////////////// - /* PA10 RELATED FUNCTIONS */ - //////////////////////////// - - - /* INITIALIZATION */ - void pa10CommInit() - { - - int status; - struct addrinfo host_info; // The struct that getaddrinfo() fills up with data. - struct addrinfo *host_info_list; // Pointer to the to the linked list of host_info's. - - - memset(&host_info, 0, sizeof host_info); - - std::cout << "Setting up the structs..." << std::endl; - - host_info.ai_family = AF_UNSPEC; // IP version not specified. Can be both. - host_info.ai_socktype = SOCK_STREAM; // Use SOCK_STREAM for TCP or SOCK_DGRAM for UDP. - - status = getaddrinfo("147.102.51.71", "4534", &host_info, &host_info_list); - - if (status != 0){ - std::cout << "getaddrinfo error" << gai_strerror(status) ; - exit(0); - } - - std::cout << "Creating a socket..." << std::endl; - - //the socket discriptor - socketfd = socket(host_info_list->ai_family, host_info_list->ai_socktype, host_info_list->ai_protocol); - - if (socketfd == -1){ - std::cout << "socket error\n" ; - exit(0); - } - - std::cout << "Connecting..." << std::endl; - status = connect(socketfd, host_info_list->ai_addr, host_info_list->ai_addrlen); - - if (status == -1){ - std::cout << "connect error\n" ; - exit(0); - } - - std::cout << "Connection with PA 10 PC Established !!!"; - - std::cout << "sending message..." << std::endl; - char *msg = "I040"; - int len; - ssize_t bytes_sent; - len = strlen(msg); - - bytes_sent = send(socketfd, msg, len, 0); - - //return socketfd; - } - - - ///////////////////////// - /* GET PA10 JOINT DATA */ - ///////////////////////// - void getPA10data() - { - float pa10data[7]; - - std::cout << "Waiting to receive data..." << std::endl; - ssize_t bytes_received2; - char incomming_data_buffer2[1000]; - bytes_received2 = recv(socketfd,incomming_data_buffer2,1000, 0); - - if (bytes_received2 == 0){ - std::cout << "host shut down." << std::endl ; - exit(0); - } - - if (bytes_received2 == -1){ - std::cout << "receive error!" << std::endl ; - exit(0); - } - - - - std::cout << bytes_received2 << " bytes received :" << std::endl ; - std::cout << incomming_data_buffer2 << std::endl; - incomming_data_buffer2[bytes_received2 -2] = '\0'; - - std::cout << "Bypassed Get Angles !!!" << "\n"; - - char *mstring; - mstring = (char *)malloc(20 * sizeof(char)); - - char *mstart,*mend; - - if(incomming_data_buffer2[0]=='I') - mstart = &(incomming_data_buffer2[7]); - else - mstart = &(incomming_data_buffer2[1]); - - - for (int count=0; count<7; count++){ - mend = strchr(mstart,97); - strncpy(mstring,mstart,(int)(mend - mstart)); - //cout << "OK" << "\n"; - strcat(mstring,"\0"); - pa10data[count]=atof(mstring); - mstart=mend+1; - } - - std::cout << "PA10: " << " q1: " << pa10data[0] << " q2: " << pa10data[1] - << " q3: " << pa10data[2] << " q4: " << pa10data[3] - << " q5: " << pa10data[4] << " q6: " << pa10data[5] - << " q7: " << pa10data[6] << "\n"; - - for (int i = 0; i < 7; i++) - // fill position pos[],vel[] and eff[] with joints state - pos[i] = pa10data[i]; - // the effort is unavailable - eff[i] = 0.0; - } - - - /////////////////////////////////// - /* SEND TO PA10 THE DESIRED DATA */ - /////////////////////////////////// - void setPA10data(double* qdot) - { - - float DOF1 = qdot[0]*180/PI; - float DOF2 = qdot[1]*180/PI; - float DOF3 = qdot[2]*180/PI; - float DOF4 = qdot[3]*180/PI; - float DOF5 = qdot[4]*180/PI; - float DOF6 = qdot[5]*180/PI; - float DOF7 = qdot[6]*180/PI; - - - std::string result; - std::stringstream convert; - convert<<"M"<::const_iterator it = msg->data.begin(); it != msg->data.end(); ++it) - { - qdot[i] = *it; - i++; - } - } - - - //////////////////////// - /* Set Controller Type*/ - //////////////////////// - void setControllerType(std::string cntr_type) - { - //define the type of the controller --> the command type - controller_type = cntr_type; - } - - - /////////////////////////////////////////////////////////////////// - /* Fuction which computes joint velocities using joint positions */ - /////////////////////////////////////////////////////////////////// - void postovel(double cmd_[9]) - { - for (int i=0;i<7;++i) - { - //compute velocity commands - cmd_vel[i]=(cmd_[i]-cmd_prev[i])*100.0; - //update previous commands - cmd_prev[i] = cmd_[i]; - } - } - - - - /////////////////// - /* Read Function*/ - ////////////////// - void read() - { - // Read position velocity torque from the robot - pa10::getPA10data(); - } - - /////////////////// - /* Write Function*/ - /////////////////// - void write() - { - - // if you want velocity commands - if (controller_type=="VelocityJointInterface") - { - // send the commands to pa10 - pa10::setPA10data(cmd); - } - - // if you want velocity commands - else if (controller_type=="PositionJointInterface") - { - //convert position commands to velocities - pa10::postovel(cmd); - // send the commands to pa10 - pa10::setPA10data(cmd_vel); - } - - else - { - std::cout << "No Supported Controller!" << std::endl ; - exit(0); - } - } - - - - ros::Time get_time() { return ros::Time::now(); } ; - ros::Duration get_period() { ros::Duration(0.1);}; - - -private: - hardware_interface::JointStateInterface jnt_state_interface; - hardware_interface::PositionJointInterface jnt_vel_interface; - double cmd[7] = {0.0}; - double cmd_prev[7] = {0.0}; - double cmd_vel[7] = {0.0}; - double pos[7] = {0.0}; - double vel[7] = {0.0}; - double eff[7] = {0.0}; - std::string controller_type; - bool VelControllWithPoseCommand; - int socketfd ; // The socket descripter -}; - - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "pa10_hardware_interface"); - ros::NodeHandle nh; - ros::Rate loop_rate(10.0); - - pa10 robot; - - //initialize control manager - controller_manager::ControllerManager cm(&robot,nh); - - //define the type of the controller --> the command type - robot.setControllerType("PositionJointInterface"); - - //Initialize Communication with PA10 - robot.pa10CommInit(); - - - while (ros::ok()) - { - //time loop starts - double start =ros::Time::now().toSec(); - - // read robot data - robot.read(); - - // update data - cm.update(robot.get_time(), robot.get_period()); - - //send data to the robot - robot.write(); - - ros::spinOnce(); - loop_rate.sleep(); - - // time loop ends - double end =ros::Time::now().toSec(); - //print dt - printf("Loop dt:%lf\n", end-start); - - loop_rate.sleep(); - } - - return 0; -} diff --git a/pa10_hardware_interface/scripts/pa10_hardware_interface.cpp~ b/pa10_hardware_interface/scripts/pa10_hardware_interface.cpp~ deleted file mode 100644 index c45269f..0000000 --- a/pa10_hardware_interface/scripts/pa10_hardware_interface.cpp~ +++ /dev/null @@ -1,547 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -#include -#include -#include "sensor_msgs/JointState.h" - - -#include "std_msgs/Float64MultiArray.h" -#include -#include - -#include - - -#define PI acos(-1) -using namespace std; -using namespace Eigen; - -//global var -double qdot[7]; - -#define RATE 50.0 - -double anglewrap(double x) -{ - double y = x; - if (x>PI) - y = x-2.0*PI; - else if (x<-M_PI) - y = x +2.0*PI; - - return y; -} - -#define SATUR(x,minx,maxx) max(min(x,maxx),minx) - -class pa10 : public hardware_interface::RobotHW -{ -public: - ros::Publisher pub; //publisher to pub the f/t mesurements - ros::NodeHandle n_; - -public: - - pa10() - { - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_1("S1", &pos[0], &vel[0], &eff[0]); - jnt_state_interface.registerHandle(state_handle_1); - - hardware_interface::JointStateHandle state_handle_2("S2", &pos[1], &vel[1], &eff[1]); - jnt_state_interface.registerHandle(state_handle_2); - - hardware_interface::JointStateHandle state_handle_3("E1", &pos[2], &vel[2], &eff[2]); - jnt_state_interface.registerHandle(state_handle_3); - - hardware_interface::JointStateHandle state_handle_4("E2", &pos[3], &vel[3], &eff[3]); - jnt_state_interface.registerHandle(state_handle_4); - - hardware_interface::JointStateHandle state_handle_5("W1", &pos[4], &vel[4], &eff[4]); - jnt_state_interface.registerHandle(state_handle_5); - - hardware_interface::JointStateHandle state_handle_6("W2", &pos[5], &vel[5], &eff[5]); - jnt_state_interface.registerHandle(state_handle_6); - - hardware_interface::JointStateHandle state_handle_7("W3", &pos[6], &vel[6], &eff[6]); - jnt_state_interface.registerHandle(state_handle_7); - - hardware_interface::JointStateHandle state_handle_8("finger_joint_1", &pos[7], &vel[7], &eff[7]); - jnt_state_interface.registerHandle(state_handle_8); - - hardware_interface::JointStateHandle state_handle_9("finger_joint_2", &pos[8], &vel[8], &eff[8]); - jnt_state_interface.registerHandle(state_handle_9); - - - - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle vel_handle_1(jnt_state_interface.getHandle("S1"), &cmd[0]); - jnt_vel_interface.registerHandle(vel_handle_1); - - hardware_interface::JointHandle vel_handle_2(jnt_state_interface.getHandle("S2"), &cmd[1]); - jnt_vel_interface.registerHandle(vel_handle_2); - - hardware_interface::JointHandle vel_handle_3(jnt_state_interface.getHandle("E1"), &cmd[2]); - jnt_vel_interface.registerHandle(vel_handle_3); - - hardware_interface::JointHandle vel_handle_4(jnt_state_interface.getHandle("E2"), &cmd[3]); - jnt_vel_interface.registerHandle(vel_handle_4); - - hardware_interface::JointHandle vel_handle_5(jnt_state_interface.getHandle("W1"), &cmd[4]); - jnt_vel_interface.registerHandle(vel_handle_5); - - hardware_interface::JointHandle vel_handle_6(jnt_state_interface.getHandle("W2"), &cmd[5]); - jnt_vel_interface.registerHandle(vel_handle_6); - - hardware_interface::JointHandle vel_handle_7(jnt_state_interface.getHandle("W3"), &cmd[6]); - jnt_vel_interface.registerHandle(vel_handle_7); - - hardware_interface::JointHandle vel_handle_8(jnt_state_interface.getHandle("finger_joint_1"), &cmd[7]); - jnt_vel_interface.registerHandle(vel_handle_8); - - hardware_interface::JointHandle vel_handle_9(jnt_state_interface.getHandle("finger_joint_2"), &cmd[8]); - jnt_vel_interface.registerHandle(vel_handle_9); - - - registerInterface(&jnt_vel_interface); - - n_ = ros::NodeHandle(); - - - } - - //////////////////////////// - /* PA10 RELATED FUNCTIONS */ - //////////////////////////// - - - /* INITIALIZATION */ - void pa10CommInit() - { - - int status; - struct addrinfo host_info; // The struct that getaddrinfo() fills up with data. - struct addrinfo *host_info_list; // Pointer to the to the linked list of host_info's. - - - memset(&host_info, 0, sizeof host_info); - - std::cout << "Setting up the structs..." << std::endl; - - host_info.ai_family = AF_UNSPEC; // IP version not specified. Can be both. - host_info.ai_socktype = SOCK_STREAM; // Use SOCK_STREAM for TCP or SOCK_DGRAM for UDP. - - status = getaddrinfo("147.102.51.71", "4534", &host_info, &host_info_list); - - if (status != 0){ - std::cout << "getaddrinfo error" << gai_strerror(status) ; - exit(0); - } - - std::cout << "Creating a socket..." << std::endl; - - //the socket discriptor - socketfd = socket(host_info_list->ai_family, host_info_list->ai_socktype, host_info_list->ai_protocol); - - if (socketfd == -1){ - std::cout << "socket error\n" ; - exit(0); - } - - std::cout << "Connecting..." << std::endl; - status = connect(socketfd, host_info_list->ai_addr, host_info_list->ai_addrlen); - - if (status == -1){ - std::cout << "connect error\n" ; - exit(0); - } - - std::cout << "Connection with PA 10 PC Established !!!"; - - std::cout << "sending message..." << std::endl; - char *msg = "I020"; - int len; - ssize_t bytes_sent; - len = strlen(msg); - - bytes_sent = send(socketfd, msg, len, 0); - - //return socketfd; - } - - - ///////////////////////// - /* GET PA10 JOINT DATA */ - ///////////////////////// - void getPA10data() - { - float pa10data[7], pa10vel[7],pa10eff[7]; - - std::cout << "Waiting to receive data..." << std::endl; - ssize_t bytes_received2; - char incomming_data_buffer2[1000]; - //return the num of bytes reads from the socket - bytes_received2 = recv(socketfd,incomming_data_buffer2,1000, 0); - - if (bytes_received2 == 0){ - std::cout << "host shut down." << std::endl ; - exit(0); - } - - if (bytes_received2 == -1){ - std::cout << "receive error!" << std::endl ; - exit(0); - } - - - - std::cout << bytes_received2 << " bytes received :" << std::endl ; - std::cout << incomming_data_buffer2 << std::endl; - incomming_data_buffer2[bytes_received2 -2] = '\0'; - - std::cout << "Bypassed Get Angles !!!" << "\n"; - - char *mstring; - mstring = (char *)malloc(20 * sizeof(char)); - - char *mstart,*mend; - - if(incomming_data_buffer2[0]=='I') - mstart = &(incomming_data_buffer2[7]); - else - mstart = &(incomming_data_buffer2[1]); - - //Read the position values - for (int count=0; count<7; count++){ - //returns a pointer to the first character 'a' in mstart - mend = strchr(mstart,97); - // copy the first mend-mstart chars of mstart to mstring - strncpy(mstring,mstart,(int)(mend - mstart)); - //cout << "OK" << "\n"; - strcat(mstring,"\0"); - // ascii to float - pa10data[count]=atof(mstring); - mstart=mend+1; - } - - // Read the velocity values - mstart = mstart + 1; // Avoid 'V' character - - for (int count=0; count<7; count++){ - //returns a pointer to the first character 'a' in mstart - mend = strchr(mstart,97); - // copy the first mend-mstart chars of mstart to mstring - strncpy(mstring,mstart,(int)(mend - mstart)); - //cout << "OK" << "\n"; - strcat(mstring,"\0"); - // ascii to float - pa10vel[count]=atof(mstring); - mstart=mend+1; - } - - // Read the effort values - mstart = mstart + 1; // Avoid 'T' character - - for (int count=0; count<7; count++){ - //returns a pointer to the first character 'a' in mstart - mend = strchr(mstart,97); - // copy the first mend-mstart chars of mstart to mstring - strncpy(mstring,mstart,(int)(mend - mstart)); - //cout << "OK" << "\n"; - strcat(mstring,"\0"); - // ascii to float - pa10eff[count]=atof(mstring); - mstart=mend+1; - } - - - std::cout << "PA10: " << " q1: " << pa10data[0] << " q2: " << pa10data[1] - << " q3: " << pa10data[2] << " q4: " << pa10data[3] - << " q5: " << pa10data[4] << " q6: " << pa10data[5] - << " q7: " << pa10data[6] << "\n"; - - for (int i = 0; i < 7; i++){ - // fill position pos[],vel[] and eff[] with joints state - pos[i] = pa10data[i]*PI/180.0; - - - - vel[i] = pa10vel[i]*PI/180.0; - - cout << "Jvel[" << i << "]= " << vel[i] << endl; - - eff[i] = pa10eff[i]; - - // if the hdw interface starts running - if (init_cmd_prev) - cmd_prev[i] = pos[i]; - } - - init_cmd_prev = false; - } - - - /////////////////////////////////// - /* SEND TO PA10 THE DESIRED DATA */ - /////////////////////////////////// - void setPA10data(double* qdot) - { - - //float limit[7] = {175.0, 92.0, 172.0, 135.0, 253.0, 163.0, 253.0}; - - cout << "Desired Velocities" << endl; - for (int i=0;i<9;i++) - cout << qdot[i] << ", "; - cout << "" << endl; - - float DOF1 = qdot[0]*180.0/PI; - float DOF2 = qdot[1]*180.0/PI; - float DOF3 = qdot[2]*180.0/PI; - float DOF4 = qdot[3]*180.0/PI; - float DOF5 = qdot[4]*180.0/PI; - float DOF6 = qdot[5]*180.0/PI; - float DOF7 = qdot[6]*180.0/PI; - - - std::string result; - std::stringstream convert; - convert<<"M"<::const_iterator it = msg->data.begin(); it != msg->data.end(); ++it) - { - qdot[i] = *it; - i++; - } - } - - - /////////////////// - /* Smooth Filter */ - /////////////////// - - /* Smoothed Velocities */ - void smooth_filter(float smooth=0.7) - { - for (int i=0;i<7;i++) { - sm_vel[i] = sm_vel[i]+smooth*(cmd_vel[i]-sm_vel[i]); - cmd_vel[i] = sm_vel[i]; - - //cout << cmd_vel[i] << endl; - } - } - - - - //////////////////////// - /* Set Controller Type*/ - //////////////////////// - void setControllerType(std::string cntr_type) - { - //define the type of the controller --> the command type - controller_type = cntr_type; - } - - - /////////////////////////////////////////////////////////////////// - /* Fuction which computes joint velocities using joint positions */ - /////////////////////////////////////////////////////////////////// - void postovel(double cmd_[9]) - { - for (int i=0;i<7;++i) - { - //compute velocity commands - cmd_vel[i]=anglewrap(cmd_[i]-cmd_prev[i])*RATE; - //update previous commands - cmd_prev[i] = cmd_[i]; - } - } - - - - /////////////////// - /* Read Function */ - /////////////////// - void read() - { - // Read position velocity torque from the robot - pa10::getPA10data(); - } - - /////////////////// - /* Write Function*/ - /////////////////// - void write() - { - - // if you want velocity commands - if (controller_type=="VelocityJointInterface") - { - // send the commands to pa10 - pa10::setPA10data(cmd); - } - - // if you want velocity commands - else if (controller_type=="PositionJointInterface") - { - //convert position commands to velocities - pa10::postovel(cmd); - //smoothed velocities - pa10::smooth_filter(1.0); - // send the commands to pa10 - - - for (int i=0;i<7;i++) - cout << "V[" << i << "]= " << cmd[i] << endl; - - double error[7]; - - error[5]=anglewrap(pos[5]-cmd[5]); - error[6]=anglewrap(pos[6]-cmd[6]); - - errorI[5] +=(1.0/RATE)*error[5]; - errorI[6] +=(1.0/RATE)*error[6]; - - errorI[5] = SATUR(errorI[5],-0.2,0.2); - errorI[6] = SATUR(errorI[6],-0.2,0.2); - - - cmd_vel[5]=(-0.06*error[5] - errorI[5]*0.0001)*180.0/PI; - cmd_vel[6]=(-0.06*error[6] - errorI[6]*0.0001)*180.0/PI; - - cout << cmd_vel[5] << endl; - cout << cmd_vel[6] << endl; - - //cmd_vel[6]=0.0; - - pa10::setPA10data(cmd_vel); - } - - else - { - std::cout << "No Supported Controller!" << std::endl ; - exit(0); - } - } - - - - ros::Time get_time() { return ros::Time::now(); } ; - ros::Duration get_period() { ros::Duration(1.0/RATE);}; - - void initvars() { - for (int i=0;i<9;i++) - { - cmd[i]=0.0; - cmd_prev[i] = 0.0; - cmd_vel[i]=0.0; - pos[i] = 0.0; - vel[i] = 0.0; - eff[i] = 0.0; - sm_vel[i] =0.0; - errorI[i]=0.0; - } - init_cmd_prev = true; - - } - -private: - hardware_interface::JointStateInterface jnt_state_interface; - hardware_interface::VelocityJointInterface jnt_vel_interface; - ros::Publisher pub_smoothed_velocities; - double cmd[9]; - double cmd_prev[9]; - double cmd_vel[9],sm_vel[9]; - double pos[9]; - double vel[9]; - double eff[9]; - std::string controller_type; - bool VelControllWithPoseCommand, init_cmd_prev; - int socketfd ; // The socket descripter - - double errorI[9]; -}; - - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "pa10_hardware_interface_node"); - ros::NodeHandle nh; - ros::Rate loop_rate(RATE); - - pa10 robot; - - //initialize control manager - controller_manager::ControllerManager cm(&robot,nh); - - //define the type of the controller --> the command type - robot.setControllerType("VelocityJointInterface"); - - //Initialize Communication with PA10 - robot.pa10CommInit(); - - //Initialize Variables - robot.initvars(); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - ros::Duration dur = robot.get_period(); - - while (ros::ok()) - { - //time loop starts - ros::Time start =ros::Time::now(); - - // read robot data - robot.read(); - - // update data - cm.update(start, dur); - - //send data to the robot - robot.write(); - - //ros::spinOnce(); - //loop_rate.sleep(); - - loop_rate.sleep(); - - // time loop ends - dur = robot.get_time()-start; - - //print dt - printf("Loop dt:%lf\n", dur.toSec()); - - - } - - return 0; -} diff --git a/pa10_hardware_interface/scripts/pa10_ms1_vrep_scene.py b/pa10_hardware_interface/scripts/pa10_ms1_vrep_scene.py new file mode 100755 index 0000000..f98d4da --- /dev/null +++ b/pa10_hardware_interface/scripts/pa10_ms1_vrep_scene.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python + +import numpy as np +from add_collision_object import * + +import rospy +from geometry_msgs.msg import Pose + + +if __name__ == '__main__': + + try: + rospy.init_node('add_collion_objects') + + scene = add_collision_object() + # Define Desired Object Position + obj_pose = Pose() + obj_pose.position.x = -0.6 + obj_pose.position.y = -1.2 + obj_pose.position.z = 0.25 + + obj_pose.orientation.x = 0.0 + obj_pose.orientation.y = 0.0 + obj_pose.orientation.z = 0.0 + obj_pose.orientation.w = 1.0 + + # Define object shape, wrt frame_id, dimensions + scene.add_object("box", "world",obj_pose, [0.41,0.41,0.5]) + rospy.sleep(0.7) + + # Define Desired Object Position + obj_pose = Pose() + obj_pose.position.x = -0.15 + obj_pose.position.y = -2.125 + obj_pose.position.z = 2.05 + + obj_pose.orientation.x = 0.0 + obj_pose.orientation.y = 0.0 + obj_pose.orientation.z = 0.0 + obj_pose.orientation.w = 1.0 + + # Define object shape, wrt frame_id, dimensions + scene.add_object("box", "world",obj_pose, [0.15,2.41,4.1]); + rospy.sleep(0.7) + + except rospy.ROSInterruptException: pass diff --git a/pa10_hardware_interface/src/home_arm.py b/pa10_hardware_interface/src/home_arm.py new file mode 100755 index 0000000..c203e98 --- /dev/null +++ b/pa10_hardware_interface/src/home_arm.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python +"""Publishes joint trajectory to move robot to given pose""" + +import rospy +from trajectory_msgs.msg import JointTrajectory +from trajectory_msgs.msg import JointTrajectoryPoint + +def moveJoint (jointcmds,nbJoints): + topic_name = 'arm_controller/command' + pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1) + jointCmd = JointTrajectory() + point = JointTrajectoryPoint() + jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0); + point.time_from_start = rospy.Duration.from_sec(5.0) + + jointCmd.joint_names.append('E1') + jointCmd.joint_names.append('E2') + jointCmd.joint_names.append('S1') + jointCmd.joint_names.append('S2') + jointCmd.joint_names.append('W1') + jointCmd.joint_names.append('W2') + jointCmd.joint_names.append('W3') + + for i in range(0, nbJoints): + point.positions.append(jointcmds[i]) + point.velocities.append(0) + point.accelerations.append(0) + point.effort.append(0) + jointCmd.points.append(point) + rate = rospy.Rate(50) + count = 0 + while (count < 500): + pub.publish(jointCmd) + count = count + 1 + rate.sleep() + +def moveFingers (jointcmds,nbJoints): + topic_name = 'gripper_controller/command' + pub = rospy.Publisher(topic_name, JointTrajectory, queue_size=1) + jointCmd = JointTrajectory() + point = JointTrajectoryPoint() + jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0); + point.time_from_start = rospy.Duration.from_sec(5.0) + for i in range(0, nbJoints): + jointCmd.joint_names.append('finger_joint_'+str(i+1)) + point.positions.append(jointcmds[i]) + point.velocities.append(0) + point.accelerations.append(0) + point.effort.append(0) + jointCmd.points.append(point) + rate = rospy.Rate(100) + count = 0 + while (count < 500): + pub.publish(jointCmd) + count = count + 1 + rate.sleep() + +if __name__ == '__main__': + try: + rospy.init_node('Move_PA10_home_position') + rospy.sleep(10) + + #home robots + moveJoint ([0.0,0.0,0.0,0.0,0.0,0.0,0.0],7) + moveFingers ([0.0,0.0],2) + except rospy.ROSInterruptException: + print "program interrupted before completion" diff --git a/pa10_hardware_interface/src/home_gripper.py b/pa10_hardware_interface/src/home_gripper.py deleted file mode 100755 index 65207aa..0000000 --- a/pa10_hardware_interface/src/home_gripper.py +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python - -import rospy -import actionlib -from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState - -joint_names_gripper = ["finger_joint_1", "finger_joint_2"] -joint_names_Arm = ["soldier_joint_1","soldier_joint_2","elbow_joint_1","elbow_joint_2","wrist_joint_1","wrist_joint_2","wrist_joint_3"] -closed_gripper = [-0.039, -0.039] -home_arm= [.0 for i in range(0, 7)] - -if __name__ == "__main__": - rospy.init_node("home_initialize") - rospy.loginfo("Waiting for gripper_controller...") - client = actionlib.SimpleActionClient("gripper_controller/command", FollowJointTrajectoryAction) - client.wait_for_server() - rospy.loginfo("...connected.") - - rospy.wait_for_message("/joint_states", JointState) - - trajectory = JointTrajectory() - trajectory.joint_names = joint_names_gripper - trajectory.points.append(JointTrajectoryPoint()) - trajectory.points[0].positions = closed_gripper - trajectory.points[0].velocities = [0.0 for i in joint_names_gripper] - trajectory.points[0].accelerations = [0.0 for i in joint_names_gripper] - trajectory.points[0].time_from_start = rospy.Duration(2.0) - - rospy.loginfo("Opening gripper...") - goal = FollowJointTrajectoryGoal() - goal.trajectory = trajectory - goal.goal_time_tolerance = rospy.Duration(0.0) - - client.send_goal(goal) - client.wait_for_result(rospy.Duration(5.0)) - rospy.loginfo("Gripper opened.") - - ### ARM CONTROLL ### - - rospy.loginfo("Waiting for arm_controller...") - client = actionlib.SimpleActionClient("arm_controller/command", FollowJointTrajectoryAction) - client.wait_for_server() - rospy.loginfo("...connected.") - - rospy.wait_for_message("/joint_states", JointState) - - trajectory = JointTrajectory() - trajectory.joint_names = joint_names_Arm - trajectory.points.append(JointTrajectoryPoint()) - trajectory.points[0].positions = home_arm - trajectory.points[0].velocities = [0.0 for i in joint_names_Arm] - trajectory.points[0].accelerations = [0.0 for i in joint_names_Arm] - trajectory.points[0].time_from_start = rospy.Duration(2.0) - - rospy.loginfo("Initializing Arm...") - goal = FollowJointTrajectoryGoal() - goal.trajectory = trajectory - goal.goal_time_tolerance = rospy.Duration(0.0) - - client.send_goal(goal) - client.wait_for_result(rospy.Duration(10.0)) - rospy.loginfo("Arm initialized.") diff --git a/pa10_hardware_interface/src/home_gripper.py~ b/pa10_hardware_interface/src/home_gripper.py~ deleted file mode 100755 index d66e227..0000000 --- a/pa10_hardware_interface/src/home_gripper.py~ +++ /dev/null @@ -1,64 +0,0 @@ -#!/usr/bin/env python - -import rospy -import actionlib -from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState - -joint_names_gripper = ["finger_joint_1", "finger_joint_2"] -joint_names_Arm = ["soldier_joint_1","soldier_joint_2","elbow_joint_1","elbow_joint_2","wrist_joint_1","wrist_joint_2","wrist_joint_3"] -closed_gripper = [-0.039, -0.039] -home_arm= [.0 for i in range(0, 7)] - -if __name__ == "__main__": - rospy.init_node("home_initialize") - rospy.loginfo("Waiting for gripper_controller...") - client = actionlib.SimpleActionClient("gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction) - client.wait_for_server() - rospy.loginfo("...connected.") - - rospy.wait_for_message("/joint_states", JointState) - - trajectory = JointTrajectory() - trajectory.joint_names = joint_names_gripper - trajectory.points.append(JointTrajectoryPoint()) - trajectory.points[0].positions = closed_gripper - trajectory.points[0].velocities = [0.0 for i in joint_names_gripper] - trajectory.points[0].accelerations = [0.0 for i in joint_names_gripper] - trajectory.points[0].time_from_start = rospy.Duration(2.0) - - rospy.loginfo("Opening gripper...") - goal = FollowJointTrajectoryGoal() - goal.trajectory = trajectory - goal.goal_time_tolerance = rospy.Duration(0.0) - - client.send_goal(goal) - client.wait_for_result(rospy.Duration(5.0)) - rospy.loginfo("Gripper opened.") - - ### ARM CONTROLL ### - - rospy.loginfo("Waiting for arm_controller...") - client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) - client.wait_for_server() - rospy.loginfo("...connected.") - - rospy.wait_for_message("/joint_states", JointState) - - trajectory = JointTrajectory() - trajectory.joint_names = joint_names_Arm - trajectory.points.append(JointTrajectoryPoint()) - trajectory.points[0].positions = home_arm - trajectory.points[0].velocities = [0.0 for i in joint_names_Arm] - trajectory.points[0].accelerations = [0.0 for i in joint_names_Arm] - trajectory.points[0].time_from_start = rospy.Duration(2.0) - - rospy.loginfo("Initializing Arm...") - goal = FollowJointTrajectoryGoal() - goal.trajectory = trajectory - goal.goal_time_tolerance = rospy.Duration(0.0) - - client.send_goal(goal) - client.wait_for_result(rospy.Duration(10.0)) - rospy.loginfo("Arm initialized.") diff --git a/pa10_hardware_interface/src/joint_position_interface.cpp~ b/pa10_hardware_interface/src/joint_position_interface.cpp~ deleted file mode 100644 index a77b654..0000000 --- a/pa10_hardware_interface/src/joint_position_interface.cpp~ +++ /dev/null @@ -1,205 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -#include -#include -#include "sensor_msgs/JointState.h" - -class pa10 : public hardware_interface::RobotHW -{ -public: - ros::Subscriber sub; - ros::NodeHandle n_; - - //OnlyPositionFeedback = true; -public: - pa10() - { - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_1("soldier_joint_1", &pos[0], &vel[0], &eff[0]); - jnt_state_interface.registerHandle(state_handle_1); - - hardware_interface::JointStateHandle state_handle_2("soldier_joint_2", &pos[1], &vel[1], &eff[1]); - jnt_state_interface.registerHandle(state_handle_2); - - hardware_interface::JointStateHandle state_handle_3("elbow_joint_1", &pos[2], &vel[2], &eff[2]); - jnt_state_interface.registerHandle(state_handle_3); - - hardware_interface::JointStateHandle state_handle_4("elbow_joint_2", &pos[3], &vel[3], &eff[3]); - jnt_state_interface.registerHandle(state_handle_4); - - hardware_interface::JointStateHandle state_handle_5("wrist_joint_1", &pos[4], &vel[4], &eff[4]); - jnt_state_interface.registerHandle(state_handle_5); - - hardware_interface::JointStateHandle state_handle_6("wrist_joint_2", &pos[5], &vel[5], &eff[5]); - jnt_state_interface.registerHandle(state_handle_6); - - hardware_interface::JointStateHandle state_handle_7("wrist_joint_3", &pos[6], &vel[6], &eff[6]); - jnt_state_interface.registerHandle(state_handle_7); - - hardware_interface::JointStateHandle state_handle_8("finger_joint_1", &pos[7], &vel[7], &eff[7]); - jnt_state_interface.registerHandle(state_handle_8); - - hardware_interface::JointStateHandle state_handle_9("finger_joint_2", &pos[8], &vel[8], &eff[8]); - jnt_state_interface.registerHandle(state_handle_9); - - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle vel_handle_1(jnt_state_interface.getHandle("soldier_joint_1"), &cmd[0]); - jnt_vel_interface.registerHandle(vel_handle_1); - - hardware_interface::JointHandle vel_handle_2(jnt_state_interface.getHandle("soldier_joint_2"), &cmd[1]); - jnt_vel_interface.registerHandle(vel_handle_2); - - hardware_interface::JointHandle vel_handle_3(jnt_state_interface.getHandle("elbow_joint_1"), &cmd[2]); - jnt_vel_interface.registerHandle(vel_handle_3); - - hardware_interface::JointHandle vel_handle_4(jnt_state_interface.getHandle("elbow_joint_2"), &cmd[3]); - jnt_vel_interface.registerHandle(vel_handle_4); - - hardware_interface::JointHandle vel_handle_5(jnt_state_interface.getHandle("wrist_joint_1"), &cmd[4]); - jnt_vel_interface.registerHandle(vel_handle_5); - - hardware_interface::JointHandle vel_handle_6(jnt_state_interface.getHandle("wrist_joint_2"), &cmd[5]); - jnt_vel_interface.registerHandle(vel_handle_6); - - hardware_interface::JointHandle vel_handle_7(jnt_state_interface.getHandle("wrist_joint_3"), &cmd[6]); - jnt_vel_interface.registerHandle(vel_handle_7); - - hardware_interface::JointHandle vel_handle_8(jnt_state_interface.getHandle("finger_joint_1"), &cmd[7]); - jnt_vel_interface.registerHandle(vel_handle_8); - - hardware_interface::JointHandle vel_handle_9(jnt_state_interface.getHandle("finger_joint_2"), &cmd[8]); - jnt_vel_interface.registerHandle(vel_handle_9); - - - registerInterface(&jnt_vel_interface); - - n_ = ros::NodeHandle(); - //subscriber to joint states - //sub = n_.subscribe("joint_states", 1, &pa10::updateJointState, this); - - - } - - -// call back function -// void updateJointState(const sensor_msgs::JointState msg) -// { -// /*for (int i=0;i++;i<7){ -// names[i] = msg.name[i]; -// pos[i] = msg.position[i]; -// vel[i] = msg.velocity[i]; -// eff[i] = msg.effort[i]; -// }*/ - -// //std::copy(msg.name.begin(),msg.name.end(),names); -// std::copy(msg.position.begin(),msg.position.end(),pos); -// std::copy(msg.velocity.begin(),msg.velocity.end(),vel); -// std::copy(msg.effort.begin(),msg.effort.end(),eff); - -// std::cout<<"Actual Joint States: \nPosition:"< names[7]; - double dt; - bool OnlyPositionFeedback; - //ros::NodeHandle n_; - bool VelocityInterface; -}; - - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "pa10_hardware_interface"); - ros::NodeHandle nh; - ros::Rate loop_rate(100); - pa10 robot; - - controller_manager::ControllerManager cm(&robot,nh); - ros::AsyncSpinner spinner(1); - spinner.start(); - while (ros::ok()) - { - robot.read(); - cm.update(robot.get_time(), robot.get_period()); - robot.write(); - loop_rate.sleep(); - } - spinner.stop(); -} diff --git a/pa10_hardware_interface/src/joint_publisher.py~ b/pa10_hardware_interface/src/joint_publisher.py~ deleted file mode 100755 index ab2e74f..0000000 --- a/pa10_hardware_interface/src/joint_publisher.py~ +++ /dev/null @@ -1,34 +0,0 @@ -#!/usr/bin/env python -import rospy -from std_msgs.msg import String, Float64MultiArray -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from math import sin,cos,pi - -def talker(): - pub = rospy.Publisher('arm_controller/command', Float64MultiArray, queue_size=10) - rospy.init_node('publisher', anonymous=True) - rate = rospy.Rate(10) # 10hz - while not rospy.is_shutdown(): - now = rospy.get_rostime() - msg = Float64MultiArray() - -# msg.joint_names = ['finger_joint_1',finger_joint_2] -# -# msg.points = [JointTrajectoryPoint()] -# msg.data = [0.4*sin(2*pi*now.nsecs),0.4*cos(2*pi*now.nsecs),0.4*sin(pi*now.nsecs),0.4*cos(2*pi*now.nsecs),0.4*sin(2*pi*now.nsecs),0.4*cos(2*pi*now.nsecs),0.4*sin(pi*now.nsecs)] - msg.data = [-0.2,-0.25,-0.3,-0.35,-0.4,-0.45,-0.5] -# msg.points[0].velocities=[0.0 for i in range(0,7)] -# msg.points[0].accelerations = [0.0 for i in range(0,7)] -# msg.points[0].effort = [0.0 for i in range(0,7)] -# msg.points[0].time_from_start = now - - rospy.loginfo(msg) - pub.publish(msg) - rate.sleep() - -if __name__ == '__main__': - try: - talker() - except rospy.ROSInterruptException: - pass - diff --git a/pa10_hardware_interface/src/motion_planning_scene_tutorial.cpp~ b/pa10_hardware_interface/src/motion_planning_scene_tutorial.cpp~ deleted file mode 100644 index 8b33ab5..0000000 --- a/pa10_hardware_interface/src/motion_planning_scene_tutorial.cpp~ +++ /dev/null @@ -1,169 +0,0 @@ -#include -#include - -// MoveIt! -#include -#include -#include -#include -#include - -#include -#include -#include - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "planning_scene_ros_api_tutorial"); - ros::AsyncSpinner spinner(1); - spinner.start(); - - ros::NodeHandle node_handle; - ros::Duration sleep_time(2.0); - sleep_time.sleep(); - sleep_time.sleep(); - - // BEGIN_TUTORIAL - // - // Advertise the required topic - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // Note that this topic may need to be remapped in the launch file - ros::Publisher planning_scene_diff_publisher = node_handle.advertise("planning_scene", 1); - while (planning_scene_diff_publisher.getNumSubscribers() < 1) - { - ros::WallDuration sleep_t(0.5); - sleep_t.sleep(); - } - - // Define the attached object message - - moveit_msgs::AttachedCollisionObject attached_object; - attached_object.link_name = "link_ee_tool"; - /* The header must contain a valid TF frame*/ - attached_object.object.header.frame_id = "link_ee_tool"; - /* The id of the object */ - attached_object.object.id = "box"; - - /* A default pose */ - geometry_msgs::Pose pose; - pose.orientation.w = 1.0; - - /* Define a box to be attached */ - shape_msgs::SolidPrimitive primitive; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[0] = 0.1; - primitive.dimensions[1] = 0.1; - primitive.dimensions[2] = 0.1; - - attached_object.object.primitives.push_back(primitive); - attached_object.object.primitive_poses.push_back(pose); - - // Note that attaching an object to the robot requires - // the corresponding operation to be specified as an ADD operation - attached_object.object.operation = attached_object.object.ADD; - - // Add an object into the environment - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - - ROS_INFO("Adding the object into the world at the location of the ee tool link."); - moveit_msgs::PlanningScene planning_scene; - planning_scene.world.collision_objects.push_back(attached_object.object); - planning_scene.is_diff = true; - planning_scene_diff_publisher.publish(planning_scene); - sleep_time.sleep(); - - // Interlude: Synchronous vs Asynchronous updates - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // There are two separate mechanisms available to interact - // with the move_group node using diffs: - // - // * Send a diff via a rosservice call and block until - // the diff is applied (synchronous update) - // * Send a diff via a topic, continue even though the diff - // might not be applied yet (asynchronous update) - // - // While most of this tutorial uses the latter mechanism (given the long sleeps - // inserted for visualization purposes asynchronous updates do not pose a problem), - // it would is perfectly justified to replace the planning_scene_diff_publisher - // by the following service client: - ros::ServiceClient planning_scene_diff_client = - node_handle.serviceClient("apply_planning_scene"); - planning_scene_diff_client.waitForExistence(); - // and send the diffs to the planning scene via a service call: - moveit_msgs::ApplyPlanningScene srv; - srv.request.scene = planning_scene; - planning_scene_diff_client.call(srv); - // Note that this does not continue until we are sure the diff has been applied. - // - // Attach an object to the robot - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // When the robot picks up an object from the environment, we need to - // "attach" the object to the robot so that any component dealing with - // the robot model knows to account for the attached object, e.g. for - // collision checking. - // - // Attaching an object requires two operations - // * Removing the original object from the environment - // * Attaching the object to the robot - - /* First, define the REMOVE object message*/ - moveit_msgs::CollisionObject remove_object; - remove_object.id = "box"; - remove_object.header.frame_id = "odom_combined"; - remove_object.operation = remove_object.REMOVE; - - // Note how we make sure that the diff message contains no other - // attached objects or collisions objects by clearing those fields - // first. - /* Carry out the REMOVE + ATTACH operation */ - ROS_INFO("Attaching the object to the right wrist and removing it from the world."); - planning_scene.world.collision_objects.clear(); - planning_scene.world.collision_objects.push_back(remove_object); - planning_scene.robot_state.attached_collision_objects.push_back(attached_object); - planning_scene_diff_publisher.publish(planning_scene); - - sleep_time.sleep(); - - // Detach an object from the robot - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // Detaching an object from the robot requires two operations - // * Detaching the object from the robot - // * Re-introducing the object into the environment - - /* First, define the DETACH object message*/ - moveit_msgs::AttachedCollisionObject detach_object; - detach_object.object.id = "box"; - detach_object.link_name = "link_ee_tool"; - detach_object.object.operation = attached_object.object.REMOVE; - - // Note how we make sure that the diff message contains no other - // attached objects or collisions objects by clearing those fields - // first. - /* Carry out the DETACH + ADD operation */ - ROS_INFO("Detaching the object from the robot and returning it to the world."); - planning_scene.robot_state.attached_collision_objects.clear(); - planning_scene.robot_state.attached_collision_objects.push_back(detach_object); - planning_scene.world.collision_objects.clear(); - planning_scene.world.collision_objects.push_back(attached_object.object); - planning_scene_diff_publisher.publish(planning_scene); - - sleep_time.sleep(); - - // Remove the object from the collision world - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // Removing the object from the collision world just requires - // using the remove object message defined earlier. - // Note, also how we make sure that the diff message contains no other - // attached objects or collisions objects by clearing those fields - // first. - ROS_INFO("Removing the object from the world."); - planning_scene.robot_state.attached_collision_objects.clear(); - planning_scene.world.collision_objects.clear(); - planning_scene.world.collision_objects.push_back(remove_object); - planning_scene_diff_publisher.publish(planning_scene); - // END_TUTORIAL - - ros::shutdown(); - return 0; -} diff --git a/pa10_hardware_interface/src/pa10_fake_hardware_interface.cpp~ b/pa10_hardware_interface/src/pa10_fake_hardware_interface.cpp~ deleted file mode 100644 index c463708..0000000 --- a/pa10_hardware_interface/src/pa10_fake_hardware_interface.cpp~ +++ /dev/null @@ -1,345 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -#include -#include -#include "sensor_msgs/JointState.h" - -#include "std_msgs/Float64MultiArray.h" - -#include - - -#define RATE 25.0//1.0/0.04 - -double -anglewrap(const double x) -{ - double y=x; - - if (x > M_PI) - y=x-2.0*M_PI; - - else if (x<-M_PI) - y=x+2.0*M_PI; - - return y; -} - -double diffAngle(double x, double y) -{ - double diff; - - diff = fmod(y-x, 2*M_PI); - if (diff < 0 ) diff = diff + 2.0*M_PI; - if (diff > M_PI) diff = diff - 2.0*M_PI; - - return (-diff); -} - - -class pa10 : public hardware_interface::RobotHW -{ -public: - ros::Subscriber sub; - ros::NodeHandle n_; - ros::Publisher pub_smoothed_velocities, pub_cmd; - - -public: - pa10() -{ - // connect and register the joint state interface - hardware_interface::JointStateHandle state_handle_1("soldier_joint_1", &pos[0], &vel[0], &eff[0]); - jnt_state_interface.registerHandle(state_handle_1); - - hardware_interface::JointStateHandle state_handle_2("soldier_joint_2", &pos[1], &vel[1], &eff[1]); - jnt_state_interface.registerHandle(state_handle_2); - - hardware_interface::JointStateHandle state_handle_3("elbow_joint_1", &pos[2], &vel[2], &eff[2]); - jnt_state_interface.registerHandle(state_handle_3); - - hardware_interface::JointStateHandle state_handle_4("elbow_joint_2", &pos[3], &vel[3], &eff[3]); - jnt_state_interface.registerHandle(state_handle_4); - - hardware_interface::JointStateHandle state_handle_5("wrist_joint_1", &pos[4], &vel[4], &eff[4]); - jnt_state_interface.registerHandle(state_handle_5); - - hardware_interface::JointStateHandle state_handle_6("wrist_joint_2", &pos[5], &vel[5], &eff[5]); - jnt_state_interface.registerHandle(state_handle_6); - - hardware_interface::JointStateHandle state_handle_7("wrist_joint_3", &pos[6], &vel[6], &eff[6]); - jnt_state_interface.registerHandle(state_handle_7); - - hardware_interface::JointStateHandle state_handle_8("finger_joint_1", &pos[7], &vel[7], &eff[7]); - jnt_state_interface.registerHandle(state_handle_8); - - hardware_interface::JointStateHandle state_handle_9("finger_joint_2", &pos[8], &vel[8], &eff[8]); - jnt_state_interface.registerHandle(state_handle_9); - - registerInterface(&jnt_state_interface); - - // connect and register the joint position interface - hardware_interface::JointHandle vel_handle_1(jnt_state_interface.getHandle("soldier_joint_1"), &cmd[0]); - jnt_vel_interface.registerHandle(vel_handle_1); - - hardware_interface::JointHandle vel_handle_2(jnt_state_interface.getHandle("soldier_joint_2"), &cmd[1]); - jnt_vel_interface.registerHandle(vel_handle_2); - - hardware_interface::JointHandle vel_handle_3(jnt_state_interface.getHandle("elbow_joint_1"), &cmd[2]); - jnt_vel_interface.registerHandle(vel_handle_3); - - hardware_interface::JointHandle vel_handle_4(jnt_state_interface.getHandle("elbow_joint_2"), &cmd[3]); - jnt_vel_interface.registerHandle(vel_handle_4); - - hardware_interface::JointHandle vel_handle_5(jnt_state_interface.getHandle("wrist_joint_1"), &cmd[4]); - jnt_vel_interface.registerHandle(vel_handle_5); - - hardware_interface::JointHandle vel_handle_6(jnt_state_interface.getHandle("wrist_joint_2"), &cmd[5]); - jnt_vel_interface.registerHandle(vel_handle_6); - - hardware_interface::JointHandle vel_handle_7(jnt_state_interface.getHandle("wrist_joint_3"), &cmd[6]); - jnt_vel_interface.registerHandle(vel_handle_7); - - hardware_interface::JointHandle vel_handle_8(jnt_state_interface.getHandle("finger_joint_1"), &cmd[7]); - jnt_vel_interface.registerHandle(vel_handle_8); - - hardware_interface::JointHandle vel_handle_9(jnt_state_interface.getHandle("finger_joint_2"), &cmd[8]); - jnt_vel_interface.registerHandle(vel_handle_9); - - - registerInterface(&jnt_vel_interface); - - n_ = ros::NodeHandle(); - //publisher for smoothed velocities - //pub_smoothed_velocities = n_.advertise("/smoothed_velocities", 10); - //pub_cmd = n_.advertise("/cmd", 10); - - mstart = ros::Time::now(); - - -} - - - /* Smoothed Velocities */ - void smooth_filter(float smooth=0.7) - { - //std_msgs::MultiArrayDimension dim; - //dim.size = 9; - //dim.stride = 9; - - //Publish Joint positions - //std_msgs::Float64MultiArray pub_array; - - //pub_array.layout.dim.push_back(dim); - //pub_array.layout.data_offset = 0; - - for (int i=0;i<9;i++) { - sm_vel[i] = sm_vel[i]+smooth*(vel[i]-sm_vel[i]); - //pub_array.data.push_back(sm_vel[i]); - } - //publish smoothed - //pub_smoothed_velocities.publish(pub_array); - } - - /////////////////////////////////////////////////////////////////// - /* Fuction which computes joint positions using joint velocities */ - /////////////////////////////////////////////////////////////////// - void veltopos(double cmd_[9]) - { - //double cmd_p[9]; - for (int i=0;i<9;++i) - { - //compute velocity commands - pos[i]=cmd_[i]/RATE+cmd_prev[i]; - //update previous commands - cmd_prev[i] = cmd_[i]; - } - //return the computed velocities - //return cmd_p; - } - - - /////////////////////////////////////////////////////////////////// - /* Fuction which computes joint velocities using joint positions */ - /////////////////////////////////////////////////////////////////// - void postovel(double cmd_[9]) - { - for (int i=0;i<9;++i) - { - //compute velocity commands - //cmd[i]=anglewrap(cmd_[i]); - - vel[i]=diffAngle(cmd_[i],cmd_prev[i])*RATE; - //update previous commands - cmd_prev[i] = cmd_[i]; - } - } - - /////////////////// - /* Write Function*/ - /////////////////// - void write() - { - - //std::cout<<"##joints Commands: "< the command type - controller_type = "VelocityJointInterface"; - VelControllWithPoseCommand = true; - - // if you want velocity commands - if (controller_type=="VelocityJointInterface") - { - // Do this cause its a fake interface!! - //pa10::veltopos(cmd); - //memcpy(vel,cmd,sizeof(double)*9); - //efford is initialized to zero - - for (int i=0;i<9;++i) - { - vel[i]=cmd[i]; - pos[i]=(pos[i]+vel[i]/RATE); - eff[i]=0.0; - } - - } - - //if you want effort commands -- NOT IMPLEMENTED YET - else if (controller_type =="EffortJointInterface") - { - for (int i=0;i<9;++i) - { - pos[i]=0.0; - vel[i]=0.0; - eff[i]=0.0; - } - } - - else if (controller_type =="PositionJointInterface") - { - //if (!VelControllWithPoseCommand) - //{ - // I want to send position commands to the robot - // give positions direct to joints_states - //memcpy(pos,cmd,sizeof(double)*9); - //for (int i=0;i<9;++i) - //{ - // vel[i]=0.0; - // eff[i]=0.0; - //} - //} - //else - // I want to send velocities command to the robot - //{ - pa10::postovel(cmd); - pa10::smooth_filter(0.8); - - for (int i=0;i<9;++i) - { - pos[i]=anglewrap(pos[i]+sm_vel[i]/RATE); - vel[i]=sm_vel[i]; - eff[i]=0.0; - } - - //std_msgs::MultiArrayDimension dim; - //dim.size = 9; - //dim.stride = 9; - - //Publish Joint positions - //sensor_msgs::JointState pub_array; - - //pub_array.header.stamp = ros::Time::now(); - - //for (int i=0;i<9;i++) - // pub_array.position.push_back(cmd[i]); - - //pub_cmd.publish(pub_array); - - //memcpy(pos,cmd,sizeof(double)*9); - //pa10::postovel(cmd); - //pa10::smooth_filter(0.8); - //} - - } - - - //std::cout << "Time: " << (ros::Time::now()-mstart).toSec()*1e-9 << std::endl; - //for (int i=0;i<9;i++) - // std::cout << vel[i] << "," ; - - //std::cout << "" << std::endl; - //std::cout << "" << std::endl; - - } - ros::Time get_time() { return ros::Time::now();} ; - ros::Duration get_period() { ros::Duration(1.0/RATE);}; - - void initialization_step() - { - for (int i=0;i<9;++i) - { - vel[i]=0.0; - pos[i]=0.0; - eff[i]=0.0; - cmd[i]=0.0; - sm_vel[i]=0.0; - } - } -private: - hardware_interface::JointStateInterface jnt_state_interface; - //hardware_interface::PositionJointInterface jnt_vel_interface; - hardware_interface::VelocityJointInterface jnt_vel_interface; - double cmd[9],cmd_prev[9],cmd_vel[9],sm_vel[9]; - double pos[9] ; - double vel[9] ; - double eff[9] ; - std::string controller_type; - bool VelControllWithPoseCommand; - ros::Time mstart; -}; - - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "pa10_hardware_interface"); - ros::NodeHandle nh; - ros::Rate loop_rate(RATE); - pa10 robot; - ros::Time now; - - controller_manager::ControllerManager cm(&robot,nh); - - robot.initialization_step(); - ros::AsyncSpinner spinner(1); - spinner.start(); - - ros::Duration dur = robot.get_period(); - - while (ros::ok()) - { - - now = robot.get_time(); - robot.read(); - cm.update(now,dur); - robot.write(); - loop_rate.sleep(); - dur = robot.get_time()-now; - } - spinner.stop(); -} diff --git a/pa10_hardware_interface/src/pa10_vrep_ms1_planning_scene.cpp b/pa10_hardware_interface/src/pa10_vrep_ms1_planning_scene.cpp new file mode 100644 index 0000000..bbba123 --- /dev/null +++ b/pa10_hardware_interface/src/pa10_vrep_ms1_planning_scene.cpp @@ -0,0 +1,21 @@ +#include +#include + +// MoveIt! +#include +#include +#include +#include +#include + +#include +#include +#include + +int main(int argc, char **argv) +{ + + + ros::shutdown(); + return 0; +} diff --git a/pa10_hardware_interface/src/simple_trajectoryry.cpp~ b/pa10_hardware_interface/src/simple_trajectoryry.cpp~ deleted file mode 100644 index 22580f0..0000000 --- a/pa10_hardware_interface/src/simple_trajectoryry.cpp~ +++ /dev/null @@ -1,129 +0,0 @@ -#include -#include -#include - -typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient; - -class RobotArm -{ -private: - // Action client for the joint trajectory action - // used to trigger the arm movement action - TrajClient* traj_client_; - -public: - //! Initialize the action client and wait for action server to come up - RobotArm() - { - // tell the action client that we want to spin a thread by default - traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true); - - // wait for action server to come up - while(!traj_client_->waitForServer(ros::Duration(5.0))){ - ROS_INFO("Waiting for the joint_trajectory_action server"); - } - } - - //! Clean up the action client - ~RobotArm() - { - delete traj_client_; - } - - //! Sends the command to start a given trajectory - void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal) - { - // When to start the trajectory: 1s from now - goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0); - traj_client_->sendGoal(goal); - } - - //! Generates a simple trajectory with two waypoints, used as an example - /*! Note that this trajectory contains two waypoints, joined together - as a single trajectory. Alternatively, each of these waypoints could - be in its own trajectory - a trajectory can have one or more waypoints - depending on the desired application. - */ - pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory() - { - //our goal variable - pr2_controllers_msgs::JointTrajectoryGoal goal; - - // First, the joint names, which apply to all waypoints - goal.trajectory.joint_names.push_back("r_shoulder_pan_joint"); - goal.trajectory.joint_names.push_back("r_shoulder_lift_joint"); - goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint"); - goal.trajectory.joint_names.push_back("r_elbow_flex_joint"); - goal.trajectory.joint_names.push_back("r_forearm_roll_joint"); - goal.trajectory.joint_names.push_back("r_wrist_flex_joint"); - goal.trajectory.joint_names.push_back("r_wrist_roll_joint"); - - // We will have two waypoints in this goal trajectory - goal.trajectory.points.resize(2); - - // First trajectory point - // Positions - int ind = 0; - goal.trajectory.points[ind].positions.resize(7); - goal.trajectory.points[ind].positions[0] = 0.0; - goal.trajectory.points[ind].positions[1] = 0.0; - goal.trajectory.points[ind].positions[2] = 0.0; - goal.trajectory.points[ind].positions[3] = 0.0; - goal.trajectory.points[ind].positions[4] = 0.0; - goal.trajectory.points[ind].positions[5] = 0.0; - goal.trajectory.points[ind].positions[6] = 0.0; - // Velocities - goal.trajectory.points[ind].velocities.resize(7); - for (size_t j = 0; j < 7; ++j) - { - goal.trajectory.points[ind].velocities[j] = 0.0; - } - // To be reached 1 second after starting along the trajectory - goal.trajectory.points[ind].time_from_start = ros::Duration(1.0); - - // Second trajectory point - // Positions - ind += 1; - goal.trajectory.points[ind].positions.resize(7); - goal.trajectory.points[ind].positions[0] = -0.3; - goal.trajectory.points[ind].positions[1] = 0.2; - goal.trajectory.points[ind].positions[2] = -0.1; - goal.trajectory.points[ind].positions[3] = -1.2; - goal.trajectory.points[ind].positions[4] = 1.5; - goal.trajectory.points[ind].positions[5] = -0.3; - goal.trajectory.points[ind].positions[6] = 0.5; - // Velocities - goal.trajectory.points[ind].velocities.resize(7); - for (size_t j = 0; j < 7; ++j) - { - goal.trajectory.points[ind].velocities[j] = 0.0; - } - // To be reached 2 seconds after starting along the trajectory - goal.trajectory.points[ind].time_from_start = ros::Duration(2.0); - - //we are done; return the goal - return goal; - } - - //! Returns the current state of the action - actionlib::SimpleClientGoalState getState() - { - return traj_client_->getState(); - } - -}; - -int main(int argc, char** argv) -{ - // Init the ROS node - ros::init(argc, argv, "robot_driver"); - - RobotArm arm; - // Start the trajectory - arm.startTrajectory(arm.armExtensionTrajectory()); - // Wait for trajectory completion - while(!arm.getState().isDone() && ros::ok()) - { - usleep(50000); - } -} diff --git a/pa10_hardware_interface/src/tuninig_script.py b/pa10_hardware_interface/src/tuninig_script.py new file mode 100755 index 0000000..bea7b25 --- /dev/null +++ b/pa10_hardware_interface/src/tuninig_script.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python +# license removed for brevity +import rospy +import argparse +from std_msgs.msg import String +from std_msgs.msg import Float64 +from math import cos, sin, pi +from trajectory_msgs.msg import JointTrajectory +from trajectory_msgs.msg import JointTrajectoryPoint +if __name__ == '__main__': + try: + + # Description of parser + parser = argparse.ArgumentParser(description='Ros controllers tuning script.') + + parser.add_argument("-j", "--joint", type=int, default=1, + help='the num of the joint to be tuned') + + parser.add_argument("-a", "--amplitude" , type=float, default=0.5, + help='the amplitude of the equation') + args = parser.parse_args() + + print "Tuning of joint {} control mode.".format(args.joint) + + # topic + topic = "arm_controller/command" + print "Topic Name: /{}".format(topic) + + pub = rospy.Publisher(topic, JointTrajectory, queue_size=100) + + + + + rospy.init_node('talker', anonymous=True) + + jointcmds = [0.0,0.0,0.0,0.0,0.0,0.0,0.0] + + rate = rospy.Rate(10) + t0 = rospy.Time.now().to_sec() + while not rospy.is_shutdown(): + t = rospy.Time.now().to_sec() - t0 + + + jointCmd = JointTrajectory() + point = JointTrajectoryPoint() + jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0); + point.time_from_start = rospy.Duration.from_sec(0.5) + + jointCmd.joint_names.append('S1') + jointCmd.joint_names.append('S2') + jointCmd.joint_names.append('E1') + jointCmd.joint_names.append('E2') + jointCmd.joint_names.append('W1') + jointCmd.joint_names.append('W2') + jointCmd.joint_names.append('W3') + + jointcmds[args.joint-1] = args.amplitude#*sin(0.2*pi*t) + + for i in range(0, 7): + point.positions.append(jointcmds[i]) + point.velocities.append(0) + point.accelerations.append(0) + point.effort.append(0) + jointCmd.points.append(point) + #rospy.loginfo(hello_str) + pub.publish(jointCmd) + rate.sleep() + except rospy.ROSInterruptException: + msg = Float64() + msg.data = 0.0 + pub.publish(msg) + rate.sleep() + pass diff --git a/pa10_hardware_interface/src/tutorial.py~ b/pa10_hardware_interface/src/tutorial.py~ deleted file mode 100644 index 7e0f07b..0000000 --- a/pa10_hardware_interface/src/tutorial.py~ +++ /dev/null @@ -1,35 +0,0 @@ -import sys -import copy -import rospy -import moveit_commander -import moveit_msgs.msg -import geometry_msgs.msg - -print "============ Starting tutorial setup" -moveit_commander.roscpp_initialize(sys.argv) -rospy.init_node('move_group_python_interface_tutorial',anonymous=True) - -robot = moveit_commander.RobotCommander() # get object robot - -scene = moveit_commander.PlanningSceneInterface() - -group = moveit_commander.MoveGroupCommander("arm") - -display_trajectory_publisher = rospy.Publisher( - '/move_group/display_planned_path', - moveit_msgs.msg.DisplayTrajectory) - - -print "============ Waiting for RVIZ..." -rospy.sleep(10) -print "============ Starting tutorial " - - -print "============ Reference frame: %s" % group.get_planning_frame() - -print "============ Robot Groups:" -print robot.get_group_names() - -print "============ Printing robot state" -print robot.get_current_state() -print "============" diff --git a/pa10_moveit_config/config/sensors_kinect2.yaml b/pa10_moveit_config/config/sensors_kinect2.yaml index 122bba8..5a8f394 100644 --- a/pa10_moveit_config/config/sensors_kinect2.yaml +++ b/pa10_moveit_config/config/sensors_kinect2.yaml @@ -1,6 +1,6 @@ sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /kinect2/qhd/points + point_cloud_topic: /kinect/depth/points max_range: 3.0 point_subsample: 1 padding_offset: 0.1 diff --git a/pa10_moveit_config/config/sensors_kinect2.yaml~ b/pa10_moveit_config/config/sensors_kinect2.yaml~ index a4a6ffc..122bba8 100644 --- a/pa10_moveit_config/config/sensors_kinect2.yaml~ +++ b/pa10_moveit_config/config/sensors_kinect2.yaml~ @@ -1,6 +1,6 @@ sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /kinect2/sd/points + point_cloud_topic: /kinect2/qhd/points max_range: 3.0 point_subsample: 1 padding_offset: 0.1 diff --git a/pa10_moveit_config/launch/demo_velocity.launch b/pa10_moveit_config/launch/demo_velocity.launch index e05371f..7dd9bab 100644 --- a/pa10_moveit_config/launch/demo_velocity.launch +++ b/pa10_moveit_config/launch/demo_velocity.launch @@ -1,10 +1,13 @@ + + + + - @@ -12,6 +15,7 @@ + @@ -21,11 +25,11 @@ - + - + diff --git a/pa10_moveit_config/launch/demo_velocity.launch~ b/pa10_moveit_config/launch/demo_velocity.launch~ index 307fbe1..e05371f 100644 --- a/pa10_moveit_config/launch/demo_velocity.launch~ +++ b/pa10_moveit_config/launch/demo_velocity.launch~ @@ -21,7 +21,7 @@ - + diff --git a/pa10_moveit_config/launch/gazebo_demo.launch b/pa10_moveit_config/launch/gazebo_demo.launch new file mode 100644 index 0000000..f3bcfe9 --- /dev/null +++ b/pa10_moveit_config/launch/gazebo_demo.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/pa10_moveit_config/launch/move_group.launch b/pa10_moveit_config/launch/move_group.launch index 343eec7..63f62f9 100644 --- a/pa10_moveit_config/launch/move_group.launch +++ b/pa10_moveit_config/launch/move_group.launch @@ -35,7 +35,6 @@ - diff --git a/pa10_moveit_config/launch/move_group.launch~ b/pa10_moveit_config/launch/move_group.launch~ index c128bf0..343eec7 100644 --- a/pa10_moveit_config/launch/move_group.launch~ +++ b/pa10_moveit_config/launch/move_group.launch~ @@ -1,8 +1,9 @@ - + + @@ -34,10 +35,14 @@ - - - - + + + + + + + + @@ -56,12 +61,12 @@ --> - +