(→Tutorial) |
(→Install and build Sawyer MoveIt Repo) |
||
(18 intermediate revisions by 2 users not shown) | |||
Line 5: | Line 5: | ||
</div> | </div> | ||
− | |||
<div class="content-block"> | <div class="content-block"> | ||
Line 14: | Line 13: | ||
</div> | </div> | ||
+ | |||
+ | {{TOClimit|limit=2}} | ||
<div class="content-block"> | <div class="content-block"> | ||
− | + | = Installation/Prerequisites = | |
+ | |||
+ | == Installing MoveIt == | ||
+ | |||
+ | <tabs> | ||
+ | <tab name="ROS Kinetic"> | ||
− | === | + | <source lang="bash"> |
+ | = Make sure to update your sources = | ||
+ | $ sudo apt-get update | ||
+ | = Install MoveIt! = | ||
+ | $ sudo apt-get install ros-kinetic-moveit | ||
+ | </source> | ||
+ | </tab> | ||
+ | <tab name="ROS Indigo"> | ||
<source lang="bash"> | <source lang="bash"> | ||
Line 25: | Line 38: | ||
$ sudo apt-get update | $ sudo apt-get update | ||
= Install MoveIt! = | = Install MoveIt! = | ||
− | $ sudo apt-get install ros-indigo-moveit | + | $ sudo apt-get install ros-indigo-moveit |
</source> | </source> | ||
+ | </tab> | ||
+ | </tabs> | ||
− | Run <code>catkin_make</code> to make the new additions to your ROS workspace | + | == Installing and building Sawyer MoveIt Repo == |
+ | Run <code>catkin_make</code> to make the new Sawyer MoveIt! additions to your ROS workspace | ||
<source lang="bash"> | <source lang="bash"> | ||
$ cd ~/ros_ws/ | $ cd ~/ros_ws/ | ||
$ ./intera.sh | $ ./intera.sh | ||
+ | $ cd ~/ros_ws/src | ||
+ | $ wstool merge https://raw.githubusercontent.com/RethinkRobotics/sawyer_moveit/master/sawyer_moveit.rosinstall | ||
+ | $ wstool update | ||
+ | $ cd ~/ros_ws/ | ||
$ catkin_make | $ catkin_make | ||
+ | |||
</source> | </source> | ||
Line 39: | Line 60: | ||
<div class="content-block"> | <div class="content-block"> | ||
− | + | = Overview = | |
MoveIt! motion planning framework provides capabilities including Kinematics (IK, FK, Jacobian), Motion Planning (OMPL, SBPL, CHOMP) integrated as MoveIt! plugins, Environment Representation (robot representation, environment representation, collision checking, contraint evaluation), execution using move_groups, benchmarking, warehouse database for storage (scenes, robot states, motion plans), a C++/Python API and more! | MoveIt! motion planning framework provides capabilities including Kinematics (IK, FK, Jacobian), Motion Planning (OMPL, SBPL, CHOMP) integrated as MoveIt! plugins, Environment Representation (robot representation, environment representation, collision checking, contraint evaluation), execution using move_groups, benchmarking, warehouse database for storage (scenes, robot states, motion plans), a C++/Python API and more! | ||
Line 46: | Line 67: | ||
This tutorial will focus on the MoveIt! Rviz plugin as an introduction to some of the capabilities of MoveIt! | This tutorial will focus on the MoveIt! Rviz plugin as an introduction to some of the capabilities of MoveIt! | ||
− | + | ||
+ | ==Sawyer Planning Groups == | ||
Describes the joints considered during motion planning. These are specified in the SRDF for Sawyer. Sawyer's SRDF includes planning groups, additional collision checking information, and default configurations. These groups are generated dynamically via Xacro | Describes the joints considered during motion planning. These are specified in the SRDF for Sawyer. Sawyer's SRDF includes planning groups, additional collision checking information, and default configurations. These groups are generated dynamically via Xacro | ||
Line 65: | Line 87: | ||
** right_gripper | ** right_gripper | ||
− | The SRDF is generated dynamically at runtime and then loaded to the param server under <code>robot_semantic_description</code>. You can view the top level SRDF Xacro file | + | The SRDF is generated dynamically at runtime and then loaded to the param server under <code>robot_semantic_description</code>. You can view the top level SRDF Xacro file under sawyer_moveit_config/config/sawyer.srdf.xacro |
− | + | ||
− | |||
− | |||
</div> | </div> | ||
Line 74: | Line 94: | ||
<div class="content-block"> | <div class="content-block"> | ||
− | + | = Video = | |
Coming Soon! | Coming Soon! | ||
<!-- {{#ev:youtube|1Zdkwym42P4|500}} --> | <!-- {{#ev:youtube|1Zdkwym42P4|500}} --> | ||
Line 82: | Line 102: | ||
<div class="content-block"> | <div class="content-block"> | ||
− | + | = Tutorial = | |
Verify that the robot is enabled from an [[SDK Shell|SDK terminal session]], ex: | Verify that the robot is enabled from an [[SDK Shell|SDK terminal session]], ex: | ||
<source lang="bash"> | <source lang="bash"> | ||
Line 92: | Line 112: | ||
</source> | </source> | ||
− | In another [[SDK Shell|SDK terminal session]], Launch the [http://moveit.ros.org/ | + | In another [[SDK Shell|SDK terminal session]], Launch the [http://moveit.ros.org/documentation/plugins rviz MoveIt! plugin], ex: |
<tabs> | <tabs> | ||
− | |||
<tab name="Without Electric Grippers"> | <tab name="Without Electric Grippers"> | ||
If you do not have a gripper plugged into the robot, use the following to bringup MoveIt: | If you do not have a gripper plugged into the robot, use the following to bringup MoveIt: | ||
Line 103: | Line 122: | ||
This version will not add electric grippers collision shapes, and MoveIt may not give a valid trajectory if you have electric grippers plugged in. | This version will not add electric grippers collision shapes, and MoveIt may not give a valid trajectory if you have electric grippers plugged in. | ||
</tab> | </tab> | ||
− | |||
<tab name="With Electric Gripper(s)"> | <tab name="With Electric Gripper(s)"> | ||
If you have the Rethink Electric grippers plugged into your robot, use the following to bringup MoveIt: | If you have the Rethink Electric grippers plugged into your robot, use the following to bringup MoveIt: | ||
Line 115: | Line 133: | ||
The Rviz gui will then open showing Sawyer with [http://www.ros.org/wiki/interactive_markers interactive markers]:<br /> | The Rviz gui will then open showing Sawyer with [http://www.ros.org/wiki/interactive_markers interactive markers]:<br /> | ||
− | |||
− | + | [[File:sawyer_moveit_start_1.png|800px]] | |
+ | |||
+ | |||
+ | == Executing a simple motion plan == | ||
You will see the goal state for the motion planning, shown for each plan group in Orange. | You will see the goal state for the motion planning, shown for each plan group in Orange. | ||
You can then move Sawyer's arms to a planning goal by clicking and dragging the arrows (Cartesian) and rings (orientation) of the interactive markers. Kinematic/collision checking is happening simultaneously and will not allow self collisions (links will turn red), or goal states out of the reachable workspace (inability to drag and endpoint/gripper will turn red). <br /> | You can then move Sawyer's arms to a planning goal by clicking and dragging the arrows (Cartesian) and rings (orientation) of the interactive markers. Kinematic/collision checking is happening simultaneously and will not allow self collisions (links will turn red), or goal states out of the reachable workspace (inability to drag and endpoint/gripper will turn red). <br /> | ||
− | [[File: | + | [[File:sawyer_moveit_2_1.png|800px]] |
With a valid goal state, and assuming that the robot is in a valid start state, you are now ready to plan and execute a trajectory from your start state to the goal state. | With a valid goal state, and assuming that the robot is in a valid start state, you are now ready to plan and execute a trajectory from your start state to the goal state. | ||
Line 130: | Line 150: | ||
Please select the ''Planning'' tab in this frame. <br /> | Please select the ''Planning'' tab in this frame. <br /> | ||
− | [[File:sawyer_moveit_tabs_planning.png| | + | [[File:sawyer_moveit_tabs_planning.png|600px]] |
Click the ''Plan'' tab under the ''Commands'' field to plan a trajectory from your start state to the goal. You will see this solution executed in Rviz. When you are happy with the planned trajectory, click ''Execute'' to run this trajectory on the robot. | Click the ''Plan'' tab under the ''Commands'' field to plan a trajectory from your start state to the goal. You will see this solution executed in Rviz. When you are happy with the planned trajectory, click ''Execute'' to run this trajectory on the robot. | ||
Line 140: | Line 160: | ||
'''Note:''' It is dangerous to plan from a start state other than your current joint positions. Please update the ''Select Start State_ option under the '''Query''' field to reflect your current position before planning. This can be done by clicking the _Select Start State:_ text, from the drop down menu select <code><current></code>, click ''Update'' to complete this step. You will now be planning from your current position as the start state. | '''Note:''' It is dangerous to plan from a start state other than your current joint positions. Please update the ''Select Start State_ option under the '''Query''' field to reflect your current position before planning. This can be done by clicking the _Select Start State:_ text, from the drop down menu select <code><current></code>, click ''Update'' to complete this step. You will now be planning from your current position as the start state. | ||
− | After trajectory execution you will see | + | After trajectory execution you will see Sawyer has achieved your specified Goal. <br /> |
− | |||
− | + | [[File:sawyer_moveit_done_1.png|800px]] | |
+ | |||
+ | |||
+ | == Introducing an environment representation for planning == | ||
Select the ''Scene Object'' tab from the Motion Planning frame. | Select the ''Scene Object'' tab from the Motion Planning frame. | ||
− | [[File:sawyer_moveit_tabs_scene_objects.png| | + | [[File:sawyer_moveit_tabs_scene_objects.png|600px]] |
We will now create a scene object in a text file to be imported into our environment. | We will now create a scene object in a text file to be imported into our environment. | ||
Line 171: | Line 193: | ||
You can now import this scene from the '''Scene Geometry''' field selecting ''Import From Text'' | You can now import this scene from the '''Scene Geometry''' field selecting ''Import From Text'' | ||
+ | |||
+ | [[File:sawyer_moveit_tabs_context.png|600px]] | ||
Navigating to select <code>sawyer_moveit_config/sawyer_scenes/sawyer_pillar.scene</code> | Navigating to select <code>sawyer_moveit_config/sawyer_scenes/sawyer_pillar.scene</code> | ||
Line 176: | Line 200: | ||
After opening this scene you will now see the pillar inserted into your environment. | After opening this scene you will now see the pillar inserted into your environment. | ||
− | + | [[File:sawyer_moveit_pillar.png|800px]] | |
'''Important:''' You must publish your current scene so that MoveIt! knows that it will need to plan around your modified environment. Do so by selecting the ''Context_ tab from the Motion Planning frame. Under this tab you must click the _Publish Current Scene'' Button under the '''Planning Library''' field. | '''Important:''' You must publish your current scene so that MoveIt! knows that it will need to plan around your modified environment. Do so by selecting the ''Context_ tab from the Motion Planning frame. Under this tab you must click the _Publish Current Scene'' Button under the '''Planning Library''' field. | ||
− | + | [[File:sawyer_moveit_pubscene.png|600px]] | |
Similar to our previous planning we can now drag our interactive markers moving the goal state to a location on the opposite side of the pillar. | Similar to our previous planning we can now drag our interactive markers moving the goal state to a location on the opposite side of the pillar. | ||
− | + | [[File:sawyer_moveit_pillar_plan.png|800px]] | |
Selecting the ''Planning'' tab in the motion planning frame you can now plan a new trajectory that will avoid collision with your environment object (the pillar). The shell in which you launched sawyer_moveit.launch will provide information regarding which planner will be used, how long it took to find a solution, path simplification/smoothing time, and more. This will also display if your planner was unsuccessful in finding an allowable solution. This is often caused by collision with the environment during testing of the execution or invalid start/goal states. In very constrained or difficult motions, you may have to plan several times to find an acceptable solution for execution. | Selecting the ''Planning'' tab in the motion planning frame you can now plan a new trajectory that will avoid collision with your environment object (the pillar). The shell in which you launched sawyer_moveit.launch will provide information regarding which planner will be used, how long it took to find a solution, path simplification/smoothing time, and more. This will also display if your planner was unsuccessful in finding an allowable solution. This is often caused by collision with the environment during testing of the execution or invalid start/goal states. In very constrained or difficult motions, you may have to plan several times to find an acceptable solution for execution. | ||
Line 190: | Line 214: | ||
Upon execution the robot will avoid this 'virtual' object tracking the commanded trajectory. | Upon execution the robot will avoid this 'virtual' object tracking the commanded trajectory. | ||
− | + | [[File:sawyer_moveit_pillar_done.png|800px]] | |
− | |||
− | That's it, you have successfully commanded | + | That's it, you have successfully commanded Sawyer using MoveIt! |
− | + | === Programmatic interaction for planning === | |
− | There is much more information, tutorials, API documentation and more on [http:// | + | There is much more information, tutorials, API documentation and more on [http://docs.ros.org/kinetic/api/moveit_tutorials/html/ moveit.ros.org]. |
− | + | [http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/move_group_interface_tutorial.html C++ Tutorial] | |
− | [http://docs.ros.org/ | ||
+ | [http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/scripts/doc/move_group_python_interface_tutorial.html Python Tutorial] | ||
− | [http://moveit.ros.org/ | + | [http://moveit.ros.org/code-api/ MoveIt! Code API] |
− | |||
− | |||
− | |||
</div> | </div> | ||
<div class="content-block"> | <div class="content-block"> | ||
− | |||
<!-- == Using Depth Sensors with Sawyer and MoveIt! == | <!-- == Using Depth Sensors with Sawyer and MoveIt! == | ||
Line 220: | Line 239: | ||
<div class="content-block"> | <div class="content-block"> | ||
--> | --> | ||
− | |||
<!-- == IKFast == | <!-- == IKFast == | ||
− | [[#ikfast IKFast]] provides analytical closed-form solutions for manipulator inverse kinematics. Part of the [http://openrave.org/ OpenRAVE] robot architecture/software framework, IKFast has been used to provide closed-form solutions for | + | [[#ikfast IKFast]] provides analytical closed-form solutions for manipulator inverse kinematics. Part of the [http://openrave.org/ OpenRAVE] robot architecture/software framework, IKFast has been used to provide closed-form solutions for Sawyer's inverse kinematics. |
Advantages include '''order of magnitude''' faster IK solutions and the ability explore the manipulator's null space! | Advantages include '''order of magnitude''' faster IK solutions and the ability explore the manipulator's null space! | ||
− | ==== Using | + | ==== Using Sawyer's MoveIt! IKFast plugins ==== |
− | In the | + | In the sawyer_moveit_config package - modify the kinematics.yaml file to specify the use of the IKFast plugins. |
<source lang="bash"> | <source lang="bash"> | ||
− | rosed | + | rosed sawyer_moveit_config kinematics.yaml |
</source> | </source> | ||
You can now uncomment the use of the IKFastKinematicsPlugin, commenting out the default KDL solver. | You can now uncomment the use of the IKFastKinematicsPlugin, commenting out the default KDL solver. | ||
Line 237: | Line 255: | ||
The result should look like so: | The result should look like so: | ||
<code> | <code> | ||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
right_arm: | right_arm: | ||
# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin | # kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin | ||
# uncomment to use ikfast kinematics plugin | # uncomment to use ikfast kinematics plugin | ||
− | kinematics_solver: | + | kinematics_solver: sawyer_right_arm_kinematics/IKFastKinematicsPlugin |
kinematics_solver_attempts: 3 | kinematics_solver_attempts: 3 | ||
kinematics_solver_search_resolution: 0.005 | kinematics_solver_search_resolution: 0.005 | ||
Line 256: | Line 267: | ||
<source lang="bash"> | <source lang="bash"> | ||
catkin_make | catkin_make | ||
− | catkin_make install --pkg | + | catkin_make install --pkg sawyer_ikfast_right_arm_plugin |
− | |||
</source> | </source> | ||
Line 268: | Line 278: | ||
<div class="content-block"> | <div class="content-block"> | ||
--> | --> | ||
+ | = Interfaces = | ||
− | + | ||
− | + | == ROS APIs == | |
''See the [[API Reference#arm-joints|API Reference]] page for details.'' | ''See the [[API Reference#arm-joints|API Reference]] page for details.'' | ||
* Joint Trajectory Action Server - /robot/limb/right/follow_joint_trajectory [control_msgs/FollowJointTrajectoryAction] | * Joint Trajectory Action Server - /robot/limb/right/follow_joint_trajectory [control_msgs/FollowJointTrajectoryAction] | ||
− | + | ||
+ | == intera_interface APIs == | ||
* JointTrajectoryActionServer class: <code>joint_trajectory_action_server.py</code> | * JointTrajectoryActionServer class: <code>joint_trajectory_action_server.py</code> | ||
Also see the [https://rethinkrobotics.github.io/intera_sdk_docs/ Full intera_interface API docs for more details] | Also see the [https://rethinkrobotics.github.io/intera_sdk_docs/ Full intera_interface API docs for more details] | ||
+ | |||
</div> | </div> | ||
<div class="content-block"> | <div class="content-block"> | ||
− | + | = Related Examples/Tutorials = | |
* [[Joint Trajectory Playback Example|Joint Trajectory Playback Example]] | * [[Joint Trajectory Playback Example|Joint Trajectory Playback Example]] | ||
Line 288: | Line 301: | ||
<div class="content-block"> | <div class="content-block"> | ||
− | + | = Troubleshooting = | |
− | + | ||
+ | == The arm is not executing the trajectory == | ||
Verify that the robot is enabled: | Verify that the robot is enabled: | ||
Line 298: | Line 312: | ||
<code>rosrun intera_interface trajectory_controller.py</code> | <code>rosrun intera_interface trajectory_controller.py</code> | ||
− | + | ||
+ | == Arm not executing plan; "Unable to identify any set of controllers" == | ||
''Problem Description:'' | ''Problem Description:'' | ||
<blockquote> | <blockquote> |
Latest revision as of 20:36, 16 October 2017
This tutorial describes how to use Sawyer with MoveIt! the standard ROS motion planning framework.
Installation/Prerequisites
Installing MoveIt
= Make sure to update your sources =
$ sudo apt-get update
= Install MoveIt! =
$ sudo apt-get install ros-kinetic-moveit
= Make sure to update your sources =
$ sudo apt-get update
= Install MoveIt! =
$ sudo apt-get install ros-indigo-moveit
Installing and building Sawyer MoveIt Repo
Run catkin_make
to make the new Sawyer MoveIt! additions to your ROS workspace
$ cd ~/ros_ws/
$ ./intera.sh
$ cd ~/ros_ws/src
$ wstool merge https://raw.githubusercontent.com/RethinkRobotics/sawyer_moveit/master/sawyer_moveit.rosinstall
$ wstool update
$ cd ~/ros_ws/
$ catkin_make
Overview
MoveIt! motion planning framework provides capabilities including Kinematics (IK, FK, Jacobian), Motion Planning (OMPL, SBPL, CHOMP) integrated as MoveIt! plugins, Environment Representation (robot representation, environment representation, collision checking, contraint evaluation), execution using move_groups, benchmarking, warehouse database for storage (scenes, robot states, motion plans), a C++/Python API and more!
Sawyer now supports using MoveIt! through the addition of the configurable joint trajectory action server, and hosting of the necessary MoveIt! configuration files on RethinkRobotics/sawyer_moveit.
This tutorial will focus on the MoveIt! Rviz plugin as an introduction to some of the capabilities of MoveIt!
Sawyer Planning Groups
Describes the joints considered during motion planning. These are specified in the SRDF for Sawyer. Sawyer's SRDF includes planning groups, additional collision checking information, and default configurations. These groups are generated dynamically via Xacro
Planning Group
- right_arm
- chain base -> right_gripper
- right_j0
- right_j1
- right_j2
- right_j3
- right_j4
- right_j5
- right_j6
- right_hand
- right_gripper
- chain base -> right_gripper
- right_hand
- right_hand
- right_gripper
The SRDF is generated dynamically at runtime and then loaded to the param server under robot_semantic_description
. You can view the top level SRDF Xacro file under sawyer_moveit_config/config/sawyer.srdf.xacro
Video
Coming Soon!
Tutorial
Verify that the robot is enabled from an SDK terminal session, ex:
$ rosrun intera_interface enable_robot.py -e
Start the joint trajectory controller, ex:
$ rosrun intera_interface joint_trajectory_action_server.py
In another SDK terminal session, Launch the rviz MoveIt! plugin, ex:
If you do not have a gripper plugged into the robot, use the following to bringup MoveIt:
$ roslaunch sawyer_moveit_config sawyer_moveit.launch
This version will not add electric grippers collision shapes, and MoveIt may not give a valid trajectory if you have electric grippers plugged in.
If you have the Rethink Electric grippers plugged into your robot, use the following to bringup MoveIt:
$ roslaunch sawyer_moveit_config sawyer_moveit.launch electric_gripper:=true
This version will add the gripper linkages to the collision shapes.
The Rviz gui will then open showing Sawyer with interactive markers:
Executing a simple motion plan
You will see the goal state for the motion planning, shown for each plan group in Orange.
You can then move Sawyer's arms to a planning goal by clicking and dragging the arrows (Cartesian) and rings (orientation) of the interactive markers. Kinematic/collision checking is happening simultaneously and will not allow self collisions (links will turn red), or goal states out of the reachable workspace (inability to drag and endpoint/gripper will turn red).
With a valid goal state, and assuming that the robot is in a valid start state, you are now ready to plan and execute a trajectory from your start state to the goal state.
In the bottom left you will the motion planning frame with a tabbed interface to access different tools of the MoveIt! Rviz visualizer.
Please select the Planning tab in this frame.
Click the Plan tab under the Commands field to plan a trajectory from your start state to the goal. You will see this solution executed in Rviz. When you are happy with the planned trajectory, click Execute to run this trajectory on the robot.
Alternatively, if you would like to immediately execute the first plan from your current position to the goal position use the Plan and Execute button.
Another useful field in the Planning tab is Query. You can specify here _Select Start State_ choosing <current>
, <random>
, and <same as goal>
, choosing these options with the _Update_ button. Also, You may Select Goal State by clicking that button. Here again you can choose <current>
, <random>
, and <same as goal>
for the Goal State.
Note: It is dangerous to plan from a start state other than your current joint positions. Please update the Select Start State_ option under the Query field to reflect your current position before planning. This can be done by clicking the _Select Start State:_ text, from the drop down menu select <current>
, click Update to complete this step. You will now be planning from your current position as the start state.
After trajectory execution you will see Sawyer has achieved your specified Goal.
Introducing an environment representation for planning
Select the Scene Object tab from the Motion Planning frame.
We will now create a scene object in a text file to be imported into our environment.
$ roscd sawyer_moveit_config
$ mkdir sawyer_scenes
$ gedit sawyer_scenes/sawyer_pillar.scene
Copy in the following scene describing a pillar
pillar
* pillar
1
box
0.2 0.2 1
0.6 0.15 0
0 0 0 1
0 0 0 0
.
Save and exit.
You can now import this scene from the Scene Geometry field selecting Import From Text
Navigating to select sawyer_moveit_config/sawyer_scenes/sawyer_pillar.scene
After opening this scene you will now see the pillar inserted into your environment.
Important: You must publish your current scene so that MoveIt! knows that it will need to plan around your modified environment. Do so by selecting the Context_ tab from the Motion Planning frame. Under this tab you must click the _Publish Current Scene Button under the Planning Library field.
Similar to our previous planning we can now drag our interactive markers moving the goal state to a location on the opposite side of the pillar.
Selecting the Planning tab in the motion planning frame you can now plan a new trajectory that will avoid collision with your environment object (the pillar). The shell in which you launched sawyer_moveit.launch will provide information regarding which planner will be used, how long it took to find a solution, path simplification/smoothing time, and more. This will also display if your planner was unsuccessful in finding an allowable solution. This is often caused by collision with the environment during testing of the execution or invalid start/goal states. In very constrained or difficult motions, you may have to plan several times to find an acceptable solution for execution.
Upon execution the robot will avoid this 'virtual' object tracking the commanded trajectory.
That's it, you have successfully commanded Sawyer using MoveIt!
Programmatic interaction for planning
There is much more information, tutorials, API documentation and more on moveit.ros.org.
Interfaces
ROS APIs
See the API Reference page for details.
- Joint Trajectory Action Server - /robot/limb/right/follow_joint_trajectory [control_msgs/FollowJointTrajectoryAction]
intera_interface APIs
- JointTrajectoryActionServer class:
joint_trajectory_action_server.py
Also see the Full intera_interface API docs for more details
Related Examples/Tutorials
Troubleshooting
The arm is not executing the trajectory
Verify that the robot is enabled:
rosrun intera_interface enable_robot.py -e
Verify that the trajectory controller has been started:
rosrun intera_interface trajectory_controller.py
Arm not executing plan; "Unable to identify any set of controllers"
Problem Description:
Pressed plan, solution was found and a trajectory execution service request was recieved after which I got the error:
Unable to identify any set of controllers that can actuate the specified joints: [right_j0 right_j1 right_j2 right_j3 right_j4 right_j5 right_j6 ]
After which nothing occurred.
If you see the following warning error in the terminal output when roslaunch-ing sawyer_moveit.launch:
[FATAL] [1372432489.342541284]: Parameter '~moveit_controller_manager' not specified. This is needed to identify the plugin to use for interacting with controllers. No paths can be executed.
This indicates you need to make sure you have the appropriate controller manager plugins installed. Make sure to follow the Sawyer MoveIt! Installation instructions and install the required Debian packages for both moveit-full and any required plugins.