Set Interaction Options Example

Revision as of 21:21, 21 November 2017 by Andypark (talk | contribs) (Arguments)
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.


This interaction option setting example demonstrates how to use interaction control mode to generate a desired interaction behavior in the current arm configuration.

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.


Start the interaction option setting example from an RSDK terminal session using:

$ rosrun intera_examples

A default interaction control command message will be printed out after start:

[INFO] [WallTime: 1511299000.977577] header: 
  seq: 1
    secs: 1511299000
    nsecs: 977231979
  frame_id: base
interaction_control_active: True
K_impedance: [1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0]
max_impedance: [True, True, True, True, True, True]
D_impedance: []
K_nullspace: [5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0]
force_command: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    x: 0
    y: 0
    z: 0
    x: 0
    y: 0
    z: 0
    w: 1
endpoint_name: right_hand
in_endpoint_frame: False
disable_damping_in_force_control: False
disable_reference_resetting: False

The interaction control command message will be published at 10Hz by default.

Pressing Control-C at any time will stop publishing the command message and exit the example.


See the available arguments on the command line by passing the -h, help argument:

$ rosrun intera_examples -h
usage: [-h] [-s {0,1}]
                                  [-k K_IMPEDANCE [K_IMPEDANCE ...]]
                                  [-m {0,1} [{0,1} ...]]
                                  [-md {1,2,3,4} [{1,2,3,4} ...]]
                                  [-fr INTERACTION_FRAME [INTERACTION_FRAME ...]]
                                  [-ef] [-en ENDPOINT_NAME]
                                  [-f FORCE_COMMAND [FORCE_COMMAND ...]]
                                  [-kn K_NULLSPACE [K_NULLSPACE ...]] [-dd]
                                  [-dr] [-rc] [-r RATE]

    Set the desired interaction control options in the current configuration.
    Note that the arm is not commanded to move but it will have the specified
    interaction control behavior. If publish rate is 0 where the interaction
    control command is only published once, after entering and exiting zero-G,
    the arm will return to normal position mode. Also, regardless of the publish
    rate, the zero-G behavior will not be affected by this. The future motion
    commands need to be sent with interaction parameters if we want to keep
    interaction control behaviors during the trajectory execution; otherwise,
    the arm will move in position mode during the motion even if this script
    is still running.

    Call using:
    $ rosrun intera_examples  [arguments: see below]

    -s 1
    --> Set interaction active to True (0 for False) in the current configuration

    -k 500.0 500.0 500.0 10.0 10.0 10.0
    --> Set K_impedance to [500.0 500.0 500.0 10.0 10.0 10.0] in the current configuration

    -m 1 1 0 1 1 1
    --> Set max_impedance to [True True False True True True] in the current configuration

    -md 1 1 2 1 1 1
    --> Set interaction_control_mode to [impedance, impedance, force, impedance, impedance, impedance]
        in the current configuration (1: impedance, 2: force, 3: impedance w/ force limit,
        4: force w/ motion limit)

    -fr 0.1 0.2 0.3 1 0 0 0
    --> Set the pose of the interaction_frame -- position: (0.1, 0.2, 0.3) and orientation (1, 0, 0, 0)

    --> Set in_endpoint_frame to True in the current configuration (use TCP frame as reference frame)

    -f 0.0 0.0 30.0 0.0 0.0 0.0
    --> Set force_command to [0.0 0.0 30.0 0.0 0.0 0.0] in the current configuration

    -kn 5.0 3.0 5.0 4.0 6.0 4.0 6.0
    --> Set K_nullspace to [5.0 3.0 5.0 4.0 6.0 4.0 6.0] in the current configuration

    -r 20
    --> Set desired publish rate (Hz)

optional arguments:
  -h, --help            show this help message and exit
  -s {0,1}, --interaction_active {0,1}
                        Activate (1) or Deactivate (0) interaction controller
                        A list of desired stiffnesses, one for each of the 6
                        directions -- stiffness units are (N/m) for first 3
                        and (Nm/rad) for second 3 values
  -m {0,1} [{0,1} ...], --max_impedance {0,1} [{0,1} ...]
                        A list of maximum stiffness behavior state, one for
                        each of the 6 directions (a single value can be
                        provided to apply the same value to all the
                        directions) -- 0 for False, 1 for True
  -md {1,2,3,4} [{1,2,3,4} ...], --interaction_control_mode {1,2,3,4} [{1,2,3,4} ...]
                        A list of desired interaction control mode (1:
                        impedance, 2: force, 3: impedance with force limit, 4:
                        force with motion limit), one for each of the 6
                        Specify the reference frame for the interaction
                        controller -- first 3 values are positions [m] and
                        last 4 values are orientation in quaternion (w, x, y,
                        z) which has to be normalized values
  -ef, --in_endpoint_frame
                        Set the desired reference frame to endpoint frame;
                        otherwise, it is base frame by default
  -en ENDPOINT_NAME, --endpoint_name ENDPOINT_NAME
                        Set the desired endpoint frame by its name; otherwise,
                        it is right_hand frame by default
                        A list of desired force commands, one for each of the
                        6 directions -- in force control mode this is the
                        vector of desired forces/torques to be regulated in
                        (N) and (Nm), in impedance with force limit mode this
                        vector specifies the magnitude of forces/torques (N
                        and Nm) that the command will not exceed
                        A list of desired nullspace stiffnesses, one for each
                        of the 7 joints (a single value can be provided to
                        apply the same value to all the directions) -- units
                        are in (Nm/rad)
  -dd, --disable_damping_in_force_control
                        Disable damping in force control
  -dr, --disable_reference_resetting
                        The reference signal is reset to actual position to
                        avoid jerks/jumps when interaction parameters are
                        changed. This option allows the user to disable this
  -rc, --rotations_for_constrained_zeroG
                        Allow arbitrary rotational displacements from the
                        current orientation for constrained zero-G (works only
                        with a stationary reference orientation)
  -r RATE, --rate RATE  A desired publish rate for updating interaction
                        control commands (10Hz by default) -- 0 if we want to
                        publish it only once

