joint_torque_springs.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2015, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 """
00030 Baxter RSDK Joint Torque Example: joint springs
00031 """
00032 
00033 import argparse
00034 
00035 import rospy
00036 
00037 from dynamic_reconfigure.server import (
00038     Server,
00039 )
00040 from std_msgs.msg import (
00041     Empty,
00042 )
00043 
00044 import baxter_interface
00045 
00046 from baxter_examples.cfg import (
00047     JointSpringsExampleConfig,
00048 )
00049 from baxter_interface import CHECK_VERSION
00050 
00051 
00052 class JointSprings(object):
00053     """
00054     Virtual Joint Springs class for torque example.
00055 
00056     @param limb: limb on which to run joint springs example
00057     @param reconfig_server: dynamic reconfigure server
00058 
00059     JointSprings class contains methods for the joint torque example allowing
00060     moving the limb to a neutral location, entering torque mode, and attaching
00061     virtual springs.
00062     """
00063     def __init__(self, limb, reconfig_server):
00064         self._dyn = reconfig_server
00065 
00066         # control parameters
00067         self._rate = 1000.0  # Hz
00068         self._missed_cmds = 20.0  # Missed cycles before triggering timeout
00069 
00070         # create our limb instance
00071         self._limb = baxter_interface.Limb(limb)
00072 
00073         # initialize parameters
00074         self._springs = dict()
00075         self._damping = dict()
00076         self._start_angles = dict()
00077 
00078         # create cuff disable publisher
00079         cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
00080         self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
00081 
00082         # verify robot is enabled
00083         print("Getting robot state... ")
00084         self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
00085         self._init_state = self._rs.state().enabled
00086         print("Enabling robot... ")
00087         self._rs.enable()
00088         print("Running. Ctrl-c to quit")
00089 
00090     def _update_parameters(self):
00091         for joint in self._limb.joint_names():
00092             self._springs[joint] = self._dyn.config[joint[-2:] +
00093                                                     '_spring_stiffness']
00094             self._damping[joint] = self._dyn.config[joint[-2:] +
00095                                                     '_damping_coefficient']
00096 
00097     def _update_forces(self):
00098         """
00099         Calculates the current angular difference between the start position
00100         and the current joint positions applying the joint torque spring forces
00101         as defined on the dynamic reconfigure server.
00102         """
00103         # get latest spring constants
00104         self._update_parameters()
00105 
00106         # disable cuff interaction
00107         self._pub_cuff_disable.publish()
00108 
00109         # create our command dict
00110         cmd = dict()
00111         # record current angles/velocities
00112         cur_pos = self._limb.joint_angles()
00113         cur_vel = self._limb.joint_velocities()
00114         # calculate current forces
00115         for joint in self._start_angles.keys():
00116             # spring portion
00117             cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
00118                                                    cur_pos[joint])
00119             # damping portion
00120             cmd[joint] -= self._damping[joint] * cur_vel[joint]
00121         # command new joint torques
00122         self._limb.set_joint_torques(cmd)
00123 
00124     def move_to_neutral(self):
00125         """
00126         Moves the limb to neutral location.
00127         """
00128         self._limb.move_to_neutral()
00129 
00130     def attach_springs(self):
00131         """
00132         Switches to joint torque mode and attached joint springs to current
00133         joint positions.
00134         """
00135         # record initial joint angles
00136         self._start_angles = self._limb.joint_angles()
00137 
00138         # set control rate
00139         control_rate = rospy.Rate(self._rate)
00140 
00141         # for safety purposes, set the control rate command timeout.
00142         # if the specified number of command cycles are missed, the robot
00143         # will timeout and disable
00144         self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
00145 
00146         # loop at specified rate commanding new joint torques
00147         while not rospy.is_shutdown():
00148             if not self._rs.state().enabled:
00149                 rospy.logerr("Joint torque example failed to meet "
00150                              "specified control rate timeout.")
00151                 break
00152             self._update_forces()
00153             control_rate.sleep()
00154 
00155     def clean_shutdown(self):
00156         """
00157         Switches out of joint torque mode to exit cleanly
00158         """
00159         print("\nExiting example...")
00160         self._limb.exit_control_mode()
00161         if not self._init_state and self._rs.state().enabled:
00162             print("Disabling robot...")
00163             self._rs.disable()
00164 
00165 
00166 def main():
00167     """RSDK Joint Torque Example: Joint Springs
00168 
00169     Moves the specified limb to a neutral location and enters
00170     torque control mode, attaching virtual springs (Hooke's Law)
00171     to each joint maintaining the start position.
00172 
00173     Run this example on the specified limb and interact by
00174     grabbing, pushing, and rotating each joint to feel the torques
00175     applied that represent the virtual springs attached.
00176     You can adjust the spring constant and damping coefficient
00177     for each joint using dynamic_reconfigure.
00178     """
00179     arg_fmt = argparse.RawDescriptionHelpFormatter
00180     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00181                                      description=main.__doc__)
00182     parser.add_argument(
00183         '-l', '--limb', dest='limb', required=True, choices=['left', 'right'],
00184         help='limb on which to attach joint springs'
00185     )
00186     args = parser.parse_args(rospy.myargv()[1:])
00187 
00188     print("Initializing node... ")
00189     rospy.init_node("rsdk_joint_torque_springs_%s" % (args.limb,))
00190     dynamic_cfg_srv = Server(JointSpringsExampleConfig,
00191                              lambda config, level: config)
00192     js = JointSprings(args.limb, dynamic_cfg_srv)
00193     # register shutdown callback
00194     rospy.on_shutdown(js.clean_shutdown)
00195     js.move_to_neutral()
00196     js.attach_springs()
00197 
00198 
00199 if __name__ == "__main__":
00200     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Sat Jun 8 2019 20:16:27