├── .gitignore
├── CHANGELOG.rst
├── CMakeLists.txt
├── LICENSE
├── README.rst
├── cfg
    ├── GripperActionServer.cfg
    ├── HeadActionServer.cfg
    ├── PositionFFJointTrajectoryActionServer.cfg
    ├── PositionJointTrajectoryActionServer.cfg
    └── VelocityJointTrajectoryActionServer.cfg
├── epydoc.config
├── package.xml
├── rosdoc.yaml
├── scripts
    ├── gripper_action_server.py
    ├── head_action_server.py
    └── joint_trajectory_action_server.py
├── setup.py
└── src
    ├── baxter_control
        ├── __init__.py
        └── pid.py
    ├── baxter_dataflow
        ├── __init__.py
        ├── signals.py
        ├── wait_for.py
        └── weakrefset.py
    ├── baxter_interface
        ├── __init__.py
        ├── analog_io.py
        ├── camera.py
        ├── digital_io.py
        ├── gripper.py
        ├── head.py
        ├── limb.py
        ├── navigator.py
        ├── robot_enable.py
        ├── robust_controller.py
        └── settings.py
    ├── gripper_action
        ├── __init__.py
        └── gripper_action.py
    ├── head_action
        ├── __init__.py
        └── head_action.py
    └── joint_trajectory_action
        ├── __init__.py
        ├── bezier.py
        └── joint_trajectory_action.py


/.gitignore:
--------------------------------------------------------------------------------
 1 | *.py[cod]
 2 | 
 3 | # C extensions
 4 | *.so
 5 | 
 6 | # Packages
 7 | *.egg
 8 | *.egg-info
 9 | dist
10 | build
11 | eggs
12 | parts
13 | bin
14 | var
15 | sdist
16 | develop-eggs
17 | .installed.cfg
18 | lib
19 | lib64
20 | 
21 | # Installer logs
22 | pip-log.txt
23 | 
24 | # Unit test / coverage reports
25 | .coverage
26 | .tox
27 | nosetests.xml
28 | 
29 | # Translations
30 | *.mo
31 | 
32 | # Mr Developer
33 | .mr.developer.cfg
34 | .project
35 | .cproject
36 | cmake_install.cmake
37 | .pydevproject
38 | *.swp
39 | *.swo
40 | CATKIN_IGNORE
41 | catkin
42 | catkin_generated
43 | devel
44 | 
45 | cpp
46 | docs
47 | msg_gen
48 | srv_gen
49 | *.smoketest
50 | *.cfgc
51 | *_msgs/src/
52 | */src/**/cfg
53 | */*/src/**/*
54 | 


--------------------------------------------------------------------------------
/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | 1.2.0 (2015-12-21)
 2 | ---------------------------------
 3 | - Added an optional parameter to Limb interface's move_to_joint_positions() to allow it to be aborted by test function
 4 |   Thanks to Yoan Mollard (ymollard) for submitting this pull request!
 5 | - Added a wait for the endpoint_state messages to be received before exiting Limb init
 6 | - Fixed a bug in the JTAS that would cause the robot to jump back into the last commanded pose when the 
 7 |   robot is Disabled/Re-enabled
 8 | - Fixed a bug that would cause the Limb's on_joint_states method to fail if extra <side> joints are added to the 
 9 |   robot's /robot/joint_states
10 | - Due to baxter_core_msgs change, updated EndEffectorProperties CUSTOM_GRIPPER to PASSIVE_GRIPPER
11 | - Due to baxter_core_msgs change, updated head interface's speed range from [0, 100] to [0, 1.0]
12 | - Due to baxter_core_msgs change, updated Navigator interface to use update Nav topics and lights msg field
13 | 
14 | 1.1.1 (2015-5-15)
15 | ---------------------------------
16 | -  Fixed a bug that caused the JTAS to error with a path of one or two points is supplied as a trajectory
17 | 
18 | 1.1.0 (2015-1-2)
19 | ---------------------------------
20 | - Updates baxter_interface to ROS Indigo
21 | - Upgrades to the Indigo robot required a reworking of how cameras power **on** / **off** - Three cameras can no longer be powered at the same time, and closing cameras turns power **off** to the specified camera and **on** to the other two
22 | - Updates joint trajectory action server to use new Inverse Dynamics Feed Forward Commands and configurations for smoother & more accurate MoveIt trajectory execution
23 | - Updates joint trajectory action server default control mode from *position* to *position_w_id* which uses 'raw' joint position control
24 | - Updates joint trajectory action server to replace linear, cubic, and quintic spline fitting with *Cubic Bezier Spline* interpolation. These splines can use Position, Velocity, and/or Acceleration to more accurately interpolate supplied trajectories
25 | - Adds head trajectory action server interface for controlling Baxter's head pan joint (thanks to @aginika for contributing this)
26 | - Updates verification of Gripper software versions to check firmware Build Date instead of Build Version (SDK gripper version 1.1 conflicted with Manufacturing versions)
27 | 
28 | 1.0.0 (2014-5-1)
29 | ---------------------------------
30 | - Adds new 'raw' joint position control mode
31 | - Updates robot_enable to validate SDK versus robot software versions
32 | - Updates joint trajectory action server adding linear, cubic and quintic spline fitting
33 | - Updates joint trajectory action server adding raw joint position control mode option for trajectory execution
34 | - Updates joint trajectory action server allowing execution parameters from messages to override parameter server options
35 | - Updates Limb class to use tcp_nodelay transport hint for joint states, endpoint state, and joint commands
36 | - Updates Limb class move_to_joint_positions adding optional accuracy threshold argument
37 | - Updates Gripper class to validate compatible gripper firmware versions
38 | - Updates Gripper class adding signaling on type, gripping, and moving state changes
39 | - Updates Gripper class adding convenience reset methods for reverting custom gripper properties
40 | - Updates Navigator class wheel_changed signal to use difference since last value
41 | - Renames Gripper class hardware_version to hardware_name
42 | - Fixes Head's nod incorrect wait_for validation
43 | 
44 | 0.7.0 (2013-11-21)
45 | ---------------------------------
46 | - Creation of baxter_interface repository from sdk-examples/baxter_interface package.
47 | - Adds joint torque control through the Limb interface.
48 | - Significant joint position accuracy improvements. Low level controls are responsible. The limb interface exploits these gains by introducing a low-pass filter in the move_to_joint_positions method allowing the robot to achieve <0.5 degrees accuracy across each joint.
49 | - Adds ability to set joint position command execution speed.
50 | - Package restructure in support of Catkin expected standards.
51 | - Limb interface API changes. Supports introduction of new singular joint command message, baxter_core_msgs/JointCommand, allowing for control in joint position, velocity and torque modes. The set_command_timeout method has been added to allow user specification of a safety timeout for joint velocity or torque control mode. If this timeout is not met, the robot will disable. Upon exiting one of these control modes, the user must specify another new method exit_control_mode which will enter the robot back into position control mode which has no command timeout.
52 | - Gripper interface API changes. Supports introduction of new gripper plugin robot interface. This new robot gripper software requires the use of the three main baxter_core_msgs gripper messages, EndEffectorCommand, EndEffectorState, and EndEffectorProperties. Commands to the gripper now require json strings containing the parameters which will describe the grippers actuation as well as the commands for execution.
53 | - Adds ability to specify a gripper/object mass.
54 | - Cameras interface adds ability to reset cameras. Useful if not all cameras are enumerated at boot.
55 | - Adds gripper action server. Follows standard ROS gripper action type control_msgs/GripperCommand.
56 | - Digital IO interface now uses signals/slots method.
57 | - Digital IO interface now uses read-only state property (no longer callable).
58 | - Adds clear_calibration to gripper interface. Sequences gripper commands for robust usage.
59 | - Adds Limb move_to_neutral command timeout parameter.
60 | - Renames trajectory_controller.py to joint_trajectory_action_server.py matching form of gripper_action_server.py.
61 | - Fixes reset errors on non-fatal error states.
62 | - Adds feedback to the joint trajectory action server.
63 | - Clarifies timeout messages to be explicit.
64 | - Removes Limb state_rate method.
65 | 
66 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(baxter_interface)
 3 | 
 4 | find_package(catkin
 5 |   REQUIRED
 6 |   COMPONENTS
 7 |   rospy
 8 |   actionlib
 9 |   sensor_msgs
10 |   std_msgs
11 |   control_msgs
12 |   trajectory_msgs
13 |   dynamic_reconfigure
14 |   baxter_core_msgs
15 | )
16 | 
17 | catkin_python_setup()
18 | 
19 | generate_dynamic_reconfigure_options(
20 |   cfg/PositionJointTrajectoryActionServer.cfg
21 |   cfg/VelocityJointTrajectoryActionServer.cfg
22 |   cfg/PositionFFJointTrajectoryActionServer.cfg
23 |   cfg/GripperActionServer.cfg
24 |   cfg/HeadActionServer.cfg
25 | )
26 | 
27 | add_dependencies(${PROJECT_NAME}_gencfg baxter_core_msgs_generate_messages_py)
28 | 
29 | catkin_package(
30 |   CATKIN_DEPENDS
31 |   rospy
32 |   actionlib
33 |   sensor_msgs
34 |   std_msgs
35 |   control_msgs
36 |   trajectory_msgs
37 |   dynamic_reconfigure
38 |   baxter_core_msgs
39 | )
40 | 
41 | install(
42 |   DIRECTORY scripts/
43 |   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
44 |   USE_SOURCE_PERMISSIONS
45 | )
46 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2013-2015, Rethink Robotics
 2 | All rights reserved.
 3 | 
 4 | Redistribution and use in source and binary forms, with or without
 5 | modification, are permitted provided that the following conditions are met:
 6 | 
 7 | 1. Redistributions of source code must retain the above copyright notice,
 8 |    this list of conditions and the following disclaimer.
 9 | 2. Redistributions in binary form must reproduce the above copyright
10 |    notice, this list of conditions and the following disclaimer in the
11 |    documentation and/or other materials provided with the distribution.
12 | 3. Neither the name of the Rethink Robotics nor the names of its
13 |    contributors may be used to endorse or promote products derived from
14 |    this software without specific prior written permission.
15 | 
16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | POSSIBILITY OF SUCH DAMAGE.
27 | 


--------------------------------------------------------------------------------
/README.rst:
--------------------------------------------------------------------------------
 1 | baxter_interface
 2 | ================
 3 | 
 4 | Python interface classes and action servers for control of
 5 | the Baxter Research Robot from Rethink Robotics
 6 | 
 7 | Code & Tickets
 8 | --------------
 9 | 
10 | +-----------------+----------------------------------------------------------------+
11 | | Documentation   | http://sdk.rethinkrobotics.com/wiki                            |
12 | +-----------------+----------------------------------------------------------------+
13 | | Issues          | https://github.com/RethinkRobotics/baxter_interface/issues     |
14 | +-----------------+----------------------------------------------------------------+
15 | | Contributions   | http://sdk.rethinkrobotics.com/wiki/Contributions              |
16 | +-----------------+----------------------------------------------------------------+
17 | 
18 | baxter_interface Repository Overview
19 | ------------------------------------
20 | 
21 | ::
22 | 
23 |      .
24 |      |
25 |      +-- src/                                  baxter_interface api
26 |      |   +-- baxter_interface/                 baxter component classes
27 |      |       +-- analog_io.py
28 |      |       +-- camera.py
29 |      |       +-- digital_io.py
30 |      |       +-- gripper.py
31 |      |       +-- head.py
32 |      |       +-- limb.py
33 |      |       +-- navigator.py
34 |      |       +-- robot_enable.py
35 |      |       +-- robust_controller.py
36 |      |       +-- settings.py
37 |      |   +-- baxter_control/                   generic control utilities
38 |      |   +-- baxter_dataflow/                  timing/program flow utilities
39 |      |   +-- joint_trajectory_action/          joint trajectory action implementation
40 |      |   +-- gripper_action/                   gripper action implementation
41 |      |   +-- head_action/                      head action implementation
42 |      |
43 |      +-- scripts/                              action server executables
44 |      |   +-- joint_trajectory_action_server.py
45 |      |   +-- gripper_action_server.py
46 |      |   +-- head_action_server.py
47 |      |
48 |      +-- cfg/                                  dynamic reconfigure action configs
49 | 
50 | 
51 | Other Baxter Repositories
52 | -------------------------
53 | 
54 | +------------------+-----------------------------------------------------+
55 | | baxter           | https://github.com/RethinkRobotics/baxter           |
56 | +------------------+-----------------------------------------------------+
57 | | baxter_tools     | https://github.com/RethinkRobotics/baxter_tools     |
58 | +------------------+-----------------------------------------------------+
59 | | baxter_examples  | https://github.com/RethinkRobotics/baxter_examples  |
60 | +------------------+-----------------------------------------------------+
61 | | baxter_common    | https://github.com/RethinkRobotics/baxter_common    |
62 | +------------------+-----------------------------------------------------+
63 | 
64 | Latest Release Information
65 | --------------------------
66 | 
67 | http://sdk.rethinkrobotics.com/wiki/Release-Changes
68 | 


--------------------------------------------------------------------------------
/cfg/GripperActionServer.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # Copyright (c) 2013-2015, Rethink Robotics
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions are met:
 8 | #
 9 | # 1. Redistributions of source code must retain the above copyright notice,
