Set Interaction Options Example

Jump to: navigation , search

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.

  1. import rospy
  2. from intera_core_msgs.msg import InteractionControlCommand
  3. import argparse
  4. from geometry_msgs.msg import Pose
  5. from intera_motion_interface import InteractionOptions
  6. 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.

  1.     try:
  2.         rospy.init_node('set_interaction_options_py')
  3.         pub = rospy.Publisher('/robot/limb/right/interaction_control_command', InteractionControlCommand, queue_size = 1)
  4.         rospy.sleep(0.5)
  5.  
  6.         if args.rate > 0:
  7.             rate = rospy.Rate(args.rate)
  8.         elif args.rate == 0:
  9.             rospy.logwarn('Interaction control options will be set only once!')
  10.         elif args.rate < 0:
  11.             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.

  1.         # set the interaction control options in the current configuration
  2.         interaction_options = InteractionOptions()
  3.  
  4.         interaction_options.set_interaction_control_active(int2bool(args.interaction_active))
  5.         interaction_options.set_K_impedance(args.K_impedance)
  6.         interaction_options.set_max_impedance(int2bool(args.max_impedance))
  7.         interaction_options.set_interaction_control_mode(args.interaction_control_mode)
  8.         interaction_options.set_in_endpoint_frame(args.in_endpoint_frame)
  9.         interaction_options.set_force_command(args.force_command)
  10.         interaction_options.set_K_nullspace(args.K_nullspace)
  11.         if len(args.interaction_frame) < 7:
  12.             rospy.logerr('The number of elements must be 7!')
  13.         elif len(args.interaction_frame) == 7:
  14.             quat_sum_square = args.interaction_frame[3]*args.interaction_frame[3] + args.interaction_frame[4]*args.interaction_frame[4]
  15.             + args.interaction_frame[5]*args.interaction_frame[5] + args.interaction_frame[6]*args.interaction_frame[6]
  16.             if quat_sum_square  < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
  17.                 interaction_frame = Pose()
  18.                 interaction_frame.position.x = args.interaction_frame[0]
  19.                 interaction_frame.position.y = args.interaction_frame[1]
  20.                 interaction_frame.position.z = args.interaction_frame[2]
  21.                 interaction_frame.orientation.w = args.interaction_frame[3]
  22.                 interaction_frame.orientation.x = args.interaction_frame[4]
  23.                 interaction_frame.orientation.y = args.interaction_frame[5]
  24.                 interaction_frame.orientation.z = args.interaction_frame[6]
  25.                 interaction_options.set_interaction_frame(interaction_frame)
  26.             else:
  27.                 rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
  28.         else:
  29.             rospy.logerr('Invalid input to interaction_frame!')
  30.  
  31.         interaction_options.set_disable_damping_in_force_control(args.disable_damping_in_force_control)
  32.         interaction_options.set_disable_reference_resetting(args.disable_reference_resetting)
  33.         interaction_options.set_rotations_for_constrained_zeroG(args.rotations_for_constrained_zeroG)
  34.  
  35.         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.

  1.         # print the resultant interaction options once
  2.         rospy.loginfo(msg)
  3.         pub.publish(msg)
  4.  
  5.         if args.rate > 0:
  6.             while not rospy.is_shutdown():
  7.                 rate.sleep()
  8.                 pub.publish(msg)

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

  1.     except rospy.ROSInterruptException:
  2.             rospy.logerr('Keyboard interrupt detected from the user. %s',
  3.                          'Exiting the node...')
  4.  
  5. if __name__ == '__main__':
  6.     main()

Once interrupted, the script exits gracefully printing an error.