Difference between revisions of "Set Interaction Options Example"

Jump to: navigation , search
(Introduction)
 
(5 intermediate revisions by 2 users not shown)
Line 1: Line 1:
 
<div class="title-block">
 
<div class="title-block">
  
<span style="font-size:120%;">'''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.'''</span>
+
<span style="font-size:120%;">'''This example shows interaction control usage by 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.'''</span>
  
 
</div>
 
</div>
Line 11: Line 11:
 
== Introduction ==
 
== Introduction ==
  
 +
'''NEW functionality in the SDK/Intera 5.2 Update!'''<br/>
 
This interaction option setting example demonstrates how to use interaction control mode to generate a desired interaction behavior in the current arm configuration.  
 
This interaction option setting example demonstrates how to use interaction control mode to generate a desired interaction behavior in the current arm configuration.  
  
Line 21: Line 22:
 
== Usage ==
 
== Usage ==
  
Start the joint torque springs example from an RSDK terminal session using:
+
Start the interaction option setting example from an RSDK terminal session using:
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples joint_torque_springs.py
+
$ rosrun intera_examples set_interaction_options.py
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Messages will appear after start:
+
A default interaction control command message will be printed out after start:
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
Initializing node...
+
[INFO] [WallTime: 1511299000.977577] header:
Getting robot state...
+
  seq: 1
Enabling robot...
+
  stamp:
Running. Ctrl-c to quit
+
    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]
 +
interaction_frame:
 +
  position:
 +
    x: 0
 +
    y: 0
 +
    z: 0
 +
  orientation:
 +
    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
 
</syntaxhighlight>
 
</syntaxhighlight>
  
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.
+
The interaction control command message will be published at 10Hz by default.  
  
Pressing <code> Control-C </code> at any time will stop torque mode and exit the example.
+
Pressing <code> Control-C </code> at any time will stop publishing the command message and exit the example.
  
 
</div>
 
</div>
Line 44: Line 67:
 
== Arguments ==
 
== Arguments ==
  
'''Important Arguments:''' 
+
See the available arguments on the command line by passing set_interaction_options.py the <code>-h</code>, help argument:
 
 
<code> -l </code> or <code> --limb </code>: 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 <code>-h</code>, help argument:
 
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
$ rosrun intera_examples joint_torque_springs.py -h
+
$ rosrun intera_examples set_interaction_options.py -h
 
</syntaxhighlight>
 
</syntaxhighlight>
  
 
<syntaxhighlight lang="bash" enclose="div">
 
