├── pr2_mechanism_controllers ├── msg │ ├── OdometryMatrix.msg │ ├── TrackLinkCmd.msg │ ├── DebugInfo.msg │ ├── Odometer.msg │ ├── BaseOdometryState.msg │ ├── BaseControllerState.msg │ └── BaseControllerState2.msg ├── doc.dox ├── srv │ └── SetProfile.srv ├── controller_plugins.xml ├── scripts │ ├── control_laser.py │ ├── send_periodic_cmd.py │ ├── send_laser_traj_cmd_ms2.py │ ├── send_periodic_cmd_srv.py │ ├── control_base_position_ground_truth.py │ ├── control_base_position.py │ └── pointhead.py ├── CHANGELOG.rst ├── package.xml ├── test │ ├── test_joint_trajectory_controller.cpp │ ├── test_whole_body_controller.cpp │ ├── test_base_controller.cpp │ ├── test_arm_trajectory_controller.cpp │ └── test_odom.cpp ├── CMakeLists.txt ├── cmake │ └── FindEigen.cmake ├── include │ └── pr2_mechanism_controllers │ │ ├── caster_controller.h │ │ └── pr2_gripper_controller.h └── src │ └── pr2_gripper_controller.cpp ├── pr2_controllers_msgs ├── srv │ ├── QueryCalibrationState.srv │ └── QueryTrajectoryState.srv ├── msg │ ├── Pr2GripperCommand.msg │ ├── JointControllerStateArray.msg │ ├── JointTrajectoryControllerState.msg │ └── JointControllerState.msg ├── action │ ├── JointTrajectory.action │ ├── SingleJointPosition.action │ ├── PointHead.action │ └── Pr2GripperCommand.action ├── mainpage.dox ├── package.xml ├── CHANGELOG.rst └── CMakeLists.txt ├── ethercat_trigger_controllers ├── srv │ ├── SetMultiWaveform.srv │ └── SetWaveform.srv ├── mainpage.dox ├── msg │ ├── MultiWaveformTransition.msg │ └── MultiWaveform.msg ├── controller_plugins.xml ├── CHANGELOG.rst ├── package.xml ├── CMakeLists.txt ├── include │ └── ethercat_trigger_controllers │ │ ├── projector_controller.h │ │ └── multi_trigger_controller.h └── src │ └── projector_controller.cpp ├── pr2_controllers ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── robot_mechanism_controllers ├── doc.dox ├── msg │ └── JTCartesianControllerState.msg ├── controller_plugins.xml ├── CHANGELOG.rst ├── posture.py ├── CMakeLists.txt ├── package.xml ├── include │ └── robot_mechanism_controllers │ │ ├── joint_effort_controller.h │ │ ├── cartesian_wrench_controller.h │ │ ├── cartesian_twist_controller.h │ │ ├── joint_spline_trajectory_controller.h │ │ ├── joint_position_controller.h │ │ ├── joint_velocity_controller.h │ │ ├── joint_group_velocity_controller.h │ │ └── cartesian_pose_controller.h ├── scripts │ └── effort.py └── src │ ├── joint_effort_controller.cpp │ └── cartesian_wrench_controller.cpp ├── README.md ├── pr2_head_action ├── head_action.launch ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── pr2_gripper_action ├── CMakeLists.txt ├── mainpage.dox ├── CHANGELOG.rst └── package.xml ├── single_joint_position_action ├── CMakeLists.txt ├── mainpage.dox ├── package.xml └── CHANGELOG.rst ├── joint_trajectory_action ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── pr2_calibration_controllers ├── controller_plugins.xml ├── mainpage.dox ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── pr2_calibration_controllers │ │ ├── fake_calibration_controller.h │ │ ├── gripper_calibration_controller.h │ │ ├── joint_calibration_controller.h │ │ ├── caster_calibration_controller.h │ │ └── wrist_calibration_controller.h └── src │ └── fake_calibration_controller.cpp ├── .travis.yml └── .gitignore /pr2_mechanism_controllers/msg/OdometryMatrix.msg: -------------------------------------------------------------------------------- 1 | float64[] m -------------------------------------------------------------------------------- /pr2_controllers_msgs/srv/QueryCalibrationState.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool is_calibrated -------------------------------------------------------------------------------- /pr2_controllers_msgs/msg/Pr2GripperCommand.msg: -------------------------------------------------------------------------------- 1 | float64 position 2 | float64 max_effort 3 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/action/JointTrajectory.action: -------------------------------------------------------------------------------- 1 | trajectory_msgs/JointTrajectory trajectory 2 | --- 3 | --- 4 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/msg/TrackLinkCmd.msg: -------------------------------------------------------------------------------- 1 | int8 enable 2 | string link_name 3 | geometry_msgs/Point point 4 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/msg/JointControllerStateArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | JointControllerState[] controllestate 3 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/msg/DebugInfo.msg: -------------------------------------------------------------------------------- 1 | float64[] timing 2 | int32 sequence 3 | bool input_valid 4 | float64 residual -------------------------------------------------------------------------------- /ethercat_trigger_controllers/srv/SetMultiWaveform.srv: -------------------------------------------------------------------------------- 1 | MultiWaveform waveform 2 | --- 3 | bool success 4 | string status_message 5 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/msg/Odometer.msg: -------------------------------------------------------------------------------- 1 | float64 distance #total distance traveled (meters) 2 | float64 angle #total angle traveled (radians) -------------------------------------------------------------------------------- /pr2_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pr2_controllers) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/srv/QueryTrajectoryState.srv: -------------------------------------------------------------------------------- 1 | time time 2 | --- 3 | string[] name 4 | float64[] position 5 | float64[] velocity 6 | float64[] acceleration 7 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | @mainpage 4 | 5 | @htmlinclude manifest.html 6 | 7 | This package has no released code API. 8 | 9 | **/ 10 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/srv/SetWaveform.srv: -------------------------------------------------------------------------------- 1 | float64 rep_rate 2 | float64 phase 3 | float64 duty_cycle 4 | int32 running 5 | int32 active_low 6 | int32 pulsed 7 | --- 8 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/doc.dox: -------------------------------------------------------------------------------- 1 | /** 2 | @mainpage 3 | @htmlinclude manifest.html 4 | 5 | Generic Robot Controllers 6 | 7 | The controllers should be used from their ROS API. 8 | **/ 9 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/msg/BaseOdometryState.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Twist velocity 2 | string[] wheel_link_names 3 | float64[] drive_constraint_errors 4 | float64[] longitudinal_slip_constraint_errors -------------------------------------------------------------------------------- /pr2_mechanism_controllers/doc.dox: -------------------------------------------------------------------------------- 1 | /** 2 | @mainpage 3 | 4 | @htmlinclude manifest.html 5 | 6 | PR2 Specific Controllers 7 | 8 | The controller should be used through their ROS APIs. 9 | 10 | **/ 11 | 12 | 13 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/action/SingleJointPosition.action: -------------------------------------------------------------------------------- 1 | float64 position 2 | duration min_duration 3 | float64 max_velocity 4 | --- 5 | --- 6 | Header header 7 | float64 position 8 | float64 velocity 9 | float64 error 10 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/action/PointHead.action: -------------------------------------------------------------------------------- 1 | geometry_msgs/PointStamped target 2 | geometry_msgs/Vector3 pointing_axis 3 | string pointing_frame 4 | duration min_duration 5 | float64 max_velocity 6 | --- 7 | --- 8 | float64 pointing_angle_error -------------------------------------------------------------------------------- /pr2_controllers_msgs/msg/JointTrajectoryControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string[] joint_names 3 | trajectory_msgs/JointTrajectoryPoint desired 4 | trajectory_msgs/JointTrajectoryPoint actual 5 | trajectory_msgs/JointTrajectoryPoint error # Redundant, but useful 6 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/srv/SetProfile.srv: -------------------------------------------------------------------------------- 1 | float64 UpperTurnaround 2 | float64 LowerTurnaround 3 | float64 upperDecelBuffer 4 | float64 lowerDecelBuffer 5 | float64 profile 6 | float64 period 7 | float64 amplitude 8 | float64 offset 9 | --- 10 | float64 time 11 | 12 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/msg/JointControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 set_point 3 | float64 process_value 4 | float64 process_value_dot 5 | float64 error 6 | float64 time_step 7 | float64 command 8 | float64 p 9 | float64 i 10 | float64 d 11 | float64 i_clamp 12 | 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | pr2_controllers [![Build Status](https://travis-ci.com/PR2/pr2_controllers.svg?branch=melodic-devel)](https://travis-ci.com/PR2/pr2_controllers) 2 | ================================================================================================================================================ 3 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/msg/BaseControllerState.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Twist command 2 | float64[] joint_velocity_measured 3 | float64[] joint_velocity_commanded 4 | float64[] joint_velocity_error 5 | float64[] joint_effort_measured 6 | float64[] joint_effort_commanded 7 | float64[] joint_effort_error 8 | string[] joint_names 9 | 10 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/msg/BaseControllerState2.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Twist command 2 | float64[] joint_command 3 | float64[] joint_error 4 | float64[] joint_velocity_commanded 5 | float64[] joint_velocity_measured 6 | float64[] joint_effort_measured 7 | float64[] joint_effort_commanded 8 | float64[] joint_effort_error 9 | string[] joint_names 10 | 11 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/msg/MultiWaveformTransition.msg: -------------------------------------------------------------------------------- 1 | # Used to specify a transition in the SetMultiWaveform service. 2 | 3 | float64 time # Transition time after start of period. 4 | uint32 value # Value of the digital output after the transition time. 5 | string topic # Topic to publish the transition timestamp to, or empty string if the transition should not be published. 6 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/msg/JTCartesianControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped x 3 | geometry_msgs/PoseStamped x_desi 4 | geometry_msgs/PoseStamped x_desi_filtered 5 | geometry_msgs/Twist x_err 6 | geometry_msgs/Twist xd 7 | geometry_msgs/Twist xd_desi 8 | geometry_msgs/Wrench F 9 | float64[] tau_pose 10 | float64[] tau_posture 11 | float64[] tau 12 | std_msgs/Float64MultiArray J 13 | std_msgs/Float64MultiArray N 14 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/msg/MultiWaveform.msg: -------------------------------------------------------------------------------- 1 | # Transitions will occur at k * period + zero_offset + transitions[j].time, where j and 2 | # k are integers. 3 | 4 | float64 period # Period of the waveform in seconds. 5 | float64 zero_offset # Time corresponding to a time of 0 in times[] in seconds 6 | MultiWaveformTransition[] transitions # Transitions in the waveform. Transition times should be in increasing order, and be between 0 (inclusive) and period (exclusive) 7 | -------------------------------------------------------------------------------- /pr2_head_action/head_action.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 10 | 11 | 12 | success_angle_threshold: 0.01 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/action/Pr2GripperCommand.action: -------------------------------------------------------------------------------- 1 | pr2_controllers_msgs/Pr2GripperCommand command 2 | --- 3 | float64 position # The current gripper gap size (in meters) 4 | float64 effort # The current effort exerted (in Newtons) 5 | bool stalled # True iff the gripper is exerting max effort and not moving 6 | bool reached_goal # True iff the gripper position has reached the commanded setpoint 7 | --- 8 | float64 position # The current gripper gap size (in meters) 9 | float64 effort # The current effort exerted (in Newtons) 10 | bool stalled # True iff the gripper is exerting max effort and not moving 11 | bool reached_goal # True iff the gripper position has reached the commanded setpoint 12 | -------------------------------------------------------------------------------- /pr2_gripper_action/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(pr2_gripper_action) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS roscpp actionlib_msgs actionlib pr2_mechanism_controllers robot_mechanism_controllers pr2_controllers_msgs pr2_mechanism_model) 7 | 8 | find_package(Boost REQUIRED COMPONENTS thread) 9 | include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 10 | 11 | catkin_package() 12 | 13 | add_executable(pr2_gripper_action src/pr2_gripper_action.cpp) 14 | target_link_libraries(pr2_gripper_action ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 15 | 16 | install(TARGETS pr2_gripper_action 17 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 18 | -------------------------------------------------------------------------------- /single_joint_position_action/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(single_joint_position_action) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS roscpp pr2_controllers_msgs actionlib) 7 | 8 | find_package(Boost REQUIRED COMPONENTS thread) 9 | include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 10 | 11 | catkin_package() 12 | 13 | add_executable(single_joint_position_action src/single_joint_position_action.cpp) 14 | target_link_libraries(single_joint_position_action ${Boost_LIBRARIES} 15 | ${catkin_LIBRARIES}) 16 | add_dependencies(single_joint_position_action ${catkin_EXPORTED_TARGETS}) 17 | 18 | install(TARGETS single_joint_position_action 19 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 20 | -------------------------------------------------------------------------------- /joint_trajectory_action/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(joint_trajectory_action) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS roscpp trajectory_msgs pr2_controllers_msgs actionlib) 7 | 8 | find_package(Boost REQUIRED COMPONENTS thread) 9 | include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 10 | 11 | catkin_package() 12 | 13 | add_definitions(-O3) 14 | 15 | add_executable(joint_trajectory_action src/joint_trajectory_action.cpp) 16 | target_link_libraries(joint_trajectory_action ${Boost_LIBRARIES} 17 | ${catkin_LIBRARIES}) 18 | add_dependencies(joint_trajectory_action ${catkin_EXPORTED_TARGETS}) 19 | 20 | install(TARGETS joint_trajectory_action 21 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 22 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /pr2_gripper_action/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_action is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /joint_trajectory_action/package.xml: -------------------------------------------------------------------------------- 1 | 2 | joint_trajectory_action 3 | 1.10.18 4 | The joint_trajectory_action is a node that exposes an action interface 5 | to a joint trajectory controller. 6 | ROS Orphaned Package Maintainers 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/joint_trajectory_action 11 | 12 | 13 | Stuart Glaser 14 | 15 | catkin 16 | 17 | roscpp 18 | trajectory_msgs 19 | pr2_controllers_msgs 20 | actionlib 21 | 22 | roscpp 23 | trajectory_msgs 24 | pr2_controllers_msgs 25 | actionlib 26 | 27 | 28 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_controllers_msgs is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /pr2_controllers_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pr2_controllers_msgs 3 | 1.10.18 4 | Messages, services, and actions used in the pr2_controllers stack. 5 | ROS Orphaned Package Maintainers 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/pr2_controllers_msgs 10 | 11 | 12 | Stuart Glaser 13 | 14 | catkin 15 | 16 | actionlib_msgs 17 | trajectory_msgs 18 | geometry_msgs 19 | message_generation 20 | 21 | actionlib_msgs 22 | trajectory_msgs 23 | geometry_msgs 24 | message_runtime 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_calibration_controllers is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /single_joint_position_action/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b single_joint_position_action is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /single_joint_position_action/package.xml: -------------------------------------------------------------------------------- 1 | 2 | single_joint_position_action 3 | 1.10.18 4 | The single joint position action is a node that provides an action 5 | interface for commanding a trajectory to move a joint to a particular 6 | position. The action reports success when the joint reaches the desired 7 | position. 8 | ROS Orphaned Package Maintainers 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/single_joint_position_action 13 | 14 | 15 | Stuart Glaser 16 | 17 | catkin 18 | 19 | roscpp 20 | pr2_controllers_msgs 21 | actionlib 22 | 23 | roscpp 24 | pr2_controllers_msgs 25 | actionlib 26 | 27 | -------------------------------------------------------------------------------- /pr2_controllers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pr2_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | 8 | 1.10.17 (2019-08-08) 9 | -------------------- 10 | 11 | 1.10.16 (2019-07-26) 12 | -------------------- 13 | 14 | 1.10.15 (2018-09-13) 15 | -------------------- 16 | 17 | 1.10.14 (2018-02-13) 18 | -------------------- 19 | * Merge pull request `#387 `_ from k-okada/maintain 20 | change maintainer to ROS orphaned package maintainer 21 | * change maintainer to ROS orphaned package maintainer 22 | * Contributors: Kei Okada 23 | 24 | 1.10.13 (2015-02-09) 25 | -------------------- 26 | 27 | 1.10.12 (2015-01-13) 28 | -------------------- 29 | 30 | 1.10.10 (2014-12-16) 31 | -------------------- 32 | 33 | 1.10.9 (2014-12-16) 34 | ------------------- 35 | * Changelogs 36 | * Added changelogs 37 | * Added indigo devel branch 38 | * Changelogs; maintainership 39 | * Contributors: TheDash 40 | 41 | * Added indigo devel branch 42 | * Changelogs; maintainership 43 | * Contributors: TheDash 44 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pr2_controllers_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | 8 | 1.10.17 (2019-08-08) 9 | -------------------- 10 | 11 | 1.10.16 (2019-07-26) 12 | -------------------- 13 | 14 | 1.10.15 (2018-09-13) 15 | -------------------- 16 | 17 | 1.10.14 (2018-02-13) 18 | -------------------- 19 | * Merge pull request `#387 `_ from k-okada/maintain 20 | change maintainer to ROS orphaned package maintainer 21 | * change maintainer to ROS orphaned package maintainer 22 | * Contributors: Kei Okada 23 | 24 | 1.10.13 (2015-02-09) 25 | -------------------- 26 | * Updated maintainership 27 | * Contributors: dash 28 | 29 | 1.10.12 (2015-01-13) 30 | -------------------- 31 | 32 | 1.10.10 (2014-12-16) 33 | -------------------- 34 | 35 | 1.10.9 (2014-12-16) 36 | ------------------- 37 | * Changelogs 38 | * Added changelogs 39 | * Changelogs; maintainership 40 | * Contributors: TheDash 41 | 42 | * Changelogs; maintainership 43 | * Contributors: TheDash 44 | -------------------------------------------------------------------------------- /pr2_controllers_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(pr2_controllers_msgs) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS actionlib_msgs trajectory_msgs geometry_msgs message_generation) 7 | 8 | add_message_files( 9 | DIRECTORY msg 10 | FILES 11 | JointControllerState.msg 12 | JointControllerStateArray.msg 13 | JointTrajectoryControllerState.msg 14 | Pr2GripperCommand.msg 15 | ) 16 | add_service_files( 17 | DIRECTORY srv 18 | FILES QueryCalibrationState.srv QueryTrajectoryState.srv 19 | ) 20 | add_action_files( 21 | DIRECTORY action 22 | FILES 23 | JointTrajectory.action 24 | PointHead.action 25 | Pr2GripperCommand.action 26 | SingleJointPosition.action 27 | ) 28 | ## Generate added messages and services with any dependencies listed here 29 | generate_messages( DEPENDENCIES trajectory_msgs std_msgs geometry_msgs 30 | actionlib_msgs) 31 | 32 | catkin_package( 33 | CATKIN_DEPENDS actionlib_msgs trajectory_msgs geometry_msgs message_runtime 34 | ) 35 | -------------------------------------------------------------------------------- /pr2_head_action/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(pr2_head_action) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS 7 | geometry_msgs 8 | trajectory_msgs 9 | sensor_msgs 10 | pr2_controllers_msgs 11 | roscpp 12 | message_filters 13 | tf 14 | tf_conversions 15 | actionlib 16 | kdl_parser) 17 | 18 | find_package(orocos_kdl REQUIRED) 19 | 20 | find_package(Boost REQUIRED COMPONENTS thread) 21 | include_directories(${Boost_INCLUDE_DIRS} 22 | ${orocos_kdl_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 23 | 24 | catkin_package() 25 | 26 | add_executable(pr2_head_action src/pr2_point_frame.cpp) 27 | target_link_libraries(pr2_head_action ${Boost_LIBRARIES} 28 | ${orocos_kdl_LIBRARIES} ${catkin_LIBRARIES}) 29 | add_dependencies(pr2_head_action ${catkin_EXPORTED_TARGETS}) 30 | 31 | install(TARGETS pr2_head_action 32 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 33 | 34 | install(FILES head_action.launch 35 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 36 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /pr2_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pr2_controllers 3 | 1.10.18 4 | Contains the controllers that run in realtime on the PR2 and supporting packages. 5 | ROS Orphaned Package Maintainers 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/pr2_controllers 10 | 11 | 12 | Stuart Glaser sglaser@willowgarage.com 13 | 14 | catkin 15 | 16 | 17 | pr2_gripper_action 18 | pr2_calibration_controllers 19 | joint_trajectory_action 20 | ethercat_trigger_controllers 21 | robot_mechanism_controllers 22 | pr2_mechanism_controllers 23 | single_joint_position_action 24 | pr2_controllers_msgs 25 | pr2_head_action 26 | control_toolbox 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /pr2_gripper_action/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pr2_gripper_action 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | 8 | 1.10.17 (2019-08-08) 9 | -------------------- 10 | 11 | 1.10.16 (2019-07-26) 12 | -------------------- 13 | * Make sure to include the correct boost libraries. 14 | This follows the principle of "include what you use", and 15 | also should in theory fix the problems on the build farm. 16 | (`#394 `_) 17 | Signed-off-by: Chris Lalancette 18 | * Contributors: Chris Lalancette 19 | 20 | 1.10.15 (2018-09-13) 21 | -------------------- 22 | 23 | 1.10.14 (2018-02-13) 24 | -------------------- 25 | * Merge pull request `#387 `_ from k-okada/maintain 26 | change maintainer to ROS orphaned package maintainer 27 | * change maintainer to ROS orphaned package maintainer 28 | * Contributors: Kei Okada 29 | 30 | 1.10.13 (2015-02-09) 31 | -------------------- 32 | * Updated maintainership 33 | * Contributors: dash 34 | 35 | 1.10.12 (2015-01-13) 36 | -------------------- 37 | 38 | 1.10.10 (2014-12-16) 39 | -------------------- 40 | 41 | 1.10.9 (2014-12-16) 42 | ------------------- 43 | * Changelogs 44 | * Added changelogs 45 | * Changelogs; maintainership 46 | * Contributors: TheDash 47 | 48 | * Changelogs; maintainership 49 | * Contributors: TheDash 50 | -------------------------------------------------------------------------------- /pr2_gripper_action/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pr2_gripper_action 3 | 1.10.18 4 | The pr2_gripper_action provides an action interface for using the 5 | gripper. Users can specify what position to move to (while limiting the 6 | force) and the action will report success when the position is reached or 7 | failure when the gripper cannot move any longer. 8 | ROS Orphaned Package Maintainers 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/pr2_gripper_action 13 | 14 | 15 | Stuart Glaser 16 | 17 | catkin 18 | 19 | roscpp 20 | actionlib_msgs 21 | actionlib 22 | pr2_mechanism_controllers 23 | robot_mechanism_controllers 24 | pr2_controllers_msgs 25 | pr2_mechanism_model 26 | 27 | roscpp 28 | actionlib_msgs 29 | actionlib 30 | pr2_mechanism_controllers 31 | robot_mechanism_controllers 32 | pr2_controllers_msgs 33 | pr2_mechanism_model 34 | 35 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ethercat_trigger_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | 8 | 1.10.17 (2019-08-08) 9 | -------------------- 10 | 11 | 1.10.16 (2019-07-26) 12 | -------------------- 13 | * Make sure to include the correct boost libraries. 14 | This follows the principle of "include what you use", and 15 | also should in theory fix the problems on the build farm. 16 | (`#393 `_) 17 | Signed-off-by: Chris Lalancette 18 | * Contributors: Chris Lalancette 19 | 20 | 1.10.15 (2018-09-13) 21 | -------------------- 22 | 23 | 1.10.14 (2018-02-13) 24 | -------------------- 25 | * Merge pull request `#387 `_ from k-okada/maintain 26 | change maintainer to ROS orphaned package maintainer 27 | * change maintainer to ROS orphaned package maintainer 28 | * Contributors: Kei Okada 29 | 30 | 1.10.13 (2015-02-09) 31 | -------------------- 32 | * Updated maintainership 33 | * Contributors: dash 34 | 35 | 1.10.12 (2015-01-13) 36 | -------------------- 37 | 38 | 1.10.10 (2014-12-16) 39 | -------------------- 40 | 41 | 1.10.9 (2014-12-16) 42 | ------------------- 43 | * Changelogs 44 | * Added changelogs 45 | * Changelogs; maintainership 46 | * Contributors: TheDash 47 | 48 | * Changelogs; maintainership 49 | * Contributors: TheDash 50 | -------------------------------------------------------------------------------- /single_joint_position_action/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package single_joint_position_action 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | 8 | 1.10.17 (2019-08-08) 9 | -------------------- 10 | 11 | 1.10.16 (2019-07-26) 12 | -------------------- 13 | * Make sure to include the correct boost libraries. 14 | This follows the principle of "include what you use", and 15 | also should in theory fix the problems on the build farm. 16 | (`#394 `_) 17 | Signed-off-by: Chris Lalancette 18 | * Contributors: Chris Lalancette 19 | 20 | 1.10.15 (2018-09-13) 21 | -------------------- 22 | 23 | 1.10.14 (2018-02-13) 24 | -------------------- 25 | * Merge pull request `#387 `_ from k-okada/maintain 26 | change maintainer to ROS orphaned package maintainer 27 | * change maintainer to ROS orphaned package maintainer 28 | * Contributors: Kei Okada 29 | 30 | 1.10.13 (2015-02-09) 31 | -------------------- 32 | * Updated maintainership 33 | * Contributors: dash 34 | 35 | 1.10.12 (2015-01-13) 36 | -------------------- 37 | 38 | 1.10.10 (2014-12-16) 39 | -------------------- 40 | 41 | 1.10.9 (2014-12-16) 42 | ------------------- 43 | * Changelogs 44 | * Added changelogs 45 | * Changelogs; maintainership 46 | * Contributors: TheDash 47 | 48 | * Changelogs; maintainership 49 | * Contributors: TheDash 50 | -------------------------------------------------------------------------------- /joint_trajectory_action/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package joint_trajectory_action 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | 8 | 1.10.17 (2019-08-08) 9 | -------------------- 10 | 11 | 1.10.16 (2019-07-26) 12 | -------------------- 13 | * Make sure to include the correct boost libraries. 14 | This follows the principle of "include what you use", and 15 | also should in theory fix the problems on the build farm. 16 | (`#394 `_) 17 | Signed-off-by: Chris Lalancette 18 | * Contributors: Chris Lalancette 19 | 20 | 1.10.15 (2018-09-13) 21 | -------------------- 22 | 23 | 1.10.14 (2018-02-13) 24 | -------------------- 25 | * Merge pull request `#387 `_ from k-okada/maintain 26 | change maintainer to ROS orphaned package maintainer 27 | * change maintainer to ROS orphaned package maintainer 28 | * Contributors: Kei Okada 29 | 30 | 1.10.13 (2015-02-09) 31 | -------------------- 32 | 33 | 1.10.12 (2015-01-13) 34 | -------------------- 35 | 36 | 1.10.10 (2014-12-16) 37 | -------------------- 38 | 39 | 1.10.9 (2014-12-16) 40 | ------------------- 41 | * Changelogs 42 | * Added changelogs 43 | * Changelogs; maintainership 44 | * Updated maintainership 45 | * Contributors: TheDash 46 | 47 | * Changelogs; maintainership 48 | * Updated maintainership 49 | * Contributors: TheDash 50 | 51 | * Updated maintainership 52 | * Contributors: TheDash 53 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/scripts/control_laser.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PKG = "pr2_mechanism_controllers" 4 | 5 | import roslib; roslib.load_manifest(PKG) 6 | 7 | import sys 8 | import os 9 | import string 10 | 11 | import rospy 12 | from std_msgs import * 13 | 14 | from pr2_mechanism_controllers.srv import * 15 | 16 | def print_usage(exit_code = 0): 17 | print('''Usage: 18 | control.py sine 19 | - Defines a sine sweep for the laser controller 20 | ''') 21 | sys.exit(exit_code) 22 | 23 | if __name__ == '__main__': 24 | if len(sys.argv) < 6: 25 | print_usage() 26 | if sys.argv[2] == 'sine' : 27 | profile_type = 4 # Implies a sine sweep 28 | controller = sys.argv[1] 29 | period = float (sys.argv[3]) 30 | amplitude = float (sys.argv[4]) 31 | offset = float (sys.argv[5]) 32 | 33 | print('Sending Command: ') 34 | print(' Profile Type: Sine') 35 | print(' Period: %f Seconds' % period) 36 | print(' Amplitude: %f Radians' % amplitude) 37 | print(' Offset: %f Radians' % offset) 38 | 39 | rospy.wait_for_service(controller + '/set_profile') 40 | s = rospy.ServiceProxy(controller + '/set_profile', SetProfile) 41 | resp = s.call(SetProfileRequest(0.0, 0.0, 0.0, 0.0, profile_type, period, amplitude, offset)) 42 | 43 | print('Command Sent') 44 | print(' Response: %s' % str(resp.time)) 45 | else : 46 | print('Unknown profile type [%s]' % sys.argv[2]) 47 | 48 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pr2_calibration_controllers 3 | 1.10.18 4 | The pr2_calibration_controllers package contains the controllers 5 | used to bring all the joints in the PR2 to a calibrated state. 6 | ROS Orphaned Package Maintainers 7 | 8 | BSD 9 | 10 | http://ros.org/wiki/pr2_calibration_controllers 11 | 12 | 13 | Stuart Glaser 14 | 15 | catkin 16 | 17 | std_msgs 18 | pr2_controller_interface 19 | roscpp 20 | pr2_mechanism_model 21 | realtime_tools 22 | robot_mechanism_controllers 23 | pr2_mechanism_controllers 24 | pluginlib 25 | 26 | std_msgs 27 | pr2_controller_interface 28 | roscpp 29 | pr2_mechanism_model 30 | realtime_tools 31 | robot_mechanism_controllers 32 | pr2_mechanism_controllers 33 | pluginlib 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ethercat_trigger_controllers 3 | 1.10.18 4 | Controllers to operate the digital output of the motor controller 5 | boards and the projector board. This package has not been reviewed and 6 | should be considered unstable. 7 | ROS Orphaned Package Maintainers 8 | 9 | BSD 10 | 11 | http://www.ros.org/wiki/ethercat_trigger_controllers 12 | 13 | 14 | Blaise Gassend 15 | 16 | catkin 17 | 18 | pr2_controller_interface 19 | realtime_tools 20 | roscpp 21 | diagnostic_msgs 22 | pluginlib 23 | std_msgs 24 | message_generation 25 | libtool 26 | 27 | rospy 28 | pr2_controller_interface 29 | realtime_tools 30 | roscpp 31 | diagnostic_msgs 32 | pluginlib 33 | message_runtime 34 | std_msgs 35 | libtool 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /pr2_head_action/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pr2_head_action 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | * Fix for noetic (`#401 `_) 8 | 9 | * install liborocos for noetic 10 | * conver to package 3 11 | 12 | * Contributors: Kei Okada 13 | 14 | 1.10.17 (2019-08-08) 15 | -------------------- 16 | 17 | 1.10.16 (2019-07-26) 18 | -------------------- 19 | * Make sure to include the correct boost libraries. 20 | This follows the principle of "include what you use", and 21 | also should in theory fix the problems on the build farm. 22 | (`#394 `_) 23 | Signed-off-by: Chris Lalancette 24 | * Contributors: Chris Lalancette 25 | 26 | 1.10.15 (2018-09-13) 27 | -------------------- 28 | 29 | 1.10.14 (2018-02-13) 30 | -------------------- 31 | * Merge pull request `#387 `_ from k-okada/maintain 32 | change maintainer to ROS orphaned package maintainer 33 | * change maintainer to ROS orphaned package maintainer 34 | * Contributors: Kei Okada 35 | 36 | 1.10.13 (2015-02-09) 37 | -------------------- 38 | * Updated maintainership 39 | * Contributors: dash 40 | 41 | 1.10.12 (2015-01-13) 42 | -------------------- 43 | 44 | 1.10.10 (2014-12-16) 45 | -------------------- 46 | 47 | 1.10.9 (2014-12-16) 48 | ------------------- 49 | * Changelogs 50 | * Added changelogs 51 | * Changelogs; maintainership 52 | * Contributors: TheDash 53 | 54 | * Changelogs; maintainership 55 | * Contributors: TheDash 56 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pr2_calibration_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | 8 | 1.10.17 (2019-08-08) 9 | -------------------- 10 | 11 | 1.10.16 (2019-07-26) 12 | -------------------- 13 | * Make sure to include the correct boost libraries. 14 | This follows the principle of "include what you use", and 15 | also should in theory fix the problems on the build farm. 16 | (`#394 `_) 17 | Signed-off-by: Chris Lalancette 18 | * Contributors: Chris Lalancette 19 | 20 | 1.10.15 (2018-09-13) 21 | -------------------- 22 | 23 | 1.10.14 (2018-02-13) 24 | -------------------- 25 | * Merge pull request `#387 `_ from k-okada/maintain 26 | change maintainer to ROS orphaned package maintainer 27 | * Merge pull request `#369 `_ from muratsevim/hydro-devel 28 | std namespace prefix is added to isnan calls 29 | * change maintainer to ROS orphaned package maintainer 30 | * std namespace prefix is added to isnan calls 31 | * Contributors: Kei Okada, Mehmet Murat Sevim 32 | 33 | 1.10.13 (2015-02-09) 34 | -------------------- 35 | * Updated maintainership 36 | * Contributors: dash 37 | 38 | 1.10.12 (2015-01-13) 39 | -------------------- 40 | 41 | 1.10.10 (2014-12-16) 42 | -------------------- 43 | 44 | 1.10.9 (2014-12-16) 45 | ------------------- 46 | * Changelogs 47 | * Added changelogs 48 | * Changelogs; maintainership 49 | * Contributors: TheDash 50 | 51 | * Changelogs; maintainership 52 | * Contributors: TheDash 53 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(pr2_calibration_controllers) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS 7 | std_msgs 8 | pr2_controller_interface 9 | roscpp 10 | pr2_mechanism_model 11 | realtime_tools 12 | robot_mechanism_controllers 13 | pr2_mechanism_controllers 14 | pluginlib) 15 | 16 | include_directories(include ${catkin_INCLUDE_DIRS}) 17 | 18 | catkin_package( 19 | CATKIN_DEPENDS 20 | std_msgs 21 | pr2_controller_interface 22 | roscpp 23 | pr2_mechanism_model 24 | realtime_tools 25 | robot_mechanism_controllers 26 | pr2_mechanism_controllers 27 | pluginlib 28 | INCLUDE_DIRS include 29 | LIBRARIES ${PROJECT_NAME} 30 | ) 31 | 32 | add_library(${PROJECT_NAME} 33 | src/caster_calibration_controller.cpp 34 | src/gripper_calibration_controller.cpp 35 | src/joint_calibration_controller.cpp 36 | src/wrist_calibration_controller.cpp 37 | src/fake_calibration_controller.cpp 38 | ) 39 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 40 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 41 | pr2_enable_rpath(${PROJECT_NAME}) 42 | 43 | install(TARGETS ${PROJECT_NAME} 44 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 45 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 47 | 48 | install(DIRECTORY include/${PROJECT_NAME}/ 49 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 50 | 51 | install(FILES controller_plugins.xml 52 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 53 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/scripts/send_periodic_cmd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PKG = "pr2_mechanism_controllers" 4 | 5 | import roslib; roslib.load_manifest(PKG) 6 | 7 | import sys 8 | import os 9 | import string 10 | 11 | import rospy 12 | from std_msgs import * 13 | 14 | from pr2_msgs.msg import PeriodicCmd 15 | from time import sleep 16 | 17 | def print_usage(exit_code = 0): 18 | print('''Usage: 19 | send_periodic_cmd.py [controller] [profile] [period] [amplitude] [offset] 20 | - [profile] - Possible options are linear or linear_blended 21 | - [period] - Time for one entire cycle to execute (in seconds) 22 | - [amplitude] - Distance max value to min value of profile (In radians for laser_tilt controller) 23 | - [offset] - Constant cmd to add to profile (offset=0 results in profile centered around 0) 24 | ''') 25 | sys.exit(exit_code) 26 | 27 | if __name__ == '__main__': 28 | rospy.init_node('periodic_cmd_commander', sys.argv, anonymous=True) 29 | 30 | if len(sys.argv) != 6: 31 | print_usage() 32 | 33 | cmd = PeriodicCmd() 34 | controller = sys.argv[1] 35 | cmd.header = rospy.Header(None, None, None) 36 | cmd.profile = sys.argv[2] 37 | cmd.period = float (sys.argv[3]) 38 | cmd.amplitude = float (sys.argv[4]) 39 | cmd.offset = float (sys.argv[5]) 40 | 41 | print('Sending Command to %s: ' % controller) 42 | print(' Profile Type: %s' % cmd.profile) 43 | print(' Period: %f Seconds' % cmd.period) 44 | print(' Amplitude: %f Radians' % cmd.amplitude) 45 | print(' Offset: %f Radians' % cmd.offset) 46 | 47 | command_publisher = rospy.Publisher(controller + '/set_periodic_cmd', PeriodicCmd) 48 | sleep(1) 49 | command_publisher.publish( cmd ) 50 | 51 | sleep(1) 52 | 53 | print('Command sent!') 54 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(ethercat_trigger_controllers) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS pr2_controller_interface realtime_tools roscpp diagnostic_msgs pluginlib message_generation std_msgs) 7 | 8 | add_message_files( 9 | DIRECTORY msg 10 | FILES MultiWaveform.msg MultiWaveformTransition.msg 11 | ) 12 | add_service_files( 13 | DIRECTORY srv 14 | FILES SetMultiWaveform.srv SetWaveform.srv 15 | ) 16 | set(_srcs 17 | src/trigger_controller.cpp 18 | src/multi_trigger_controller.cpp 19 | src/projector_controller.cpp 20 | ) 21 | 22 | include_directories(include ${catkin_INCLUDE_DIRS}) 23 | 24 | generate_messages(DEPENDENCIES std_msgs) 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS pr2_controller_interface realtime_tools roscpp diagnostic_msgs pluginlib message_runtime std_msgs 28 | INCLUDE_DIRS include 29 | LIBRARIES ethercat_trigger_controllers 30 | ) 31 | 32 | add_definitions(-O3) 33 | 34 | add_library(ethercat_trigger_controllers ${_srcs}) 35 | target_link_libraries(ethercat_trigger_controllers ltdl ${catkin_LIBRARIES}) 36 | add_dependencies(ethercat_trigger_controllers ${ethercat_trigger_controllers_EXPORTED_TARGETS}) 37 | pr2_enable_rpath(ethercat_trigger_controllers) 38 | 39 | install(TARGETS ethercat_trigger_controllers 40 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 41 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 43 | 44 | install(DIRECTORY include/${PROJECT_NAME}/ 45 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 46 | 47 | install(FILES controller_plugins.xml 48 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 49 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/scripts/send_laser_traj_cmd_ms2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PKG = "pr2_mechanism_controllers" 4 | 5 | import roslib; roslib.load_manifest(PKG) 6 | 7 | import sys 8 | import os 9 | import string 10 | 11 | import rospy 12 | from std_msgs import * 13 | 14 | from pr2_msgs.msg import LaserTrajCmd 15 | from pr2_msgs.srv import * 16 | from time import sleep 17 | 18 | def print_usage(exit_code = 0): 19 | print('''Usage: 20 | send_periodic_cmd.py [controller] 21 | ''') 22 | sys.exit(exit_code) 23 | 24 | if __name__ == '__main__': 25 | if len(sys.argv) < 2: 26 | print_usage() 27 | 28 | cmd = LaserTrajCmd() 29 | controller = sys.argv[1] 30 | cmd.header = rospy.Header(None, None, None) 31 | cmd.profile = "blended_linear" 32 | #cmd.pos = [1.0, .26, -.26, -.7, -.7, -.26, .26, 1.0, 1.0] 33 | d = .025 34 | #cmd.time = [0.0, 0.4, 1.0, 1.1, 1.1+d, 1.2+d, 1.8+d, 2.2+d, 2.2+2*d] 35 | 36 | cmd.position = [1.2, -.7, 1.2] 37 | cmd.time_from_start = [0.0, 1.8, 2.025] 38 | cmd.time_from_start = [rospy.Duration.from_sec(x) for x in cmd.time_from_start] 39 | 40 | cmd.max_velocity = 5 41 | cmd.max_acceleration = 5 42 | 43 | print('Sending Command to %s: ' % controller) 44 | print(' Profile Type: %s' % cmd.profile) 45 | print(' Pos: %s ' % ','.join(['%.3f' % x for x in cmd.position])) 46 | print(' Time: %s' % ','.join(['%.3f' % x.to_sec() for x in cmd.time_from_start])) 47 | print(' MaxVelocity: %f' % cmd.max_velocity) 48 | print(' MaxAcceleration: %f' % cmd.max_acceleration) 49 | 50 | rospy.wait_for_service(controller + '/set_traj_cmd') 51 | s = rospy.ServiceProxy(controller + '/set_traj_cmd', SetLaserTrajCmd) 52 | resp = s.call(SetLaserTrajCmdRequest(cmd)) 53 | 54 | print('Command sent!') 55 | print(' Resposne: %f' % resp.start_time.to_seconds()) 56 | -------------------------------------------------------------------------------- /pr2_head_action/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pr2_head_action 3 | 1.10.18 4 | The PR2 head action is a node that provides an action interface for 5 | pointing the head of the PR2. It passes trajectory goals to the 6 | controller, and reports success when they have finished executing. 7 | ROS Orphaned Package Maintainers 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/pr2_head_action 12 | 13 | 14 | Stuart Glaser 15 | 16 | catkin 17 | 18 | geometry_msgs 19 | trajectory_msgs 20 | sensor_msgs 21 | pr2_controllers_msgs 22 | roscpp 23 | message_filters 24 | tf 25 | tf_conversions 26 | actionlib 27 | orocos_kdl 28 | liborocos-kdl-dev 29 | kdl_parser 30 | 31 | geometry_msgs 32 | trajectory_msgs 33 | sensor_msgs 34 | pr2_controllers_msgs 35 | roscpp 36 | message_filters 37 | tf 38 | tf_conversions 39 | actionlib 40 | orocos_kdl 41 | liborocos-kdl 42 | kdl_parser 43 | 44 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/scripts/send_periodic_cmd_srv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PKG = "pr2_mechanism_controllers" 4 | 5 | import roslib; roslib.load_manifest(PKG) 6 | 7 | import sys 8 | import os 9 | import string 10 | 11 | import rospy 12 | from std_msgs import * 13 | 14 | from pr2_msgs.msg import PeriodicCmd 15 | from pr2_msgs.srv import * 16 | from time import sleep 17 | 18 | def print_usage(exit_code = 0): 19 | print('''Usage: 20 | send_periodic_cmd.py [controller] [profile] [period] [amplitude] [offset] 21 | - [profile] - Possible options are linear or linear_blended 22 | - [period] - Time for one entire cycle to execute (in seconds) 23 | - [amplitude] - Distance max value to min value of profile (In radians for laser_tilt controller) 24 | - [offset] - Constant cmd to add to profile (offset=0 results in profile centered around 0) 25 | ''') 26 | sys.exit(exit_code) 27 | 28 | if __name__ == '__main__': 29 | if len(sys.argv) < 6: 30 | print_usage() 31 | 32 | cmd = PeriodicCmd() 33 | controller = sys.argv[1] 34 | cmd.header = rospy.Header(None, None, None) 35 | cmd.profile = sys.argv[2] 36 | cmd.period = float (sys.argv[3]) 37 | cmd.amplitude = float (sys.argv[4]) 38 | cmd.offset = float (sys.argv[5]) 39 | 40 | print('Sending Command to %s: ' % controller) 41 | print(' Profile Type: %s' % cmd.profile) 42 | print(' Period: %f Seconds' % cmd.period) 43 | print(' Amplitude: %f Radians' % cmd.amplitude) 44 | print(' Offset: %f Radians' % cmd.offset) 45 | 46 | rospy.wait_for_service(controller + '/set_periodic_cmd') 47 | s = rospy.ServiceProxy(controller + '/set_periodic_cmd', SetPeriodicCmd) 48 | resp = s.call(SetPeriodicCmdRequest(cmd)) 49 | 50 | #rospy.init_node('periodic_cmd_commander', anonymous=True) 51 | #sleep(1) 52 | #command_publisher.publish( cmd ) 53 | 54 | #sleep(1) 55 | 56 | print('Command sent!') 57 | print(' Response: %f' % resp.start_time.to_sec()) 58 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | 2 | # this is .traivs.yml written by - 3 | 4 | # https://github.com/ros-infrastructure/ros_buildfarm/blob/master/doc/jobs/devel_jobs.rst 5 | # https://github.com/ros-infrastructure/ros_buildfarm/blob/master/doc/jobs/prerelease_jobs.rst 6 | # while this doesn't require sudo we don't want to run within a Docker container 7 | sudo: true 8 | dist: bionic 9 | language: python 10 | env: 11 | global: 12 | - JOB_PATH=/tmp/devel_job 13 | - ABORT_ON_TEST_FAILURE=1 14 | - INDEX_URL=https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml 15 | matrix: 16 | - CHECK_PYTHON3_COMPILE=true 17 | - ROS_DISTRO_NAME=indigo OS_NAME=ubuntu OS_CODE_NAME=trusty ARCH=amd64 INDEX_URL=https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/6a93d17/index.yaml 18 | - ROS_DISTRO_NAME=kinetic OS_NAME=ubuntu OS_CODE_NAME=xenial ARCH=amd64 19 | - ROS_DISTRO_NAME=melodic OS_NAME=ubuntu OS_CODE_NAME=bionic ARCH=amd64 20 | - ROS_DISTRO_NAME=noetic OS_NAME=ubuntu OS_CODE_NAME=focal ARCH=amd64 21 | # matrix: 22 | # allow_failures: 23 | # - env: ROS_DISTRO_NAME=indigo OS_NAME=ubuntu OS_CODE_NAME=trusty ARCH=amd64 24 | install: 25 | # check python3 compatibility 26 | - if [ "${CHECK_PYTHON3_COMPILE}" == "true" ]; then python3 -m compileall -x asmach .; exit $?; fi 27 | # either install the latest released version of ros_buildfarm 28 | - pip install ros_buildfarm 29 | # or checkout a specific branch 30 | #- git clone -b master https://github.com/ros-infrastructure/ros_buildfarm /tmp/ros_buildfarm 31 | #- pip install /tmp/ros_buildfarm 32 | # checkout catkin for catkin_test_results script 33 | - git clone https://github.com/ros/catkin /tmp/catkin 34 | # run devel job for a ROS repository with the same name as this repo 35 | - export REPOSITORY_NAME=`basename $TRAVIS_BUILD_DIR` 36 | # use the code already checked out by Travis 37 | - mkdir -p $JOB_PATH/ws/src 38 | - cp -R $TRAVIS_BUILD_DIR $JOB_PATH/ws/src/ 39 | # generate the script to run a pre-release job for that target and repo 40 | - generate_prerelease_script.py $INDEX_URL $ROS_DISTRO_NAME default $OS_NAME $OS_CODE_NAME $ARCH --output-dir $JOB_PATH 41 | # run the actual job which involves Docker 42 | - cd $JOB_PATH; sh ./prerelease.sh -y 43 | script: 44 | # get summary of test results 45 | - /tmp/catkin/bin/catkin_test_results $JOB_PATH/ws/test_results --all 46 | notifications: 47 | email: false 48 | 49 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pr2_mechanism_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | * 2to3 -w -f print . (`#401 `_) 8 | * Contributors: Kei Okada 9 | 10 | 1.10.17 (2019-08-08) 11 | -------------------- 12 | 13 | 1.10.16 (2019-07-26) 14 | -------------------- 15 | * Make sure to include the correct boost libraries. 16 | This follows the principle of "include what you use", and 17 | also should in theory fix the problems on the build farm. 18 | (`#394 `_) 19 | Signed-off-by: Chris Lalancette 20 | * Contributors: Chris Lalancette 21 | 22 | 1.10.15 (2018-09-13) 23 | -------------------- 24 | * Merge pull request `#390 `_ from k-okada/add_travis 25 | update travis.yml 26 | * add fix for urdfmodel 1.0.0(melodic), 27 | since 12.04 have urdfmodel < 1.0.0, it will fail to compile on indigo, so we need to chaeck URDFDOM_version 28 | * Contributors: Kei Okada 29 | 30 | 1.10.14 (2018-02-13) 31 | -------------------- 32 | * Merge pull request `#389 `_ from k-okada/kinetic-devel 33 | - use Eigen3 instead of Eigen 34 | * use Eigen3 instead of Eigen 35 | * Merge pull request `#388 `_ from k-okada/add_missing_dep 36 | add missing dependency 37 | * Merge pull request `#387 `_ from k-okada/maintain 38 | change maintainer to ROS orphaned package maintainer 39 | * Merge pull request `#369 `_ from muratsevim/hydro-devel 40 | std namespace prefix is added to isnan calls 41 | * mechanism_controllers: add missing pr2_msgs dependency 42 | * change maintainer to ROS orphaned package maintainer 43 | * std namespace prefix is added to isnan calls 44 | * Contributors: Kei Okada, Mehmet Murat Sevim, Michael Görner 45 | 46 | 1.10.13 (2015-02-09) 47 | -------------------- 48 | * Updated maintainership 49 | * Contributors: dash 50 | 51 | 1.10.12 (2015-01-13) 52 | -------------------- 53 | 54 | 1.10.10 (2014-12-16) 55 | -------------------- 56 | 57 | 1.10.9 (2014-12-16) 58 | ------------------- 59 | * Changelogs 60 | * Added changelogs 61 | * Changelogs; maintainership 62 | * Contributors: TheDash 63 | 64 | * Changelogs; maintainership 65 | * Contributors: TheDash 66 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | control_toolbox/src/control_toolbox 2 | control_toolbox/srv/cpp 3 | control_toolbox/srv/java 4 | control_toolbox/srv/lisp 5 | control_toolbox/srv/oct 6 | joint_trajectory_action/bin 7 | pr2_controllers_msgs/msg/cpp 8 | pr2_controllers_msgs/msg/java 9 | pr2_controllers_msgs/msg/lisp 10 | pr2_controllers_msgs/msg/oct 11 | pr2_controllers_msgs/src 12 | pr2_controllers_msgs/srv/cpp 13 | pr2_controllers_msgs/srv/java 14 | pr2_controllers_msgs/srv/lisp 15 | pr2_controllers_msgs/srv/oct 16 | pr2_gripper_action/bin 17 | pr2_head_action/bin 18 | pr2_mechanism_controllers/msg/cpp 19 | pr2_mechanism_controllers/msg/java 20 | pr2_mechanism_controllers/msg/lisp 21 | pr2_mechanism_controllers/msg/oct 22 | pr2_mechanism_controllers/src/pr2_mechanism_controllers 23 | pr2_mechanism_controllers/srv/cpp 24 | pr2_mechanism_controllers/srv/java 25 | pr2_mechanism_controllers/srv/lisp 26 | pr2_mechanism_controllers/srv/oct 27 | single_joint_position_action/bin 28 | trajectory_msgs/msg/cpp 29 | trajectory_msgs/msg/java 30 | trajectory_msgs/msg/lisp 31 | trajectory_msgs/msg/oct 32 | trajectory_msgs/src 33 | 34 | pr2_controllers_msgs/msg/JointTrajectoryAction.msg 35 | pr2_controllers_msgs/msg/JointTrajectoryActionFeedback.msg 36 | pr2_controllers_msgs/msg/JointTrajectoryActionGoal.msg 37 | pr2_controllers_msgs/msg/JointTrajectoryActionResult.msg 38 | pr2_controllers_msgs/msg/JointTrajectoryFeedback.msg 39 | pr2_controllers_msgs/msg/JointTrajectoryGoal.msg 40 | pr2_controllers_msgs/msg/JointTrajectoryResult.msg 41 | pr2_controllers_msgs/msg/PointHeadAction.msg 42 | pr2_controllers_msgs/msg/PointHeadActionFeedback.msg 43 | pr2_controllers_msgs/msg/PointHeadActionGoal.msg 44 | pr2_controllers_msgs/msg/PointHeadActionResult.msg 45 | pr2_controllers_msgs/msg/PointHeadFeedback.msg 46 | pr2_controllers_msgs/msg/PointHeadGoal.msg 47 | pr2_controllers_msgs/msg/PointHeadResult.msg 48 | pr2_controllers_msgs/msg/Pr2GripperCommandAction.msg 49 | pr2_controllers_msgs/msg/Pr2GripperCommandActionFeedback.msg 50 | pr2_controllers_msgs/msg/Pr2GripperCommandActionGoal.msg 51 | pr2_controllers_msgs/msg/Pr2GripperCommandActionResult.msg 52 | pr2_controllers_msgs/msg/Pr2GripperCommandFeedback.msg 53 | pr2_controllers_msgs/msg/Pr2GripperCommandGoal.msg 54 | pr2_controllers_msgs/msg/Pr2GripperCommandResult.msg 55 | pr2_controllers_msgs/msg/SingleJointPositionAction.msg 56 | pr2_controllers_msgs/msg/SingleJointPositionActionFeedback.msg 57 | pr2_controllers_msgs/msg/SingleJointPositionActionGoal.msg 58 | pr2_controllers_msgs/msg/SingleJointPositionActionResult.msg 59 | pr2_controllers_msgs/msg/SingleJointPositionFeedback.msg 60 | pr2_controllers_msgs/msg/SingleJointPositionGoal.msg 61 | pr2_controllers_msgs/msg/SingleJointPositionResult.msg 62 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2008, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Vijay Pradeep 30 | # (Derived from pointhead.py) 31 | 32 | PKG = "pr2_mechanism_controllers" 33 | 34 | import roslib; roslib.load_manifest(PKG) 35 | 36 | import sys 37 | import os 38 | import string 39 | from time import sleep 40 | 41 | import rospy 42 | from geometry_msgs.msg import PointStamped, Point 43 | 44 | 45 | def control_base_pose_odom_frame(x,y,w): 46 | head_angles = rospy.Publisher('ground_truth_controller/set_cmd', Point) 47 | rospy.init_node('base_position_commander', anonymous=True) 48 | p = Point(x, y, w) ; 49 | sleep(1) 50 | head_angles.publish( p ) 51 | sleep(1) 52 | 53 | def usage(): 54 | return "%s [pos_x] [pos_y] [theta (rad)]"%sys.argv[0] 55 | 56 | if __name__ == "__main__": 57 | 58 | if len(sys.argv) ==4: 59 | control_base_pose_odom_frame(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]) ) 60 | else: 61 | print(usage()) 62 | sys.exit(1) 63 | 64 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/scripts/control_base_position.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2008, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Vijay Pradeep 30 | # (Derived from pointhead.py) 31 | 32 | PKG = "pr2_mechanism_controllers" 33 | 34 | import roslib; roslib.load_manifest(PKG) 35 | 36 | import sys 37 | import os 38 | import string 39 | from time import sleep 40 | 41 | import rospy 42 | from geometry_msgs.msg import PointStamped, Point 43 | 44 | 45 | def control_base_pose_odom_frame(x,y,w): 46 | head_angles = rospy.Publisher('base_position_controller/set_pose_odom_frame_command', Point) 47 | rospy.init_node('base_position_commander', anonymous=True) 48 | p = Point(x, y, w) ; 49 | sleep(1) 50 | head_angles.publish( p ) 51 | sleep(1) 52 | 53 | def usage(): 54 | return "%s [pos_x] [pos_y] [theta (rad)]"%sys.argv[0] 55 | 56 | if __name__ == "__main__": 57 | 58 | if len(sys.argv) ==4: 59 | control_base_pose_odom_frame(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]) ) 60 | else: 61 | print(usage()) 62 | sys.exit(1) 63 | 64 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package robot_mechanism_controllers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.10.18 (2021-05-08) 6 | -------------------- 7 | * Fix for noetic, 2to3 -w -f print (`#401 `_) 8 | * use install(PROGRAMS scripts/effort.py posture.py instaed of install(FILES (`#398 `_) 9 | * add effort values in joint_trajectory_action_controller/state (`#397 `_) 10 | * do not find deprecated boost signals package, it was not used either (`#395 `_) 11 | * add missing / to constraints parameter, This never worked before... 12 | * Contributors: Kei Okada, Michael Görner, Shingo Kitagawa 13 | 14 | 1.10.17 (2019-08-08) 15 | -------------------- 16 | * Remove actionlib typedefs defined in actionlib 1.12.0 (`#396 `_) 17 | * check actinlib_VERSION to removing re-typedef ResultPtr and FeedbackPtr https://github.com/ros/actionlib/issues/106 18 | * Remove actionlib typedefs defined upstream 19 | Signed-off-by: Shane Loretz 20 | * Contributors: Kei Okada, Shane Loretz 21 | 22 | 1.10.16 (2019-07-26) 23 | -------------------- 24 | * Make sure to include the correct boost libraries. 25 | This follows the principle of "include what you use", and 26 | also should in theory fix the problems on the build farm. 27 | (`#394 `_) 28 | Signed-off-by: Chris Lalancette 29 | * Contributors: Chris Lalancette 30 | 31 | 1.10.15 (2018-09-13) 32 | -------------------- 33 | 34 | 1.10.14 (2018-02-13) 35 | -------------------- 36 | * Merge pull request `#388 `_ from k-okada/add_missing_dep 37 | add missing dependency 38 | * Merge pull request `#387 `_ from k-okada/maintain 39 | change maintainer to ROS orphaned package maintainer 40 | * robot_mechanism_controllers: add missing deps 41 | * change maintainer to ROS orphaned package maintainer 42 | * Contributors: Furushchev, Kei Okada 43 | 44 | 1.10.13 (2015-02-09) 45 | -------------------- 46 | * Updated maintainership 47 | * Contributors: dash 48 | 49 | 1.10.12 (2015-01-13) 50 | -------------------- 51 | 52 | 1.10.10 (2014-12-16) 53 | -------------------- 54 | 55 | 1.10.9 (2014-12-16) 56 | ------------------- 57 | * Changelogs 58 | * Added changelogs 59 | * Changelogs; maintainership 60 | * Contributors: TheDash 61 | 62 | * Changelogs; maintainership 63 | * Contributors: TheDash 64 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | pr2_mechanism_controllers 3 | 1.10.18 4 | The pr2_mechanism_controllers package contains realtime 5 | controllers that are meant for specific mechanisms of the PR2. 6 | ROS Orphaned Package Maintainers 7 | 8 | BSD 9 | 10 | http://ros.org/pr2_mechanism_controllers 11 | 12 | 13 | Sachin Chita 14 | John Hsu 15 | Melonee Wise 16 | 17 | catkin 18 | 19 | std_msgs 20 | geometry_msgs 21 | pr2_mechanism_msgs 22 | nav_msgs 23 | pr2_msgs 24 | visualization_msgs 25 | diagnostic_msgs 26 | pr2_controller_interface 27 | roscpp 28 | pr2_controllers_msgs 29 | pr2_mechanism_model 30 | realtime_tools 31 | robot_mechanism_controllers 32 | rospy 33 | pluginlib 34 | rosconsole 35 | tf 36 | angles 37 | control_toolbox 38 | filters 39 | diagnostic_updater 40 | message_generation 41 | 42 | std_msgs 43 | geometry_msgs 44 | pr2_mechanism_msgs 45 | nav_msgs 46 | pr2_msgs 47 | visualization_msgs 48 | diagnostic_msgs 49 | pr2_controller_interface 50 | roscpp 51 | pr2_controllers_msgs 52 | pr2_mechanism_model 53 | realtime_tools 54 | robot_mechanism_controllers 55 | rospy 56 | pluginlib 57 | rosconsole 58 | tf 59 | angles 60 | control_toolbox 61 | filters 62 | diagnostic_updater 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/posture.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2009, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import time 30 | 31 | import roslib 32 | roslib.load_manifest('robot_mechanism_controllers') 33 | import rospy 34 | from std_msgs.msg import * 35 | 36 | rospy.init_node('posture_publisher', disable_signals=True, anonymous=True) 37 | 38 | controller = rospy.myargv()[1] 39 | posture = rospy.myargv()[2] 40 | 41 | 42 | pub_posture = rospy.Publisher("/%s/command_posture" % controller, Float64MultiArray) 43 | postures = { 44 | 'off': [], 45 | 'mantis': [0, 1, 0, -1, 3.14, -1, 3.14], 46 | 'elbowupr': [-0.79,0,-1.6, 9999, 9999, 9999, 9999], 47 | 'elbowupl': [0.79,0,1.6 , 9999, 9999, 9999, 9999], 48 | 'old_elbowupr': [-0.79,0,-1.6, -0.79,3.14, -0.79,5.49], 49 | 'old_elbowupl': [0.79,0,1.6, -0.79,3.14, -0.79,5.49], 50 | 'elbowdownr': [-0.028262077316910873, 1.2946342642324222, -0.25785640577652386, -1.5498884526859626, -31.278913849571776, -1.0527644894829107, -1.8127318367654268], 51 | 'elbowdownl': [-0.0088195719039858515, 1.2834828245284853, 0.20338442004843196, -1.5565279256852611, -0.096340012666916802, -1.0235018652439782, 1.7990893054129216] 52 | } 53 | 54 | while not rospy.is_shutdown(): 55 | m = Float64MultiArray(data = postures[posture]) 56 | pub_posture.publish(m) 57 | time.sleep(1.0) 58 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(robot_mechanism_controllers) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS roscpp angles filters tf tf_conversions eigen_conversions pluginlib actionlib std_msgs geometry_msgs diagnostic_msgs trajectory_msgs pr2_controllers_msgs pr2_controller_interface pr2_controller_manager kdl_parser pr2_mechanism_model control_toolbox realtime_tools message_filters control_msgs message_generation) 7 | 8 | 9 | add_message_files( 10 | DIRECTORY msg 11 | FILES 12 | JTCartesianControllerState.msg 13 | ) 14 | 15 | find_package(Boost REQUIRED) 16 | include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 17 | 18 | generate_messages(DEPENDENCIES geometry_msgs std_msgs) 19 | 20 | catkin_package( 21 | CATKIN_DEPENDS roscpp angles filters tf tf_conversions eigen_conversions pluginlib actionlib std_msgs geometry_msgs diagnostic_msgs trajectory_msgs pr2_controllers_msgs pr2_controller_interface pr2_controller_manager kdl_parser pr2_mechanism_model control_toolbox realtime_tools message_filters control_msgs message_runtime 22 | INCLUDE_DIRS include 23 | LIBRARIES robot_mechanism_controllers 24 | ) 25 | 26 | string(REPLACE "." ";" VERSION_LIST ${actionlib_VERSION}) 27 | list(GET VERSION_LIST 0 actionlib_VERSION_MAJOR) 28 | list(GET VERSION_LIST 1 actionlib_VERSION_MINOR) 29 | list(GET VERSION_LIST 2 actionlib_VERSION_PATCH) 30 | add_definitions(-O3 -Dactionlib_VERSION=${actionlib_VERSION} -Dactionlib_VERSION_MAJOR=${actionlib_VERSION_MAJOR} -Dactionlib_VERSION_MINOR=${actionlib_VERSION_MINOR} -Dactionlib_VERSION_PATCH=${actionlib_VERSION_PATCH}) 31 | 32 | add_library(robot_mechanism_controllers 33 | src/cartesian_pose_controller.cpp 34 | src/cartesian_twist_controller.cpp 35 | src/cartesian_wrench_controller.cpp 36 | src/joint_effort_controller.cpp 37 | src/joint_position_controller.cpp 38 | src/joint_velocity_controller.cpp 39 | src/joint_group_velocity_controller.cpp 40 | src/joint_spline_trajectory_controller.cpp 41 | src/joint_trajectory_action_controller.cpp 42 | src/jt_cartesian_controller.cpp 43 | ) 44 | target_link_libraries(robot_mechanism_controllers ltdl ${Boost_LIBRARIES} 45 | ${catkin_LIBRARIES}) 46 | add_dependencies(robot_mechanism_controllers 47 | ${robot_mechanism_controllers_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 48 | pr2_enable_rpath(robot_mechanism_controllers) 49 | 50 | install(TARGETS robot_mechanism_controllers 51 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 52 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 54 | 55 | install(DIRECTORY include/${PROJECT_NAME}/ 56 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 57 | 58 | install(FILES controller_plugins.xml 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 60 | 61 | install(PROGRAMS scripts/effort.py posture.py 62 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 63 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | robot_mechanism_controllers 3 | 1.10.18 4 | Generic Mechanism Controller Library 5 | ROS Orphaned Package Maintainers 6 | 7 | BSD 8 | 9 | http://ros.org/wiki/robot_mechanism_controllers 10 | 11 | 12 | John Hsu 13 | Melonee Wise 14 | Stuart Glaser 15 | 16 | catkin 17 | 18 | roscpp 19 | angles 20 | filters 21 | tf 22 | tf_conversions 23 | eigen_conversions 24 | pluginlib 25 | actionlib 26 | std_msgs 27 | geometry_msgs 28 | diagnostic_msgs 29 | trajectory_msgs 30 | pr2_controllers_msgs 31 | pr2_controller_interface 32 | pr2_controller_manager 33 | kdl_parser 34 | pr2_mechanism_model 35 | control_toolbox 36 | realtime_tools 37 | message_filters 38 | control_msgs 39 | libtool 40 | message_generation 41 | 42 | rospy 43 | roscpp 44 | angles 45 | filters 46 | tf 47 | tf_conversions 48 | eigen_conversions 49 | pluginlib 50 | actionlib 51 | std_msgs 52 | geometry_msgs 53 | diagnostic_msgs 54 | trajectory_msgs 55 | pr2_controllers_msgs 56 | pr2_controller_interface 57 | pr2_controller_manager 58 | kdl_parser 59 | pr2_mechanism_model 60 | control_toolbox 61 | realtime_tools 62 | message_filters 63 | control_msgs 64 | libtool 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/test/test_joint_trajectory_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | 38 | static int done = 0; 39 | 40 | void finalize(int donecare) 41 | { 42 | done = 1; 43 | } 44 | 45 | int main( int argc, char** argv ) 46 | { 47 | 48 | /*********** Initialize ROS ****************/ 49 | ros::init(argc,argv); 50 | ros::Node *node = new ros::Node("test_arm_trajectory_controller"); 51 | 52 | signal(SIGINT, finalize); 53 | signal(SIGQUIT, finalize); 54 | signal(SIGTERM, finalize); 55 | 56 | 57 | /*********** Start moving the robot ************/ 58 | manipulation_msgs::JointTraj cmd; 59 | 60 | int num_points = 1; 61 | int num_joints = 3; 62 | 63 | cmd.set_points_size(num_points); 64 | 65 | for(int i=0; iadvertise("base/trajectory_controller/command",1); 74 | sleep(1); 75 | node->publish("base/trajectory_controller/command",cmd); 76 | sleep(4); 77 | 78 | } 79 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/include/pr2_calibration_controllers/fake_calibration_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | #include "pr2_mechanism_model/robot.h" 40 | #include "robot_mechanism_controllers/joint_velocity_controller.h" 41 | #include "realtime_tools/realtime_publisher.h" 42 | #include "std_msgs/Empty.h" 43 | #include "pr2_controllers_msgs/QueryCalibrationState.h" 44 | 45 | namespace controller 46 | { 47 | 48 | class FakeCalibrationController : public pr2_controller_interface::Controller 49 | { 50 | public: 51 | FakeCalibrationController(); 52 | ~FakeCalibrationController(); 53 | 54 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 55 | virtual void starting(); 56 | virtual void update(); 57 | 58 | bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req, pr2_controllers_msgs::QueryCalibrationState::Response& resp); 59 | 60 | 61 | protected: 62 | 63 | ros::NodeHandle node_; 64 | pr2_mechanism_model::RobotState *robot_; 65 | ros::Time last_publish_time_; 66 | ros::ServiceServer is_calibrated_srv_; 67 | boost::scoped_ptr > pub_calibrated_; 68 | 69 | pr2_mechanism_model::JointState *joint_; 70 | }; 71 | 72 | 73 | } 74 | 75 | 76 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/scripts/pointhead.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2008, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Melonee Wise 30 | 31 | PKG = "pr2_mechanism_controllers" 32 | 33 | import roslib; roslib.load_manifest(PKG) 34 | 35 | import sys 36 | import os 37 | import string 38 | from time import sleep 39 | 40 | import rospy 41 | from geometry_msgs.msg import PointStamped, Point 42 | from sensor_msgs.msg import JointState 43 | 44 | def point_head_client(pan, tilt): 45 | 46 | head_angles = rospy.Publisher('head_controller/command', JointState) 47 | rospy.init_node('head_commander', anonymous=True) 48 | sleep(1) 49 | js = JointState() 50 | js.name = ['head_pan_joint', 'head_tilt_joint']; 51 | js.position = [pan,tilt]; 52 | head_angles.publish(js) 53 | sleep(1) 54 | 55 | def point_head_cart_client(x,y,z,frame): 56 | 57 | head_angles = rospy.Publisher('head_controller/point_head', PointStamped) 58 | rospy.init_node('head_commander', anonymous=True) 59 | sleep(1) 60 | head_angles.publish(PointStamped(rospy.Header(None, rospy.get_rostime(), frame), Point(x, y, z))) 61 | sleep(1) 62 | 63 | 64 | def usage(): 65 | return "%s [pan tilt] or [x,y,z,frame]"%sys.argv[0] 66 | 67 | if __name__ == "__main__": 68 | 69 | if len(sys.argv) < 3: 70 | print(usage()) 71 | sys.exit(1) 72 | elif len(sys.argv) ==3: 73 | point_head_client(float(sys.argv[1]), float(sys.argv[2])) 74 | elif len(sys.argv) ==5: 75 | point_head_cart_client(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), sys.argv[4]) 76 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(pr2_mechanism_controllers) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS std_msgs geometry_msgs pr2_mechanism_msgs nav_msgs pr2_msgs visualization_msgs diagnostic_msgs pr2_controller_interface roscpp pr2_controllers_msgs pr2_mechanism_model realtime_tools robot_mechanism_controllers pluginlib rosconsole tf angles control_toolbox filters diagnostic_updater message_generation) 7 | 8 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 9 | 10 | add_message_files( 11 | DIRECTORY msg 12 | FILES 13 | BaseControllerState2.msg 14 | BaseControllerState.msg 15 | BaseOdometryState.msg 16 | DebugInfo.msg 17 | Odometer.msg 18 | OdometryMatrix.msg 19 | TrackLinkCmd.msg 20 | ) 21 | 22 | add_service_files( 23 | DIRECTORY srv 24 | FILES 25 | SetProfile.srv 26 | ) 27 | 28 | find_package(Eigen3 REQUIRED) 29 | find_package(Boost REQUIRED COMPONENTS thread) 30 | 31 | include_directories(include ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} 32 | ${catkin_INCLUDE_DIRS}) 33 | 34 | ## Generate added messages and services with any dependencies listed here 35 | generate_messages( DEPENDENCIES geometry_msgs std_msgs) 36 | 37 | catkin_package( 38 | DEPENDS EIGEN3 39 | CATKIN_DEPENDS std_msgs geometry_msgs pr2_mechanism_msgs nav_msgs pr2_msgs visualization_msgs diagnostic_msgs pr2_controller_interface roscpp pr2_controllers_msgs pr2_mechanism_model realtime_tools robot_mechanism_controllers pluginlib rosconsole tf angles control_toolbox filters diagnostic_updater message_runtime 40 | INCLUDE_DIRS include 41 | LIBRARIES pr2_mechanism_controllers 42 | ) 43 | 44 | add_definitions(-O3) 45 | 46 | add_library( pr2_mechanism_controllers 47 | src/laser_scanner_traj_controller.cpp 48 | src/caster_controller.cpp 49 | src/base_kinematics.cpp 50 | src/pr2_odometry.cpp 51 | src/pr2_base_controller.cpp 52 | src/pr2_base_controller2.cpp 53 | src/pr2_gripper_controller.cpp 54 | src/trajectory.cpp 55 | ) 56 | find_package(PkgConfig) 57 | pkg_check_modules(URDFDOM REQUIRED urdfdom) 58 | message(STATUS "Find URDFDOM ${URDFDOM_VERSION}") 59 | target_compile_definitions(pr2_mechanism_controllers PRIVATE -DURDFDOM_1_0_0_API) 60 | target_link_libraries(pr2_mechanism_controllers ${Boost_LIBRARIES} 61 | ${catkin_LIBRARIES}) 62 | add_dependencies(pr2_mechanism_controllers 63 | ${pr2_mechanism_controllers_EXPORTED_TARGETS} ${pr2_msgs_EXPORTED_TARGETS}) 64 | pr2_enable_rpath(pr2_mechanism_controllers) 65 | 66 | # TODO: tests 67 | 68 | install(TARGETS pr2_mechanism_controllers 69 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 72 | 73 | install(DIRECTORY include/${PROJECT_NAME}/ 74 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 75 | 76 | install(FILES controller_plugins.xml 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 78 | 79 | install(DIRECTORY scripts/ 80 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 81 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/include/ethercat_trigger_controllers/projector_controller.h: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | #ifndef PROJECTOR_CONTROLLER_H 36 | #define PROJECTOR_CONTROLLER_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace controller 46 | { 47 | class ProjectorController : public pr2_controller_interface::Controller 48 | { 49 | public: 50 | ProjectorController(); 51 | 52 | ~ProjectorController(); 53 | 54 | void update(); 55 | void starting(); 56 | void stopping(); 57 | 58 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 59 | 60 | private: 61 | pr2_mechanism_model::RobotState * robot_; 62 | pr2_hardware_interface::Projector *projector_; 63 | 64 | uint32_t old_rising_; 65 | uint32_t old_falling_; 66 | 67 | boost::scoped_ptr< 68 | realtime_tools::RealtimePublisher< 69 | std_msgs::Header> > rising_edge_pub_, falling_edge_pub_; 70 | 71 | ros::NodeHandle node_handle_; 72 | 73 | // Configuration of controller. 74 | std::string actuator_name_; 75 | 76 | double current_setting_; 77 | 78 | double start_time_; /// @todo KILLME FIXME 79 | }; 80 | 81 | }; 82 | 83 | #endif 84 | 85 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/test/test_whole_body_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | 38 | static int done = 0; 39 | 40 | void finalize(int donecare) 41 | { 42 | done = 1; 43 | } 44 | 45 | int main( int argc, char** argv ) 46 | { 47 | 48 | /*********** Initialize ROS ****************/ 49 | ros::init(argc,argv); 50 | ros::Node *node = new ros::Node("test_arm_trajectory_controller"); 51 | 52 | signal(SIGINT, finalize); 53 | signal(SIGQUIT, finalize); 54 | signal(SIGTERM, finalize); 55 | 56 | 57 | /*********** Start moving the robot ************/ 58 | manipulation_msgs::JointTraj cmd; 59 | 60 | int num_points = 1; 61 | int num_joints = 10; 62 | 63 | cmd.set_points_size(num_points); 64 | 65 | for(int i=0; iadvertise("base/trajectory_controller/command",1); 81 | sleep(1); 82 | node->publish("base/trajectory_controller/command",cmd); 83 | sleep(4); 84 | 85 | } 86 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef JOINT_EFFORT_CONTROLLER_H 36 | #define JOINT_EFFORT_CONTROLLER_H 37 | 38 | /** 39 | @class pr2_controller_interface::JointEffortController 40 | @brief Joint Effort Controller (torque or force) 41 | 42 | This class passes the commanded effort down through the 43 | transmissions and safety code. 44 | 45 | @section ROS ROS interface 46 | 47 | @param type Must be "JointEffortController" 48 | @param joint Name of the joint to control. 49 | 50 | Subscribes to: 51 | - @b command (std_msgs::Float64) : The joint effort to apply 52 | */ 53 | 54 | #include 55 | #include 56 | #include 57 | 58 | #include 59 | 60 | 61 | namespace controller 62 | { 63 | 64 | class JointEffortController : public pr2_controller_interface::Controller 65 | { 66 | public: 67 | 68 | JointEffortController(); 69 | ~JointEffortController(); 70 | 71 | bool init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name); 72 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 73 | 74 | virtual void starting() { command_ = 0.0;} 75 | virtual void update(); 76 | 77 | pr2_mechanism_model::JointState *joint_state_; 78 | 79 | double command_; 80 | 81 | private: 82 | pr2_mechanism_model::RobotState *robot_; 83 | 84 | ros::NodeHandle node_; 85 | ros::Subscriber sub_command_; 86 | void commandCB(const std_msgs::Float64ConstPtr& msg); 87 | 88 | }; 89 | 90 | } 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/scripts/effort.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | # Copyright (c) 2008, Willow Garage, Inc. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # * Neither the name of the Willow Garage, Inc. nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # This script brings up an effort controller on your joint of choice 30 | # and allows you to type in the desired efforts. 31 | # 32 | # Author: Stuart Glaser 33 | 34 | import random 35 | CONTROLLER_NAME = "quick_effort_controller_%08d" % random.randint(0,10**8-1) 36 | 37 | import sys 38 | import signal 39 | import roslib 40 | roslib.load_manifest('robot_mechanism_controllers') 41 | import rospy 42 | from std_msgs.msg import * 43 | from pr2_controller_manager import pr2_controller_manager_interface 44 | 45 | prev_handler = None 46 | 47 | def shutdown(sig, stackframe): 48 | pr2_controller_manager_interface.stop_controller(CONTROLLER_NAME) 49 | pr2_controller_manager_interface.unload_controller(CONTROLLER_NAME) 50 | if prev_handler is not None: 51 | prev_handler(signal.SIGINT,None) 52 | 53 | def load_joint_config(joint_name): 54 | rospy.set_param(CONTROLLER_NAME+'/type', 'JointEffortController') 55 | rospy.set_param(CONTROLLER_NAME+'/joint', joint_name) 56 | 57 | def main(): 58 | if len(sys.argv) < 2: 59 | print("Usage: effort.py ") 60 | sys.exit(1) 61 | joint = sys.argv[1] 62 | rospy.init_node('effort', anonymous=True) 63 | 64 | # Override rospy's signal handling. We'll invoke rospy's handler after 65 | # we're done shutting down 66 | prev_handler = signal.getsignal(signal.SIGINT) 67 | signal.signal(signal.SIGINT, shutdown) 68 | 69 | load_joint_config(joint) 70 | pr2_controller_manager_interface.load_controller(CONTROLLER_NAME) 71 | pr2_controller_manager_interface.start_controller(CONTROLLER_NAME) 72 | 73 | pub = rospy.Publisher("%s/command" % CONTROLLER_NAME, Float64) 74 | 75 | print("Enter efforts:") 76 | while not rospy.is_shutdown(): 77 | effort = float(sys.stdin.readline().strip()) 78 | pub.publish(Float64(effort)) 79 | 80 | if __name__ == '__main__': 81 | main() 82 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/include/pr2_calibration_controllers/gripper_calibration_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | #include "pr2_mechanism_model/robot.h" 40 | #include "robot_mechanism_controllers/joint_velocity_controller.h" 41 | #include "realtime_tools/realtime_publisher.h" 42 | #include "std_msgs/Empty.h" 43 | #include "pr2_controllers_msgs/QueryCalibrationState.h" 44 | 45 | namespace controller 46 | { 47 | 48 | class GripperCalibrationController : public pr2_controller_interface::Controller 49 | { 50 | public: 51 | GripperCalibrationController(); 52 | ~GripperCalibrationController(); 53 | 54 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 55 | virtual void starting(); 56 | virtual void update(); 57 | 58 | bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req, pr2_controllers_msgs::QueryCalibrationState::Response& resp); 59 | 60 | 61 | protected: 62 | 63 | enum { INITIALIZED, BEGINNING, STARTING, CLOSING, BACK_OFF, CLOSING_SLOWLY, CALIBRATED }; 64 | int state_; 65 | int count_; 66 | int stop_count_; 67 | 68 | ros::NodeHandle node_; 69 | pr2_mechanism_model::RobotState *robot_; 70 | ros::Time last_publish_time_; 71 | ros::ServiceServer is_calibrated_srv_; 72 | boost::scoped_ptr > pub_calibrated_; 73 | 74 | double search_velocity_; 75 | pr2_hardware_interface::Actuator *actuator_; 76 | pr2_mechanism_model::JointState *joint_; 77 | std::vector other_joints_; 78 | 79 | double init_time; 80 | double stopped_velocity_tolerance_; 81 | 82 | controller::JointVelocityController vc_; /** The joint velocity controller used to sweep the joint.*/ 83 | }; 84 | 85 | 86 | } 87 | 88 | 89 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/include/pr2_calibration_controllers/joint_calibration_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef PR2_JOINT_CALIBRATION_CONTROLLER 36 | #define PR2_JOINT_CALIBRATION_CONTROLLER 37 | 38 | #include 39 | #include 40 | 41 | #include "ros/node_handle.h" 42 | #include "pr2_mechanism_model/robot.h" 43 | #include "robot_mechanism_controllers/joint_velocity_controller.h" 44 | #include "realtime_tools/realtime_publisher.h" 45 | #include "std_msgs/Empty.h" 46 | #include "pr2_controllers_msgs/QueryCalibrationState.h" 47 | 48 | 49 | namespace controller 50 | { 51 | 52 | class JointCalibrationController : public pr2_controller_interface::Controller 53 | { 54 | public: 55 | JointCalibrationController(); 56 | virtual ~JointCalibrationController(); 57 | 58 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 59 | virtual void starting(); 60 | virtual void update(); 61 | 62 | bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req, pr2_controllers_msgs::QueryCalibrationState::Response& resp); 63 | 64 | 65 | protected: 66 | pr2_mechanism_model::RobotState* robot_; 67 | ros::NodeHandle node_; 68 | ros::Time last_publish_time_; 69 | ros::ServiceServer is_calibrated_srv_; 70 | boost::scoped_ptr > pub_calibrated_; 71 | 72 | enum { INITIALIZED, BEGINNING, MOVING_TO_LOW, MOVING_TO_HIGH, CALIBRATED }; 73 | int state_; 74 | int countdown_; 75 | 76 | double search_velocity_; 77 | double original_position_; 78 | 79 | pr2_hardware_interface::Actuator *actuator_; 80 | pr2_mechanism_model::JointState *joint_; 81 | boost::shared_ptr transmission_; 82 | 83 | controller::JointVelocityController vc_; 84 | }; 85 | 86 | } 87 | 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/include/pr2_calibration_controllers/caster_calibration_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* 31 | * Author: Stuart Glaser 32 | */ 33 | 34 | #ifndef CASTER_CALIBRATION_CONTROLLER_H 35 | #define CASTER_CALIBRATION_CONTROLLER_H 36 | 37 | #include 38 | #include 39 | 40 | #include "ros/node_handle.h" 41 | #include "pr2_mechanism_controllers/caster_controller.h" 42 | #include "realtime_tools/realtime_publisher.h" 43 | #include "std_msgs/Empty.h" 44 | #include "pr2_controllers_msgs/QueryCalibrationState.h" 45 | 46 | namespace controller { 47 | 48 | class CasterCalibrationController : public pr2_controller_interface::Controller 49 | { 50 | public: 51 | CasterCalibrationController(); 52 | ~CasterCalibrationController(); 53 | 54 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 55 | virtual void starting(); 56 | virtual void update(); 57 | 58 | bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req, pr2_controllers_msgs::QueryCalibrationState::Response& resp); 59 | 60 | protected: 61 | 62 | ros::NodeHandle node_; 63 | pr2_mechanism_model::RobotState *robot_; 64 | 65 | enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED }; 66 | int state_; 67 | 68 | double search_velocity_; 69 | bool original_switch_state_; 70 | double original_position_; 71 | 72 | ros::Time beginning_; 73 | int unstick_iter_; 74 | 75 | pr2_hardware_interface::Actuator *actuator_; 76 | pr2_mechanism_model::JointState *joint_, *wheel_l_joint_, *wheel_r_joint_; 77 | boost::shared_ptr transmission_; 78 | 79 | // Preallocated, for use in update() 80 | std::vector fake_as; 81 | std::vector fake_js; 82 | 83 | controller::CasterController cc_; 84 | 85 | ros::Time last_publish_time_; 86 | ros::ServiceServer is_calibrated_srv_; 87 | boost::scoped_ptr > pub_calibrated_; 88 | }; 89 | 90 | } // namespace 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** 31 | @class pr2_controller_interface::CartesianWrenchController 32 | @author Wim Meeussen 33 | 34 | @brief Cartesian wrench controller 35 | 36 | Controls the wrench at the end effector of a chain of the robot. 37 | 38 | @section ROS ROS interface 39 | 40 | @param type Must be "CartesianWrenchController" 41 | 42 | @param root_name The name of the root link of the chain of links 43 | that you wish to control. 44 | 45 | @param tip_name The name of the tip link (end effector) of the 46 | chain of links that you wish to control. 47 | 48 | Subscribes to: 49 | 50 | - @b command (geometry_msgs::Wrench) : The desired wrench to achieve 51 | 52 | */ 53 | 54 | #ifndef CARTESIAN_WRENCH_CONTROLLER_H 55 | #define CARTESIAN_WRENCH_CONTROLLER_H 56 | 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | 69 | 70 | namespace controller { 71 | 72 | class CartesianWrenchController : public pr2_controller_interface::Controller 73 | { 74 | public: 75 | CartesianWrenchController(); 76 | ~CartesianWrenchController(); 77 | 78 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 79 | 80 | void starting(); 81 | void update(); 82 | 83 | // input of the controller 84 | KDL::Wrench wrench_desi_; 85 | 86 | private: 87 | ros::NodeHandle node_; 88 | ros::Subscriber sub_command_; 89 | void command(const geometry_msgs::WrenchConstPtr& wrench_msg); 90 | pr2_mechanism_model::RobotState *robot_state_; 91 | pr2_mechanism_model::Chain chain_; 92 | 93 | KDL::Chain kdl_chain_; 94 | boost::scoped_ptr jnt_to_jac_solver_; 95 | KDL::JntArray jnt_pos_, jnt_eff_; 96 | KDL::Jacobian jacobian_; 97 | 98 | }; 99 | 100 | } // namespace 101 | 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/src/joint_effort_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "robot_mechanism_controllers/joint_effort_controller.h" 36 | #include "pluginlib/class_list_macros.hpp" 37 | 38 | PLUGINLIB_EXPORT_CLASS( controller::JointEffortController, pr2_controller_interface::Controller) 39 | 40 | namespace controller { 41 | 42 | JointEffortController::JointEffortController() 43 | : joint_state_(NULL), command_(0), robot_(NULL) 44 | { 45 | } 46 | 47 | JointEffortController::~JointEffortController() 48 | { 49 | sub_command_.shutdown(); 50 | } 51 | 52 | bool JointEffortController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name) 53 | { 54 | if (!robot) 55 | { 56 | ROS_ERROR("The given robot was NULL"); 57 | return false; 58 | } 59 | robot_ = robot; 60 | 61 | joint_state_ = robot_->getJointState(joint_name); 62 | if (!joint_state_) 63 | { 64 | ROS_ERROR("JointEffortController could not find joint named \"%s\"", 65 | joint_name.c_str()); 66 | return false; 67 | } 68 | 69 | return true; 70 | } 71 | 72 | bool JointEffortController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) 73 | { 74 | assert(robot); 75 | node_ = n; 76 | robot_ = robot; 77 | 78 | std::string joint_name; 79 | if (!node_.getParam("joint", joint_name)) 80 | { 81 | ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str()); 82 | return false; 83 | } 84 | 85 | if (!(joint_state_ = robot_->getJointState(joint_name))) 86 | { 87 | ROS_ERROR("Could not find joint \"%s\" (namespace: %s)", 88 | joint_name.c_str(), node_.getNamespace().c_str()); 89 | return false; 90 | } 91 | 92 | sub_command_ = node_.subscribe( 93 | "command", 1, &JointEffortController::commandCB, this); 94 | 95 | return true; 96 | } 97 | 98 | void JointEffortController::update() 99 | { 100 | joint_state_->commanded_effort_ += command_; 101 | } 102 | 103 | void JointEffortController::commandCB(const std_msgs::Float64ConstPtr& msg) 104 | { 105 | command_ = msg->data; 106 | } 107 | 108 | } 109 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/src/fake_calibration_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "pr2_calibration_controllers/fake_calibration_controller.h" 36 | #include "ros/time.h" 37 | #include "pluginlib/class_list_macros.hpp" 38 | 39 | using namespace std; 40 | using namespace controller; 41 | 42 | PLUGINLIB_EXPORT_CLASS(controller::FakeCalibrationController, pr2_controller_interface::Controller) 43 | 44 | namespace controller 45 | { 46 | 47 | FakeCalibrationController::FakeCalibrationController() 48 | : last_publish_time_(0), joint_(NULL) 49 | { 50 | } 51 | 52 | FakeCalibrationController::~FakeCalibrationController() 53 | { 54 | } 55 | 56 | bool FakeCalibrationController::init(pr2_mechanism_model::RobotState *robot, 57 | ros::NodeHandle &n) 58 | { 59 | assert(robot); 60 | robot_ = robot; 61 | node_ = n; 62 | 63 | std::string joint_name; 64 | if (!node_.getParam("joint", joint_name)) 65 | { 66 | ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str()); 67 | return false; 68 | } 69 | if (!(joint_ = robot->getJointState(joint_name))) 70 | { 71 | ROS_ERROR("Could not find joint \"%s\" (namespace: %s)", 72 | joint_name.c_str(), node_.getNamespace().c_str()); 73 | return false; 74 | } 75 | 76 | // advertise service to check calibration 77 | is_calibrated_srv_ = node_.advertiseService("is_calibrated", &FakeCalibrationController::isCalibrated, this); 78 | 79 | // "Calibrated" topic 80 | pub_calibrated_.reset(new realtime_tools::RealtimePublisher(node_, "calibrated", 1)); 81 | 82 | return true; 83 | } 84 | 85 | 86 | void FakeCalibrationController::starting() 87 | { 88 | } 89 | 90 | 91 | bool FakeCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req, 92 | pr2_controllers_msgs::QueryCalibrationState::Response& resp) 93 | { 94 | resp.is_calibrated = joint_->calibrated_; 95 | return true; 96 | } 97 | 98 | 99 | void FakeCalibrationController::update() 100 | { 101 | assert(joint_); 102 | joint_->calibrated_ = true; 103 | } 104 | 105 | } 106 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/test/test_base_controller.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | int main( int argc, char** argv ) 10 | { 11 | if(argc != 3) 12 | { 13 | printf("Usage: %s \n",argv[0]); 14 | exit(-1); 15 | } 16 | printf("robot file:: %s, controller file:: %s\n",argv[1],argv[2]); 17 | 18 | 19 | /*********** Create the robot model ****************/ 20 | pr2_mechanism::Robot *robot_model = new pr2_mechanism::Robot; 21 | controller::BaseControllerNode bc; 22 | HardwareInterface hw(0); 23 | robot_model->hw_ = &hw; 24 | 25 | 26 | /*********** Initialize ROS ****************/ 27 | ros::init(argc,argv); 28 | 29 | /*********** Load the robot model and state file ************/ 30 | char *xml_robot_file = argv[1]; 31 | TiXmlDocument xml(xml_robot_file); // Load robot description 32 | xml.LoadFile(); 33 | TiXmlElement *root = xml.FirstChildElement("robot"); 34 | urdf::normalizeXml(root); 35 | robot_model->initXml(root); 36 | pr2_mechanism_model::RobotState *robot_state = new pr2_mechanism_model::RobotState(robot_model, &hw); 37 | 38 | 39 | /*********** Load the controller file ************/ 40 | #error Broken because the base controller does not have initXml anymore 41 | char *xml_control_file = argv[2]; 42 | TiXmlDocument xml_control(xml_control_file); // Load robot description 43 | xml_control.LoadFile(); 44 | TiXmlElement *root_control = xml_control.FirstChildElement("controllers"); 45 | TiXmlElement *root_controller = root_control->FirstChildElement("controller"); 46 | //bc.initXml(robot_state,root_controller); 47 | 48 | 49 | /************ Testing the odometry calculations themselves ******************/ 50 | /* NEWMAT::Matrix A(16,3); 51 | 52 | A.Row(1) << 10 << 8.95 << 0.05; 53 | A.Row(2) << 0 << -2 << 0; 54 | A.Row(3) << 1 << -0.1 << 0.01; 55 | A.Row(4) << 2 << 1.1 << 0; 56 | 57 | A.Row(5) << 3 << 2 << -0.05; 58 | A.Row(6) << 4 << 3 << 0.01; 59 | A.Row(7) << 5 << 4.1 << 0.05; 60 | A.Row(8) << -1 << -2 << 0.025; 61 | 62 | A.Row(9) << 6.15 << 5.05 << 0.01; 63 | A.Row(10) << 6.985 << 6.02 << 0.01; 64 | A.Row(11) << 8.01 << 8.05 << -0.05; 65 | A.Row(12) << 9.03 << 8.1 << -0.01; 66 | 67 | A.Row(13) << -8.03 << -9.1 << 0.01; 68 | A.Row(14) << -10.03 << -13.1 << 0.05; 69 | A.Row(15) << -15.03 << -16.1 << -0.015; 70 | A.Row(16) << -16.03 << -17.1 << -0.01; 71 | 72 | NEWMAT::Matrix B(16,1); 73 | B << 1.1 << 1 << 1.1 << 1.15 << 0.95 << 0.99 << 0.98 << 0.95 << 1.05 << 1.1 << 1.05 << 1 << 1.13 << 0.995 << 1.035 << 1.08; 74 | NEWMAT::Matrix xfit(3,1); 75 | 76 | xfit = bc.c_->iterativeLeastSquares(A,B,"Gaussian",10); 77 | cout << "done" << xfit << endl; 78 | */ 79 | 80 | delete robot_model; 81 | delete robot_state; 82 | } 83 | 84 | 85 | 86 | 87 | 88 | 89 | /*class BaseControllerTest : public testing::Test { 90 | protected: 91 | virtual void SetUp() { 92 | } 93 | 94 | // virtual void TearDown() {} 95 | 96 | Queue q0_; 97 | Queue q1_; 98 | Queue q2_; 99 | }; 100 | 101 | 102 | TEST (BaseControllerTests, constructionDestruction) 103 | { 104 | controller::BaseController *bc = new controller::BaseController(); 105 | delete bc; 106 | } 107 | 108 | TEST (BaseControllerTests, loadXML) 109 | { 110 | pr2_mechanism::Robot *robot = new pr2_mechanism::Robot("r2d2"); 111 | controller::BaseController bc; 112 | 113 | const string xml_controller_file = "controller_base.xml"; 114 | const string xml_robot_file = "pr2_base.xml" 115 | 116 | TiXmlDocument xml(xml_robot_file); // Load robot description 117 | xml.LoadFile(); 118 | TiXmlElement *root = xml.FirstChildElement("robot"); 119 | robot->initXml(root); 120 | } 121 | 122 | int main(int argc, char **argv) 123 | { 124 | testing::InitGoogleTest(&argc, argv); 125 | return RUN_ALL_TESTS(); 126 | } 127 | */ 128 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/include/ethercat_trigger_controllers/multi_trigger_controller.h: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | #ifndef TRIGGER_CONTROLLER_H 36 | #define TRIGGER_CONTROLLER_H 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | /** @class MultiMultiTriggerController 51 | * @brief Allows complex periodic triggering waveforms through a digital output 52 | * pin of the motor controller boards. 53 | * 54 | */ 55 | 56 | namespace controller 57 | { 58 | class MultiTriggerController : public pr2_controller_interface::Controller 59 | { 60 | typedef ethercat_trigger_controllers::MultiWaveform config_t; 61 | public: 62 | MultiTriggerController(); 63 | 64 | ~MultiTriggerController(); 65 | 66 | void update(); 67 | 68 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 69 | 70 | private: 71 | boost::mutex config_mutex_; // Held while config_ is changing. 72 | 73 | bool setMultiWaveformSrv( 74 | ethercat_trigger_controllers::SetMultiWaveform::Request &req, 75 | ethercat_trigger_controllers::SetMultiWaveform::Response &resp); 76 | 77 | pr2_mechanism_model::RobotState *robot_; 78 | pr2_hardware_interface::DigitalOutCommand *digital_out_command_; 79 | 80 | // Information about the next transition 81 | double transition_time_; 82 | double transition_period_; 83 | unsigned int transition_index_; 84 | 85 | ros::ServiceServer set_waveform_handle_; 86 | ros::NodeHandle node_handle_; 87 | ros::Publisher waveform_; 88 | 89 | std::vector > > pubs_; 90 | 91 | // Configuration of controller. 92 | config_t config_; 93 | std::string digital_output_name_; 94 | }; 95 | 96 | }; 97 | 98 | #endif 99 | 100 | -------------------------------------------------------------------------------- /pr2_calibration_controllers/include/pr2_calibration_controllers/wrist_calibration_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* 31 | * Author: Stuart Glaser 32 | */ 33 | 34 | #ifndef WRIST_CALIBRATION_CONTROLLER_H 35 | #define WRIST_CALIBRATION_CONTROLLER_H 36 | 37 | #include 38 | #include 39 | 40 | #include "robot_mechanism_controllers/joint_velocity_controller.h" 41 | #include "realtime_tools/realtime_publisher.h" 42 | #include "pr2_mechanism_model/wrist_transmission.h" 43 | #include "std_msgs/Empty.h" 44 | #include "pr2_controllers_msgs/QueryCalibrationState.h" 45 | 46 | namespace controller { 47 | 48 | class WristCalibrationController : public pr2_controller_interface::Controller 49 | { 50 | public: 51 | WristCalibrationController(); 52 | ~WristCalibrationController(); 53 | 54 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 55 | virtual void starting(); 56 | virtual void update(); 57 | 58 | bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req, pr2_controllers_msgs::QueryCalibrationState::Response& resp); 59 | 60 | protected: 61 | 62 | enum { INITIALIZED, BEGINNING, 63 | MOVING_FLEX_TO_HIGH, MOVING_FLEX, 64 | MOVING_ROLL_TO_LOW, MOVING_ROLL, CALIBRATED }; 65 | int state_; 66 | 67 | pr2_mechanism_model::RobotState *robot_; 68 | ros::NodeHandle node_; 69 | ros::Time last_publish_time_; 70 | ros::ServiceServer is_calibrated_srv_; 71 | boost::scoped_ptr > pub_calibrated_; 72 | 73 | double roll_search_velocity_; 74 | double flex_search_velocity_; 75 | bool original_switch_state_; 76 | int countdown_; 77 | 78 | // Tracks the actuator positions for when the optical switch occurred. 79 | double flex_switch_l_, flex_switch_r_; 80 | double roll_switch_l_, roll_switch_r_; 81 | 82 | double prev_actuator_l_position_, prev_actuator_r_position_; 83 | 84 | pr2_hardware_interface::Actuator *actuator_l_, *actuator_r_; 85 | pr2_mechanism_model::JointState *flex_joint_, *roll_joint_; 86 | boost::shared_ptr transmission_; 87 | 88 | // Preallocated, for use in update() 89 | std::vector fake_as; 90 | std::vector fake_js; 91 | 92 | controller::JointVelocityController vc_flex_, vc_roll_; 93 | }; 94 | 95 | 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* 31 | * Author: Stuart Glaser 32 | 33 | Example config: 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | YAML config 42 | \verbatim 43 | caster_fl: 44 | type: CasterController 45 | caster_pid: {p: 6.0} 46 | wheel_pid: {p: 4.0} 47 | joints: 48 | caster: fl_caster_rotation_joint 49 | wheel_l: fl_caster_l_wheel_joint 50 | wheel_r: fl_caster_r_wheel_joint 51 | \endverbatim 52 | 53 | */ 54 | 55 | #ifndef CASTER_CONTROLLER_H 56 | #define CASTER_CONTROLLER_H 57 | 58 | #include "ros/node_handle.h" 59 | 60 | #include "pr2_controller_interface/controller.h" 61 | #include "pr2_mechanism_model/robot.h" 62 | #include "control_toolbox/pid.h" 63 | #include "robot_mechanism_controllers/joint_velocity_controller.h" 64 | #include "std_msgs/Float64.h" 65 | #include 66 | 67 | namespace controller { 68 | 69 | class CasterController : public pr2_controller_interface::Controller 70 | { 71 | public: 72 | const static double WHEEL_RADIUS; 73 | const static double WHEEL_OFFSET; 74 | 75 | CasterController(); 76 | ~CasterController(); 77 | 78 | bool init(pr2_mechanism_model::RobotState *robot_state, 79 | const std::string &caster_joint, 80 | const std::string &wheel_l_joint, const std::string &wheel_r_joint, 81 | const control_toolbox::Pid &caster_pid, const control_toolbox::Pid &wheel_pid); 82 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 83 | 84 | void update(); 85 | 86 | double steer_velocity_; 87 | double drive_velocity_; 88 | 89 | double getSteerPosition() { return caster_->position_; } 90 | double getSteerVelocity() { return caster_->velocity_; } 91 | 92 | pr2_mechanism_model::JointState *caster_; 93 | 94 | private: 95 | ros::NodeHandle node_; 96 | JointVelocityController caster_vel_, wheel_l_vel_, wheel_r_vel_; 97 | ros::Subscriber steer_cmd_; 98 | ros::Subscriber drive_cmd_; 99 | 100 | void setSteerCB(const std_msgs::Float64ConstPtr& msg); 101 | void setDriveCB(const std_msgs::Float64ConstPtr& msg); 102 | }; 103 | 104 | } 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | 38 | static int done = 0; 39 | 40 | void finalize(int donecare) 41 | { 42 | done = 1; 43 | } 44 | 45 | int main( int argc, char** argv ) 46 | { 47 | 48 | /*********** Initialize ROS ****************/ 49 | ros::init(argc,argv); 50 | ros::Node *node = new ros::Node("test_arm_trajectory_controller"); 51 | 52 | signal(SIGINT, finalize); 53 | signal(SIGQUIT, finalize); 54 | signal(SIGTERM, finalize); 55 | 56 | 57 | /*********** Start moving the robot ************/ 58 | manipulation_msgs::JointTraj cmd; 59 | 60 | int num_points = 3; 61 | int num_joints = 14; 62 | 63 | cmd.set_points_size(num_points); 64 | 65 | for(int i=0; iadvertise("/arm/trajectory_controller/arm_trajectory_command",1); 113 | node->publish("/arm/trajectory_controller/arm_trajectory_command",cmd); 114 | sleep(4); 115 | 116 | ros::Time start_time = ros::Time::now(); 117 | ros::Duration sleep_time = ros::Duration().fromSec(0.01); 118 | 119 | } 120 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef PR2_GRIPPER_CONTROLLER_H__ 36 | #define PR2_GRIPPER_CONTROLLER_H__ 37 | 38 | /** 39 | @class pr2_controller_interface::Pr2GripperController 40 | @brief Joint Position Controller 41 | 42 | This class controls positon using a pid loop. 43 | 44 | @section ROS ROS interface 45 | 46 | @param type Must be "Pr2GripperController" 47 | @param joint Name of the joint to control. 48 | @param pid Contains the gains for the PID loop around position. See: control_toolbox::Pid 49 | 50 | Subscribes to: 51 | 52 | - @b command (std_msgs::Float64) : The joint position to achieve. 53 | 54 | Publishes: 55 | 56 | - @b state (pr2_controllers_msgs::JointControllerState) : 57 | Current state of the controller, including pid error and gains. 58 | 59 | */ 60 | 61 | #include 62 | 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | 71 | #include 72 | #include 73 | 74 | namespace controller 75 | { 76 | 77 | class Pr2GripperController : public pr2_controller_interface::Controller 78 | { 79 | public: 80 | 81 | Pr2GripperController(); 82 | ~Pr2GripperController(); 83 | 84 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 85 | 86 | virtual void starting() { 87 | using namespace pr2_controllers_msgs; 88 | Pr2GripperCommandPtr c(new Pr2GripperCommand); 89 | c->position = joint_state_->position_; 90 | c->max_effort = 0.0; 91 | command_box_.set(c); 92 | } 93 | 94 | /*! 95 | * \brief Issues commands to the joint. Should be called at regular intervals 96 | */ 97 | virtual void update(); 98 | 99 | pr2_mechanism_model::JointState *joint_state_; 100 | realtime_tools::RealtimeBox command_box_; 101 | 102 | private: 103 | int loop_count_; 104 | pr2_mechanism_model::RobotState *robot_; 105 | control_toolbox::Pid pid_; 106 | ros::Time last_time_; 107 | 108 | ros::NodeHandle node_; 109 | 110 | boost::scoped_ptr< 111 | realtime_tools::RealtimePublisher< 112 | pr2_controllers_msgs::JointControllerState> > controller_state_publisher_ ; 113 | 114 | ros::Subscriber sub_command_; 115 | void commandCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr& msg); 116 | }; 117 | 118 | } // namespace 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** 31 | @class pr2_controller_interface::CartesianTwistController 32 | @author Wim Meeussen 33 | 34 | @brief Cartesian twist controller 35 | 36 | Controls the twist at the end effector of a chain of the robot. 37 | 38 | @section ROS ROS interface 39 | 40 | @param type Must be "CartesianTwistController" 41 | 42 | @param root_name The name of the root link of the chain of links 43 | that you wish to control. 44 | 45 | @param tip_name The name of the tip link (end effector) of the 46 | chain of links that you wish to control. 47 | 48 | @param fb_trans The gains for the PID loop around linear velocity. See: control_toolbox::Pid 49 | 50 | @param fb_rot The gains for the PID loop around angular velocity. See: control_toolbox::Pid 51 | 52 | Subscribes to: 53 | 54 | - @b command (geometry_msgs::Twist) : The desired twist to 55 | achieve. 56 | */ 57 | 58 | #ifndef CARTESIAN_TWIST_CONTROLLER_H 59 | #define CARTESIAN_TWIST_CONTROLLER_H 60 | 61 | #include 62 | #include 63 | #include 64 | 65 | #include 66 | #include 67 | 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | #include 74 | #include 75 | #include 76 | #include 77 | 78 | 79 | namespace controller { 80 | 81 | class CartesianTwistController : public pr2_controller_interface::Controller 82 | { 83 | public: 84 | CartesianTwistController(); 85 | ~CartesianTwistController(); 86 | 87 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 88 | 89 | void starting(); 90 | void update(); 91 | 92 | // input of the controller 93 | KDL::Twist twist_desi_, twist_meas_; 94 | 95 | private: 96 | ros::NodeHandle node_; 97 | ros::Subscriber sub_command_; 98 | void command(const geometry_msgs::TwistConstPtr& twist_msg); 99 | double ff_trans_, ff_rot_; 100 | ros::Time last_time_; 101 | 102 | // pid controllers 103 | std::vector fb_pid_controller_; 104 | 105 | // robot description 106 | pr2_mechanism_model::RobotState *robot_state_; 107 | pr2_mechanism_model::Chain chain_; 108 | 109 | // kdl stuff for kinematics 110 | KDL::Chain kdl_chain_; 111 | boost::scoped_ptr jnt_to_twist_solver_; 112 | boost::scoped_ptr jac_solver_; 113 | KDL::JntArrayVel jnt_posvel_; 114 | KDL::JntArray jnt_eff_; 115 | KDL::Jacobian jacobian_; 116 | KDL::Wrench wrench_out_; 117 | 118 | geometry_msgs::Twist twist_msg_; 119 | }; 120 | 121 | } // namespace 122 | 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/joint_spline_trajectory_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /*! 31 | \author Stuart Glaser 32 | 33 | \class pr2_controller_interface::JointSplineTrajectoryController 34 | 35 | */ 36 | 37 | #ifndef JOINT_TRAJECTORY_CONTROLLER_H__ 38 | #define JOINT_TRAJECTORY_CONTROLLER_H__ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include "trajectory_msgs/JointTrajectory.h" 51 | //#include "robot_mechanism_controllers/Trajectory.h" 52 | #include "pr2_controllers_msgs/QueryTrajectoryState.h" 53 | #include "pr2_controllers_msgs/JointTrajectoryControllerState.h" 54 | 55 | namespace controller { 56 | 57 | class JointSplineTrajectoryController : public pr2_controller_interface::Controller 58 | { 59 | public: 60 | 61 | JointSplineTrajectoryController(); 62 | ~JointSplineTrajectoryController(); 63 | 64 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 65 | 66 | void starting(); 67 | void update(); 68 | 69 | private: 70 | int loop_count_; 71 | pr2_mechanism_model::RobotState *robot_; 72 | ros::Time last_time_; 73 | std::vector joints_; 74 | std::vector pids_; 75 | 76 | ros::NodeHandle node_; 77 | 78 | void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); 79 | ros::Subscriber sub_command_; 80 | 81 | bool queryStateService(pr2_controllers_msgs::QueryTrajectoryState::Request &req, 82 | pr2_controllers_msgs::QueryTrajectoryState::Response &resp); 83 | ros::ServiceServer serve_query_state_; 84 | 85 | boost::scoped_ptr< 86 | realtime_tools::RealtimePublisher< 87 | pr2_controllers_msgs::JointTrajectoryControllerState> > controller_state_publisher_; 88 | 89 | 90 | // ------ Mechanisms for passing the trajectory into realtime 91 | 92 | // coef[0] + coef[1]*t + ... + coef[5]*t^5 93 | struct Spline 94 | { 95 | std::vector coef; 96 | 97 | Spline() : coef(6, 0.0) {} 98 | }; 99 | 100 | struct Segment 101 | { 102 | double start_time; 103 | double duration; 104 | std::vector splines; 105 | }; 106 | typedef std::vector SpecifiedTrajectory; 107 | 108 | realtime_tools::RealtimeBox< 109 | boost::shared_ptr > current_trajectory_box_; 110 | 111 | // Holds the trajectory that we are currently following. The mutex 112 | // guarding current_trajectory_ is locked from within realtime, so 113 | // it may only be locked for a bounded duration. 114 | //boost::shared_ptr current_trajectory_; 115 | //boost::recursive_mutex current_trajectory_lock_RT_; 116 | 117 | std::vector q, qd, qdd; // Preallocated in init 118 | 119 | // Samples, but handling time bounds. When the time is past the end 120 | // of the spline duration, the position is the last valid position, 121 | // and the derivatives are all 0. 122 | static void sampleSplineWithTimeBounds(const std::vector& coefficients, double duration, double time, 123 | double& position, double& velocity, double& acceleration); 124 | }; 125 | 126 | } 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef JOINT_POSITION_CONTROLLER_H 36 | #define JOINT_POSITION_CONTROLLER_H 37 | 38 | /** 39 | @class pr2_controller_interface::JointPositionController 40 | @brief Joint Position Controller 41 | 42 | This class controls positon using a pid loop. 43 | 44 | @section ROS ROS interface 45 | 46 | @param type Must be "JointPositionController" 47 | @param joint Name of the joint to control. 48 | @param pid Contains the gains for the PID loop around position. See: control_toolbox::Pid 49 | 50 | Subscribes to: 51 | 52 | - @b command (std_msgs::Float64) : The joint position to achieve. 53 | 54 | Publishes: 55 | 56 | - @b state (robot_mechanism_controllers::JointControllerState) : 57 | Current state of the controller, including pid error and gains. 58 | 59 | */ 60 | 61 | #include 62 | 63 | #include 64 | #include 65 | #include "control_toolbox/pid_gains_setter.h" 66 | #include 67 | #include 68 | #include 69 | #include 70 | 71 | namespace controller 72 | { 73 | 74 | class JointPositionController : public pr2_controller_interface::Controller 75 | { 76 | public: 77 | 78 | JointPositionController(); 79 | ~JointPositionController(); 80 | 81 | bool init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name,const control_toolbox::Pid &pid); 82 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 83 | 84 | /*! 85 | * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) 86 | * 87 | * \param command 88 | */ 89 | void setCommand(double cmd); 90 | 91 | /*! 92 | * \brief Get latest position command to the joint: revolute (angle) and prismatic (position). 93 | */ 94 | void getCommand(double & cmd); 95 | 96 | virtual void starting() { 97 | command_ = joint_state_->position_; 98 | pid_controller_.reset(); 99 | } 100 | 101 | /*! 102 | * \brief Issues commands to the joint. Should be called at regular intervals 103 | */ 104 | virtual void update(); 105 | 106 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 107 | void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min); 108 | 109 | std::string getJointName(); 110 | pr2_mechanism_model::JointState *joint_state_; /**< Joint we're controlling. */ 111 | ros::Duration dt_; 112 | double command_; /**< Last commanded position. */ 113 | 114 | private: 115 | int loop_count_; 116 | bool initialized_; 117 | pr2_mechanism_model::RobotState *robot_; /**< Pointer to robot structure. */ 118 | control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ 119 | ros::Time last_time_; /**< Last time stamp of update. */ 120 | 121 | 122 | ros::NodeHandle node_; 123 | 124 | boost::scoped_ptr< 125 | realtime_tools::RealtimePublisher< 126 | pr2_controllers_msgs::JointControllerState> > controller_state_publisher_ ; 127 | 128 | ros::Subscriber sub_command_; 129 | void setCommandCB(const std_msgs::Float64ConstPtr& msg); 130 | }; 131 | 132 | } // namespace 133 | 134 | #endif 135 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/src/cartesian_wrench_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* 31 | * Author: Wim Meeussen 32 | */ 33 | 34 | #include "robot_mechanism_controllers/cartesian_wrench_controller.h" 35 | #include 36 | #include "pluginlib/class_list_macros.hpp" 37 | 38 | 39 | using namespace KDL; 40 | 41 | PLUGINLIB_EXPORT_CLASS( controller::CartesianWrenchController, pr2_controller_interface::Controller) 42 | 43 | namespace controller { 44 | 45 | CartesianWrenchController::CartesianWrenchController() 46 | : robot_state_(NULL), 47 | jnt_to_jac_solver_(NULL) 48 | {} 49 | 50 | 51 | 52 | CartesianWrenchController::~CartesianWrenchController() 53 | { 54 | sub_command_.shutdown(); 55 | } 56 | 57 | 58 | bool CartesianWrenchController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) 59 | { 60 | // test if we got robot pointer 61 | assert(robot); 62 | robot_state_ = robot; 63 | 64 | node_ = n; 65 | 66 | // get name of root and tip from the parameter server 67 | std::string root_name, tip_name; 68 | if (!node_.getParam("root_name", root_name)){ 69 | ROS_ERROR("CartesianWrenchController: No root name found on parameter server (namespace: %s)", 70 | node_.getNamespace().c_str()); 71 | return false; 72 | } 73 | if (!node_.getParam("tip_name", tip_name)){ 74 | ROS_ERROR("CartesianWrenchController: No tip name found on parameter server (namespace: %s)", 75 | node_.getNamespace().c_str()); 76 | return false; 77 | } 78 | 79 | // create robot chain from root to tip 80 | if (!chain_.init(robot_state_, root_name, tip_name)){ 81 | ROS_ERROR("Initializing chain from %s to %s failed", root_name.c_str(), tip_name.c_str()); 82 | return false; 83 | } 84 | if (!chain_.allCalibrated()) 85 | { 86 | ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str()); 87 | return false; 88 | } 89 | chain_.toKDL(kdl_chain_); 90 | 91 | // create solver 92 | jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_)); 93 | jnt_pos_.resize(kdl_chain_.getNrOfJoints()); 94 | jnt_eff_.resize(kdl_chain_.getNrOfJoints()); 95 | jacobian_.resize(kdl_chain_.getNrOfJoints()); 96 | 97 | 98 | // subscribe to wrench commands 99 | sub_command_ = node_.subscribe 100 | ("command", 1, &CartesianWrenchController::command, this); 101 | 102 | return true; 103 | } 104 | 105 | void CartesianWrenchController::starting() 106 | { 107 | // set desired wrench to 0 108 | wrench_desi_ = Wrench::Zero(); 109 | } 110 | 111 | 112 | 113 | void CartesianWrenchController::update() 114 | { 115 | // check if joints are calibrated 116 | if (!chain_.allCalibrated()){ 117 | return; 118 | } 119 | 120 | // get joint positions 121 | chain_.getPositions(jnt_pos_); 122 | 123 | // get the chain jacobian 124 | jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_); 125 | 126 | // convert the wrench into joint efforts 127 | for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++){ 128 | jnt_eff_(i) = 0; 129 | for (unsigned int j=0; j<6; j++) 130 | jnt_eff_(i) += (jacobian_(j,i) * wrench_desi_(j)); 131 | } 132 | 133 | // set effort to joints 134 | chain_.setEfforts(jnt_eff_); 135 | } 136 | 137 | 138 | 139 | void CartesianWrenchController::command(const geometry_msgs::WrenchConstPtr& wrench_msg) 140 | { 141 | // convert to wrench command 142 | wrench_desi_.force(0) = wrench_msg->force.x; 143 | wrench_desi_.force(1) = wrench_msg->force.y; 144 | wrench_desi_.force(2) = wrench_msg->force.z; 145 | wrench_desi_.torque(0) = wrench_msg->torque.x; 146 | wrench_desi_.torque(1) = wrench_msg->torque.y; 147 | wrench_desi_.torque(2) = wrench_msg->torque.z; 148 | } 149 | 150 | }; // namespace 151 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef JOINT_VELOCITY_CONTROLLER_H 36 | #define JOINT_VELOCITY_CONTROLLER_H 37 | 38 | /** 39 | @class pr2_controller_interface::JointVelocityController 40 | @author Stuart Glaser 41 | @brief Joint Velocity Controller 42 | 43 | This controller controls velocity using a pid loop. 44 | 45 | @section ROS ROS interface 46 | 47 | @param type Must be "JointVelocityController" 48 | @param joint Name of the joint to control. 49 | @param pid Contains the gains for the PID loop around velocity. See: control_toolbox::Pid 50 | 51 | Subscribes to: 52 | 53 | - @b command (std_msgs::Float64) : The joint velocity to achieve 54 | 55 | Publishes: 56 | 57 | - @b state (robot_mechanism_controllers::JointControllerState) : 58 | Current state of the controller, including pid error and gains. 59 | 60 | */ 61 | 62 | #include 63 | #include 64 | 65 | #include "pr2_controller_interface/controller.h" 66 | #include "control_toolbox/pid.h" 67 | #include "control_toolbox/pid_gains_setter.h" 68 | 69 | // Services 70 | #include 71 | 72 | //Realtime publisher 73 | #include 74 | #include 75 | 76 | namespace controller 77 | { 78 | 79 | class JointVelocityController : public pr2_controller_interface::Controller 80 | { 81 | public: 82 | 83 | JointVelocityController(); 84 | ~JointVelocityController(); 85 | 86 | bool init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name, const control_toolbox::Pid &pid); 87 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 88 | 89 | /*! 90 | * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) 91 | * 92 | * \param double pos Velocity command to issue 93 | */ 94 | void setCommand(double cmd); 95 | 96 | /*! 97 | * \brief Get latest position command to the joint: revolute (angle) and prismatic (position). 98 | */ 99 | void getCommand(double & cmd); 100 | 101 | /*! 102 | * \brief Issues commands to the joint. Should be called at regular intervals 103 | */ 104 | 105 | virtual void starting() { 106 | command_ = 0.0; 107 | pid_controller_.reset(); 108 | } 109 | virtual void update(); 110 | 111 | void getGains(double &p, double &i, double &d, double &i_max, double &i_min); 112 | void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min); 113 | 114 | std::string getJointName(); 115 | pr2_mechanism_model::JointState *joint_state_; /**< Joint we're controlling. */ 116 | ros::Duration dt_; 117 | 118 | double command_; /**< Last commanded position. */ 119 | private: 120 | ros::NodeHandle node_; 121 | pr2_mechanism_model::RobotState *robot_; /**< Pointer to robot structure. */ 122 | control_toolbox::Pid pid_controller_; /**< Internal PID controller. */ 123 | ros::Time last_time_; /**< Last time stamp of update. */ 124 | int loop_count_; 125 | 126 | friend class JointVelocityControllerNode; 127 | 128 | boost::scoped_ptr< 129 | realtime_tools::RealtimePublisher< 130 | pr2_controllers_msgs::JointControllerState> > controller_state_publisher_ ; 131 | 132 | ros::Subscriber sub_command_; 133 | void setCommandCB(const std_msgs::Float64ConstPtr& msg); 134 | }; 135 | 136 | } // namespace 137 | 138 | #endif 139 | -------------------------------------------------------------------------------- /ethercat_trigger_controllers/src/projector_controller.cpp: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | #include "ethercat_trigger_controllers/projector_controller.h" 36 | #include "ros/console.h" 37 | #include "pluginlib/class_list_macros.hpp" 38 | 39 | PLUGINLIB_EXPORT_CLASS( controller::ProjectorController, pr2_controller_interface::Controller) 40 | 41 | using std::string; 42 | using namespace controller; 43 | 44 | ProjectorController::ProjectorController() 45 | { 46 | ROS_DEBUG("creating controller..."); 47 | } 48 | 49 | ProjectorController::~ProjectorController() 50 | { 51 | } 52 | 53 | void ProjectorController::update() 54 | { 55 | /// @todo These calculations stink but they will do for now... 56 | uint32_t rising = projector_->state_.rising_timestamp_us_; 57 | uint32_t falling = projector_->state_.falling_timestamp_us_; 58 | double curtime = robot_->getTime().toSec(); 59 | double delta = curtime - start_time_; 60 | delta -= fmod(delta, 0.001); 61 | 62 | projector_->command_.current_ = current_setting_; 63 | 64 | if (falling != old_falling_) 65 | { 66 | old_falling_ = falling; 67 | if (falling_edge_pub_ && falling_edge_pub_->trylock()) 68 | { 69 | //falling_edge_pub_->msg_.stamp = ros::Time(curtime - (stamp - falling) * 1e-6); 70 | //falling_edge_pub_->msg_.stamp = ros::Time((stamp - falling) * 1e-6); 71 | falling_edge_pub_->msg_.stamp = ros::Time(delta); 72 | falling_edge_pub_->unlockAndPublish(); 73 | } 74 | } 75 | if (rising != old_rising_) 76 | { 77 | old_rising_ = rising; 78 | if (rising_edge_pub_ && rising_edge_pub_->trylock()) 79 | { 80 | //rising_edge_pub_->msg_.stamp = ros::Time(curtime - (stamp - rising) * 1e-6); 81 | //rising_edge_pub_->msg_.stamp = ros::Time((stamp - rising) * 1e-6); 82 | rising_edge_pub_->msg_.stamp = ros::Time(delta); 83 | rising_edge_pub_->unlockAndPublish(); 84 | } 85 | } 86 | } 87 | 88 | void ProjectorController::starting() 89 | { 90 | projector_->command_.enable_ = true; 91 | projector_->command_.pulse_replicator_ = false; 92 | //projector_->command_.M_ = 0xf; 93 | old_rising_ = projector_->state_.rising_timestamp_us_; 94 | old_falling_ = projector_->state_.falling_timestamp_us_; 95 | start_time_ = 0;//robot_->getTime().toSec(); 96 | } 97 | 98 | void ProjectorController::stopping() 99 | { 100 | projector_->command_.enable_ = false; 101 | projector_->command_.pulse_replicator_ = true; 102 | //projector_->command_.M_ = 0x0; 103 | projector_->command_.current_ = 0; 104 | } 105 | 106 | bool ProjectorController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle& n) 107 | { 108 | node_handle_ = n; 109 | 110 | assert(robot); 111 | robot_=robot; 112 | 113 | ROS_DEBUG("ProjectorController::init starting"); 114 | 115 | // Get the actuator name. 116 | 117 | if (!n.getParam("actuator", actuator_name_)){ 118 | ROS_ERROR("ProjectorController was not given an actuator."); 119 | return false; 120 | } 121 | 122 | rising_edge_pub_.reset(new realtime_tools::RealtimePublisher(n, "rising_edge_timestamps", 10)); 123 | falling_edge_pub_.reset(new realtime_tools::RealtimePublisher(n, "falling_edge_timestamps", 10)); 124 | 125 | projector_ = robot_->model_->hw_->getProjector(actuator_name_); 126 | ROS_DEBUG("Got projector: %p\n", projector_); 127 | if (!projector_) 128 | { 129 | ROS_ERROR("ProjectorController could not find digital out named \"%s\".", 130 | actuator_name_.c_str()); 131 | return false; 132 | } 133 | 134 | n.param("current", current_setting_, 27.0); 135 | ROS_DEBUG("Projector current = %f", current_setting_); 136 | 137 | return true; 138 | } 139 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/joint_group_velocity_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef JOINT_GROUP_VELOCITY_CONTROLLER_H 36 | #define JOINT_GROUP_VELOCITY_CONTROLLER_H 37 | 38 | /** 39 | @class pr2_controller_interface::JointGroupVelocityController 40 | @author Stuart Glaser 41 | @brief Joint Group Velocity Controller 42 | 43 | This controller controls velocity using a pid loop. 44 | 45 | @section ROS ROS interface 46 | 47 | @param type Must be "JointGroupVelocityController" 48 | @param joint Name of the joint to control. 49 | @param pid Contains the gains for the PID loop around velocity. See: control_toolbox::Pid 50 | 51 | Subscribes to: 52 | 53 | - @b command (std_msgs::Float64) : The joint velocity to achieve 54 | 55 | Publishes: 56 | 57 | - @b state (robot_mechanism_controllers::JointControllerState) : 58 | Current state of the controller, including pid error and gains. 59 | 60 | */ 61 | 62 | #include 63 | #include 64 | #include 65 | 66 | #include "pr2_controller_interface/controller.h" 67 | #include "control_toolbox/pid.h" 68 | #include "control_toolbox/pid_gains_setter.h" 69 | 70 | // Services 71 | #include 72 | #include 73 | 74 | //Realtime publisher 75 | #include 76 | #include 77 | #include 78 | 79 | namespace controller 80 | { 81 | 82 | class JointGroupVelocityController : public pr2_controller_interface::Controller 83 | { 84 | public: 85 | 86 | JointGroupVelocityController(); 87 | ~JointGroupVelocityController(); 88 | 89 | bool init(pr2_mechanism_model::RobotState *robot, const std::vector< std::string> &joint_names, const control_toolbox::Pid &pid); 90 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 91 | 92 | /*! 93 | * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position) 94 | * 95 | * \param double pos Velocity command to issue 96 | */ 97 | void setCommand(std::vector cmd); 98 | 99 | /*! 100 | * \brief Get latest position command to the joint: revolute (angle) and prismatic (position). 101 | */ 102 | void getCommand(std::vector & cmd); 103 | 104 | /*! 105 | * \brief Issues commands to the joint. Should be called at regular intervals 106 | */ 107 | 108 | virtual void starting(); 109 | virtual void update(); 110 | 111 | void getGains(control_toolbox::Pid &pid, double &p, double &i, double &d, double &i_max, double &i_min); 112 | //void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min); 113 | 114 | std::vector< std::string > getJointName(); 115 | unsigned int n_joints_; 116 | 117 | private: 118 | ros::NodeHandle node_; 119 | pr2_mechanism_model::RobotState *robot_; /**< Pointer to robot structure. */ 120 | std::vector pid_controllers_; /**< Internal PID controller. */ 121 | std::vector joints_; /**< Joint we're controlling. */ 122 | ros::Time last_time_; /**< Last time stamp of update. */ 123 | int loop_count_; 124 | 125 | realtime_tools::RealtimeBuffer > commands_buffer_; /**< Last commanded position. */ 126 | 127 | friend class JointGroupVelocityControllerNode; 128 | 129 | boost::scoped_ptr< 130 | realtime_tools::RealtimePublisher< 131 | pr2_controllers_msgs::JointControllerStateArray> > controller_state_publisher_ ; 132 | 133 | ros::Subscriber sub_command_; 134 | void setCommandCB(const std_msgs::Float64MultiArrayConstPtr& msg); 135 | }; 136 | 137 | } // namespace 138 | 139 | #endif 140 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/test/test_odom.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | static int done = 0; 44 | 45 | void finalize(int donecare) 46 | { 47 | done = 1; 48 | } 49 | 50 | 51 | //////////////////////////////////////////////////////////////////////////////// 52 | // Return the rotation in Euler angles 53 | libTF::Vector GetAsEuler(geometry_msgs::Quaternion quat) 54 | { 55 | libTF::Vector vec; 56 | 57 | double squ; 58 | double sqx; 59 | double sqy; 60 | double sqz; 61 | 62 | // this->Normalize(); 63 | 64 | squ = quat.w * quat.w; 65 | sqx = quat.x * quat.x; 66 | sqy = quat.y * quat.y; 67 | sqz = quat.z * quat.z; 68 | 69 | // Roll 70 | vec.x = atan2(2 * (quat.y*quat.z + quat.w*quat.x), squ - sqx - sqy + sqz); 71 | 72 | // Pitch 73 | vec.y = asin(-2 * (quat.x*quat.z - quat.w * quat.y)); 74 | 75 | // Yaw 76 | vec.z = atan2(2 * (quat.x*quat.y + quat.w*quat.z), squ + sqx - sqy - sqz); 77 | 78 | return vec; 79 | } 80 | 81 | 82 | class test_run_base 83 | { 84 | public: 85 | 86 | test_run_base(){}; 87 | 88 | ~test_run_base() {} 89 | 90 | nav_msgs::Odometry odom; 91 | 92 | void odomMsgReceived() 93 | { 94 | }; 95 | 96 | }; 97 | 98 | int main( int argc, char** argv ) 99 | { 100 | 101 | /*********** Initialize ROS ****************/ 102 | ros::init(argc,argv); 103 | ros::Node *node = new ros::Node("test_run_base_controller"); 104 | 105 | test_run_base tb; 106 | 107 | node->subscribe("odom",tb.odom,&test_run_base::odomMsgReceived,&tb,10); 108 | 109 | signal(SIGINT, finalize); 110 | signal(SIGQUIT, finalize); 111 | signal(SIGTERM, finalize); 112 | 113 | 114 | /*********** Start moving the robot ************/ 115 | geometry_msgs::Twist cmd; 116 | cmd.linear.x = 0; 117 | cmd.linear.y = 0; 118 | cmd.linear.z = 0; 119 | cmd.angular.x = 0; 120 | cmd.angular.y = 0; 121 | cmd.angular.z = 0; 122 | 123 | double run_time = 0; 124 | bool run_time_set = false; 125 | int file_num = 0; 126 | 127 | if(argc >= 2) 128 | cmd.linear.x = atof(argv[1]); 129 | 130 | if(argc >= 3) 131 | cmd.linear.y = atof(argv[2]); 132 | 133 | if(argc >= 4) 134 | cmd.angular.z = atof(argv[3]); 135 | 136 | if(argc >=5) 137 | { 138 | run_time = atof(argv[4]); 139 | run_time_set = true; 140 | } 141 | 142 | if(argc ==6) 143 | { 144 | file_num = atoi(argv[5]); 145 | } 146 | 147 | node->advertise("cmd_vel",10); 148 | sleep(1); 149 | 150 | libTF::Vector ang_rates; 151 | ros::Time start_time = ros::Time::now(); 152 | ros::Duration sleep_time = ros::Duration().fromSec(0.01); 153 | 154 | std::ofstream odom_log_file; 155 | odom_log_file.open("odom_log.txt"); 156 | 157 | 158 | while(!done) 159 | { 160 | ros::Duration delta_time = ros::Time::now() - start_time; 161 | 162 | if(run_time_set && delta_time.toSec() > run_time) 163 | break; 164 | 165 | odom_log_file << tb.odom.pose.pose.position.x << " " << tb.odom.pose.pose.position.y << " " << tf::getYaw(tb.odom.pose.pose.orientation) << " " << tb.odom.twist.twist.linear.x << " " << tb.odom.twist.twist.linear.y << " " << tb.odom.twist.twist.angulat.z << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl; 166 | 167 | cout << endl << "odometry:: " << endl << "velocity:" << endl << " x:" << tb.odom.twist.twist.linear.x << endl << " y:" << tb.odom.twist.twist.linear.y << endl << " omega:" << tb.odom.twist.twist.angular.z << std::endl; 168 | node->publish("cmd_vel",cmd); 169 | sleep_time.sleep(); 170 | } 171 | 172 | node->unsubscribe("odom"); 173 | node->unadvertise("cmd_vel"); 174 | 175 | odom_log_file.close(); 176 | 177 | 178 | } 179 | -------------------------------------------------------------------------------- /robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** 31 | @class pr2_controller_interface::CartesianPoseController 32 | @author Wim Meeussen 33 | 34 | @brief Cartesian pose controller 35 | 36 | Controls the pose of the end effector of a chain of the robot. 37 | 38 | @section ROS ROS interface 39 | 40 | @param type Must be "CartesianPoseController" 41 | 42 | @param root_name The name of the root link of the chain of links 43 | that you wish to control. 44 | 45 | @param tip_name The name of the tip link (end effector) of the 46 | chain of links that you wish to control. 47 | 48 | @param output The name of the CartesianWrenchController which will 49 | achieve the desired wrench computed by this controller. 50 | 51 | @param fb_trans The gains for the PID loop around position. See: control_toolbox::Pid 52 | 53 | @param fb_rot The gains for the PID loop around orientation. See: control_toolbox::Pid 54 | 55 | Subscribes to: 56 | 57 | - @b command (geometry_msgs::PoseStamped) : The desired pose to 58 | achieve. The controller will transform this into the root link 59 | (using tf), and then attempt to move the tip link to the 60 | transformed pose. 61 | 62 | Publishes: 63 | 64 | - @b state/error (geometry_msgs::Twist) : The error from the measured pose to the desired pose. 65 | 66 | - @b state/pose (geometry_msgs::PoseStamped) : The current measured pose of the tip link. 67 | */ 68 | 69 | #ifndef CARTESIAN_POSE_CONTROLLER_H 70 | #define CARTESIAN_POSE_CONTROLLER_H 71 | 72 | #include 73 | #include 74 | 75 | #include 76 | 77 | #include 78 | #include 79 | 80 | #include 81 | #include 82 | #include 83 | #include 84 | #include 85 | #include 86 | #include 87 | #include 88 | #include 89 | #include 90 | #include 91 | #include 92 | 93 | 94 | namespace controller { 95 | 96 | class CartesianPoseController : public pr2_controller_interface::Controller 97 | { 98 | public: 99 | CartesianPoseController(); 100 | ~CartesianPoseController(); 101 | 102 | bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 103 | void starting(); 104 | void update(); 105 | 106 | void command(const geometry_msgs::PoseStamped::ConstPtr& pose_msg); 107 | 108 | // input of the controller 109 | KDL::Frame pose_desi_, pose_meas_; 110 | KDL::Twist twist_ff_; 111 | 112 | // state output 113 | KDL::Twist twist_error_; 114 | 115 | private: 116 | KDL::Frame getPose(); 117 | 118 | ros::NodeHandle node_; 119 | std::string controller_name_, root_name_; 120 | ros::Time last_time_; 121 | 122 | // robot structure 123 | pr2_mechanism_model::RobotState *robot_state_; 124 | pr2_mechanism_model::Chain chain_; 125 | 126 | // pid controllers 127 | std::vector pid_controller_; 128 | 129 | // kdl stuff for kinematics 130 | KDL::Chain kdl_chain_; 131 | boost::scoped_ptr jnt_to_pose_solver_; 132 | boost::scoped_ptr jac_solver_; 133 | KDL::JntArray jnt_pos_; 134 | KDL::JntArray jnt_eff_; 135 | KDL::Jacobian jacobian_; 136 | 137 | // reatltime publisher 138 | boost::scoped_ptr > state_error_publisher_; 139 | boost::scoped_ptr > state_pose_publisher_; 140 | unsigned int loop_count_; 141 | 142 | tf::TransformListener tf_; 143 | message_filters::Subscriber sub_command_; 144 | boost::scoped_ptr > command_filter_; 145 | //boost::scoped_ptr > command_notifier_; 146 | }; 147 | 148 | } // namespace 149 | 150 | 151 | #endif 152 | -------------------------------------------------------------------------------- /pr2_mechanism_controllers/src/pr2_gripper_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "pr2_mechanism_controllers/pr2_gripper_controller.h" 36 | #include "angles/angles.h" 37 | #include "pluginlib/class_list_macros.hpp" 38 | 39 | PLUGINLIB_EXPORT_CLASS( controller::Pr2GripperController, pr2_controller_interface::Controller) 40 | 41 | using namespace std; 42 | 43 | namespace controller { 44 | 45 | Pr2GripperController::Pr2GripperController() 46 | : joint_state_(NULL), 47 | loop_count_(0), robot_(NULL), last_time_(0) 48 | { 49 | } 50 | 51 | Pr2GripperController::~Pr2GripperController() 52 | { 53 | sub_command_.shutdown(); 54 | } 55 | 56 | bool Pr2GripperController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) 57 | { 58 | assert(robot); 59 | node_ = n; 60 | robot_ = robot; 61 | 62 | std::string joint_name; 63 | if (!node_.getParam("joint", joint_name)) { 64 | ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str()); 65 | return false; 66 | } 67 | if (!(joint_state_ = robot_->getJointState(joint_name))) 68 | { 69 | ROS_ERROR("Could not find joint named \"%s\" (namespace: %s)", 70 | joint_name.c_str(), node_.getNamespace().c_str()); 71 | return false; 72 | } 73 | if (joint_state_->joint_->type != urdf::Joint::PRISMATIC) 74 | { 75 | ROS_ERROR("The joint \"%s\" was not prismatic (namespace: %s)", 76 | joint_name.c_str(), node_.getNamespace().c_str()); 77 | return false; 78 | } 79 | 80 | if (!joint_state_->calibrated_) 81 | { 82 | ROS_ERROR("Joint %s is not calibrated (namespace: %s)", 83 | joint_state_->joint_->name.c_str(), node_.getNamespace().c_str()); 84 | return false; 85 | } 86 | 87 | if (!pid_.init(ros::NodeHandle(node_, "pid"))) 88 | return false; 89 | 90 | controller_state_publisher_.reset( 91 | new realtime_tools::RealtimePublisher 92 | (node_, "state", 1)); 93 | 94 | sub_command_ = node_.subscribe( 95 | "command", 1, &Pr2GripperController::commandCB, this); 96 | 97 | return true; 98 | } 99 | 100 | void Pr2GripperController::update() 101 | { 102 | if (!joint_state_->calibrated_) 103 | return; 104 | 105 | assert(robot_ != NULL); 106 | double error(0); 107 | ros::Time time = robot_->getTime(); 108 | assert(joint_state_->joint_); 109 | ros::Duration dt = time - last_time_; 110 | 111 | pr2_controllers_msgs::Pr2GripperCommandConstPtr command; 112 | command_box_.get(command); 113 | assert(command); 114 | 115 | // Computes the position error 116 | error = command->position - joint_state_->position_; 117 | 118 | // Sets the effort (limited) 119 | double effort = pid_.computeCommand(error, 0.0 - joint_state_->velocity_, dt); 120 | if (command->max_effort >= 0.0) 121 | { 122 | effort = std::max(-command->max_effort, std::min(effort, command->max_effort)); 123 | } 124 | joint_state_->commanded_effort_ = effort; 125 | 126 | if(loop_count_ % 10 == 0) 127 | { 128 | if(controller_state_publisher_ && controller_state_publisher_->trylock()) 129 | { 130 | controller_state_publisher_->msg_.header.stamp = time; 131 | controller_state_publisher_->msg_.set_point = command->position; 132 | controller_state_publisher_->msg_.process_value = joint_state_->position_; 133 | controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_; 134 | controller_state_publisher_->msg_.error = error; 135 | controller_state_publisher_->msg_.time_step = dt.toSec(); 136 | controller_state_publisher_->msg_.command = effort; 137 | 138 | double dummy; 139 | pid_.getGains(controller_state_publisher_->msg_.p, 140 | controller_state_publisher_->msg_.i, 141 | controller_state_publisher_->msg_.d, 142 | controller_state_publisher_->msg_.i_clamp, 143 | dummy); 144 | controller_state_publisher_->unlockAndPublish(); 145 | } 146 | } 147 | loop_count_++; 148 | 149 | last_time_ = time; 150 | } 151 | 152 | void Pr2GripperController::commandCB(const pr2_controllers_msgs::Pr2GripperCommandConstPtr& msg) 153 | { 154 | command_box_.set(msg); 155 | } 156 | 157 | } 158 | --------------------------------------------------------------------------------