text: # Copyright (c) 2016-2018, Rethink Robotics Inc.
  #
  # Licensed under the Apache License, Version 2.0 (the "License");
  # you may not use this file except in compliance with the License.
  # You may obtain a copy of the License at
  #
  #     http://www.apache.org/licenses/LICENSE-2.0
  #
  # Unless required by applicable law or agreed to in writing, software
  # distributed under the License is distributed on an "AS IS" BASIS,
  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  # See the License for the specific language governing permissions and
  # limitations under the License.
  
  import rospy
  from intera_core_msgs.msg import InteractionControlCommand
  from geometry_msgs.msg import Pose, Point, Quaternion
  from copy import deepcopy
  from utility_functions import (
      ensure_path_to_file_exists,
      clamp_float_warn
  )
  from rospy_message_converter import message_converter
  import yaml
  
  class InteractionOptions(object):
      """
      This class is a wrapper for the intera message:  InteractionControlCommand.msg
      It's primary purpose is to facilitate message creation and input checking.
      """
  
      n_dim_cart = 6 # number of dimensions in Cartesian space
  
      # constants for interaction control modes
      force_mode = InteractionControlCommand.FORCE_MODE
      impedance_mode = InteractionControlCommand.IMPEDANCE_MODE
      impedance_limit_mode = InteractionControlCommand.IMPEDANCE_WITH_FORCE_LIMIT_MODE
      force_limit_mode = InteractionControlCommand.FORCE_WITH_MOTION_LIMIT_MODE
  
      # default parameters
      default_interaction_control_active = True
      default_n_dim_joint = 7
      default_K_impedance = [1300, 1300, 1300, 30, 30, 30]  # N/m for first three and Nm/rad for the rest
      default_K_nullspace = [5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0]  # Nm/rad
      default_endpoint_name = 'right_hand'
      default_max_impedance = [True]*n_dim_cart
      default_in_endpoint_frame = False
      default_force_command = [0.0]*n_dim_cart
      default_interaction_frame = Pose(position=Point(x=0.0,y=0.0,z=0.0),
          orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=1.0))
      default_interaction_control_mode = [impedance_mode]*n_dim_cart
      default_disable_damping_in_force_control = False
      default_disable_reference_resetting = False
      default_rotations_for_constrained_zeroG = False
  
      def __init__(self, header = None,
                   interaction_control_active = default_interaction_control_active,
                   K_impedance = default_K_impedance,
                   max_impedance = default_max_impedance,
                   n_joints = default_n_dim_joint,
                   K_nullspace = default_K_nullspace,
                   force_command= default_force_command,
                   interaction_frame = default_interaction_frame,
                   endpoint_name = default_endpoint_name,
                   in_endpoint_frame = default_in_endpoint_frame,
                   interaction_control_mode = default_interaction_control_mode,
                   disable_damping_in_force_control = default_disable_damping_in_force_control,
                   disable_reference_resetting = default_disable_reference_resetting,
                   rotations_for_constrained_zeroG = default_rotations_for_constrained_zeroG):
          """
          Create a interaction options object. All parameters are
          optional. If ommitted or set to None, then use default value.
  
          See setter functions for more specific details.
          """
          self._data = InteractionControlCommand()
          self.set_header(header)
          self.set_interaction_control_active(interaction_control_active)
          self.set_number_joints(n_joints),
          self.set_K_impedance(K_impedance)
          self.set_max_impedance(max_impedance)
          self.set_K_nullspace(K_nullspace)
          self.set_force_command(force_command)
          self.set_interaction_frame(interaction_frame)
          self.set_endpoint_name(endpoint_name)
          self.set_in_endpoint_frame(in_endpoint_frame)
          self.set_interaction_control_mode(interaction_control_mode)
          self.set_disable_damping_in_force_control(disable_damping_in_force_control)
          self.set_disable_reference_resetting(disable_reference_resetting)
          self.set_rotations_for_constrained_zeroG(rotations_for_constrained_zeroG)
  
      def set_header(self, header = None):
          """
          @param header:
            - None: set default header with current timestamp
            - [header]: set message header
          """
          if header is None:
              self._data.header.stamp = rospy.get_rostime()
              self._data.header.seq = 1
              self._data.header.frame_id = "base"
          else:
              self._data.header = header
  
      def set_interaction_control_active(self, interaction_active = default_interaction_control_active):
          """
          @param interaction_control_active:
            - None:  set it to True by default
            - [bool]: copy the element.
          """
          self._data.interaction_control_active = interaction_active
  
      def set_K_impedance(self, K_impedance = default_K_impedance):
          """
          @param K_impedance (Cartesian stiffness):
            - None:  populate with vector of default values
            - [float]:  copy all elements. Checks length.
          """
          if len(K_impedance) == self.n_dim_cart:
              self._data.K_impedance = deepcopy(K_impedance)
          else:
              rospy.logerr('The number of K Impedance elements must be %d',
                           self.n_dim_cart)
  
      def set_max_impedance(self, max_impedance = default_max_impedance):
          """
          @param max_impedance (impedance modulation state):
            - None:  populate with vector of default values (True)
            - bool:  set each dimension by the input boolean
            - [bool]:  copy all elements. Checks length.
          """
          if len(max_impedance) == 1:
              self._data.max_impedance = [max_impedance[0]]*self.n_dim_cart
          elif len(max_impedance) == self.n_dim_cart:
              self._data.max_impedance = deepcopy(max_impedance)
          else:
              rospy.logerr('The number of max_impedance must be 1 or %d',
                           self.n_dim_cart)
  
      def set_number_joints(self, n_joints = default_n_dim_joint):
          """
          @param n_joints (number of arm joints):
            - None:  use default number
            - [int]: use provided number
          """
          self._n_dim_joint = n_joints
  
      def set_K_nullspace(self, K_nullspace = default_K_nullspace):
          """
          @param K_nullspace (nullspace stiffness gains):
            - None:  populate with vector of default values
            - float:  set each dimension by the input float number
            - [float]:  copy all elements. Checks length.
          """
          if len(K_nullspace) == 1:
              self._data.K_nullspace = [K_nullspace[0]]*self._n_dim_joint
          elif len(K_nullspace) == self._n_dim_joint:
              self._data.K_nullspace = deepcopy(K_nullspace)
          else:
              rospy.logerr('The number of K_nullspace must be 1 or %d',
                           self._n_dim_joint)
  
      def set_force_command(self, force_cmd = default_force_command):
          """
          @param force_command:
            - None:  populate with vector of default values
            - [float]:  copy all elements. Checks length.
          """
          if len(force_cmd) == self.n_dim_cart:
              self._data.force_command = deepcopy(force_cmd)
          else:
              rospy.logerr('The number of force_command elements must be %d',
                           self.n_dim_cart)
  
      def set_interaction_frame(self, interaction_frame = default_interaction_frame):
          """
          @param interaction_frame:
            - None:  populate with vector of default values
            - Pose: copy all the elements
          """
          if isinstance(interaction_frame, Pose):
              self._data.interaction_frame = interaction_frame
          else:
              rospy.logerr('interaction_frame must be of type geometry_msgs.Pose')
  
      def set_endpoint_name(self, endpoint_name = default_endpoint_name):
          """
          @param endpoint_name:
            - None:  set it to a default name (right hand)
            - string: copy element.
          """
          self._data.endpoint_name = endpoint_name
  
      def set_in_endpoint_frame(self, in_endpoint_frame = default_in_endpoint_frame):
          """
          @param in_endpoint_frame:
            - None:  set it to a default frame (False, i.e., base frame)
            - bool:  copy element
          """
          self._data.in_endpoint_frame = in_endpoint_frame
  
      def set_interaction_control_mode(self, interaction_mode = default_interaction_control_mode):
          """
          @param interaction_mode:
            - None:  set impedance mode by default
            - mode:  set each direction by the input mode
              (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
            - [mode]:  copy all elements. Checks length.
          """
          # first check for valid modes
          for i in range(len(interaction_mode)):
              if (interaction_mode[i] < self.impedance_mode
                  or interaction_mode[i] > self.force_limit_mode):
                  rospy.logerr('Interaction mode option %d is invalid', interaction_mode[i])
                  return
  
          if len(interaction_mode) == 1:
              self._data.interaction_control_mode = [interaction_mode[0]]*self.n_dim_cart
          elif len(interaction_mode) == self.n_dim_cart:
              self._data.interaction_control_mode = deepcopy(interaction_mode)
          else:
              rospy.logerr('The number of interaction_control_mode elements must be 1 or %d',
                           self.n_dim_cart)
  
      def set_disable_damping_in_force_control(self, disable_damping_in_force_control = default_disable_damping_in_force_control):
          """
          @param disable_damping_in_force_control:
            - None:  set it to False by default
            - [bool]: copy the element.
          """
          self._data.disable_damping_in_force_control = disable_damping_in_force_control
  
      def set_disable_reference_resetting(self, disable_reference_resetting = default_disable_reference_resetting):
          """
          @param disable_reference_resetting:
            - None:  set it to False by default
            - [bool]: copy the element.
          """
          self._data.disable_reference_resetting = disable_reference_resetting
  
      def set_rotations_for_constrained_zeroG(self, rotations_constrained_zeroG = default_rotations_for_constrained_zeroG):
          """
          @param rotations_for_constrained_zeroG:
            - None:  set it to False by default
            - [bool]: copy the element.
          """
          self._data.rotations_for_constrained_zeroG = rotations_constrained_zeroG
  
      def to_msg(self):
          """
          @return: InteractionControlCommand.msg
          """
          return deepcopy(self._data)
  
      def to_dict(self):
          """
          @return: the interaction control options as a dictionary object
          """
          return message_converter.convert_ros_message_to_dictionary(self._data)
  
      def to_string(self):
          """
          @return: a yaml-formatted string with the interaction control options
          """
          return yaml.dump(self.to_dict(), default_flow_style=False)
  
      def to_yaml_file(self, file_name):
          """
          Write the contents of the interaction control options to a yaml file
          """
          file_name = ensure_path_to_file_exists(file_name)
          with open(file_name, "w") as outfile:
              yaml.dump(self.to_dict(), outfile, default_flow_style=False)
  