Introduction
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
34
35 import rospy
36
37 from dynamic_reconfigure.server import (
38 Server,
39 )
40 from std_msgs.msg import (
41 Empty,
42 )
43
44 import baxter_interface
45
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.
55
56 @param limb: limb on which to run joint springs example
57 @param reconfig_server: dynamic reconfigure server
58
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
65
66 # control parameters
67 self._rate = 1000.0 # Hz
68 self._missed_cmds = 20.0 # Missed cycles before triggering timeout
69
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()
77
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)
81
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()
105
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()
137
138 # set control rate
139 control_rate = rospy.Rate(self._rate)
140
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)
145
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
168
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.
172
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()
197
198
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.
