'''

Python 3.x library to control an UR robot through its TCP/IP interfaces

Copyright (C) 2017  Martin Huus Bjerge, Rope Robotics ApS, Denmark

Permission is hereby granted, free of charge, to any person obtaining a copy of this software

and associated documentation files (the "Software"), to deal in the Software without restriction,

including without limitation the rights to use, copy, modify, merge, publish, distribute,

sublicense, and/or sell copies of the Software, and to permit persons to whom the Software

is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies

or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,

INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR

PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL "Rope Robotics ApS" BE LIABLE FOR ANY CLAIM,

DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Except as contained in this notice, the name of "Rope Robotics ApS" shall not be used

in advertising or otherwise to promote the sale, use or other dealings in this Software

without prior written authorization from "Rope Robotics ApS".

'''

__author__ = "Martin Huus Bjerge"

__copyright__ = "Copyright 2017, Rope Robotics ApS, Denmark"

__license__ = "MIT License"

import URBasic

import numpy as np

import time

class UrScriptExt(URBasic.urScript.UrScript):

    '''

    Interface to remote access UR script commands, and add some extended features as well.

    For more details see the script manual at this site:

    http://www.universal-robots.com/download/

    Beside the implementation of the script interface, this class also inherits from the

    Real Time Client and RTDE interface and thereby also open a connection to these data interfaces.

    The Real Time Client in this version is only used to send program and script commands

    to the robot, not to read data from the robot, all data reading is done via the RTDE interface.

    This class also opens a connection to the UR Dashboard server and enables you to

    e.g. reset error and warnings from the UR controller.

    The constructor takes a UR robot hostname as input, and a RTDE configuration file, and optional a logger object.

    Input parameters:

    host (string):  hostname or IP of UR Robot (RT CLient server)

    rtde_conf_filename (string):  Path to xml file describing what channels to activate

    logger (URBasis_DataLogging obj): A instance if a logger object if common logging is needed.

    Example:

    rob = URBasic.urScriptExt.UrScriptExt('192.168.56.101', rtde_conf_filename='rtde_configuration.xml')

    self.close_rtc()

    '''

    def __init__(self, host, robotModel, hasForceTorque=False, conf_filename=None):

        if host is None:  # Only for enable code completion

            return

        super(UrScriptExt, self).__init__(host, robotModel, hasForceTorque, conf_filename)

        logger = URBasic.dataLogging.DataLogging()

        name = logger.AddEventLogging(__name__, log2Consol=False)

        self.__logger = logger.__dict__[name]

        self.print_actual_tcp_pose()

        self.print_actual_joint_positions()

        self.__logger.info('Init done')

    def close(self):

        self.print_actual_tcp_pose()

        self.print_actual_joint_positions()

        self.robotConnector.close()

    def reset_error(self):

        '''

        Check if the UR controller is powered on and ready to run.

        If controller isn't power on it will be power up.

        If there is a safety error, it will be tried rest it once.

        Return Value:

        state (boolean): True of power is on and no safety errors active.

        '''

        if not self.robotConnector.RobotModel.RobotStatus().PowerOn:

            # self.robotConnector.DashboardClient.PowerOn()

            self.robotConnector.DashboardClient.ur_power_on()

            self.robotConnector.DashboardClient.wait_dbs()

            # self.robotConnector.DashboardClient.BrakeRelease()

            self.robotConnector.DashboardClient.ur_brake_release()

            self.robotConnector.DashboardClient.wait_dbs()

        if self.robotConnector.RobotModel.SafetyStatus().StoppedDueToSafety:  # self.get_safety_status()['StoppedDueToSafety']:

            # self.robotConnector.DashboardClient.UnlockProtectiveStop()

            self.robotConnector.DashboardClient.ur_unlock_protective_stop()

            self.robotConnector.DashboardClient.wait_dbs()

            # self.robotConnector.DashboardClient.CloseSafetyPopup()

            self.robotConnector.DashboardClient.ur_close_safety_popup()

            self.robotConnector.DashboardClient.wait_dbs()

            # self.robotConnector.DashboardClient.BrakeRelease()

            self.robotConnector.DashboardClient.ur_brake_release()

            self.robotConnector.DashboardClient.wait_dbs()

            # ADDED: If there was a safety stop -> reupload the realtime control program

            self.init_realtime_control()

        # return self.get_robot_status()['PowerOn'] & (not self.get_safety_status()['StoppedDueToSafety'])

        return self.robotConnector.RobotModel.RobotStatus().PowerOn and not self.robotConnector.RobotModel.SafetyStatus().StoppedDueToSafety

    def get_in(self, port, wait=True):

        '''

        Get input signal level

        Parameters:

        port (HW profile str): Hardware profile tag

        wait (bool): True if wait for next RTDE sample, False, to get the latest sample

        Return Value:

        out (bool or float), The signal level.

        '''

        if 'BCI' == port[:3]:

            return self.get_configurable_digital_in(int(port[4:]), wait)

        elif 'BDI' == port[:3]:

            return self.get_standard_digital_in(int(port[4:]), wait)

        elif 'BAI' == port[:3]:

            return self.get_standard_analog_in(int(port[4:]), wait)

    def set_output(self, port, value):

        '''

        Get output signal level

        Parameters:

        port (HW profile str): Hardware profile tag

        value (bool or float): The output value to be set

        Return Value:

        Status (bool): Status, True if signal set successfully.

        '''

        if 'BCO' == port[:3]:

            self.set_configurable_digital_out(int(port[4:]), value)

        elif 'BDO' == port[:3]:

            self.set_standard_digital_out(int(port[4:]), value)

        elif 'BAO' == port[:3]:

            pass

        elif 'TDO' == port[:3]:

            pass

            # if self.sendData():

            #    return True

            return True  # Vi har sendt det .. vi checker ikke

        else:

            return False

    def init_force_remote(self, task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], f_type=2):

        '''

        The Force Remote function enables changing the force settings dynamically,

        without sending new programs to the robot, and thereby exit and enter force mode again.

        As the new settings are send via RTDE, the force can be updated every 8ms.

        This function initializes the remote force function,

        by sending a program to the robot that can receive new force settings.

        See "force_mode" for more details on force functions

        Parameters:

        task_frame (6D-vector): Initial task frame (can be changed via the update function)

        f_type (int): Initial force type (can be changed via the update function)

        Return Value:

        Status (bool): Status, True if successfully initialized.

        '''

        if not self.robotConnector.RTDE.isRunning():

            self.__logger.error('RTDE need to be running to use force remote')

            return False

        selection_vector = [0, 0, 0, 0, 0, 0]

        wrench = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

        limits = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

        self.robotConnector.RTDE.setData('input_int_register_0', selection_vector[0])

        self.robotConnector.RTDE.setData('input_int_register_1', selection_vector[1])

        self.robotConnector.RTDE.setData('input_int_register_2', selection_vector[2])

        self.robotConnector.RTDE.setData('input_int_register_3', selection_vector[3])

        self.robotConnector.RTDE.setData('input_int_register_4', selection_vector[4])

        self.robotConnector.RTDE.setData('input_int_register_5', selection_vector[5])

        self.robotConnector.RTDE.setData('input_double_register_0', wrench[0])

        self.robotConnector.RTDE.setData('input_double_register_1', wrench[1])

        self.robotConnector.RTDE.setData('input_double_register_2', wrench[2])

        self.robotConnector.RTDE.setData('input_double_register_3', wrench[3])

        self.robotConnector.RTDE.setData('input_double_register_4', wrench[4])

        self.robotConnector.RTDE.setData('input_double_register_5', wrench[5])

        self.robotConnector.RTDE.setData('input_double_register_6', limits[0])

        self.robotConnector.RTDE.setData('input_double_register_7', limits[1])

        self.robotConnector.RTDE.setData('input_double_register_8', limits[2])

        self.robotConnector.RTDE.setData('input_double_register_9', limits[3])

        self.robotConnector.RTDE.setData('input_double_register_10', limits[4])

        self.robotConnector.RTDE.setData('input_double_register_11', limits[5])

        self.robotConnector.RTDE.setData('input_double_register_12', task_frame[0])

        self.robotConnector.RTDE.setData('input_double_register_13', task_frame[1])

        self.robotConnector.RTDE.setData('input_double_register_14', task_frame[2])

        self.robotConnector.RTDE.setData('input_double_register_15', task_frame[3])

        self.robotConnector.RTDE.setData('input_double_register_16', task_frame[4])

        self.robotConnector.RTDE.setData('input_double_register_17', task_frame[5])

        self.robotConnector.RTDE.setData('input_int_register_6', f_type)

        self.robotConnector.RTDE.sendData()

        prog = '''def force_remote():

    while (True):

        global task_frame =  p[read_input_float_register(12),

                              read_input_float_register(13),

                              read_input_float_register(14),

                              read_input_float_register(15),

                              read_input_float_register(16),

                              read_input_float_register(17)]

        global selection_vector = [ read_input_integer_register(0),

                                    read_input_integer_register(1),

                                    read_input_integer_register(2),

                                    read_input_integer_register(3),

                                    read_input_integer_register(4),

                                    read_input_integer_register(5)]

        global wrench = [ read_input_float_register(0),

                          read_input_float_register(1),

                          read_input_float_register(2),

                          read_input_float_register(3),

                          read_input_float_register(4),

                          read_input_float_register(5)]

        global limits = [ read_input_float_register(6),

                          read_input_float_register(7),

                          read_input_float_register(8),

                          read_input_float_register(9),

                          read_input_float_register(10),

                          read_input_float_register(11)]

        global f_type = read_input_integer_register(6)

        force_mode(task_frame, selection_vector, wrench, f_type , limits)

        sync()

    end

end

'''

        self.robotConnector.RealTimeClient.SendProgram(prog.format(**locals()))

        self.robotConnector.RobotModel.forceRemoteActiveFlag = True

    def set_force_remote(self, task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], selection_vector=[0, 0, 0, 0, 0, 0],

                         wrench=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], limits=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1], f_type=2):

        '''

        Update/set remote force, see "init_force_remote" for more details.

        Parameters:

        task frame: A pose vector that defines the force frame relative to the base frame.

        selection vector: A 6d vector that may only contain 0 or 1. 1 means that the robot will be

                          compliant in the corresponding axis of the task frame, 0 means the robot is

                          not compliant along/about that axis.

        wrench: The forces/torques the robot is to apply to its environment. These values

                have different meanings whether they correspond to a compliant axis or not.

                Compliant axis: The robot will adjust its position along/about the axis in order

                to achieve the specified force/torque. Non-compliant axis: The robot follows

                the trajectory of the program but will account for an external force/torque

                of the specified value.

        limits: A 6d vector with float values that are interpreted differently for

                compliant/non-compliant axes:

                Compliant axes: The limit values for compliant axes are the maximum

                                allowed tcp speed along/about the axis.

                Non-compliant axes: The limit values for non-compliant axes are the

                                    maximum allowed deviation along/about an axis between the

                                    actual tcp position and the one set by the program.

        f_type: An integer specifying how the robot interprets the force frame.

                1: The force frame is transformed in a way such that its y-axis is aligned with a vector

                   pointing from the robot tcp towards the origin of the force frame.

                2: The force frame is not transformed.

                3: The force frame is transformed in a way such that its x-axis is the projection of

                   the robot tcp velocity vector onto the x-y plane of the force frame.

                All other values of f_type are invalid.

        Return Value:

        Status (bool): Status, True if parameters successfully updated.

        '''

        if not self.robotConnector.RobotModel.forceRemoteActiveFlag:

            self.init_force_remote(task_frame, f_type)

        if self.robotConnector.RTDE.isRunning() and self.robotConnector.RobotModel.forceRemoteActiveFlag:

            self.robotConnector.RTDE.setData('input_int_register_0', selection_vector[0])

            self.robotConnector.RTDE.setData('input_int_register_1', selection_vector[1])

            self.robotConnector.RTDE.setData('input_int_register_2', selection_vector[2])

            self.robotConnector.RTDE.setData('input_int_register_3', selection_vector[3])

            self.robotConnector.RTDE.setData('input_int_register_4', selection_vector[4])

            self.robotConnector.RTDE.setData('input_int_register_5', selection_vector[5])

            self.robotConnector.RTDE.setData('input_double_register_0', wrench[0])

            self.robotConnector.RTDE.setData('input_double_register_1', wrench[1])

            self.robotConnector.RTDE.setData('input_double_register_2', wrench[2])

            self.robotConnector.RTDE.setData('input_double_register_3', wrench[3])

            self.robotConnector.RTDE.setData('input_double_register_4', wrench[4])

            self.robotConnector.RTDE.setData('input_double_register_5', wrench[5])

            self.robotConnector.RTDE.setData('input_double_register_6', limits[0])

            self.robotConnector.RTDE.setData('input_double_register_7', limits[1])

            self.robotConnector.RTDE.setData('input_double_register_8', limits[2])

            self.robotConnector.RTDE.setData('input_double_register_9', limits[3])

            self.robotConnector.RTDE.setData('input_double_register_10', limits[4])

            self.robotConnector.RTDE.setData('input_double_register_11', limits[5])

            self.robotConnector.RTDE.setData('input_double_register_12', task_frame[0])

            self.robotConnector.RTDE.setData('input_double_register_13', task_frame[1])

            self.robotConnector.RTDE.setData('input_double_register_14', task_frame[2])

            self.robotConnector.RTDE.setData('input_double_register_15', task_frame[3])

            self.robotConnector.RTDE.setData('input_double_register_16', task_frame[4])

            self.robotConnector.RTDE.setData('input_double_register_17', task_frame[5])

            self.robotConnector.RTDE.setData('input_int_register_6', f_type)

            self.robotConnector.RTDE.sendData()

            return True

        else:

            if not self.robotConnector.RobotModel.forceRemoteActiveFlag:

                self.__logger.warning('Force Remote not initialized')

            else:

                self.__logger.warning('RTDE is not running')

            return False

    def init_realtime_control(self):

        '''

        The realtime control mode enables continuous updates to a servoj program which is

        initialized by this function. This way no new program has to be sent to the robot

        and the robot can perform a smooth trajectory.

        Sending new servoj commands is done by utilizing RTDE of this library

        Parameters:

        sample_time: time of one sample, standard is 8ms as this is the thread-cycle time of UR

        Return Value:

        Status (bool): Status, True if successfully initialized.

        '''

        if not self.robotConnector.RTDE.isRunning():

            self.__logger.error('RTDE needs to be running to use realtime control')

            return False

        # get current tcp pos

        init_pose = self.get_actual_tcp_pose()

        self.robotConnector.RTDE.setData('input_double_register_0', init_pose[0])

        self.robotConnector.RTDE.setData('input_double_register_1', init_pose[1])

        self.robotConnector.RTDE.setData('input_double_register_2', init_pose[2])

        self.robotConnector.RTDE.setData('input_double_register_3', init_pose[3])

        self.robotConnector.RTDE.setData('input_double_register_4', init_pose[4])

        self.robotConnector.RTDE.setData('input_double_register_5', init_pose[5])

        self.robotConnector.RTDE.sendData()

        prog = '''def realtime_control():

    while (True):

        new_pose = p[read_input_float_register(0),

                    read_input_float_register(1),

                    read_input_float_register(2),

                    read_input_float_register(3),

                    read_input_float_register(4),

                    read_input_float_register(5)]

        servoj(get_inverse_kin(new_pose), t=0.2, lookahead_time= 0.1, gain=350)

        sync()

    end

end

'''

        # , t=0.1

        self.robotConnector.RealTimeClient.SendProgram(prog.format(**locals()))

        self.robotConnector.RobotModel.realtimeControlFlag = True

    def set_realtime_pose(self, pose):

        """

        Update/Set realtime_pose after sample_time seconds.

        Parameters

        pose: pose to transition to in sample_time seconds

        sample_time: time to take to perform servoj to next pose. 0.008 = thread cycle time of Universal Robot

        """

        if not self.robotConnector.RobotModel.realtimeControlFlag:

            print("Realtime control not initialized!")

            self.init_realtime_control()

            print("Realtime control initialized!")

        if self.robotConnector.RTDE.isRunning() and self.robotConnector.RobotModel.realtimeControlFlag:

            self.robotConnector.RTDE.setData('input_double_register_0', pose[0])

            self.robotConnector.RTDE.setData('input_double_register_1', pose[1])

            self.robotConnector.RTDE.setData('input_double_register_2', pose[2])

            self.robotConnector.RTDE.setData('input_double_register_3', pose[3])

            self.robotConnector.RTDE.setData('input_double_register_4', pose[4])

            self.robotConnector.RTDE.setData('input_double_register_5', pose[5])

            self.robotConnector.RTDE.sendData()

            return True

        else:

            if not self.robotConnector.RobotModel.realtimeControlFlag:

                self.__logger.warning('Realtime Remote Control not initialized')

            else:

                self.__logger.warning('RTDE is not running')

            return False

    def move_force_2stop(self, start_tolerance=0.01,

                         stop_tolerance=0.01,

                         wrench_gain=[1.0, 1.0, 1.0, 1.0, 1.0, 1.0],

                         timeout=10,

                         task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],

                         selection_vector=[0, 0, 0, 0, 0, 0],

                         wrench=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],

                         limits=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1],

                         f_type=2):

        '''

        Move force will set the robot in force mode (see force_mode) and move the TCP until it meets an object making the TCP stand still.

        Parameters:

        start_tolerance (float): sum of all elements in a pose vector defining a robot has started moving (60 samples)

        stop_tolerance (float): sum of all elements in a pose vector defining a standing still robot (60 samples)

        wrench_gain (6D vector): Gain multiplied with wrench each 8ms sample

        timeout (float): Seconds to timeout if tolerance not reached

        task frame: A pose vector that defines the force frame relative to the base frame.

        selection vector: A 6d vector that may only contain 0 or 1. 1 means that the robot will be

                          compliant in the corresponding axis of the task frame, 0 means the robot is

                          not compliant along/about that axis.

        wrench: The forces/torques the robot is to apply to its environment. These values

                have different meanings whether they correspond to a compliant axis or not.

                Compliant axis: The robot will adjust its position along/about the axis in order

                to achieve the specified force/torque. Non-compliant axis: The robot follows

                the trajectory of the program but will account for an external force/torque

                of the specified value.

        limits: A 6d vector with float values that are interpreted differently for

                compliant/non-compliant axes:

                Compliant axes: The limit values for compliant axes are the maximum

                                allowed tcp speed along/about the axis.

                Non-compliant axes: The limit values for non-compliant axes are the

                                    maximum allowed deviation along/about an axis between the

                                    actual tcp position and the one set by the program.

        f_type: An integer specifying how the robot interprets the force frame.

                1: The force frame is transformed in a way such that its y-axis is aligned with a vector

                   pointing from the robot tcp towards the origin of the force frame.

                2: The force frame is not transformed.

                3: The force frame is transformed in a way such that its x-axis is the projection of

                   the robot tcp velocity vector onto the x-y plane of the force frame.

                All other values of f_type are invalid.

        Return Value:

        Status (bool): Status, True if signal set successfully.

        '''

        timeoutcnt = 125 * timeout

        wrench = np.array(wrench)

        wrench_gain = np.array(wrench_gain)

        self.set_force_remote(task_frame, selection_vector, wrench, limits, f_type)

        dist = np.array(range(60), float)

        dist.fill(0.)

        cnt = 0

        old_pose = self.get_actual_tcp_pose() * np.array(selection_vector)

        while np.sum(dist) < start_tolerance and cnt < timeoutcnt:

            new_pose = self.get_actual_tcp_pose() * np.array(selection_vector)

            wrench = wrench * wrench_gain  # Need a max wrencd check

            self.set_force_remote(task_frame, selection_vector, wrench, limits, f_type)

            dist[np.mod(cnt, 60)] = np.abs(np.sum(new_pose - old_pose))

            old_pose = new_pose

            cnt += 1

        # Check if robot started to move

        if cnt < timeoutcnt:

            dist.fill(stop_tolerance)

            cnt = 0

            while np.sum(dist) > stop_tolerance and cnt < timeoutcnt:

                new_pose = self.get_actual_tcp_pose() * np.array(selection_vector)

                dist[np.mod(cnt, 60)] = np.abs(np.sum(new_pose - old_pose))

                old_pose = new_pose

                cnt += 1

        self.set_force_remote(task_frame, selection_vector, [0, 0, 0, 0, 0, 0], limits, f_type)

        self.end_force_mode()

        if cnt >= timeoutcnt:

            return False

        else:

            return True

    def move_force(self, pose=None,

                   a=1.2,

                   v=0.25,

                   t=0,

                   r=0.0,

                   movetype='l',

                   task_frame=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],

                   selection_vector=[0, 0, 0, 0, 0, 0],

                   wrench=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],

                   limits=[0.1, 0.1, 0.1, 0.1, 0.1, 0.1],

                   f_type=2,

                   wait=True,

                   q=None):

        """

        Concatenate several move commands and applies a blending radius

        pose or q is a list of pose or joint-pose, and apply a force in a direction

        Parameters:

        pose: list of target pose (pose can also be specified as joint

              positions, then forward kinematics is used to calculate the corresponding pose see q)

        a:    tool acceleration [m/s^2]

        v:    tool speed [m/s]

        t:    time [S]

        r:    blend radius [m]

        movetype: (str): 'j', 'l', 'p', 'c'

        task frame: A pose vector that defines the force frame relative to the base frame.

        selection vector: A 6d vector that may only contain 0 or 1. 1 means that the robot will be

                          compliant in the corresponding axis of the task frame, 0 means the robot is

                          not compliant along/about that axis.

        wrench: The forces/torques the robot is to apply to its environment. These values

                have different meanings whether they correspond to a compliant axis or not.

                Compliant axis: The robot will adjust its position along/about the axis in order

                to achieve the specified force/torque. Non-compliant axis: The robot follows

                the trajectory of the program but will account for an external force/torque

                of the specified value.

        limits: A 6d vector with float values that are interpreted differently for

                compliant/non-compliant axes:

                Compliant axes: The limit values for compliant axes are the maximum

                                allowed tcp speed along/about the axis.

                Non-compliant axes: The limit values for non-compliant axes are the

                                    maximum allowed deviation along/about an axis between the

                                    actual tcp position and the one set by the program.

        f_type: An integer specifying how the robot interprets the force frame.

                1: The force frame is transformed in a way such that its y-axis is aligned with a vector

                   pointing from the robot tcp towards the origin of the force frame.

                2: The force frame is not transformed.

                3: The force frame is transformed in a way such that its x-axis is the projection of

                   the robot tcp velocity vector onto the x-y plane of the force frame.

                All other values of f_type are invalid.

        wait: function return when movement is finished

        q:    list of target joint positions

        Return Value:

        Status (bool): Status, True if signal set successfully.

        """

        task_frame = np.array(task_frame)

        if np.size(task_frame.shape) == 2:

            prefix = "p"

            t_val = ''

            if pose is None:

                prefix = ""

                pose = q

            pose = np.array(pose)

            if movetype == 'j' or movetype == 'l':

                tval = 't={t},'.format(**locals())

            prg = 'def move_force():\n'

            for idx in range(np.size(pose, 0)):

                posex = np.round(pose[idx], 4)

                posex = posex.tolist()

                task_framex = np.round(task_frame[idx], 4)

                task_framex = task_framex.tolist()

                if (np.size(pose, 0) - 1) == idx:

                    r = 0

                prg += '    force_mode(p{task_framex}, {selection_vector}, {wrench}, {f_type}, {limits})\n'.format(

                    **locals())

                prg += '    move{movetype}({prefix}{posex}, a={a}, v={v}, {t_val} r={r})\n'.format(**locals())

            prg += '    stopl({a})\n'.format(**locals())

            prg += '    end_force_mode()\nend\n'

        else:

            prg = '''def move_force():

    force_mode(p{task_frame}, {selection_vector}, {wrench}, {f_type}, {limits})

{movestr}

    end_force_mode()

end

'''

            task_frame = task_frame.tolist()

            movestr = self._move(movetype, pose, a, v, t, r, wait, q)

        self.robotConnector.RealTimeClient.SendProgram(prg.format(**locals()))

        if (wait):

            self.waitRobotIdleOrStopFlag()

    def movej_waypoints(self, waypoints, wait=True):

        '''

        Movej along multiple waypoints. By configuring a blend radius continuous movements can be enabled.

        Parameters:

        waypoints: List waypoint dictionaries {pose: [6d], a, v, t, r}

        '''

        prg = '''def move_waypoints():

{exec_str}

end

'''

        exec_str = ""

        for waypoint in waypoints:

            movestr = self._move(movetype='j', **waypoint)

            exec_str += movestr + "\n"

        programString = prg.format(**locals())

        self.robotConnector.RealTimeClient.SendProgram(programString)

        if (wait):

            self.waitRobotIdleOrStopFlag()

    def movel_waypoints(self, waypoints, wait=True):

        '''

        Movel along multiple waypoints. By configuring a blend radius continuous movements can be enabled.

        Parameters:

        waypoints: List waypoint dictionaries {pose: [6d], a, v, t, r}

        '''

        prg = '''def move_waypoints():

{exec_str}

end

'''

        exec_str = ""

        for waypoint in waypoints:

            movestr = self._move(movetype='l', **waypoint)

            exec_str += movestr + "\n"

        programString = prg.format(**locals())

        self.robotConnector.RealTimeClient.SendProgram(programString)

        if (wait):

            self.waitRobotIdleOrStopFlag()

    def print_actual_tcp_pose(self):

        '''

        print the actual TCP pose

        '''

        self.print_pose(self.get_actual_tcp_pose())

    def print_actual_joint_positions(self):

        '''

        print the actual TCP pose

        '''

        self.print_pose(q=self.get_actual_joint_positions())

    def print_pose(self, pose=None, q=None):

        '''

        print a pose

        '''

        if q is None:

            print('Robot Pose: [{: 06.4f}, {: 06.4f}, {: 06.4f},   {: 06.4f}, {: 06.4f}, {: 06.4f}]'.format(*pose))

        else:

            print('Robot joint positions: [{: 06.4f}, {: 06.4f}, {: 06.4f},   {: 06.4f}, {: 06.4f}, {: 06.4f}]'.format(

                *q))