10 | #    this list of conditions and the following disclaimer.
11 | # 2. Redistributions in binary form must reproduce the above copyright
12 | #    notice, this list of conditions and the following disclaimer in the
13 | #    documentation and/or other materials provided with the distribution.
14 | # 3. Neither the name of the Rethink Robotics nor the names of its
15 | #    contributors may be used to endorse or promote products derived from
16 | #    this software without specific prior written permission.
17 | #
18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | # POSSIBILITY OF SUCH DAMAGE.
29 | 
30 | from dynamic_reconfigure.parameter_generator_catkin import (
31 |     ParameterGenerator,
32 |     double_t,
33 | )
34 | 
35 | gen = ParameterGenerator()
36 | 
37 | grippers = ('left_gripper', 'right_gripper')
38 | 
39 | params = (
40 |     '_timeout', '_goal', '_velocity', '_moving_force', '_holding_force',
41 |     '_vacuum_threshold', '_blow_off',
42 |     )
43 | 
44 | msg = (
45 |     " - Timeout (Seconds) to achieve command or determined gripping",
46 |     " - Electric Gripper - Maximum final error",
47 |     " - Electric Gripper - Velocity",
48 |     " - Electric Gripper - Force threshold. Grip achieved when surpassed.",
49 |     " - Electric Gripper - Holding force applied when gripping/after motion.",
50 |     " - Suction Gripper - Vacuum threshold. Grip achieved when surpassed.",
51 |     " - Suction Gripper - When suction ceased, seconds of blown air.",
52 |     )
53 | min = (-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
54 | default = (5.0, 5.0, 50.0, 40.0, 30.0, 18.0, 0.0)
55 | max = (20.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0)
56 | 
57 | for gripper in grippers:
58 |     for idx, param in enumerate(params):
59 |         gen.add(
60 |             gripper + param, double_t, 0, gripper + msg[idx],
61 |             default[idx], min[idx], max[idx]
62 |             )
63 | 
64 | exit(gen.generate('baxter_interface', '', 'GripperActionServer'))
65 | 


--------------------------------------------------------------------------------
/cfg/HeadActionServer.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # Copyright (c) 2013-2015, Rethink Robotics
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions are met:
 8 | #
 9 | # 1. Redistributions of source code must retain the above copyright notice,
10 | #    this list of conditions and the following disclaimer.
11 | # 2. Redistributions in binary form must reproduce the above copyright
12 | #    notice, this list of conditions and the following disclaimer in the
13 | #    documentation and/or other materials provided with the distribution.
14 | # 3. Neither the name of the Rethink Robotics nor the names of its
15 | #    contributors may be used to endorse or promote products derived from
16 | #    this software without specific prior written permission.
17 | #
18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | # POSSIBILITY OF SUCH DAMAGE.
29 | 
30 | from dynamic_reconfigure.parameter_generator_catkin import (
31 |     ParameterGenerator,
32 |     double_t,
33 | )
34 | 
35 | from baxter_interface import settings
36 | 
37 | gen = ParameterGenerator()
38 | 
39 | params = (
40 |     'timeout', 'goal'
41 |     )
42 | 
43 | msg = (
44 |     " - Timeout (Seconds) to achieve command",
45 |     " - Maximum final error",
46 |     )
47 | min = (-1.0, 0.0)
48 | default = (5.0, settings.HEAD_PAN_ANGLE_TOLERANCE)
49 | max = (20.0, 1.57)
50 | 
51 | for idx, param in enumerate(params):
52 |     gen.add(
53 |         param, double_t, 0, msg[idx],
54 |         default[idx], min[idx], max[idx]
55 |         )
56 | 
57 | exit(gen.generate('baxter_interface', '', 'HeadActionServer'))
58 | 


--------------------------------------------------------------------------------
/cfg/PositionFFJointTrajectoryActionServer.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # Copyright (c) 2013-2015, Rethink Robotics
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions are met:
 8 | #
 9 | # 1. Redistributions of source code must retain the above copyright notice,
10 | #    this list of conditions and the following disclaimer.
11 | # 2. Redistributions in binary form must reproduce the above copyright
12 | #    notice, this list of conditions and the following disclaimer in the
13 | #    documentation and/or other materials provided with the distribution.
14 | # 3. Neither the name of the Rethink Robotics nor the names of its
15 | #    contributors may be used to endorse or promote products derived from
16 | #    this software without specific prior written permission.
17 | #
18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | # POSSIBILITY OF SUCH DAMAGE.
29 | 
30 | from dynamic_reconfigure.parameter_generator_catkin import (
31 |     ParameterGenerator,
32 |     double_t,
33 | )
34 | 
35 | gen = ParameterGenerator()
36 | 
37 | gen.add(
38 |     'goal_time', double_t, 0,
39 |     "Amount of time (s) controller is permitted to be late achieving goal",
40 |     0.1, 0.0, 120.0,
41 | )
42 | gen.add(
43 |     'stopped_velocity_tolerance', double_t, 0,
44 |     "Maximum velocity (m/s) at end of trajectory to be considered stopped",
45 |     0.20, -1.0, 1.0,
46 | )
47 | 
48 | joints = (
49 |     'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
50 |     'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
51 |     'right_w1', 'right_w2',
52 |     )
53 | 
54 | params = ('_goal', '_trajectory',)
55 | msg = (
56 |     " - maximum final error",
57 |     " - maximum error during trajectory execution",
58 |     )
59 | min = (-1.0, -1.0,)
60 | default = (-1.0, 0.35,)
61 | max = (1.0, 1.0,)
62 | 
63 | for idx, param in enumerate(params):
64 |     for joint in joints:
65 |         gen.add(
66 |             joint + param, double_t, 0, joint + msg[idx],
67 |             default[idx], min[idx], max[idx]
68 |         )
69 | 
70 | exit(gen.generate('baxter_interface', '', 'PositionFFJointTrajectoryActionServer'))
71 | 


--------------------------------------------------------------------------------
/cfg/PositionJointTrajectoryActionServer.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # Copyright (c) 2013-2015, Rethink Robotics
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions are met:
 8 | #
 9 | # 1. Redistributions of source code must retain the above copyright notice,
10 | #    this list of conditions and the following disclaimer.
11 | # 2. Redistributions in binary form must reproduce the above copyright
12 | #    notice, this list of conditions and the following disclaimer in the
13 | #    documentation and/or other materials provided with the distribution.
14 | # 3. Neither the name of the Rethink Robotics nor the names of its
15 | #    contributors may be used to endorse or promote products derived from
16 | #    this software without specific prior written permission.
17 | #
18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | # POSSIBILITY OF SUCH DAMAGE.
29 | 
30 | from dynamic_reconfigure.parameter_generator_catkin import (
31 |     ParameterGenerator,
32 |     double_t,
33 | )
34 | 
35 | gen = ParameterGenerator()
36 | 
37 | gen.add(
38 |     'goal_time', double_t, 0,
39 |     "Amount of time (s) controller is permitted to be late achieving goal",
40 |     0.1, 0.0, 120.0,
41 | )
42 | gen.add(
43 |     'stopped_velocity_tolerance', double_t, 0,
44 |     "Maximum velocity (m/s) at end of trajectory to be considered stopped",
45 |     0.25, -1.0, 1.0,
46 | )
47 | 
48 | joints = (
49 |     'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
50 |     'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
51 |     'right_w1', 'right_w2',
52 |     )
53 | 
54 | params = ('_goal', '_trajectory',)
55 | msg = (
56 |     " - maximum final error",
57 |     " - maximum error during trajectory execution",
58 |     )
59 | min = (-1.0, -1.0,)
60 | default = (-1.0, 0.2,)
61 | max = (1.0, 1.0,)
62 | 
63 | for idx, param in enumerate(params):
64 |     for joint in joints:
65 |         gen.add(
66 |             joint + param, double_t, 0, joint + msg[idx],
67 |             default[idx], min[idx], max[idx]
68 |         )
69 | 
70 | exit(gen.generate('baxter_interface', '', 'PositionJointTrajectoryActionServer'))
71 | 


--------------------------------------------------------------------------------
/cfg/VelocityJointTrajectoryActionServer.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # Copyright (c) 2013-2015, Rethink Robotics
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions are met:
 8 | #
 9 | # 1. Redistributions of source code must retain the above copyright notice,
10 | #    this list of conditions and the following disclaimer.
11 | # 2. Redistributions in binary form must reproduce the above copyright
12 | #    notice, this list of conditions and the following disclaimer in the
13 | #    documentation and/or other materials provided with the distribution.
14 | # 3. Neither the name of the Rethink Robotics nor the names of its
15 | #    contributors may be used to endorse or promote products derived from
16 | #    this software without specific prior written permission.
17 | #
18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | # POSSIBILITY OF SUCH DAMAGE.
29 | 
30 | from dynamic_reconfigure.parameter_generator_catkin import (
31 |     ParameterGenerator,
32 |     double_t,
33 | )
34 | 
35 | gen = ParameterGenerator()
36 | 
37 | gen.add(
38 |     'goal_time', double_t, 0,
39 |     "Amount of time (s) controller is permitted to be late achieving goal",
40 |     0.0, 0.0, 120.0,
41 | )
42 | 
43 | gen.add(
44 |     'stopped_velocity_tolerance', double_t, 0,
45 |     "Maximum velocity (m/s) at end of trajectory to be considered stopped",
46 |     -1.0, -1.0, 1.0,
47 | )
48 | 
49 | joints = (
50 |     'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
51 |     'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
52 |     'right_w1', 'right_w2',
53 |     )
54 | 
55 | params = ('_goal', '_trajectory', '_kp', '_ki', '_kd',)
56 | msg = (
57 |     " - maximum final error",
58 |     " - maximum error during trajectory execution",
59 |     " - Kp proportional control gain",
60 |     " - Ki integral control gain",
61 |     " - Kd derivative control gain",
62 |     )
63 | min = (-1.0, -1.0, 0.0, 0.0, 0.0,)
64 | default = (-1.0, -1.0, 2.0, 0.0, 0.0,)
65 | max = (3.0, 3.0, 500.0, 100.0, 100.0,)
66 | 
67 | for idx, param in enumerate(params):
68 |     if idx < 2:
69 |         for joint in joints:
70 |             gen.add(
71 |                 joint + param, double_t, 0, joint + msg[idx],
72 |                 default[idx], min[idx], max[idx]
73 |             )
74 | for joint in joints:
75 |     for idx, param in enumerate(params):
76 |         if idx >= 2:
77 |             gen.add(
78 |                 joint + param, double_t, 0, joint + msg[idx],
79 |                 default[idx], min[idx], max[idx]
80 |             )
81 | 
82 | exit(gen.generate('baxter_interface', '', 'VelocityJointTrajectoryActionServer'))
83 | 


--------------------------------------------------------------------------------
/epydoc.config:
--------------------------------------------------------------------------------
1 | [epydoc]
2 | inheritance: included
3 | url: http://sdk.rethinkrobotics.com/wiki/
4 | imports: yes
5 | modules: baxter_interface, baxter_dataflow, baxter_control
6 | top: baxter_interface
7 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>baxter_interface</name>
 4 |   <version>1.2.0</version>
 5 |   <description>
 6 |     Convenient python interface classes for control
 7 |     of the Baxter Research Robot from Rethink Robotics.
 8 |   </description>
 9 | 
10 |   <maintainer email="rsdk.support@rethinkrobotics.com">
11 |     Rethink Robotics Inc.
12 |   </maintainer>
13 |   <license>BSD</license>
14 |   <url type="website">http://sdk.rethinkrobotics.com</url>
15 |   <url type="repository">
16 |     https://github.com/RethinkRobotics/baxter_interface
17 |   </url>
18 |   <url type="bugtracker">
19 |     https://github.com/RethinkRobotics/baxter_interface/issues
20 |   </url>
21 |   <author>Rethink Robotics Inc.</author>
22 | 
23 |   <buildtool_depend>catkin</buildtool_depend>
24 | 
25 |   <build_depend>rospy</build_depend>
26 |   <build_depend>actionlib</build_depend>
27 |   <build_depend>baxter_core_msgs</build_depend>
28 |   <build_depend>control_msgs</build_depend>
29 |   <build_depend>dynamic_reconfigure</build_depend>
30 |   <build_depend>sensor_msgs</build_depend>
31 |   <build_depend>std_msgs</build_depend>
32 |   <build_depend>trajectory_msgs</build_depend>
33 | 
34 |   <run_depend>rospy</run_depend>
35 |   <run_depend>actionlib</run_depend>
36 |   <run_depend>baxter_core_msgs</run_depend>
37 |   <run_depend>control_msgs</run_depend>
38 |   <run_depend>dynamic_reconfigure</run_depend>
39 |   <run_depend>sensor_msgs</run_depend>
40 |   <run_depend>std_msgs</run_depend>
41 |   <run_depend>trajectory_msgs</run_depend>
42 | 
43 |   <export>
44 |     <rosdoc config="rosdoc.yaml" />
45 |   </export>
46 | 
47 | </package>
48 | 


--------------------------------------------------------------------------------
/rosdoc.yaml:
--------------------------------------------------------------------------------
1 |  - builder: epydoc
2 |    name: Baxter SDK Python API
3 |    config: epydoc.config
4 | 


--------------------------------------------------------------------------------
/scripts/gripper_action_server.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # Copyright (c) 2013-2015, Rethink Robotics
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions are met:
 8 | #
 9 | # 1. Redistributions of source code must retain the above copyright notice,
10 | #    this list of conditions and the following disclaimer.
11 | # 2. Redistributions in binary form must reproduce the above copyright
12 | #    notice, this list of conditions and the following disclaimer in the
13 | #    documentation and/or other materials provided with the distribution.
14 | # 3. Neither the name of the Rethink Robotics nor the names of its
15 | #    contributors may be used to endorse or promote products derived from
16 | #    this software without specific prior written permission.
17 | #
18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | # POSSIBILITY OF SUCH DAMAGE.
29 | 
30 | """
31 | Baxter RSDK Gripper Action Server
32 | """
33 | import argparse
34 | 
35 | import rospy
36 | 
37 | from dynamic_reconfigure.server import Server
38 | 
39 | from baxter_interface.cfg import (
40 |     GripperActionServerConfig
41 | )
42 | from gripper_action.gripper_action import (
43 |     GripperActionServer,
44 | )
45 | 
46 | 
47 | def start_server(gripper):
48 |     print("Initializing node... ")
49 |     rospy.init_node("rsdk_gripper_action_server%s" %
50 |                     ("" if gripper == 'both' else "_" + gripper,))
51 |     print("Initializing gripper action server...")
52 | 
53 |     dynamic_cfg_srv = Server(GripperActionServerConfig,
54 |                              lambda config, level: config)
55 | 
56 |     if gripper == 'both':
57 |         GripperActionServer('right', dynamic_cfg_srv)
58 |         GripperActionServer('left', dynamic_cfg_srv)
59 |     else:
60 |         GripperActionServer(gripper, dynamic_cfg_srv)
61 |     print("Running. Ctrl-c to quit")
62 |     rospy.spin()
63 | 
64 | 
65 | def main():
66 |     arg_fmt = argparse.ArgumentDefaultsHelpFormatter
67 |     parser = argparse.ArgumentParser(formatter_class=arg_fmt)
68 |     parser.add_argument("-g", "--gripper", dest="gripper", default="both",
69 |                         choices=['both', 'left', 'right'],
70 |                         help="gripper action server limb",)
71 |     args = parser.parse_args(rospy.myargv()[1:])
72 |     start_server(args.gripper)
73 | 
74 | 
75 | if __name__ == "__main__":
76 |     main()
77 | 


--------------------------------------------------------------------------------
/scripts/head_action_server.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | # Copyright (c) 2013-2015, Rethink Robotics
 4 | # All rights reserved.
 5 | #
 6 | # Redistribution and use in source and binary forms, with or without
 7 | # modification, are permitted provided that the following conditions are met:
 8 | #
 9 | # 1. Redistributions of source code must retain the above copyright notice,
10 | #    this list of conditions and the following disclaimer.
11 | # 2. Redistributions in binary form must reproduce the above copyright
12 | #    notice, this list of conditions and the following disclaimer in the
13 | #    documentation and/or other materials provided with the distribution.
14 | # 3. Neither the name of the Rethink Robotics nor the names of its
15 | #    contributors may be used to endorse or promote products derived from
16 | #    this software without specific prior written permission.
17 | #
18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | # POSSIBILITY OF SUCH DAMAGE.
29 | 
30 | """
31 | Baxter RSDK Head Action Server
32 | """
33 | import argparse
34 | 
35 | import rospy
36 | 
37 | from dynamic_reconfigure.server import Server
38 | 
39 | from baxter_interface.cfg import (
40 |     HeadActionServerConfig
41 | )
42 | from head_action.head_action import (
43 |     HeadActionServer,
44 | )
45 | 
46 | 
47 | def start_server():
48 |     print("Initializing node... ")
49 |     rospy.init_node("rsdk_head_action_server")
50 |     print("Initializing head action server...")
51 | 
52 |     dynamic_cfg_srv = Server(HeadActionServerConfig,
53 |                              lambda config, level: config)
54 | 
55 |     HeadActionServer(dynamic_cfg_srv)
56 |     print("Running. Ctrl-c to quit")
57 |     rospy.spin()
58 | 
59 | def main():
60 |     start_server()
61 | 
62 | if __name__ == "__main__":
63 |     main()
64 | 


--------------------------------------------------------------------------------
/scripts/joint_trajectory_action_server.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python
  2 | 
  3 | # Copyright (c) 2013-2015, Rethink Robotics
  4 | # All rights reserved.
  5 | #
  6 | # Redistribution and use in source and binary forms, with or without
  7 | # modification, are permitted provided that the following conditions are met:
  8 | #
  9 | # 1. Redistributions of source code must retain the above copyright notice,
 10 | #    this list of conditions and the following disclaimer.
 11 | # 2. Redistributions in binary form must reproduce the above copyright
 12 | #    notice, this list of conditions and the following disclaimer in the
 13 | #    documentation and/or other materials provided with the distribution.
 14 | # 3. Neither the name of the Rethink Robotics nor the names of its
 15 | #    contributors may be used to endorse or promote products derived from
 16 | #    this software without specific prior written permission.
 17 | #
 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 28 | # POSSIBILITY OF SUCH DAMAGE.
 29 | 
 30 | """
 31 | Baxter RSDK Joint Trajectory Controller
 32 |     Unlike other robots running ROS, this is not a Motor Controller plugin,
 33 |     but a regular node using the SDK interface.
 34 | """
 35 | import argparse
 36 | 
 37 | import rospy
 38 | 
 39 | from dynamic_reconfigure.server import Server
 40 | 
 41 | from baxter_interface.cfg import (
 42 |     PositionJointTrajectoryActionServerConfig,
 43 |     VelocityJointTrajectoryActionServerConfig,
 44 |     PositionFFJointTrajectoryActionServerConfig,
 45 | )
 46 | from joint_trajectory_action.joint_trajectory_action import (
 47 |     JointTrajectoryActionServer,
 48 | )
 49 | 
 50 | from trajectory_msgs.msg import (
 51 |     JointTrajectoryPoint,
 52 | )
 53 | 
 54 | 
 55 | def start_server(limb, rate, mode):
 56 |     print("Initializing node... ")
 57 |     rospy.init_node("rsdk_%s_joint_trajectory_action_server%s" %
 58 |                     (mode, "" if limb == 'both' else "_" + limb,))
 59 |     print("Initializing joint trajectory action server...")
 60 | 
 61 |     if mode == 'velocity':
 62 |         dyn_cfg_srv = Server(VelocityJointTrajectoryActionServerConfig,
 63 |                              lambda config, level: config)
 64 |     elif mode == 'position':
 65 |         dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig,
 66 |                              lambda config, level: config)
 67 |     else:
 68 |         dyn_cfg_srv = Server(PositionFFJointTrajectoryActionServerConfig,
 69 |                              lambda config, level: config)
 70 |     jtas = []
 71 |     if limb == 'both':
 72 |         jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv,
 73 |                                                 rate, mode))
 74 |         jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv,
 75 |                                                 rate, mode))
 76 |     else:
 77 |         jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
 78 | 
 79 |     def cleanup():
 80 |         for j in jtas:
 81 |             j.clean_shutdown()
 82 | 
 83 |     rospy.on_shutdown(cleanup)
 84 |     print("Running. Ctrl-c to quit")
 85 |     rospy.spin()
 86 | 
 87 | 
 88 | def main():
 89 |     arg_fmt = argparse.ArgumentDefaultsHelpFormatter
 90 |     parser = argparse.ArgumentParser(formatter_class=arg_fmt)
 91 |     parser.add_argument(
 92 |         "-l", "--limb", dest="limb", default="both",
 93 |         choices=['both', 'left', 'right'],
 94 |         help="joint trajectory action server limb"
 95 |     )
 96 |     parser.add_argument(
 97 |         "-r", "--rate", dest="rate", default=100.0,
 98 |         type=float, help="trajectory control rate (Hz)"
 99 |     )
100 |     parser.add_argument(
101 |         "-m", "--mode", default='position_w_id',
102 |         choices=['position_w_id', 'position', 'velocity'],
103 |         help="control mode for trajectory execution"
104 |     )
105 |     args = parser.parse_args(rospy.myargv()[1:])
106 |     start_server(args.limb, args.rate, args.mode)
107 | 
108 | 
109 | if __name__ == "__main__":
110 |     main()
111 | 


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | from distutils.core import setup
 3 | from catkin_pkg.python_setup import generate_distutils_setup
 4 | 
 5 | d = generate_distutils_setup()
 6 | d['packages'] = ['baxter_interface', 'baxter_control', 'baxter_dataflow',
 7 |                  'joint_trajectory_action', 'gripper_action', 'head_action']
 8 | d['package_dir'] = {'': 'src'}
 9 | 
10 | setup(**d)
11 | 


--------------------------------------------------------------------------------
/src/baxter_control/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | from .pid import PID
29 | 


--------------------------------------------------------------------------------
/src/baxter_control/pid.py:
--------------------------------------------------------------------------------
 1 | # Modified example source code from the book
 2 | # "Real-World Instrumentation with Python"
 3 | # by J. M. Hughes, published by O'Reilly Media, December 2010,
 4 | # ISBN 978-0-596-80956-0.
 5 | 
 6 | import rospy
 7 | 
 8 | 
 9 | class PID(object):
10 |     """
11 |     PID control class
12 | 
13 |     This class implements a simplistic PID control algorithm. When first
14 |     instantiated all the gain variables are set to zero, so calling
15 |     the method compute_output will just return zero.
16 |     """
17 | 
18 |     def __init__(self, kp=0.0, ki=0.0, kd=0.0):
19 |         # initialize gains
20 |         self._kp = kp
21 |         self._ki = ki
22 |         self._kd = kd
23 | 
24 |         # initialize error, results, and time descriptors
25 |         self._prev_err = 0.0
26 |         self._cp = 0.0
27 |         self._ci = 0.0
28 |         self._cd = 0.0
29 |         self._cur_time = 0.0
30 |         self._prev_time = 0.0
31 | 
32 |         self.initialize()
33 | 
34 |     def initialize(self):
35 |         """
36 |         Initialize pid controller.
37 |         """
38 |         # reset delta t variables
39 |         self._cur_time = rospy.get_time()
40 |         self._prev_time = self._cur_time
41 | 
42 |         self._prev_err = 0.0
43 | 
44 |         # reset result variables
45 |         self._cp = 0.0
46 |         self._ci = 0.0
47 |         self._cd = 0.0
48 | 
49 |     def set_kp(self, invar):
50 |         """
51 |         Set proportional gain.
52 |         """
53 |         self._kp = invar
54 | 
55 |     def set_ki(self, invar):
56 |         """
57 |         Set integral gain.
58 |         """
59 |         self._ki = invar
60 | 
61 |     def set_kd(self, invar):
62 |         """
63 |         Set derivative gain.
64 |         """
65 |         self._kd = invar
66 | 
67 |     def compute_output(self, error):
68 |         """
69 |         Performs a PID computation and returns a control value based on
70 |         the elapsed time (dt) and the error signal from a summing junction
71 |         (the error parameter).
72 |         """
73 |         self._cur_time = rospy.get_time()  # get t
74 |         dt = self._cur_time - self._prev_time  # get delta t
75 |         de = error - self._prev_err  # get delta error
76 | 
77 |         self._cp = error  # proportional term
78 |         self._ci += error * dt  # integral term
79 | 
80 |         self._cd = 0
81 |         if dt > 0:  # no div by zero
82 |             self._cd = de / dt  # derivative term
83 | 
84 |         self._prev_time = self._cur_time  # save t for next pass
85 |         self._prev_err = error  # save t-1 error
86 | 
87 |         # sum the terms and return the result
88 |         return ((self._kp * self._cp) + (self._ki * self._ci) +
89 |                 (self._kd * self._cd))
90 | 


--------------------------------------------------------------------------------
/src/baxter_dataflow/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | from .wait_for import wait_for
29 | from .signals import Signal
30 | 


--------------------------------------------------------------------------------
/src/baxter_dataflow/signals.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | import inspect
29 | from weakref import WeakKeyDictionary
30 | 
31 | try:
32 |     from weakref import WeakSet
33 | except ImportError:
34 |     from weakrefset import WeakSet
35 | 
36 | 
37 | class Signal(object):
38 |     def __init__(self):
39 |         self._functions = WeakSet()
40 |         self._methods = WeakKeyDictionary()
41 | 
42 |     def __call__(self, *args, **kargs):
43 |         for f in self._functions:
44 |             f(*args, **kargs)
45 | 
46 |         for obj, functions in self._methods.items():
47 |             for f in functions:
48 |                 f(obj, *args, **kargs)
49 | 
50 |     def connect(self, slot):
51 |         if inspect.ismethod(slot):
52 |             if not slot.__self__ in self._methods:
53 |                 self._methods[slot.__self__] = set()
54 |             self._methods[slot.__self__].add(slot.__func__)
55 |         else:
56 |             self._functions.add(slot)
57 | 
58 |     def disconnect(self, slot):
59 |         if inspect.ismethod(slot):
60 |             if slot.__self__ in self._methods:
61 |                 self._methods[slot.__self__].remove(slot.__func__)
62 |         else:
63 |             if slot in self._functions:
64 |                 self._functions.remove(slot)
65 | 