<syntaxhighlight lang="bash" enclose="div">
usage: joint_torque_springs.py [-h] [-l LIMB]
+
usage: set_interaction_options.py [-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 set_interaction_options.py  [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)
 +
 
 +
    -ef
 +
    --> Set in_endpoint_frame to True in the current configuration (use TCP frame as reference frame)
  
RSDK Joint Torque Example: Joint Springs
+
    -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
  
     Moves the default limb to a neutral location and enters
+
     -kn 5.0 3.0 5.0 4.0 6.0 4.0 6.0
    torque control mode, attaching virtual springs (Hooke.s Law)
+
     --> Set K_nullspace to [5.0 3.0 5.0 4.0 6.0 4.0 6.0] in the current configuration
     to each joint maintaining the start position.
 
  
     Run this example and interact by grabbing, pushing, and rotating
+
     -r 20
     each joint to feel the torques applied that represent the
+
     --> Set desired publish rate (Hz)
    virtual springs attached. You can adjust the spring
 
    constant and damping coefficient for each joint using
 
    dynamic_reconfigure.
 
 
      
 
      
  
 
optional arguments:
 
optional arguments:
 
   -h, --help            show this help message and exit
 
   -h, --help            show this help message and exit
   -l LIMB, --limb LIMB
+
   -s {0,1}, --interaction_active {0,1}
                         limb on which to attach joint springs
+
                         Activate (1) or Deactivate (0) interaction controller
 +
  -k K_IMPEDANCE [K_IMPEDANCE ...], --K_impedance K_IMPEDANCE [K_IMPEDANCE ...]
 +
                        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
 +
                        directions
 +
  -fr INTERACTION_FRAME [INTERACTION_FRAME ...], --interaction_frame INTERACTION_FRAME [INTERACTION_FRAME ...]
 +
                        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
 +
  -f FORCE_COMMAND [FORCE_COMMAND ...], --force_command FORCE_COMMAND [FORCE_COMMAND ...]
 +
                        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
 +
  -kn K_NULLSPACE [K_NULLSPACE ...], --K_nullspace K_NULLSPACE [K_NULLSPACE ...]
 +
                        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
 +
                        feature.
 +
  -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
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Line 84: Line 193:
  
 
<syntaxhighlight lang="python" line start="33" enclose="div">
 
<syntaxhighlight lang="python" line start="33" enclose="div">
 +
import rospy
 +
from intera_core_msgs.msg import InteractionControlCommand
 
import argparse
 
import argparse
import importlib
+
from geometry_msgs.msg import Pose
 
+
from intera_motion_interface import InteractionOptions
import rospy
+
from intera_motion_interface.utility_functions import int2bool
from dynamic_reconfigure.server import (
 
    Server,
 
)
 
from std_msgs.msg import (
 
    Empty,
 
)
 
 
 
import intera_interface
 
from intera_interface import CHECK_VERSION
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
This imports the intera interface for accessing the limb class. The <code>CHECK_VERSION</code> 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.
+
This imports the intera motion interface to access the InteractionOptions class. Also, <code>InteractionControlCommand</code> message is imported from <code>geometry_msgs.msg</code>.
  
 
<syntaxhighlight lang="python" line start="43" enclose="div">
 
<syntaxhighlight lang="python" line start="43" enclose="div">
class JointSprings(object):
+
    try:
    """
+
        rospy.init_node('set_interaction_options_py')
    Virtual Joint Springs class for torque example.
+
        pub = rospy.Publisher('/robot/limb/right/interaction_control_command', InteractionControlCommand, queue_size = 1)
 
+
         rospy.sleep(0.5)
    @param limb: limb on which to run joint springs example
 
    @param reconfig_server: dynamic reconfigure server
 
 
 
    JointSprings class contains methods for the joint torque example allowing
 
    moving the limb to a neutral location, entering torque mode, and attaching
 
    virtual springs.
 
    """
 
    def __init__(self, reconfig_server, limb = "right"):
 
         self._dyn = reconfig_server
 
  
         # control parameters
+
         if args.rate > 0:
        self._rate = 1000.0 # Hz
+
            rate = rospy.Rate(args.rate)
        self._missed_cmds = 20.0  # Missed cycles before triggering timeout
+
         elif args.rate == 0:
 
+
            rospy.logwarn('Interaction control options will be set only once!')
        # create our limb instance
+
         elif args.rate < 0:
         self._limb = intera_interface.Limb(limb)
+
            rospy.logerr('Invalid publish rate!')
 
 
        # initialize parameters
 
        self._springs = dict()
 
         self._damping = dict()
 
        self._start_angles = dict()
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>JointSprings</code> 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.
+
The rospy is first initialized, and a publisher for ros topic (<code>/robot/limb/right/interaction_control_command</code>) is created. The publish rate is set to what is provided in the argument.  
  
 
<syntaxhighlight lang="python" line start="69" enclose="div">
 
<syntaxhighlight lang="python" line start="69" enclose="div">
         # create cuff disable publisher
+
         # set the interaction control options in the current configuration
         cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
+
         interaction_options = InteractionOptions()
        self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
 
  
         # verify robot is enabled
+
         interaction_options.set_interaction_control_active(int2bool(args.interaction_active))
         print("Getting robot state... ")
+
         interaction_options.set_K_impedance(args.K_impedance)
         self._rs = intera_interface.RobotEnable(CHECK_VERSION)
+
        interaction_options.set_max_impedance(int2bool(args.max_impedance))
         self._init_state = self._rs.state().enabled
+
         interaction_options.set_interaction_control_mode(args.interaction_control_mode)
         print("Enabling robot... ")
+
        interaction_options.set_in_endpoint_frame(args.in_endpoint_frame)
        self._rs.enable()
+
         interaction_options.set_force_command(args.force_command)
         print("Running. Ctrl-c to quit")
+
        interaction_options.set_K_nullspace(args.K_nullspace)
</syntaxhighlight>
+
        if len(args.interaction_frame) < 7:
 +
            rospy.logerr('The number of elements must be 7!')
 +
         elif len(args.interaction_frame) == 7:
 +
            quat_sum_square = args.interaction_frame[3]*args.interaction_frame[3] + args.interaction_frame[4]*args.interaction_frame[4]
 +
            + args.interaction_frame[5]*args.interaction_frame[5] + args.interaction_frame[6]*args.interaction_frame[6]
 +
            if quat_sum_square  < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
 +
                interaction_frame = Pose()
 +
                interaction_frame.position.x = args.interaction_frame[0]
 +
                interaction_frame.position.y = args.interaction_frame[1]
 +
                interaction_frame.position.z = args.interaction_frame[2]
 +
                interaction_frame.orientation.w = args.interaction_frame[3]
 +
                interaction_frame.orientation.x = args.interaction_frame[4]
 +
                interaction_frame.orientation.y = args.interaction_frame[5]
 +
                interaction_frame.orientation.z = args.interaction_frame[6]
 +
                interaction_options.set_interaction_frame(interaction_frame)
 +
            else:
 +
                rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
 +
         else:
 +
            rospy.logerr('Invalid input to interaction_frame!')
  
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 <code>_init_state</code> variable. This is to ensure that the robot is sent back to that state once the example program ends.
+
        interaction_options.set_disable_damping_in_force_control(args.disable_damping_in_force_control)
 +
        interaction_options.set_disable_reference_resetting(args.disable_reference_resetting)
 +
        interaction_options.set_rotations_for_constrained_zeroG(args.rotations_for_constrained_zeroG)
  
<syntaxhighlight lang="python" line start="81" enclose="div">
+
        msg = interaction_options.to_msg()
    def _update_parameters(self):
 
        for joint in self._limb.joint_names():
 
            self._springs[joint] = self._dyn.config[joint[-2:] + '_spring_stiffness']
 
            self._damping[joint] = self._dyn.config[joint[-2:] + '_damping_coefficient']
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>_update_parameters()</code> method captures the dynamically entered user's input for the spring and damping constants through the dynamic reconfigure server.
+
An object for <code>InteractionOptions</code> class is created. Then the interaction parameters are set as specified in the arguments using the helper functions provided by the class. After setting all the parameters, a ROS message is generated.
  
<syntaxhighlight lang="python" line start="86" enclose="div">
+
<syntaxhighlight lang="python" line start="81" enclose="div">
    def _update_forces(self):
+
         # print the resultant interaction options once
         """
+
         rospy.loginfo(msg)
        Calculates the current angular difference between the start position
+
         pub.publish(msg)
        and the current joint positions applying the joint torque spring forces
 
         as defined on the dynamic reconfigure server.
 
        """
 
        # get latest spring constants
 
        self._update_parameters()
 
 
 
         # disable cuff interaction
 
        self._pub_cuff_disable.publish()
 
</syntaxhighlight>
 
 
 
The cuff interaction is disabled as long as the example runs and each time the forces are updated, the <code>_pub_cuff_disable</code> publisher publishes an empty message.
 
 
 
<syntaxhighlight lang="python" line start="98" enclose="div">
 
        # create our command dict
 
        cmd = dict()
 
        # record current angles/velocities
 
        cur_pos = self._limb.joint_angles()
 
        cur_vel = self._limb.joint_velocities()
 
        # calculate current forces
 
        for joint in self._start_angles.keys():
 
            # spring portion
 
            cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
 
                                                  cur_pos[joint])
 
            # damping portion
 
            cmd[joint] -= self._damping[joint] * cur_vel[joint]
 
        # command new joint torques
 
        self._limb.set_joint_torques(cmd)
 
