Joint Torque Springs - Code Walkthrough

From sdk-wiki
Jump to: navigation, search


The Joint Torque Springs Example demonstrates the usage of torque controller to apply torque commands to Baxter's joints. In the main() function, the dynamic reconfigure from rqt is initialized and passed to an object of the JointSprings class along with the side of the arm on which the user decides to run the example, as parameters. 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 rqt plugin. 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(<double>)
  • Limb.move_to_neutral()
  • Limb.joint_angles
  • Limb.joint_velocities
  • Limb.set_command_timeout(<double>)
  • Limb.exit_control_mode()

Code Walkthrough

Now, let's break down the code.

33 import argparse
35 import rospy
37 from dynamic_reconfigure.server import (
38     Server,
39 )
40 from std_msgs.msg import (
41     Empty,
42 )
44 import baxter_interface
46 from baxter_examples.cfg import (
47     JointSpringsExampleConfig,
48 )
49 from baxter_interface import CHECK_VERSION

This imports the baxter interface for accessing the limb class. The JointSpringsExampleConfig holds the default spring and damping constants for Baxter's limbs. 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.

52 class JointSprings(object):
53     """
54     Virtual Joint Springs class for torque example.
56     @param limb: limb on which to run joint springs example
57     @param reconfig_server: dynamic reconfigure server
59     JointSprings class contains methods for the joint torque example allowing
60     moving the limb to a neutral location, entering torque mode, and attaching
61     virtual springs.
62     """
63     def __init__(self, limb, reconfig_server):
64         self._dyn = reconfig_server
66         # control parameters
67         self._rate = 1000.0  # Hz
68         self._missed_cmds = 20.0  # Missed cycles before triggering timeout
70         # create our limb instance
71         self._limb = baxter_interface.Limb(limb)

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 rqt plugin.

73         # initialize parameters
74         self._springs = dict()
75         self._damping = dict()
76         self._start_angles = dict()
78         # create cuff disable publisher
79         cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
80         self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
82         # verify robot is enabled
83         print("Getting robot state... ")
84         self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
85         self._init_state = self._rs.state().enabled
86         print("Enabling robot... ")
87         self._rs.enable()
88         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.

90     def _update_parameters(self):
91         for joint in self._limb.joint_names():
92             self._springs[joint] = self._dyn.config[joint[-2:] +
93                                                     '_spring_stiffness']
94             self._damping[joint] = self._dyn.config[joint[-2:] +
95                                                     '_damping_coefficient'

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

 97      def _update_forces(self):
 98         """
 99         Calculates the current angular difference between the start position
100         and the current joint positions applying the joint torque spring forces
101         as defined on the dynamic reconfigure server.
102         """
103         # get latest spring constants
104         self._update_parameters()
106         # disable cuff interaction
107         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 mesage.

109         # create our command dict
110         cmd = dict()
111         # record current angles/velocities
112         cur_pos = self._limb.joint_angles()
113         cur_vel = self._limb.joint_velocities()

The current position and the velocity is captured.

114         # calculate current forces
115         for joint in self._start_angles.keys():
116             # spring portion
117             cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
118                                                    cur_pos[joint])
119             # damping portion
120             cmd[joint] -= self._damping[joint] * cur_vel[joint]
121         # command new joint torques
122         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/side/joint_command. The mode indicates that the joint commands are to be passed to the torque controller.

124     def move_to_neutral(self):
125         """
126         Moves the limb to neutral location.
127         """
128         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.

130     def attach_springs(self):
131         """
132         Switches to joint torque mode and attached joint springs to current
133         joint positions.
134         """
135         # record initial joint angles
136         self._start_angles = self._limb.joint_angles()
138         # set control rate
139         control_rate = rospy.Rate(self._rate)
141         # for safety purposes, set the control rate command timeout.
142         # if the specified number of command cycles are missed, the robot
143         # will timeout and disable
144         self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
146         # loop at specified rate commanding new joint torques
147         while not rospy.is_shutdown():
148             if not self._rs.state().enabled:
149                 rospy.logerr("Joint torque example failed to meet "
150                              "specified control rate timeout.")
151                 break
152             self._update_forces()
153             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.

155     def clean_shutdown(self):
156         """
157         Switches out of joint torque mode to exit cleanly
158         """
159         print("\nExiting example...")
160         self._limb.exit_control_mode()
161         if not self._init_state and self._rs.state().enabled:
162             print("Disabling robot...")
163             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.

166 def main():
167     """RSDK Joint Torque Example: Joint Springs
169     Moves the specified limb to a neutral location and enters
170     torque control mode, attaching virtual springs (Hooke's Law)
171     to each joint maintaining the start position.
173     Run this example on the specified limb and interact by
174     grabbing, pushing, and rotating each joint to feel the torques
175     applied that represent the virtual springs attached.
176     You can adjust the spring constant and damping coefficient
177     for each joint using dynamic_reconfigure.
178     """
179     arg_fmt = argparse.RawDescriptionHelpFormatter
180     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
181                                      description=main.__doc__)
182     parser.add_argument(
183         '-l', '--limb', dest='limb', required=True, choices=['left', 'right'],
184         help='limb on which to attach joint springs'
185     )
186     args = parser.parse_args()

This section includes the parser that lists out the options for the user and captures the one that is inputted as a command line argument.

188     print("Initializing node... ")
189     rospy.init_node("rsdk_joint_torque_springs_%s" % (args.limb,))
190     dynamic_cfg_srv = Server(JointSpringsExampleConfig,
191                              lambda config, level: config)
192     js = JointSprings(args.limb, dynamic_cfg_srv)
193     # register shutdown callback
194     rospy.on_shutdown(js.clean_shutdown)
195     js.move_to_neutral()
196     js.attach_springs()
199 if __name__ == "__main__":
200     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.