--------------------------------------------------------------------------------
/src/baxter_dataflow/wait_for.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | import errno
29 | 
30 | import rospy
31 | 
32 | 
33 | def wait_for(test, timeout=1.0, raise_on_error=True, rate=100,
34 |              timeout_msg="timeout expired", body=None):
35 |     """
36 |     Waits until some condition evaluates to true.
37 | 
38 |     @param test: zero param function to be evaluated
39 |     @param timeout: max amount of time to wait. negative/inf for indefinitely
40 |     @param raise_on_error: raise or just return False
41 |     @param rate: the rate at which to check
42 |     @param timout_msg: message to supply to the timeout exception
43 |     @param body: optional function to execute while waiting
44 |     """
45 |     end_time = rospy.get_time() + timeout
46 |     rate = rospy.Rate(rate)
47 |     notimeout = (timeout < 0.0) or timeout == float("inf")
48 |     while not test():
49 |         if rospy.is_shutdown():
50 |             if raise_on_error:
51 |                 raise OSError(errno.ESHUTDOWN, "ROS Shutdown")
52 |             return False
53 |         elif (not notimeout) and (rospy.get_time() >= end_time):
54 |             if raise_on_error:
55 |                 raise OSError(errno.ETIMEDOUT, timeout_msg)
56 |             return False
57 |         if callable(body):
58 |             body()
59 |         rate.sleep()
60 |     return True
61 | 


--------------------------------------------------------------------------------
/src/baxter_dataflow/weakrefset.py:
--------------------------------------------------------------------------------
  1 | # Copyright (C) 2010 Michael Foord
  2 | # E-mail: fuzzyman AT voidspace DOT org DOT uk
  3 | 
  4 | # This software is licensed under the terms of the BSD license.
  5 | # http://www.voidspace.org.uk/python/license.shtml
  6 | # https://pypi.python.org/pypi/weakrefset
  7 | 
  8 | from __future__ import with_statement
  9 | 
 10 | from _weakref import ref
 11 | 
 12 | __all__ = ['WeakSet']
 13 | 
 14 | __version__ = '1.0.0'
 15 | 
 16 | 
 17 | class _IterationGuard(object):
 18 |     # This context manager registers itself in the current iterators of the
 19 |     # weak container, such as to delay all removals until the context manager
 20 |     # exits.
 21 |     # This technique should be relatively thread-safe (since sets are).
 22 | 
 23 |     def __init__(self, weakcontainer):
 24 |         # Don't create cycles
 25 |         self.weakcontainer = ref(weakcontainer)
 26 | 
 27 |     def __enter__(self):
 28 |         w = self.weakcontainer()
 29 |         if w is not None:
 30 |             w._iterating.add(self)
 31 |         return self
 32 | 
 33 |     def __exit__(self, e, t, b):
 34 |         w = self.weakcontainer()
 35 |         if w is not None:
 36 |             s = w._iterating
 37 |             s.remove(self)
 38 |             if not s:
 39 |                 w._commit_removals()
 40 | 
 41 | 
 42 | class WeakSet(object):
 43 |     def __init__(self, data=None):
 44 |         self.data = set()
 45 | 
 46 |         def _remove(item, selfref=ref(self)):
 47 |             self = selfref()
 48 |             if self is not None:
 49 |                 if self._iterating:
 50 |                     self._pending_removals.append(item)
 51 |                 else:
 52 |                     self.data.discard(item)
 53 |         self._remove = _remove
 54 |         # A list of keys to be removed
 55 |         self._pending_removals = []
 56 |         self._iterating = set()
 57 |         if data is not None:
 58 |             self.update(data)
 59 | 
 60 |     def _commit_removals(self):
 61 |         l = self._pending_removals
 62 |         discard = self.data.discard
 63 |         while l:
 64 |             discard(l.pop())
 65 | 
 66 |     def __iter__(self):
 67 |         with _IterationGuard(self):
 68 |             for itemref in self.data:
 69 |                 item = itemref()
 70 |                 if item is not None:
 71 |                     yield item
 72 | 
 73 |     def __len__(self):
 74 |         return sum(x() is not None for x in self.data)
 75 | 
 76 |     def __contains__(self, item):
 77 |         return ref(item) in self.data
 78 | 
 79 |     def __reduce__(self):
 80 |         return (self.__class__, (list(self),),
 81 |                 getattr(self, '__dict__', None))
 82 | 
 83 |     __hash__ = None
 84 | 
 85 |     def add(self, item):
 86 |         if self._pending_removals:
 87 |             self._commit_removals()
 88 |         self.data.add(ref(item, self._remove))
 89 | 
 90 |     def clear(self):
 91 |         if self._pending_removals:
 92 |             self._commit_removals()
 93 |         self.data.clear()
 94 | 
 95 |     def copy(self):
 96 |         return self.__class__(self)
 97 | 
 98 |     def pop(self):
 99 |         if self._pending_removals:
100 |             self._commit_removals()
101 |         while True:
102 |             try:
103 |                 itemref = self.data.pop()
104 |             except KeyError:
105 |                 raise KeyError('pop from empty WeakSet')
106 |             item = itemref()
107 |             if item is not None:
108 |                 return item
109 | 
110 |     def remove(self, item):
111 |         if self._pending_removals:
112 |             self._commit_removals()
113 |         self.data.remove(ref(item))
114 | 
115 |     def discard(self, item):
116 |         if self._pending_removals:
117 |             self._commit_removals()
118 |         self.data.discard(ref(item))
119 | 
120 |     def update(self, other):
121 |         if self._pending_removals:
122 |             self._commit_removals()
123 |         if isinstance(other, self.__class__):
124 |             self.data.update(other.data)
125 |         else:
126 |             for element in other:
127 |                 self.add(element)
128 | 
129 |     def __ior__(self, other):
130 |         self.update(other)
131 |         return self
132 | 
133 |     # Helper functions for simple delegating methods.
134 |     def _apply(self, other, method):
135 |         if not isinstance(other, self.__class__):
136 |             other = self.__class__(other)
137 |         newdata = method(other.data)
138 |         newset = self.__class__()
139 |         newset.data = newdata
140 |         return newset
141 | 
142 |     def difference(self, other):
143 |         return self._apply(other, self.data.difference)
144 |     __sub__ = difference
145 | 
146 |     def difference_update(self, other):
147 |         if self._pending_removals:
148 |             self._commit_removals()
149 |         if self is other:
150 |             self.data.clear()
151 |         else:
152 |             self.data.difference_update(ref(item) for item in other)
153 | 
154 |     def __isub__(self, other):
155 |         if self._pending_removals:
156 |             self._commit_removals()
157 |         if self is other:
158 |             self.data.clear()
159 |         else:
160 |             self.data.difference_update(ref(item) for item in other)
161 |         return self
162 | 
163 |     def intersection(self, other):
164 |         return self._apply(other, self.data.intersection)
165 |     __and__ = intersection
166 | 
167 |     def intersection_update(self, other):
168 |         if self._pending_removals:
169 |             self._commit_removals()
170 |         self.data.intersection_update(ref(item) for item in other)
171 | 
172 |     def __iand__(self, other):
173 |         if self._pending_removals:
174 |             self._commit_removals()
175 |         self.data.intersection_update(ref(item) for item in other)
176 |         return self
177 | 
178 |     def issubset(self, other):
179 |         return self.data.issubset(ref(item) for item in other)
180 |     __lt__ = issubset
181 | 
182 |     def __le__(self, other):
183 |         return self.data <= set(ref(item) for item in other)
184 | 
185 |     def issuperset(self, other):
186 |         return self.data.issuperset(ref(item) for item in other)
187 |     __gt__ = issuperset
188 | 
189 |     def __ge__(self, other):
190 |         return self.data >= set(ref(item) for item in other)
191 | 
192 |     def __eq__(self, other):
193 |         if not isinstance(other, self.__class__):
194 |             return NotImplemented
195 |         return self.data == set(ref(item) for item in other)
196 | 
197 |     def symmetric_difference(self, other):
198 |         return self._apply(other, self.data.symmetric_difference)
199 |     __xor__ = symmetric_difference
200 | 
201 |     def symmetric_difference_update(self, other):
202 |         if self._pending_removals:
203 |             self._commit_removals()
204 |         if self is other:
205 |             self.data.clear()
206 |         else:
207 |             self.data.symmetric_difference_update(ref(item) for item in other)
208 | 
209 |     def __ixor__(self, other):
210 |         if self._pending_removals:
211 |             self._commit_removals()
212 |         if self is other:
213 |             self.data.clear()
214 |         else:
215 |             self.data.symmetric_difference_update(ref(item) for item in other)
216 |         return self
217 | 
218 |     def union(self, other):
219 |         return self._apply(other, self.data.union)
220 |     __or__ = union
221 | 
222 |     def isdisjoint(self, other):
223 |         return len(self.intersection(other)) == 0
224 | 


--------------------------------------------------------------------------------
/src/baxter_interface/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | from .analog_io import AnalogIO
29 | from .camera import CameraController
30 | from .digital_io import DigitalIO
31 | from .gripper import Gripper
32 | from .head import Head
33 | from .limb import Limb
34 | from .navigator import Navigator
35 | from .robot_enable import RobotEnable
36 | from .robust_controller import RobustController
37 | from .settings import (
38 |     JOINT_ANGLE_TOLERANCE,
39 |     HEAD_PAN_ANGLE_TOLERANCE,
40 |     SDK_VERSION,
41 |     CHECK_VERSION,
42 |     VERSIONS_SDK2ROBOT,
43 |     VERSIONS_SDK2GRIPPER,
44 | )
45 | 


--------------------------------------------------------------------------------
/src/baxter_interface/analog_io.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | import errno
 29 | 
 30 | import rospy
 31 | 
 32 | import baxter_dataflow
 33 | 
 34 | from baxter_core_msgs.msg import (
 35 |     AnalogIOState,
 36 |     AnalogOutputCommand,
 37 | )
 38 | 
 39 | 
 40 | class AnalogIO(object):
 41 |     """
 42 |     Interface class for a simple Analog Input and/or Output on the
 43 |     Baxter robot.
 44 | 
 45 |     Input
 46 |       - read input state
 47 |     Output
 48 |       - set new output state
 49 |       - read current output state
 50 |     """
 51 |     def __init__(self, component_id):
 52 |         """
 53 |         Constructor.
 54 | 
 55 |         @param component_id: unique id of analog component
 56 |         """
 57 |         self._id = component_id
 58 |         self._component_type = 'analog_io'
 59 |         self._is_output = False
 60 | 
 61 |         self._state = dict()
 62 | 
 63 |         type_ns = '/robot/' + self._component_type
 64 |         topic_base = type_ns + '/' + self._id
 65 | 
 66 |         self._sub_state = rospy.Subscriber(
 67 |             topic_base + '/state',
 68 |             AnalogIOState,
 69 |             self._on_io_state)
 70 | 
 71 |         baxter_dataflow.wait_for(
 72 |             lambda: len(self._state.keys()) != 0,
 73 |             timeout=2.0,
 74 |             timeout_msg="Failed to get current analog_io state from %s" \
 75 |             % (topic_base,),
 76 |             )
 77 | 
 78 |         # check if output-capable before creating publisher
 79 |         if self._is_output:
 80 |             self._pub_output = rospy.Publisher(
 81 |                 type_ns + '/command',
 82 |                 AnalogOutputCommand,
 83 |                 queue_size=10)
 84 | 
 85 |     def _on_io_state(self, msg):
 86 |         """
 87 |         Updates the internally stored state of the Analog Input/Output.
 88 |         """
 89 |         self._is_output = not msg.isInputOnly
 90 |         self._state['value'] = msg.value
 91 | 
 92 |     def state(self):
 93 |         """
 94 |         Return the latest value of the Analog Input/Output.
 95 |         """
 96 |         return self._state['value']
 97 | 
 98 |     def is_output(self):
 99 |         """
100 |         Accessor to check if IO is capable of output.
101 |         """
102 |         return self._is_output
103 | 
104 |     def set_output(self, value, timeout=2.0):
105 |         """
106 |         Control the state of the Analog Output.
107 | 
108 |         @type value: uint16
109 |         @param value: new state of the Output.
110 |         @type timeout: float
111 |         @param timeout: Seconds to wait for the io to reflect command.
112 |                         If 0, just command once and return. [0]
113 |         """
114 |         if not self._is_output:
115 |             raise IOError(errno.EACCES, "Component is not an output [%s: %s]" %
116 |                 (self._component_type, self._id))
117 |         cmd = AnalogOutputCommand()
118 |         cmd.name = self._id
119 |         cmd.value = value
120 |         self._pub_output.publish(cmd)
121 | 
122 |         if not timeout == 0:
123 |             baxter_dataflow.wait_for(
124 |                 test=lambda: self.state() == value,
125 |                 timeout=timeout,
126 |                 rate=100,
127 |                 timeout_msg=("Failed to command analog io to: %d" % (value,)),
128 |                 body=lambda: self._pub_output.publish(cmd)
129 |                 )
130 | 


--------------------------------------------------------------------------------
/src/baxter_interface/camera.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | import errno
 29 | 
 30 | import rospy
 31 | 
 32 | from baxter_core_msgs.msg import (
 33 |     CameraControl,
 34 |     CameraSettings,
 35 | )
 36 | from baxter_core_msgs.srv import (
 37 |     CloseCamera,
 38 |     ListCameras,
 39 |     OpenCamera,
 40 | )
 41 | 
 42 | 
 43 | class CameraController(object):
 44 |     """
 45 |     Interface class for controlling camera settings on the Baxter robot.
 46 |     """
 47 | 
 48 |     # Valid resolutions
 49 |     MODES = [
 50 |              (1280, 800),
 51 |              (960, 600),
 52 |              (640, 400),
 53 |              (480, 300),
 54 |              (384, 240),
 55 |              (320, 200),
 56 |              ]
 57 | 
 58 |     # Used to represent when the camera is using automatic controls.
 59 |     # Valid for exposure, gain and white balance.
 60 |     CONTROL_AUTO = -1
 61 | 
 62 |     def __init__(self, name):
 63 |         """
 64 |         Constructor.
 65 | 
 66 |         @param name: camera identifier.  You can get a list of valid
 67 |                      identifiers by calling the ROS service /cameras/list.
 68 | 
 69 |                      Expected names are right_hand_camera, left_hand_camera
 70 |                      and head_camera.  However if the cameras are not
 71 |                      identified via the parameter server, they are simply
 72 |                      indexed starting at 0.
 73 |         """
 74 |         self._id = name
 75 | 
 76 |         list_svc = rospy.ServiceProxy('/cameras/list', ListCameras)
 77 |         rospy.wait_for_service('/cameras/list', timeout=10)
 78 |         if not self._id in list_svc().cameras:
 79 |             raise AttributeError(
 80 |                 ("Cannot locate a service for camera name '{0}'. "
 81 |                 "Close a different camera first and try again.".format(self._id)))
 82 | 
 83 |         self._open_svc = rospy.ServiceProxy('/cameras/open', OpenCamera)
 84 |         self._close_svc = rospy.ServiceProxy('/cameras/close', CloseCamera)
 85 | 
 86 |         self._settings = CameraSettings()
 87 |         self._settings.width = 320
 88 |         self._settings.height = 200
 89 |         self._settings.fps = 20
 90 |         self._open = False
 91 | 
 92 |     def _reload(self):
 93 |         self.open()
 94 | 
 95 |     def _get_value(self, control, default):
 96 |         lookup = [c.value for c in self._settings.controls if c.id == control]
 97 |         try:
 98 |             return lookup[0]
 99 |         except IndexError:
100 |             return default
101 | 
102 |     def _set_control_value(self, control, value):
103 |         lookup = [c for c in self._settings.controls if c.id == control]
104 |         try:
105 |             lookup[0].value = value
106 |         except IndexError:
107 |             self._settings.controls.append(CameraControl(control, value))
108 | 
109 |     @property
110 |     def resolution(self):
111 |         """
112 |         Camera resolution as a tuple.  (width, height).  Valid resolutions are
113 |         listed as tuples in CameraController.MODES
114 |         """
115 |         return (self._settings.width, self._settings.height)
116 | 
117 |     @resolution.setter
118 |     def resolution(self, res):
119 |         res = tuple(res)
120 |         if len(res) != 2:
121 |             raise AttributeError("Invalid resolution specified")
122 | 
123 |         if not res in self.MODES:
124 |             raise ValueError("Invalid camera mode %dx%d" % (res[0], res[1]))
125 | 
126 |         self._settings.width = res[0]
127 |         self._settings.height = res[1]
128 |         self._reload()
129 | 
130 |     @property
131 |     def fps(self):
132 |         """
133 |         Camera frames per second
134 |         """
135 |         return self._settings.fps
136 | 
137 |     @fps.setter
138 |     def fps(self, fps):
139 |         self._settings.fps = fps
140 |         self._reload()
141 | 
142 |     @property
143 |     def exposure(self):
144 |         """
145 |         Camera exposure.  If autoexposure is on, returns
146 |         CameraController.CONTROL_AUTO
147 |         """
148 |         return self._get_value(CameraControl.CAMERA_CONTROL_EXPOSURE,
149 |                                self.CONTROL_AUTO)
150 | 
151 |     @exposure.setter
152 |     def exposure(self, exposure):
153 |         """
154 |         Camera Exposure.  Valid range is 0-100 or CameraController.CONTROL_AUTO
155 |         """
156 |         exposure = int(exposure)
157 |         if (exposure < 0 or exposure > 100) and exposure != self.CONTROL_AUTO:
158 |             raise ValueError("Invalid exposure value")
159 | 
160 |         self._set_control_value(CameraControl.CAMERA_CONTROL_EXPOSURE,
161 |                                 exposure)
162 |         self._reload()
163 | 
164 |     @property
165 |     def gain(self):
166 |         """
167 |         Camera gain.  If autogain is on, returns CameraController.CONTROL_AUTO
168 |         """
169 |         return self._get_value(CameraControl.CAMERA_CONTROL_GAIN,
170 |                                self.CONTROL_AUTO)
171 | 
172 |     @gain.setter
173 |     def gain(self, gain):
174 |         """
175 |         Camera gain.  Range is 0-79 or CameraController.CONTROL_AUTO
176 |         """
177 |         gain = int(gain)
178 |         if (gain < 0 or gain > 79) and gain != self.CONTROL_AUTO:
179 |             raise ValueError("Invalid gain value")
180 | 
181 |         self._set_control_value(CameraControl.CAMERA_CONTROL_GAIN, gain)
182 |         self._reload()
183 | 
184 |     @property
185 |     def white_balance_red(self):
186 |         """
187 |         White balance red.  If autocontrol is on, returns
188 |         CameraController.CONTROL_AUTO
189 |         """
190 |         return self._get_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_R,
191 |                                self.CONTROL_AUTO)
192 | 
193 |     @white_balance_red.setter
194 |     def white_balance_red(self, value):
195 |         """
196 |         White balance red.  Range is 0-4095 or CameraController.CONTROL_AUTO
197 |         """
198 |         value = int(value)
199 |         if (value < 0 or value > 4095) and value != self.CONTROL_AUTO:
200 |             raise ValueError("Invalid white balance value")
201 | 
202 |         self._set_control_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_R,
203 |                                 value)
204 |         self._reload()
205 | 
206 |     @property
207 |     def white_balance_green(self):
208 |         """
209 |         White balance green.  If autocontrol is on, returns
210 |         CameraController.CONTROL_AUTO
211 |         """
212 |         return self._get_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_G,
213 |                                self.CONTROL_AUTO)
214 | 
215 |     @white_balance_green.setter
216 |     def white_balance_green(self, value):
217 |         """
218 |         White balance green.  Range is 0-4095 or CameraController.CONTROL_AUTO
219 |         """
220 |         value = int(value)
221 |         if (value < 0 or value > 4095) and value != self.CONTROL_AUTO:
222 |             raise ValueError("Invalid white balance value")
223 | 
224 |         self._set_control_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_G,
225 |                                 value)
226 |         self._reload()
227 | 
228 |     @property
229 |     def white_balance_blue(self):
230 |         """
231 |         White balance blue.  If autocontrol is on, returns
232 |         CameraController.CONTROL_AUTO
233 |         """
234 |         return self._get_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_B,
235 |                                self.CONTROL_AUTO)
236 | 
237 |     @white_balance_blue.setter
238 |     def white_balance_blue(self, value):
239 |         """
240 |         White balance blue.  Range is 0-4095 or CameraController.CONTROL_AUTO
241 |         """
242 |         value = int(value)
243 |         if (value < 0 or value > 4095) and value != self.CONTROL_AUTO:
244 |             raise ValueError("Invalid white balance value")
245 | 
246 |         self._set_control_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_B,
247 |                                 value)
248 |         self._reload()
249 | 
250 |     @property
251 |     def window(self):
252 |         """
253 |         Camera windowing, returns a tuple, (x, y)
254 |         """
255 |         x = self._get_value(CameraControl.CAMERA_CONTROL_WINDOW_X,
256 |                             self.CONTROL_AUTO)
257 |         if (x == self.CONTROL_AUTO):
258 |             return (tuple(map(lambda x: x / 2, self.resolution)) if
259 |             self.half_resolution else
260 |             self.resolution)
261 |         else:
262 |             return (x, self._get_value(CameraControl.CAMERA_CONTROL_WINDOW_Y,
263 |                                        self.CONTROL_AUTO))
264 | 
265 |     @window.setter
266 |     def window(self, win):
267 |         """
268 |         Set camera window.  The max size is a function of the current camera
269 |         resolution and if half_resolution is enabled or not.
270 |         """
271 |         x, y = tuple(win)
272 |         cur_x, cur_y = self.resolution
273 |         limit_x = 1280 - cur_x
274 |         limit_y = 800 - cur_y
275 | 
276 |         if self.half_resolution:
277 |             limit_x /= 2
278 |             limit_y /= 2
279 | 
280 |         if x < 0 or x > limit_x:
281 |             raise ValueError("Max X window is %d" % (limit_x,))
282 | 
283 |         if y < 0 or y > limit_y:
284 |             raise ValueError("Max Y window is %d" % (limit_y,))
285 | 
286 |         self._set_control_value(CameraControl.CAMERA_CONTROL_WINDOW_X, x)
287 |         self._set_control_value(CameraControl.CAMERA_CONTROL_WINDOW_Y, y)
288 |         self._reload()
289 | 
290 |     @property
291 |     def flip(self):
292 |         """
293 |         Camera flip. Returns True if flip is enabled on the camera.
294 |         """
295 |         return self._get_value(CameraControl.CAMERA_CONTROL_FLIP, False)
296 | 
297 |     @flip.setter
298 |     def flip(self, value):
299 |         self._set_control_value(CameraControl.CAMERA_CONTROL_FLIP,
300 |                                 int(value != 0))
301 |         self._reload()
302 | 
303 |     @property
304 |     def mirror(self):
305 |         """
306 |         Camera mirror. Returns True if mirror is enabled on the camera.
307 |         """
308 |         return self._get_value(CameraControl.CAMERA_CONTROL_MIRROR, False)
309 | 
310 |     @mirror.setter
311 |     def mirror(self, value):
312 |         self._set_control_value(CameraControl.CAMERA_CONTROL_MIRROR,
313 |                                 int(value != 0))
314 |         self._reload()
315 | 
316 |     @property
317 |     def half_resolution(self):
318 |         """
319 |         Return True if binning/half resolution is enabled on the camera.
320 |         """
321 |         return self._get_value(CameraControl.CAMERA_CONTROL_RESOLUTION_HALF,
322 |                                False)
323 | 
324 |     @half_resolution.setter
325 |     def half_resolution(self, value):
326 |         self._set_control_value(CameraControl.CAMERA_CONTROL_RESOLUTION_HALF,
327 |                                 int(value != 0))
328 |         self._reload()
329 | 
330 |     def open(self):
331 |         """
332 |         Open the camera currently under control.
333 |         """
334 |         if self._id == 'head_camera':
335 |             self._set_control_value(CameraControl.CAMERA_CONTROL_FLIP, True)
336 |             self._set_control_value(CameraControl.CAMERA_CONTROL_MIRROR, True)
337 |         ret = self._open_svc(self._id, self._settings)
338 |         if ret.err != 0:
339 |             raise OSError(ret.err, "Failed to open camera")
340 |         self._open = True
341 | 
342 |     def close(self):
343 |         """
344 |         Close, if necessary the camera.
345 |         """
346 |         ret = self._close_svc(self._id)
347 |         if ret.err != 0 and ret.err != errno.EINVAL:
348 |             raise OSError(ret.err, "Failed to close camera")
349 |         self._open = False
350 | 


--------------------------------------------------------------------------------
/src/baxter_interface/digital_io.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | import errno
 29 | 
 30 | import rospy
 31 | 
 32 | import baxter_dataflow
 33 | 
 34 | from baxter_core_msgs.msg import (
 35 |     DigitalIOState,
 36 |     DigitalOutputCommand,
 37 | )
 38 | 
 39 | 
 40 | class DigitalIO(object):
 41 |     """
 42 |     Interface class for a simple Digital Input and/or Output on the
 43 |     Baxter robot
 44 | 
 45 |     Input
 46 |       - read input state
 47 |     Output
 48 |       - turn output On/Off
 49 |       - read current output state
 50 |     """
 51 |     def __init__(self, component_id):
 52 |         """
 53 |         Constructor.
 54 | 
 55 |         @param component_id: unique id of the digital component
 56 |         """
 57 |         self._id = component_id
 58 |         self._component_type = 'digital_io'
 59 |         self._is_output = False
 60 |         self._state = None
 61 | 
 62 |         self.state_changed = baxter_dataflow.Signal()
 63 | 
 64 |         type_ns = '/robot/' + self._component_type
 65 |         topic_base = type_ns + '/' + self._id
 66 | 
 67 |         self._sub_state = rospy.Subscriber(
 68 |             topic_base + '/state',
 69 |             DigitalIOState,
 70 |             self._on_io_state)
 71 | 
 72 |         baxter_dataflow.wait_for(
 73 |             lambda: self._state != None,
 74 |             timeout=2.0,
 75 |             timeout_msg="Failed to get current digital_io state from %s" \
 76 |             % (topic_base,),
 77 |         )
 78 | 
 79 |         # check if output-capable before creating publisher
 80 |         if self._is_output:
 81 |             self._pub_output = rospy.Publisher(
 82 |                 type_ns + '/command',
 83 |                 DigitalOutputCommand,
 84 |                 queue_size=10)
 85 | 
 86 |     def _on_io_state(self, msg):
 87 |         """
 88 |         Updates the internally stored state of the Digital Input/Output.
 89 |         """
 90 |         new_state = (msg.state == DigitalIOState.PRESSED)
 91 |         if self._state is None:
 92 |             self._is_output = not msg.isInputOnly
 93 |         old_state = self._state
 94 |         self._state = new_state
 95 | 
 96 |         # trigger signal if changed
 97 |         if old_state is not None and old_state != new_state:
 98 |             self.state_changed(new_state)
 99 | 
100 |     @property
101 |     def is_output(self):
102 |         """
103 |         Accessor to check if IO is capable of output.
104 |         """
105 |         return self._is_output
106 | 
107 |     @property
108 |     def state(self):
109 |         """
110 |         Current state of the Digital Input/Output.
111 |         """
112 |         return self._state
113 | 
114 |     @state.setter
115 |     def state(self, value):
116 |         """
117 |         Control the state of the Digital Output. (is_output must be True)
118 | 
119 |         @type value: bool
120 |         @param value: new state to output {True, False}
121 |         """
122 |         self.set_output(value)
123 | 
124 |     def set_output(self, value, timeout=2.0):
125 |         """
126 |         Control the state of the Digital Output.
127 | 
128 |         Use this function for finer control over the wait_for timeout.
129 | 
130 |         @type value: bool
131 |         @param value: new state {True, False} of the Output.
132 |         @type timeout: float
133 |         @param timeout: Seconds to wait for the io to reflect command.
134 |                         If 0, just command once and return. [0]
135 |         """
136 |         if not self._is_output:
137 |             raise IOError(errno.EACCES, "Component is not an output [%s: %s]" %
138 |                 (self._component_type, self._id))
139 |         cmd = DigitalOutputCommand()
140 |         cmd.name = self._id
141 |         cmd.value = value
142 |         self._pub_output.publish(cmd)
143 | 
144 |         if not timeout == 0:
145 |             baxter_dataflow.wait_for(
146 |                 test=lambda: self.state == value,
147 |                 timeout=timeout,
148 |                 rate=100,
149 |                 timeout_msg=("Failed to command digital io to: %r" % (value,)),
150 |                 body=lambda: self._pub_output.publish(cmd)
151 |             )
152 | 


--------------------------------------------------------------------------------
/src/baxter_interface/head.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | from copy import deepcopy
 29 | from math import fabs
 30 | 
 31 | import rospy
 32 | 
 33 | from std_msgs.msg import (
 34 |     Bool
 35 | )
 36 | 
 37 | import baxter_dataflow
 38 | 
 39 | from baxter_core_msgs.msg import (
 40 |    HeadPanCommand,
 41 |    HeadState,
 42 | )
 43 | from baxter_interface import settings
 44 | 
 45 | 
 46 | class Head(object):
 47 |     """
 48 |     Interface class for the head on the Baxter Robot.
 49 | 
 50 |     Used to control the head pan angle and to enable/disable the head nod
 51 |     action.
 52 |     """
 53 |     def __init__(self):
 54 |         """
 55 |         Constructor.
 56 |         """
 57 |         self._state = dict()
 58 | 
 59 |         self._pub_pan = rospy.Publisher(
 60 |             '/robot/head/command_head_pan',
 61 |             HeadPanCommand,
 62 |             queue_size=10)
 63 | 
 64 |         self._pub_nod = rospy.Publisher(
 65 |             '/robot/head/command_head_nod',
 66 |             Bool,
 67 |             queue_size=10)
 68 | 
 69 |         state_topic = '/robot/head/head_state'
 70 |         self._sub_state = rospy.Subscriber(
 71 |             state_topic,
 72 |             HeadState,
 73 |             self._on_head_state)
 74 | 
 75 |         baxter_dataflow.wait_for(
 76 |             lambda: len(self._state) != 0,
 77 |             timeout=5.0,
 78 |             timeout_msg=("Failed to get current head state from %s" %
 79 |                          (state_topic,)),
 80 |         )
 81 | 
 82 |     def _on_head_state(self, msg):
 83 |         self._state['pan'] = msg.pan
 84 |         self._state['panning'] = msg.isTurning
 85 |         self._state['nodding'] = msg.isNodding
 86 | 
 87 |     def pan(self):
 88 |         """
 89 |         Get the current pan angle of the head.
 90 | 
 91 |         @rtype: float
 92 |         @return: current angle in radians
 93 |         """
 94 |         return self._state['pan']
 95 | 
 96 |     def nodding(self):
 97 |         """
 98 |         Check if the head is currently nodding.
 99 | 
100 |         @rtype: bool
101 |         @return: True if the head is currently nodding, False otherwise.
102 |         """
103 |         return self._state['nodding']
104 | 
105 |     def panning(self):
106 |         """
107 |         Check if the head is currently panning.
108 | 
109 |         @rtype: bool
110 |         @return: True if the head is currently panning, False otherwise.
111 |         """
112 |         return self._state['panning']
113 | 
114 |     def set_pan(self, angle, speed=1.0, timeout=10.0, scale_speed=False):
115 |         """
116 |         Pan at the given speed to the desired angle.
117 | 
118 |         @type angle: float
119 |         @param angle: Desired pan angle in radians.
120 |         @type speed: int
121 |         @param speed: Desired speed to pan at, range is 0-1.0 [1.0]
122 |         @type timeout: float
123 |         @param timeout: Seconds to wait for the head to pan to the
124 |                         specified angle. If 0, just command once and
125 |                         return. [10]
126 |         @param scale_speed: Scale speed to pan at by a factor of 100,
127 |                             to use legacy range between 0-100 [100]
128 |         """
129 |         if scale_speed:
130 |             cmd_speed = speed / 100.0;
131 |         else:
132 |             cmd_speed = speed
133 |         if (cmd_speed < HeadPanCommand.MIN_SPEED_RATIO or
134 |               cmd_speed > HeadPanCommand.MAX_SPEED_RATIO):
135 |             rospy.logerr(("Commanded Speed, ({0}), outside of valid range"
136 |                           " [{1}, {2}]").format(cmd_speed,
137 |                           HeadPanCommand.MIN_SPEED_RATIO,
138 |                           HeadPanCommand.MAX_SPEED_RATIO))
139 |         msg = HeadPanCommand(angle, cmd_speed, True)
140 |         self._pub_pan.publish(msg)
141 | 
142 |         if not timeout == 0:
143 |             baxter_dataflow.wait_for(
144 |                 lambda: (abs(self.pan() - angle) <=
145 |                          settings.HEAD_PAN_ANGLE_TOLERANCE),
146 |                 timeout=timeout,
147 |                 rate=100,
148 |                 timeout_msg="Failed to move head to pan command %f" % angle,
149 |                 body=lambda: self._pub_pan.publish(msg)
150 |                 )
151 | 
152 |     def command_nod(self, timeout=5.0):
153 |         """
154 |         Command the head to nod once.
155 | 
156 |         @type timeout: float
157 |         @param timeout: Seconds to wait for the head to nod.
158 |                         If 0, just command once and return. [0]
159 |         """
160 |         self._pub_nod.publish(True)
161 | 
162 |         if not timeout == 0:
163 |             # Wait for nod to initiate
164 |             baxter_dataflow.wait_for(
165 |                 test=self.nodding,
166 |                 timeout=timeout,
167 |                 rate=100,
168 |                 timeout_msg="Failed to initiate head nod command",
169 |                 body=lambda: self._pub_nod.publish(True)
170 |             )
171 | 
172 |             # Wait for nod to complete
173 |             baxter_dataflow.wait_for(
174 |                 test=lambda: not self.nodding(),
175 |                 timeout=timeout,
176 |                 rate=100,
177 |                 timeout_msg="Failed to complete head nod command",
178 |                 body=lambda: self._pub_nod.publish(False)
179 |             )
180 | 