Code Walkthrough

Now, let's break down the code.

33 import argparse
34 import importlib
36 import rospy
37 from dynamic_reconfigure.server import (
38     Server,
39 )
40 from std_msgs.msg import (
41     Empty,
42 )
44 import intera_interface
45 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.

43 class JointSprings(object):
44     """
45     Virtual Joint Springs class for torque example.
47     @param limb: limb on which to run joint springs example
48     @param reconfig_server: dynamic reconfigure server
50     JointSprings class contains methods for the joint torque example allowing
51     moving the limb to a neutral location, entering torque mode, and attaching
52     virtual springs.
53     """
54     def __init__(self, reconfig_server, limb = "right"):
55         self._dyn = reconfig_server
57         # control parameters
58         self._rate = 1000.0  # Hz
59         self._missed_cmds = 20.0  # Missed cycles before triggering timeout
61         # create our limb instance
62         self._limb = intera_interface.Limb(limb)
64         # initialize parameters
65         self._springs = dict()
66         self._damping = dict()
67         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.

69         # create cuff disable publisher
70         cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
71         self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
73         # verify robot is enabled
74         print("Getting robot state... ")
75         self._rs = intera_interface.RobotEnable(CHECK_VERSION)
76         self._init_state = self._rs.state().enabled
77         print("Enabling robot... ")
78         self._rs.enable()
79         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.

81     def _update_parameters(self):
82         for joint in self._limb.joint_names():
83             self._springs[joint] = self._dyn.config[joint[-2:] + '_spring_stiffness']
84             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.

86     def _update_forces(self):
87         """
88         Calculates the current angular difference between the start position
89         and the current joint positions applying the joint torque spring forces
90         as defined on the dynamic reconfigure server.
91         """
92         # get latest spring constants
93         self._update_parameters()
95         # disable cuff interaction
96         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.

 98         # create our command dict
 99         cmd = dict()
100         # record current angles/velocities
101         cur_pos = self._limb.joint_angles()
102         cur_vel = self._limb.joint_velocities()
103         # calculate current forces
104         for joint in self._start_angles.keys():
105             # spring portion
106             cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
107                                                    cur_pos[joint])
108             # damping portion
109             cmd[joint] -= self._damping[joint] * cur_vel[joint]
110         # command new joint torques
111         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.

113     def move_to_neutral(self):
114         """
115         Moves the limb to neutral location.
116         """
117         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.

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

144     def clean_shutdown(self):
145         """
146         Switches out of joint torque mode to exit cleanly
147         """
148         print("\nExiting example...")
149         self._limb.exit_control_mode()
150         if not self._init_state and self._rs.state().enabled:
151             print("Disabling robot...")
152             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.

155 def main():
156     """RSDK Joint Torque Example: Joint Springs
158     Moves the default limb to a neutral location and enters
159     torque control mode, attaching virtual springs (Hooke's Law)
160     to each joint maintaining the start position.
162     Run this example and interact by grabbing, pushing, and rotating
163     each joint to feel the torques applied that represent the
164     virtual springs attached. You can adjust the spring
165     constant and damping coefficient for each joint using
166     dynamic_reconfigure.
167     """
168     # Querying the parameter server to determine Robot model and limb name(s)
169     rp = intera_interface.RobotParams()
170     valid_limbs = rp.get_limb_names()
171     if not valid_limbs:
172         rp.log_message(("Cannot detect any limb parameters on this robot. "
173                         "Exiting."), "ERROR")
174     robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
175     # Parsing Input Arguments
176     arg_fmt = argparse.ArgumentDefaultsHelpFormatter
177     parser = argparse.ArgumentParser(formatter_class=arg_fmt)
178     parser.add_argument(
179         "-l", "--limb", dest="limb", default=valid_limbs[0],
180         choices=valid_limbs,
181         help='limb on which to attach joint springs'
182         )
183     args = parser.parse_args(rospy.myargv()[1:])
184     # Grabbing Robot-specific parameters for Dynamic Reconfigure
185     config_name = ''.join([robot_name,"JointSpringsExampleConfig"])
186     config_module = "intera_examples.cfg"
187     cfg = importlib.import_module('.'.join([config_module,config_name]))
188     # Starting node connection to ROS
189     print("Initializing node... ")
190     rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb))
191     dynamic_cfg_srv = Server(cfg, lambda config, level: config)
192     js = JointSprings(dynamic_cfg_srv, limb=args.limb)
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.