</syntaxhighlight>
 
  
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 <code>set_joint_torques()</code> method of the limb interface publishes the set of joint torques and their corresponding names with the mode as 3 to the topic <code>robot/limb/right/joint_command </code>. The mode indicates that the joint commands are to be passed to the torque controller.
+
        if args.rate > 0:
 
+
            while not rospy.is_shutdown():
<syntaxhighlight lang="python" line start="113" enclose="div">
+
                rate.sleep()
    def move_to_neutral(self):
+
                pub.publish(msg)
        """
 
        Moves the limb to neutral location.
 
        """
 
        self._limb.move_to_neutral()
 
 
</syntaxhighlight>
 
</syntaxhighlight>
  
The <code>move_to_neutral()</code> 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.
+
The generated ROS message is printed out once and it is published at a specified rate.
 
 
<syntaxhighlight lang="python" line start="119" enclose="div">
 
    def attach_springs(self):
 
        """
 
        Switches to joint torque mode and attached joint springs to current
 
        joint positions.
 
        """
 
        # record initial joint angles
 
        self._start_angles = self._limb.joint_angles()
 
 
 
        # set control rate
 
        control_rate = rospy.Rate(self._rate)
 
 
 
        # for safety purposes, set the control rate command timeout.
 
        # if the specified number of command cycles are missed, the robot
 
        # will timeout and disable
 
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
 
 
 
        # loop at specified rate commanding new joint torques
 
        while not rospy.is_shutdown():
 
            if not self._rs.state().enabled:
 
                rospy.logerr("Joint torque example failed to meet "
 
                            "specified control rate timeout.")
 
                break
 
            self._update_forces()
 
            control_rate.sleep()
 