--------------------------------------------------------------------------------
/src/baxter_interface/limb.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | import collections
 29 | 
 30 | from copy import deepcopy
 31 | 
 32 | import rospy
 33 | 
 34 | from sensor_msgs.msg import (
 35 |     JointState
 36 | )
 37 | from std_msgs.msg import (
 38 |     Float64,
 39 | )
 40 | 
 41 | import baxter_dataflow
 42 | 
 43 | from baxter_core_msgs.msg import (
 44 |     JointCommand,
 45 |     EndpointState,
 46 | )
 47 | from baxter_interface import settings
 48 | 
 49 | 
 50 | class Limb(object):
 51 |     """
 52 |     Interface class for a limb on the Baxter robot.
 53 |     """
 54 | 
 55 |     # Containers
 56 |     Point = collections.namedtuple('Point', ['x', 'y', 'z'])
 57 |     Quaternion = collections.namedtuple('Quaternion', ['x', 'y', 'z', 'w'])
 58 | 
 59 |     def __init__(self, limb):
 60 |         """
 61 |         Constructor.
 62 | 
 63 |         @type limb: str
 64 |         @param limb: limb to interface
 65 |         """
 66 |         self.name = limb
 67 |         self._joint_angle = dict()
 68 |         self._joint_velocity = dict()
 69 |         self._joint_effort = dict()
 70 |         self._cartesian_pose = dict()
 71 |         self._cartesian_velocity = dict()
 72 |         self._cartesian_effort = dict()
 73 | 
 74 |         self._joint_names = {
 75 |             'left': ['left_s0', 'left_s1', 'left_e0', 'left_e1',
 76 |                      'left_w0', 'left_w1', 'left_w2'],
 77 |             'right': ['right_s0', 'right_s1', 'right_e0', 'right_e1',
 78 |                       'right_w0', 'right_w1', 'right_w2']
 79 |             }
 80 | 
 81 |         ns = '/robot/limb/' + limb + '/'
 82 | 
 83 |         self._command_msg = JointCommand()
 84 | 
 85 |         self._pub_speed_ratio = rospy.Publisher(
 86 |             ns + 'set_speed_ratio',
 87 |             Float64,
 88 |             latch=True,
 89 |             queue_size=10)
 90 | 
 91 |         self._pub_joint_cmd = rospy.Publisher(
 92 |             ns + 'joint_command',
 93 |             JointCommand,
 94 |             tcp_nodelay=True,
 95 |             queue_size=1)
 96 | 
 97 |         self._pub_joint_cmd_timeout = rospy.Publisher(
 98 |             ns + 'joint_command_timeout',
 99 |             Float64,
100 |             latch=True,
101 |             queue_size=10)
102 | 
103 |         _cartesian_state_sub = rospy.Subscriber(
104 |             ns + 'endpoint_state',
105 |             EndpointState,
106 |             self._on_endpoint_states,
107 |             queue_size=1,
108 |             tcp_nodelay=True)
109 | 
110 |         joint_state_topic = 'robot/joint_states'
111 |         _joint_state_sub = rospy.Subscriber(
112 |             joint_state_topic,
113 |             JointState,
114 |             self._on_joint_states,
115 |             queue_size=1,
116 |             tcp_nodelay=True)
117 | 
118 |         err_msg = ("%s limb init failed to get current joint_states "
119 |                    "from %s") % (self.name.capitalize(), joint_state_topic)
120 |         baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
121 |                                  timeout_msg=err_msg)
122 |         err_msg = ("%s limb init failed to get current endpoint_state "
123 |                    "from %s") % (self.name.capitalize(), ns + 'endpoint_state')
124 |         baxter_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
125 |                                  timeout_msg=err_msg)
126 | 
127 |     def _on_joint_states(self, msg):
128 |         for idx, name in enumerate(msg.name):
129 |             if name in self._joint_names[self.name]:
130 |                 self._joint_angle[name] = msg.position[idx]
131 |                 self._joint_velocity[name] = msg.velocity[idx]
132 |                 self._joint_effort[name] = msg.effort[idx]
133 | 
134 |     def _on_endpoint_states(self, msg):
135 |         # Comments in this private method are for documentation purposes.
136 |         # _pose = {'position': (x, y, z), 'orientation': (x, y, z, w)}
137 |         self._cartesian_pose = {
138 |             'position': self.Point(
139 |                 msg.pose.position.x,
140 |                 msg.pose.position.y,
141 |                 msg.pose.position.z,
142 |             ),
143 |             'orientation': self.Quaternion(
144 |                 msg.pose.orientation.x,
145 |                 msg.pose.orientation.y,
146 |                 msg.pose.orientation.z,
147 |                 msg.pose.orientation.w,
148 |             ),
149 |         }
150 |         # _twist = {'linear': (x, y, z), 'angular': (x, y, z)}
151 |         self._cartesian_velocity = {
152 |             'linear': self.Point(
153 |                 msg.twist.linear.x,
154 |                 msg.twist.linear.y,
155 |                 msg.twist.linear.z,
156 |             ),
157 |             'angular': self.Point(
158 |                 msg.twist.angular.x,
159 |                 msg.twist.angular.y,
160 |                 msg.twist.angular.z,
161 |             ),
162 |         }
163 |         # _wrench = {'force': (x, y, z), 'torque': (x, y, z)}
164 |         self._cartesian_effort = {
165 |             'force': self.Point(
166 |                 msg.wrench.force.x,
167 |                 msg.wrench.force.y,
168 |                 msg.wrench.force.z,
169 |             ),
170 |             'torque': self.Point(
171 |                 msg.wrench.torque.x,
172 |                 msg.wrench.torque.y,
173 |                 msg.wrench.torque.z,
174 |             ),
175 |         }
176 | 
177 |     def joint_names(self):
178 |         """
179 |         Return the names of the joints for the specified limb.
180 | 
181 |         @rtype: [str]
182 |         @return: ordered list of joint names from proximal to distal
183 |         (i.e. shoulder to wrist).
184 |         """
185 |         return self._joint_names[self.name]
186 | 
187 |     def joint_angle(self, joint):
188 |         """
189 |         Return the requested joint angle.
190 | 
191 |         @type joint: str
192 |         @param joint: name of a joint
193 |         @rtype: float
194 |         @return: angle in radians of individual joint
195 |         """
196 |         return self._joint_angle[joint]
197 | 
198 |     def joint_angles(self):
199 |         """
200 |         Return all joint angles.
201 | 
202 |         @rtype: dict({str:float})
203 |         @return: unordered dict of joint name Keys to angle (rad) Values
204 |         """
205 |         return deepcopy(self._joint_angle)
206 | 
207 |     def joint_velocity(self, joint):
208 |         """
209 |         Return the requested joint velocity.
210 | 
211 |         @type joint: str
212 |         @param joint: name of a joint
213 |         @rtype: float
214 |         @return: velocity in radians/s of individual joint
215 |         """
216 |         return self._joint_velocity[joint]
217 | 
218 |     def joint_velocities(self):
219 |         """
220 |         Return all joint velocities.
221 | 
222 |         @rtype: dict({str:float})
223 |         @return: unordered dict of joint name Keys to velocity (rad/s) Values
224 |         """
225 |         return deepcopy(self._joint_velocity)
226 | 
227 |     def joint_effort(self, joint):
228 |         """
229 |         Return the requested joint effort.
230 | 
231 |         @type joint: str
232 |         @param joint: name of a joint
233 |         @rtype: float
234 |         @return: effort in Nm of individual joint
235 |         """
236 |         return self._joint_effort[joint]
237 | 
238 |     def joint_efforts(self):
239 |         """
240 |         Return all joint efforts.
241 | 
242 |         @rtype: dict({str:float})
243 |         @return: unordered dict of joint name Keys to effort (Nm) Values
244 |         """
245 |         return deepcopy(self._joint_effort)
246 | 
247 |     def endpoint_pose(self):
248 |         """
249 |         Return Cartesian endpoint pose {position, orientation}.
250 | 
251 |         @rtype: dict({str:L{Limb.Point},str:L{Limb.Quaternion}})
252 |         @return: position and orientation as named tuples in a dict
253 | 
254 |         C{pose = {'position': (x, y, z), 'orientation': (x, y, z, w)}}
255 | 
256 |           - 'position': Cartesian coordinates x,y,z in
257 |                         namedtuple L{Limb.Point}
258 |           - 'orientation': quaternion x,y,z,w in named tuple
259 |                            L{Limb.Quaternion}
260 |         """
261 |         return deepcopy(self._cartesian_pose)
262 | 
263 |     def endpoint_velocity(self):
264 |         """
265 |         Return Cartesian endpoint twist {linear, angular}.
266 | 
267 |         @rtype: dict({str:L{Limb.Point},str:L{Limb.Point}})
268 |         @return: linear and angular velocities as named tuples in a dict
269 | 
270 |         C{twist = {'linear': (x, y, z), 'angular': (x, y, z)}}
271 | 
272 |           - 'linear': Cartesian velocity in x,y,z directions in
273 |                       namedtuple L{Limb.Point}
274 |           - 'angular': Angular velocity around x,y,z axes in named tuple
275 |                        L{Limb.Point}
276 |         """
277 |         return deepcopy(self._cartesian_velocity)
278 | 
279 |     def endpoint_effort(self):
280 |         """
281 |         Return Cartesian endpoint wrench {force, torque}.
282 | 
283 |         @rtype: dict({str:L{Limb.Point},str:L{Limb.Point}})
284 |         @return: force and torque at endpoint as named tuples in a dict
285 | 
286 |         C{wrench = {'force': (x, y, z), 'torque': (x, y, z)}}
287 | 
288 |           - 'force': Cartesian force on x,y,z axes in
289 |                      namedtuple L{Limb.Point}
290 |           - 'torque': Torque around x,y,z axes in named tuple
291 |                       L{Limb.Point}
292 |         """
293 |         return deepcopy(self._cartesian_effort)
294 | 
295 |     def set_command_timeout(self, timeout):
296 |         """
297 |         Set the timeout in seconds for the joint controller
298 | 
299 |         @type timeout: float
300 |         @param timeout: timeout in seconds
301 |         """
302 |         self._pub_joint_cmd_timeout.publish(Float64(timeout))
303 | 
304 |     def exit_control_mode(self, timeout=0.2):
305 |         """
306 |         Clean exit from advanced control modes (joint torque or velocity).
307 | 
308 |         Resets control to joint position mode with current positions.
309 | 
310 |         @type timeout: float
311 |         @param timeout: control timeout in seconds [0.2]
312 |         """
313 |         self.set_command_timeout(timeout)
314 |         self.set_joint_positions(self.joint_angles())
315 | 
316 |     def set_joint_position_speed(self, speed):
317 |         """
318 |         Set ratio of max joint speed to use during joint position moves.
319 | 
320 |         Set the proportion of maximum controllable velocity to use
321 |         during joint position control execution. The default ratio
322 |         is `0.3`, and can be set anywhere from [0.0-1.0] (clipped).
323 |         Once set, a speed ratio will persist until a new execution
324 |         speed is set.
325 | 
326 |         @type speed: float
327 |         @param speed: ratio of maximum joint speed for execution
328 |                       default= 0.3; range= [0.0-1.0]
329 |         """
330 |         self._pub_speed_ratio.publish(Float64(speed))
331 | 
332 |     def set_joint_positions(self, positions, raw=False):
333 |         """
334 |         Commands the joints of this limb to the specified positions.
335 | 
336 |         B{IMPORTANT:} 'raw' joint position control mode allows for commanding
337 |         joint positions, without modification, directly to the JCBs
338 |         (Joint Controller Boards). While this results in more unaffected
339 |         motions, 'raw' joint position control mode bypasses the safety system
340 |         modifications (e.g. collision avoidance).
341 |         Please use with caution.
342 | 
343 |         @type positions: dict({str:float})
344 |         @param positions: joint_name:angle command
345 |         @type raw: bool
346 |         @param raw: advanced, direct position control mode
347 |         """
348 |         self._command_msg.names = positions.keys()
349 |         self._command_msg.command = positions.values()
350 |         if raw:
351 |             self._command_msg.mode = JointCommand.RAW_POSITION_MODE
352 |         else:
353 |             self._command_msg.mode = JointCommand.POSITION_MODE
354 |         self._pub_joint_cmd.publish(self._command_msg)
355 | 
356 |     def set_joint_velocities(self, velocities):
357 |         """
358 |         Commands the joints of this limb to the specified velocities.
359 | 
360 |         B{IMPORTANT}: set_joint_velocities must be commanded at a rate great
361 |         than the timeout specified by set_command_timeout. If the timeout is
362 |         exceeded before a new set_joint_velocities command is received, the
363 |         controller will switch modes back to position mode for safety purposes.
364 | 
365 |         @type velocities: dict({str:float})
366 |         @param velocities: joint_name:velocity command
367 |         """
368 |         self._command_msg.names = velocities.keys()
369 |         self._command_msg.command = velocities.values()
370 |         self._command_msg.mode = JointCommand.VELOCITY_MODE
371 |         self._pub_joint_cmd.publish(self._command_msg)
372 | 
373 |     def set_joint_torques(self, torques):
374 |         """
375 |         Commands the joints of this limb to the specified torques.
376 | 
377 |         B{IMPORTANT}: set_joint_torques must be commanded at a rate great than
378 |         the timeout specified by set_command_timeout. If the timeout is
379 |         exceeded before a new set_joint_torques command is received, the
380 |         controller will switch modes back to position mode for safety purposes.
381 | 
382 |         @type torques: dict({str:float})
383 |         @param torques: joint_name:torque command
384 |         """
385 |         self._command_msg.names = torques.keys()
386 |         self._command_msg.command = torques.values()
387 |         self._command_msg.mode = JointCommand.TORQUE_MODE
388 |         self._pub_joint_cmd.publish(self._command_msg)
389 | 
390 |     def move_to_neutral(self, timeout=15.0):
391 |         """
392 |         Command the joints to the center of their joint ranges
393 | 
394 |         Neutral is defined as::
395 |           ['*_s0', '*_s1', '*_e0', '*_e1', '*_w0', '*_w1', '*_w2']
396 |           [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0]
397 | 
398 |         @type timeout: float
399 |         @param timeout: seconds to wait for move to finish [15]
400 |         """
401 |         angles = dict(zip(self.joint_names(),
402 |                           [0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0]))
403 |         return self.move_to_joint_positions(angles, timeout)
404 | 
405 |     def move_to_joint_positions(self, positions, timeout=15.0,
406 |                                 threshold=settings.JOINT_ANGLE_TOLERANCE,
407 |                                 test=None):
408 |         """
409 |         (Blocking) Commands the limb to the provided positions.
410 | 
411 |         Waits until the reported joint state matches that specified.
412 | 
413 |         This function uses a low-pass filter to smooth the movement.
414 | 
415 |         @type positions: dict({str:float})
416 |         @param positions: joint_name:angle command
417 |         @type timeout: float
418 |         @param timeout: seconds to wait for move to finish [15]
419 |         @type threshold: float
420 |         @param threshold: position threshold in radians across each joint when
421 |         move is considered successful [0.008726646]
422 |         @param test: optional function returning True if motion must be aborted
423 |         """
424 |         cmd = self.joint_angles()
425 | 
426 |         def filtered_cmd():
427 |             # First Order Filter - 0.2 Hz Cutoff
428 |             for joint in positions.keys():
429 |                 cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
430 |             return cmd
431 | 
432 |         def genf(joint, angle):
433 |             def joint_diff():
434 |                 return abs(angle - self._joint_angle[joint])
435 |             return joint_diff
436 | 
437 |         diffs = [genf(j, a) for j, a in positions.items() if
438 |                  j in self._joint_angle]
439 | 
440 |         self.set_joint_positions(filtered_cmd())
441 |         baxter_dataflow.wait_for(
442 |             test=lambda: callable(test) and test() == True or \
443 |                          (all(diff() < threshold for diff in diffs)),
444 |             timeout=timeout,
445 |             timeout_msg=("%s limb failed to reach commanded joint positions" %
446 |                          (self.name.capitalize(),)),
447 |             rate=100,
448 |             raise_on_error=False,
449 |             body=lambda: self.set_joint_positions(filtered_cmd())
450 |             )
451 | 


--------------------------------------------------------------------------------
/src/baxter_interface/navigator.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | import rospy
 29 | 
 30 | import baxter_dataflow
 31 | 
 32 | from baxter_core_msgs.msg import (
 33 |     NavigatorState,
 34 | )
 35 | 
 36 | from baxter_interface import (
 37 |     digital_io,
 38 | )
 39 | 
 40 | class Navigator(object):
 41 |     """
 42 |     Interface class for a Navigator on the Baxter robot.
 43 | 
 44 |     Inputs:
 45 |         Button 0     - press wheel
 46 |         Button 1     - above wheel
 47 |         Button 2     - below wheel
 48 |         Scroll wheel - 0-255
 49 | 
 50 |     Outputs:
 51 |         Inner LED
 52 |         Outer LED
 53 | 
 54 |     Signals:
 55 |         button0_changed - True/False
 56 |         button1_changed - True/False
 57 |         button2_changed - True/False
 58 |         wheel_changed   - New wheel value
 59 | 
 60 |     Valid identifiers:
 61 |         left, right, torso_left, torso_right
 62 |     """
 63 | 
 64 |     __LOCATIONS = ('left', 'right', 'torso_left', 'torso_right')
 65 | 
 66 |     def __init__(self, location):
 67 |         """
 68 |         Constructor.
 69 | 
 70 |         @type location: str
 71 |         @param location: body location (prefix) of Navigator to control.
 72 | 
 73 |         Valid locations are in L{Navigator.__LOCATIONS}::
 74 |           left, right, torso_left, torso_right
 75 |         """
 76 |         if not location in self.__LOCATIONS:
 77 |             raise AttributeError("Invalid Navigator name '%s'" % (location,))
 78 |         self._id = location
 79 |         self._state = None
 80 |         self.button0_changed = baxter_dataflow.Signal()
 81 |         self.button1_changed = baxter_dataflow.Signal()
 82 |         self.button2_changed = baxter_dataflow.Signal()
 83 |         self.wheel_changed = baxter_dataflow.Signal()
 84 | 
 85 |         nav_state_topic = 'robot/navigators/{0}_navigator/state'.format(self._id)
 86 |         self._state_sub = rospy.Subscriber(
 87 |             nav_state_topic,
 88 |             NavigatorState,
 89 |             self._on_state)
 90 | 
 91 |         self._inner_led = digital_io.DigitalIO(
 92 |             '%s_inner_light' % (self._id,))
 93 |         self._inner_led_idx = 0
 94 | 
 95 |         self._outer_led = digital_io.DigitalIO(
 96 |             '%s_outer_light' % (self._id,))
 97 |         self._outer_led_idx = 1
 98 | 
 99 |         init_err_msg = ("Navigator init failed to get current state from %s" %
100 |                         (nav_state_topic,))
101 |         baxter_dataflow.wait_for(lambda: self._state != None,
102 |                                  timeout_msg=init_err_msg)
103 | 
104 |     @property
105 |     def wheel(self):
106 |         """
107 |         Current state of the wheel
108 |         """
109 |         return self._state.wheel
110 | 
111 |     @property
112 |     def button0(self):
113 |         """
114 |         Current state of button 0
115 |         """
116 |         return self._state.buttons[0]
117 | 
118 |     @property
119 |     def button1(self):
120 |         """
121 |         Current state of button 1
122 |         """
123 |         return self._state.buttons[1]
124 | 
125 |     @property
126 |     def button2(self):
127 |         """
128 |         Current state of button 2
129 |         """
130 |         return self._state.buttons[2]
131 | 
132 |     @property
133 |     def inner_led(self):
134 |         """
135 |         Current state of the inner LED
136 |         """
137 |         return self._state.lights[self._inner_led_idx]
138 | 
139 |     @inner_led.setter
140 |     def inner_led(self, enable):
141 |         """
142 |         Control the inner LED.
143 | 
144 |         @type enable: bool
145 |         @param enable: True to enable the light, False otherwise
146 |         """
147 |         self._inner_led.set_output(enable)
148 | 
149 |     @property
150 |     def outer_led(self):
151 |         """
152 |         Current state of the outer LED.
153 |         """
154 |         return self._state.lights[self._outer_led_idx]
155 | 
156 |     @outer_led.setter
157 |     def outer_led(self, enable):
158 |         """
159 |         Control the outer LED.
160 | 
161 |         @type enable: bool
162 |         @param enable: True to enable the light, False otherwise
163 |         """
164 |         self._outer_led.set_output(enable)
165 | 
166 |     def _on_state(self, msg):
167 |         if not self._state:
168 |             self._state = msg
169 |             try:
170 |                 self._inner_led_idx = self._state.light_names.index("inner")
171 |             except:
172 |                 pass
173 |             try:
174 |                 self._outer_led_idx = self._state.light_names.index("outer")
175 |             except:
176 |                 pass
177 |         if self._state == msg:
178 |             return
179 | 
180 |         old_state = self._state
181 |         self._state = msg
182 | 
183 |         buttons = [self.button0_changed,
184 |                    self.button1_changed,
185 |                    self.button2_changed
186 |                    ]
187 |         for i, signal in enumerate(buttons):
188 |             if old_state.buttons[i] != msg.buttons[i]:
189 |                 signal(msg.buttons[i])
190 | 
191 |         if old_state.wheel != msg.wheel:
192 |             diff = msg.wheel - old_state.wheel
193 |             if abs(diff % 256) < 127:
194 |                 self.wheel_changed(diff % 256)
195 |             else:
196 |                 self.wheel_changed(diff % (-256))
197 | 


--------------------------------------------------------------------------------
/src/baxter_interface/robot_enable.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | import errno
 29 | import re
 30 | import sys
 31 | 
 32 | from threading import Lock
 33 | 
 34 | import rospy
 35 | 
 36 | from std_msgs.msg import (
 37 |     Bool,
 38 |     Empty,
 39 | )
 40 | 
 41 | import baxter_dataflow
 42 | 
 43 | from baxter_core_msgs.msg import (
 44 |     AssemblyState,
 45 | )
 46 | from baxter_interface import settings
 47 | 
 48 | 
 49 | class RobotEnable(object):
 50 |     """
 51 |     Class RobotEnable - simple control/status wrapper around robot state
 52 | 
 53 |     enable()  - enable all joints
 54 |     disable() - disable all joints
 55 |     reset()   - reset all joints, reset all jrcp faults, disable the robot
 56 |     stop()    - stop the robot, similar to hitting the e-stop button
 57 |     """
 58 | 
 59 |     param_lock = Lock()
 60 | 
 61 |     def __init__(self, versioned=False):
 62 |         """
 63 |         Version checking capable constructor.
 64 | 
 65 |         @type versioned: bool
 66 |         @param versioned: True to check robot software version
 67 |         compatibility on initialization. False (default) to ignore.
 68 | 
 69 |         The compatibility of robot versions to SDK (baxter_interface)
 70 |         versions is defined in the L{baxter_interface.VERSIONS_SDK2ROBOT}.
 71 | 
 72 |         By default, the class does not check, but all examples do. The
 73 |         example behavior can be overridden by changing the value of
 74 |         L{baxter_interface.CHECK_VERSION} to False.
 75 |         """
 76 |         self._state = None
 77 |         state_topic = 'robot/state'
 78 |         self._state_sub = rospy.Subscriber(state_topic,
 79 |                                            AssemblyState,
 80 |                                            self._state_callback
 81 |                                            )
 82 |         if versioned and not self.version_check():
 83 |             sys.exit(1)
 84 | 
 85 |         baxter_dataflow.wait_for(
 86 |             lambda: not self._state is None,
 87 |             timeout=2.0,
 88 |             timeout_msg=("Failed to get robot state on %s" %
 89 |             (state_topic,)),
 90 |         )
 91 | 
 92 |     def _state_callback(self, msg):
 93 |         self._state = msg
 94 | 
 95 |     def _toggle_enabled(self, status):
 96 | 
 97 |         pub = rospy.Publisher('robot/set_super_enable', Bool, 
 98 |                               queue_size=10)
 99 | 
