Joint Torque Springs Example

Jump to: navigation , search

This example shows joint torque control usage. After moving to neutral, the robot will enter torque control mode, applying torques representing virtual springs holding the joints to their start position. The example calculates and applies torques linearly relative to offset from the start position creating the illusion that springs of configurable stiffness are attached to each joint.

Introduction

The Joint Torque Springs Example demonstrates the usage of torque controller to apply torque commands to robot's joints. In the main() function, the dynamic reconfigure from server is initialized and passed to an object of the JointSprings class. On initialization, the object creates an instance of the limb class for the particular side that is passed, and captures the default spring and damping constants from the dynamic reconfigure server. The main() function calls the method, move_to_neutral() to send the limbs to a neutral position. Then, the attach_spring() method captures the initial joint angles of the limb and calls the update_forces() method which calculates the necessary torques based on Hooke's law and publishes them as joint commands.

Interfaces -

  • Limb.set_joint_torques()
  • Limb.move_to_neutral()
  • Limb.joint_angles()
  • Limb.joint_velocities()
  • Limb.set_command_timeout()
  • Limb.exit_control_mode()

If you would like to follow along with the actual source code for the example on GitHub, it can be found through this link for joint torque spring example.

Usage

Start the joint torque springs example from an RSDK terminal session using:

$ rosrun intera_examples joint_torque_springs.py

Messages will appear after start:

Initializing node...
Getting robot state...
Enabling robot...
Running. Ctrl-c to quit

The robot will move to neutral pose first, when done moving to neutral pose, you can interact with the robot by grabbing, pushing, and rotating each joint to feel the torques applied that represent the virtual springs attached.

Pressing Control-C at any time will stop torque mode and exit the example.

Arguments

Important Arguments:

-l or --limb : The robot arm name (default="right")

See the joint torque spring's available arguments on the command line by passing joint_torque_springs.py the -h, help argument:

$ rosrun intera_examples joint_torque_springs.py -h
usage: joint_torque_springs.py [-h] [-l LIMB]

RSDK Joint Torque Example: Joint Springs

    Moves the default limb to a neutral location and enters
    torque control mode, attaching virtual springs (Hooke.s Law)
    to each joint maintaining the start position.

    Run this example and interact by grabbing, pushing, and rotating
    each joint to feel the torques applied that represent the
    virtual springs attached. You can adjust the spring
    constant and damping coefficient for each joint using
    dynamic_reconfigure.
   

optional arguments:
  -h, --help            show this help message and exit
  -l LIMB, --limb LIMB
                        limb on which to attach joint springs

Code Walkthrough

Now, let's break down the code.

  1. import argparse
  2. import importlib
  3.  
  4. import rospy
  5. from dynamic_reconfigure.server import (
  6.     Server,
  7. )
  8. from std_msgs.msg import (
  9.     Empty,
  10. )
  11.  
  12. import intera_interface
  13. from intera_interface import CHECK_VERSION

This imports the intera interface for accessing the limb class. The CHECK_VERSION is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.

  1. class JointSprings(object):
  2.     """
  3.    Virtual Joint Springs class for torque example.
  4.  
  5.    @param limb: limb on which to run joint springs example
  6.    @param reconfig_server: dynamic reconfigure server
  7.  
  8.    JointSprings class contains methods for the joint torque example allowing
  9.    moving the limb to a neutral location, entering torque mode, and attaching
  10.    virtual springs.
  11.    """
  12.     def __init__(self, reconfig_server, limb = "right"):
  13.         self._dyn = reconfig_server
  14.  
  15.         # control parameters
  16.         self._rate = 1000.0  # Hz
  17.         self._missed_cmds = 20.0  # Missed cycles before triggering timeout
  18.  
  19.         # create our limb instance
  20.         self._limb = intera_interface.Limb(limb)
  21.  
  22.         # initialize parameters
  23.         self._springs = dict()
  24.         self._damping = dict()
  25.         self._start_angles = dict()

The JointSprings class object is initialized by passing the side of the limb under interest, an object of the reconfigure server that holds the default spring and damping constants for the limb. These default values can be modified at runtime through the dynamic reconfigure server.

  1.         # create cuff disable publisher
  2.         cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
  3.         self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
  4.  
  5.         # verify robot is enabled
  6.         print("Getting robot state... ")
  7.         self._rs = intera_interface.RobotEnable(CHECK_VERSION)
  8.         self._init_state = self._rs.state().enabled
  9.         print("Enabling robot... ")
  10.         self._rs.enable()
  11.         print("Running. Ctrl-c to quit")

The zero gravity mode can be enabled by pressing the cuff and so it is important to disable the cuff. A publisher for the cuff interaction suppress topic is created. It is also important to note that the robot should be enabled before trying to manipulate its joints. So, the robot is checked if it is enabled and if not, it is enabled. The initial state of the variable is recorded in _init_state variable. This is to ensure that the robot is sent back to that state once the example program ends.

  1.     def _update_parameters(self):
  2.         for joint in self._limb.joint_names():
  3.             self._springs[joint] = self._dyn.config[joint[-2:] + '_spring_stiffness']
  4.             self._damping[joint] = self._dyn.config[joint[-2:] + '_damping_coefficient']

The _update_parameters() method captures the dynamically entered user's input for the spring and damping constants through the dynamic reconfigure server.

  1.     def _update_forces(self):
  2.         """
  3.        Calculates the current angular difference between the start position
  4.        and the current joint positions applying the joint torque spring forces
  5.        as defined on the dynamic reconfigure server.
  6.        """
  7.         # get latest spring constants
  8.         self._update_parameters()
  9.  
  10.         # disable cuff interaction
  11.         self._pub_cuff_disable.publish()