</syntaxhighlight>
 
 
 
The start angles recorded here are used by the <code>_update_forces</code> method to determine the dx, which is the displacement caused from the starting position of the joints. The <code>_update_forces</code> method is invoked at 1000Hz, which performs the spring motion as explained above.
 
 
 
<syntaxhighlight lang="python" line start="144" enclose="div">
 
    def clean_shutdown(self):
 
        """
 
        Switches out of joint torque mode to exit cleanly
 
        """
 
        print("\nExiting example...")
 
        self._limb.exit_control_mode()
 
        if not self._init_state and self._rs.state().enabled:
 
            print("Disabling robot...")
 
            self._rs.disable()
 
</syntaxhighlight>
 
 
 
The method <code>exit_control_mode()</code> 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.
 
  
 
<syntaxhighlight lang="python" line start="155" enclose="div">
 
<syntaxhighlight lang="python" line start="155" enclose="div">
def main():
+
     except rospy.ROSInterruptException:
     """RSDK Joint Torque Example: Joint Springs
+
            rospy.logerr('Keyboard interrupt detected from the user. %s',
 
+
                        'Exiting the node...')
    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.
 
    """
 
    # Querying the parameter server to determine Robot model and limb name(s)
 
    rp = intera_interface.RobotParams()
 
    valid_limbs = rp.get_limb_names()
 
    if not valid_limbs:
 
        rp.log_message(("Cannot detect any limb parameters on this robot. "
 
                        "Exiting."), "ERROR")
 
    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
 
    # Parsing Input Arguments
 
    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
 
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
 
    parser.add_argument(
 
        "-l", "--limb", dest="limb", default=valid_limbs[0],
 
        choices=valid_limbs,
 
        help='limb on which to attach joint springs'
 
        )
 
    args = parser.parse_args(rospy.myargv()[1:])
 
    # Grabbing Robot-specific parameters for Dynamic Reconfigure
 
    config_name = ''.join([robot_name,"JointSpringsExampleConfig"])
 
    config_module = "intera_examples.cfg"
 
    cfg = importlib.import_module('.'.join([config_module,config_name]))
 
    # Starting node connection to ROS
 
    print("Initializing node... ")
 
    rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb))
 
    dynamic_cfg_srv = Server(cfg, lambda config, level: config)
 
    js = JointSprings(dynamic_cfg_srv, limb=args.limb)
 
    # register shutdown callback
 
    rospy.on_shutdown(js.clean_shutdown)
 
    js.move_to_neutral()
 
    js.attach_springs()
 
 
 
  
if __name__ == "__main__":
+
if __name__ == '__main__':
 
     main()
 
     main()
 
</syntaxhighlight>
 
</syntaxhighlight>
  
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.
+
Once interrupted, the script exits gracefully printing an error.
  
 
</div>
 
</div>

Latest revision as of 19:54, 28 November 2017

This example shows interaction control usage by 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.

Introduction

NEW functionality in the SDK/Intera 5.2 Update!
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.

Usage

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

$ rosrun intera_examples set_interaction_options.py

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

[INFO] [WallTime: 1511299000.977577] header: 
  seq: 1
  stamp: 
    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]
interaction_frame: 
  position: 
    x: 0
    y: 0
    z: 0
  orientation: 
    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.

Arguments

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