100 |         baxter_dataflow.wait_for(
101 |             test=lambda: self._state.enabled == status,
102 |             timeout=2.0 if status else 5.0,
103 |             timeout_msg=("Failed to %sable robot" %
104 |                          ('en' if status else 'dis',)),
105 |             body=lambda: pub.publish(status),
106 |         )
107 |         rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled'))
108 | 
109 |     def state(self):
110 |         """
111 |         Returns the last known robot state.
112 |         """
113 |         return self._state
114 | 
115 |     def enable(self):
116 |         """
117 |         Enable all joints
118 |         """
119 |         if self._state.stopped:
120 |             rospy.loginfo("Robot Stopped: Attempting Reset...")
121 |             self.reset()
122 |         self._toggle_enabled(True)
123 | 
124 |     def disable(self):
125 |         """
126 |         Disable all joints
127 |         """
128 |         self._toggle_enabled(False)
129 | 
130 |     def reset(self):
131 |         """
132 |         Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable
133 |         the robot.
134 |         """
135 |         error_estop = """\
136 | E-Stop is ASSERTED. Disengage E-Stop and then reset the robot.
137 | """
138 |         error_nonfatal = """Non-fatal Robot Error on reset.
139 | Robot reset cleared stopped state and robot can be enabled, but a non-fatal
140 | error persists. Check diagnostics or rethink.log for more info.
141 | """
142 |         error_env = """Failed to reset robot.
143 | Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set
144 | and resolvable. For more information please visit:
145 | http://sdk.rethinkrobotics.com/wiki/RSDK_Shell#Initialize
146 | """
147 |         is_reset = lambda: (self._state.enabled == False and
148 |                             self._state.stopped == False and
149 |                             self._state.error == False and
150 |                             self._state.estop_button == 0 and
151 |                             self._state.estop_source == 0)
152 |         pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10)
153 | 
154 |         if (self._state.stopped and
155 |             self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED):
156 |             rospy.logfatal(error_estop)
157 |             raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged")
158 | 
159 |         rospy.loginfo("Resetting robot...")
160 |         try:
161 |             baxter_dataflow.wait_for(
162 |                 test=is_reset,
163 |                 timeout=3.0,
164 |                 timeout_msg=error_env,
165 |                 body=pub.publish
166 |             )
167 |         except OSError, e:
168 |             if e.errno == errno.ETIMEDOUT:
169 |                 if self._state.error == True and self._state.stopped == False:
170 |                     rospy.logwarn(error_nonfatal)
171 |                     return False
172 |             raise
173 | 
174 |     def stop(self):
175 |         """
176 |         Simulate an e-stop button being pressed.  Robot must be reset to clear
177 |         the stopped state.
178 |         """
179 |         pub = rospy.Publisher('robot/set_super_stop', Empty, queue_size=10)
180 |         baxter_dataflow.wait_for(
181 |             test=lambda: self._state.stopped == True,
182 |             timeout=3.0,
183 |             timeout_msg="Failed to stop the robot",
184 |             body=pub.publish,
185 |         )
186 | 
187 |     def version_check(self):
188 |         """
189 |         Verifies the version of the software running on the robot is
190 |         compatible with this local version of the Baxter RSDK.
191 | 
192 |         Currently uses the variables in baxter_interface.settings and
193 |         can be overridden for all default examples by setting CHECK_VERSION
194 |         to False.
195 |         """
196 |         param_name = "rethink/software_version"
197 |         sdk_version = settings.SDK_VERSION
198 | 
199 |         # get local lock for rosparam threading bug
200 |         with self.__class__.param_lock:
201 |             robot_version = rospy.get_param(param_name, None)
202 |         if not robot_version:
203 |             rospy.logwarn("RobotEnable: Failed to retrieve robot version "
204 |                           "from rosparam: %s\n"
205 |                           "Verify robot state and connectivity "
206 |                           "(i.e. ROS_MASTER_URI)", param_name)
207 |             return False
208 |         else:
209 |             # parse out first 3 digits of robot version tag
210 |             pattern = ("^([0-9]+)\.([0-9]+)\.([0-9]+)")
211 |             match = re.search(pattern, robot_version)
212 |             if not match:
213 |                 rospy.logwarn("RobotEnable: Invalid robot version: %s",
214 |                               robot_version)
215 |                 return False
216 |             robot_version = match.string[match.start(1):match.end(3)]
217 |             if robot_version not in settings.VERSIONS_SDK2ROBOT[sdk_version]:
218 |                 errstr_version = """RobotEnable: Software Version Mismatch.
219 | Robot Software version (%s) does not match local SDK version (%s). Please
220 | Update your Robot Software. \
221 | See: http://sdk.rethinkrobotics.com/wiki/Software_Update"""
222 |                 rospy.logerr(errstr_version, robot_version, sdk_version)
223 |                 return False
224 |         return True
225 | 


--------------------------------------------------------------------------------
/src/baxter_interface/robust_controller.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | import errno
 29 | 
 30 | import rospy
 31 | 
 32 | from baxter_core_msgs.msg import (
 33 |     RobustControllerStatus,
 34 | )
 35 | 
 36 | 
 37 | class RobustController(object):
 38 |     (STATE_IDLE,
 39 |      STATE_STARTING,
 40 |      STATE_RUNNING,
 41 |      STATE_STOPPING) = range(4)
 42 | 
 43 |     def __init__(self, namespace, enable_msg, disable_msg, timeout=60):
 44 |         """
 45 |         Wrapper around controlling a RobustController
 46 | 
 47 |         @param namespace: namespace containing the enable and status topics
 48 |         @param enable_msg: message to send to enable the RC
 49 |         @param disable_msg: message to send to disable the RC
 50 |         @param timeout: seconds to wait for the RC to finish [60]
 51 |         """
 52 |         self._command_pub = rospy.Publisher(
 53 |             namespace + '/enable',
 54 |             type(enable_msg),
 55 |             queue_size=10)
 56 |         self._status_sub = rospy.Subscriber(
 57 |             namespace + '/status',
 58 |             RobustControllerStatus,
 59 |             self._callback)
 60 | 
 61 |         self._enable_msg = enable_msg
 62 |         self._disable_msg = disable_msg
 63 |         self._timeout = timeout
 64 |         self._state = self.STATE_IDLE
 65 |         self._return = 0
 66 | 
 67 |         rospy.on_shutdown(self._on_shutdown)
 68 | 
 69 |     def _callback(self, msg):
 70 |         if self._state == self.STATE_RUNNING:
 71 |             if msg.complete == RobustControllerStatus.COMPLETE_W_SUCCESS:
 72 |                 self._state = self.STATE_STOPPING
 73 |                 self._return = 0
 74 | 
 75 |             elif msg.complete == RobustControllerStatus.COMPLETE_W_FAILURE:
 76 |                 self._state = self.STATE_STOPPING
 77 |                 self._return = errno.EIO
 78 | 
 79 |             elif not msg.isEnabled:
 80 |                 self._state = self.STATE_IDLE
 81 |                 self._return = errno.ENOMSG
 82 | 
 83 |         elif self._state == self.STATE_STOPPING and not msg.isEnabled:
 84 |             # Would be nice to use msg.state here, but it does not
 85 |             # consistently reflect reality.
 86 |             self._state = self.STATE_IDLE
 87 | 
 88 |         elif self._state == self.STATE_STARTING and msg.isEnabled:
 89 |             self._state = self.STATE_RUNNING
 90 | 
 91 |     def _run_loop(self):
 92 |         # RobustControllers need messages at < 1Hz in order to continue
 93 |         # their current operation.
 94 |         rate = rospy.Rate(2)
 95 |         start = rospy.Time.now()
 96 | 
 97 |         while not rospy.is_shutdown():
 98 |             if (self._state == self.STATE_RUNNING and
 99 |                 (rospy.Time.now() - start).to_sec() > self._timeout):
100 |                 self._state = self.STATE_STOPPING
101 |                 self._command_pub.publish(self._disable_msg)
102 |                 self._return = errno.ETIMEDOUT
103 | 
104 |             elif self._state in (self.STATE_STARTING, self.STATE_RUNNING):
105 |                 self._command_pub.publish(self._enable_msg)
106 | 
107 |             elif self._state == self.STATE_STOPPING:
108 |                 self._command_pub.publish(self._disable_msg)
109 | 
110 |             elif self._state == self.STATE_IDLE:
111 |                 break
112 | 
113 |             rate.sleep()
114 | 
115 |     def _on_shutdown(self):
116 |         rate = rospy.Rate(2)
117 | 
118 |         while not self._state == self.STATE_IDLE:
119 |             self._command_pub.publish(self._disable_msg)
120 |             rate.sleep()
121 | 
122 |         self._return = errno.ECONNABORTED
123 | 
124 |     def run(self):
125 |         """
126 |         Enable the RobustController and run until completion or error.
127 |         """
128 |         self._state = self.STATE_STARTING
129 |         self._command_pub.publish(self._enable_msg)
130 |         self._run_loop()
131 |         if self._return != 0:
132 |             msgs = {
133 |                 errno.EIO:          "Robust controller failed",
134 |                 errno.ENOMSG:       "Robust controller failed to enable",
135 |                 errno.ETIMEDOUT:    "Robust controller timed out",
136 |                 errno.ECONNABORTED: "Robust controller interrupted by user",
137 |                 }
138 | 
139 |             msg = msgs.get(self._return, None)
140 |             if msg:
141 |                 raise IOError(self._return, msg)
142 |             raise IOError(self._return)
143 | 


--------------------------------------------------------------------------------
/src/baxter_interface/settings.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | JOINT_ANGLE_TOLERANCE = 0.008726646
29 | HEAD_PAN_ANGLE_TOLERANCE = 0.1396263401
30 | 
31 | ## Versioning
32 | SDK_VERSION = '1.2.0'
33 | CHECK_VERSION = True
34 | # Version Compatibility Maps - {current: compatible}
35 | VERSIONS_SDK2ROBOT = {'1.2.0': ['1.2.0']}
36 | VERSIONS_SDK2GRIPPER = {'1.2.0':
37 |                           {
38 |                            'warn': '2014/5/20 00:00:00',  # Version 1.0.0
39 |                            'fail': '2013/10/15 00:00:00', # Version 0.6.2
40 |                           }
41 |                        }
42 | 


--------------------------------------------------------------------------------
/src/gripper_action/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | from .gripper_action import GripperActionServer
29 | 


--------------------------------------------------------------------------------
/src/gripper_action/gripper_action.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | """
 29 | Baxter RSDK Gripper Action Server
 30 | """
 31 | from math import fabs
 32 | 
 33 | import rospy
 34 | 
 35 | import actionlib
 36 | 
 37 | from control_msgs.msg import (
 38 |     GripperCommandAction,
 39 |     GripperCommandFeedback,
 40 |     GripperCommandResult,
 41 | )
 42 | 
 43 | import baxter_interface
 44 | 
 45 | from baxter_interface import CHECK_VERSION
 46 | 
 47 | 
 48 | class GripperActionServer(object):
 49 |     def __init__(self, gripper, reconfig_server):
 50 |         self._dyn = reconfig_server
 51 |         self._ee = gripper + '_gripper'
 52 |         self._ns = 'robot/end_effector/' + self._ee + '/gripper_action'
 53 |         self._gripper = baxter_interface.Gripper(gripper, CHECK_VERSION)
 54 |         # Store Gripper Type
 55 |         self._type = self._gripper.type()
 56 |         if self._type == 'custom':
 57 |             msg = ("Stopping %s action server - %s gripper not capable of "
 58 |                    "gripper actions" % (self._gripper.name, self._type))
 59 |             rospy.logerr(msg)
 60 |             return
 61 | 
 62 |         # Verify Grippers Have No Errors and are Calibrated
 63 |         if self._gripper.error():
 64 |             self._gripper.reset()
 65 |             if self._gripper.error():
 66 |                 msg = ("Stopping %s action server - Unable to clear error" %
 67 |                        self._gripper.name)
 68 |                 rospy.logerr(msg)
 69 |                 return
 70 |         if not self._gripper.calibrated():
 71 |             self._gripper.calibrate()
 72 |             if not self._gripper.calibrated():
 73 |                 msg = ("Stopping %s action server - Unable to calibrate" %
 74 |                        self._gripper.name)
 75 |                 rospy.logerr(msg)
 76 |                 return
 77 | 
 78 |         # Action Server
 79 |         self._server = actionlib.SimpleActionServer(
 80 |             self._ns,
 81 |             GripperCommandAction,
 82 |             execute_cb=self._on_gripper_action,
 83 |             auto_start=False)
 84 |         self._action_name = rospy.get_name()
 85 |         self._server.start()
 86 | 
 87 |         # Action Feedback/Result
 88 |         self._fdbk = GripperCommandFeedback()
 89 |         self._result = GripperCommandResult()
 90 | 
 91 |         # Initialize Parameters
 92 |         self._prm = self._gripper.parameters()
 93 |         self._timeout = 5.0
 94 | 
 95 |     def _get_gripper_parameters(self):
 96 |         self._timeout = self._dyn.config[self._ee + '_timeout']
 97 |         if self._type == 'electric':
 98 |             self._prm['dead_zone'] = self._dyn.config[self._ee + '_goal']
 99 |             self._prm['velocity'] = self._dyn.config[self._ee + '_velocity']
100 |             self._prm['moving_force'] = self._dyn.config[self._ee +
101 |                                                          '_moving_force']
102 |             self._prm['holding_force'] = self._dyn.config[self._ee +
103 |                                                           '_holding_force']
104 |         elif self._type == 'suction':
105 |             self._prm['vacuum_sensor_threshold'] = self._dyn.config[
106 |                 self._ee + '_vacuum_threshold']
107 |             self._prm['blow_off_seconds'] = self._dyn.config[
108 |                 self._ee + '_blow_off']
109 |         self._gripper.set_parameters(parameters=self._prm)
110 | 
111 |     def _update_feedback(self, position):
112 |         if self._type == 'electric':
113 |             self._fdbk.position = self._gripper.position()
114 |             self._fdbk.effort = self._gripper.force()
115 |             self._fdbk.stalled = (self._gripper.force() >
116 |                                   self._gripper.parameters()['moving_force'])
117 |             self._fdbk.reached_goal = (fabs(self._gripper.position() -
118 |                                             position) <
119 |                                        self._gripper.parameters()['dead_zone'])
120 |         if self._type == 'suction':
121 |             self._fdbk.effort = self._gripper.vacuum_sensor()
122 |             if position >= 100.0:
123 |                 self._fdbk.reached_goal = (not self._gripper.sucking() and
124 |                                            not self._gripper.blowing())
125 |             else:
126 |                 self._fdbk.reached_goal = self._gripper.gripping()
127 |         self._result = self._fdbk
128 |         self._server.publish_feedback(self._fdbk)
129 | 
130 |     def _command_gripper(self, position):
131 |         if self._type == 'electric':
132 |             self._gripper.command_position(position, block=False)
133 |         elif self._type == 'suction':
134 |             if position >= 100.0:
135 |                 self._gripper.open(block=False)
136 |             else:
137 |                 # if infinite timeout, command suction for 1 hour
138 |                 if self._timeout < 0.0:
139 |                     self._timeout = 3600.0
140 |                 self._gripper.close(block=False, timeout=self._timeout)
141 | 
142 |     def _check_state(self, position):
143 |         if self._type == 'electric':
144 |             return (self._gripper.force() >
145 |                     self._gripper.parameters()['moving_force'] or
146 |                     fabs(self._gripper.position() - position) <
147 |                     self._gripper.parameters()['dead_zone'])
148 |         elif self._type == 'suction':
149 |             if position >= 100.0:
150 |                 return (not self._gripper.sucking() and
151 |                         not self._gripper.blowing())
152 |             else:
153 |                 return self._gripper.gripping()
154 | 
155 |     def _on_gripper_action(self, goal):
156 |         # Store position and effort from call
157 |         # Position to 0:100 == close:open
158 |         position = goal.command.position
159 |         effort = goal.command.max_effort
160 |         # Apply max effort if specified < 0
161 |         if effort == -1.0:
162 |             effort = 100.0
163 | 
164 |         # Check for errors
165 |         if self._gripper.error():
166 |             rospy.logerr("%s: Gripper error - please restart action server." %
167 |                          (self._action_name,))
168 |             self._server.set_aborted()
169 | 
170 |         # Pull parameters that will define the gripper actuation
171 |         self._get_gripper_parameters()
172 | 
173 |         # Reset feedback/result
174 |         self._update_feedback(position)
175 | 
176 |         # 20 Hz gripper state rate
177 |         control_rate = rospy.Rate(20.0)
178 | 
179 |         # Record start time
180 |         start_time = rospy.get_time()
181 | 
182 |         # Set the moving_force/vacuum_threshold based on max_effort provided
183 |         # If effort not specified (0.0) use parameter server value
184 |         if self._type == 'electric':
185 |             if fabs(effort) < 0.0001:
186 |                 effort = self._prm['moving_force']
187 |             self._gripper.set_moving_force(effort)
188 |         elif self._type == 'suction':
189 |             if fabs(effort) < 0.0001:
190 |                 effort = self._prm['vacuum_sensor_threshold']
191 |             self._gripper.set_vacuum_threshold(effort)
192 | 
193 |         def now_from_start(start):
194 |             return rospy.get_time() - start
195 | 
196 |         # Continue commanding goal until success or timeout
197 |         while ((now_from_start(start_time) < self._timeout or
198 |                self._timeout < 0.0) and not rospy.is_shutdown()):
199 |             if self._server.is_preempt_requested():
200 |                 self._gripper.stop()
201 |                 rospy.loginfo("%s: Gripper Action Preempted" %
202 |                               (self._action_name,))
203 |                 self._server.set_preempted(self._result)
204 |                 return
205 |             self._update_feedback(position)
206 |             if self._check_state(position):
207 |                 self._server.set_succeeded(self._result)
208 |                 return
209 |             self._command_gripper(position)
210 |             control_rate.sleep()
211 | 
212 |         # Gripper failed to achieve goal before timeout/shutdown
213 |         self._gripper.stop()
214 |         if not rospy.is_shutdown():
215 |             rospy.logerr("%s: Gripper Command Not Achieved in Allotted Time" %
216 |                          (self._action_name,))
217 |         self._update_feedback(position)
218 |         self._server.set_aborted(self._result)
219 | 


--------------------------------------------------------------------------------
/src/head_action/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | from .head_action import HeadActionServer
29 | 


--------------------------------------------------------------------------------
/src/head_action/head_action.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | """
 29 | Baxter RSDK Head Action Server
 30 | """
 31 | from math import fabs
 32 | 
 33 | import rospy
 34 | 
 35 | import actionlib
 36 | 
 37 | from control_msgs.msg import (
 38 |     SingleJointPositionAction,
 39 |     SingleJointPositionFeedback,
 40 |     SingleJointPositionResult, 
 41 | )
 42 | 
 43 | import baxter_interface
 44 | from baxter_core_msgs.msg import (
 45 |    HeadPanCommand,
 46 | )
 47 | 
 48 | 
 49 | class HeadActionServer(object):
 50 |     def __init__(self, reconfig_server):
 51 |         self._dyn = reconfig_server
 52 |         self._ns = 'robot/head/head_action'
 53 |         self._head = baxter_interface.Head()
 54 | 
 55 |         # Action Server
 56 |         self._server = actionlib.SimpleActionServer(
 57 |             self._ns,
 58 |             SingleJointPositionAction,
 59 |             execute_cb=self._on_head_action,
 60 |             auto_start=False)
 61 |         self._action_name = rospy.get_name()
 62 |         self._server.start()
 63 | 
 64 |         # Action Feedback/Result
 65 |         self._fdbk = SingleJointPositionFeedback()
 66 |         self._result = SingleJointPositionResult()
 67 | 
 68 |         # Initialize Parameters
 69 |         self._prm = {"dead_zone" : baxter_interface.settings.HEAD_PAN_ANGLE_TOLERANCE}
 70 |         self._timeout = 5.0
 71 | 
 72 |     def _get_head_parameters(self):
 73 |         self._timeout = self._dyn.config['timeout']
 74 |         self._prm['dead_zone'] = self._dyn.config['goal']
 75 | 
 76 |     def _update_feedback(self):
 77 |         self._fdbk.position = self._head.pan()
 78 |         self._result = self._fdbk
 79 |         self._server.publish_feedback(self._fdbk)
 80 | 
 81 |     def _command_head(self, position, speed):
 82 |         self._head.set_pan(position, speed, timeout=self._timeout)
 83 | 
 84 |     def _check_state(self, position):
 85 |         return (fabs(self._head.pan() - position) <
 86 |                 self._prm['dead_zone'])
 87 | 
 88 |     def _on_head_action(self, goal):
 89 |         position = goal.position
 90 |         velocity = goal.max_velocity
 91 |         # Apply max velocity if specified < 0
 92 |         if velocity < HeadPanCommand.MIN_SPEED_RATIO:
 93 |             velocity = HeadPanCommand.MAX_SPEED_RATIO
 94 | 
 95 |         # Pull parameters that will define the head actuation
 96 |         self._get_head_parameters()
 97 | 
 98 |         # Reset feedback/result
 99 |         self._update_feedback()