The cuff interaction is disabled as long as the example runs and each time the forces are updated, the _pub_cuff_disable publisher publishes an empty message.

  1.         # create our command dict
  2.         cmd = dict()
  3.         # record current angles/velocities
  4.         cur_pos = self._limb.joint_angles()
  5.         cur_vel = self._limb.joint_velocities()
  6.         # calculate current forces
  7.         for joint in self._start_angles.keys():
  8.             # spring portion
  9.             cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
  10.                                                    cur_pos[joint])
  11.             # damping portion
  12.             cmd[joint] -= self._damping[joint] * cur_vel[joint]
  13.         # command new joint torques
  14.         self._limb.set_joint_torques(cmd)

Hooke's law is applied to calculate the necessary torques to be applied on each joints. The updated spring and damping constants are used in the calculation. The set_joint_torques() method of the limb interface publishes the set of joint torques and their corresponding names with the mode as 3 to the topic robot/limb/right/joint_command . The mode indicates that the joint commands are to be passed to the torque controller.

  1.     def move_to_neutral(self):
  2.         """
  3.        Moves the limb to neutral location.
  4.        """
  5.         self._limb.move_to_neutral()

The move_to_neutral() method of the limb interface, moves all the joints to their neutral pose. This method employs the position controller to send the joints to a pre-defined neutral position.

  1.     def attach_springs(self):
  2.         """
  3.        Switches to joint torque mode and attached joint springs to current
  4.        joint positions.
  5.        """
  6.         # record initial joint angles
  7.         self._start_angles = self._limb.joint_angles()
  8.  
  9.         # set control rate
  10.         control_rate = rospy.Rate(self._rate)
  11.  
  12.         # for safety purposes, set the control rate command timeout.
  13.         # if the specified number of command cycles are missed, the robot
  14.         # will timeout and disable
  15.         self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
  16.  
  17.         # loop at specified rate commanding new joint torques
  18.         while not rospy.is_shutdown():
  19.             if not self._rs.state().enabled:
  20.                 rospy.logerr("Joint torque example failed to meet "
  21.                              "specified control rate timeout.")
  22.                 break
  23.             self._update_forces()
  24.             control_rate.sleep()

The start angles recorded here are used by the _update_forces method to determine the dx, which is the displacement caused from the starting position of the joints. The _update_forces method is invoked at 1000Hz, which performs the spring motion as explained above.

  1.     def clean_shutdown(self):
  2.         """
  3.        Switches out of joint torque mode to exit cleanly
  4.        """
  5.         print("\nExiting example...")
  6.         self._limb.exit_control_mode()
  7.         if not self._init_state and self._rs.state().enabled:
  8.             print("Disabling robot...")
  9.             self._rs.disable()

The method exit_control_mode() of the limb interface switches to position controller mode from torque/velocity controller and saves the current joint angles as the current joint position. Finally, it checks if the robot was initially disabled and if so disables it.

  1. def main():
  2.     """RSDK Joint Torque Example: Joint Springs
  3.  
  4.    Moves the default limb to a neutral location and enters
  5.    torque control mode, attaching virtual springs (Hooke's Law)
  6.    to each joint maintaining the start position.
  7.  
  8.    Run this example and interact by grabbing, pushing, and rotating
  9.    each joint to feel the torques applied that represent the
  10.    virtual springs attached. You can adjust the spring
  11.    constant and damping coefficient for each joint using
  12.    dynamic_reconfigure.
  13.    """
  14.     # Querying the parameter server to determine Robot model and limb name(s)
  15.     rp = intera_interface.RobotParams()
  16.     valid_limbs = rp.get_limb_names()
  17.     if not valid_limbs:
  18.         rp.log_message(("Cannot detect any limb parameters on this robot. "
  19.                         "Exiting."), "ERROR")
  20.     robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
  21.     # Parsing Input Arguments
  22.     arg_fmt = argparse.ArgumentDefaultsHelpFormatter
  23.     parser = argparse.ArgumentParser(formatter_class=arg_fmt)
  24.     parser.add_argument(
  25.         "-l", "--limb", dest="limb", default=valid_limbs[0],
  26.         choices=valid_limbs,
  27.         help='limb on which to attach joint springs'
  28.         )
  29.     args = parser.parse_args(rospy.myargv()[1:])
  30.     # Grabbing Robot-specific parameters for Dynamic Reconfigure
  31.     config_name = ''.join([robot_name,"JointSpringsExampleConfig"])
  32.     config_module = "intera_examples.cfg"
  33.     cfg = importlib.import_module('.'.join([config_module,config_name]))
  34.     # Starting node connection to ROS
  35.     print("Initializing node... ")
  36.     rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb))
  37.     dynamic_cfg_srv = Server(cfg, lambda config, level: config)
  38.     js = JointSprings(dynamic_cfg_srv, limb=args.limb)
  39.     # register shutdown callback
  40.     rospy.on_shutdown(js.clean_shutdown)
  41.     js.move_to_neutral()
  42.     js.attach_springs()
  43.  
  44.  
  45. if __name__ == "__main__":
  46.     main()

The rospy node is initialized and then an instance of the dynamic reconfigure is created. As indicated above, this supplies the default and runtime, spring and dynamic constants. In custom programs this can be avoided by assigning spring and damping constants directly in the code.