$ rosrun intera_examples set_interaction_options.py -h
usage: set_interaction_options.py [-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 set_interaction_options.py  [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)

    -ef
    --> 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
  -k K_IMPEDANCE [K_IMPEDANCE ...], --K_impedance K_IMPEDANCE [K_IMPEDANCE ...]
                        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
                        directions
  -fr INTERACTION_FRAME [INTERACTION_FRAME ...], --interaction_frame INTERACTION_FRAME [INTERACTION_FRAME ...]
                        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
  -f FORCE_COMMAND [FORCE_COMMAND ...], --force_command FORCE_COMMAND [FORCE_COMMAND ...]
                        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
  -kn K_NULLSPACE [K_NULLSPACE ...], --K_nullspace K_NULLSPACE [K_NULLSPACE ...]
                        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
                        feature.
  -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 rospy
34 from intera_core_msgs.msg import InteractionControlCommand
35 import argparse
36 from geometry_msgs.msg import Pose
37 from intera_motion_interface import InteractionOptions
38 from intera_motion_interface.utility_functions import int2bool

This imports the intera motion interface to access the InteractionOptions class. Also, InteractionControlCommand message is imported from geometry_msgs.msg.

43     try:
44         rospy.init_node('set_interaction_options_py')
45         pub = rospy.Publisher('/robot/limb/right/interaction_control_command', InteractionControlCommand, queue_size = 1)
46         rospy.sleep(0.5)
47 
48         if args.rate > 0:
49             rate = rospy.Rate(args.rate)
50         elif args.rate == 0:
51             rospy.logwarn('Interaction control options will be set only once!')
52         elif args.rate < 0:
53             rospy.logerr('Invalid publish rate!')

The rospy is first initialized, and a publisher for ros topic (/robot/limb/right/interaction_control_command) is created. The publish rate is set to what is provided in the argument.

 69         # set the interaction control options in the current configuration
 70         interaction_options = InteractionOptions()
 71 
 72         interaction_options.set_interaction_control_active(int2bool(args.interaction_active))
 73         interaction_options.set_K_impedance(args.K_impedance)
 74         interaction_options.set_max_impedance(int2bool(args.max_impedance))
 75         interaction_options.set_interaction_control_mode(args.interaction_control_mode)
 76         interaction_options.set_in_endpoint_frame(args.in_endpoint_frame)
 77         interaction_options.set_force_command(args.force_command)
 78         interaction_options.set_K_nullspace(args.K_nullspace)
 79         if len(args.interaction_frame) < 7:
 80             rospy.logerr('The number of elements must be 7!')
 81         elif len(args.interaction_frame) == 7:
 82             quat_sum_square = args.interaction_frame[3]*args.interaction_frame[3] + args.interaction_frame[4]*args.interaction_frame[4] 
 83             + args.interaction_frame[5]*args.interaction_frame[5] + args.interaction_frame[6]*args.interaction_frame[6]
 84             if quat_sum_square  < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
 85                 interaction_frame = Pose()
 86                 interaction_frame.position.x = args.interaction_frame[0]
 87                 interaction_frame.position.y = args.interaction_frame[1]
 88                 interaction_frame.position.z = args.interaction_frame[2]
 89                 interaction_frame.orientation.w = args.interaction_frame[3]
 90                 interaction_frame.orientation.x = args.interaction_frame[4]
 91                 interaction_frame.orientation.y = args.interaction_frame[5]
 92                 interaction_frame.orientation.z = args.interaction_frame[6]
 93                 interaction_options.set_interaction_frame(interaction_frame)
 94             else:
 95                 rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
 96         else:
 97             rospy.logerr('Invalid input to interaction_frame!')
 98 
 99         interaction_options.set_disable_damping_in_force_control(args.disable_damping_in_force_control)
100         interaction_options.set_disable_reference_resetting(args.disable_reference_resetting)
101         interaction_options.set_rotations_for_constrained_zeroG(args.rotations_for_constrained_zeroG)
102 
103         msg = interaction_options.to_msg()

An object for InteractionOptions class is created. Then the interaction parameters are set as specified in the arguments using the helper functions provided by the class. After setting all the parameters, a ROS message is generated.

81         # print the resultant interaction options once
82         rospy.loginfo(msg)
83         pub.publish(msg)
84 
85         if args.rate > 0:
86             while not rospy.is_shutdown():
87                 rate.sleep()
88                 pub.publish(msg)

The generated ROS message is printed out once and it is published at a specified rate.

155     except rospy.ROSInterruptException:
156             rospy.logerr('Keyboard interrupt detected from the user. %s',
157                          'Exiting the node...')
158 
159 if __name__ == '__main__':
160     main()

Once interrupted, the script exits gracefully printing an error.