100 | 
101 |         # 20 Hz head state rate
102 |         control_rate = rospy.Rate(20.0)
103 | 
104 |         # Record start time
105 |         start_time = rospy.get_time()
106 | 
107 |         def now_from_start(start):
108 |             return rospy.get_time() - start
109 | 
110 |         # Continue commanding goal until success or timeout
111 |         while ((now_from_start(start_time) < self._timeout or
112 |                self._timeout < 0.0) and not rospy.is_shutdown()):
113 |             if self._server.is_preempt_requested():
114 |                 rospy.loginfo("%s: Head Action Can't Preempted" %
115 |                               (self._action_name,))
116 |                 self._server.set_preempted(self._result)
117 |                 return
118 |             self._update_feedback()
119 |             if self._check_state(position):
120 |                 self._server.set_succeeded(self._result)
121 |                 return
122 |             self._command_head(position, velocity)
123 |             control_rate.sleep()
124 | 
125 |         if not rospy.is_shutdown():
126 |             rospy.logerr("%s: Head Command Not Achieved in Allotted Time" %
127 |                          (self._action_name,))
128 |         self._update_feedback()
129 |         self._server.set_aborted(self._result)
130 | 


--------------------------------------------------------------------------------
/src/joint_trajectory_action/__init__.py:
--------------------------------------------------------------------------------
 1 | # Copyright (c) 2013-2015, Rethink Robotics
 2 | # All rights reserved.
 3 | #
 4 | # Redistribution and use in source and binary forms, with or without
 5 | # modification, are permitted provided that the following conditions are met:
 6 | #
 7 | # 1. Redistributions of source code must retain the above copyright notice,
 8 | #    this list of conditions and the following disclaimer.
 9 | # 2. Redistributions in binary form must reproduce the above copyright
10 | #    notice, this list of conditions and the following disclaimer in the
11 | #    documentation and/or other materials provided with the distribution.
12 | # 3. Neither the name of the Rethink Robotics nor the names of its
13 | #    contributors may be used to endorse or promote products derived from
14 | #    this software without specific prior written permission.
15 | #
16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 | # POSSIBILITY OF SUCH DAMAGE.
27 | 
28 | from .joint_trajectory_action import JointTrajectoryActionServer
29 | 


--------------------------------------------------------------------------------
/src/joint_trajectory_action/bezier.py:
--------------------------------------------------------------------------------
  1 | #! /usr/bin/python
  2 | # Software License Agreement (BSD License)
  3 | #
  4 | # Copyright (c) 2013-2015, Rethink Robotics
  5 | # All rights reserved.
  6 | #
  7 | # Copyright (c) 2011, Ian McMahon
  8 | # All rights reserved.
  9 | #
 10 | # Redistribution and use in source and binary forms, with or without
 11 | # modification, are permitted provided that the following conditions
 12 | # are met:
 13 | #
 14 | #  * Redistributions of source code must retain the above copyright
 15 | #    notice, this list of conditions and the following disclaimer.
 16 | #  * Redistributions in binary form must reproduce the above
 17 | #    copyright notice, this list of conditions and the following
 18 | #    disclaimer in the documentation and/or other materials provided
 19 | #    with the distribution.
 20 | #  * Neither the name of Ian McMahon nor the names of its
 21 | #    contributors may be used to endorse or promote products derived
 22 | #    from this software without specific prior written permission.
 23 | #
 24 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 25 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 26 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 27 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 28 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 29 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 30 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 31 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 32 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 33 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 34 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 35 | # POSSIBILITY OF SUCH DAMAGE.
 36 | #
 37 | 
 38 | """
 39 | The Bezier library was  implemented as a class project in CIS515,
 40 | Fundamentals of Linear Algebra, taught by Professor Jean Gallier
 41 | in the summer of 2011 at the University of Pennsylvania. For an
 42 | excellent explanation of Cubic Bezier Curves, and the math
 43 | represented in this library, see
 44 | http://www.cis.upenn.edu/~cis515/proj1-12.pdf
 45 | 
 46 | ~~~~~~~~~~~~~~~~~~~~~~~~ Bezier ~~~~~~~~~~~~~~~~~~~~~~~~
 47 | A library for computing Bezier Cubic Splines for an arbitrary
 48 | set of control points in R2, R3, up to RN space.
 49 | 
 50 | Cubic Segment:
 51 | C(t) = (1 - t)^3*b0 + 3(1 - t)*b1 + 3(1 - t)*t^2*b2 + t^3*b3
 52 | 
 53 | Bezier Spline of Cubic Segments:
 54 | B(t) = C_(i)(t-i+1), i-1 <= t <= i
 55 | where C0 continuity exists: C_(i)(1) = C_(i+1)(0)
 56 | where C1 continuity exists: C'_(i)(1) = C'_(i+1)(0)
 57 | and where C2 continuity exists: C"_(i)(1) = C"_(i+1)(0)
 58 | 
 59 | ex. usage:
 60 | import numpy
 61 | import bezier
 62 | points_array = numpy.array([[1, 2, 3], [4, 4, 4],
 63 |                             [6, 4, 6], [2, 5, 6],
 64 |                             [5, 6, 7]])
 65 | d_pts = bezier.de_boor_control_pts(points_array)
 66 | b_coeffs = bezier.bezier_coefficients(points_array, d_pts)
 67 | b_curve = bezier.bezier_curve(b_coeffs, 50)
 68 | #  plotting example
 69 | import matplotlib.pyplot as plt
 70 | from mpl_toolkits.mplot3d import Axes3D
 71 | fig = plt.figure()
 72 | ax = fig.gca(projection='3d')
 73 | #plot bezier curve
 74 | ax.plot(b_curve[:,0], b_curve[:,1], b_curve[:,2])
 75 | #plot specified points
 76 | ax.plot(points_array[:,0], points_array[:,1], points_array[:,2], 'g*')
 77 | ax.set_title("Cubic Bezier Spline")
 78 | ax.set_xlabel("X")
 79 | ax.set_ylabel("Y")
 80 | ax.set_zlabel("Z")
 81 | ax.legend(["Bezier Curve", "Control Points"], loc=2)
 82 | plt.show()
 83 | """
 84 | import numpy as np
 85 | 
 86 | 
 87 | def de_boor_control_pts(points_array, d0=None,
 88 |                         dN=None, natural=True):
 89 |     """
 90 |     Compute the de Boor control points for a given
 91 |     set for control points
 92 | 
 93 |     params:
 94 |         points_array: array of user-supplied control points
 95 |             numpy.array of size N by k
 96 |             N is the number of input control points
 97 |             k is the number of dimensions for each point
 98 | 
 99 |         d0: the first control point - None if "natural"
100 |             numpy.array of size 1 by k
101 | 
102 |         dN: the last control point - None if "natural"
103 |             numpy.array of size 1 by k
104 | 
105 |         natural: flag to signify natural start/end conditions
106 |             bool
107 | 
108 |     returns:
109 |         d_pts: array of de Boor control points
110 |             numpy.array of size N+3 by k
111 |     """
112 |     # N+3 auxiliary points required to compute d_pts
113 |     # dpts_(-1) = x_(0)
114 |     # dpts_(N+1) = x_(N)
115 |     # so it is only necessary to find N+1 pts, dpts_(0) to to dpts_(N)
116 |     (rows, k) = np.shape(points_array)
117 |     N = rows - 1  # minus 1 because list includes x_(0)
118 |     # Compute A matrix
119 |     if natural:
120 |         if N > 2:
121 |             A = np.zeros((N-1, N-1))
122 |             A[np.ix_([0], [0, 1])] = [4, 1]
123 |             A[np.ix_([N-2], [N-3, N-2])] = [1, 4]
124 |         else:
125 |             A = 4.0
126 |     else:
127 |         if N > 2:
128 |             A = np.zeros((N-1, N-1))
129 |             A[np.ix_([0], [0, 1])] = [3.5, 1]
130 |             A[np.ix_([N-2], [N-3, N-2])] = [1, 3.5]
131 |         else:
132 |             A = 3.5
133 |     for i in range(1, N-2):
134 |         A[np.ix_([i], [i-1, i, i+1])] = [1, 4, 1]
135 |     # Construct de Boor Control Points from A matrix
136 |     d_pts = np.zeros((N+3, k))
137 |     for col in range(0, k):
138 |         x = np.zeros((max(N-1, 1), 1))
139 |         if N > 2:
140 |             # Compute start / end conditions
141 |             if natural:
142 |                 x[N-2, 0] = 6*points_array[-2, col] - points_array[-1, col]
143 |                 x[0, 0] = 6*points_array[1, col] - points_array[0, col]
144 |             else:
145 |                 x[N-2, 0] = 6*points_array[-2, col] - 1.5*dN[0, col]
146 |                 x[0, 0] = 6*points_array[1, col] - 1.5*d0[0, col]
147 |             x[range(1, N-3+1), 0] = 6*points_array[range(2, N-2+1), col]
148 |             # Solve bezier interpolation
149 |             d_pts[2:N+1, col] = np.linalg.solve(A, x).T
150 |         else:
151 |             # Compute start / end conditions
152 |             if natural:
153 |                 x[0, 0] = 6*points_array[1, col] - points_array[0, col]
154 |             else:
155 |                 x[0, 0] = 6*points_array[1, col] - 1.5*d0[col]
156 |             # Solve bezier interpolation
157 |             d_pts[2, col] = x / A
158 |     # Store off start and end positions
159 |     d_pts[0, :] = points_array[0, :]
160 |     d_pts[-1, :] = points_array[-1, :]
161 |     # Compute the second to last de Boor point based on end conditions
162 |     if natural:
163 |         one_third = (1.0/3.0)
164 |         two_thirds = (2.0/3.0)
165 |         d_pts[1, :] = (two_thirds)*points_array[0, :] + (one_third)*d_pts[2, :]
166 |         d_pts[N+1, :] = ((one_third)*d_pts[-3, :] +
167 |                          (two_thirds)*points_array[-1, :])
168 |     else:
169 |         d_pts[1, :] = d0
170 |         d_pts[N+1, :] = dN
171 |     return d_pts
172 | 
173 | 
174 | def bezier_coefficients(points_array, d_pts):
175 |     """
176 |     Compute the Bezier coefficients for a given
177 |     set for user-supplied control pts and
178 |     de Boor control pts.
179 | 
180 |     These B coeffs are used to compute the cubic
181 |     splines for each cubic spline segment as
182 |     follows (where t is a percentage of time between
183 |     b_coeff segments):
184 |     C(t) = (1 - t)^3*b0 + 3(1 - t)*b1
185 |             + 3(1 - t)*t^2*b2 + t^3*b3
186 | 
187 |     params:
188 |         points_array: array of user-supplied control points
189 |             numpy.array of size N by k
190 |             N is the number of control points
191 |             k is the number of dimensions for each point
192 | 
193 |         d_pts: array of de Boor control points
194 |             numpy.array of size N+3 by k
195 | 
196 |     returns:
197 |         b_coeffs: k-dimensional array of 4 Bezier coefficients
198 |             for every control point
199 |             numpy.array of size N by 4 by k
200 |     """
201 |     (rows, k) = np.shape(points_array)
202 |     N = rows - 1  # N minus 1 because points array includes x_0
203 |     b_coeffs = np.zeros(shape=(k, N, 4))
204 |     for i in range(0, N):
205 |         points_array_i = i+1
206 |         d_pts_i = i + 2
207 |         if i == 0:
208 |             for axis_pos in range(0, k):
209 |                 b_coeffs[axis_pos, i, 0] = points_array[points_array_i - 1,
210 |                                                         axis_pos]
211 |                 b_coeffs[axis_pos, i, 1] = d_pts[d_pts_i - 1, axis_pos]
212 |                 b_coeffs[axis_pos, i, 2] = (0.5 * d_pts[d_pts_i - 1, axis_pos]
213 |                                             + 0.5 * d_pts[d_pts_i, axis_pos])
214 |                 b_coeffs[axis_pos, i, 3] = points_array[points_array_i,
215 |                                                         axis_pos]
216 |         elif i == N-1:
217 |             for axis_pos in range(0, k):
218 |                 b_coeffs[axis_pos, i, 0] = points_array[points_array_i - 1,
219 |                                                         axis_pos]
220 |                 b_coeffs[axis_pos, i, 1] = (0.5 * d_pts[d_pts_i - 1, axis_pos]
221 |                                             + 0.5 * d_pts[d_pts_i, axis_pos])
222 |                 b_coeffs[axis_pos, i, 2] = d_pts[d_pts_i, axis_pos]
223 |                 b_coeffs[axis_pos, i, 3] = points_array[points_array_i,
224 |                                                         axis_pos]
225 |         else:
226 |             for axis_pos in range(0, k):
227 |                 b_coeffs[axis_pos, i, 0] = points_array[points_array_i - 1,
228 |                                                         axis_pos]
229 |                 b_coeffs[axis_pos, i, 1] = (2.0/3.0 * d_pts[d_pts_i - 1,
230 |                                                             axis_pos]
231 |                                             + 1.0/3.0 * d_pts[d_pts_i,
232 |                                                               axis_pos])
233 |                 b_coeffs[axis_pos, i, 2] = (1.0/3.0 * d_pts[d_pts_i - 1,
234 |                                                             axis_pos]
235 |                                             + 2.0/3.0 * d_pts[d_pts_i,
236 |                                                               axis_pos])
237 |                 b_coeffs[axis_pos, i, 3] = points_array[points_array_i,
238 |                                                         axis_pos]
239 | 
240 |     return b_coeffs
241 | 
242 | 
243 | def _cubic_spline_point(b_coeff, t):
244 |     """
245 |     Internal convenience function for calculating
246 |     a k-dimensional point defined by the supplied
247 |     Bezier coefficients. Finds the point that
248 |     describes the current position along the bezier
249 |     segment for k dimensions.
250 | 
251 |     params:
252 |         b_coeff => b0...b3: Four k-dimensional Bezier
253 |             coefficients each one is a numpy.array
254 |             of size k by 1, so
255 |             b_coeff is a numpy array of size k by 4
256 |             k is the number of dimensions for each
257 |             coefficient
258 |         t: percentage of time elapsed for this segment
259 |             0 <= int <= 1.0
260 | 
261 |     returns:
262 |         current position in k dimensions
263 |             numpy.array of size 1 by k
264 |     """
265 |     return (pow((1-t), 3)*b_coeff[:, 0] +
266 |             3*pow((1-t), 2)*t*b_coeff[:, 1] +
267 |             3*(1-t)*pow(t, 2)*b_coeff[:, 2] +
268 |             pow(t, 3)*b_coeff[:, 3]
269 |             )
270 | 
271 | 
272 | def bezier_point(b_coeffs, b_index, t):
273 |     """
274 |     Finds the k values that describe the current
275 |     position along the bezier curve for k dimensions.
276 | 
277 |     params:
278 |         b_coeffs: k-dimensional array
279 |             for every control point with 4 Bezier coefficients
280 |             numpy.array of size k by N by 4
281 |             N is the number of control points
282 |             k is the number of dimensions for each point
283 |         b_index: index position out between two of
284 |             the N b_coeffs for this point in time
285 |             int
286 |         t: percentage of time that has passed between
287 |             the two control points
288 |             0 <= int <= 1.0
289 | 
290 |     returns:
291 |         b_point: current position in k dimensions
292 |             numpy.array of size 1 by k
293 |     """
294 |     if b_index <= 0:
295 |         b_point = b_coeffs[:, 0, 0]
296 |     elif b_index > b_coeffs.shape[1]:
297 |         b_point = b_coeffs[:, -1, -1]
298 |     else:
299 |         t = 0.0 if t < 0.0 else t
300 |         t = 1.0 if t > 1.0 else t
301 |         b_coeff_set = b_coeffs[:, b_index-1, range(4)]
302 |         b_point = _cubic_spline_point(b_coeff_set, t)
303 |     return b_point
304 | 
305 | 
306 | def bezier_curve(b_coeffs, num_intervals):
307 |     """
308 |     Iterpolation of the entire Bezier curve at once,
309 |     using a specified number of intervals between
310 |     control points (encapsulated by b_coeffs).
311 | 
312 |     params:
313 |         b_coeffs: k-dimensional array of 4 Bezier coefficients
314 |             for every control point
315 |             numpy.array of size N by 4 by k
316 |             N is the number of control points
317 |             k is the number of dimensions for each point
318 |         num_intervals: the number of intervals between
319 |             control points
320 |             int > 0
321 | 
322 |     returns:
323 |         b_curve: positions along the bezier curve in k-dimensions
324 |             numpy.array of size N*num_interval+1  by k
325 |             (the +1 is to include the start position on the curve)
326 |     """
327 |     assert num_intervals > 0,\
328 |         "Invalid number of intervals chosen (must be greater than 0)"
329 |     interval = 1.0 / num_intervals
330 |     (num_axes, num_bpts, _) = np.shape(b_coeffs)
331 |     b_curve = np.zeros((num_bpts*num_intervals+1, num_axes))
332 |     # Copy out initial point
333 |     b_curve[0, :] = b_coeffs[:, 0, 0]
334 |     for current_bpt in range(num_bpts):
335 |             b_coeff_set = b_coeffs[:, current_bpt, range(4)]
336 |             for iteration, t in enumerate(np.linspace(interval, 1,
337 |                                                       num_intervals)):
338 |                 b_curve[(current_bpt *
339 |                          num_intervals +
340 |                          iteration+1), :] = _cubic_spline_point(b_coeff_set, t)
341 |     return b_curve
342 | 


--------------------------------------------------------------------------------
/src/joint_trajectory_action/joint_trajectory_action.py:
--------------------------------------------------------------------------------
  1 | # Copyright (c) 2013-2015, Rethink Robotics
  2 | # All rights reserved.
  3 | #
  4 | # Redistribution and use in source and binary forms, with or without
  5 | # modification, are permitted provided that the following conditions are met:
  6 | #
  7 | # 1. Redistributions of source code must retain the above copyright notice,
  8 | #    this list of conditions and the following disclaimer.
  9 | # 2. Redistributions in binary form must reproduce the above copyright
 10 | #    notice, this list of conditions and the following disclaimer in the
 11 | #    documentation and/or other materials provided with the distribution.
 12 | # 3. Neither the name of the Rethink Robotics nor the names of its
 13 | #    contributors may be used to endorse or promote products derived from
 14 | #    this software without specific prior written permission.
 15 | #
 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 26 | # POSSIBILITY OF SUCH DAMAGE.
 27 | 
 28 | """
 29 | Baxter RSDK Joint Trajectory Action Server
 30 | """
 31 | import bisect
 32 | from copy import deepcopy
 33 | import math
 34 | import operator
 35 | import numpy as np
 36 | 
 37 | import bezier
 38 | 
 39 | import rospy
 40 | 
 41 | import actionlib
 42 | 
 43 | from control_msgs.msg import (
 44 |     FollowJointTrajectoryAction,
 45 |     FollowJointTrajectoryFeedback,
 46 |     FollowJointTrajectoryResult,
 47 | )
 48 | from std_msgs.msg import (
 49 |     UInt16,
 50 | )
 51 | from trajectory_msgs.msg import (
 52 |     JointTrajectoryPoint,
 53 | )
 54 | 
 55 | import baxter_control
 56 | import baxter_dataflow
 57 | import baxter_interface
 58 | 
 59 | 
 60 | class JointTrajectoryActionServer(object):
 61 |     def __init__(self, limb, reconfig_server, rate=100.0,
 62 |                  mode='position_w_id'):
 63 |         self._dyn = reconfig_server
 64 |         self._ns = 'robot/limb/' + limb
 65 |         self._fjt_ns = self._ns + '/follow_joint_trajectory'
 66 |         self._server = actionlib.SimpleActionServer(
 67 |             self._fjt_ns,
 68 |             FollowJointTrajectoryAction,
 69 |             execute_cb=self._on_trajectory_action,
 70 |             auto_start=False)
 71 |         self._action_name = rospy.get_name()
 72 |         self._limb = baxter_interface.Limb(limb)
 73 |         self._enable = baxter_interface.RobotEnable()
 74 |         self._name = limb
 75 |         self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb,))
 76 |         self._cuff.state_changed.connect(self._cuff_cb)
 77 |         # Verify joint control mode
 78 |         self._mode = mode
 79 |         if (self._mode != 'position' and self._mode != 'position_w_id'
 80 |             and self._mode != 'velocity'):
 81 |             rospy.logerr("%s: Action Server Creation Failed - "
 82 |                          "Provided Invalid Joint Control Mode '%s' (Options: "
 83 |                          "'position_w_id', 'position', 'velocity')" %
 84 |                     (self._action_name, self._mode,))
 85 |             return
 86 |         self._server.start()
 87 |         self._alive = True
 88 |         self._cuff_state = False
 89 |         # Action Feedback/Result
 90 |         self._fdbk = FollowJointTrajectoryFeedback()
 91 |         self._result = FollowJointTrajectoryResult()
 92 | 
 93 |         # Controller parameters from arguments, messages, and dynamic
 94 |         # reconfigure
 95 |         self._control_rate = rate  # Hz
 96 |         self._control_joints = []
 97 |         self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
 98 |         self._goal_time = 0.0
 99 |         self._stopped_velocity = 0.0
