diff --git a/.alexignore b/.alexignore new file mode 100644 index 00000000..5a47f034 --- /dev/null +++ b/.alexignore @@ -0,0 +1,2 @@ +CHANGELOG.md +CONTRIBUTING.md \ No newline at end of file diff --git a/.flake8 b/.flake8 new file mode 100644 index 00000000..9ffbabe0 --- /dev/null +++ b/.flake8 @@ -0,0 +1,14 @@ +[flake8] +max-line-length = 89 +extend-ignore = E203 +exclude = + .git, + __pycache__, + sandbox, + docs, + tests, + franka_ros, + panda_moveit_config +per-file-ignores = + __init__.py: F401, E501 +max-complexity = 10 diff --git a/.github/renovate.json b/.github/renovate.json index 4f39080e..47a454e1 100644 --- a/.github/renovate.json +++ b/.github/renovate.json @@ -1,3 +1,4 @@ { - "extends": ["config:base"] + "extends": ["config:base"], + "labels": ["bump:patch"] } diff --git a/.husky/pre-commit b/.husky/pre-commit index 39a87b8c..00f24e5b 100755 --- a/.husky/pre-commit +++ b/.husky/pre-commit @@ -1,4 +1,4 @@ #!/bin/sh . "$(dirname "$0")/_/husky.sh" -node_modules/.bin/lint-staged && git add -A +node_modules/.bin/lint-staged && git add -A \ No newline at end of file diff --git a/.remarkignore b/.remarkignore new file mode 100644 index 00000000..1b763b1b --- /dev/null +++ b/.remarkignore @@ -0,0 +1 @@ +CHANGELOG.md diff --git a/CHANGELOG.md b/CHANGELOG.md index 55886c58..8f3caa7e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,8 @@ All notable changes to this project will be documented in this file. See [standard-version](https://github.com/conventional-changelog/standard-version) for commit guidelines. +### [1.0.18](https://github.com/rickstaa/panda-gazebo/compare/v1.0.17...v1.0.18) (2021-09-20) + ### [1.0.17](https://github.com/rickstaa/panda-gazebo/compare/v1.0.16...v1.0.17) (2021-09-13) ### [1.0.16](https://github.com/rickstaa/panda-gazebo/compare/v1.0.15...v1.0.16) (2021-09-13) diff --git a/TODO.md b/TODO.md deleted file mode 100644 index 8d80d51b..00000000 --- a/TODO.md +++ /dev/null @@ -1,3 +0,0 @@ -# TODOS - -- Remove MoveIt from rviz config when it is not used! diff --git a/contributing.md b/contributing.md new file mode 100644 index 00000000..891d7ed6 --- /dev/null +++ b/contributing.md @@ -0,0 +1,85 @@ +# Contributing + +When contributing to this repository, please first discuss the change you wish to make via issue, +email, or any other method with the owners of this repository before making a change. + +Please note we have a code of conduct, please follow it in all your interactions with the project. + +## Find projects to work on + +Projects that are open for contributions can be found on the [issue tab](https://github.com/rickstaa/panda-gazebo/) and will be labelled with the `help wanted` tag. Projects with the `beginner` label are ideal for beginning programmers. If you find a project that spikes your interest, leave a comment. We will then assign you to this project. After you did one project, you will be added as a contributor after which you can attach yourself to projects to work on. You can also submit a feature you would like to work on yourself by by opening [an issue](https://github.com/rickstaa/panda-gazebo/issues/new). + +## Forking process + +1. Fork the main [repository](https://github.com/rickstaa/panda-gazebo/) +2. Create your feature branch `git checkout -b feature/fooBar` +3. Commit your changes `git commit -am 'Add some fooBar'` +4. Push to the branch `git push origin feature/fooBar` +5. Create a new Pull Request + +## Commit instructions + +We use husky pre-commit hooks to ensure code quality. To enable these hooks please: + +1. Install [node](https://nodejs.org/en/download/package-manager/). +2. Install [python](https://www.python.org/downloads/). +3. Run `npm install -D` and `pip install .[dev]` to install husky and the required linters. +4. Run `npm run prepare` to setup the pre-commit hooks. + +## Pull Request Process + +1. Ensure any install or build dependencies are removed before the end of the layer when doing a build. +2. Update the README.md with details of changes to the interface, this includes new environment variables, exposed ports, useful file locations and container parameters. +3. Increase the version numbers in any examples files and the README.md to the new version that this Pull Request would represent. The versioning scheme we use is [SemVer](http://semver.org/). To prevent errors you are recommended to use the [standard-version](https://github.com/conventional-changelog/standard-version) tool. +4. Update the `CHANGELOG.md`. This is also done using the [standard-version](https://github.com/conventional-changelog/standard-version) tool. +5. You may merge the Pull Request in once you have the sign-off of two other developers, or if you do not have permission to do that, you may request the second reviewer to merge it for you. + +### Versioning instructions + +We use the [standard-version](https://github.com/conventional-changelog/standard-version) package for versioning. This tool will automatically increase the version and create a changelog by parsing your commits using the [conventional Commits](https://conventionalcommits.org/). This package can be invoked using the `npm run release` command. After you cut a release, you can push the new git tag using the `git push --follow-tags origin main` command. + +### Linting instructions + +We use [husky](https://github.com/typicode/husky) pre-commit hooks to perform some linting and formatting actions. These actions will automatically run when you create a commit. Apart from this also some linting actions are performed by github actions when you create a pull request. + +## Code of Conduct + +### Our Pledge + +In the interest of fostering an open and welcoming environment, we as contributors and maintainers pledge to making participation in our project and our community a harassment-free experience for everyone, regardless of age, body size, disability, ethnicity, gender identity and expression, level of experience, nationality, personal appearance, race, religion, or sexual identity and orientation. + +### Our Standards + +Examples of behaviour that contributes to creating a positive environment include: + +* Using welcoming and inclusive language +* Being respectful of differing viewpoints and experiences +* Gracefully accepting constructive criticism +* Focusing on what is best for the community +* Showing empathy towards other community members + +Examples of unacceptable behaviour by participants include: + +* The use of sexualized language or imagery and unwelcome sexual attention or advances +* Trolling, insulting/derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or electronic address, without explicit permission +* Other conduct which could reasonably be considered inappropriate in a professional setting + +### Our Responsibilities + +Project maintainers are responsible for clarifying the standards of acceptable behaviour and are expected to take appropriate and fair corrective action in response to any instances of unacceptable behaviour. + +Project maintainers have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, or to ban temporarily or permanently any contributor for other behaviours that they deem inappropriate, threatening, offensive, or harmful. + +### Scope + +This Code of Conduct applies both within project spaces and in public spaces when an individual is representing the project or its community. Examples of representing a project or community include using an official project e-mail address posting via an official social media account, or acting as an appointed representative at an online or offline event. Representation of a project may be further defined and clarified by project maintainers. + +### Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, available at [http://contributor-covenant.org/version/1/4][version] + +[homepage]: http://contributor-covenant.org + +[version]: http://contributor-covenant.org/version/1/4/ diff --git a/franka_ros b/franka_ros index 8772fea0..d8cd5fd0 160000 --- a/franka_ros +++ b/franka_ros @@ -1 +1 @@ -Subproject commit 8772fea0cb3416cb851df2d9242a616b07cf209b +Subproject commit d8cd5fd0c8988b878f2c427410c6f71a06d56d8d diff --git a/package.json b/package.json index 65b48a96..e2d56372 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "panda-gazebo", - "version": "1.0.17", + "version": "1.0.18", "description": "Gazebo panda simulation created for the Panda environment that is found in the 'openai_ros' ROS package.", "keywords": [ "reinforcement-learning", diff --git a/panda_gazebo/CMakeLists.txt b/panda_gazebo/CMakeLists.txt index c9632535..fc0a2b9b 100755 --- a/panda_gazebo/CMakeLists.txt +++ b/panda_gazebo/CMakeLists.txt @@ -13,8 +13,8 @@ find_package(catkin REQUIRED COMPONENTS std_msgs message_generation geometry_msgs - trajectory_msgs actionlib_msgs + trajectory_msgs control_msgs dynamic_reconfigure ) @@ -70,6 +70,8 @@ add_service_files( GetRandomEePose.srv GetRandomJointPositions.srv SetEePose.srv + SetGripperWidth.srv + SetJointCommands.srv SetJointEfforts.srv SetJointPositions.srv ) @@ -128,7 +130,7 @@ catkin_package( CATKIN_DEPENDS message_runtime rospy gazebo_msgs std_msgs controller_manager_msgs geometry_msgs actionlib_msgs trajectory_msgs sensor_msgs control_msgs rviz actionlib - franka_description panda_moveit_config + franka_description franka_gazebo panda_moveit_config # DEPENDS system_lib ) diff --git a/panda_gazebo/action/FollowJointTrajectory.action b/panda_gazebo/action/FollowJointTrajectory.action index 63b41252..92cf16ef 100644 --- a/panda_gazebo/action/FollowJointTrajectory.action +++ b/panda_gazebo/action/FollowJointTrajectory.action @@ -1,8 +1,6 @@ # Follow_joint_trajectory action -# NOTE: This action is a direct copy of the action found in the control_msgs package. It -# was created to provide a better distinction between the messages of the -# panda_gazebo/follow_joint_trajectory action server and the -# panda_arm_controller/follow_joint_trajectory action serer. +# NOTE: This action extends the control_msgs/FollowJointTrajectoryAction such that +# it also contains values to automatically generate the time axis # The joint trajectory to follow trajectory_msgs/JointTrajectory trajectory @@ -30,6 +28,9 @@ control_msgs/JointTolerance[] path_tolerance # GOAL_TOLERANCE_VIOLATED control_msgs/JointTolerance[] goal_tolerance duration goal_time_tolerance + +# Extra time axis attributes +# NOTE: Used for automatic time axis generation bool create_time_axis # Automatically create a time axis float64 time_axis_step # Time axis step size diff --git a/panda_gazebo/cfg/_cfg/parms_config.yaml b/panda_gazebo/cfg/_cfg/parms_config.yaml deleted file mode 100644 index ad8d34e7..00000000 --- a/panda_gazebo/cfg/_cfg/parms_config.yaml +++ /dev/null @@ -1,36 +0,0 @@ -# This configuration file contains a number of parameters that are used in multiple -# panda_openai_sim scripts. - -robot_control_types: # The currently supported control types - arm: - - "trajectory_control" - - "position_control" - - "effort_control" - - "ee_control" - hand: - - "trajectory_control" - - "position_control" - - "effort_control" - -panda_joints: # The current robot joints - arm: - - "panda_joint1" - - "panda_joint2" - - "panda_joint3" - - "panda_joint4" - - "panda_joint5" - - "panda_joint6" - - "panda_joint7" - hand: - - "panda_finger_joint1" - - "panda_finger_joint2" - both: - - "panda_finger_joint1" - - "panda_finger_joint2" - - "panda_joint1" - - "panda_joint2" - - "panda_joint3" - - "panda_joint4" - - "panda_joint5" - - "panda_joint6" - - "panda_joint7" diff --git a/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml b/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml index cf685b3e..acf8a9e2 100644 --- a/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml +++ b/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml @@ -10,6 +10,23 @@ panda_arm_controller: - panda_joint5 - panda_joint6 - panda_joint7 + constraints: + goal_time: 0.5 + stopped_velocity_tolerance: 0 # Added because of wrong PID gains see #9 + panda_joint1: + goal: 0.05 + panda_joint2: + goal: 0.05 + panda_joint3: + goal: 0.05 + panda_joint4: + goal: 0.05 + panda_joint5: + goal: 0.05 + panda_joint6: + goal: 0.05 + panda_joint7: + goal: 0.05 gains: panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 } panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 } @@ -18,38 +35,3 @@ panda_arm_controller: panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 } panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 } panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 } - constraints: - goal_time: 2.0 - state_publish_rate: 25 - -panda_hand_controller: - type: effort_controllers/JointTrajectoryController - joints: - - panda_finger_joint1 - - panda_finger_joint2 - gains: - panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 } - panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 } - state_publish_rate: 25 - -# Make controllers visible to MoveIt -controller_list: - - name: effort_panda_arm_trajectory_controller - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - - name: effort_panda_hand_trajectory_controller - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - panda_finger_joint1 - - panda_finger_joint2 diff --git a/panda_gazebo/cfg/controllers/joint_effort_controllers.yaml b/panda_gazebo/cfg/controllers/joint_effort_controllers.yaml index 7fd3b828..54b05821 100644 --- a/panda_gazebo/cfg/controllers/joint_effort_controllers.yaml +++ b/panda_gazebo/cfg/controllers/joint_effort_controllers.yaml @@ -20,11 +20,4 @@ panda_arm_joint6_effort_controller: joint: panda_joint6 panda_arm_joint7_effort_controller: type: effort_controllers/JointEffortController - joint: panda_joint7 - -panda_hand_finger1_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_finger_joint1 -panda_hand_finger2_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_finger_joint2 + joint: panda_joint7 \ No newline at end of file diff --git a/panda_gazebo/cfg/controllers/joint_position_controllers.yaml b/panda_gazebo/cfg/controllers/joint_position_controllers.yaml index 3c5f1a32..9f53ddc9 100644 --- a/panda_gazebo/cfg/controllers/joint_position_controllers.yaml +++ b/panda_gazebo/cfg/controllers/joint_position_controllers.yaml @@ -21,10 +21,3 @@ panda_arm_joint6_position_controller: panda_arm_joint7_position_controller: type: position_controllers/JointPositionController joint: panda_joint7 - -panda_hand_finger1_position_controller: - type: position_controllers/JointPositionController - joint: panda_finger_joint1 -panda_hand_finger2_position_controller: - type: position_controllers/JointPositionController - joint: panda_finger_joint2 diff --git a/panda_gazebo/cfg/controllers/joint_state_controller.yaml b/panda_gazebo/cfg/controllers/joint_state_controller.yaml deleted file mode 100755 index dd439f25..00000000 --- a/panda_gazebo/cfg/controllers/joint_state_controller.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# A configuration file that contains the configuration values for creating a joint state -# controller which can be used to publish the panda robot joint states. -# NOTE: Creates the /joint_states topic necessary in ROS -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 diff --git a/panda_gazebo/cfg/controllers/joint_trajectory_controllers.yaml b/panda_gazebo/cfg/controllers/joint_trajectory_controllers.yaml deleted file mode 100644 index 668cfd94..00000000 --- a/panda_gazebo/cfg/controllers/joint_trajectory_controllers.yaml +++ /dev/null @@ -1,45 +0,0 @@ -# Configuration file that contains the configuration values for setting up the panda -# trajectory controllers -panda_arm_controller: - type: position_controllers/JointTrajectoryController - joints: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - constraints: - goal_time: 2.0 - state_publish_rate: 25 - -panda_hand_controller: - type: position_controllers/JointTrajectoryController - joints: - - panda_finger_joint1 - - panda_finger_joint2 - - state_publish_rate: 25 - -# Make controllers visible to Moveit -controller_list: - - name: panda_arm_controller - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - - name: panda_hand_controller - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - panda_finger_joint1 - - panda_finger_joint2 diff --git a/panda_gazebo/cfg/controllers/moveit.yaml b/panda_gazebo/cfg/controllers/moveit.yaml new file mode 100644 index 00000000..57517c5e --- /dev/null +++ b/panda_gazebo/cfg/controllers/moveit.yaml @@ -0,0 +1,11 @@ +# Configuration that contains the general MoveIt configuration values +# MoveIt-specific simulation settings +move_group: + moveit_sim_hw_interface: + joint_model_group: panda_arm + joint_model_group_pose: ready + # Increase MoveIt execution time limit + # NOTE: Needed because PID gains are not well tuned see #9 + trajectory_execution: + allowed_execution_duration_scaling: 5.0 + # allowed_goal_duration_margin: 2.5 diff --git a/panda_gazebo/cfg/controllers/moveit_ros_control.yaml b/panda_gazebo/cfg/controllers/moveit_ros_control.yaml index 675a18fb..e03790c8 100644 --- a/panda_gazebo/cfg/controllers/moveit_ros_control.yaml +++ b/panda_gazebo/cfg/controllers/moveit_ros_control.yaml @@ -1,4 +1,16 @@ -# MoveIt-specific simulation settings -moveit_sim_hw_interface: - joint_model_group: controllers_initial_group_ - joint_model_group_pose: controllers_initial_pose_ +# Configuration that contains the MoveIt config values for when the hand is attached +# Make controllers visible to MoveIt +move_group: + controller_list: + - name: panda_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 diff --git a/panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml b/panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml new file mode 100644 index 00000000..a105c4d7 --- /dev/null +++ b/panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml @@ -0,0 +1,23 @@ +# Configuration that contains the MoveIt config values for when the hand is attached +# Make controllers visible to MoveIt +move_group: + controller_list: + - name: panda_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + - name: franka_gripper + action_ns: gripper_action + type: GripperCommand + default: true + joints: + - panda_finger_joint1 + - panda_finger_joint2 diff --git a/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml b/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml new file mode 100644 index 00000000..57764157 --- /dev/null +++ b/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml @@ -0,0 +1,36 @@ +# Configuration file that contains the configuration values for setting up the panda +# trajectory controllers +panda_arm_controller: + type: position_controllers/JointTrajectoryController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + constraints: + goal_time: 0.5 + panda_joint1: + goal: 0.05 + panda_joint2: + goal: 0.05 + panda_joint3: + goal: 0.05 + panda_joint4: + goal: 0.05 + panda_joint5: + goal: 0.05 + panda_joint6: + goal: 0.05 + panda_joint7: + goal: 0.05 + gains: + panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 } + panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 } + panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 } + panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 } + panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 } + panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 } + panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 } diff --git a/panda_gazebo/cfg/controllers/ros_control.yaml b/panda_gazebo/cfg/controllers/ros_control.yaml index cc81af41..c2358a29 100644 --- a/panda_gazebo/cfg/controllers/ros_control.yaml +++ b/panda_gazebo/cfg/controllers/ros_control.yaml @@ -1,16 +1,4 @@ -# Settings for ros_control control loop +# Settings for ros_control_boilerplate control loop generic_hw_control_loop: loop_hz: 300 cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: - joints: - - panda_joint1 - - panda_joint2 - - panda_joint3 - - panda_joint4 - - panda_joint5 - - panda_joint6 - - panda_joint7 - - panda_finger_joint1 - sim_control_mode: 1 # 0: position, 1: velocity diff --git a/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg b/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg index 412b0268..593a1a24 100755 --- a/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg +++ b/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg @@ -9,33 +9,33 @@ gen = ParameterGenerator() # Generate arm effort control parameters arm = gen.add_group("arm") -arm.add("joint1_effort", double_t, 0, "Joint 1 effort control command", 0, -100, 100) -arm.add("joint2_effort", double_t, 1, "Joint 2 effort control command", 0, -100, 100) -arm.add("joint3_effort", double_t, 2, "Joint 3 effort control command", 0, -100, 100) -arm.add("joint4_effort", double_t, 3, "Joint 4 effort control command", 0, -100, 100) -arm.add("joint5_effort", double_t, 4, "Joint 5 effort control command", 0, -100, 100) -arm.add("joint6_effort", double_t, 5, "Joint 6 effort control command", 0, -100, 100) -arm.add("joint7_effort", double_t, 6, "Joint 7 effort control command", 0, -100, 100) +arm.add("joint1_effort", double_t, 0, "Joint 1 effort control command", 0, -87.0, 87.0) +arm.add("joint2_effort", double_t, 1, "Joint 2 effort control command", 0, -87.0, 87.0) +arm.add("joint3_effort", double_t, 2, "Joint 3 effort control command", 0, -87.0, 87.0) +arm.add("joint4_effort", double_t, 3, "Joint 4 effort control command", 0, -87.0, 87.0) +arm.add("joint5_effort", double_t, 4, "Joint 5 effort control command", 0, -12.0, 12.0) +arm.add("joint6_effort", double_t, 5, "Joint 6 effort control command", 0, -12.0, 12.0) +arm.add("joint7_effort", double_t, 6, "Joint 7 effort control command", 0, -12.0, 12.0) # Generate hand effort control parameters hand = gen.add_group("hand") hand.add( - "finger_joint1_effort", + "width", double_t, 7, - "Finger joint 1 effort control command", + "Gripper width", 0, - -2, - 2, + 0.0, + 0.08, ) hand.add( - "finger_joint2_effort", + "speed", double_t, 8, - "Finger joint 2 effort control command", - 0, - -2, - 2, + "Gripper speed", + 0.2, + 0.0, + 0.2, ) # Generate the necessary files and exit the program diff --git a/panda_gazebo/cfg/dyn_reconf/MoveitServerDynReconf.cfg b/panda_gazebo/cfg/dyn_reconf/MoveitServerDynReconf.cfg index fa22e5c3..cf166596 100755 --- a/panda_gazebo/cfg/dyn_reconf/MoveitServerDynReconf.cfg +++ b/panda_gazebo/cfg/dyn_reconf/MoveitServerDynReconf.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# NOTE: Config for changeing the parametesr of the panda_moveit_control node +# NOTE: Config for changeing the parameters of the panda_moveit_control node. from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t diff --git a/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg b/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg index e64729b0..143f858a 100755 --- a/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg +++ b/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg @@ -9,33 +9,33 @@ gen = ParameterGenerator() # Generate arm position control parameters arm = gen.add_group("arm") -arm.add("joint1_position", double_t, 0, "Joint 1 position control command", 0, -3, 3) -arm.add("joint2_position", double_t, 1, "Joint 2 position control command", 0, -3, 3) -arm.add("joint3_position", double_t, 2, "Joint 3 position control command", 0, -3, 3) -arm.add("joint4_position", double_t, 3, "Joint 4 position control command", 0, -3, 3) -arm.add("joint5_position", double_t, 4, "Joint 5 position control command", 0, -3, 3) -arm.add("joint6_position", double_t, 5, "Joint 6 position control command", 0, -3, 3) -arm.add("joint7_position", double_t, 6, "Joint 7 position control command", 0, -3, 3) +arm.add("joint1_position", double_t, 0, "Joint 1 position control command", 0, -2.8973, 2.8973) +arm.add("joint2_position", double_t, 1, "Joint 2 position control command", 0, -1.7628, 1.7628) +arm.add("joint3_position", double_t, 2, "Joint 3 position control command", 0, -2.8973, 2.8973) +arm.add("joint4_position", double_t, 3, "Joint 4 position control command", 0, -3.0718, -0.0698) +arm.add("joint5_position", double_t, 4, "Joint 5 position control command", 0, -2.8973, 2.8973) +arm.add("joint6_position", double_t, 5, "Joint 6 position control command", 0, -0.0175, 3.7525) +arm.add("joint7_position", double_t, 6, "Joint 7 position control command", 0, -2.8973, 2.8973) # Generate hand position control parameters hand = gen.add_group("hand") hand.add( - "finger_joint1_position", + "width", double_t, 7, - "Finger joint 1 position control command", + "Gripper width", 0, - -0.05, - 0.05, + 0.0, + 0.08, ) hand.add( - "finger_joint2_position", + "speed", double_t, 8, - "Finger joint 2 position control command", - 0, - -0.05, - 0.05, + "Gripper speed", + 0.2, + 0.0, + 0.2, ) # Generate the necessary files and exit the program diff --git a/panda_gazebo/cfg/env_config.yaml b/panda_gazebo/cfg/env_config.yaml deleted file mode 100644 index 6468abc0..00000000 --- a/panda_gazebo/cfg/env_config.yaml +++ /dev/null @@ -1,157 +0,0 @@ -# This config file contains the configuration values that are used by the Panda task, -# robot and Gazebo goal environments. Variables marked with the (DEFAULT) tag are -# overloaded by PandaTaskEnv constructor arguments. - -######################################### -# Action space parameters ############## -######################################### -action_space: - use_gripper_width: True # (DEFAULT) - Whether to use gripper_width instead of individual finger joint commands - bounds: - ee_pose: - low: - x: -1.3 - y: -1.3 - z: 0.0 - rx: 0 - ry: 0 - rz: 0 - rw: 0 - high: - x: 1.3 - y: 1.3 - z: 1.3 - rx: 1 - ry: 1 - rz: 1 - rw: 1 - joint_positions: # NOTE: The position and effort limits can be found in the robot urdf file (see franka_description). - low: - panda_joint1: -2.8973 - panda_joint2: -1.7628 - panda_joint3: -2.8973 - panda_joint4: -3.0718 - panda_joint5: -2.8973 - panda_joint6: -0.0175 - panda_joint7: -2.8973 - panda_finger_joint1: 0.0 - panda_finger_joint2: 0.0 - gripper_width: 0.0 - high: - panda_joint1: 2.8973 - panda_joint2: 1.7628 - panda_joint3: 2.8973 - panda_joint4: -0.0698 - panda_joint5: 2.8973 - panda_joint6: 3.7525 - panda_joint7: 2.8973 - panda_finger_joint1: 0.04 - panda_finger_joint2: 0.04 - gripper_width: 0.2 - joint_efforts: - low: - panda_joint1: -87 - panda_joint2: -87 - panda_joint3: -87 - panda_joint4: -87 - panda_joint5: -12 - panda_joint6: -12 - panda_joint7: -12 - panda_finger_joint1: -20 - panda_finger_joint2: -20 - high: - panda_joint1: 87 - panda_joint2: 87 - panda_joint3: 87 - panda_joint4: 87 - panda_joint5: 12 - panda_joint6: 12 - panda_joint7: 12 - panda_finger_joint1: 20 - panda_finger_joint2: 20 - -######################################### -# Training related parameters ########### -######################################### -training: - # Main training parameters - reward_type: "Sparse" # (DEFAULT) - The reward type used in the reward function ("sparse" or "dense"). - distance_threshold: 0.05 # The threshold for determining a goal has been reached - has_object: False # (DEFAULT) - Train with object - - # Settings for setting up the object - object_sampling: - distance_threshold: 0.1 # Threshold for determining whether new object pose is far enough from old obj pose - init_obj_pose: # (DEFAULT) - The initial pose of the grasp object. - x: 0.4 - y: 0.0 - z: 0.0 - rx: 0.0 - ry: 0.0 - rz: 0.0 - rw: 1.0 - visualize_bounds: True # Turn object sampling region visualization on and off - bounds: # (DEFAULT) - The bounds that define the region, rel. to the 'init_obj_pose' in which the object pose is sampled. - x_min: -0.7 - x_max: 0.7 - y_min: -0.7 - y_max: 0.7 - - # Settings for sampling of the target (Goal position) - target_sampling: - # Main settings - strategy: "global" # Global or local goal target sampling - target_in_the_air: False # (DEFAULT) - Whether the object place target can be in the air - visualize_target: True # Turn on and off rviz target visualization - visualize_bounds: True # Turn target sampling region visualization on and off - target_offset: # (DEFAULT) - An additional offset added to the target - x: 0.0 - y: 0.0 - z: 0.0 - - # Target sampling bounds - bounds: # (DEFAULT) - The bounds that define the region in which the target pose is sampled - global: # The global sample bounds relative to the world frame - x_min: -0.7 - x_max: 0.7 - y_min: -0.7 - y_max: 0.7 - z_min: 0.0 - z_max: 1.3 - local: # The target sample bounds relative to the current ee_pose. - x_min: -0.15 - x_max: 0.15 - y_min: -0.15 - y_max: 0.15 - z_min: -0.15 - z_max: 0.15 - -######################################### -# Simulation parameters ################# -######################################### -simulation: - # Main simulation settings - reset_robot_pose: True # Whether we want to reset the pose when the environment is reset - - # Control settings - control: - robot_arm_control_type: "position_control" # (DEFAULT) - The type of control - robot_hand_control_type: "position_control" # (DEFAULT) - The type of control - block_gripper: False # (DEFAULT) - Block the gripper control - ee_link: "panda_hand" # Link that is specified as the end effector. Used for the distance to the target position. - gripper_extra_height: 0.10 # (DEFAULT) - Add extra height - - # Initial pose given in generalized coordinates (q) - init_pose_sampling: - random_init_pose: True # Use random pose when resetting the simulation - randomize_first_episode: True # Also randomize the pose in the first episode - visualize_bounds: True # Turn init pose sampling region visualization on and off - init_robot_pose: # (DEFAULT) - The initial robot pose - x: 0.23 - y: 0.29 - z: 0.35 - rx: 0.78 - ry: 0.62 - rz: -0.0 - rw: 4.42 - gripper_width: 0.0 diff --git a/panda_gazebo/docs/source/conf.py b/panda_gazebo/docs/source/conf.py index 50c929b0..93914fe9 100644 --- a/panda_gazebo/docs/source/conf.py +++ b/panda_gazebo/docs/source/conf.py @@ -67,9 +67,9 @@ # built documents. # # The short X.Y version. -version = "1.0.17" +version = "1.0.18" # The full version, including alpha/beta/rc tags. -release = "1.0.17" +release = "1.0.18" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. diff --git a/panda_gazebo/launch/includes/rviz/moveit.rviz b/panda_gazebo/launch/includes/rviz/moveit.rviz deleted file mode 100644 index 89a646b0..00000000 --- a/panda_gazebo/launch/includes/rviz/moveit.rviz +++ /dev/null @@ -1,305 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 84 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.7425600290298462 - Tree Height: 362 - - Class: rviz/Help - Name: Help - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Acceleration_Scaling_Factor: 1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: true - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Planning Group: panda_arm_hand - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: move_group/monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 1 - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: panda_link0 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - Value: true - Views: - Current: - Class: rviz/XYOrbit - Distance: 2.996500015258789 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.11356700211763382 - Y: 0.10592000186443329 - Z: 2.2351800055275817e-07 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5097969770431519 - Target Frame: panda_link0 - Yaw: 5.659949779510498 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 995 - Help: - collapsed: false - Hide Left Dock: false - Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a200000389fc0200000007fb000000100044006900730070006c006100790073010000003d000001fb000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000023e000001880000017d00ffffff000004d80000038900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/panda_gazebo/launch/includes/rviz/panda.rviz b/panda_gazebo/launch/includes/rviz/panda.rviz index 61cc6487..d8ecdf6e 100644 --- a/panda_gazebo/launch/includes/rviz/panda.rviz +++ b/panda_gazebo/launch/includes/rviz/panda.rviz @@ -3,8 +3,7 @@ Panels: Help Height: 84 Name: Displays Property Tree Widget: - Expanded: - - /MotionPlanning1 + Expanded: ~ Splitter Ratio: 0.7425600290298462 Tree Height: 760 - Class: rviz/Help @@ -39,217 +38,22 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.2 - Class: moveit_rviz_plugin/MotionPlanning + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Planning Group: panda_arm - Query Goal State: false - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: move_group/monitored_planning_scene + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Name: RobotModel Robot Description: robot_description - Scene Geometry: - Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true + TF Prefix: "" + Update Interval: 0 Value: true - Velocity_Scaling_Factor: 0.2 + Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -294,11 +98,7 @@ Window Geometry: collapsed: false Hide Left Dock: false Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a200000389fc0200000007fb000000100044006900730070006c006100790073010000003d00000389000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000023e000001880000017d00ffffff000004d80000038900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000002a200000389fc0200000007fb000000100044006900730070006c006100790073010000003d00000389000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000023e000001880000000000000000000004d80000038900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1920 diff --git a/panda_gazebo/launch/includes/rviz/panda_moveit.rviz b/panda_gazebo/launch/includes/rviz/panda_moveit.rviz index 89a646b0..0dae41e4 100644 --- a/panda_gazebo/launch/includes/rviz/panda_moveit.rviz +++ b/panda_gazebo/launch/includes/rviz/panda_moveit.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.7425600290298462 - Tree Height: 362 + Tree Height: 760 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -125,7 +125,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true panda_rightfinger: Alpha: 1 Show Axes: false @@ -135,7 +134,7 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Loop Animation: true + Loop Animation: false Robot Alpha: 0.5 Robot Color: 150; 50; 150 Show Robot Collision: false @@ -157,8 +156,8 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: panda_arm_hand - Query Goal State: true + Planning Group: panda_arm + Query Goal State: false Query Start State: false Show Workspace: false Start State Alpha: 1 @@ -234,7 +233,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true panda_rightfinger: Alpha: 1 Show Axes: false @@ -244,7 +242,7 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Robot Alpha: 0.5 + Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true Value: true @@ -297,7 +295,7 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002a200000389fc0200000007fb000000100044006900730070006c006100790073010000003d000001fb000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000023e000001880000017d00ffffff000004d80000038900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000002a200000389fc0200000007fb000000100044006900730070006c006100790073010000003d00000389000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000023e000001880000017d00ffffff000004d80000038900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1920 diff --git a/panda_gazebo/launch/load_controllers.launch b/panda_gazebo/launch/load_controllers.launch new file mode 100644 index 00000000..1c6dedbe --- /dev/null +++ b/panda_gazebo/launch/load_controllers.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/panda_gazebo/launch/load_effort_joint_trajectory_controllers.launch b/panda_gazebo/launch/load_effort_joint_trajectory_controllers.launch index e57868e9..47a5bab7 100755 --- a/panda_gazebo/launch/load_effort_joint_trajectory_controllers.launch +++ b/panda_gazebo/launch/load_effort_joint_trajectory_controllers.launch @@ -1,29 +1,13 @@ - - - - - - - - - - - - - - - - - - + + \ No newline at end of file diff --git a/panda_gazebo/launch/load_joint_effort_controllers.launch b/panda_gazebo/launch/load_joint_effort_controllers.launch index ec7b6f67..8c07bdf8 100755 --- a/panda_gazebo/launch/load_joint_effort_controllers.launch +++ b/panda_gazebo/launch/load_joint_effort_controllers.launch @@ -1,30 +1,13 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/panda_gazebo/launch/load_joint_position_controllers.launch b/panda_gazebo/launch/load_joint_position_controllers.launch index 3db26aac..a8ccd0c9 100755 --- a/panda_gazebo/launch/load_joint_position_controllers.launch +++ b/panda_gazebo/launch/load_joint_position_controllers.launch @@ -1,30 +1,13 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/panda_gazebo/launch/load_joint_trajectory_controllers.launch b/panda_gazebo/launch/load_joint_trajectory_controllers.launch deleted file mode 100755 index c54ec014..00000000 --- a/panda_gazebo/launch/load_joint_trajectory_controllers.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/panda_gazebo/launch/load_position_joint_trajectory_controllers.launch b/panda_gazebo/launch/load_position_joint_trajectory_controllers.launch new file mode 100755 index 00000000..4ff328d2 --- /dev/null +++ b/panda_gazebo/launch/load_position_joint_trajectory_controllers.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/panda_gazebo/launch/put_robot_in_world.launch b/panda_gazebo/launch/put_robot_in_world.launch index 7186dedf..c40f9afc 100644 --- a/panda_gazebo/launch/put_robot_in_world.launch +++ b/panda_gazebo/launch/put_robot_in_world.launch @@ -1,118 +1,72 @@ - - - - - + + + + - - - - - + + + + + + + - - + + - - - - - - - - - - [/joint_states] - [/move_group/fake_controller_joint_states] - - - - - - - - + + + + + + + + + - - - + - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - + + + + + + - - + + + - - + + - + - + \ No newline at end of file diff --git a/panda_gazebo/launch/start_simulation.launch b/panda_gazebo/launch/start_simulation.launch index 401a5c0f..3f9ac48a 100755 --- a/panda_gazebo/launch/start_simulation.launch +++ b/panda_gazebo/launch/start_simulation.launch @@ -8,9 +8,7 @@ - - - + diff --git a/panda_gazebo/launch/start_world.launch b/panda_gazebo/launch/start_world.launch index 9e0fda9a..cd8b961f 100644 --- a/panda_gazebo/launch/start_world.launch +++ b/panda_gazebo/launch/start_world.launch @@ -3,8 +3,8 @@ + - diff --git a/panda_gazebo/nodes/panda_control_server.py b/panda_gazebo/nodes/panda_control_server.py index e70db9db..650c216f 100755 --- a/panda_gazebo/nodes/panda_control_server.py +++ b/panda_gazebo/nodes/panda_control_server.py @@ -22,14 +22,9 @@ autofill_traj_positions = rospy.get_param("~autofill_traj_positions") except KeyError: autofill_traj_positions = False - try: # Check required services - create_extra_services = rospy.get_param("~create_extra_services") - except KeyError: - create_extra_services = False # Start control server control_server = PandaControlServer( autofill_traj_positions=autofill_traj_positions, - create_extra_services=create_extra_services, ) rospy.spin() # Maintain the service open diff --git a/panda_gazebo/nodes/panda_moveit_server.py b/panda_gazebo/nodes/panda_moveit_server.py index e853f2b5..56f17fb4 100755 --- a/panda_gazebo/nodes/panda_moveit_server.py +++ b/panda_gazebo/nodes/panda_moveit_server.py @@ -21,15 +21,14 @@ try: arm_ee_link = rospy.get_param("~end_effector") except KeyError: - # arm_ee_link = "panda_hand" arm_ee_link = "panda_link8" - try: # Check required services - create_extra_services = rospy.get_param("~create_extra_services") + try: + load_gripper = rospy.get_param("~load_gripper") except KeyError: - create_extra_services = False + load_gripper = True # Start MoveIt planner server moveit_planner_server = PandaMoveitPlannerServer( - arm_ee_link=arm_ee_link, create_extra_services=create_extra_services + arm_ee_link=arm_ee_link, load_gripper=load_gripper ) rospy.spin() # Maintain the service open diff --git a/panda_gazebo/package.xml b/panda_gazebo/package.xml index 19aa22ae..3d6aa439 100755 --- a/panda_gazebo/package.xml +++ b/panda_gazebo/package.xml @@ -1,7 +1,7 @@ panda_gazebo - 1.0.17 + 1.0.18 A package that allows you to train several ROS based robots using Gazebo. @@ -92,11 +92,10 @@ franka_description + franka_gazebo panda_moveit_config - gym-pip - python3-yaml diff --git a/panda_gazebo/resources/documents/controller_start_commands.md b/panda_gazebo/resources/documents/controller_start_commands.md deleted file mode 100644 index f4bab5ba..00000000 --- a/panda_gazebo/resources/documents/controller_start_commands.md +++ /dev/null @@ -1,105 +0,0 @@ -# Controller start commands - -Here you find some ROS commands which can be used to start the controllers. These commands use the `controller_manager/switch_controllers` service. - -## PANDA ARM - -### Start Panda arm position controllers stop panda_arm_controller - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_arm_joint1_position_controller', 'panda_arm_joint2_position_controller', 'panda_arm_joint3_position_controller', 'panda_arm_joint4_position_controller', -'panda_arm_joint5_position_controller', 'panda_arm_joint6_position_controller', 'panda_arm_joint7_position_controller'] -stop_controllers: ['panda_arm_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -### Start panda_arm_controller and stop Panda arm position controllers - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_arm_controller'] -stop_controllers: ['panda_arm_joint1_position_controller', 'panda_arm_joint2_position_controller', 'panda_arm_joint3_position_controller', 'panda_arm_joint4_position_controller', -'panda_arm_joint5_position_controller', 'panda_arm_joint6_position_controller', 'panda_arm_joint7_position_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -### Start Panda arm joint effort controllers and stop panda_arm_controller - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_arm_joint1_effort_controller', 'panda_arm_joint2_effort_controller', 'panda_arm_joint3_effort_controller', 'panda_arm_joint4_effort_controller', -'panda_arm_joint5_effort_controller', 'panda_arm_joint6_effort_controller', 'panda_arm_joint7_effort_controller'] -stop_controllers: ['panda_arm_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -### Start Panda arm position group controller - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_arm_joint_group_position_controller'] -stop_controllers: ['panda_arm_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -### Start Panda arm effort group controller - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_arm_joint_group_effort_controller'] -stop_controllers: ['panda_arm_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -## PANDA HAND - -### Start Panda hand position controllers - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_hand_finger1_position_controller', 'panda_hand_finger2_position_controller'] -stop_controllers: ['panda_hand_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -### Start Panda hand effort controllers - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_hand_finger1_effort_controller', 'panda_hand_finger2_effort_controller'] -stop_controllers: ['panda_hand_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -### Start Panda hand position group controller - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_hand_joint_group_position_controller'] -stop_controllers: ['panda_hand_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -### Start Panda hand effort group controller - -```bash -rosservice call /controller_manager/switch_controller "start_controllers: ['panda_hand_joint_group_effort_controller'] -stop_controllers: ['panda_hand_controller'] -strictness: 0 -start_asap: false -timeout: 0.0" -``` - -## Other commands - -- List controllers: `rosrun controller_manager controller_manager list` -- Controller gui: `rosrun rqt_controller_manager rqt_controller_manager` diff --git a/panda_gazebo/resources/documents/endeffector-config.json b/panda_gazebo/resources/documents/endeffector-config.json deleted file mode 100644 index bfe2104f..00000000 --- a/panda_gazebo/resources/documents/endeffector-config.json +++ /dev/null @@ -1,23 +0,0 @@ -{ - "mass": 0.73, - "centerOfMass": [-0.01, 0, 0.03], - "transformation": [ - 0.7071, - -0.7071, - 0, - 0, - 0.7071, - 0.7071, - 0, - 0, - 0, - 0, - 1, - 0, - 0, - 0, - 0.1034, - 1 - ], - "inertia": [0.001, 0, 0, 0, 0.0025, 0, 0, 0, 0.0017] -} diff --git a/panda_gazebo/resources/documents/geometrical_calculation.md b/panda_gazebo/resources/documents/geometrical_calculation.md deleted file mode 100644 index eb7e9be0..00000000 --- a/panda_gazebo/resources/documents/geometrical_calculation.md +++ /dev/null @@ -1,55 +0,0 @@ -# Geometrical calculations - -A guide on how to calculate geometrical properties like moment of inertial, center of gravity and mass can be found [here](http://gazebosim.org/tutorials?tut=inertia&cat=build_robot). For our robot, this was already done by [@mkrizmancic](https://github.com/mkrizmancic/franka_gazebo). Here a small validation. - -## Robot specs - -A data-sheet for the robot is published [here](https://s3-eu-central-1.amazonaws.com/franka-de-uploads-staging/uploads/2018/05/2018-05-datasheet-panda.pdf). It states that the total mass of the ar is ~18 kg while the mass of the hand is ~0.7 kg. - -### Arm - -#### Volumes - -- j0: `2.435092 m^3` -- j1: `2.293251 m^3` -- j2: `2.312302 m^3` -- j3: `2.020883 m^3` -- j4: `2.005719 m^3` -- j5: `2.275315 m^3` -- j6: `1.263041 m^3` -- j7: `0.422458 m^3` - -Total: `15.028061000000001 m^3` - -#### Masses - -- m1: `(2.435092/15.028061000000001)*18=2.9166541179198036 kg` -- m2: `(2.293251/15.028061000000001)*18=2.746762739384675 kg` -- m3: `(2.312302/15.028061000000001)*18=2.7695812520324474 kg` -- m4: `(2.020883/15.028061000000001)*18=2.4205314311673343 kg` -- m5: `(2.005719/15.028061000000001)*18=2.402368608964257 kg` -- m6: `(2.275315/15.028061000000001)*18=2.7252797283694816 kg` -- m7: `(1.263041/15.028061000000001)*18=1.5128191188470688 kg` -- m8: `(0.422458/15.028061000000001)*18=0.506003003314932 kg` - -#### Inertias, center of gravities - -Fill in in the inertial_xml_marker.py script to account for scaling. - -### Hand - -#### Volumes - -- Hand: `0.000488 m^3` -- Finger1/2: `0.000011 m^3` - -Total: `0.000510 m^2` - -#### Masses - -- Hand: `(0.000488/0.000510)*0.7=0.6698039215686273 kg` -- Finger1/2: `(0.000011/0.000510)*0.7=0.015098039215686272 kg` - -#### Inertias, center of gravities - -Fill in in the inertial_xml_marker.py script to account for scaling. diff --git a/panda_gazebo/resources/documents/joint_trajectory_control_msg_examples.md b/panda_gazebo/resources/documents/joint_trajectory_control_msg_examples.md deleted file mode 100644 index 04dae728..00000000 --- a/panda_gazebo/resources/documents/joint_trajectory_control_msg_examples.md +++ /dev/null @@ -1,77 +0,0 @@ -# Example commands for joint trajectory control - -Here you find an example of valid joint trajectory command that can be send to the joint trajectory action server. - -## EXAMPLE POSE 1 (Positions) - -```txt -trajectory: - header: - seq: 19357 - stamp: - secs: 0 - nsecs: 0 - frame_id: '' - joint_names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7] - points: - - - positions: [0.0, 0.02, -0.0, -0.07, 0.0, -0.02, 0.91] - velocities: [] - accelerations: [] - effort: [] - time_from_start: - secs: 1 - nsecs: 0 -``` - -## EXAMPLE POSE 2 (Positions) - -```txt -trajectory: - header: - seq: 19357 - stamp: - secs: 0 - nsecs: 0 - frame_id: '' - joint_names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7] - points: - - - positions: [1.7383799999999998, 0.8461439999999998, -1.796326, -0.07, -1.274812, -0.02, 0.91] - velocities: [] - accelerations: [] - effort: [] - time_from_start: - secs: 1 - nsecs: 0 -``` - -## EXAMPLE POSE 3 (Positions) - -```txt -trajectory: - header: - seq: 19357 - stamp: - secs: 0 - nsecs: 0 - frame_id: '' - joint_names: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6, panda_joint7] - points: - - - positions: [1.7383799999999998, 0.8461439999999998, 0.0, -0.07, -1.274812, -0.02, 0.91] - velocities: [] - accelerations: [] - effort: [] - time_from_start: - secs: 1 - nsecs: 0 -``` - -## How to use - -You can use the `actionlib axclient.py` to send the commands: - -```bash -rosrun actionlib axclient.py /panda_arm_controller/follow_joint_trajectory -``` diff --git a/panda_gazebo/resources/documents/max_effort_calculations.md b/panda_gazebo/resources/documents/max_effort_calculations.md deleted file mode 100644 index 147e2a38..00000000 --- a/panda_gazebo/resources/documents/max_effort_calculations.md +++ /dev/null @@ -1,19 +0,0 @@ -# Calculate stabilization torques - -Here the maximum effort for the panda joints are calculated. - -## Joint 2 - -### Solo - -mass = 2.36414 -Length = 1 -Torque = m_g_(Length/2) -Torque = 2.36414_9.81_0.5 -Torque = 11.5961067 Nm - -### More joints - -mass_up_chain = 2.36414+2.38050+2.42754+3.49611+1.46736+0.45606 = 12.59171 -Length_up_chain = 6 -Torque=12.59_9.81_3=370.52 Nm diff --git a/panda_gazebo/resources/tools/panda_effort_control_tester.py b/panda_gazebo/resources/tools/panda_effort_control_tester.py deleted file mode 100755 index c1b9cba1..00000000 --- a/panda_gazebo/resources/tools/panda_effort_control_tester.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python3 -"""Script that creates a dynamic parameter server which can be used to test out the -Panda effort_controllers -""" - -import rospy -from dynamic_reconfigure.server import Server -from panda_gazebo.cfg import JointEffortConfig -from panda_gazebo.srv import SetJointEfforts, SetJointEffortsRequest - - -class JointEffortTester(object): - """Class used for testing whether the joint efforts work. - - Attributes: - :obj:`rospy.impl.tcpros_service.ServiceProxy`: Joint efforts server client. - :obj:`dynamic_reconfigure.server.Server`: Dynamic reconfigure server client. - """ - - def __init__(self): - """Initiate joint tester instance.""" - rospy.logdebug( - "Connecting to '/panda_control_server/set_joint_efforts' service." - ) - rospy.wait_for_service("/panda_control_server/set_joint_efforts", timeout=10) - self.set_joint_efforts_client = rospy.ServiceProxy( - "/panda_control_server/set_joint_efforts", SetJointEfforts - ) - rospy.logdebug("Connected to 'panda_control_server/set_joint_efforts' service!") - - # Generate joint_efforts msg - self.dynamic_reconfig_srv = Server( - JointEffortConfig, self._dynamic_reconfig_callback - ) - - def _dynamic_reconfig_callback(self, config, level): - """The callback function for the dynamic reconfigure server - - Args: - config (:obs:`dynamic_reconfigure.encoding.Config`): Dynamic reconfigure - config dictionary. - level (int): Dynamic reconfigure level parameter indicating which - parameter has been changed. - - Returns: - :obs:`dynamic_reconfigure.encoding.Config`: Dynamic reconfigure config - dictionary. - """ - rospy.loginfo( - "Joint efforts reconfigure Request: [{finger_joint1_effort}, " - "{finger_joint2_effort}, {joint1_effort}, {joint2_effort}, " - "{joint3_effort}, {joint4_effort}, {joint5_effort}, {joint6_effort}, " - "{joint7_effort}]".format(**config) - ) - - # Put changed values on object - self.config = config - self.joint_efforts_setpoint = [ - self.config["finger_joint1_effort"], - self.config["finger_joint2_effort"], - self.config["joint1_effort"], - self.config["joint2_effort"], - self.config["joint3_effort"], - self.config["joint4_effort"], - self.config["joint5_effort"], - self.config["joint6_effort"], - self.config["joint7_effort"], - ] - - # Call set_joint_efforts service - set_joint_efforts_msg = SetJointEffortsRequest() - set_joint_efforts_msg.joint_efforts = self.joint_efforts_setpoint - self.set_joint_efforts_client.call(set_joint_efforts_msg) - - return config - - -if __name__ == "__main__": - rospy.init_node("panda_effort_control_tester", anonymous=False) - - # Create joint effort tester class - joint_tester = JointEffortTester() - - # Spin till killed - rospy.spin() diff --git a/panda_gazebo/resources/tools/panda_position_control_tester.py b/panda_gazebo/resources/tools/panda_position_control_tester.py deleted file mode 100755 index e0aed7ae..00000000 --- a/panda_gazebo/resources/tools/panda_position_control_tester.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python3 -"""Script that creates a dynamic parameter server which can be used to test out the -Panda position_controllers -""" - -import rospy -from dynamic_reconfigure.server import Server -from panda_gazebo.cfg import JointPositionConfig -from panda_gazebo.srv import SetJointPositions, SetJointPositionsRequest - - -class JointPositionTester(object): - """Class used for testing whether the joint positions work. - - Attributes: - :obj:`rospy.impl.tcpros_service.ServiceProxy`: Joint positions server client. - :obj:`dynamic_reconfigure.server.Server`: Dynamic reconfigure server client. - """ - - def __init__(self): - """Initiate joint tester instance.""" - rospy.logdebug( - "Connecting to '/panda_control_server/set_joint_positions' service." - ) - rospy.wait_for_service("/panda_control_server/set_joint_positions", timeout=10) - self.set_joint_positions_client = rospy.ServiceProxy( - "/panda_control_server/set_joint_positions", SetJointPositions - ) - rospy.logdebug( - "Connected to 'panda_control_server/set_joint_positions' service!" - ) - - # Generate joint_positions msg - self.dynamic_reconfig_srv = Server( - JointPositionConfig, self._dynamic_reconfig_callback - ) - - def _dynamic_reconfig_callback(self, config, level): - """The callback function for the dynamic reconfigure server - - Args: - config (:obs:`dynamic_reconfigure.encoding.Config`): Dynamic reconfigure - config dictionary. - level (int): Dynamic reconfigure level parameter indicating which - parameter has been changed. - - Returns: - :obs:`dynamic_reconfigure.encoding.Config`: Dynamic reconfigure config - dictionary. - """ - rospy.loginfo( - "Joint positions reconfigure Request: [{finger_joint1_position}, " - "{finger_joint2_position}, {joint1_position}, {joint2_position}, " - "{joint3_position}, {joint4_position}, {joint5_position}, " - "{joint6_position}, {joint7_position}]".format(**config) - ) - - # Put changed values on object - self.config = config - self.joint_positions_setpoint = [ - self.config["finger_joint1_position"], - self.config["finger_joint2_position"], - self.config["joint1_position"], - self.config["joint2_position"], - self.config["joint3_position"], - self.config["joint4_position"], - self.config["joint5_position"], - self.config["joint6_position"], - self.config["joint7_position"], - ] - - # Call set_joint_positions service - set_joint_positions_msg = SetJointPositionsRequest() - set_joint_positions_msg.joint_positions = self.joint_positions_setpoint - self.set_joint_positions_client.call(set_joint_positions_msg) - - return config - - -if __name__ == "__main__": - rospy.init_node("panda_position_control_tester", anonymous=False) - - # Create joint position tester class - joint_tester = JointPositionTester() - - # Spin till killed - rospy.spin() diff --git a/panda_gazebo/setup.cfg b/panda_gazebo/setup.cfg index 4eafaf62..cefaaaea 100755 --- a/panda_gazebo/setup.cfg +++ b/panda_gazebo/setup.cfg @@ -19,7 +19,7 @@ exclude = .git, __pycache__, sandbox, - tests/*, + tests, conf.py, per-file-ignores = __init__.py: F401, E501 diff --git a/panda_gazebo/src/panda_gazebo/common/__init__.py b/panda_gazebo/src/panda_gazebo/common/__init__.py index a54c6c61..3a40b2bf 100644 --- a/panda_gazebo/src/panda_gazebo/common/__init__.py +++ b/panda_gazebo/src/panda_gazebo/common/__init__.py @@ -4,6 +4,4 @@ from panda_gazebo.common.action_client_state import ActionClientState from panda_gazebo.common.controller_info_dict import ControllerInfoDict -from panda_gazebo.common.euler_angles import EulerAngles -from panda_gazebo.common.nested_dict import NestedDict from panda_gazebo.common.quaternion import Quaternion diff --git a/panda_gazebo/src/panda_gazebo/common/action_client_state.py b/panda_gazebo/src/panda_gazebo/common/action_client_state.py index be2382df..ed86d3ab 100644 --- a/panda_gazebo/src/panda_gazebo/common/action_client_state.py +++ b/panda_gazebo/src/panda_gazebo/common/action_client_state.py @@ -18,7 +18,7 @@ class ActionClientState(object): """ def __init__(self, action_client=None, state=-1, state_string=""): - """Initializes the InputMessageInvalidError exception object. + """Initializes the ActionClientState object. Args: action_client (:obj:`actionlib.simple_action_client.SimpleActionClient, optional): diff --git a/panda_gazebo/src/panda_gazebo/common/euler_angles.py b/panda_gazebo/src/panda_gazebo/common/euler_angles.py deleted file mode 100644 index 6967fd3d..00000000 --- a/panda_gazebo/src/panda_gazebo/common/euler_angles.py +++ /dev/null @@ -1,24 +0,0 @@ -"""Class for storing euler angles. It uses the ypr (x-y-z) euler angle convention. -""" - - -class EulerAngles(object): - """Used for storing euler angles. - - Attributes: - y (float): Yaw angle (z). - p (float): Pitch angle (p), by default 0.0. - r (float): Roll angle (r), by default 0.0. - """ - - def __init__(self, y=0.0, p=0.0, r=0.0): - """Initializes the EulerAngles object. - - Args: - y (float, optional): Yaw angle (z). Defaults to ``0.0``. - p (float, optional): Pitch angle (p). Defaults to ``0.0``. - r (float, optional): Roll angle (r). Defaults to ``0.0``. - """ - self.y = y - self.p = p - self.r = r diff --git a/panda_gazebo/src/panda_gazebo/common/functions.py b/panda_gazebo/src/panda_gazebo/common/functions.py index bb4f2545..e3254b5a 100644 --- a/panda_gazebo/src/panda_gazebo/common/functions.py +++ b/panda_gazebo/src/panda_gazebo/common/functions.py @@ -7,8 +7,7 @@ import control_msgs.msg as control_msgs import rospy from actionlib_msgs.msg import GoalStatusArray -from panda_gazebo.msg import FollowJointTrajectoryGoal -from panda_gazebo.srv import SetJointPositionsRequest +from control_msgs.msg import FollowJointTrajectoryGoal from rospy.exceptions import ROSException from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectoryPoint @@ -48,7 +47,7 @@ def action_dict_2_joint_trajectory_msg(action_dict): action_dict (dict): Dictionary containing actions and joints. Returns: - :obj:`panda_gazebo.msg.FollowJointTrajectoryGoal`: New FollowJointTrajectoryGoal + :obj:`control_msgs.msg.FollowJointTrajectoryGoal`: New FollowJointTrajectoryGoal message. """ # Initiate waypoints and new trajectory message @@ -72,7 +71,7 @@ def panda_action_msg_2_control_msgs_action_msg(panda_action_msg): ` action message. Args: - panda_action_msg :obj:`panda_gazebo.msg.FollowJointTrajectoryGoal`: Panda_gazebo + panda_action_msg :obj:`control_msgs.msg.FollowJointTrajectoryGoal`: Panda_gazebo follow joint trajectory goal message. Returns: @@ -87,46 +86,6 @@ def panda_action_msg_2_control_msgs_action_msg(panda_action_msg): return control_msgs_action_msg -def joint_positions_2_follow_joint_trajectory_goal(joint_positions, time_from_start=1): - """Converts a dictionary of joint_positions into a FollowJointTrajectoryGoal - msgs. - - Args: - joint_positions (union[dict, :obj:`panda_gazebo.msg.SetJointPositionsRequest`]): - Dictionary or message containing the joint positions of each of the robot - joints. - time_from_start (dict, optional): The time from the start at which the joint - position has to be achieved. Defaults to 1 sec. - - Returns: - :obj:`panda_gazebo.msg.FollowJointTrajectoryGoal`):New FollowJointTrajectoryGoal - message. - """ - # Initiate waypoints and new trajectory message - goal_msg = FollowJointTrajectoryGoal() - waypoint = JointTrajectoryPoint() - waypoint.time_from_start.secs = time_from_start - - # creates waypoint from joint_posisitions - if isinstance(joint_positions, SetJointPositionsRequest): - waypoint.positions = joint_positions.joint_positions - goal_msg.trajectory.joint_names = joint_positions.joint_names - elif isinstance(joint_positions, dict): - waypoint.positions = list(joint_positions.values()) - goal_msg.trajectory.joint_names = list(joint_positions.keys()) - else: - TypeError( - "FollowJointTrajectory message could not be created since the " - "joint_positions argument has the %s type while the " - "'joint_positions_2_follow_joint_trajectory_goal' function only accepts " - "a dictionary or a SetJointPositions message." % type(joint_positions) - ) - - # Add waypoint to trajectory message and return goal msgs - goal_msg.trajectory.points.append(waypoint) - return goal_msg - - def controller_list_array_2_dict(controller_list_msgs): """Converts a :controller_manager_msgs:`Controller_manager/list_controllers ` message into a controller information dictionary. @@ -174,48 +133,26 @@ def translate_actionclient_result_error_code(actionclient_retval): ) -def translate_gripper_width_2_finger_joint_commands(input_dict): - """Translate any ``gripper_width`` keys that are present in the action dictionary - into the corresponding finger joint control commands which are used by the - controllers. +def translate_moveit_error_code(moveit_error_code): + """Translates a MoveIt error code object into a human readable error message. Args: - input_dict (dict): Dictionary containing the desired actions. + moveit_error_code (:obj:`~moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes`): + The MoveIt error code object Returns: - dict: Action dictionary in which the gripper_width is translated to finger joint - commands. + str: Error string that corresponds to the error code. """ - input_dict = input_dict.copy() # Ensure that changes stay within this scope - - # Translate any 'gripper_width' keys into Panda finger joint command keys - if isinstance(input_dict, dict): - if "gripper_width" in input_dict.keys(): # If dictionary contains commands - finger_position = input_dict["gripper_width"] / 2.0 - del input_dict["gripper_width"] - input_dict["panda_finger_joint1"] = finger_position - input_dict["panda_finger_joint2"] = finger_position - elif ( - "gripper_width_min" in input_dict.keys() - or "gripper_width_max" in input_dict.keys() - ): # If dictionary contains bounds commands - if "gripper_width_min" in input_dict.keys(): # Translate min - finger_position_min = input_dict["gripper_width_min"] / 2.0 - del input_dict["gripper_width_min"] - input_dict["panda_finger_joint1_min"] = finger_position_min - input_dict["panda_finger_joint2_min"] = finger_position_min - if "gripper_width_max" in input_dict.keys(): # Translate max - finger_position_max = input_dict["gripper_width_max"] / 2.0 - del input_dict["gripper_width_max"] - input_dict["panda_finger_joint1_max"] = finger_position_max - input_dict["panda_finger_joint2_max"] = finger_position_max - else: - raise TypeError( - "Input argument has the wrong type the" - "'translate_gripper_width_2_finger_joint_commands'" - "function only takes a dictionary." - ) - return input_dict + error_dict = { + value: attr + for attr, value in moveit_error_code.__class__.__dict__.items() + if attr[0] != "_" and all(map(str.isupper, attr.replace("_", ""))) + } + return ( + error_dict[moveit_error_code.val].lower().capitalize().replace("_", " ") + "." + if error_dict[moveit_error_code.val] != "SUCCESSFUL" + else "" + ) ################################################# diff --git a/panda_gazebo/src/panda_gazebo/common/nested_dict.py b/panda_gazebo/src/panda_gazebo/common/nested_dict.py deleted file mode 100644 index aa13afed..00000000 --- a/panda_gazebo/src/panda_gazebo/common/nested_dict.py +++ /dev/null @@ -1,18 +0,0 @@ -"""This dictionary class overloads the collections defaultdict class such that you can -create dictionary new dictionary keys and values even when the top key doesn't exist. -""" - -from collections import defaultdict - - -class NestedDict(defaultdict): - """A dictionary class that allows you to create dictionary fields that are multiple - layers deep. - """ - - def __init__(self, value=None): - """Initiate nested dict object""" - super(NestedDict, self).__init__(NestedDict) - - # Store value - self.value = value diff --git a/panda_gazebo/src/panda_gazebo/common/quaternion.py b/panda_gazebo/src/panda_gazebo/common/quaternion.py index 6e103374..b149c810 100644 --- a/panda_gazebo/src/panda_gazebo/common/quaternion.py +++ b/panda_gazebo/src/panda_gazebo/common/quaternion.py @@ -19,6 +19,11 @@ class Quaternion(QuaternionParent): z (float): The quaternion z coordinate. w (float): The quaternion w coordinate. norm (float): The quaternion norm. + + Methods: + quaternion_norm(cls, Quaternion): Calculates the norm of a quaternion. + normalize_quaternion(cls, quaternion): Normalizes a given quaternion. + normalize(): Normalize the quaternion. """ def __init__(self, *args, **kwds): @@ -28,8 +33,6 @@ def __init__(self, *args, **kwds): def normalize(self): """Normalizes the quaternion.""" norm = self.norm - - # Normalize quaternion and return if norm == 0.0 or isnan(norm): rospy.logwarn( "Quaternion could not be normalized since the norm could not be " @@ -55,17 +58,15 @@ def normalize_quaternion(cls, quaternion): """Normalizes a given quaternion. Args: - Quaternion (:obj:`geometry_msgs.msg.Quaternion`): A quaternion. + quaternion (:obj:`geometry_msgs.msg.Quaternion`): A quaternion. Returns: :obj:`geometry_msgs.msg.Quaternion`: The normalized quaternion. """ - quaternion = copy.deepcopy( - quaternion - ) # Create a hardcopy such that the original object is not modified + quaternion = copy.deepcopy(quaternion) norm = cls.quaternion_norm(quaternion) - # Normalize quaternion and return + # Normalize quaternion if norm == nan: # test rospy.logwarn( diff --git a/panda_gazebo/src/panda_gazebo/core/control_server.py b/panda_gazebo/src/panda_gazebo/core/control_server.py index 2f1ec7dd..8a65bf44 100644 --- a/panda_gazebo/src/panda_gazebo/core/control_server.py +++ b/panda_gazebo/src/panda_gazebo/core/control_server.py @@ -1,22 +1,18 @@ #! /usr/bin/env python3 -"""This server is responsible for controlling the Panda arm/hand. It created a number of +"""This server is responsible for controlling the Panda arm. It created a number of (action) services that can be used to send control commands to the Panda Robot arm and hand. Main services: - * set_joint_positions - * set_joint_efforts * get_controlled_joints * follow_joint_trajectory - -Extra services: + * set_joint_commands * panda_arm/set_joint_positions * panda_arm/set_joint_efforts * panda_arm/follow_joint_trajectory - * panda_hand/set_joint_positions - * panda_hand/set_joint_efforts - * panda_hand/follow_joint_trajectory + * panda_hand/set_gripper_width """ +# TODO: Add gravity compensation effort control. import copy import os @@ -25,16 +21,13 @@ from itertools import compress import actionlib -import control_msgs.msg as control_msgs import numpy as np import rospy -import yaml from controller_manager_msgs.srv import ListControllers, ListControllersRequest from panda_gazebo.common import ActionClientState from panda_gazebo.common.functions import ( action_server_exists, controller_list_array_2_dict, - dict_clean, flatten_list, get_duplicate_list, list_2_human_text, @@ -44,14 +37,19 @@ ) from panda_gazebo.core.group_publisher import GroupPublisher from panda_gazebo.exceptions import InputMessageInvalidError +import control_msgs.msg as control_msgs +from franka_gripper.msg import MoveAction, MoveGoal from panda_gazebo.msg import ( FollowJointTrajectoryAction, - FollowJointTrajectoryFeedback, FollowJointTrajectoryResult, ) from panda_gazebo.srv import ( GetControlledJoints, GetControlledJointsResponse, + SetGripperWidth, + SetGripperWidthResponse, + SetJointCommands, + SetJointCommandsResponse, SetJointEfforts, SetJointEffortsResponse, SetJointPositions, @@ -59,13 +57,22 @@ ) from rospy.exceptions import ROSException, ROSInterruptException from sensor_msgs.msg import JointState -from std_msgs.msg import Float64, Header +from std_msgs.msg import Float64 # Global script variables DIRNAME = os.path.dirname(__file__) -PARAMS_CONFIG_PATH = os.path.abspath( - os.path.join(DIRNAME, "../../../cfg/_cfg/parms_config.yaml") -) +PANDA_JOINTS = { + "arm": [ + "panda_joint1", + "panda_joint2", + "panda_joint3", + "panda_joint4", + "panda_joint5", + "panda_joint6", + "panda_joint7", + ], + "hand": ["panda_finger_joint1", "panda_finger_joint2"], +} ARM_POSITION_CONTROLLERS = [ "panda_arm_joint1_position_controller", "panda_arm_joint2_position_controller", @@ -85,17 +92,7 @@ "panda_arm_joint7_effort_controller", ] ARM_TRAJ_CONTROLLERS = ["panda_arm_controller"] -HAND_POSITION_CONTROLLERS = [ - "panda_hand_finger1_position_controller", - "panda_hand_finger2_position_controller", -] -HAND_EFFORT_CONTROLLERS = [ - "panda_hand_finger1_effort_controller", - "panda_hand_finger2_effort_controller", -] -HAND_TRAJ_CONTROLLERS = ["panda_hand_controller"] -ARM_TRAJ_ACTION_SERVER_TOPIC = "/panda_arm_controller/follow_joint_trajectory" -HAND_TRAJ_ACTION_SERVER_TOPIC = "/panda_hand_controller/follow_joint_trajectory" +HAND_CONTROLLERS = ["franka_gripper"] class PandaControlServer(object): @@ -113,25 +110,14 @@ class PandaControlServer(object): the joint positions are within the given setpoint. joint_efforts_threshold (int): Threshold for determining whether the joint efforts are within the given setpoint. - use_group_controller (bool): Whether we are using the group controllers or - controlling the individual joints. autofill_traj_positions (bool): Whether you want to automatically set the current states as positions when the positions field of the joint trajectory message is left empty. - arm_position_controllers (list): List containing the names of the position - controllers used for the arm. - arm_effort_controllers (list): List containing the names of the effort - controllers used for the arm. - hand_position_controllers (list):List containing the names of the position - controllers used for the hand. - hand_effort_controller (list): List containing the names of the effort - controllers used for the hand. """ def __init__( # noqa: C901 self, autofill_traj_positions=False, - create_extra_services=False, connection_timeout=10, ): """Initializes the PandaControlServer object. @@ -140,47 +126,20 @@ def __init__( # noqa: C901 autofill_traj_positions (bool, optional): Whether you want to automatically set the current states as positions when the positions field of the joint trajectory message is left empty. Defaults to ``False``. - create_extra_services (bool, optional): Specifies whether the extra services - should also be created. Defaults to ``False``. connection_timeout (int, optional): The timeout for connecting to the controller_manager services. Defaults to 3 sec. """ - self.joint_positions_setpoint = [] - self.joint_efforts_setpoint = [] + self.arm_joint_positions_setpoint = [] + self.arm_joint_efforts_setpoint = [] + self.gripper_width_setpoint = None self.joint_positions_threshold = 0.01 self.joint_efforts_threshold = 0.01 self.autofill_traj_positions = autofill_traj_positions self._wait_till_done_timeout = rospy.Duration(8) self._joint_efforts_grad_threshold = 0.01 self._joint_positions_grad_threshold = 0.01 - self._joint_state_topic = "/joint_states" - self._full_joint_traj_control = False - self._as_arm_feedback = FollowJointTrajectoryFeedback() - self._as_hand_feedback = FollowJointTrajectoryFeedback() - self._as_feedback = FollowJointTrajectoryFeedback() - - # Try to load package parameters from the package configuration file - try: - with open(PARAMS_CONFIG_PATH, "r") as stream: - try: - parms_config = yaml.safe_load(stream) - except yaml.YAMLError as e: - rospy.logwarn( - "Shutting down '%s' as the 'panda_gazebo' parameters " - "could not be loaded from the parameter configuration '%s' " - "as the following error was thrown: %s" - % (rospy.get_name(), PARAMS_CONFIG_PATH, e) - ) - sys.exit(0) - except FileNotFoundError: - rospy.logwarn( - "Shutting down '%s' as the 'panda_gazebo' package parameters " - "could not be loaded since the required parameter configuration file " - "'%s' was not found. Please make sure the configuration file is " - "present." % (rospy.get_name(), PARAMS_CONFIG_PATH) - ) - sys.exit(0) - panda_joints = parms_config["panda_joints"] + self._gripper_move_client_connected = False + self._arm_joint_traj_client_connected = False ######################################## # Create Panda control services ######## @@ -188,68 +147,57 @@ def __init__( # noqa: C901 # Create main PandaControlServer services rospy.loginfo("Creating '%s' services." % rospy.get_name()) - rospy.logdebug("Creating '%s/set_joint_positions' service." % rospy.get_name()) - self._set_joint_positions_srv = rospy.Service( - "%s/set_joint_positions" % rospy.get_name()[1:], - SetJointPositions, - self._set_joint_positions_cb, - ) - rospy.logdebug("Creating '%s/set_joint_efforts' service." % rospy.get_name()) - self._set_joint_efforts_srv = rospy.Service( - "%s/set_joint_efforts" % rospy.get_name()[1:], - SetJointEfforts, - self._set_joint_efforts_cb, - ) rospy.logdebug( "Creating '%s/get_controlled_joints' service." % rospy.get_name() ) self._get_controlled_joints_srv = rospy.Service( - "%s/get_controlled_joints" % rospy.get_name()[1:], + "%s/get_controlled_joints" % rospy.get_name().split("/")[-1], GetControlledJoints, self._get_controlled_joints_cb, ) - - # Create extra services - if create_extra_services: - rospy.logdebug( - "Creating '%s/panda_arm/set_joint_positions' service." - % rospy.get_name() - ) - self._set_arm_joint_positions_srv = rospy.Service( - "%s/panda_arm/set_joint_positions" % rospy.get_name()[1:], - SetJointPositions, - self._arm_set_joint_positions_cb, - ) - rospy.logdebug( - "Creating '%s/panda_arm/set_joint_efforts' service." % rospy.get_name() - ) - self._set_arm_joint_efforts_srv = rospy.Service( - "%s/panda_arm/set_joint_efforts" % rospy.get_name()[1:], - SetJointEfforts, - self._arm_set_joint_efforts_cb, - ) - rospy.logdebug( - "Creating '%s/panda_hand/set_joint_positions' service." - % rospy.get_name() - ) - self._set_hand_joint_positions_srv = rospy.Service( - "%s/panda_hand/set_joint_positions" % rospy.get_name()[1:], - SetJointPositions, - self._hand_set_joint_positions_cb, - ) - rospy.logdebug( - "Creating '%s/panda_hand/set_joint_efforts' service." % rospy.get_name() - ) - self._set_hand_joint_efforts_srv = rospy.Service( - "%s/panda_hand/set_joint_efforts" % rospy.get_name()[1:], - SetJointEfforts, - self._hand_set_joint_efforts_cb, - ) + rospy.logdebug("Creating '%s/set_joint_commands' service." % rospy.get_name()) + self._set_joint_commands_srv = rospy.Service( + "%s/set_joint_commands" % rospy.get_name().split("/")[-1], + SetJointCommands, + self._set_joint_commands_cb, + ) + rospy.logdebug( + "Creating '%s/panda_arm/set_joint_efforts' service." % rospy.get_name() + ) + rospy.logdebug( + "Creating '%s/panda_arm/set_joint_positions' service." % rospy.get_name() + ) + self._set_arm_joint_positions_srv = rospy.Service( + "%s/panda_arm/set_joint_positions" % rospy.get_name().split("/")[-1], + SetJointPositions, + self._arm_set_joint_positions_cb, + ) + rospy.logdebug( + "Creating '%s/panda_arm/set_joint_efforts' service." % rospy.get_name() + ) + self._set_arm_joint_efforts_srv = rospy.Service( + "%s/panda_arm/set_joint_efforts" % rospy.get_name().split("/")[-1], + SetJointEfforts, + self._arm_set_joint_efforts_cb, + ) + # NOTE: The service below serves as a wrapper around the original + # 'franka_gripper/move' action. It makes sure that users only need to set the + # gripper width while it automatically sets the speed to the maximum. It also + # clips gripper width that are not possible. + rospy.logdebug( + "Creating '%s/panda_hand/set_gripper_width' service." % rospy.get_name() + ) + self._set_gripper_width_srv = rospy.Service( + "%s/panda_hand/set_gripper_width" % rospy.get_name().split("/")[-1], + SetGripperWidth, + self._set_gripper_width_cb, + ) rospy.loginfo("'%s' services created successfully." % rospy.get_name()) ######################################## # Create panda_control publishers and ## - # and connect to required services. ## + # and connect to required (action) ##### + # services. ############################ ######################################## # Create arm joint position controller publishers @@ -273,27 +221,6 @@ def __init__( # noqa: C901 queue_size=10, ) ) - # Create and joint position publishers - self._hand_joint_position_pub = GroupPublisher() - for position_controller in HAND_POSITION_CONTROLLERS: - self._hand_joint_position_pub.append( - rospy.Publisher( - "%s/command" % (position_controller), - Float64, - queue_size=10, - ) - ) - - # Create hand joint group effort publishers - self._hand_joint_effort_pub = GroupPublisher() - for effort_controller in HAND_EFFORT_CONTROLLERS: - self._hand_joint_effort_pub.append( - rospy.Publisher( - "%s/command" % (effort_controller), - Float64, - queue_size=10, - ) - ) # Connect to controller_manager services try: @@ -303,24 +230,48 @@ def __init__( # noqa: C901 rospy.wait_for_service( "controller_manager/list_controllers", timeout=connection_timeout ) - self.list_controllers_client = rospy.ServiceProxy( + self._list_controllers_client = rospy.ServiceProxy( "controller_manager/list_controllers", ListControllers ) rospy.logdebug( "Connected to 'controller_manager/list_controllers' service!" ) - except (rospy.ServiceException, ROSException, ROSInterruptException) as e: + except (rospy.ServiceException, ROSException, ROSInterruptException): rospy.logerr( "Shutting down '%s' because no connection could be established " - "with the '%s' service and this service is needed " - "when using 'position_control'." - % ( - rospy.get_name(), - e.args[0].strip("timeout exceeded while waiting for service"), - ) + "with the 'controller_manager/list_controllers' service." + % (rospy.get_name(),) ) sys.exit(0) + # Connect to the gripper command action server + rospy.logdebug("Connecting to '%s' action service." % "franka_gripper/move") + if action_server_exists("franka_gripper/move"): + # Connect to robot control action server + self._gripper_move_client = actionlib.SimpleActionClient( + "franka_gripper/move", + MoveAction, + ) + + # Waits until the action server has started up + retval = self._gripper_move_client.wait_for_server( + timeout=rospy.Duration(secs=5) + ) + if not retval: + rospy.logwarn( + "No connection could be established with the '%s' service. " + "The Panda Robot Environment therefore can not use this action " + "service to control the Panda Robot." % ("franka_gripper/move") + ) + else: + self._gripper_move_client_connected = True + else: + rospy.logwarn( + "No connection could be established with the '%s' service. " + "The Panda Robot Environment therefore can not use this action " + "service to control the Panda Robot." % ("franka_gripper/move") + ) + ######################################## # Connect joint state subscriber ####### ######################################## @@ -330,87 +281,42 @@ def __init__( # noqa: C901 while self.joint_states is None and not rospy.is_shutdown(): try: self.joint_states = rospy.wait_for_message( - self._joint_state_topic, JointState, timeout=1.0 + "joint_states", JointState, timeout=1.0 ) - - # Set joint setpoint to current position - self.joint_positions_setpoint = self.joint_states.position - self.joint_efforts_setpoint = self.joint_states.effort except ROSException: rospy.logwarn( - "Current /joint_states not ready yet, retrying for getting %s" - % self._joint_state_topic + "Current joint_states not ready yet, retrying for getting %s" + % "joint_states" ) # Create joint_state subscriber self._joint_states_sub = rospy.Subscriber( - self._joint_state_topic, JointState, self._joints_cb - ) - - ######################################## - # Get controller information ########### - ######################################## - - # Set panda_control to right controller type (Group or individual) - self.arm_position_controllers = ARM_POSITION_CONTROLLERS - self.arm_effort_controllers = ARM_EFFORT_CONTROLLERS - self.hand_position_controllers = HAND_POSITION_CONTROLLERS - self.hand_effort_controllers = HAND_EFFORT_CONTROLLERS - self.arm_traj_controllers = ARM_TRAJ_CONTROLLERS - self.hand_traj_controllers = HAND_TRAJ_CONTROLLERS - self._arm_position_pub = self._arm_joint_position_pub - self._arm_effort_pub = self._arm_joint_effort_pub - self._hand_position_pub = self._hand_joint_position_pub - self._hand_effort_pub = self._hand_joint_effort_pub - self._arm_position_controller_msg_type = Float64 - self._arm_effort_controller_msg_type = Float64 - self._hand_position_controller_msg_type = Float64 - self._hand_effort_controller_msg_type = Float64 - - # Create combined position/effort controllers lists - self._position_controllers = flatten_list( - [self.arm_position_controllers, self.hand_position_controllers] - ) - self._effort_controllers = flatten_list( - [self.arm_effort_controllers, self.hand_effort_controllers] + "joint_states", JointState, self._joints_cb ) - self._traj_controllers = flatten_list( - [self.arm_traj_controllers, self.hand_traj_controllers] - ) - - # Retrieve the names of the joints that are controlled when we use joint - # trajectory control - try: - self._controlled_joints_traj_control = self._get_controlled_joints( - control_type="traj_control" - ) - except InputMessageInvalidError: - self._controlled_joints_traj_control = panda_joints ######################################## # Create joint trajectory action ####### # servers ############################## ######################################## - # NOTE: Here setup three new action services that serve as a wrapper around the - # original 'panda_arm_controller/follow_joint_trajectory' and - # 'panda_hand_controller/follow_joint_trajectory' action servers. By doing this + # NOTE: Here setup a new action service that serves as a wrapper around the + # original 'panda_arm_controller/follow_joint_trajectory'. By doing this # we add the following features to the original action servers. # - The ability to send partial joint messages. # - The ability to send joint trajectory messages that do not specify joints. - # - The ability to send both arm and hand traj commands at the same time. # - The ability to automatic generate a time axes when the create_time_axis # field is set to True. # Connect to original 'panda_arm_controller/follow_joint_trajectory' action # server rospy.logdebug( - "Connecting to '%s' action service." % ARM_TRAJ_ACTION_SERVER_TOPIC + "Connecting to '%s' action service." + % "panda_arm_controller/follow_joint_trajectory" ) - if action_server_exists(ARM_TRAJ_ACTION_SERVER_TOPIC): # Check if exists - + if action_server_exists("panda_arm_controller/follow_joint_trajectory"): # Connect to robot control action server self._arm_joint_traj_client = actionlib.SimpleActionClient( - ARM_TRAJ_ACTION_SERVER_TOPIC, control_msgs.FollowJointTrajectoryAction + "panda_arm_controller/follow_joint_trajectory", + control_msgs.FollowJointTrajectoryAction, ) # Waits until the action server has started up @@ -422,68 +328,17 @@ def __init__( # noqa: C901 "No connection could be established with the '%s' service. " "The Panda Robot Environment therefore can not use this action " "service to control the Panda Robot." - % (ARM_TRAJ_ACTION_SERVER_TOPIC) - ) - self._arm_joint_traj_connected = False - else: - self._arm_joint_traj_connected = True - else: - rospy.logwarn( - "No connection could be established with the '%s' service. " - "The Panda Robot Environment therefore can not use this action " - "service to control the Panda Robot." % (ARM_TRAJ_ACTION_SERVER_TOPIC) - ) - self._arm_joint_traj_connected = False - - # Connect to original 'panda_hand_controller/follow_joint_trajectory' action - # server - rospy.logdebug( - "Connecting to '%s' action service." % HAND_TRAJ_ACTION_SERVER_TOPIC - ) - if action_server_exists(HAND_TRAJ_ACTION_SERVER_TOPIC): # Check if exists - - # Connect to robot control action server - self._hand_joint_traj_client = actionlib.SimpleActionClient( - HAND_TRAJ_ACTION_SERVER_TOPIC, control_msgs.FollowJointTrajectoryAction - ) - - # Waits until the action server has started up - retval = self._hand_joint_traj_client.wait_for_server( - timeout=rospy.Duration(secs=5) - ) - if not retval: - rospy.logwarn( - "No connection could be established with the '%s' service. " - "The Panda Robot Environment therefore can not use this action " - "service to control the Panda Robot." - % (HAND_TRAJ_ACTION_SERVER_TOPIC) + % ("panda_arm_controller/follow_joint_trajectory") ) - self._hand_joint_traj_connected = False else: - self._hand_joint_traj_connected = True + self._arm_joint_traj_client_connected = True else: rospy.logwarn( "No connection could be established with the '%s' service. " "The Panda Robot Environment therefore can not use this action " - "service to control the Panda Robot." % (HAND_TRAJ_ACTION_SERVER_TOPIC) + "service to control the Panda Robot." + % ("panda_arm_controller/follow_joint_trajectory") ) - self._hand_joint_traj_connected = False - - # Setup a new Panda joint trajectory action server - rospy.loginfo( - "Creating '%s' joint trajectory action services." % rospy.get_name() - ) - rospy.logdebug( - "Creating '%s/follow_joint_trajectory' service." % rospy.get_name() - ) - self._joint_traj_as = actionlib.SimpleActionServer( - "%s/follow_joint_trajectory" % rospy.get_name()[1:], - FollowJointTrajectoryAction, - execute_cb=self._joint_traj_execute_cb, - auto_start=False, - ) - self._joint_traj_as.register_preempt_callback(self._joint_traj_preempt_cb) - self._joint_traj_as.start() # Setup a new Panda arm joint trajectory action server rospy.logdebug( @@ -491,7 +346,7 @@ def __init__( # noqa: C901 % rospy.get_name() ) self._arm_joint_traj_as = actionlib.SimpleActionServer( - "%s/panda_arm/follow_joint_trajectory" % rospy.get_name()[1:], + "%s/panda_arm/follow_joint_trajectory" % rospy.get_name().split("/")[-1], FollowJointTrajectoryAction, execute_cb=self._arm_joint_traj_execute_cb, auto_start=False, @@ -501,242 +356,9 @@ def __init__( # noqa: C901 ) self._arm_joint_traj_as.start() - # Setup a new Panda hand joint trajectory action server - rospy.logdebug( - "Creating '%s/panda_hand/follow_joint_trajectory' service." - % rospy.get_name() - ) - self._hand_joint_traj_as = actionlib.SimpleActionServer( - "%s/panda_hand/follow_joint_trajectory" % rospy.get_name()[1:], - FollowJointTrajectoryAction, - execute_cb=self._hand_joint_traj_execute_cb, - auto_start=False, - ) - self._hand_joint_traj_as.register_preempt_callback( - self._hand_joint_traj_preempt_cb - ) - self._hand_joint_traj_as.start() - rospy.loginfo( - "'%s' joint trajectory action services created successfully." - % rospy.get_name() - ) - - # Initialize joint trajectory feedback messages - self._init_action_servers_fb_msgs() - ################################################ # Panda control member functions ############### ################################################ - def _init_action_servers_fb_msgs(self): - """Initiate the ``panda_control_server/panda_arm/follow_joint_trajectory`` and - ``panda_control_server/panda_hand/follow_joint_trajectory`` feedback messages - with the current robot states. - """ - header = Header() - header.stamp = rospy.get_rostime() - - # Get current robot state ditionary - state_dict = self._retrieve_state_dict(self._controlled_joints_traj_control) - arm_state_dict = state_dict["arm"] - hand_state_dict = state_dict["hand"] - - # Fill arm feedback message - self._as_arm_feedback.joint_names = self._controlled_joints_traj_control["arm"] - self._as_arm_feedback.header = header - self._as_arm_feedback.actual.positions = arm_state_dict["positions"].values() - self._as_arm_feedback.actual.velocities = arm_state_dict["velocities"].values() - self._as_arm_feedback.actual.effort = arm_state_dict["efforts"].values() - - # Fill hand feedback message - self._as_hand_feedback.joint_names = self._controlled_joints_traj_control[ - "hand" - ] - self._as_hand_feedback.header = header - self._as_hand_feedback.actual.positions = hand_state_dict["positions"].values() - self._as_hand_feedback.actual.velocities = hand_state_dict[ - "velocities" - ].values() - self._as_hand_feedback.actual.effort = hand_state_dict["efforts"].values() - self._as_feedback.joint_names = self._controlled_joints_traj_control["both"] - self._as_hand_feedback.header = header - - # Fill combined feedback message - self._as_feedback.actual.positions = list(self.joint_states.position) - self._as_feedback.actual.velocities = list(self.joint_states.velocity) - self._as_feedback.actual.effort = list(self.joint_states.effort) - - def _get_combined_action_server_fb_msg(self): - """Combine the action server feedback messages from the original - ``panda_arm_controller/follow_joint_trajectory`` and - ``panda_hand_controller/follow_joint_trajectory`` action servers so that they - can be used as a feedback message to the - ``panda_control_server/follow_joint_trajectory`` wrapper action server. - - Returns: - :obj:`panda_gazebo.msg.FollowJointTrajectoryFeedback`: New combined action - server feedback message. - """ - # Use newest message as starting point - if self._as_arm_feedback.header.stamp >= self._as_hand_feedback.header.stamp: - feedback_combined = copy.deepcopy(self._as_arm_feedback) - else: - feedback_combined = copy.deepcopy(self._as_hand_feedback) - - # Combine feedback messages - if self.joint_states.name[0] in self._controlled_joints_traj_control["arm"]: - feedback_combined.joint_names = flatten_list( - [self._as_arm_feedback.joint_names, self._as_hand_feedback.joint_names] - ) - feedback_combined.actual.positions = flatten_list( - [ - list(self._as_arm_feedback.actual.positions), - list(self._as_hand_feedback.actual.positions), - ] - ) - feedback_combined.actual.velocities = flatten_list( - [ - list(self._as_arm_feedback.actual.velocities), - list(self._as_hand_feedback.actual.velocities), - ] - ) - feedback_combined.actual.accelerations = flatten_list( - [ - list(self._as_arm_feedback.actual.accelerations), - list(self._as_hand_feedback.actual.accelerations), - ] - ) - feedback_combined.actual.effort = flatten_list( - [ - list(self._as_arm_feedback.actual.effort), - list(self._as_hand_feedback.actual.effort), - ] - ) - feedback_combined.desired.positions = flatten_list( - [ - list(self._as_arm_feedback.desired.positions), - list(self._as_hand_feedback.desired.positions), - ] - ) - feedback_combined.desired.velocities = flatten_list( - [ - list(self._as_arm_feedback.desired.velocities), - list(self._as_hand_feedback.desired.velocities), - ] - ) - feedback_combined.desired.accelerations = flatten_list( - [ - list(self._as_arm_feedback.desired.accelerations), - list(self._as_hand_feedback.desired.accelerations), - ] - ) - feedback_combined.desired.effort = flatten_list( - [ - list(self._as_arm_feedback.desired.effort), - list(self._as_hand_feedback.desired.effort), - ] - ) - feedback_combined.error.positions = flatten_list( - [ - list(self._as_arm_feedback.error.positions), - list(self._as_hand_feedback.error.positions), - ] - ) - feedback_combined.error.velocities = flatten_list( - [ - list(self._as_arm_feedback.error.velocities), - list(self._as_hand_feedback.error.velocities), - ] - ) - feedback_combined.error.accelerations = flatten_list( - [ - list(self._as_arm_feedback.error.accelerations), - list(self._as_hand_feedback.error.accelerations), - ] - ) - feedback_combined.error.effort = flatten_list( - [ - list(self._as_arm_feedback.error.effort), - list(self._as_hand_feedback.error.effort), - ] - ) - else: - feedback_combined.joint_names = flatten_list( - [self._as_hand_feedback.joint_names, self._as_arm_feedback.joint_names] - ) - feedback_combined.actual.positions = flatten_list( - [ - list(self._as_hand_feedback.actual.positions), - list(self._as_arm_feedback.actual.positions), - ] - ) - feedback_combined.actual.velocities = flatten_list( - [ - list(self._as_hand_feedback.actual.velocities), - list(self._as_arm_feedback.actual.velocities), - ] - ) - feedback_combined.actual.accelerations = flatten_list( - [ - list(self._as_hand_feedback.actual.accelerations), - list(self._as_arm_feedback.actual.accelerations), - ] - ) - feedback_combined.actual.effort = flatten_list( - [ - list(self._as_hand_feedback.actual.effort), - list(self._as_arm_feedback.actual.effort), - ] - ) - feedback_combined.desired.positions = flatten_list( - [ - list(self._as_hand_feedback.desired.positions), - list(self._as_arm_feedback.desired.positions), - ] - ) - feedback_combined.desired.velocities = flatten_list( - [ - list(self._as_hand_feedback.desired.velocities), - list(self._as_arm_feedback.desired.velocities), - ] - ) - feedback_combined.desired.accelerations = flatten_list( - [ - list(self._as_hand_feedback.desired.accelerations), - list(self._as_arm_feedback.desired.accelerations), - ] - ) - feedback_combined.desired.effort = flatten_list( - [ - list(self._as_hand_feedback.desired.effort), - list(self._as_arm_feedback.desired.effort), - ] - ) - feedback_combined.error.positions = flatten_list( - [ - list(self._as_hand_feedback.error.positions), - list(self._as_arm_feedback.error.positions), - ] - ) - feedback_combined.error.velocities = flatten_list( - [ - list(self._as_hand_feedback.error.velocities), - list(self._as_arm_feedback.error.velocities), - ] - ) - feedback_combined.error.accelerations = flatten_list( - [ - list(self._as_hand_feedback.error.accelerations), - list(self._as_arm_feedback.error.accelerations), - ] - ) - feedback_combined.error.effort = flatten_list( - [ - list(self._as_hand_feedback.error.effort), - list(self._as_arm_feedback.error.effort), - ] - ) - return feedback_combined - def _retrieve_state_dict(self, controlled_joints_dict): """Retrieves the current Panda arm and hand position, velocity and effort states in dictionary form. @@ -899,22 +521,26 @@ def _retrieve_state_dict(self, controlled_joints_dict): } def _wait_till_done( # noqa: C901 - self, control_type, control_group="both", timeout=None, controlled_joints=None + self, + control_type, + controlled_joints=None, + timeout=None, + check_gradient=True, ): """Wait control is finished. Meaning the robot state is within range of the - Joint position and joint effort setpoints. + Joint position and joint effort setpoints (or the velocity is zero). Args: control_type (str): The type of control that is being executed and on which we should wait. Options are ``effort_control`` and ``position_control``. - control_group (str, optional): The control group for which the control is - being performed. Defaults to "both". - controlled_joints (dict, optional): A dictionary containing the joints that + controlled_joints (list, optional): A dictionary containing the joints that are currently being controlled, these joints will be determined if no dictionary is given. - connection_timeout (int, optional): The timeout when waiting for the control + timeout (int, optional): The timeout when waiting for the control to be done. Defaults to :attr:`_wait_till_done_timeout`. + check_gradient (boolean, optional): If enabled the script will also return + when the gradients become zero. Defaults to ``True``. """ # Validate control type and control group if control_type not in ["position_control", "effort_control"]: @@ -925,22 +551,11 @@ def _wait_till_done( # noqa: C901 return False else: control_type = control_type.lower() - if control_group not in ["arm", "hand", "both"]: - rospy.logwarn( - "The control group '%s' you specified is not valid. Valid values are " - "%s. Control group 'both' used instead." % ("['arm', 'hand', 'both']") - ) - control_group = "both" - else: - control_group = control_group.lower() # Compute the state masks if controlled_joints: arm_states_mask = [ - joint in controlled_joints["arm"] for joint in self.joint_states.name - ] - hand_states_mask = [ - joint in controlled_joints["hand"] for joint in self.joint_states.name + joint in controlled_joints for joint in self.joint_states.name ] else: # Try to determine the controlled joints and state mask try: @@ -951,19 +566,31 @@ def _wait_till_done( # noqa: C901 joint in controlled_joints["arm"] for joint in self.joint_states.name ] - hand_states_mask = [ - joint in controlled_joints["hand"] - for joint in self.joint_states.name - ] - except InputMessageInvalidError as e: - rospy.loginfo( + except InputMessageInvalidError: + rospy.logwarn( "Not waiting for control to be completed as no information could " "be retrieved about which joints are controlled when using '%s' " "control. Please make sure the '%s' controllers that are needed " "for '%s' control are initialized." % ( control_type, - e.details["controlled_joints"], + ARM_POSITION_CONTROLLERS + if control_type == "position_control" + else ARM_EFFORT_CONTROLLERS, + control_type, + ) + ) + return False + if not any(arm_states_mask): + rospy.logwarn( + "Not waiting for control to be completed as no joints appear to be " + "controlled when using '%s' control. Please make sure the '%s' " + "controllers that are needed for '%s' control are initialized." + % ( + control_type, + ARM_POSITION_CONTROLLERS + if control_type == "position_control" + else ARM_EFFORT_CONTROLLERS, control_type, ) ) @@ -975,138 +602,75 @@ def _wait_till_done( # noqa: C901 else: timeout = self._wait_till_done_timeout - # Select the right mask for the control_group - if control_group == "arm": - states_mask = arm_states_mask - elif control_group == "hand": - states_mask = hand_states_mask - else: - states_mask = [ - any(bool_tuple) for bool_tuple in zip(arm_states_mask, hand_states_mask) - ] - - # Wait till robot positions/efforts are not changing anymore - # NOTE: We have to use the std to determine whether the control was finished - # as the velocity in the joint_states topic is wrong (see issue 14) - # Improve: We can use real gazebo velocity if issue 14 is fixed. + # Wait till robot positions/efforts reach the setpoint or the velocities are + # not changing anymore timeout_time = rospy.get_rostime() + timeout - positions_buffer = np.full((2, len(self.joint_states.position)), np.nan) - positions_grad = np.full((2, len(self.joint_states.position)), np.nan) - efforts_buffer = np.full((2, len(self.joint_states.effort)), np.nan) - efforts_grad = np.full((2, len(self.joint_states.effort)), np.nan) + state_buffer = np.full((2, len(self.joint_states.position)), np.nan) + grad_buffer = np.full((2, len(self.joint_states.position)), np.nan) while not rospy.is_shutdown() and rospy.get_rostime() < timeout_time: - # Wait till joint positions are within range or arm not changing anymore if control_type == "position_control": + joint_setpoint = self.arm_joint_positions_setpoint + joint_states = self.joint_states.position + state_threshold = self.joint_positions_threshold + grad_threshold = self._joint_positions_grad_threshold + # Check if joint effort states are within setpoint + elif control_type == "effort_control": + joint_setpoint = self.arm_joint_efforts_setpoint + joint_states = self.joint_states.effort + state_threshold = self.joint_efforts_threshold + grad_threshold = self._joint_efforts_grad_threshold - # Retrieve positions setpoint - joint_positions_setpoint = flatten_list( - [ - self.joint_positions_setpoint["hand"], - self.joint_positions_setpoint["arm"], - ] - if hand_states_mask[0] - else [ - self.joint_positions_setpoint["arm"], - self.joint_positions_setpoint["hand"], - ] - ) - - # Add state to position to buffer - positions_buffer = np.append( - positions_buffer, [self.joint_states.position], axis=0 - ) - positions_buffer = np.delete( - positions_buffer, 0, axis=0 - ) # Delete oldest entry - positions_grad = np.gradient(positions_buffer, axis=0) + # Add current state to state_buffer and delete oldest entry + state_buffer = np.delete( + np.append(state_buffer, [joint_states], axis=0), 0, axis=0 + ) + grad_buffer = np.gradient(state_buffer, axis=0) - # Check if joint states are within setpoints + # Check if setpoint is reached + if check_gradient: # Check position/effort and gradients if ( np.linalg.norm( - np.array( - list(compress(self.joint_states.position, states_mask)) - ) - - np.array( - list(compress(joint_positions_setpoint, states_mask)) - ) + np.array(list(compress(joint_states, arm_states_mask))) + - np.array(list(compress(joint_setpoint, arm_states_mask))) ) - <= self.joint_positions_threshold + <= state_threshold # Check if difference norm is within threshold ) or all( [ - ( - np.abs(val) <= self._joint_positions_grad_threshold - and val != 0.0 - ) - for val in list(compress(positions_grad[-1], states_mask)) - ] - ): # Check if difference norm is within threshold + (np.abs(val) <= grad_threshold and val != 0.0) + for val in list(compress(grad_buffer[-1], arm_states_mask)) + ] # Check if all velocities are close to zero + ): break - - # Check if joint effort states are within setpoint - elif control_type == "effort_control": - - # Retrieve positions setpoint - joint_efforts_setpoint = flatten_list( - [ - self.joint_efforts_setpoint["hand"], - self.joint_efforts_setpoint["arm"], - ] - if hand_states_mask[0] - else [ - self.joint_efforts_setpoint["arm"], - self.joint_efforts_setpoint["hand"], - ] - ) - - # Add state to effort to buffer - efforts_buffer = np.append( - efforts_buffer, [self.joint_states.effort], axis=0 - ) - efforts_buffer = np.delete( - efforts_buffer, 0, axis=0 - ) # Delete oldest entry - efforts_grad = np.gradient(efforts_buffer, axis=0) + else: # Only check position/effort if ( np.linalg.norm( - np.array(list(compress(self.joint_states.effort, states_mask))) - - np.array(list(compress(joint_efforts_setpoint, states_mask))) + np.array(list(compress(joint_states, arm_states_mask))) + - np.array(list(compress(joint_setpoint, arm_states_mask))) ) - <= self.joint_efforts_threshold - ) or all( - [ - ( - np.abs(val) <= self._joint_efforts_grad_threshold - and val != 0.0 - ) - for val in list(compress(efforts_grad[-1], states_mask)) - ] - ): # Check if difference norm is within threshold + <= state_threshold # Check if difference norm is within threshold + ): break - else: - rospy.loginfo( - "Not waiting for control to be completed as '%s' is not " - "a valid control type." - ) + # Reset setpoints and return result + if control_type == "position_control": + self.arm_joint_positions_setpoint = [] + else: + self.arm_joint_efforts_setpoint = [] return True - def _create_traj_action_server_msg( # noqa: C901 - self, input_msg, control_group, verbose=False + def _create_arm_traj_action_server_msg( # noqa: C901 + self, input_msg, verbose=False ): - """Converts the ``panda_gazebo.msg.FollowJointTrajectoryGoal`` message that + """Converts the ``control_msgs.msg.FollowJointTrajectoryGoal`` message that is received by the ``panda_control_server`` follow joint trajectory wrapper action servers into the right format for the original ``panda_arm_controller`` - and ``panda_hand_controller`` `follow_joint_trajectory `_ - action servers. + action server. Args: input_msg (:obj:`trajectory_msgs/JointTrajectory`): The service input message we want to validate. - control_type (str): The type of control that is being executed and on which - we should wait. Options are ``effort_control`` and - ``position_control``. verbose (bool): Boolean specifying whether you want to send a warning message to the ROSlogger. @@ -1120,56 +684,19 @@ def _create_traj_action_server_msg( # noqa: C901 input_msg could not be converted into panda_arm_controller control messages. """ # noqa: E501 - # Validate control_group argument - if control_group.lower() not in ["arm", "hand", "both"]: - logwarn_message = ( - "Control group '%s' does not exist. Please specify a valid control " - "group. Valid values are %s." - % (control_group.lower(), "['arm', 'hand', 'both']") - ) - if verbose: - rospy.logwarn(logwarn_message) - raise InputMessageInvalidError( - message="Control_group '%s' invalid." % control_group.lower(), - log_message=logwarn_message, - ) - else: - control_group = control_group.lower() - # Retrieve controlled joints - try: - controlled_joints_dict = self._get_controlled_joints( - control_type="traj_control", verbose=verbose - ) - except InputMessageInvalidError: - logwarn_message = ( - "The joint trajectory publisher message could not be created as the " - "'%s' are not initialized." % (self._traj_controllers) - ) - if verbose: - rospy.logwarn(logwarn_message) - raise InputMessageInvalidError( - message="Required controllers not initialised.", - details={"controlled_joints": self._effort_controllers}, - log_message=logwarn_message, - ) + controlled_joints_dict = self._get_controlled_joints( + control_type="traj_control", verbose=verbose + ) - # Get needed contolled joints information out of dictionary - if control_group == "arm": - controlled_joints = controlled_joints_dict["arm"] - elif control_group == "hand": - controlled_joints = controlled_joints_dict["hand"] - else: - controlled_joints = controlled_joints_dict["both"] + # Get controlled joints, arm_state and joint_names + controlled_joints = controlled_joints_dict["arm"] controlled_joints_size = len(controlled_joints) - - # Retrieve joint names and current robot states state_dict = self._retrieve_state_dict(controlled_joints_dict) arm_state_dict = state_dict["arm"] - hand_state_dict = state_dict["hand"] joint_names = input_msg.trajectory.joint_names - # Initiate new arm and hand action server messages using the current robot state + # Initiate new arm action server messages using the current robot state arm_control_msg = copy.deepcopy(input_msg) arm_control_msg.trajectory.joint_names = controlled_joints_dict["arm"] for idx, waypoint in enumerate(input_msg.trajectory.points): @@ -1179,26 +706,11 @@ def _create_traj_action_server_msg( # noqa: C901 arm_control_msg.trajectory.points[idx].velocities = list( arm_state_dict["velocities"].values() ) - arm_control_msg.trajectory.points[idx].effort = list( - arm_state_dict["efforts"].values() - ) arm_control_msg.trajectory.points[idx].accelerations = [0.0] * len( arm_control_msg.trajectory.joint_names ) - hand_control_msg = copy.deepcopy(input_msg) - hand_control_msg.trajectory.joint_names = controlled_joints_dict["hand"] - for idx, waypoint in enumerate(input_msg.trajectory.points): - hand_control_msg.trajectory.points[idx].positions = list( - hand_state_dict["positions"].values() - ) - hand_control_msg.trajectory.points[idx].velocities = list( - hand_state_dict["velocities"].values() - ) - hand_control_msg.trajectory.points[idx].effort = list( - hand_state_dict["efforts"].values() - ) - hand_control_msg.trajectory.points[idx].accelerations = [0.0] * len( - hand_control_msg.trajectory.joint_names + arm_control_msg.trajectory.points[idx].effort = list( + arm_state_dict["efforts"].values() ) # Retrieve the length of the positions/velocities/accelerations and efforts in @@ -1227,10 +739,9 @@ def _create_traj_action_server_msg( # noqa: C901 # Check action server goal request message if len(joint_names) == 0: # If not joint_names were given - # Check if the number of joint positions/velocities/accelerations or # efforts is unequal to the the number of joints - waypoints_length_not_equal = [ + waypoints_lengths_not_equal_to_joints = [ any( [ length != controlled_joints_size and length != 0 @@ -1258,11 +769,10 @@ def _create_traj_action_server_msg( # noqa: C901 ] # Check if trajectory message is valid - if any(waypoints_length_not_equal): - + if any(waypoints_lengths_not_equal_to_joints): # Check if the number of joint positions/velocities/accelerations or # efforts exceeds the number of joints - waypoints_length_to_big = { + waypoints_lengths_to_big = { "positions": any( [ length > controlled_joints_size @@ -1290,21 +800,16 @@ def _create_traj_action_server_msg( # noqa: C901 } # Throw error if aforementioned is the case - if any(waypoints_length_to_big.values()): + if any(waypoints_lengths_to_big.values()): invalid_fields_string = list_2_human_text( - [key for key, val in waypoints_length_to_big.items() if val] + [key for key, val in waypoints_lengths_to_big.items() if val] ) logwarn_message = ( "Your joint trajectory goal message contains more joint %s " - "than the %s joints that are found in the %s." + "than the %s joints that are found in the arm control group." % ( invalid_fields_string, controlled_joints_size, - ( - "arm and hand control groups" - if control_group == "both" - else control_group + "control group" - ), ) ) if verbose: @@ -1322,17 +827,10 @@ def _create_traj_action_server_msg( # noqa: C901 ): # Show warning if less control commands are given than joints logwarn_message = ( "Some of the trajectory waypoints contain less position " - "commands than %s joints that are found in the %s. When " - "this is the case the current joint states will be used for " - "the joints without a position command." - % ( - controlled_joints_size, - ( - "arm and hand control groups" - if control_group == "both" - else control_group + "control group" - ), - ) + "commands than %s joints that are found in the arm control " + "group. When this is the case the current joint states will be " + "used for the joints without a position command." + % (controlled_joints_size,) ) if verbose: rospy.logwarn(logwarn_message) @@ -1345,350 +843,54 @@ def _create_traj_action_server_msg( # noqa: C901 # autofill_traj_positions ROS parameter to True. In this case the position # field will always be filled with the current joint states. for idx, waypoint in enumerate(input_msg.trajectory.points): - - # Add position/velocity/acceleration and effort control commands - if control_group == "arm": - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) + input_msg.time_axis_step - ) - arm_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Add joint position commands - if len(waypoint.positions) != 0: - arm_control_msg.trajectory.points[idx].positions[ - : len(waypoint.positions) - ] = list(waypoint.positions) - else: - # Make sure empty position field stays empty - if not self.autofill_traj_positions: - arm_control_msg.trajectory.points[idx].positions = [] - - # Add joint velocity commands - if len(waypoint.velocities) != 0: - arm_control_msg.trajectory.points[idx].velocities[ - : len(waypoint.velocities) - ] = list(waypoint.velocities) - else: - # Make sure empty velocity field stays empty - arm_control_msg.trajectory.points[idx].velocities = [] - - # Add joint acceleration commands - if len(waypoint.accelerations) != 0: - arm_control_msg.trajectory.points[idx].accelerations[ - : len(waypoint.accelerations) - ] = list(waypoint.accelerations) - else: - # Make sure empty acceleration field stays empty - arm_control_msg.trajectory.points[idx].accelerations = [] - - # Add joint effort commands - if len(waypoint.effort) != 0: - arm_control_msg.trajectory.points[idx].effort[ - : len(waypoint.effort) - ] = list(waypoint.effort) - else: - # Make sure empty effort field stays empty - arm_control_msg.trajectory.points[idx].effort = [] - elif control_group == "hand": - - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) + input_msg.time_axis_step - ) - hand_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Add joint position commands - if len(waypoint.positions) != 0: - hand_control_msg.trajectory.points[idx].positions[ - : len(waypoint.positions) - ] = list(waypoint.positions) - else: - # Make sure empty position field stays empty - if not self.autofill_traj_positions: - hand_control_msg.trajectory.points[idx].positions = [] - - # Add joint velocity commands - if len(waypoint.velocities) != 0: - hand_control_msg.trajectory.points[idx].velocities[ - : len(waypoint.velocities) - ] = list(waypoint.velocities) - else: - # Make sure empty velocity field stays empty - hand_control_msg.trajectory.points[idx].velocities = [] - - # Add joint acceleration commands - if len(waypoint.accelerations) != 0: - hand_control_msg.trajectory.points[idx].accelerations[ - : len(waypoint.accelerations) - ] = list(waypoint.accelerations) - else: - # Make sure empty acceleration field stays empty - hand_control_msg.trajectory.points[idx].accelerations = [] - - # Add joint effort commands - if len(waypoint.effort) != 0: - hand_control_msg.trajectory.points[idx].effort[ - : len(waypoint.effort) - ] = list(waypoint.effort) - else: - # Make sure empty effort field stays empty - hand_control_msg.trajectory.points[idx].effort = [] + # Add time_from_start variable if create_time_axis == TRUE + if input_msg.create_time_axis: + time_axis_tmp = rospy.Duration.from_sec( + (idx * input_msg.time_axis_step) + input_msg.time_axis_step + ) + arm_control_msg.trajectory.points[ + idx + ].time_from_start = time_axis_tmp + + # Add joint position commands + if len(waypoint.positions) != 0: + arm_control_msg.trajectory.points[idx].positions[ + : len(waypoint.positions) + ] = list(waypoint.positions) else: + # Make sure empty position field stays empty + if not self.autofill_traj_positions: + arm_control_msg.trajectory.points[idx].positions = [] + + # Add joint velocity commands + if len(waypoint.velocities) != 0: + arm_control_msg.trajectory.points[idx].velocities[ + : len(waypoint.velocities) + ] = list(waypoint.velocities) + else: + # Make sure empty velocity field stays empty + arm_control_msg.trajectory.points[idx].velocities = [] + + # Add joint acceleration commands + if len(waypoint.accelerations) != 0: + arm_control_msg.trajectory.points[idx].accelerations[ + : len(waypoint.accelerations) + ] = list(waypoint.accelerations) + else: + # Make sure empty acceleration field stays empty + arm_control_msg.trajectory.points[idx].accelerations = [] + + # Add joint effort commands + if len(waypoint.effort) != 0: + arm_control_msg.trajectory.points[idx].effort[ + : len(waypoint.effort) + ] = list(waypoint.effort) + else: + # Make sure empty effort field stays empty + arm_control_msg.trajectory.points[idx].effort = [] - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) + input_msg.time_axis_step - ) - arm_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) + input_msg.time_axis_step - ) - hand_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Check joint state order (hand,arm) or (arm, hand) and add control - # commands accordingly - if self.joint_states.name[0] in controlled_joints_dict["arm"]: - - # Add joint position commands - if len(waypoint.positions) != 0: - if len(waypoint.positions) <= len( - arm_control_msg.trajectory.joint_names - ): # If less commands than there are arm joints - arm_control_msg.trajectory.points[idx].positions[ - : len(waypoint.positions) - ] = list(waypoint.positions) - else: - arm_control_msg.trajectory.points[idx].positions[ - : - ] = waypoint.positions[ - : len(arm_control_msg.trajectory.joint_names) - ] - hand_control_msg.trajectory.points[idx].positions[ - : ( - len(waypoint.positions) - - len(arm_control_msg.trajectory.joint_names) - ) - ] = waypoint.positions[ - len(arm_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty position field stays empty - if not self.autofill_traj_positions: - arm_control_msg.trajectory.points[idx].positions = [] - hand_control_msg.trajectory.points[idx].positions = [] - - # Add joint velocity commands - if len(waypoint.velocities) != 0: - if len(waypoint.velocities) <= len( - arm_control_msg.trajectory.joint_names - ): # If less commands than there are arm joints - arm_control_msg.trajectory.points[idx].velocities[ - : len(waypoint.velocities) - ] = list(waypoint.velocities) - else: - arm_control_msg.trajectory.points[idx].velocities[ - : - ] = waypoint.velocities[ - : len(arm_control_msg.trajectory.joint_names) - ] - hand_control_msg.trajectory.points[idx].velocities[ - : ( - len(waypoint.velocities) - - len(arm_control_msg.trajectory.joint_names) - ) - ] = waypoint.velocities[ - len(arm_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty velocity field stays empty - arm_control_msg.trajectory.points[idx].velocities = [] - hand_control_msg.trajectory.points[idx].velocities = [] - - # Add joint acceleration commands - if len(waypoint.accelerations) != 0: - if len(waypoint.accelerations) <= len( - arm_control_msg.trajectory.joint_names - ): # If less commands than there are arm joints - arm_control_msg.trajectory.points[idx].accelerations[ - : len(waypoint.accelerations) - ] = list(waypoint.accelerations) - else: - arm_control_msg.trajectory.points[idx].accelerations[ - : - ] = waypoint.accelerations[ - : len(arm_control_msg.trajectory.joint_names) - ] - hand_control_msg.trajectory.points[idx].accelerations[ - : ( - len(waypoint.accelerations) - - len(arm_control_msg.trajectory.joint_names) - ) - ] = waypoint.accelerations[ - len(arm_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty acceleration field stays empty - arm_control_msg.trajectory.points[idx].accelerations = [] - hand_control_msg.trajectory.points[idx].accelerations = [] - - # Add joint effort commands - if len(waypoint.effort) != 0: - if len(waypoint.effort) <= len( - arm_control_msg.trajectory.joint_names - ): # If less commands than there are arm joints - arm_control_msg.trajectory.points[idx].effort[ - : len(waypoint.effort) - ] = list(waypoint.effort) - else: - arm_control_msg.trajectory.points[idx].effort[ - : - ] = waypoint.effort[ - : len(arm_control_msg.trajectory.joint_names) - ] - hand_control_msg.trajectory.points[idx].effort[ - : ( - len(waypoint.effort) - - len(arm_control_msg.trajectory.joint_names) - ) - ] = waypoint.effort[ - len(arm_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty effort field stays empty - arm_control_msg.trajectory.points[idx].effort = [] - hand_control_msg.trajectory.points[idx].effort = [] - else: - - # Add joint position commands - if len(waypoint.positions) != 0: - if len(waypoint.positions) <= len( - hand_control_msg.trajectory.joint_names - ): # If less commands than there are hand joints - hand_control_msg.trajectory.points[idx].positions[ - : len(waypoint.positions) - ] = list(waypoint.positions) - else: - hand_control_msg.trajectory.points[idx].positions[ - : - ] = waypoint.positions[ - : len(hand_control_msg.trajectory.joint_names) - ] - arm_control_msg.trajectory.points[idx].positions[ - : ( - len(waypoint.positions) - - len(hand_control_msg.trajectory.joint_names) - ) - ] = waypoint.positions[ - len(hand_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty position field stays empty - if not self.autofill_traj_positions: - arm_control_msg.trajectory.points[idx].positions = [] - hand_control_msg.trajectory.points[idx].positions = [] - - # Add joint velocity commands - if len(waypoint.velocities) != 0: - if len(waypoint.velocities) <= len( - hand_control_msg.trajectory.joint_names - ): # If less commands than there are hand joints - hand_control_msg.trajectory.points[idx].velocities[ - : len(waypoint.velocities) - ] = list(waypoint.velocities) - else: - hand_control_msg.trajectory.points[idx].velocities[ - : - ] = waypoint.velocities[ - : len(hand_control_msg.trajectory.joint_names) - ] - arm_control_msg.trajectory.points[idx].velocities[ - : ( - len(waypoint.velocities) - - len(hand_control_msg.trajectory.joint_names) - ) - ] = waypoint.velocities[ - len(hand_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty velocity field stays empty - arm_control_msg.trajectory.points[idx].velocities = [] - hand_control_msg.trajectory.points[idx].velocities = [] - - # Add joint acceleration commands - if len(waypoint.accelerations) != 0: - if len(waypoint.accelerations) <= len( - hand_control_msg.trajectory.joint_names - ): # If less commands than there are hand joints - hand_control_msg.trajectory.points[idx].accelerations[ - : len(waypoint.accelerations) - ] = list(waypoint.accelerations) - else: - hand_control_msg.trajectory.points[idx].accelerations[ - : - ] = waypoint.accelerations[ - : len(hand_control_msg.trajectory.joint_names) - ] - arm_control_msg.trajectory.points[idx].accelerations[ - : ( - len(waypoint.accelerations) - - len(hand_control_msg.trajectory.joint_names) - ) - ] = waypoint.accelerations[ - len(hand_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty acceleration field stays empty - arm_control_msg.trajectory.points[idx].accelerations = [] - hand_control_msg.trajectory.points[idx].accelerations = [] - - # Add joint effort commands - if len(waypoint.effort) != 0: - if len(waypoint.effort) <= len( - hand_control_msg.trajectory.joint_names - ): # If less commands than there are hand joints - hand_control_msg.trajectory.points[idx].effort[ - : len(waypoint.effort) - ] = list(waypoint.effort) - else: - hand_control_msg.trajectory.points[idx].effort[ - : - ] = waypoint.effort[ - : len(hand_control_msg.trajectory.joint_names) - ] - arm_control_msg.trajectory.points[idx].effort[ - : ( - len(waypoint.effort) - - len(hand_control_msg.trajectory.joint_names) - ) - ] = waypoint.effort[ - len(hand_control_msg.trajectory.joint_names) : - ] - else: - # Make sure empty effort field stays empty - arm_control_msg.trajectory.points[idx].effort = [] - hand_control_msg.trajectory.points[idx].effort = [] - - return { - "arm": panda_action_msg_2_control_msgs_action_msg(arm_control_msg), - "hand": panda_action_msg_2_control_msgs_action_msg(hand_control_msg), - } + return panda_action_msg_2_control_msgs_action_msg(arm_control_msg) else: # If joints were specified - # Check if the number of joint positions/velocities/accelerations or # efforts is unequal to the the number of joints waypoints_length_not_equal = { @@ -1750,20 +952,17 @@ def _create_traj_action_server_msg( # noqa: C901 logwarn_msg_strings = [ "Joint" if len(invalid_joint_names) == 1 else "Joints", "was" if len(invalid_joint_names) == 1 else "were", - "panda_gazebo/SetJointPositions", + "panda_gazebo/SetJointPositions", # NOTE: Why? ] logwarn_message = ( "%s that %s specified in the 'joint_names' field of the '%s' " "message %s invalid. Valid joint names for controlling the " - "Panda %s are %s." + "Panda arm are %s." % ( "%s %s" % (logwarn_msg_strings[0], invalid_joint_names), logwarn_msg_strings[1], logwarn_msg_strings[2], logwarn_msg_strings[1], - control_group - if control_group in ["hand", "arm"] - else "arm & hand", controlled_joints, ) ) @@ -1784,8 +983,6 @@ def _create_traj_action_server_msg( # noqa: C901 # case the position field will always be filled with the current # joint states. for idx, waypoint in enumerate(input_msg.trajectory.points): - - # Create input control command dictionary input_command_dict = { "positions": OrderedDict( zip(joint_names, waypoint.positions) @@ -1799,454 +996,124 @@ def _create_traj_action_server_msg( # noqa: C901 "efforts": OrderedDict(zip(joint_names, waypoint.effort)), } - # Create position/velocity/acceleration and effort control - # command arrays - if control_group == "arm": + # Add time_from_start variable if create_time_axis == TRUE + if input_msg.create_time_axis: + time_axis_tmp = rospy.Duration.from_sec( + (idx * input_msg.time_axis_step) + + input_msg.time_axis_step + ) + arm_control_msg.trajectory.points[ + idx + ].time_from_start = time_axis_tmp + + # Create position command array + if len(waypoint.positions) != 0: + arm_position_commands_dict = copy.deepcopy( + arm_state_dict["positions"] + ) # Start from the current states + for (joint, position,) in input_command_dict[ + "positions" + ].items(): # Add control commands + if joint in arm_state_dict["positions"]: + arm_position_commands_dict[joint] = position + arm_position_commands = arm_position_commands_dict.values() + else: + # Make sure empty position field stays empty + if not self.autofill_traj_positions: + arm_position_commands = [] - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) - + input_msg.time_axis_step - ) - arm_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Create position command array - if len(waypoint.positions) != 0: - arm_position_commands_dict = copy.deepcopy( - arm_state_dict["positions"] - ) # Start from the current states - for (joint, position,) in input_command_dict[ - "positions" - ].items(): # Add control commands - if joint in arm_state_dict["positions"]: - arm_position_commands_dict[joint] = position - arm_position_commands = ( - arm_position_commands_dict.values() - ) - hand_position_commands = hand_state_dict[ - "positions" - ].values() - else: - # Make sure empty position field stays empty - if not self.autofill_traj_positions: - arm_position_commands = [] - hand_position_commands = [] - - # Create velocity command array - if len(waypoint.velocities) != 0: - arm_velocity_commands_dict = copy.deepcopy( - arm_state_dict["velocities"] - ) # Start from the current states - for (joint, velocity,) in input_command_dict[ - "velocities" - ].items(): # Add control commands - if joint in arm_state_dict["velocities"]: - arm_velocity_commands_dict[joint] = velocity - arm_velocity_commands = ( - arm_velocity_commands_dict.values() - ) - hand_velocity_commands = hand_state_dict[ - "velocities" - ].values() - else: - # Make sure empty velocity field stays empty - arm_velocity_commands = [] - hand_velocity_commands = [] - - # Create acceleration command array - if len(waypoint.accelerations) != 0: - arm_acceleration_commands_dict = OrderedDict( - zip( - arm_state_dict["velocities"].keys(), - [0.0] - * len(arm_state_dict["velocities"].keys()), - ) - ) # Initiate at 0 as accelerations are unknown - for (joint, acceleration,) in input_command_dict[ - "accelerations" - ].items(): # Add control commands - if joint in arm_state_dict["velocities"]: - arm_acceleration_commands_dict[ - joint - ] = acceleration - arm_acceleration_commands = ( - arm_acceleration_commands_dict.values() - ) - hand_acceleration_commands = [0.0] * len( - hand_control_msg.trajectory.joint_names - ) # Initiate at 0 as accelerations are unknown - else: - # Make sure empty acceleration field stays empty - arm_acceleration_commands = [] - hand_acceleration_commands = [] - - # Create effort command array - if len(waypoint.effort) != 0: - arm_effort_commands_dict = copy.deepcopy( - arm_state_dict["efforts"] - ) # Start from the current states - for (joint, effort,) in input_command_dict[ - "efforts" - ].items(): # Add control commands - if joint in arm_state_dict["efforts"]: - arm_effort_commands_dict[joint] = effort - arm_effort_commands = arm_effort_commands_dict.values() - hand_effort_commands = hand_state_dict[ - "efforts" - ].values() - else: - # Make sure empty effort field stays empty - arm_effort_commands = [] - hand_effort_commands = [] - elif control_group == "hand": - - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) - + input_msg.time_axis_step - ) - hand_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Create position command array - if len(waypoint.positions) != 0: - hand_position_commands_dict = copy.deepcopy( - hand_state_dict["positions"] - ) # Start from the current states - for (joint, position,) in input_command_dict[ - "positions" - ].items(): # Add control commands - if joint in hand_state_dict["positions"]: - hand_position_commands_dict[joint] = position - arm_position_commands = arm_state_dict[ - "positions" - ].values() - hand_position_commands = ( - hand_position_commands_dict.values() - ) - else: - # Make sure empty position field stays empty - if not self.autofill_traj_positions: - arm_position_commands = [] - hand_position_commands = [] - - # Create velocity command array - if len(waypoint.velocities) != 0: - hand_velocity_commands_dict = copy.deepcopy( - hand_state_dict["velocities"] - ) # Start from the current states - for (joint, velocity,) in input_command_dict[ - "velocities" - ].items(): # Add control commands - if joint in hand_state_dict["velocities"]: - hand_velocity_commands_dict[joint] = velocity - arm_velocity_commands = arm_state_dict[ - "velocities" - ].values() - hand_velocity_commands = ( - hand_velocity_commands_dict.values() - ) - else: - # Make sure empty velocity field stays empty - arm_velocity_commands = [] - hand_velocity_commands = [] - - # Create acceleration command array - if len(waypoint.accelerations) != 0: - hand_acceleration_commands_dict = OrderedDict( - zip( - hand_state_dict["velocities"].keys(), - [0.0] - * len(hand_state_dict["velocities"].keys()), - ) - ) # Initiate at 0 as accelerations are unknown - for (joint, acceleration,) in input_command_dict[ - "accelerations" - ].items(): # Add control commands - if joint in hand_state_dict["velocities"]: - hand_acceleration_commands_dict[ - joint - ] = acceleration - arm_acceleration_commands = [0.0] * len( - arm_control_msg.trajectory.joint_names - ) # Initiate at 0 as accelerations are unknown - hand_acceleration_commands = ( - hand_acceleration_commands_dict.values() - ) - else: - # Make sure empty acceleration field stays empty - arm_acceleration_commands = [] - hand_acceleration_commands = [] - - # Create effort command array - if len(waypoint.effort) != 0: - hand_effort_commands_dict = copy.deepcopy( - hand_state_dict["efforts"] - ) # Start from the current states - for (joint, effort,) in input_command_dict[ - "efforts" - ].items(): # Add control commands - if joint in hand_state_dict["efforts"]: - hand_effort_commands_dict[joint] = effort - arm_effort_commands = arm_state_dict["efforts"].values() - hand_effort_commands = ( - hand_effort_commands_dict.values() - ) - else: - # Make sure empty effort field stays empty - arm_effort_commands = [] - hand_effort_commands = [] + # Create velocity command array + if len(waypoint.velocities) != 0: + arm_velocity_commands_dict = copy.deepcopy( + arm_state_dict["velocities"] + ) # Start from the current states + for (joint, velocity,) in input_command_dict[ + "velocities" + ].items(): # Add control commands + if joint in arm_state_dict["velocities"]: + arm_velocity_commands_dict[joint] = velocity + arm_velocity_commands = arm_velocity_commands_dict.values() else: + # Make sure empty velocity field stays empty + arm_velocity_commands = [] - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) - + input_msg.time_axis_step - ) - arm_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Add time_from_start variable if create_time_axis == TRUE - if input_msg.create_time_axis: - time_axis_tmp = rospy.Duration.from_sec( - (idx * input_msg.time_axis_step) - + input_msg.time_axis_step - ) - hand_control_msg.trajectory.points[ - idx - ].time_from_start = time_axis_tmp - - # Create arm position control message - if len(waypoint.positions) != 0: - arm_position_commands_dict = copy.deepcopy( - arm_state_dict["positions"] - ) # Start from the current states - for (joint, position,) in input_command_dict[ - "positions" - ].items(): # Add control commands - if joint in arm_state_dict["positions"]: - arm_position_commands_dict[joint] = position - - # Create hand position control message - hand_position_commands_dict = copy.deepcopy( - hand_state_dict["positions"] - ) # Start from the current states - for (joint, position,) in input_command_dict[ - "positions" - ].items(): # Add control commands - if joint in hand_state_dict["positions"]: - hand_position_commands_dict[joint] = position - - # Retrieve commands from dictionaries - arm_position_commands = ( - arm_position_commands_dict.values() - ) - hand_position_commands = ( - hand_position_commands_dict.values() - ) - else: - # Make sure empty position field stays empty - if not self.autofill_traj_positions: - arm_position_commands = [] - hand_position_commands = [] - - # Create arm velocity control message - if len(waypoint.velocities) != 0: - arm_velocity_commands_dict = copy.deepcopy( - arm_state_dict["velocities"] - ) # Start from the current states - for (joint, velocity,) in input_command_dict[ - "velocities" - ].items(): # Add control commands - if joint in arm_state_dict["velocities"]: - arm_velocity_commands_dict[joint] = velocity - - # Create hand velocity control message - hand_velocity_commands_dict = copy.deepcopy( - hand_state_dict["velocities"] - ) # Start from the current states - for (joint, velocity,) in input_command_dict[ - "velocities" - ].items(): # Add control commands - if joint in hand_state_dict["velocities"]: - hand_velocity_commands_dict[joint] = velocity - - # Retrieve commands from dictionaries - arm_velocity_commands = ( - arm_velocity_commands_dict.values() - ) - hand_velocity_commands = ( - hand_velocity_commands_dict.values() - ) - else: - # Make sure empty velocity field stays empty - arm_velocity_commands = [] - hand_velocity_commands = [] - - # Create arm acceleration control message - if len(waypoint.accelerations) != 0: - arm_acceleration_commands_dict = OrderedDict( - zip( - arm_state_dict["velocities"].keys(), - [0.0] - * len(arm_state_dict["velocities"].keys()), - ) - ) # Initiate at 0 as accelerations are unknown - for (joint, acceleration,) in input_command_dict[ - "accelerations" - ].items(): # Add control commands - if joint in arm_state_dict["velocities"]: - arm_acceleration_commands_dict[ - joint - ] = acceleration - - # Create hand acceleration control message - hand_acceleration_commands_dict = OrderedDict( - zip( - hand_state_dict["velocities"].keys(), - [0.0] - * len(hand_state_dict["velocities"].keys()), - ) - ) # Initiate at 0 as accelerations are unknown - for (joint, acceleration,) in input_command_dict[ - "accelerations" - ].items(): # Add control commands - if joint in hand_state_dict["velocities"]: - hand_acceleration_commands_dict[ - joint - ] = acceleration - - # Retrieve commands from dictionaries - arm_acceleration_commands = ( - arm_acceleration_commands_dict.values() - ) - hand_acceleration_commands = ( - hand_acceleration_commands_dict.values() - ) - else: - # Make sure empty acceleration field stays empty - arm_acceleration_commands = [] - hand_acceleration_commands = [] - - # Create arm effort control message - if len(waypoint.effort) != 0: - arm_effort_commands_dict = copy.deepcopy( - arm_state_dict["efforts"] - ) # Start from the current states - for (joint, effort,) in input_command_dict[ - "efforts" - ].items(): # Add control commands - if joint in arm_state_dict["efforts"]: - arm_effort_commands_dict[joint] = effort - - # Create hand effort control message - hand_effort_commands_dict = copy.deepcopy( - hand_state_dict["efforts"] - ) # Start from the current states - for (joint, effort,) in input_command_dict[ - "efforts" - ].items(): # Add control commands - if joint in hand_state_dict["efforts"]: - hand_effort_commands_dict[joint] = effort - arm_effort_commands = arm_effort_commands_dict.values() - hand_effort_commands = ( - hand_effort_commands_dict.values() + # Create acceleration command array + if len(waypoint.accelerations) != 0: + arm_acceleration_commands_dict = OrderedDict( + zip( + arm_state_dict["velocities"].keys(), + [0.0] * len(arm_state_dict["velocities"].keys()), ) - else: - # Make sure empty effort field stays empty - arm_effort_commands = [] - hand_effort_commands = [] + ) # Initiate at 0 as accelerations are unknown + for (joint, acceleration,) in input_command_dict[ + "accelerations" + ].items(): # Add control commands + if joint in arm_state_dict["velocities"]: + arm_acceleration_commands_dict[joint] = acceleration + arm_acceleration_commands = ( + arm_acceleration_commands_dict.values() + ) + else: + # Make sure empty acceleration field stays empty + arm_acceleration_commands = [] + + # Create effort command array + if len(waypoint.effort) != 0: + arm_effort_commands_dict = copy.deepcopy( + arm_state_dict["efforts"] + ) # Start from the current states + for (joint, effort,) in input_command_dict[ + "efforts" + ].items(): # Add control commands + if joint in arm_state_dict["efforts"]: + arm_effort_commands_dict[joint] = effort + arm_effort_commands = arm_effort_commands_dict.values() + else: + # Make sure empty effort field stays empty + arm_effort_commands = [] # Add new control commands to joint trajectory control message # waypoint arm_control_msg.trajectory.points[ idx ].positions = arm_position_commands - hand_control_msg.trajectory.points[ - idx - ].positions = hand_position_commands arm_control_msg.trajectory.points[ idx ].velocities = arm_velocity_commands - hand_control_msg.trajectory.points[ - idx - ].velocities = hand_velocity_commands arm_control_msg.trajectory.points[ idx ].accelerations = arm_acceleration_commands - hand_control_msg.trajectory.points[ - idx - ].accelerations = hand_acceleration_commands arm_control_msg.trajectory.points[ idx ].effort = arm_effort_commands - hand_control_msg.trajectory.points[ - idx - ].effort = hand_effort_commands - return { - "arm": panda_action_msg_2_control_msgs_action_msg( - arm_control_msg - ), - "hand": panda_action_msg_2_control_msgs_action_msg( - hand_control_msg - ), - } + return panda_action_msg_2_control_msgs_action_msg(arm_control_msg) def _create_control_publisher_msg( # noqa: C901 - self, input_msg, control_type, control_group, verbose=False + self, input_msg, control_type, verbose=False ): - """Converts the service input message into a control commands that is used by - the control publishers. While doing this it also verifies whether the given - input message is valid. + """Converts the input message of the position/effort control service into a + control commands that is used by the control publishers. While doing this it + also verifies whether the given input message is valid. Args: - input_msg (:obj:`trajectory_msgs/JointTrajectory`): The service input - message we want to validate. + input_msg (union[:obj:`panda_gazebo.msgs.SetJointPositions`,:obj:`panda_gazebo.msgs.SetJointEfforts`]): + The service input message we want to convert and validate. control_type (str): The type of control that is being executed and on which we should wait. Options are ``effort_control`` and ``position_control``. - control_group (str): The robot control group which is being controlled. - Options are ``arm``, ``hand`` or ``both``. - verbose (bool, optional): Boolean specifying whether you want to send a - warning message to the ROSlogger. Defaults to ``False``. + verbose (bool, optional): Boolean specifying whether you want to send + warning and error message to the ROSlogger. Defaults to ``False``. Returns: - (tuple): tuple containing: - - - :obj:`dict`: The Panda arm and hand control commands in the order - which is are required by the publishers. - - :obj:`dict`: The joints that are being controlled. + dict: The Panda arm control commands in the order which is are required by + the publishers. Raises: :obj:`panda_gazebo.exceptions.InputMessageInvalidError`: Raised when the - input_msg could not be converted into ``moveit_commander`` arm hand - joint position/effort commands. - """ - # Validate control_group argument - if control_group.lower() not in ["arm", "hand", "both"]: - logwarn_message = ( - "Control group '%s' does not exist. Please specify a valid control " - "group. Valid values are %s." - % (control_group.lower(), "['arm', 'hand', 'both']") - ) - if verbose: - rospy.logwarn(logwarn_message) - raise InputMessageInvalidError( - message="Control_group '%s' invalid." % control_group.lower(), - log_message=logwarn_message, - ) - else: - control_group = control_group.lower() - control_type = control_type.lower() - + input_msg could not be converted into ``moveit_commander`` arm joint + position/effort commands. + """ # noqa: E501 # Validate control_type argument and extract information from input message if control_type == "position_control": joint_names = input_msg.joint_names @@ -2267,59 +1134,23 @@ def _create_control_publisher_msg( # noqa: C901 ) # Retrieve controlled joints - try: - controlled_joints_dict = self._get_controlled_joints( - control_type=control_type, verbose=verbose - ) - except InputMessageInvalidError: - logwarn_message = ( - "The '%s' publisher messages could not be created as the '%s' %s " - "are not initialized." - % ( - "effort control" - if control_type == "effort_control" - else "position control", - self._effort_controllers - if control_type == "effort_control" - else self._position_controllers, - "joint effort controllers" - if control_type == "effort_control" - else "joint position controllers", - ) - ) - if verbose: - rospy.logwarn(logwarn_message) - raise InputMessageInvalidError( - message="Required controllers not initialised.", - details={"controlled_joints": self._effort_controllers}, - log_message=logwarn_message, - ) + controlled_joints_dict = self._get_controlled_joints( + control_type=control_type, verbose=verbose + ) - # Get controlled joints and robot state - if control_group == "arm": - controlled_joints = controlled_joints_dict["arm"] - elif control_group == "hand": - controlled_joints = controlled_joints_dict["hand"] - else: - controlled_joints = controlled_joints_dict["both"] + # Get controlled joints and arm state + controlled_joints = controlled_joints_dict["arm"] controlled_joints_size = len(controlled_joints) state_dict = self._retrieve_state_dict(controlled_joints_dict) # Get control publisher message type and current joint states if control_type == "position_control": arm_state_dict = state_dict["arm"]["positions"] - hand_state_dict = state_dict["hand"]["positions"] - arm_msg_type = self._arm_position_controller_msg_type - hand_msg_type = self._hand_position_controller_msg_type elif control_type == "effort_control": arm_state_dict = state_dict["arm"]["efforts"] - hand_state_dict = state_dict["hand"]["efforts"] - arm_msg_type = self._arm_effort_controller_msg_type - hand_msg_type = self._hand_effort_controller_msg_type # Check service request input if len(joint_names) == 0: # If not joint_names were given - # Check if enough joint position commands were given otherwise give warning if len(control_input) != controlled_joints_size: if control_type == "position_control": @@ -2337,12 +1168,12 @@ def _create_control_publisher_msg( # noqa: C901 # Check if control input is bigger than controllable joints if len(control_input) > controlled_joints_size: - logwarn_message = "You specified %s while the Panda %s %s." % ( - "%s %s" % (len(control_input), logwarn_msg_strings[0]), - control_group + " control group has" - if control_group in ["arm", "hand"] - else "arm and hand control groups have", - "%s %s" % (controlled_joints_size, logwarn_msg_strings[1]), + logwarn_message = ( + "You specified %s while the Panda arm control group has %s." + % ( + "%s %s" % (len(control_input), logwarn_msg_strings[0]), + "%s %s" % (controlled_joints_size, logwarn_msg_strings[1]), + ) ) if verbose: rospy.logwarn(logwarn_message) @@ -2355,55 +1186,24 @@ def _create_control_publisher_msg( # noqa: C901 }, ) elif len(control_input) < controlled_joints_size: - logwarn_message = "You specified %s while the Panda %s %s." % ( - "%s %s" % (len(control_input), logwarn_msg_strings[0]), - control_group + " control group has" - if control_group in ["arm", "hand"] - else "arm and hand control groups have", - "%s %s" % (controlled_joints_size, logwarn_msg_strings[1]), - ) + " As a result only joints %s will be controlled." % ( - controlled_joints[: len(control_input)] + logwarn_message = ( + "You specified %s while the Panda arm control group has %s." + % ( + "%s %s" % (len(control_input), logwarn_msg_strings[0]), + "%s %s" % (controlled_joints_size, logwarn_msg_strings[1]), + ) + + " As a result only joints %s will be controlled." + % (controlled_joints[: len(control_input)]) ) rospy.logwarn(logwarn_message) # Update current state dictionary with given joint_position commands - if control_group == "arm": - arm_position_commands = list(arm_state_dict.values()) - arm_position_commands[: len(control_input)] = control_input - hand_position_commands = list(hand_state_dict.values()) - elif control_group == "hand": - hand_position_commands = list(hand_state_dict) - arm_position_commands = list(arm_state_dict.values()) - else: - arm_position_commands = list(arm_state_dict.values()) - hand_position_commands = list(hand_state_dict.values()) - if self.joint_states.name[0] in controlled_joints_dict["arm"]: - if len(control_input) <= len(arm_position_commands): - arm_position_commands[: len(control_input)] = control_input - else: - arm_position_commands[:] = control_input[ - : len(arm_position_commands) - ] - hand_position_commands[ - : len(control_input) - len(arm_position_commands) - ] = control_input[len(arm_position_commands) :] - else: - if len(control_input) <= len(hand_position_commands): - hand_position_commands[: len(control_input)] = control_input - else: - hand_position_commands[:] = control_input[ - : len(hand_position_commands) - ] - arm_position_commands[ - : len(control_input) - len(hand_position_commands) - ] = control_input[len(hand_position_commands) :] + arm_position_commands = list(arm_state_dict.values()) + arm_position_commands[: len(control_input)] = control_input # Return publishers command dictionary - control_commands = { - "arm": [arm_msg_type(item) for item in arm_position_commands], - "hand": [hand_msg_type(item) for item in hand_position_commands], - } - return control_commands, controlled_joints_dict + control_commands = [Float64(item) for item in arm_position_commands] + return control_commands else: # Check if enough control values were given if len(joint_names) != len(control_input): @@ -2475,15 +1275,12 @@ def _create_control_publisher_msg( # noqa: C901 logwarn_message = ( "%s that %s specified in the 'joint_names' field of the '%s' " "message %s invalid. Valid joint names for controlling the " - "Panda %s are %s." + "Panda arm are %s." % ( "%s %s" % (logwarn_msg_strings[0], invalid_joint_names), logwarn_msg_strings[1], logwarn_msg_strings[2], logwarn_msg_strings[1], - control_group - if control_group in ["hand", "arm"] - else "arm & hand", controlled_joints, ) ) @@ -2501,72 +1298,99 @@ def _create_control_publisher_msg( # noqa: C901 input_command_dict = OrderedDict(zip(joint_names, control_input)) # Update current state dictionary with given joint_position commands - if control_group == "arm": - arm_position_commands_dict = copy.deepcopy( - arm_state_dict - ) # Start from the current state - for ( - joint, - position, - ) in input_command_dict.items(): # Add control commands - if joint in arm_state_dict: - arm_position_commands_dict[joint] = position - arm_position_commands = arm_position_commands_dict.values() - hand_position_commands = hand_state_dict.values() - elif control_group == "hand": - hand_position_commands_dict = copy.deepcopy( - hand_state_dict - ) # Start from the current state - for ( - joint, - position, - ) in input_command_dict.items(): # Add control commands - if joint in hand_state_dict: - hand_position_commands_dict[joint] = position - arm_position_commands = arm_state_dict.values() - hand_position_commands = hand_position_commands_dict.values() - else: - - # Create arm control message - arm_position_commands_dict = copy.deepcopy( - arm_state_dict - ) # Start from the current state - for ( - joint, - position, - ) in input_command_dict.items(): # Add control commands - if joint in arm_state_dict: - arm_position_commands_dict[joint] = position - - # Create hand control message - hand_position_commands_dict = copy.deepcopy( - hand_state_dict - ) # Start from the current state - for ( - joint, - position, - ) in input_command_dict.items(): # Add control commands - if joint in hand_state_dict: - hand_position_commands_dict[joint] = position - arm_position_commands = arm_position_commands_dict.values() - hand_position_commands = hand_position_commands_dict.values() + arm_position_commands_dict = copy.deepcopy( + arm_state_dict + ) # Start from the current state + for ( + joint, + position, + ) in input_command_dict.items(): # Add control commands + if joint in arm_state_dict: + arm_position_commands_dict[joint] = position + arm_position_commands = arm_position_commands_dict.values() # Return publishers command dictionary - control_commands = { - "arm": [arm_msg_type(item) for item in arm_position_commands], - "hand": [ - hand_msg_type(item) for item in hand_position_commands - ], - } - return control_commands, controlled_joints_dict + control_commands = [Float64(item) for item in arm_position_commands] + return control_commands + + def _split_joint_commands_req( + self, joint_commands_req, control_type, verbose=False + ): + """Splits the combined :obj:`~panda_gazebo.msg.SetJointControlCommandRequest` + message into separate arm and gripper messages. + + Args: + joint_commands_req :obj:`~panda_gazebo.msg.SetJointControlCommandRequest`): + The joint control command message. + control_type (str): The type of control that is being executed. Options + are ``effort_control``, ``position_control`` and ``traj_control``. + verbose (bool, optional): Boolean specifying whether you want to send a + warning message to the ROS logger. Defaults to ``False``. + + Returns: + (tuple): tuple containing: + - union[:obj:`~panda_gazebo.msg.SetJointPositionsRequest`, :obj:`~panda_gazebo.msg.SetJointEffortsRequest`, ``None``]: + The arm set joint position/effort message or ``None`` if no + joint positions/efforts were found in the joint control command. + - union[:obj:`~franka_gripper.msg.MoveGoal`, ``None``]: The gripper + action goal message or ``None`` if 'gripper_width' was not found in + the joint control command. + """ # noqa: E501 + # Retrieve controlled joints + controlled_arm_joints = self._get_controlled_joints( + control_type=control_type, verbose=verbose + )["arm"] + controlled_arm_joints = ( + PANDA_JOINTS["arm"] if not controlled_arm_joints else controlled_arm_joints + ) + + # Split control message + joint_commands_dict = dict( + zip(joint_commands_req.joint_names, joint_commands_req.joint_commands) + ) + arm_joint_commands_dict = { + key: val + for key, val in joint_commands_dict.items() + if key in controlled_arm_joints + } + gripper_width_command = [ + val + for key, val in joint_commands_dict.items() + if key.lower() == "gripper_width" + ] + + # Create arm control message + if arm_joint_commands_dict: + if control_type == "position_control": + arm_req = SetJointPositions() + arm_req.joint_names = list(arm_joint_commands_dict.keys()) + arm_req.joint_positions = list(arm_joint_commands_dict.values()) + arm_req.wait = joint_commands_req.wait + else: + arm_req = SetJointEfforts() + arm_req.joint_names = list(arm_joint_commands_dict.keys()) + arm_req.joint_efforts = list(arm_joint_commands_dict.values()) + arm_req.wait = joint_commands_req.wait + else: + arm_req = None + + # Create hand control message + # NOTE: Use 'kDefaultGripperActionSpeed' see 'franka_gripper.sim.h' + if gripper_width_command: + gripper_req = MoveGoal() + gripper_req.width = gripper_width_command[0] + gripper_req.speed = 0.1 + else: + gripper_req = None + + return arm_req, gripper_req def _get_controlled_joints(self, control_type, verbose=False): # noqa: C901 """Returns the joints that can be controlled by a given control type. Args: - control_type (str): The type of control that is being executed and on which - we should wait. Options are ``effort_control`` and - ``position_control``. + control_type (str): The type of control that is being executed. Options + are ``effort_control``, ``position_control`` and ``traj_control``. verbose (bool, optional): Boolean specifying whether you want to send a warning message to the ROS logger. Defaults to ``False``. @@ -2576,48 +1400,48 @@ def _get_controlled_joints(self, control_type, verbose=False): # noqa: C901 Raises: :obj:`panda_gazebo.exceptions.InputMessageInvalidError`: Raised when the - the control_type is invalid. + the ``control_type`` is invalid. """ control_type = control_type.lower() # Get the joints which are contolled by a given control type controlled_joints_dict = OrderedDict(zip(["arm", "hand", "both"], [[], [], []])) if control_type == "position_control": - for position_controller in self._position_controllers: + for controller in ARM_POSITION_CONTROLLERS + HAND_CONTROLLERS: try: for claimed_resources in self.controllers[ - position_controller + controller ].claimed_resources: for resource in claimed_resources.resources: - if position_controller in self.arm_position_controllers: + if controller in ARM_POSITION_CONTROLLERS: controlled_joints_dict["arm"].append(resource) - elif position_controller in self.hand_position_controllers: + elif controller in HAND_CONTROLLERS: controlled_joints_dict["hand"].append(resource) except KeyError: # Controller not initialized pass elif control_type == "effort_control": - for effort_controller in self._effort_controllers: + for controller in ARM_EFFORT_CONTROLLERS + HAND_CONTROLLERS: try: for claimed_resources in self.controllers[ - effort_controller + controller ].claimed_resources: for resource in claimed_resources.resources: - if effort_controller in self.arm_effort_controllers: + if controller in ARM_EFFORT_CONTROLLERS: controlled_joints_dict["arm"].append(resource) - elif effort_controller in self.hand_effort_controllers: + elif controller in HAND_CONTROLLERS: controlled_joints_dict["hand"].append(resource) except KeyError: # Controller not initialized pass elif control_type == "traj_control" or control_type == "ee_control": - for traj_controller in self._traj_controllers: + for controller in ARM_TRAJ_CONTROLLERS + HAND_CONTROLLERS: try: for claimed_resources in self.controllers[ - traj_controller + controller ].claimed_resources: for resource in claimed_resources.resources: - if traj_controller in self.arm_traj_controllers: + if controller in ARM_TRAJ_CONTROLLERS: controlled_joints_dict["arm"].append(resource) - elif traj_controller in self.hand_traj_controllers: + elif controller in HAND_CONTROLLERS: controlled_joints_dict["hand"].append(resource) except KeyError: # Controller not initialized pass @@ -2634,8 +1458,6 @@ def _get_controlled_joints(self, control_type, verbose=False): # noqa: C901 ) # Return controlled joints dict - controlled_joints_dict["arm"] = flatten_list(controlled_joints_dict["arm"]) - controlled_joints_dict["hand"] = flatten_list(controlled_joints_dict["hand"]) controlled_joints_dict["both"] = ( flatten_list( [controlled_joints_dict["hand"], controlled_joints_dict["arm"]] @@ -2667,11 +1489,37 @@ def _get_joint_controllers(self): return joint_controllers_dict + def _controllers_running(self, controllers): + """Check if all controllers that are needed for controlling a given control + group are running. + + Args: + controllers (list): A list of controllers for which you want to check if + they are running. + + Returns: + (tuple): tuple containing: + + - missing_controllers (:obj:`list`): The controllers that are not + loaded. + - stopped_controllers (:obj:`list`): The controllers that are loaded but + not started. + """ + missing_controllers = [] + stopped_controllers = [] + for controller in ARM_POSITION_CONTROLLERS: + try: + if self.controllers[controller].state != "running": + stopped_controllers.append(controller) + except KeyError: + missing_controllers.append(controller) + return missing_controllers, stopped_controllers + @property def controllers(self): """Retrieves information about the currently running controllers.""" return controller_list_array_2_dict( - self.list_controllers_client.call(ListControllersRequest()) + self._list_controllers_client.call(ListControllersRequest()) ) ################################################ @@ -2684,18 +1532,78 @@ def _joints_cb(self, data): ################################################ # Control services callback functions ########## ################################################ - def _set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 - """Request arm and hand joint position control. + def _set_joint_commands_cb(self, set_joint_commands_req): # noqa: C901 + """Request arm and hand joint command control. + + Args: + set_joint_commands_req (:obj:`panda_gazebo.srv.SetJointPositionsRequest`): + Service request message specifying the joint position/effort commands + for the robot arm and hand joints. + + Returns: + :obj:`panda_gazebo.srv.SetJointCommandsResponse`: Service response. + """ + # Validate the control type + resp = SetJointCommandsResponse() + if set_joint_commands_req.control_type not in [ + "position_control", + "effort_control", + ]: + rospy.logwarn( + "Please specify a valid control type. Valid values are %s." + % ("['position_control', 'effort_control']") + ) + resp.success = False + resp.message = "Specified 'control_type' invalid." + return resp + else: + control_type = set_joint_commands_req.control_type.lower() + + # Split input message and send arm and gripper control commands + arm_command_msg, gripper_command_msg = self._split_joint_commands_req( + set_joint_commands_req, control_type + ) + + # Send control commands + if gripper_command_msg is not None: + self._gripper_move_client.send_goal(gripper_command_msg) + if arm_command_msg is not None: + if control_type == "position_control": + arm_resp = self._arm_set_joint_positions_cb(arm_command_msg) + else: + arm_resp = self._arm_set_joint_efforts_cb(arm_command_msg) + if set_joint_commands_req.wait: + if gripper_command_msg is not None: + gripper_result = self._gripper_move_client.wait_for_result() + + # Return result + resp = SetJointCommandsResponse() + if all([gripper_result, arm_resp.success]): + resp.success = True + resp.message = "Everything went OK" + elif all([not item for item in [gripper_result, arm_resp.success]]): + resp.success = False + resp.message = "Joint control failed" + elif not arm_resp.success: + resp.success = False + resp.message = "Arm control failed" + elif not gripper_result: + resp.success = False + resp.message = "Gripper control failed" + return resp + + def _arm_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 + """Request arm joint position control. Args: - set_joint_positions_req (:obj:`panda_gazebo.srv.SetJointPositionsRequest`: - Service request message specifying the positions for the robot arm and - hand joints. + set_joint_positions_req (:obj:`panda_gazebo.srv.SetJointPositionsRequest`): + Service request message specifying the positions for the robot arm + joints. Returns: :obj:`panda_gazebo.srv.SetJointPositionsResponse`: Service response. """ - # Check if set_joint_efforts_req.joint_names contains duplicates + # Check if set_joint_positions_req.joint_names contains duplicates duplicate_list = get_duplicate_list(set_joint_positions_req.joint_names) if duplicate_list: rospy.logwarn( @@ -2705,622 +1613,22 @@ def _set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 % ( "joints" if len(duplicate_list) > 1 else "joint", duplicate_list, - "%s/set_joint_positions" % rospy.get_name(), + "%s/panda_arm/set_joint_positions" % rospy.get_name(), ) ) - # Create required variables and messages - controllers_missing = {"arm": False, "hand": False} - resp = SetJointPositionsResponse() - # Check if all controllers are available and running - stopped_controllers = {"arm": [], "hand": []} - missing_controllers = {"arm": [], "hand": []} - for (group, position_controllers) in { - "arm": self.arm_position_controllers, - "hand": self.hand_position_controllers, - }.items(): - for position_controller in position_controllers: - try: - if self.controllers[position_controller].state != "running": - stopped_controllers[group].append(position_controller) - except KeyError: - missing_controllers[group].append(position_controller) + resp = SetJointPositionsResponse() + missing_controllers, stopped_controllers = self._controllers_running( + ARM_POSITION_CONTROLLERS + ) # Return failed result if we miss a controller - if len(flatten_list(missing_controllers.values())) >= 1: + if len(missing_controllers) >= 1: rospy.logwarn( - "Panda arm and hand joint position command could not be send as the %s " - "%s not initialized. Please make sure you load the controller " - "parameters onto the ROS parameter server." - % ( - flatten_list(missing_controllers.values()), - "joint position controller is" - if len(flatten_list(missing_controllers.values())) == 1 - else "joint position controllers are", - ) - ) - missing_controllers_group_string = ( - "Arm and hand" - if all( - [values != [] for (group, values) in missing_controllers.items()] - ) - else ("Arm" if "arm" in dict_clean(missing_controllers) else "Hand") - ) - resp.success = False - resp.message = ( - "%s controllers not initialised." % missing_controllers_group_string - ) - return resp - - # Validate request and create control publisher message - try: - control_pub_msgs, controlled_joints = self._create_control_publisher_msg( - input_msg=set_joint_positions_req, - control_type="position_control", - control_group="both", - ) - except InputMessageInvalidError as e: - logwarn_msg = "Panda arm joint positions not set as " + lower_first_char( - e.log_message - ) - rospy.logwarn(logwarn_msg) - resp.success = False - resp.message = e.message - return resp - - # Check if required joints are running - if len(flatten_list(stopped_controllers.values())) >= 1: - - # Check from which group the controllers are missing - controllers_missing = { - group: (True if (position_controllers) else False) - for (group, position_controllers) in stopped_controllers.items() - } - - # Check if these controllers are required for the current command - joint_controllers_dict = self._get_joint_controllers() - req_missing_controllers = [] - if not set_joint_positions_req.joint_names: - log_warning = True - req_missing_controllers = flatten_list(stopped_controllers.values()) - else: - for joint in set_joint_positions_req.joint_names: - if joint in joint_controllers_dict.keys(): - req_missing_controllers.append( - [ - stopped_controller - for stopped_controller in flatten_list( - stopped_controllers.values() - ) - if stopped_controller in joint_controllers_dict[joint] - ] - ) - req_missing_controllers = flatten_list(req_missing_controllers) - if req_missing_controllers: - log_warning = True - else: - log_warning = False - - if log_warning: - rospy.logwarn( - "Panda %s joint positions command send but probably not executed " - "as the %s %s not running." - % ( - "arm and hand" - if all(controllers_missing.values()) - else ("arm" if controllers_missing["arm"] else "hand"), - flatten_list(stopped_controllers.values()), - "joint position controller is" - if len(flatten_list(stopped_controllers.values())) == 1 - else "joint position controllers are", - ) - ) - - # Save position control setpoint - self.joint_positions_setpoint = { - group: [command.data for command in control_list] - for (group, control_list) in control_pub_msgs.items() - } - - # Publish request - rospy.logdebug("Publishing Panda arm and hand joint positions control message.") - self._arm_position_pub.publish(control_pub_msgs["arm"]) - self._hand_position_pub.publish(control_pub_msgs["hand"]) - - # Wait till control is finished or timeout has been reached - if set_joint_positions_req.wait and not all(controllers_missing.values()): - wait_control_group = ( - "both" - if all([not bool_val for bool_val in controllers_missing.values()]) - else ("arm" if not controllers_missing["arm"] else "hand") - ) - self._wait_till_done( - control_type="position_control", - control_group=wait_control_group, - controlled_joints=controlled_joints, - ) - - resp.success = True - resp.message = "Everything went OK" - return resp - - def _set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 - """Request arm and hand joint effort control. - - Args: - set_joint_efforts_req (:obj:`panda_gazebo.srv.SetJointEffortsRequest`): - Service request message specifying the efforts for the robot arm and hand - joints. - - Returns: - :obj:`panda_gazebo.srv.SetJointEffortsResponse`: Service response. - """ - # Check if set_joint_efforts_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(set_joint_efforts_req.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "efforts." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/set_joint_efforts" % rospy.get_name(), - ) - ) - - # Create required variables and messages - controllers_missing = {"arm": False, "hand": False} - resp = SetJointEffortsResponse() - - # Check if all controllers are available and running - stopped_controllers = {"arm": [], "hand": []} - missing_controllers = {"arm": [], "hand": []} - for (group, effort_controllers) in { - "arm": self.arm_effort_controllers, - "hand": self.hand_effort_controllers, - }.items(): - for effort_controller in effort_controllers: - try: - if self.controllers[effort_controller].state != "running": - stopped_controllers[group].append(effort_controller) - except KeyError: - missing_controllers[group].append(effort_controller) - - # Return failed result if we miss a controller - if len(flatten_list(missing_controllers.values())) >= 1: - rospy.logwarn( - "Panda arm and hand joint effort command could not be send as the %s " - "%s not initialized. Please make sure you load the controller " - "parameters onto the ROS parameter server." - % ( - flatten_list(missing_controllers.values()), - "joint effort controller is" - if len(flatten_list(missing_controllers.values())) == 1 - else "joint effort controllers are", - ) - ) - missing_controllers_group_string = ( - "Arm and hand" - if all( - [values != [] for (group, values) in missing_controllers.items()] - ) - else ("Arm" if "arm" in dict_clean(missing_controllers) else "Hand") - ) - resp.success = False - resp.message = ( - "%s controllers not initialised." % missing_controllers_group_string - ) - return resp - - # Validate request and create control publisher message - try: - control_pub_msgs, controlled_joints = self._create_control_publisher_msg( - input_msg=set_joint_efforts_req, - control_type="effort_control", - control_group="both", - ) - except InputMessageInvalidError as e: - logwarn_msg = "Panda arm joint efforts not set as " + lower_first_char( - e.log_message - ) - rospy.logwarn(logwarn_msg) - resp.success = False - resp.message = e.message - return resp - - # Check if required joints are running - if len(flatten_list(stopped_controllers.values())) >= 1: - - # Check from which group the controllers are missing - controllers_missing = { - group: (True if (effort_controllers) else False) - for (group, effort_controllers) in stopped_controllers.items() - } - - # Check if these controllers are required for the current command - joint_controllers_dict = self._get_joint_controllers() - req_missing_controllers = [] - if not set_joint_efforts_req.joint_names: - log_warning = True - req_missing_controllers = flatten_list(stopped_controllers.values()) - else: - for joint in set_joint_efforts_req.joint_names: - if joint in joint_controllers_dict.keys(): - req_missing_controllers.append( - [ - stopped_controller - for stopped_controller in flatten_list( - stopped_controllers.values() - ) - if stopped_controller in joint_controllers_dict[joint] - ] - ) - req_missing_controllers = flatten_list(req_missing_controllers) - if req_missing_controllers: - log_warning = True - else: - log_warning = False - - if log_warning: - rospy.logwarn( - "Panda %s joint efforts command send but probably not executed as " - "the %s %s not running." - % ( - "arm and hand" - if all(controllers_missing.values()) - else ("arm" if controllers_missing["arm"] else "hand"), - req_missing_controllers, - "joint effort controller is" - if len(req_missing_controllers) == 1 - else "joint effort controllers are", - ) - ) - - # Save effort control setpoint - self.joint_efforts_setpoint = { - group: [command.data for command in control_list] - for (group, control_list) in control_pub_msgs.items() - } - - # Publish request - rospy.logdebug("Publishing Panda arm and hand joint efforts control message.") - self._arm_effort_pub.publish(control_pub_msgs["arm"]) - self._hand_effort_pub.publish(control_pub_msgs["hand"]) - - # Wait till control is finished or timeout has been reached - if set_joint_efforts_req.wait and not all(controllers_missing.values()): - wait_control_group = ( - "both" - if all([not bool_val for bool_val in controllers_missing.values()]) - else ("arm" if not controllers_missing["arm"] else "hand") - ) - self._wait_till_done( - control_type="effort_control", - control_group=wait_control_group, - controlled_joints=controlled_joints, - ) - - resp.success = True - resp.message = "Everything went OK" - return resp - - def _arm_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 - """Request arm joint position control. - - Args: - set_joint_positions_req (:obj:`panda_gazebo.srv.SetJointPositionsRequest`): - Service request message specifying the positions for the robot arm - joints. - - Returns: - :obj:`panda_gazebo.srv.SetJointPositionsResponse`: Service response. - """ - # Check if set_joint_efforts_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(set_joint_positions_req.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "positions." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/panda_arm/set_joint_positions" % rospy.get_name(), - ) - ) - - # Create required variables and messages - controllers_missing = False - resp = SetJointPositionsResponse() - - # Check if all controllers are available and running - stopped_controllers = [] - missing_controllers = [] - for position_controller in self.arm_position_controllers: - try: - if self.controllers[position_controller].state != "running": - stopped_controllers.append(position_controller) - except KeyError: - missing_controllers.append(position_controller) - - # Return failed result if we miss a controller - if len(missing_controllers) >= 1: - rospy.logwarn( - "Panda arm joint position command could not be send as the %s %s " - "not initialized. Please make sure you load the controller parameters " - "onto the ROS parameter server." - % ( - missing_controllers, - "joint position controller is" - if len(missing_controllers) == 1 - else "joint position controllers are", - ) - ) - resp.success = False - resp.message = "Arm controllers not initialised." - return resp - - # Validate request and create control publisher message - try: - control_pub_msgs, controlled_joints = self._create_control_publisher_msg( - input_msg=set_joint_positions_req, - control_type="position_control", - control_group="arm", - ) - except InputMessageInvalidError as e: - logwarn_msg = "Panda arm joint positions not set as " + lower_first_char( - e.log_message - ) - rospy.logwarn(logwarn_msg) - resp.success = False - resp.message = e.message - return resp - - # Check if required joints are running - if len(stopped_controllers) >= 1: - - # Set controllers missing boolean - controllers_missing = True - - # Check if these controllers are required for the current command - joint_controllers_dict = self._get_joint_controllers() - req_missing_controllers = [] - if not set_joint_positions_req.joint_names: - log_warning = True - req_missing_controllers = flatten_list(stopped_controllers) - else: - for joint in set_joint_positions_req.joint_names: - if joint in joint_controllers_dict.keys(): - req_missing_controllers.append( - [ - stopped_controller - for stopped_controller in flatten_list( - stopped_controllers - ) - if stopped_controller in joint_controllers_dict[joint] - ] - ) - req_missing_controllers = flatten_list(req_missing_controllers) - if req_missing_controllers: - log_warning = True - else: - log_warning = False - - if log_warning: - rospy.logwarn( - "Panda arm joint positions command send but probably not executed " - "as the %s %s not running." - % ( - stopped_controllers, - "joint position controller is" - if len(stopped_controllers) == 1 - else "joint position controllers are", - ) - ) - # Save position control setpoint - self.joint_positions_setpoint = { - group: [command.data for command in control_list] - for (group, control_list) in control_pub_msgs.items() - } - - # Publish request - rospy.logdebug("Publishing Panda arm joint positions control message.") - self._arm_position_pub.publish(control_pub_msgs["arm"]) - - # Wait till control is finished or timeout has been reached - if set_joint_positions_req.wait and not controllers_missing: - self._wait_till_done( - control_type="position_control", - control_group="arm", - controlled_joints=controlled_joints, - ) - - resp.success = True - resp.message = "Everything went OK" - return resp - - def _arm_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 - """Request arm Joint effort control. - - Args: - set_joint_efforts_req (:obj:`panda_gazebo.srv.SetJointEffortsRequest`): - Service request message specifying the efforts for the robot arm joints. - - Returns: - :obj:`panda_gazebo.srv.SetJointEffortsResponse`: Service response. - """ - # Check if set_joint_efforts_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(set_joint_efforts_req.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "efforts." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/panda_arm/set_joint_efforts" % rospy.get_name(), - ) - ) - - # Create required variables and messages - controllers_missing = False - resp = SetJointEffortsResponse() - - # Check if all controllers are available and running - stopped_controllers = [] - missing_controllers = [] - for effort_controller in self.arm_effort_controllers: - try: - if self.controllers[effort_controller].state != "running": - stopped_controllers.append(effort_controller) - except KeyError: - missing_controllers.append(effort_controller) - - # Return failed result if we miss a controller - if len(missing_controllers) >= 1: - rospy.logwarn( - "Panda arm joint effort command could not be send as the %s %s " - "not initialized. Please make sure you load the controller parameters " - "onto the ROS parameter server." - % ( - missing_controllers, - "joint effort controller is" - if len(missing_controllers) == 1 - else "joint effort controllers are", - ) - ) - resp.success = False - resp.message = "Arm controllers not initialised." - return resp - - # Validate request and create control publisher message - try: - control_pub_msgs, controlled_joints = self._create_control_publisher_msg( - input_msg=set_joint_efforts_req, - control_type="effort_control", - control_group="arm", - ) - except InputMessageInvalidError as e: - logwarn_msg = "Panda arm joint efforts not set as " + lower_first_char( - e.log_message - ) - rospy.logwarn(logwarn_msg) - resp.success = False - resp.message = e.message - return resp - - # Check if required controllers are running - if len(stopped_controllers) >= 1: - - # Set controllers missing boolean - controllers_missing = True - - # Check if these controllers are required for the current command - joint_controllers_dict = self._get_joint_controllers() - req_missing_controllers = [] - if not set_joint_efforts_req.joint_names: - log_warning = True - req_missing_controllers = flatten_list(stopped_controllers) - else: - for joint in set_joint_efforts_req.joint_names: - if joint in joint_controllers_dict.keys(): - req_missing_controllers.append( - [ - stopped_controller - for stopped_controller in flatten_list( - stopped_controllers - ) - if stopped_controller in joint_controllers_dict[joint] - ] - ) - req_missing_controllers = flatten_list(req_missing_controllers) - if req_missing_controllers: - log_warning = True - else: - log_warning = False - - if log_warning: - rospy.logwarn( - "Panda arm joint efforts command send but probably not executed as " - "the %s %s not running." - % ( - stopped_controllers, - "joint effort controller is" - if len(stopped_controllers) == 1 - else "joint effort controllers are", - ) - ) - - # Save effort control setpoint - self.joint_efforts_setpoint = { - group: [command.data for command in control_list] - for (group, control_list) in control_pub_msgs.items() - } - - # Publish request - rospy.logdebug("Publishing Panda arm joint efforts control message.") - self._arm_effort_pub.publish(control_pub_msgs["arm"]) - - # Wait till control is finished or timeout has been reached - if set_joint_efforts_req.wait and not controllers_missing: - self._wait_till_done( - control_type="effort_control", - control_group="arm", - controlled_joints=controlled_joints, - ) - - resp.success = True - resp.message = "Everything went OK" - return resp - - def _hand_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 - """Request hand joint position control - - Args: - set_joint_positions_req (:obj:`panda_gazebo.srv.SetJointPositionsRequest`): - Service request message specifying the positions for the robot hand - joints. - - Returns: - :obj:`panda_gazebo.srv.SetJointPositionsResponse`: Service response. - """ - # Check if set_joint_efforts_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(set_joint_positions_req.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "positions." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/panda_hand/set_joint_positions" % rospy.get_name(), - ) - ) - - # Create required variables and messages - controllers_missing = False - resp = SetJointPositionsResponse() - - # Check if all controllers are available and running - stopped_controllers = [] - missing_controllers = [] - for position_controller in self.hand_position_controllers: - try: - if self.controllers[position_controller].state != "running": - stopped_controllers.append(position_controller) - except KeyError: - missing_controllers.append(position_controller) - - # Return failed result if we miss a controller - if len(missing_controllers) >= 1: - rospy.logwarn( - "Panda hand joint position command could not be send as the %s %s " - "not initialized. Please make sure you load the controller parameters " - "onto the ROS parameter server." + "Panda arm joint position command could not be send as the %s %s " + "not initialized. Please make sure you load the controller parameters " + "onto the ROS parameter server." % ( missing_controllers, "joint position controller is" @@ -3329,18 +1637,17 @@ def _hand_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 ) ) resp.success = False - resp.message = "Hand controllers not initialised." + resp.message = "Arm controllers not initialised." return resp # Validate request and create control publisher message try: - control_pub_msgs, controlled_joints = self._create_control_publisher_msg( + control_pub_msgs = self._create_control_publisher_msg( input_msg=set_joint_positions_req, control_type="position_control", - control_group="hand", ) except InputMessageInvalidError as e: - logwarn_msg = "Panda hand joint positions not set as " + lower_first_char( + logwarn_msg = "Panda arm joint positions not set as " + lower_first_char( e.log_message ) rospy.logwarn(logwarn_msg) @@ -3350,10 +1657,6 @@ def _hand_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 # Check if required joints are running if len(stopped_controllers) >= 1: - - # Set controllers missing boolean - controllers_missing = True - # Check if these controllers are required for the current command joint_controllers_dict = self._get_joint_controllers() req_missing_controllers = [] @@ -3380,7 +1683,7 @@ def _hand_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 if log_warning: rospy.logwarn( - "Panda hand joint positions command send but probably not executed " + "Panda arm joint positions command send but probably not executed " "as the %s %s not running." % ( stopped_controllers, @@ -3389,35 +1692,30 @@ def _hand_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 else "joint position controllers are", ) ) - # Save position control setpoint - self.joint_positions_setpoint = { - group: [command.data for command in control_list] - for (group, control_list) in control_pub_msgs.items() - } - # Publish request - rospy.logdebug("Publishing Panda hand joint positions control message.") - self._hand_position_pub.publish(control_pub_msgs["hand"]) + # Save setpoint and publish request + self.arm_joint_positions_setpoint = [ + command.data for command in control_pub_msgs + ] + rospy.logdebug("Publishing Panda arm joint positions control message.") + self._arm_joint_position_pub.publish(control_pub_msgs) # Wait till control is finished or timeout has been reached - if set_joint_positions_req.wait and not controllers_missing: + if set_joint_positions_req.wait and not len(stopped_controllers) >= 1: self._wait_till_done( control_type="position_control", - control_group="hand", - controlled_joints=controlled_joints, ) resp.success = True resp.message = "Everything went OK" return resp - def _hand_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 - """Request hand joint effort control. + def _arm_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 + """Request arm Joint effort control. Args: set_joint_efforts_req (:obj:`panda_gazebo.srv.SetJointEffortsRequest`): - Service request message specifying the efforts for the robot hand - joints. + Service request message specifying the efforts for the robot arm joints. Returns: :obj:`panda_gazebo.srv.SetJointEffortsResponse`: Service response. @@ -3432,28 +1730,20 @@ def _hand_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 % ( "joints" if len(duplicate_list) > 1 else "joint", duplicate_list, - "%s/panda_hand/set_joint_efforts" % rospy.get_name(), + "%s/panda_arm/set_joint_efforts" % rospy.get_name(), ) ) - # Create required variables and messages - controllers_missing = False - resp = SetJointEffortsResponse() - # Check if all controllers are available and running - stopped_controllers = [] - missing_controllers = [] - for effort_controller in self.hand_effort_controllers: - try: - if self.controllers[effort_controller].state != "running": - stopped_controllers.append(effort_controller) - except KeyError: - missing_controllers.append(effort_controller) + resp = SetJointEffortsResponse() + missing_controllers, stopped_controllers = self._controllers_running( + ARM_EFFORT_CONTROLLERS + ) # Return failed result if we miss a controller if len(missing_controllers) >= 1: rospy.logwarn( - "Panda hand joint efforts command could not be send as the %s %s " + "Panda arm joint effort command could not be send as the %s %s " "not initialized. Please make sure you load the controller parameters " "onto the ROS parameter server." % ( @@ -3464,18 +1754,17 @@ def _hand_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 ) ) resp.success = False - resp.message = "Hand controllers not initialised." + resp.message = "Arm controllers not initialised." return resp # Validate request and create control publisher message try: - control_pub_msgs, controlled_joints = self._create_control_publisher_msg( + control_pub_msgs = self._create_control_publisher_msg( input_msg=set_joint_efforts_req, control_type="effort_control", - control_group="hand", ) except InputMessageInvalidError as e: - logwarn_msg = "Panda hand joint efforts not set as " + lower_first_char( + logwarn_msg = "Panda arm joint efforts not set as " + lower_first_char( e.log_message ) rospy.logwarn(logwarn_msg) @@ -3485,10 +1774,6 @@ def _hand_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 # Check if required controllers are running if len(stopped_controllers) >= 1: - - # Set controllers missing boolean - controllers_missing = True - # Check if these controllers are required for the current command joint_controllers_dict = self._get_joint_controllers() req_missing_controllers = [] @@ -3515,8 +1800,8 @@ def _hand_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 if log_warning: rospy.logwarn( - "Panda hand joint efforts command send but probably not executed " - "as the %s %s not running." + "Panda arm joint efforts command send but probably not executed as " + "the %s %s not running." % ( stopped_controllers, "joint effort controller is" @@ -3525,28 +1810,62 @@ def _hand_set_joint_efforts_cb(self, set_joint_efforts_req): # noqa: C901 ) ) - # Save effort control setpoint - self.joint_efforts_setpoint = { - group: [command.data for command in control_list] - for (group, control_list) in control_pub_msgs.items() - } - - # Publish request - rospy.logdebug("Publishing Panda hand joint efforts control message.") - self._hand_effort_pub.publish(control_pub_msgs["hand"]) + # Save setpoint and publish request + self.arm_joint_efforts_setpoint = [command.data for command in control_pub_msgs] + rospy.logdebug("Publishing Panda arm joint efforts control message.") + self._arm_joint_effort_pub.publish(control_pub_msgs) # Wait till control is finished or timeout has been reached - if set_joint_efforts_req.wait and not controllers_missing: + if set_joint_efforts_req.wait and not len(stopped_controllers) >= 1: self._wait_till_done( control_type="effort_control", - control_group="hand", - controlled_joints=controlled_joints, ) resp.success = True resp.message = "Everything went OK" return resp + def _set_gripper_width_cb(self, set_gripper_width_req): + """Request gripper width. + + Args: + set_gripper_width_req (:obj:`panda_gazebo.srv.SetGripperWidth`): + Service request message specifying the gripper width for the robot hand. + + Returns: + :obj:`panda_gazebo.srv.SetGripperWidthReponse`: Service response. + """ + # Check if gripper width is within boundaries + if set_gripper_width_req.width < 0.0 or set_gripper_width_req.width > 0.08: + rospy.logwarn( + "Gripper width was clipped as it was not within bounds [0, 0.8]." + ) + gripper_width = np.clip(set_gripper_width_req.width, 0, 0.08) + else: + gripper_width = set_gripper_width_req.width + + # Create 'franka_gripper/move' action request message + # NOTE: Use 'kDefaultGripperActionSpeed' see 'franka_gripper.sim.h' + req = MoveGoal() + req.width = gripper_width + req.speed = 0.1 + + # Invoke 'franka_gripper/move' aciton server + self._gripper_move_client.send_goal(req) + + # Return result + resp = SetGripperWidthResponse() + if set_gripper_width_req.wait: + gripper_result = self._gripper_move_client.wait_for_result() + resp.success = gripper_result + else: + resp.success = True + if resp.success: + resp.message = "Everything went OK" + else: + resp.message = "Gripper width could not be set" + return resp + def _get_controlled_joints_cb(self, get_controlled_joints_req): """Returns the joints that are controlled when using a given control type. @@ -3583,7 +1902,7 @@ def _arm_joint_traj_execute_cb(self, goal): Goal (:obj:`control_msgs.msg.FollowJointTrajectoryGoal`): Goal execution action server goal message. """ - # Check if set_joint_efforts_req.joint_names contains duplicates + # Check if joint goal.joint_names contains duplicates duplicate_list = get_duplicate_list(goal.trajectory.joint_names) if duplicate_list: rospy.logwarn( @@ -3602,9 +1921,7 @@ def _arm_joint_traj_execute_cb(self, goal): # the right format for the original panda joint trajectory action servers. try: # Convert input goal message to the right format - goal_msg_dict = self._create_traj_action_server_msg( - goal, control_group="arm" - ) + goal_msg_dict = self._create_arm_traj_action_server_msg(goal) except InputMessageInvalidError as e: self._result = FollowJointTrajectoryResult() self._result.error_code = ( @@ -3616,7 +1933,7 @@ def _arm_joint_traj_execute_cb(self, goal): # Abort joint trajectory control if original panda arm control server is not # available - if not self._arm_joint_traj_connected: + if not self._arm_joint_traj_client_connected: self._result = FollowJointTrajectoryResult() self._result.error_code = -7 self._result.error_string = ( @@ -3626,10 +1943,9 @@ def _arm_joint_traj_execute_cb(self, goal): # Execute joint trajectory goal if not error_occurred: - # Send goal to the original Panda arm action servers self._arm_joint_traj_client.send_goal( - goal_msg_dict["arm"], feedback_cb=self._arm_joint_traj_feedback_cb + goal_msg_dict, feedback_cb=self._arm_joint_traj_feedback_cb ) # Wait for the server to finish performing the action @@ -3652,212 +1968,6 @@ def _arm_joint_traj_execute_cb(self, goal): else: # If not successful self._arm_joint_traj_as.set_aborted(self._result) - def _hand_joint_traj_execute_cb(self, goal): - """Goal execution callback function for the Panda hand joint trajectory action - server. - - Args: - Goal (:obj:`control_msgs.msg.FollowJointTrajectoryGoal`): Goal execution - action server goal message. - """ - # Check if set_joint_efforts_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(goal.trajectory.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "trajectory." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/panda_hand/follow_joint_trajectory" % rospy.get_name(), - ) - ) - error_occurred = False - - # Check the input goal message for errors and if valid convert it to - # the right format for the original panda joint trajectory action servers. - try: - # Convert input goal message to the right format - goal_msg_dict = self._create_traj_action_server_msg( - goal, control_group="hand" - ) - except InputMessageInvalidError as e: - self._result = FollowJointTrajectoryResult() - self._result.error_code = ( - -2 if e.args[0] == "Invalid joint_names were given." else -6 - ) - self._result.error_string = e.log_message - self._hand_joint_traj_as.set_aborted(result=self._result) - error_occurred = True - - # Abort joint trajectory control if original panda hand control server is not - # available - if not self._hand_joint_traj_connected: - self._result = FollowJointTrajectoryResult() - self._result.error_code = -7 - self._result.error_string = ( - "Could not connect to original Panda hand action server." - ) - self._hand_joint_traj_as.set_aborted(result=self._result) - - # Execute joint trajectory goal - if not error_occurred: - - # Send goal to the original Panda hand action servers - self._hand_joint_traj_client.send_goal( - goal_msg_dict["hand"], feedback_cb=self._hand_joint_traj_feedback_cb - ) - - # Wait for the server to finish performing the action - self._hand_joint_traj_client.wait_for_result() - - # Get result from action server - self._result = self._hand_joint_traj_client.get_result() - self._state = ActionClientState(self._hand_joint_traj_client) - self._result.error_string = translate_actionclient_result_error_code( - self._result - ) - - # Set result - if not ( - self._hand_joint_traj_as.is_preempt_requested() - or self._state.state == self._state.state_dict["PREEMPTED"] - ): - if self._result.error_code == self._result.SUCCESSFUL: # If successful - self._hand_joint_traj_as.set_succeeded(self._result) - else: # If not successful - self._hand_joint_traj_as.set_aborted(self._result) - - def _joint_traj_execute_cb(self, goal): - """Goal execution callback function for the combined Panda arm and hand joint - trajectory action server. - - Args: - Goal (:obj:`control_msgs.msg.FollowJointTrajectoryGoal`): Goal execution - action server goal message. - """ - # Check if set_joint_efforts_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(goal.trajectory.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "trajectory." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/panda_arm/follow_joint_trajectory" % rospy.get_name(), - ) - ) - error_occurred = False - - # Check the input goal message for errors and if valid convert it to - # the right format for the original panda joint trajectory action servers. - try: - # Convert input goal message to the right format - goal_msg_dict = self._create_traj_action_server_msg( - goal, control_group="both" - ) - except InputMessageInvalidError as e: - self._result = FollowJointTrajectoryResult() - self._result.error_code = ( - -2 if e.args[0] == "Invalid joint_names were given." else -6 - ) - self._result.error_string = e.log_message - self._joint_traj_as.set_aborted(result=self._result) - error_occurred = True - - # Abort joint trajectory control if original panda control servers are not - # available - if any( - [not self._arm_joint_traj_connected, not self._hand_joint_traj_connected] - ): - self._result = FollowJointTrajectoryResult() - self._result.error_code = -7 - self._result.error_string = ( - "Could not connect to original Panda arm and hand action servers." - ) - self._joint_traj_as.set_aborted(result=self._result) - - # Execute joint trajectory goal - if not error_occurred: - - # Send goal to the original Panda arm and hand action servers - self._full_joint_traj_control = True - self._arm_joint_traj_client.send_goal( - goal_msg_dict["arm"], feedback_cb=self._arm_joint_traj_feedback_cb - ) - self._hand_joint_traj_client.send_goal( - goal_msg_dict["hand"], feedback_cb=self._hand_joint_traj_feedback_cb - ) - - # Wait for the server to finish performing the action - self._arm_joint_traj_client.wait_for_result() - self._hand_joint_traj_client.wait_for_result() - self._full_joint_traj_control = False - - # Get result and state from action server - self._arm_result = self._arm_joint_traj_client.get_result() - self._hand_result = self._hand_joint_traj_client.get_result() - self._arm_result.error_string = translate_actionclient_result_error_code( - self._arm_result - ) - self._hand_result.error_string = translate_actionclient_result_error_code( - self._hand_result - ) - self._arm_state = ActionClientState(self._arm_joint_traj_client) - self._hand_state = ActionClientState(self._hand_joint_traj_client) - - # Create combined result message - self._result = FollowJointTrajectoryResult() - self._result.error_code = int( - "-" - + "".join( - [ - str(-1 * self._arm_result.error_code), - str(-1 * self._hand_result.error_code), - ] - ) - ) - self._result.error_string = ( - "Arm: " - + "status=" - + self._arm_state.state_value_dict[self._arm_state.state] - + ", result=" - + self._arm_result.error_string - + " Hand: " - + "status=" - + self._hand_state.state_value_dict[self._hand_state.state] - + ", result=" - + self._hand_result.error_string - ) - - # Set result - if not ( - any( - [ - self._arm_joint_traj_as.is_preempt_requested(), - self._hand_joint_traj_as.is_preempt_requested(), - ] - ) - or any( - [ - self._arm_state.state - == self._arm_state.state_dict["PREEMPTED"], - self._hand_state.state - == self._hand_state.state_dict["PREEMPTED"], - ] - ) - ): - if ( - self._arm_result == self._arm_result.SUCCESSFUL - and self._hand_result == self._hand_result.SUCCESSFUL - ): # If successful - self._joint_traj_as.set_succeeded(self._result) - else: # If not successful - self._joint_traj_as.set_aborted(self._result) - def _arm_joint_traj_feedback_cb(self, feedback): """Relays the feedback messages from the original ``panda_arm_controller/follow_joint_trajectory`` server to our to our @@ -3868,41 +1978,7 @@ def _arm_joint_traj_feedback_cb(self, feedback): feedback (:obj:`control_msgs.msg.FollowJointTrajectoryFeedback`): Goal execution action server feedback message. """ - self._as_arm_feedback = feedback - - # Check if callback was called from the combined action server - if self._full_joint_traj_control: - # Combine feedback with hand feedback - combined_feedback = self._get_combined_action_server_fb_msg() - - # Publish feedback to the combined action server - self._joint_traj_as.publish_feedback(combined_feedback) - else: - # Publish feedback to hand action server - self._arm_joint_traj_as.publish_feedback(feedback) - - def _hand_joint_traj_feedback_cb(self, feedback): - """Relays the feedback messages from the original - ``panda_hand_controller/follow_joint_trajectory`` server to our to our - ``panda_control_server/panda_hand/follow_joint_trajectory`` wrapper action - server. - - Args: - feedback :obj:`control_msgs.msg.FollowJointTrajectoryFeedback`): Goal - execution action server feedback message. - """ - self._as_hand_feedback = feedback - - # Check if callback was called from the combined action server - if self._full_joint_traj_control: - # Combine feedback with arm feedback - combined_feedback = self._get_combined_action_server_fb_msg() - - # Publish feedback to the combined action server - self._joint_traj_as.publish_feedback(combined_feedback) - else: - # Publish feedback to hand action server - self._hand_joint_traj_as.publish_feedback(feedback) + self._arm_joint_traj_as.publish_feedback(feedback) def _arm_joint_traj_preempt_cb(self): """Relays the preempt request made to the @@ -3912,27 +1988,3 @@ def _arm_joint_traj_preempt_cb(self): # Stop panda_arm_controller action server self._arm_joint_traj_client.cancel_goal() self._arm_joint_traj_as.set_preempted() - self._full_joint_traj_control = False - - def _hand_joint_traj_preempt_cb(self): - """Relays the preempt request made to the - ``panda_control_server/panda_hand/follow_joint_trajectory`` action server - wrapper to the original ``panda_hand_controller/follow_joint_trajectory`` - action server. - """ - # Stop panda_hand_controller action server - self._hand_joint_traj_client.cancel_goal() - self._hand_joint_traj_as.set_preempted() - self._full_joint_traj_control = False - - def _joint_traj_preempt_cb(self): - """Relays the preempt request made to the - ``panda_control_server/follow_joint_trajectory`` action server wrapper - to the original ``panda_arm_controller/follow_joint_trajectory`` and - ``panda_hand_controller/follow_joint_trajectory`` action servers. - """ - # Stop panda_arm_controller and panda_hand_controller action servers - self._arm_joint_traj_client.cancel_goal() - self._hand_joint_traj_client.cancel_goal() - self._joint_traj_as.set_preempted() - self._full_joint_traj_control = False diff --git a/panda_gazebo/src/panda_gazebo/core/control_switcher.py b/panda_gazebo/src/panda_gazebo/core/control_switcher.py index edc080a8..7f3f3ac0 100644 --- a/panda_gazebo/src/panda_gazebo/core/control_switcher.py +++ b/panda_gazebo/src/panda_gazebo/core/control_switcher.py @@ -1,7 +1,6 @@ """This class is responsible for switching the control type that is used for -controlling the Panda Robot robot ``arm`` and ``hand``. It serves as a wrapper around -the services created by the ROS -`controller_manager `_ class. +controlling the Panda Robot robot ``arm``. It serves as a wrapper aroundthe services +created by the ROS `controller_manager `_ class. Control types: * `trajectory_control `_ diff --git a/panda_gazebo/src/panda_gazebo/core/moveit_server.py b/panda_gazebo/src/panda_gazebo/core/moveit_server.py index 1b8a5b84..e526306e 100644 --- a/panda_gazebo/src/panda_gazebo/core/moveit_server.py +++ b/panda_gazebo/src/panda_gazebo/core/moveit_server.py @@ -12,8 +12,6 @@ * get_random_joint_positions * get_random_ee_pose * get_controlled_joints - -Extra services: * panda_arm/set_joint_positions * panda_hand/set_joint_positions @@ -45,6 +43,7 @@ get_unique_list, joint_state_dict_2_joint_state_msg, lower_first_char, + translate_moveit_error_code, ) from panda_gazebo.common.quaternion import Quaternion from panda_gazebo.exceptions import InputMessageInvalidError @@ -78,28 +77,28 @@ class PandaMoveitPlannerServer(object): """Used to control or request information from the Panda Robot. This is done using - the Moveit `moveit_commander` module. + the MoveIt `moveit_commander` module. Attributes: - robot (:obj:`moveit_commander.robot.RobotCommander`): The Moveit robot + robot (:obj:`moveit_commander.robot.RobotCommander`): The MoveIt robot commander object. scene (:obj:`moveit_commander.planning_scene_interface.PlanningSceneInterface`): - The Moveit robot scene commander object. + The MoveIt robot scene commander object. move_group_arm (:obj:`moveit_commander.move_group.MoveGroupCommander`): - The Moveit arm move group object. + The MoveIt arm move group object. move_group_hand (:obj:`moveit_commander.move_group.MoveGroupCommander`): - The Moveit hand move group object. + The MoveIt hand move group object. ee_pose_target (:obj:`geometry_msgs.msg.Pose`): The last set ee pose. joint_positions_target (:obj:`dict`): Dictionary containing the last Panda arm and hand joint positions setpoint. """ - def __init__( + def __init__( # noqa: C901 self, arm_move_group="panda_arm", arm_ee_link="panda_link8", hand_move_group="hand", - create_extra_services=False, + load_gripper=True, ): """Initializes the PandaMoveitPlannerServer object. @@ -110,19 +109,22 @@ def __init__( controlling the Panda arm. Defaults to ``panda_link8``. hand_move_group (str, optional): The name of the move group you want to use for controlling the Panda hand. Defaults to ``hand``. - create_extra_services (bool, optional): Specifies whether the extra services - should also be created. + load_gripper (boolean, optional): Whether we also want to load the gripper + control services. """ - self._joint_state_topic = "/joint_states" + self._load_gripper = load_gripper - # Initialize Moveit/Robot/Scene and group commanders - rospy.logdebug("Initialize Moveit Robot/Scene and group commanders.") + # Initialize MoveIt/Robot/Scene and group commanders + rospy.logdebug("Initialize MoveIt Robot/Scene and group commanders.") try: moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.move_group_arm = moveit_commander.MoveGroupCommander(arm_move_group) - self.move_group_hand = moveit_commander.MoveGroupCommander(hand_move_group) + if self._load_gripper: + self.move_group_hand = moveit_commander.MoveGroupCommander( + hand_move_group + ) except Exception as e: if "invalid robot mode" in e.args[0]: rospy.logerr( @@ -152,12 +154,14 @@ def __init__( self.move_group_arm.set_end_effector_link(arm_ee_link) self._display_trajectory_publisher = rospy.Publisher( - "/move_group/display_planned_path", + "move_group/display_planned_path", DisplayTrajectory, queue_size=10, ) # Retrieve parameters and create dynamic reconfigure server + # NOTE: Used to change the max velocity and acceleration scaling that is used + # by the MoveIt control server. self._get_params() self._dyn_reconfigure_srv = Server(MoveitServerConfig, self._dyn_reconfigure_cb) @@ -171,19 +175,19 @@ def __init__( "Creating '%s/panda_arm/set_ee_pose' service." % rospy.get_name() ) self._arm_set_ee_pose_srv = rospy.Service( - "%s/panda_arm/set_ee_pose" % rospy.get_name()[1:], + "%s/panda_arm/set_ee_pose" % rospy.get_name().split("/")[-1], SetEePose, self._arm_set_ee_pose_callback, ) rospy.logdebug("Creating '%s/panda_arm/get_ee' service." % rospy.get_name()) self._arm_get_ee = rospy.Service( - "%s/panda_arm/get_ee" % rospy.get_name()[1:], + "%s/panda_arm/get_ee" % rospy.get_name().split("/")[-1], GetEe, self._arm_get_ee_callback, ) rospy.logdebug("Creating '%s/panda_arm/set_ee' service." % rospy.get_name()) self._arm_set_ee = rospy.Service( - "%s/panda_arm/set_ee" % rospy.get_name()[1:], + "%s/panda_arm/set_ee" % rospy.get_name().split("/")[-1], SetEe, self._arm_set_ee_callback, ) @@ -191,19 +195,19 @@ def __init__( "Creating '%s/panda_arm/get_ee_pose' service." % rospy.get_name() ) self._arm_get_ee_pose_srv = rospy.Service( - "%s/panda_arm/get_ee_pose" % rospy.get_name()[1:], + "%s/panda_arm/get_ee_pose" % rospy.get_name().split("/")[-1], GetEePose, self._arm_get_ee_pose_callback, ) rospy.logdebug("Creating '%s/panda_arm/get_ee_rpy' service." % rospy.get_name()) self._arm_get_ee_rpy_srv = rospy.Service( - "%s/panda_arm/get_ee_rpy" % rospy.get_name()[1:], + "%s/panda_arm/get_ee_rpy" % rospy.get_name().split("/")[-1], GetEeRpy, self._arm_get_ee_rpy_callback, ) rospy.logdebug("Creating '%s/set_joint_positions' service." % rospy.get_name()) self._set_joint_positions_srv = rospy.Service( - "%s/set_joint_positions" % rospy.get_name()[1:], + "%s/set_joint_positions" % rospy.get_name().split("/")[-1], SetJointPositions, self._set_joint_positions_callback, ) @@ -211,13 +215,13 @@ def __init__( "Creating '%s/get_random_joint_positions' service." % rospy.get_name() ) self._get_random_joints_positions_srv = rospy.Service( - "%s/get_random_joint_positions" % rospy.get_name()[1:], + "%s/get_random_joint_positions" % rospy.get_name().split("/")[-1], GetRandomJointPositions, self._get_random_joint_positions_callback, ) rospy.logdebug("Creating '%s/get_random_ee_pose' service." % rospy.get_name()) self._get_random_ee_pose_srv = rospy.Service( - "%s/get_random_ee_pose" % rospy.get_name()[1:], + "%s/get_random_ee_pose" % rospy.get_name().split("/")[-1], GetRandomEePose, self._get_random_ee_pose_callback, ) @@ -225,28 +229,25 @@ def __init__( "Creating '%s/get_controlled_joints' service." % rospy.get_name() ) self._get_controlled_joints_srv = rospy.Service( - "%s/get_controlled_joints" % rospy.get_name()[1:], + "%s/get_controlled_joints" % rospy.get_name().split("/")[-1], GetMoveItControlledJoints, self._get_controlled_joints_cb, ) - - # Create extra services - if create_extra_services: - rospy.logdebug( - "Creating '%s/panda_arm/set_joint_positions' service." - % rospy.get_name() - ) - self._arm_set_joint_positions_srv = rospy.Service( - "%s/panda_arm/set_joint_positions" % rospy.get_name()[1:], - SetJointPositions, - self._arm_set_joint_positions_callback, - ) + rospy.logdebug( + "Creating '%s/panda_arm/set_joint_positions' service." % rospy.get_name() + ) + self._arm_set_joint_positions_srv = rospy.Service( + "%s/panda_arm/set_joint_positions" % rospy.get_name().split("/")[-1], + SetJointPositions, + self._arm_set_joint_positions_callback, + ) + if self._load_gripper: rospy.logdebug( "Creating '%s/panda_hand/set_joint_positions' service." % rospy.get_name() ) self._hand_set_joint_positions_srv = rospy.Service( - "%s/panda_hand/set_joint_positions" % rospy.get_name()[1:], + "%s/panda_hand/set_joint_positions" % rospy.get_name().split("/")[-1], SetJointPositions, self._hand_set_joint_positions_callback, ) @@ -264,16 +265,18 @@ def __init__( while self._joint_states is None and not rospy.is_shutdown(): try: self._joint_states = rospy.wait_for_message( - self._joint_state_topic, JointState, timeout=1.0 + "joint_states", JointState, timeout=1.0 ) except ROSException: rospy.logwarn( - "Current /joint_states not ready yet, retrying for getting %s" - % self._joint_state_topic + "Current joint_states not ready yet, retrying for getting %s" + % "joint_states" ) self._controlled_joints_dict = { "arm": flatten_list(self.move_group_arm.get_active_joints()), - "hand": flatten_list(self.move_group_hand.get_active_joints()), + "hand": flatten_list(self.move_group_hand.get_active_joints()) + if self._load_gripper + else [], } # Retrieve joint state mask @@ -322,44 +325,100 @@ def _link_exists(self, link_name): """ return link_name in self.robot.get_link_names() - def _execute(self, control_group="both"): + def _execute(self, control_group="both", wait=True): # noqa: C901 """Plan and execute a trajectory/pose or orientation setpoints Args: control_group (str, optional): The robot control group for which you want to execute the control. Options are ``arm`` or ``hand`` or ``both``. Defaults to ``both``. + wait (boolean, optional): Whether to wait on the control to be executed. Returns: list: List specifying whether the arm and/or hand execution was successful. If ``control_group == "both"`` then ``["arm_success", "hand_success"]``. """ - if control_group.lower() == "arm": - self.arm_plan = self.move_group_arm.plan() - arm_retval = self.move_group_arm.go(wait=True) - retval = [arm_retval] - elif control_group.lower() == "hand": - self.hand_plan = self.move_group_hand.plan() - hand_retval = self.move_group_hand.go(wait=True) - retval = [hand_retval] - elif control_group.lower() == "both": - self.arm_plan = self.move_group_arm.plan() - arm_retval = self.move_group_arm.go(wait=True) - self.hand_plan = self.move_group_hand.plan() - hand_retval = self.move_group_hand.go(wait=True) - retval = [arm_retval, hand_retval] + control_group = control_group.lower() + + # Execute + retval = [] + if control_group == "arm": + (arm_plan_retval, _, _, error_code) = self.move_group_arm.plan() + if arm_plan_retval: + arm_retval = self.move_group_arm.go(wait=wait) + if wait: + self.move_group_arm.stop() + else: + rospy.logwarn( + "No plan found for the current hand setpoints since '%s'" + % translate_moveit_error_code(error_code) + ) + arm_retval = False + retval.append(arm_retval) + elif control_group == "hand": + if self._load_gripper: + (hand_plan_retval, _, _, error_code) = self.move_group_hand.plan() + if hand_plan_retval: + hand_retval = self.move_group_hand.go(wait=wait) + if wait: + self.move_group_hand.stop() + else: + rospy.logwarn( + "No plan found for the current hand setpoints since '%s'" + % translate_moveit_error_code(error_code) + ) + hand_retval = False + retval.append(hand_retval) + else: + rospy.logwarn("Hand commands not executed since gripper is not loaded.") + retval.append(False) + elif control_group == "both": + # Get hand plan + if not self._load_gripper: + rospy.logwarn_once( + "Hand commands not executed since gripper is not loaded." + ) + else: + (hand_plan_retval, _, _, error_code) = self.move_group_hand.plan() + + # Get arm plan + (arm_plan_retval, _, _, error_code) = self.move_group_arm.plan() + + # Execute arm/hand plans + if self._load_gripper: + if hand_plan_retval: + hand_retval = self.move_group_hand.go(wait=False) + else: + rospy.logwarn( + "No plan found for the current hand setpoints since '%s'" + % translate_moveit_error_code(error_code) + ) + hand_retval = False + retval.append(hand_retval) + if arm_plan_retval: + arm_retval = self.move_group_arm.go(wait=wait) + if wait: + self.move_group_hand.stop() + self.move_group_arm.stop() + else: + rospy.logwarn( + "No plan found for the current hand setpoints since '%s'" + % translate_moveit_error_code(error_code) + ) + arm_retval = False + retval.append(arm_retval) else: logwarn_msg = ( "Control group '%s' does not exist. Please specify a valid control " "group. Valid values are %s." - % (control_group.lower(), "['arm', 'hand', 'both']") + % (control_group, "['arm', 'hand', 'both']") ) rospy.logwarn(logwarn_msg) retval = [False] return retval def _create_joint_positions_commands( # noqa: C901 - self, input_msg, control_group="both", verbose=False + self, input_msg, control_group="both" ): """Converts the service input message in `moveit_commander` compatible joint position setpoint commands. While doing this it also verifies whether the given @@ -370,8 +429,6 @@ def _create_joint_positions_commands( # noqa: C901 control_group (str, optional): The robot control group for which you want to execute the control. Options are ``arm`` or ``hand`` or ``both``. Defaults to ``both``. - verbose (bool): Boolean specifying whether you want to send a warning - message to the ROS logger. Returns: dict: Dictionary that contains the 'moveit_commander' arm and hand joint @@ -384,72 +441,147 @@ def _create_joint_positions_commands( # noqa: C901 """ joint_names = input_msg.joint_names joint_positions = list(input_msg.joint_positions) + control_group = control_group.lower() - # Get controlled joints - if control_group.lower() == "arm": + # Validate control_group + if control_group not in ["arm", "hand", "both"]: + logwarn_msg = ( + "The '%s' control group does not exist. Please specify a valid control " + "group (options: %s)." % (control_group, "['arm', 'hand', 'both']") + ) + rospy.logerr(logwarn_msg) + raise InputMessageInvalidError( + message="Control group '%s' does not exist." % control_group, + log_message=logwarn_msg, + ) + elif control_group == "hand" and not self._load_gripper: + logwarn_msg = ( + "The 'hand' control group can not be used when the 'load_gripper' " + "variable is set to False. Please change 'load_gripper' to True or " + "specify a valid control group (options: ['arm'])." + ) + rospy.logerr(logwarn_msg) + raise InputMessageInvalidError( + message="Control group '%s' does not exist." % control_group, + log_message=logwarn_msg, + ) + elif control_group == "both" and not self._load_gripper: + # Change to arm if the hand is not loaded + control_group = "arm" + + # Get controlled joints, throw warning if control group is incorrect. + if control_group == "arm": controlled_joints = self._controlled_joints_dict["arm"] - elif control_group.lower() == "hand": + elif control_group == "hand": controlled_joints = self._controlled_joints_dict["hand"] - elif control_group.lower() == "both": + else: controlled_joints = flatten_list( [ self._controlled_joints_dict["arm"], self._controlled_joints_dict["hand"], ] ) - else: - logwarn_msg = ( - "The '%s' control group does not exist. Please specify a valid control " - "group (options: %s)." - % (control_group.lower(), "['arm', 'hand', 'both']") + + # Get the current state of the arm and hand + arm_state_dict = OrderedDict( + zip( + self.move_group_arm.get_active_joints(), + self.move_group_arm.get_current_joint_values(), ) - if verbose: - rospy.logwarn(logwarn_msg) - raise InputMessageInvalidError( - message="Control group '%s' does not exist." % control_group.lower(), - log_message=logwarn_msg, + ) + if control_group in ["hand", "both"]: + hand_state_dict = OrderedDict( + zip( + self.move_group_hand.get_active_joints(), + self.move_group_hand.get_current_joint_values(), + ) ) # Generate 'moveit_commander' control command controlled_joints_size = len(controlled_joints) if len(joint_names) == 0: # Check if enough joint position commands were given - if len(joint_positions) != controlled_joints_size: - logwarn_msg = "You specified %s while the Panda Robot %s %s %s." % ( - "%s %s" - % ( - len(joint_positions), - "joint position" - if len(joint_positions) == 1 - else "joint positions", - ), - "arm and hand" - if control_group.lower() == "both" - else control_group.lower(), - "contain" if control_group.lower() == "both" else "contains", - "%s %s" + if len(joint_positions) > controlled_joints_size: + logwarn_msg = ( + "You specified %s while the Panda Robot %s only %s %s. Only the " + "first %s are used in the control." % ( - controlled_joints_size, - "active joint" - if controlled_joints_size == 1 - else "active joints", - ), + "%s %s" + % ( + len(joint_positions), + "joint position" + if len(joint_positions) == 1 + else "joint positions", + ), + "arm and hand" if control_group == "both" else control_group, + "contain" if control_group == "both" else "contains", + "%s %s" + % ( + controlled_joints_size, + "active joint" + if controlled_joints_size == 1 + else "active joints", + ), + "%s %s" + % ( + controlled_joints_size, + "joint position" + if controlled_joints_size == 1 + else "joint positions", + ), + ) ) - if verbose: - rospy.logwarn(logwarn_msg) - raise InputMessageInvalidError( - message="Invalid number of joint position commands.", - log_message=logwarn_msg, - details={ - "joint_positions_command_length": len(joint_positions), - "controlled_joints": controlled_joints_size, - }, + rospy.logwarn(logwarn_msg) + + # Remove the extra joint commands + joint_positions = joint_positions[:controlled_joints_size] + elif len(joint_positions) < controlled_joints_size: + logwarn_msg = ( + "You specified %s while the Panda Robot %s %s %s. As a result " + "only the first %s joints are controlled while for the other " + "joints the current robot state is used." + % ( + "%s %s" + % ( + len(joint_positions), + "joint position" + if len(joint_positions) == 1 + else "joint positions", + ), + "arm and hand" if control_group == "both" else control_group, + "contain" if control_group == "both" else "contains", + "%s %s" + % ( + controlled_joints_size, + "active joint" + if controlled_joints_size == 1 + else "active joints", + ), + len(joint_positions), + ) ) + rospy.logwarn(logwarn_msg) + + # Fill the missing joint commands + if control_group == "both": + joint_states = ( + list(arm_state_dict.values()) + list(hand_state_dict.values()) + if self._arm_states_mask[0] + else list(hand_state_dict.values()) + + list(arm_state_dict.values()) + ) + else: + joint_states = ( + list(arm_state_dict.values()) + if control_group == "arm" + else list(hand_state_dict.values()) + ) + joint_positions = joint_positions + joint_states[len(joint_positions) :] # Return moveit_commander_control command dictionary - if control_group.lower() == "arm": + if control_group == "arm": control_commands = {"arm": joint_positions} - elif control_group.lower() == "hand": + elif control_group == "hand": control_commands = {"hand": joint_positions} else: control_commands = { @@ -481,8 +613,7 @@ def _create_joint_positions_commands( # noqa: C901 ), ) ) - if verbose: - rospy.logwarn(logwarn_msg) + rospy.logerr(logwarn_msg) raise InputMessageInvalidError( message=( "Joint_names and joint_positions fields of the input " @@ -501,59 +632,38 @@ def _create_joint_positions_commands( # noqa: C901 for joint_name in joint_names if joint_name not in controlled_joints ] + input_command_dict = OrderedDict(zip(joint_names, joint_positions)) if len(invalid_joint_names) != 0: + # Remove invalid keys and throw warning + [input_command_dict.pop(joint, None) for joint in invalid_joint_names] logwarn_msg = ( - "%s %s that %s specified in the 'joint_names' field of the " - "'panda_gazebo/SetJointPositions' message %s invalid. Valid " - "joint names for controlling the panda %s are %s." + "Ignoring %s %s since %s invalid. Valid joint names for " + "controlling the panda %s are %s." % ( - "Joint" if len(invalid_joint_names) == 1 else "Joints", invalid_joint_names, - "was" if len(invalid_joint_names) == 1 else "were", - "was" if len(invalid_joint_names) == 1 else "were", - "arm and hand" - if control_group.lower() == "both" - else control_group.lower(), + "joint" if len(invalid_joint_names) == 1 else "joints", + "it is" if len(invalid_joint_names) == 1 else "they are", + "arm and hand" if control_group == "both" else control_group, controlled_joints, ) ) - if verbose: - rospy.logwarn(logwarn_msg) - raise InputMessageInvalidError( - message="Invalid joint_names were given.", - log_message=logwarn_msg, - details={"invalid_joint_names": invalid_joint_names}, - ) - - # Get the current state of the arm and hand - arm_state_dict = OrderedDict( - zip( - self.move_group_arm.get_active_joints(), - self.move_group_arm.get_current_joint_values(), - ) - ) - hand_state_dict = OrderedDict( - zip( - self.move_group_hand.get_active_joints(), - self.move_group_hand.get_current_joint_values(), - ) - ) - input_command_dict = OrderedDict(zip(joint_names, joint_positions)) + rospy.logwarn(logwarn_msg) # Update current state dictionary with given joint_position setpoints arm_output_command_dict = copy.deepcopy(arm_state_dict) - hand_output_command_dict = copy.deepcopy(hand_state_dict) for (joint, position) in input_command_dict.items(): # Update arm if joint in arm_state_dict: arm_output_command_dict[joint] = position - for (joint, position) in input_command_dict.items(): # Update hand - if joint in hand_state_dict: - hand_output_command_dict[joint] = position + if control_group in ["hand", "both"]: + hand_output_command_dict = copy.deepcopy(hand_state_dict) + for (joint, position) in input_command_dict.items(): # Update hand + if joint in hand_state_dict: + hand_output_command_dict[joint] = position # Return moveit_commander_control command dictionary - if control_group.lower() == "arm": + if control_group == "arm": control_commands = {"arm": list(arm_output_command_dict.values())} - elif control_group.lower() == "hand": + elif control_group == "hand": control_commands = {"hand": list(hand_output_command_dict.values())} else: control_commands = { @@ -565,8 +675,10 @@ def _create_joint_positions_commands( # noqa: C901 ############################################### # Service callback functions ################## ############################################### - def _dyn_reconfigure_cb(self, config, level): - """Dynamic reconfigure callback function. + def _dyn_reconfigure_cb(self, config, level): # noqa: C901 + """Dynamic reconfigure callback function. Can be used to update the + ``max_velocity_scaling_factor`` and ``max_acceleration_scaling_factor`` used by + the MoveIt control server. Args: config (:obj:`dynamic_reconfigure.encoding.Config`): The current dynamic @@ -612,31 +724,34 @@ def _dyn_reconfigure_cb(self, config, level): self.move_group_arm.set_max_velocity_scaling_factor( config["max_velocity_scaling_factor"] ) - self.move_group_hand.set_max_velocity_scaling_factor( - config["max_velocity_scaling_factor"] - ) self.move_group_arm.set_max_acceleration_scaling_factor( config["max_acceleration_scaling_factor"] ) - self.move_group_hand.set_max_acceleration_scaling_factor( - config["max_acceleration_scaling_factor"] - ) + if self._load_gripper: + self.move_group_hand.set_max_velocity_scaling_factor( + config["max_velocity_scaling_factor"] + ) + self.move_group_hand.set_max_acceleration_scaling_factor( + config["max_acceleration_scaling_factor"] + ) elif level == 0: # Update move group velocity settings self.move_group_arm.set_max_velocity_scaling_factor( config["max_velocity_scaling_factor"] ) - self.move_group_hand.set_max_velocity_scaling_factor( - config["max_velocity_scaling_factor"] - ) + if self._load_gripper: + self.move_group_hand.set_max_velocity_scaling_factor( + config["max_velocity_scaling_factor"] + ) elif level == 1: # Update move group acceleration settings self.move_group_arm.set_max_acceleration_scaling_factor( config["max_acceleration_scaling_factor"] ) - self.move_group_hand.set_max_acceleration_scaling_factor( - config["max_acceleration_scaling_factor"] - ) + if self._load_gripper: + self.move_group_hand.set_max_acceleration_scaling_factor( + config["max_acceleration_scaling_factor"] + ) return config def _arm_set_ee_pose_callback(self, set_ee_pose_req): @@ -677,6 +792,7 @@ def _arm_set_ee_pose_callback(self, set_ee_pose_req): try: self.move_group_arm.set_pose_target(self.ee_pose_target) retval = self._execute(control_group="arm") + self.move_group_arm.clear_pose_targets() # Check if setpoint execution was successful if not all(retval): @@ -735,42 +851,40 @@ def _set_joint_positions_callback(self, set_joint_positions_req): # noqa: C901 # Set joint positions setpoint arm_joint_states = self.move_group_arm.get_current_joint_values() - hand_joint_states = self.move_group_hand.get_current_joint_values() rospy.logdebug("Current arm joint positions: %s" % arm_joint_states) - rospy.logdebug( - "Arm joint positions setpoint: %s" % moveit_commander_commands["arm"] - ) - rospy.logdebug("Current hand joint positions: %s" % hand_joint_states) - rospy.logdebug( - "Hand joint positions setpoint: %s" % moveit_commander_commands["hand"] - ) - rospy.logdebug("Setting arm and hand setpoints.") + if self._load_gripper: + hand_joint_states = self.move_group_hand.get_current_joint_values() + rospy.logdebug("Current hand joint positions: %s" % hand_joint_states) self.joint_positions_target = moveit_commander_commands set_joint_value_target_success_bool = [] set_joint_value_target_error_msg = [] try: - self.move_group_arm.set_joint_value_target(moveit_commander_commands["arm"]) - set_joint_value_target_success_bool.append(True) - except MoveItCommanderException as e: - set_joint_value_target_success_bool.append(False) - set_joint_value_target_error_msg.append(e.args[0]) - try: - self.move_group_hand.set_joint_value_target( - moveit_commander_commands["hand"] + rospy.logdebug( + "Arm joint positions setpoint: %s" % moveit_commander_commands["arm"] ) + self.move_group_arm.set_joint_value_target(moveit_commander_commands["arm"]) set_joint_value_target_success_bool.append(True) except MoveItCommanderException as e: set_joint_value_target_success_bool.append(False) set_joint_value_target_error_msg.append(e.args[0]) + if self._load_gripper: + try: + rospy.logdebug( + "Hand joint positions setpoint: %s" + % moveit_commander_commands["hand"] + ) + self.move_group_hand.set_joint_value_target( + moveit_commander_commands["hand"] + ) + set_joint_value_target_success_bool.append(True) + except MoveItCommanderException as e: + set_joint_value_target_success_bool.append(False) + set_joint_value_target_error_msg.append(e.args[0]) # Print error message if an error occurred and return if set_joint_value_target_error_msg: - log_warn_string = ( - "arm and hand" - if len(set_joint_value_target_error_msg) > 1 - else ("arm" if set_joint_value_target_success_bool[0] else "hand") - ) if len(set_joint_value_target_error_msg) > 1: + log_warn_string = "arm and hand" rospy.logwarn( "Setting arm joint position targets failed since there was an %s" % (lower_first_char(set_joint_value_target_error_msg[0])) @@ -780,6 +894,11 @@ def _set_joint_positions_callback(self, set_joint_positions_req): # noqa: C901 % (lower_first_char(set_joint_value_target_error_msg[1])) ) else: + log_warn_string = ( + "arm" + if not self._load_gripper + else ("arm" if set_joint_value_target_success_bool[0] else "hand") + ) rospy.logwarn( "Setting %s joint position targets failed since there was an %s" % ( @@ -820,7 +939,7 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req): """ rospy.logdebug("Setting arm joint position targets.") - # Check if set_joint_positions_req.joint_names contains duplicates\ + # Check if set_joint_positions_req.joint_names contains duplicates duplicate_list = get_duplicate_list(set_joint_positions_req.joint_names) if duplicate_list: rospy.logwarn( @@ -852,12 +971,11 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req): # Set joint positions setpoint arm_joint_states = self.move_group_arm.get_current_joint_values() rospy.logdebug("Current arm joint positions: %s" % arm_joint_states) - rospy.logdebug( - "Arm joint positions setpoint: %s" % moveit_commander_commands["arm"] - ) self.joint_positions_target = moveit_commander_commands - rospy.logdebug("Setting arm setpoints.") try: + rospy.logdebug( + "Arm joint positions setpoint: %s" % moveit_commander_commands["arm"] + ) self.move_group_arm.set_joint_value_target(moveit_commander_commands["arm"]) except MoveItCommanderException as e: rospy.logwarn( @@ -929,12 +1047,11 @@ def _hand_set_joint_positions_callback(self, set_joint_positions_req): # Set joint positions setpoint hand_joint_states = self.move_group_hand.get_current_joint_values() rospy.logdebug("Current hand joint positions: %s" % hand_joint_states) - rospy.logdebug( - "Hand joint positions setpoint: %s" % moveit_commander_commands["hand"] - ) self.joint_positions_target = moveit_commander_commands - rospy.logdebug("Setting hand joint position setpoints.") try: + rospy.logdebug( + "Hand joint positions setpoint: %s" % moveit_commander_commands["hand"] + ) self.move_group_hand.set_joint_value_target( moveit_commander_commands["hand"] ) @@ -963,7 +1080,7 @@ def _hand_set_joint_positions_callback(self, set_joint_positions_req): resp.message = e.args[0] return resp - def _arm_get_ee_pose_callback(self, get_ee_pose_req): + def _arm_get_ee_pose_callback(self, _): """Request end effector pose. Args: @@ -978,7 +1095,7 @@ def _arm_get_ee_pose_callback(self, get_ee_pose_req): ee_pose_resp = ee_pose.pose return ee_pose_resp - def _arm_get_ee_rpy_callback(self, get_ee_rpy_req): + def _arm_get_ee_rpy_callback(self, _): """Request current end effector (EE) orientation. Args: @@ -996,7 +1113,7 @@ def _arm_get_ee_rpy_callback(self, get_ee_rpy_req): ee_rpy_res.p = ee_rpy[2] return ee_rpy_res - def _arm_get_ee_callback(self, get_ee_req): + def _arm_get_ee_callback(self, _): """Request end effector (EE) name. Args: @@ -1055,7 +1172,9 @@ def _get_random_joint_positions_callback( # noqa: C901 """ # Retrieve possible joints arm_joints = self.move_group_arm.get_active_joints() - hand_joints = self.move_group_hand.get_active_joints() + hand_joints = ( + self.move_group_hand.get_active_joints() if self._load_gripper else [] + ) valid_joint_names = flatten_list( [ [item + "_min", item + "_max"] @@ -1111,17 +1230,20 @@ def _get_random_joint_positions_callback( # noqa: C901 ) except MoveItCommanderException: get_random_arm_joint_positions_srvs_exception = True - try: - random_hand_joint_values_unbounded = OrderedDict( - zip(hand_joints, self.move_group_hand.get_random_joint_values()) - ) - except MoveItCommanderException: - get_random_hand_joint_positions_srvs_exception = True + if self._load_gripper: + try: + random_hand_joint_values_unbounded = OrderedDict( + zip(hand_joints, self.move_group_hand.get_random_joint_values()) + ) + except MoveItCommanderException: + get_random_hand_joint_positions_srvs_exception = True # Get random joint positions (while taking into possible joint limits) resp = GetRandomJointPositionsResponse() - random_arm_joint_values = random_arm_joint_values_unbounded # Fixme: Why here? - random_hand_joint_values = random_hand_joint_values_unbounded + random_arm_joint_values = random_arm_joint_values_unbounded + random_hand_joint_values = ( + random_hand_joint_values_unbounded if self._load_gripper else {} + ) if ( not get_random_position_req.joint_limits.names and not get_random_position_req.joint_limits.values @@ -1130,8 +1252,6 @@ def _get_random_joint_positions_callback( # noqa: C901 not get_random_arm_joint_positions_srvs_exception and not get_random_hand_joint_positions_srvs_exception ): - random_arm_joint_values = random_arm_joint_values_unbounded - random_hand_joint_values = random_hand_joint_values_unbounded resp.success = True else: resp.success = False @@ -1147,8 +1267,10 @@ def _get_random_joint_positions_callback( # noqa: C901 # Try to find random joint values within the joint limits n_sample = 0 arm_joint_commands_valid = False - hand_joint_commands_valid = False + hand_joint_commands_valid = False if self._load_gripper else True while True: # Continue till joint positions are valid or max samples size + if n_sample > 0: + rospy.logdebug("Retrieving a valid random joint positions.") for joint in get_unique_list( [ names.replace("_min", "").replace("_max", "") @@ -1157,16 +1279,16 @@ def _get_random_joint_positions_callback( # noqa: C901 ): # Sample random value for the given joint within the joint limits if ( - joint in random_arm_joint_values.keys() - and not arm_joint_commands_valid + not arm_joint_commands_valid + and joint in random_arm_joint_values.keys() ): random_arm_joint_values[joint] = np.random.uniform( joint_limits_dict[joint + "_min"], joint_limits_dict[joint + "_max"], ) if ( - joint in random_hand_joint_values.keys() - and not hand_joint_commands_valid + not hand_joint_commands_valid + and joint in random_hand_joint_values.keys() ): random_hand_joint_values[joint] = np.random.uniform( joint_limits_dict[joint + "_min"], @@ -1218,7 +1340,10 @@ def _get_random_joint_positions_callback( # noqa: C901 # Create response message and break out of loop random_arm_joint_values = random_arm_joint_values_unbounded - random_hand_joint_values = random_hand_joint_values_unbounded + if self._load_gripper: + random_hand_joint_values = ( + random_hand_joint_values_unbounded + ) resp.success = True break else: @@ -1276,9 +1401,9 @@ def _get_random_ee_pose_callback(self, get_random_ee_pose_req): # noqa: C901 """ # Get a random ee pose rospy.logdebug("Retrieving a valid random end effector pose.") - get_random_pose_srvs_exception = False try: random_ee_pose_unbounded = self.move_group_arm.get_random_pose() + get_random_pose_srvs_exception = False except MoveItCommanderException: get_random_pose_srvs_exception = True @@ -1303,7 +1428,6 @@ def _get_random_ee_pose_callback(self, get_random_ee_pose_req): # noqa: C901 else: resp.success = False else: # A bounding region was set - # Try to find a valid ee_pose within the bounding region n_sample = 0 ee_pose_valid = False @@ -1370,7 +1494,7 @@ def _get_random_ee_pose_callback(self, get_random_ee_pose_req): # noqa: C901 n_sample += 1 return resp - def _get_controlled_joints_cb(self, get_controlled_joints_req): + def _get_controlled_joints_cb(self, _): """Returns the joints that are currently being controlled by moveit. Args: diff --git a/panda_gazebo/src/panda_gazebo/version.py b/panda_gazebo/src/panda_gazebo/version.py index 10f5568a..26ef8a2f 100644 --- a/panda_gazebo/src/panda_gazebo/version.py +++ b/panda_gazebo/src/panda_gazebo/version.py @@ -1,6 +1,6 @@ # coding: utf-8 """The panda-gazebo version.""" -version = "1.0.17" +version = "1.0.18" __version__ = version # format: # ('mlc_major', 'mlc_minor', 'mlc_patch') diff --git a/panda_gazebo/srv/GetControlledJoints.srv b/panda_gazebo/srv/GetControlledJoints.srv index 92e781a7..4f42b928 100644 --- a/panda_gazebo/srv/GetControlledJoints.srv +++ b/panda_gazebo/srv/GetControlledJoints.srv @@ -1,5 +1,5 @@ # Service that can be used to retrieve the joints that are controlled for a given -# control type +# control type. string control_type --- bool success diff --git a/panda_gazebo/srv/GetEePose.srv b/panda_gazebo/srv/GetEePose.srv index c7233116..3b53e15f 100755 --- a/panda_gazebo/srv/GetEePose.srv +++ b/panda_gazebo/srv/GetEePose.srv @@ -1,4 +1,4 @@ -# Service that can be used to get pose information about the EE using moveit +# Service that can be used to get pose information about the EE using moveit. --- geometry_msgs/Pose pose \ No newline at end of file diff --git a/panda_gazebo/srv/GetEeRpy.srv b/panda_gazebo/srv/GetEeRpy.srv index 57989814..a0562ff4 100755 --- a/panda_gazebo/srv/GetEeRpy.srv +++ b/panda_gazebo/srv/GetEeRpy.srv @@ -1,4 +1,4 @@ -# Service that can be used to get orientation information about the EE using moveit +# Service that can be used to get orientation information about the EE using moveit. --- float32 r diff --git a/panda_gazebo/srv/GetMoveItControlledJoints.srv b/panda_gazebo/srv/GetMoveItControlledJoints.srv index 46578139..07a0072e 100644 --- a/panda_gazebo/srv/GetMoveItControlledJoints.srv +++ b/panda_gazebo/srv/GetMoveItControlledJoints.srv @@ -1,4 +1,4 @@ -# Service that can be used to retrieve the joints that are currently controlled by MoveIt +# Service that can be used to retrieve the joints that are currently controlled by MoveIt. --- bool success string[] controlled_joints diff --git a/panda_gazebo/srv/GetRandomEePose.srv b/panda_gazebo/srv/GetRandomEePose.srv index 79702f17..57ee1a02 100644 --- a/panda_gazebo/srv/GetRandomEePose.srv +++ b/panda_gazebo/srv/GetRandomEePose.srv @@ -1,4 +1,4 @@ -# Service that can be used to get a valid EE pose for the robot +# Service that can be used to get a valid EE pose for the robot. BoundingRegion bounding_region --- bool success diff --git a/panda_gazebo/srv/GetRandomJointPositions.srv b/panda_gazebo/srv/GetRandomJointPositions.srv index 610771d8..6b9808c6 100644 --- a/panda_gazebo/srv/GetRandomJointPositions.srv +++ b/panda_gazebo/srv/GetRandomJointPositions.srv @@ -1,5 +1,5 @@ # Service that can be used to get valid joint positions for controlling the robot arm -# and hand +# and hand. JointLimits joint_limits --- bool success diff --git a/panda_gazebo/srv/SetGripperWidth.srv b/panda_gazebo/srv/SetGripperWidth.srv new file mode 100755 index 00000000..fe7a0667 --- /dev/null +++ b/panda_gazebo/srv/SetGripperWidth.srv @@ -0,0 +1,9 @@ +# Service that can be used to control the robot hand gripper width using the panda_control_server. +# NOTE: It serves as a small wrapper around the 'franka_gripper/move` action but automatically +# sets the speed to the maximum speed. It further clips gripper width such that it is within +# the set max/min boundaries. +float64 width +bool wait +--- +bool success +string message \ No newline at end of file diff --git a/panda_gazebo/srv/SetJointCommands.srv b/panda_gazebo/srv/SetJointCommands.srv new file mode 100755 index 00000000..2fbf0302 --- /dev/null +++ b/panda_gazebo/srv/SetJointCommands.srv @@ -0,0 +1,11 @@ +# Service that can be used to control the robot joint positions/efforts using the +# panda_control_server. +# NOTE: This is done by publishing the joint commands to the right topics while sending +# the gripper width to the 'franka_gripper/move' action service. +string[] joint_names +float64[] joint_commands +string control_type +bool wait +--- +bool success +string message \ No newline at end of file diff --git a/panda_gazebo/srv/SetJointEfforts.srv b/panda_gazebo/srv/SetJointEfforts.srv index 1fb1d228..3821f548 100755 --- a/panda_gazebo/srv/SetJointEfforts.srv +++ b/panda_gazebo/srv/SetJointEfforts.srv @@ -1,5 +1,5 @@ # Service that can be used to control the robot efforts using the panda_control_server. -# This is done by publishing the joint efforts commands to the right topic. +# NOTE: This is done by publishing the joint efforts commands to the right topics. string[] joint_names float64[] joint_efforts bool wait diff --git a/panda_gazebo/srv/SetJointPositions.srv b/panda_gazebo/srv/SetJointPositions.srv index 9a043107..82eaa045 100755 --- a/panda_gazebo/srv/SetJointPositions.srv +++ b/panda_gazebo/srv/SetJointPositions.srv @@ -1,6 +1,6 @@ # Service that can be used to control the robot joint positions using the -# panda_control_server. This is done by publishing the joint efforts commands -# to the right topic. +# panda_control_server. +# NOTE: This is done by publishing the joint efforts commands to the right topics. string[] joint_names float64[] joint_positions bool wait diff --git a/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server.py b/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server.py new file mode 100755 index 00000000..a41c8db5 --- /dev/null +++ b/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +"""Small node that spins up a dynamic reconfigure server that can be used to change the +joint efforts. +""" +import rospy +from dynamic_reconfigure.server import Server +from panda_gazebo.cfg import JointEffortConfig +from std_msgs.msg import Float64 +import actionlib +from franka_gripper.msg import MoveAction, MoveGoal +from sensor_msgs.msg import JointState + + +class JointEffortDynamicReconfigureServer: + def __init__(self): + self.srv = Server(JointEffortConfig, self.callback) + + # Create joint effort publishers + self.arm_joint1_pub = rospy.Publisher( + "panda_arm_joint1_effort_controller/command", Float64, queue_size=10 + ) + self.arm_joint2_pub = rospy.Publisher( + "panda_arm_joint2_effort_controller/command", Float64, queue_size=10 + ) + self.arm_joint3_pub = rospy.Publisher( + "panda_arm_joint3_effort_controller/command", Float64, queue_size=10 + ) + self.arm_joint4_pub = rospy.Publisher( + "panda_arm_joint4_effort_controller/command", Float64, queue_size=10 + ) + self.arm_joint5_pub = rospy.Publisher( + "panda_arm_joint5_effort_controller/command", Float64, queue_size=10 + ) + self.arm_joint6_pub = rospy.Publisher( + "panda_arm_joint6_effort_controller/command", Float64, queue_size=10 + ) + self.arm_joint7_pub = rospy.Publisher( + "panda_arm_joint7_effort_controller/command", Float64, queue_size=10 + ) + self.gripper_move_client = actionlib.SimpleActionClient( + "franka_gripper/move", MoveAction + ) + self.gripper_move_client.wait_for_server() + self.arm_pubs = [ + self.arm_joint1_pub, + self.arm_joint2_pub, + self.arm_joint3_pub, + self.arm_joint4_pub, + self.arm_joint5_pub, + self.arm_joint6_pub, + self.arm_joint7_pub, + ] + + def callback(self, config, level): + """Dynamic reconfigure callback function. + + Args: + config (:obj:`dynamic_reconfigure.encoding.Config`): The current dynamic + reconfigure configuration object. + level (int): Bitmask that gives information about which parameter has been + changed. + + Returns: + :obj:`~dynamic_reconfigure.encoding.Config`: Modified dynamic reconfigure + configuration object. + """ + rospy.loginfo( + ( + "Reconfigure Request: {joint1_effort}, {joint2_effort}, " + "{joint3_effort} {joint4_effort}, {joint5_effort}, {joint6_effort}, " + "{joint7_effort}, {width}, {speed}" + ).format(**config) + ) + + # Set initial joint states + if level == -1: + joint_states = rospy.wait_for_message("joint_states", JointState) + position_dict = dict(zip(joint_states.name, joint_states.position)) + effort_dict = dict(zip(joint_states.name, joint_states.effort)) + set_values = list(effort_dict.values())[:-2] + set_values.append(list(position_dict.values())[-1] * 2) + config.update( + **dict( + zip([key for key in config.keys() if key != "groups"], set_values) + ) + ) + + # Write joint positions to controller topics + if level > -1 and level < 7: + self.arm_pubs[level].publish(list(config.values())[level]) + elif level in [7, 8]: + move_goal = MoveGoal(width=config["width"], speed=config["speed"]) + self.gripper_move_client.send_goal(move_goal) + result = self.gripper_move_client.wait_for_result() + if not result: + rospy.logerr("Something went wrong while setting the gripper commands.") + return config + + +if __name__ == "__main__": + rospy.init_node("joint_effort_reconfig_server", anonymous=False) + + effort_dyn_server = JointEffortDynamicReconfigureServer() + + rospy.spin() diff --git a/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server_test.py b/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server_test.py deleted file mode 100755 index 2b208e62..00000000 --- a/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server_test.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python3 -"""Small node that spins up a dynamic reconfigure server that can be used to change the -joint efforts. - -.. note:: - Please make sure to uncomment the -""" -import rospy -from dynamic_reconfigure.server import Server -from panda_gazebo.cfg import JointEffortConfig -from std_msgs.msg import Float64 - - -class JointEffortDynamicReconfigureServer: - def __init__(self): - self.srv = Server(JointEffortConfig, self.callback) - - # Create joint effort publishers - self.arm_joint1_pub = rospy.Publisher( - "/panda_arm_joint1_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint2_pub = rospy.Publisher( - "/panda_arm_joint2_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint3_pub = rospy.Publisher( - "/panda_arm_joint3_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint4_pub = rospy.Publisher( - "/panda_arm_joint4_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint5_pub = rospy.Publisher( - "/panda_arm_joint5_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint6_pub = rospy.Publisher( - "/panda_arm_joint6_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint7_pub = rospy.Publisher( - "/panda_arm_joint7_effort_controller/command", Float64, queue_size=10 - ) - self.hand_finger_joint1_pub = rospy.Publisher( - "/panda_hand_finger1_effort_controller/command", Float64, queue_size=10 - ) - self.hand_finger_joint2_pub = rospy.Publisher( - "/panda_hand_finger2_effort_controller/command", Float64, queue_size=10 - ) - self.pubs = [ - self.arm_joint1_pub, - self.arm_joint2_pub, - self.arm_joint3_pub, - self.arm_joint4_pub, - self.arm_joint5_pub, - self.arm_joint6_pub, - self.arm_joint7_pub, - self.hand_finger_joint1_pub, - self.hand_finger_joint2_pub, - ] - - def callback(self, config, level): - """Dynamic reconfigure callback function. - - Args: - config (:obj:`dynamic_reconfigure.encoding.Config`): The current dynamic - reconfigure configuration object. - level (int): Bitmask that gives information about which parameter has been - changed. - - Returns: - :obj:`~dynamic_reconfigure.encoding.Config`: Modified dynamic reconfigure - configuration object. - """ - rospy.loginfo( - ( - "Reconfigure Request: {joint1_effort}, {joint2_effort}, " - "{joint3_effort} {joint4_effort}, {joint5_effort}, {joint6_effort}, " - "{joint7_effort}" - ).format(**config) - ) - - # Write joint efforts to controller topics - if level != -1: - self.pubs[level].publish(list(config.values())[level]) - - return config - - -if __name__ == "__main__": - rospy.init_node( - "joint_effort_control_test_dynamic_reconfigure_server_test", anonymous=False - ) - - effort_dyn_server = JointEffortDynamicReconfigureServer() - - rospy.spin() diff --git a/panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server_test.py b/panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server.py similarity index 50% rename from panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server_test.py rename to panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server.py index 218affed..00d5a954 100755 --- a/panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server_test.py +++ b/panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server.py @@ -6,6 +6,9 @@ from dynamic_reconfigure.server import Server from panda_gazebo.cfg import JointPositionConfig from std_msgs.msg import Float64 +import actionlib +from franka_gripper.msg import MoveAction, MoveGoal +from sensor_msgs.msg import JointState class JointPositionDynamicReconfigureServer: @@ -14,33 +17,31 @@ def __init__(self): # Create joint position publishers self.arm_joint1_pub = rospy.Publisher( - "/panda_arm_joint1_position_controller/command", Float64, queue_size=10 + "panda_arm_joint1_position_controller/command", Float64, queue_size=10 ) self.arm_joint2_pub = rospy.Publisher( - "/panda_arm_joint2_position_controller/command", Float64, queue_size=10 + "panda_arm_joint2_position_controller/command", Float64, queue_size=10 ) self.arm_joint3_pub = rospy.Publisher( - "/panda_arm_joint3_position_controller/command", Float64, queue_size=10 + "panda_arm_joint3_position_controller/command", Float64, queue_size=10 ) self.arm_joint4_pub = rospy.Publisher( - "/panda_arm_joint4_position_controller/command", Float64, queue_size=10 + "panda_arm_joint4_position_controller/command", Float64, queue_size=10 ) self.arm_joint5_pub = rospy.Publisher( - "/panda_arm_joint5_position_controller/command", Float64, queue_size=10 + "panda_arm_joint5_position_controller/command", Float64, queue_size=10 ) self.arm_joint6_pub = rospy.Publisher( - "/panda_arm_joint6_position_controller/command", Float64, queue_size=10 + "panda_arm_joint6_position_controller/command", Float64, queue_size=10 ) self.arm_joint7_pub = rospy.Publisher( - "/panda_arm_joint7_position_controller/command", Float64, queue_size=10 + "panda_arm_joint7_position_controller/command", Float64, queue_size=10 ) - self.hand_finger_joint1_pub = rospy.Publisher( - "/panda_hand_finger1_position_controller/command", Float64, queue_size=10 + self.gripper_move_client = actionlib.SimpleActionClient( + "franka_gripper/move", MoveAction ) - self.hand_finger_joint2_pub = rospy.Publisher( - "/panda_hand_finger2_position_controller/command", Float64, queue_size=10 - ) - self.pubs = [ + self.gripper_move_client.wait_for_server() + self.arm_pubs = [ self.arm_joint1_pub, self.arm_joint2_pub, self.arm_joint3_pub, @@ -48,8 +49,6 @@ def __init__(self): self.arm_joint5_pub, self.arm_joint6_pub, self.arm_joint7_pub, - self.hand_finger_joint1_pub, - self.hand_finger_joint2_pub, ] def callback(self, config, level): @@ -69,21 +68,36 @@ def callback(self, config, level): ( "Reconfigure Request: {joint1_position}, {joint2_position}, " "{joint3_position} {joint4_position}, {joint5_position}, " - "{joint6_position}, {joint7_position}" + "{joint6_position}, {joint7_position}, {width}, {speed}" ).format(**config) ) - # Write joint positions to controller topics - if level != -1: - self.pubs[level].publish(list(config.values())[level]) + # Set initial joint states + if level == -1: + joint_states = rospy.wait_for_message("joint_states", JointState) + position_dict = dict(zip(joint_states.name, joint_states.position)) + set_values = list(position_dict.values())[:-2] + set_values.append(list(position_dict.values())[-1] * 2) + config.update( + **dict( + zip([key for key in config.keys() if key != "groups"], set_values) + ) + ) + # Write joint positions to controller topics + if level > -1 and level < 7: + self.arm_pubs[level].publish(list(config.values())[level]) + elif level in [7, 8]: + move_goal = MoveGoal(width=config["width"], speed=config["speed"]) + self.gripper_move_client.send_goal(move_goal) + result = self.gripper_move_client.wait_for_result() + if not result: + rospy.logerr("Something went wrong while setting the gripper commands.") return config if __name__ == "__main__": - rospy.init_node( - "joint_position_control_test_dynamic_reconfigure_server_test", anonymous=False - ) + rospy.init_node("joint_position_reconfig_server", anonymous=False) position_dyn_server = JointPositionDynamicReconfigureServer() diff --git a/panda_gazebo/tests/manual/moveit_commander_test.py b/panda_gazebo/tests/manual/moveit_commander_test.py index 8ec30337..b1d55230 100644 --- a/panda_gazebo/tests/manual/moveit_commander_test.py +++ b/panda_gazebo/tests/manual/moveit_commander_test.py @@ -28,8 +28,10 @@ def dist(p, q): # Create commanders robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() - group_name = "panda_arm" - move_group = moveit_commander.MoveGroupCommander(group_name) + + # Load arm and hand groups + move_group = moveit_commander.MoveGroupCommander("panda_arm") + hand_move_group = moveit_commander.MoveGroupCommander("hand") display_trajectory_publisher = rospy.Publisher( "/move_group/display_planned_path", @@ -55,49 +57,60 @@ def dist(p, q): print(robot.get_current_state()) print("") - # -- Plan joint goal -- - - # We get the joint values from the group and change some of the values: - joint_goal = move_group.get_current_joint_values() - joint_goal[0] = 0 - joint_goal[1] = -tau / 8 - joint_goal[2] = 0 - joint_goal[3] = -tau / 4 - joint_goal[4] = 0 - joint_goal[5] = tau / 6 # 1/6 of a turn - joint_goal[6] = 0 - - # METHOD 1 - # -- UNCOMMENT BELOW TO SEE THE SACLING FACTOR IN ACTION -- - # max_vel_scale_factor = 0.2 - # max_vel_scale_factor = 1.0 - # max_acc_scale_factor = 0.2 - # max_acc_scale_factor = 1.0 - # move_group.set_max_velocity_scaling_factor(max_vel_scale_factor) - # move_group.set_max_acceleration_scaling_factor(max_acc_scale_factor) - # -- UNCOMMENT ABOVE TO SEE THE SACLING FACTOR IN ACTION -- - move_group.set_joint_value_target(joint_goal) - move_group.plan() - move_group.go(wait=True) - - # # METHOD 2 - # # The go command can be called with joint values, poses, or without any - # # parameters if you have already set the pose or joint target for the group - # move_group.go(joint_goal, wait=True) - # # Calling ``stop()`` ensures that there is no residual movement + # # -- Plan arm joint goal -- + + # # We get the joint values from the group and change some of the values: + # joint_goal = move_group.get_current_joint_values() + # joint_goal[0] = 0 + # joint_goal[1] = -tau / 8 + # joint_goal[2] = 0 + # joint_goal[3] = -tau / 4 + # joint_goal[4] = 0 + # joint_goal[5] = tau / 6 # 1/6 of a turn + # joint_goal[6] = 0 + + # # METHOD 1 + # # -- UNCOMMENT BELOW TO SEE THE SACLING FACTOR IN ACTION -- + # # max_vel_scale_factor = 0.2 + # # max_vel_scale_factor = 1.0 + # # max_acc_scale_factor = 0.2 + # # max_acc_scale_factor = 1.0 + # # move_group.set_max_velocity_scaling_factor(max_vel_scale_factor) + # # move_group.set_max_acceleration_scaling_factor(max_acc_scale_factor) + # # -- UNCOMMENT ABOVE TO SEE THE SACLING FACTOR IN ACTION -- + # move_group.set_joint_value_target(joint_goal) + # move_group.plan() + # move_group.go(wait=True) + + # # # METHOD 2 + # # # The go command can be called with joint values, poses, or without any + # # # parameters if you have already set the pose or joint target for the group + # # move_group.go(joint_goal, wait=True) + + # # # Calling ``stop()`` ensures that there is no residual movement + # # move_group.stop() + + # # -- Plan pose goal -- + # pose_goal = geometry_msgs.msg.Pose() + # pose_goal.orientation.w = 1.0 + # pose_goal.position.x = 0.4 + # pose_goal.position.y = 0.1 + # pose_goal.position.z = 0.4 + + # move_group.set_pose_target(pose_goal) + # plan = move_group.go(wait=True) + + # # Calling `stop()` ensures that there is no residual movement # move_group.stop() - # -- Plan pose goal -- - pose_goal = geometry_msgs.msg.Pose() - pose_goal.orientation.w = 1.0 - pose_goal.position.x = 0.4 - pose_goal.position.y = 0.1 - pose_goal.position.z = 0.4 + # # It is always good to clear your targets after planning with poses. + # # Note: there is no equivalent function for clear_joint_value_targets() + # move_group.clear_pose_targets() + + # -- Plan hand goal -- + hand_move_group.set_joint_value_target([0.0, 0.0]) + (hand_plan_retval, plan, _, error_code) = hand_move_group.plan() + retval = hand_move_group.execute(plan, wait=True) - move_group.set_pose_target(pose_goal) - plan = move_group.go(wait=True) - # Calling `stop()` ensures that there is no residual movement + # Calling ``stop()`` ensures that there is no residual movement move_group.stop() - # It is always good to clear your targets after planning with poses. - # Note: there is no equivalent function for clear_joint_value_targets() - move_group.clear_pose_targets() diff --git a/panda_gazebo/tests/manual/panda_control_server_test.py b/panda_gazebo/tests/manual/panda_control_server_test.py index 483729b4..1ceb9c4c 100644 --- a/panda_gazebo/tests/manual/panda_control_server_test.py +++ b/panda_gazebo/tests/manual/panda_control_server_test.py @@ -3,12 +3,15 @@ import sys import actionlib -import actionlib_tutorials.msg import rospy from panda_gazebo.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal from panda_gazebo.srv import ( SetJointEfforts, SetJointEffortsRequest, + SetGripperWidth, + SetGripperWidthRequest, + SetJointCommands, + SetJointCommandsRequest, SetJointPositions, SetJointPositionsRequest, GetControlledJointsRequest, @@ -30,168 +33,85 @@ if __name__ == "__main__": rospy.init_node("panda_control_server_test") - # -- TEST SET JOINT EFFORTS -- - # %% /panda_control_server/set_joint_efforts test + # -- TEST SET JOINT COMMANDS -- - # # Connect to /panda_control_server/set_joint_efforts - # rospy.logdebug("Connecting to '/panda_control_server/set_joint_efforts' service.") - # rospy.wait_for_service("/panda_control_server/set_joint_efforts", timeout=10) - # set_joint_effort_srv = rospy.ServiceProxy( - # "/panda_control_server/set_joint_efforts", SetJointEfforts + # %% panda_control_server/set_joint_commands test + + # # Connect to panda_control_server/set_joint_commands + # rospy.logdebug( + # "Connecting to '/panda/panda_control_server/set_joint_commands' service." + # ) + # rospy.wait_for_service("/panda/panda_control_server/set_joint_commands", timeout=10) + # set_arm_joint_effort_srv = rospy.ServiceProxy( + # "/panda/panda_control_server/set_joint_commands", SetJointCommands + # ) + # rospy.logdebug( + # "Connected to '/panda/panda_control_server/set_joint_commands' service!" # ) - # rospy.logdebug("Connected to 'panda_control_server/set_joint_efforts' service!") # # Generate joint_efforts msg - # set_joint_efforts_msg = SetJointEffortsRequest() - # set_joint_efforts_msg.wait = True - # set_joint_efforts_msg.joint_names = [ - # "panda_finger_joint1", - # "panda_finger_joint2", + # set_joint_commands_msg = SetJointCommandsRequest() + # set_joint_commands_msg.joint_names = [ # "panda_joint1", # "panda_joint2", + # "panda_joint3", + # "gripper_width", # ] - # set_joint_efforts_msg.joint_efforts = [0, 0, 0, 0] - # # set_joint_efforts_msg.joint_efforts = [50, 50, 50, 30] - # retval = set_joint_effort_srv.call(set_joint_efforts_msg) - # print(retval.message) + # set_joint_commands_msg.wait = True + # set_joint_commands_msg.control_type = "position_control" + # set_joint_commands_msg.joint_commands = [1, 2, 3, 0.08] + # retval = set_arm_joint_effort_srv.call(set_joint_commands_msg) + # print(retval) # -- TEST SET ARM JOINT EFFORTS -- - # %% /panda_control_server/panda_arm/set_joint_efforts test - # # Connect to /panda_control_server/set_joint_efforts + # %% panda_control_server/panda_arm/set_joint_efforts test + + # # Connect to panda_control_server/set_joint_efforts # rospy.logdebug( - # "Connecting to '/panda_control_server/panda_arm/set_joint_efforts' service." + # "Connecting to '/panda/panda_control_server/panda_arm/set_joint_efforts' service." # ) # rospy.wait_for_service( - # "/panda_control_server/panda_arm/set_joint_efforts", timeout=10 + # "/panda/panda_control_server/panda_arm/set_joint_efforts", timeout=10 # ) # set_arm_joint_effort_srv = rospy.ServiceProxy( - # "/panda_control_server/panda_arm/set_joint_efforts", SetJointEfforts + # "/panda/panda_control_server/panda_arm/set_joint_efforts", SetJointEfforts # ) # rospy.logdebug( - # "Connected to 'panda_control_server/panda_arm/set_joint_efforts' service!" + # "Connected to '/panda/panda_control_server/panda_arm/set_joint_efforts' service!" # ) # # Generate joint_efforts msg # set_arm_joint_efforts_msg = SetJointEffortsRequest() - # # set_arm_joint_efforts_msg.joint_names = ["panda_joint2", "panda_joint3"] + # set_arm_joint_efforts_msg.joint_names = ["panda_joint2", "panda_joint3"] # set_arm_joint_efforts_msg.joint_efforts = [0, 0] # # set_arm_joint_efforts_msg.joint_efforts = [0, 0, 0] # retval = set_arm_joint_effort_srv.call(set_arm_joint_efforts_msg) # print(retval.message) - # --# - TEST SET HAND EFFORTS -- - # %% /panda_control_server/panda_hand/set_joint_positions test - - # # Connect to /panda_control_server/set_joint_positions - # rospy.logdebug( - # "Connecting to '/panda_control_server/panda_hand/set_joint_efforts' service." - # ) - # rospy.wait_for_service( - # "/panda_control_server/panda_hand/set_joint_efforts", timeout=10 - # ) - # set_hand_joint_effort_srv = rospy.ServiceProxy( - # "/panda_control_server/panda_hand/set_joint_efforts", SetJointEfforts - # ) - # rospy.logdebug( - # "Connected to 'panda_control_server/panda_hand/set_joint_efforts' service!" - # ) - - # # Generate joint_efforts msg - # set_hand_joint_efforts_msg = SetJointEffortsRequest() - # set_hand_joint_efforts_msg.joint_names = [ - # "panda_finger_joint1", - # "panda_finger_joint2", - # ] - # set_hand_joint_efforts_msg.joint_efforts = [-0.08, 0.05] - # # set_hand_joint_efforts_msg.joint_efforts = [-0.08, -0.08] - # set_hand_joint_efforts_msg.wait = True - # retval = set_hand_joint_effort_srv.call(set_hand_joint_efforts_msg) - # print(retval.message) - - # --# - TEST SET JOINT POSITIONS -- - # %% /panda_control_server/set_joint_positions test - - # # Connect to /panda_control_server/set_joint_positions - # rospy.logdebug("Connecting to '/panda_control_server/set_joint_positions' service.") - # rospy.wait_for_service("/panda_control_server/set_joint_positions", timeout=10) - # set_joint_positions_srv = rospy.ServiceProxy( - # "/panda_control_server/set_joint_positions", SetJointPositions - # ) - # rospy.logdebug("Connected to 'panda_control_server/set_joint_positions' service!") - - # # Generate joint_efforts msg - # set_joint_positions_msg = SetJointPositionsRequest() - # set_joint_positions_msg.joint_names = [ - # "panda_finger_joint1", - # "panda_finger_joint2", - # "panda_joint1", - # "panda_joint2", - # "panda_joint3", - # "panda_joint4", - # "panda_joint5", - # "panda_joint6", - # "panda_joint7", - # ] - # # set_joint_positions_msg.joint_positions = (1,) - # # set_joint_positions_msg.joint_positions = [1.5, 2, 4, 5] - # # set_joint_positions_msg.joint_positions = [ - # # 0.0, - # # 0.0, - # # 0.0, - # # 1.5, - # # 1.5, - # # 0.0, - # # 0.0, - # # 0.0, - # # ] - # # set_joint_positions_msg.joint_positions = [ - # # 1.5, - # # 0.0, - # # 1.0, - # # 1.5, - # # 1.5, - # # 1.0, - # # 1.0, - # # 0.02, - # # ] - # set_joint_positions_msg.joint_positions = [ - # 0.0, - # 0.0, - # 0.2, - # 0.2, - # 0.2, - # 0.2, - # 0.2, - # 1.2, - # 1.2, - # ] - # # set_arm_joint_positions_msg.joint_positions = [0.0, 1.5] - # set_joint_positions_msg.wait = True - # # set_joint_positions_msg.joint_names = ["panda_finger_joint1", "panda_joint2"] - # retval = set_joint_positions_srv.call(set_joint_positions_msg) - # print(retval.message) - # -- TEST SET ARM JOINT POSITIONS -- - # %% /panda_control_server/panda_arm/set_joint_positions test - # # Connect to /panda_control_server/set_joint_positions + # %% panda_control_server/panda_arm/set_joint_positions test + + # # Connect to panda_control_server/set_joint_positions # rospy.logdebug( - # "Connecting to '/panda_control_server/panda_arm/set_joint_positions' service." + # "Connecting to '/panda/panda_control_server/panda_arm/set_joint_positions' " + # "service." # ) # rospy.wait_for_service( - # "/panda_control_server/panda_arm/set_joint_positions", timeout=10 + # "/panda/panda_control_server/panda_arm/set_joint_positions", timeout=10 # ) # set_arm_joint_positions_srv = rospy.ServiceProxy( - # "/panda_control_server/panda_arm/set_joint_positions", SetJointPositions + # "/panda/panda_control_server/panda_arm/set_joint_positions", SetJointPositions # ) # rospy.logdebug( - # "Connected to 'panda_control_server/panda_arm/set_joint_positions' service!" + # "Connected to '/panda/panda_control_server/panda_arm/set_joint_positions' " + # "service!" # ) # # Generate set_arm_joint_positions_msg # set_arm_joint_positions_msg = SetJointPositionsRequest() - # set_arm_joint_positions_msg.joint_names = ["panda_joint5", "panda_joint6"] + # # set_arm_joint_positions_msg.joint_names = ["panda_joint5", "panda_joint6"] # set_arm_joint_positions_msg.joint_positions = [1.5, 2] # # set_arm_joint_positions_msg.joint_positions = [ # # 0.0, @@ -216,145 +136,27 @@ # retval = set_arm_joint_positions_srv.call(set_arm_joint_positions_msg) # print(retval.message) - # # -- TEST SET HAND JOINT POSITIONS -- - # # %% /panda_control_server/panda_hand/set_joint_positions test - # # Connect to /panda_control_server/set_joint_positions - # rospy.logdebug( - # "Connecting to '/panda_control_server/panda_hand/set_joint_positions' service." - # ) - # rospy.wait_for_service( - # "/panda_control_server/panda_hand/set_joint_positions", timeout=10 - # ) - # set_hand_joint_positions_srv = rospy.ServiceProxy( - # "/panda_control_server/panda_hand/set_joint_positions", SetJointPositions - # ) - # rospy.logdebug( - # "Connected to 'panda_control_server/panda_hand/set_joint_positions' service!" - # ) - - # # Generate joint_efforts msg - # set_hand_joint_positions_msg = SetJointPositionsRequest() - # set_hand_joint_positions_msg.joint_names = [ - # "panda_finger_joint1", - # "panda_finger_joint2", - # ] - # set_hand_joint_positions_msg.joint_positions = [0.04, 0.04] - # set_hand_joint_positions_msg.wait = True - # retval = set_hand_joint_positions_srv.call(set_hand_joint_positions_msg) - # print(retval.message) - - # -- TEST SET JOINT TRAJ SERVICE -- - - # Create action client - follow_joint_traj_client = actionlib.SimpleActionClient( - "/panda_control_server/follow_joint_trajectory", - FollowJointTrajectoryAction, - ) - - # Waits until the action server has started up and started - # listening for goals. - retval = follow_joint_traj_client.wait_for_server(timeout=rospy.Duration(5)) - if not retval: - rospy.logerr("Shutting down") - sys.exit(0) - - # Create action client goal - header = Header() - # header.stamp = rospy.get_rostime() - goal = FollowJointTrajectoryGoal() - goal.trajectory.joint_names = [ - "panda_joint1", - "panda_joint2", - "panda_joint3", - "panda_joint4", - "panda_joint5", - "panda_joint6", - "panda_joint7", - ] - goal.trajectory.joint_names = [ - "panda_finger_joint1", - "panda_finger_joint2", - ] - point = JointTrajectoryPoint() - point.positions = [ - 0.04, - 0.04, - ] - # point.time_from_start.secs = 1 - point.time_from_start = rospy.rostime.Duration(nsecs=1) - goal.trajectory.points.append(point) - # goal.trajectory.header = header - # goal.goal_time_tolerance.secs = 5 - - # Send goal - follow_joint_traj_client.send_goal(goal) - follow_joint_traj_client.wait_for_result() - result = follow_joint_traj_client.get_result() - print(result) - # -- TEST SET ARM JOINT TRAJ SERVICE -- - # Create action client - follow_joint_traj_client = actionlib.SimpleActionClient( - "/panda_control_server/panda_arm/follow_joint_trajectory", - FollowJointTrajectoryAction, - ) - - # Waits until the action server has started up and started - # listening for goals. - retval = follow_joint_traj_client.wait_for_server(timeout=rospy.Duration(5)) - if not retval: - rospy.logerr("Shutting down") - sys.exit(0) - - # Create action client goal - header = Header() - # header.stamp = rospy.get_rostime() - goal = FollowJointTrajectoryGoal() - # goal.trajectory.joint_names = [ - # "panda_joint1", - # "panda_joint2", - # "panda_joint3", - # "panda_joint4", - # "panda_joint5", - # "panda_joint6", - # "panda_joint7", - # ] - point = JointTrajectoryPoint() - point.positions = [ - 0.02, - 0.02, - ] - point.time_from_start.secs = 1 - goal.trajectory.points.append(point) - # goal.trajectory.header = header - # goal.goal_time_tolerance.secs = 5 - - # Send goal - follow_joint_traj_client.send_goal(goal) - follow_joint_traj_client.wait_for_result() - result = follow_joint_traj_client.get_result() - print(result) - - # -- TEST SET HAND JOINT TRAJ SERVICE -- + # %% panda_control_server/panda_arm/follow_joint_trajectory test - # Create action client - follow_joint_traj_client = actionlib.SimpleActionClient( - "/panda_control_server/panda_hand/follow_joint_trajectory", - FollowJointTrajectoryAction, - ) + # # Create action client + # follow_joint_traj_client = actionlib.SimpleActionClient( + # "/panda/panda_control_server/panda_arm/follow_joint_trajectory", + # FollowJointTrajectoryAction, + # ) - # Waits until the action server has started up and started - # listening for goals. - retval = follow_joint_traj_client.wait_for_server(timeout=rospy.Duration(5)) - if not retval: - rospy.logerr("Shutting down") - sys.exit(0) + # # Waits until the action server has started up and started + # # listening for goals. + # retval = follow_joint_traj_client.wait_for_server(timeout=rospy.Duration(5)) + # if not retval: + # rospy.logerr("Shutting down") + # sys.exit(0) - # Create action client goal - header = Header() - # header.stamp = rospy.get_rostime() - goal = FollowJointTrajectoryGoal() + # # Create action client goal + # header = Header() + # # header.stamp = rospy.get_rostime() + # goal = FollowJointTrajectoryGoal() # goal.trajectory.joint_names = [ # "panda_joint1", # "panda_joint2", @@ -364,27 +166,43 @@ # "panda_joint6", # "panda_joint7", # ] - point = JointTrajectoryPoint() - point.positions = [ - 0.02, - 0.02, - ] - point.time_from_start.secs = 1 - goal.trajectory.points.append(point) - # goal.trajectory.header = header - # goal.goal_time_tolerance.secs = 5 - - # Send goal - follow_joint_traj_client.send_goal(goal) - follow_joint_traj_client.wait_for_result() - result = follow_joint_traj_client.get_result() - print(result) + # point = JointTrajectoryPoint() + # point.positions = [ + # 0.007681771204969046, + # -0.0032211054935684658, + # 0.008062242200223224, + # -1.579418370643596, + # -0.008283306265037815, + # 0.2501401410371793, + # 0.24266583523250507, + # ] + # point.time_from_start.secs = 1 + # goal.trajectory.points.append(point) + # # goal.trajectory.header = header + # # goal.goal_time_tolerance.secs = 5 + + # # Send goal + # follow_joint_traj_client.send_goal(goal) + # follow_joint_traj_client.wait_for_result() + # result = follow_joint_traj_client.get_result() + # print(result) + + # -- Test set gripper width service -- + req = SetGripperWidthRequest() + req.width = -0.09 + req.wait = True + get_controlled_joints_srv = rospy.ServiceProxy( + "/panda/panda_control_server/panda_hand/set_gripper_width", + SetGripperWidth, + ) + resp = get_controlled_joints_srv.call(req) + print(resp) - # # -- Test get controlled joints service -- + # -- Test get controlled joints service -- # req = GetControlledJointsRequest() # req.control_type = "position_control" # get_controlled_joints_srv = rospy.ServiceProxy( - # "/panda_control_server/get_controlled_joints", + # "/panda/panda_control_server/get_controlled_joints", # GetControlledJoints, # ) # resp = get_controlled_joints_srv.call(req) diff --git a/panda_gazebo/tests/manual/panda_moveit_server_test.py b/panda_gazebo/tests/manual/panda_moveit_server_test.py index c8c6430e..81303733 100644 --- a/panda_gazebo/tests/manual/panda_moveit_server_test.py +++ b/panda_gazebo/tests/manual/panda_moveit_server_test.py @@ -26,89 +26,69 @@ if __name__ == "__main__": rospy.init_node("test_panda_moveit_server") - # -- Test set robot joint positions service -- - req = SetJointPositionsRequest() - req.joint_positions = [ - 1.499895698181251, - 1.503605675322719, - 1.498779085379117, - -0.07011811940859314, - 1.5185806604171868, - 1.5004606311218627, - 1.4998977401721056, - 0.02, - 0.02, - ] + # # -- Test set robot joint positions service -- + # req = SetJointPositionsRequest() # req.joint_positions = [ - # -1.5588153829789288e-06, - # 0.007296781094924877, - # 6.0527248901820485e-06, - # 0.00016249652149724625, - # -4.777426150681663e-06, - # -0.0698786875740316, - # 3.394301404391342e-06, - # 0.5098243409314671, - # 8.292685809152545e-07, - # ] # NOTE: Second pose - # req.joint_positions = [0.00] # NOTE: Should fail - req.joint_names = [ - "panda_joint1", - "panda_joint2", - "panda_joint3", - "panda_joint4", - "panda_joint5", - "panda_joint6", - "panda_joint7", - "panda_finger_joint1", - "panda_finger_joint2", - ] - set_joint_positions_srv = rospy.ServiceProxy( - "panda_moveit_planner_server/set_joint_positions", SetJointPositions - ) - resp = set_joint_positions_srv.call(req) - print(resp.message) + # 2.15, + # 1.12, + # -1.59, + # -1.94, + # -2.44, + # 1.88, + # 1.54, + # 0.02, + # 0.02, + # ] + # req.joint_names = [ + # "panda_joint1", + # "panda_joint2", + # "panda_joint3", + # "panda_joint4", + # "panda_joint5", + # "panda_joint6", + # "panda_joint7", + # "panda_finger_joint1", + # "panda_finger_joint2", + # ] + # set_joint_positions_srv = rospy.ServiceProxy( + # "panda_moveit_planner_server/set_joint_positions", SetJointPositions + # ) + # resp = set_joint_positions_srv.call(req) + # print(resp.message) # # -- Test panda_arm set robot joint positions service -- # req = SetJointPositionsRequest() - # req.joint_positions = [ - # 1.499895698181251, - # 1.503605675322719, - # 1.498779085379117, - # -0.07011811940859314, - # 1.5185806604171868, - # 1.5004606311218627, - # 1.4998977401721056, - # ] - # # req.joint_positions = [0.5, 0.5, 0, 5, 4, 3, 2] # NOTE:: Should fail + # req.joint_positions = [-0.60, 0.33, -0.83, -1.46, 0.26, 1.69, -0.64, 0.016, 0.016] # # req.joint_names = ["panda_joint1", "panda_joint2"] # setarm__joint_positions_srv = rospy.ServiceProxy( # "panda_moveit_planner_server/panda_arm/set_joint_positions", SetJointPositions # ) # resp = setarm__joint_positions_srv.call(req) - # print(resp) - - # # -- Test set panda_hand robot joint positions service -- - # req = SetJointPositionsRequest() - # # req.joint_positions = [0.23, 0.03] # NOTE:: Should fail - # req.joint_positions = [0.03, 0.03] - # # req.joint_positions = [0.03] - # # req.joint_names = ["panda_finger_joint1"] - # set_hand_joint_positions_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_hand/set_joint_positions", SetJointPositions - # ) - # resp = set_hand_joint_positions_srv.call(req) - # print(resp) + # print(resp.message) - # -- Test set ee pose service -- - req = SetEePoseRequest() - req.pose.position.x = 0 - req.pose.position.y = 0.5 - req.pose.position.z = 0.5 - set_ee_pose_srv = rospy.ServiceProxy( - "panda_moveit_planner_server/panda_arm/set_ee_pose", SetEePose + # -- Test set panda_hand robot joint positions service -- + req = SetJointPositionsRequest() + # req.joint_positions = [0.23, 0.03] # NOTE:: Should fail + req.joint_positions = [0.0, 0.0] + # req.joint_positions = [0.03] + # req.joint_names = ["panda_finger_joint1"] + set_hand_joint_positions_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/panda_hand/set_joint_positions", + SetJointPositions, ) - resp = set_ee_pose_srv.call(req) - print(resp) + resp = set_hand_joint_positions_srv.call(req) + print(resp.message) + + # # -- Test set ee pose service -- + # req = SetEePoseRequest() + # req.pose.position.x = 0 + # req.pose.position.y = 0.5 + # req.pose.position.z = 0.5 + # set_ee_pose_srv = rospy.ServiceProxy( + # "panda_moveit_planner_server/panda_arm/set_ee_pose", SetEePose + # ) + # resp = set_ee_pose_srv.call(req) + # print(resp.message) # # -- Test get ee pose service -- # req = GetEePoseRequest() @@ -116,7 +96,7 @@ # "panda_moveit_planner_server/panda_arm/get_ee_pose", GetEePose # ) # resp = get_ee_pose_srv.call(req) - # print(resp) + # print(resp.message) # # -- Test get ee rpy service -- # req = GetEeRpyRequest() @@ -124,7 +104,7 @@ # "panda_moveit_planner_server/panda_arm/get_ee_rpy", GetEeRpy # ) # resp = get_ee_rpy_srv.call(req) - # print(resp) + # print(resp.message) # # -- Test get ee service -- # req = GetEeRequest() @@ -132,7 +112,7 @@ # "panda_moveit_planner_server/panda_arm/get_ee", GetEe # ) # resp = get_ee_srv.call(req) - # print(resp) + # print(resp.message) # # -- Test set ee service -- # req = SetEeRequest() @@ -141,7 +121,7 @@ # "panda_moveit_planner_server/panda_arm/set_ee", SetEe # ) # resp = set_ee_srv.call(req) - # print(resp) + # print(resp.message) # # Set back # req = SetEeRequest() @@ -160,7 +140,7 @@ # GetRandomJointPositions, # ) # resp = get_random_joint_positions_srv.call(req) - # print(resp) + # print(resp.message) # # -- Test get random pose service -- # req = GetRandomEePoseRequest() @@ -170,7 +150,7 @@ # GetRandomEePose, # ) # resp = get_random_ee_pose_srv.call(req) - # print(resp) + # print(resp.message) # # -- Test get controlled joints service -- # req = GetMoveItControlledJointsRequest() @@ -179,4 +159,4 @@ # GetMoveItControlledJoints, # ) # resp = get_controlled_joints_srv.call(req) - # print(resp) + # print(resp.message) diff --git a/panda_moveit_config b/panda_moveit_config index 3be0aa03..a50a9767 160000 --- a/panda_moveit_config +++ b/panda_moveit_config @@ -1 +1 @@ -Subproject commit 3be0aa035f4bd795a6dd70838935a7032332b191 +Subproject commit a50a97672d97b1dca66b8f0d036dbab8bca23b7b