100 |         self._goal_error = dict()
101 |         self._path_thresh = dict()
102 | 
103 |         # Create our PID controllers
104 |         self._pid = dict()
105 |         for joint in self._limb.joint_names():
106 |             self._pid[joint] = baxter_control.PID()
107 | 
108 |         # Create our spline coefficients
109 |         self._coeff = [None] * len(self._limb.joint_names())
110 | 
111 |         # Set joint state publishing to specified control rate
112 |         self._pub_rate = rospy.Publisher(
113 |             '/robot/joint_state_publish_rate',
114 |              UInt16,
115 |              queue_size=10)
116 |         self._pub_rate.publish(self._control_rate)
117 | 
118 |         self._pub_ff_cmd = rospy.Publisher(
119 |             self._ns + '/inverse_dynamics_command',
120 |             JointTrajectoryPoint,
121 |             tcp_nodelay=True,
122 |             queue_size=1)
123 | 
124 |     def robot_is_enabled(self):
125 |         return self._enable.state().enabled
126 | 
127 |     def clean_shutdown(self):
128 |         self._alive = False
129 |         self._limb.exit_control_mode()
130 | 
131 |     def _cuff_cb(self, value):
132 |         self._cuff_state = value
133 | 
134 |     def _get_trajectory_parameters(self, joint_names, goal):
135 |         # For each input trajectory, if path, goal, or goal_time tolerances
136 |         # provided, we will use these as opposed to reading from the
137 |         # parameter server/dynamic reconfigure
138 | 
139 |         # Goal time tolerance - time buffer allowing goal constraints to be met
140 |         if goal.goal_time_tolerance:
141 |             self._goal_time = goal.goal_time_tolerance.to_sec()
142 |         else:
143 |             self._goal_time = self._dyn.config['goal_time']
144 |         # Stopped velocity tolerance - max velocity at end of execution
145 |         self._stopped_velocity = self._dyn.config['stopped_velocity_tolerance']
146 | 
147 |         # Path execution and goal tolerances per joint
148 |         for jnt in joint_names:
149 |             if jnt not in self._limb.joint_names():
150 |                 rospy.logerr(
151 |                     "%s: Trajectory Aborted - Provided Invalid Joint Name %s" %
152 |                     (self._action_name, jnt,))
153 |                 self._result.error_code = self._result.INVALID_JOINTS
154 |                 self._server.set_aborted(self._result)
155 |                 return
156 |             # Path execution tolerance
157 |             path_error = self._dyn.config[jnt + '_trajectory']
158 |             if goal.path_tolerance:
159 |                 for tolerance in goal.path_tolerance:
160 |                     if jnt == tolerance.name:
161 |                         if tolerance.position != 0.0:
162 |                             self._path_thresh[jnt] = tolerance.position
163 |                         else:
164 |                             self._path_thresh[jnt] = path_error
165 |             else:
166 |                 self._path_thresh[jnt] = path_error
167 |             # Goal error tolerance
168 |             goal_error = self._dyn.config[jnt + '_goal']
169 |             if goal.goal_tolerance:
170 |                 for tolerance in goal.goal_tolerance:
171 |                     if jnt == tolerance.name:
172 |                         if tolerance.position != 0.0:
173 |                             self._goal_error[jnt] = tolerance.position
174 |                         else:
175 |                             self._goal_error[jnt] = goal_error
176 |             else:
177 |                 self._goal_error[jnt] = goal_error
178 | 
179 |             # PID gains if executing using the velocity (integral) controller
180 |             if self._mode == 'velocity':
181 |                 self._pid[jnt].set_kp(self._dyn.config[jnt + '_kp'])
182 |                 self._pid[jnt].set_ki(self._dyn.config[jnt + '_ki'])
183 |                 self._pid[jnt].set_kd(self._dyn.config[jnt + '_kd'])
184 |                 self._pid[jnt].initialize()
185 | 
186 |     def _get_current_position(self, joint_names):
187 |         return [self._limb.joint_angle(joint) for joint in joint_names]
188 | 
189 |     def _get_current_velocities(self, joint_names):
190 |         return [self._limb.joint_velocity(joint) for joint in joint_names]
191 | 
192 |     def _get_current_error(self, joint_names, set_point):
193 |         current = self._get_current_position(joint_names)
194 |         error = map(operator.sub, set_point, current)
195 |         return zip(joint_names, error)
196 | 
197 |     def _update_feedback(self, cmd_point, jnt_names, cur_time):
198 |         self._fdbk.header.stamp = rospy.Duration.from_sec(rospy.get_time())
199 |         self._fdbk.joint_names = jnt_names
200 |         self._fdbk.desired = cmd_point
201 |         self._fdbk.desired.time_from_start = rospy.Duration.from_sec(cur_time)
202 |         self._fdbk.actual.positions = self._get_current_position(jnt_names)
203 |         self._fdbk.actual.time_from_start = rospy.Duration.from_sec(cur_time)
204 |         self._fdbk.error.positions = map(operator.sub,
205 |                                          self._fdbk.desired.positions,
206 |                                          self._fdbk.actual.positions
207 |                                         )
208 |         self._fdbk.error.time_from_start = rospy.Duration.from_sec(cur_time)
209 |         self._server.publish_feedback(self._fdbk)
210 | 
211 |     def _reorder_joints_ff_cmd(self, joint_names, point):
212 | 	joint_name_order = self._limb.joint_names()
213 | 	pnt = JointTrajectoryPoint()
214 | 	pnt.time_from_start = point.time_from_start
215 | 	pos_cmd = dict(zip(joint_names, point.positions))
216 | 	for jnt_name in joint_name_order:
217 | 	    pnt.positions.append(pos_cmd[jnt_name])
218 |         if point.velocities:
219 | 	    vel_cmd = dict(zip(joint_names, point.velocities))
220 | 	    for jnt_name in joint_name_order:
221 | 	        pnt.velocities.append(vel_cmd[jnt_name])
222 |         if point.accelerations:
223 | 	    accel_cmd = dict(zip(joint_names, point.accelerations))
224 | 	    for jnt_name in joint_name_order:
225 | 	        pnt.accelerations.append(accel_cmd[jnt_name])
226 |         return pnt
227 | 
228 |     def _command_stop(self, joint_names, joint_angles, start_time, dimensions_dict):
229 |         if self._mode == 'velocity':
230 |             velocities = [0.0] * len(joint_names)
231 |             cmd = dict(zip(joint_names, velocities))
232 |             while (not self._server.is_new_goal_available() and self._alive
233 |                    and self.robot_is_enabled()):
234 |                 self._limb.set_joint_velocities(cmd)
235 |                 if self._cuff_state:
236 |                     self._limb.exit_control_mode()
237 |                     break
238 |                 rospy.sleep(1.0 / self._control_rate)
239 |         elif self._mode == 'position' or self._mode == 'position_w_id':
240 |             raw_pos_mode = (self._mode == 'position_w_id')
241 |             if raw_pos_mode:
242 |                 pnt = JointTrajectoryPoint()
243 |                 pnt.positions = self._get_current_position(joint_names)
244 |                 if dimensions_dict['velocities']:
245 |                     pnt.velocities = [0.0] * len(joint_names)
246 |                 if dimensions_dict['accelerations']:
247 |                     pnt.accelerations = [0.0] * len(joint_names)
248 |             while (not self._server.is_new_goal_available() and self._alive
249 |                    and self.robot_is_enabled()):
250 |                 self._limb.set_joint_positions(joint_angles, raw=raw_pos_mode)
251 |                 # zero inverse dynamics feedforward command
252 |                 if self._mode == 'position_w_id':
253 |                     pnt.time_from_start = rospy.Duration(rospy.get_time() - start_time)
254 |                     ff_pnt = self._reorder_joints_ff_cmd(joint_names, pnt)
255 |                     self._pub_ff_cmd.publish(ff_pnt)
256 |                 if self._cuff_state:
257 |                     self._limb.exit_control_mode()
258 |                     break
259 |                 rospy.sleep(1.0 / self._control_rate)
260 | 
261 |     def _command_joints(self, joint_names, point, start_time, dimensions_dict):
262 |         if self._server.is_preempt_requested() or not self.robot_is_enabled():
263 |             rospy.loginfo("%s: Trajectory Preempted" % (self._action_name,))
264 |             self._server.set_preempted()
265 |             self._command_stop(joint_names, self._limb.joint_angles(), start_time, dimensions_dict)
266 |             return False
267 |         velocities = []
268 |         deltas = self._get_current_error(joint_names, point.positions)
269 |         for delta in deltas:
270 |             if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]]
271 |                 and self._path_thresh[delta[0]] >= 0.0)) or not self.robot_is_enabled():
272 |                 rospy.logerr("%s: Exceeded Error Threshold on %s: %s" %
273 |                              (self._action_name, delta[0], str(delta[1]),))
274 |                 self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED
275 |                 self._server.set_aborted(self._result)
276 |                 self._command_stop(joint_names, self._limb.joint_angles(), start_time, dimensions_dict)
277 |                 return False
278 |             if self._mode == 'velocity':
279 |                 velocities.append(self._pid[delta[0]].compute_output(delta[1]))
280 |         if ((self._mode == 'position' or self._mode == 'position_w_id')
281 |               and self._alive):
282 |             cmd = dict(zip(joint_names, point.positions))
283 |             raw_pos_mode = (self._mode == 'position_w_id')
284 |             self._limb.set_joint_positions(cmd, raw=raw_pos_mode)
285 |             if raw_pos_mode:
286 |                 ff_pnt = self._reorder_joints_ff_cmd(joint_names, point)
287 |                 self._pub_ff_cmd.publish(ff_pnt)
288 |         elif self._alive:
289 |             cmd = dict(zip(joint_names, velocities))
290 |             self._limb.set_joint_velocities(cmd)
291 |         return True
292 | 
293 |     def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):
294 |         pnt = JointTrajectoryPoint()
295 |         pnt.time_from_start = rospy.Duration(cmd_time)
296 |         num_joints = b_matrix.shape[0]
297 |         pnt.positions = [0.0] * num_joints
298 |         if dimensions_dict['velocities']:
299 |             pnt.velocities = [0.0] * num_joints
300 |         if dimensions_dict['accelerations']:
301 |             pnt.accelerations = [0.0] * num_joints
302 |         for jnt in range(num_joints):
303 |             b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
304 |             # Positions at specified time
305 |             pnt.positions[jnt] = b_point[0]
306 |             # Velocities at specified time
307 |             if dimensions_dict['velocities']:
308 |                 pnt.velocities[jnt] = b_point[1]
309 |             # Accelerations at specified time
310 |             if dimensions_dict['accelerations']:
311 |                 pnt.accelerations[jnt] = b_point[-1]
312 |         return pnt
313 | 
314 |     def _compute_bezier_coeff(self, joint_names, trajectory_points, dimensions_dict):
315 |         # Compute Full Bezier Curve
316 |         num_joints = len(joint_names)
317 |         num_traj_pts = len(trajectory_points)
318 |         num_traj_dim = sum(dimensions_dict.values())
319 |         num_b_values = len(['b0', 'b1', 'b2', 'b3'])
320 |         b_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts-1, num_b_values))
321 |         for jnt in xrange(num_joints):
322 |             traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim))
323 |             for idx, point in enumerate(trajectory_points):
324 |                 current_point = list()
325 |                 current_point.append(point.positions[jnt])
326 |                 if dimensions_dict['velocities']:
327 |                     current_point.append(point.velocities[jnt])
328 |                 if dimensions_dict['accelerations']:
329 |                     current_point.append(point.accelerations[jnt])
330 |                 traj_array[idx, :] = current_point
331 |             d_pts = bezier.de_boor_control_pts(traj_array)
332 |             b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(traj_array, d_pts)
333 |         return b_matrix
334 | 
335 |     def _determine_dimensions(self, trajectory_points):
336 |         # Determine dimensions supplied
337 |         position_flag = True
338 |         velocity_flag = (len(trajectory_points[0].velocities) != 0 and
339 |                          len(trajectory_points[-1].velocities) != 0)
340 |         acceleration_flag = (len(trajectory_points[0].accelerations) != 0 and
341 |                              len(trajectory_points[-1].accelerations) != 0)
342 |         return {'positions':position_flag,
343 |                 'velocities':velocity_flag,
344 |                 'accelerations':acceleration_flag}
345 | 
346 |     def _on_trajectory_action(self, goal):
347 |         joint_names = goal.trajectory.joint_names
348 |         trajectory_points = goal.trajectory.points
349 |         # Load parameters for trajectory
350 |         self._get_trajectory_parameters(joint_names, goal)
351 |         # Create a new discretized joint trajectory
352 |         num_points = len(trajectory_points)
353 |         if num_points == 0:
354 |             rospy.logerr("%s: Empty Trajectory" % (self._action_name,))
355 |             self._server.set_aborted()
356 |             return
357 |         rospy.loginfo("%s: Executing requested joint trajectory" %
358 |                       (self._action_name,))
359 |         rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
360 |         control_rate = rospy.Rate(self._control_rate)
361 | 
362 |         dimensions_dict = self._determine_dimensions(trajectory_points)
363 | 
364 |         if num_points == 1:
365 |             # Add current position as trajectory point
366 |             first_trajectory_point = JointTrajectoryPoint()
367 |             first_trajectory_point.positions = self._get_current_position(joint_names)
368 |             # To preserve desired velocities and accelerations, copy them to the first
369 |             # trajectory point if the trajectory is only 1 point.
370 |             if dimensions_dict['velocities']:
371 |                 first_trajectory_point.velocities = deepcopy(trajectory_points[0].velocities)
372 |             if dimensions_dict['accelerations']:
373 |                 first_trajectory_point.accelerations = deepcopy(trajectory_points[0].accelerations)
374 |             first_trajectory_point.time_from_start = rospy.Duration(0)
375 |             trajectory_points.insert(0, first_trajectory_point)
376 |             num_points = len(trajectory_points)
377 | 
378 |         # Force Velocites/Accelerations to zero at the final timestep
379 |         # if they exist in the trajectory
380 |         # Remove this behavior if you are stringing together trajectories,
381 |         # and want continuous, non-zero velocities/accelerations between
382 |         # trajectories
383 |         if dimensions_dict['velocities']:
384 |             trajectory_points[-1].velocities = [0.0] * len(joint_names)
385 |         if dimensions_dict['accelerations']:
386 |             trajectory_points[-1].accelerations = [0.0] * len(joint_names)
387 | 
388 |         # Compute Full Bezier Curve Coefficients for all 7 joints
389 |         pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
390 |         try:
391 |             b_matrix = self._compute_bezier_coeff(joint_names,
392 |                                                   trajectory_points,
393 |                                                   dimensions_dict)
394 |         except Exception as ex:
395 |             rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
396 |                          " arm with error \"{2}: {3}\"").format(
397 |                                                   self._action_name,
398 |                                                   self._name,
399 |                                                   type(ex).__name__, ex))
400 |             self._server.set_aborted()
401 |             return
402 |         # Wait for the specified execution time, if not provided use now
403 |         start_time = goal.trajectory.header.stamp.to_sec()
404 |         if start_time == 0.0:
405 |             start_time = rospy.get_time()
406 |         baxter_dataflow.wait_for(
407 |             lambda: rospy.get_time() >= start_time,
408 |             timeout=float('inf')
409 |         )
410 |         # Loop until end of trajectory time.  Provide a single time step
411 |         # of the control rate past the end to ensure we get to the end.
412 |         # Keep track of current indices for spline segment generation
413 |         now_from_start = rospy.get_time() - start_time
414 |         end_time = trajectory_points[-1].time_from_start.to_sec()
415 |         while (now_from_start < end_time and not rospy.is_shutdown() and
416 |                self.robot_is_enabled()):
417 |             #Acquire Mutex
418 |             now = rospy.get_time()
419 |             now_from_start = now - start_time
420 |             idx = bisect.bisect(pnt_times, now_from_start)
421 |             #Calculate percentage of time passed in this interval
422 |             if idx >= num_points:
423 |                 cmd_time = now_from_start - pnt_times[-1]
424 |                 t = 1.0
425 |             elif idx >= 0:
426 |                 cmd_time = (now_from_start - pnt_times[idx-1])
427 |                 t = cmd_time / (pnt_times[idx] - pnt_times[idx-1])
428 |             else:
429 |                 cmd_time = 0
430 |                 t = 0
431 | 
432 | 	    point = self._get_bezier_point(b_matrix, idx,
433 |                                            t, cmd_time,
434 | 				           dimensions_dict)
435 | 
436 |             # Command Joint Position, Velocity, Acceleration
437 |             command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)
438 |             self._update_feedback(deepcopy(point), joint_names, now_from_start)
439 |             # Release the Mutex
440 |             if not command_executed:
441 |                 return
442 |             control_rate.sleep()
443 |         # Keep trying to meet goal until goal_time constraint expired
444 |         last = trajectory_points[-1]
445 |         last_time = trajectory_points[-1].time_from_start.to_sec()
446 |         end_angles = dict(zip(joint_names, last.positions))
447 | 
448 |         def check_goal_state():
449 |             for error in self._get_current_error(joint_names, last.positions):
450 |                 if (self._goal_error[error[0]] > 0
451 |                         and self._goal_error[error[0]] < math.fabs(error[1])):
452 |                     return error[0]
453 |             if (self._stopped_velocity > 0.0 and
454 |                 max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)]) >
455 |                     self._stopped_velocity):
456 |                 return False
457 |             else:
458 |                 return True
459 | 
460 |         while (now_from_start < (last_time + self._goal_time)
461 |                and not rospy.is_shutdown() and self.robot_is_enabled()):
462 |             if not self._command_joints(joint_names, last, start_time, dimensions_dict):
463 |                 return
464 |             now_from_start = rospy.get_time() - start_time
465 |             self._update_feedback(deepcopy(last), joint_names,
466 |                                   now_from_start)
467 |             control_rate.sleep()
468 | 
469 |         now_from_start = rospy.get_time() - start_time
470 |         self._update_feedback(deepcopy(last), joint_names,
471 |                                   now_from_start)
472 | 
473 |         # Verify goal constraint
474 |         result = check_goal_state()
475 |         if result is True:
476 |             rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" %
477 |                           (self._action_name, self._name))
478 |             self._result.error_code = self._result.SUCCESSFUL
479 |             self._server.set_succeeded(self._result)
480 |         elif result is False:
481 |             rospy.logerr("%s: Exceeded Max Goal Velocity Threshold for %s arm" %
482 |                          (self._action_name, self._name))
483 |             self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
484 |             self._server.set_aborted(self._result)
485 |         else:
486 |             rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" %
487 |                          (self._action_name, result, self._name))
488 |             self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
489 |             self._server.set_aborted(self._result)
490 |         self._command_stop(goal.trajectory.joint_names, end_angles, start_time, dimensions_dict)
491 | 


--------------------------------------------------------------------------------