<commit_before>import os
import math
import types
import functools
import numpy as np
import ddapp
from ddapp import matlab
from ddapp import callbacks
from ddapp.asynctaskqueue import AsyncTaskQueue
from ddapp.ikconstraints import *

from ddapp import drcargs

class AsyncIKCommunicator():

    STARTUP_COMPLETED = 'STARTUP_COMPLETED'

    def __init__(self, robotURDF, fixedPointFile):

        self.comm = None
        self.outputConsole = None
        self.ready = False
        self.robotURDF = robotURDF
        self.fixedPointFile = fixedPointFile

        self.seedName = 'q_nom'
        self.nominalName = 'q_nom'
        self.infoFunc = None

        self.maxDegreesPerSecond = 30.0
        self.maxBaseMetersPerSecond = 0.05
        self.maxBaseRPYDegreesPerSecond = 2
        self.accelerationParam = 2;
        self.accelerationFraction = 0.3;
        self.maxPlanDuration = 30.0
        self.usePointwise = True
        self.useCollision = False
        self.fixInitialState = True
        self.numberOfAddedKnots = 0
        self.numberOfInterpolatedCollisionChecks = 2
        self.collisionMinDistance = 0.03
        self.majorIterationsLimit = 500
        self.majorOptimalityTolerance = 1e-4
        self.majorFeasibilityTolerance = 1e-6
        self.rrtMaxEdgeLength = 0.05
        self.rrtGoalBias = 1.0
        self.rrtMaxNumVertices = 5000
        self.rrtNSmoothingPasses = 10;

        self.callbacks = callbacks.CallbackRegistry([self.STARTUP_COMPLETED])


    def getStartupCommands(self):

        commands = []
        commands.append('\n%-------- startup --------\n')
        commands.append('format long e')
        commands.append('addpath_control')
        commands.append("addpath([getenv('DRC_BASE'), '/software/ddapp/src/matlab'])")
        commands.append("robotURDF = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.robotURDF, ddapp.getDRCBaseDir()))
        commands.append("fixed_point_file = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.fixedPointFile, ddapp.getDRCBaseDir()))
        commands.append('runIKServer')
        commands.append('\n%------ startup end ------\n')
        return commands

    def _createMatlabClient(self):

        hostname = drcargs.args().matlab_host
        if hostname is not None:
            return matlab.MatlabSocketClient(host=hostname)
        else:
            return matlab.MatlabPipeClient()

    def startServerAsync(self):

        self.comm = matlab.MatlabCommunicator(self._createMatlabClient())
        self.comm.echoToStdOut = False
        self.comm.outputConsole = self.outputConsole

        taskQueue = AsyncTaskQueue()
        taskQueue.addTask(functools.partial(self.comm.sendCommandsAsync, ['\n']))
        taskQueue.addTask(functools.partial(self.comm.sendCommandsAsync, self.getStartupCommands()))
        taskQueue.addTask(self._checkServerStartup)
        taskQueue.addTask(functools.partial(setattr, self.comm, 'echoToStdOut', True))

        self.taskQueue = taskQueue
        self.taskQueue.start()

    def connectStartupCompleted(self, func):
        return self.callbacks.connect(self.STARTUP_COMPLETED, func)

    def disconnectStartupCompleted(self, callbackId):
        self.callbacks.disconnect(callbackId)

    def _checkServerStartup(self):
        try:
            self.ready = self.comm.getFloatArray('ikServerStarted')[0] == 1
        except:
            self.ready = False
        self._notifyStartupCompleted()

    def _notifyStartupCompleted(self):
        self.callbacks.process(self.STARTUP_COMPLETED, self, self.ready)

    def interact(self):
        self.comm.interact()


    def fetchPoseFromServer(self, poseName):
        return self.comm.getFloatArray(poseName)


    def sendPoseToServer(self, pose, poseName):<commit_msg>update ikserver start logic to better handle restarts<commit_after>import os
import math
import types
import functools
import numpy as np
import ddapp
from ddapp import matlab
from ddapp import callbacks
from ddapp.asynctaskqueue import AsyncTaskQueue
from ddapp.ikconstraints import *

from ddapp import drcargs

class AsyncIKCommunicator():

    STARTUP_COMPLETED = 'STARTUP_COMPLETED'

    def __init__(self, robotURDF, fixedPointFile):

        self.comm = None
        self.outputConsole = None
        self.ready = False
        self.restarted = False
        self.robotURDF = robotURDF
        self.fixedPointFile = fixedPointFile

        self.seedName = 'q_nom'
        self.nominalName = 'q_nom'
        self.infoFunc = None

        self.maxDegreesPerSecond = 30.0
        self.maxBaseMetersPerSecond = 0.05
        self.maxBaseRPYDegreesPerSecond = 2
        self.accelerationParam = 2;
        self.accelerationFraction = 0.3;
        self.maxPlanDuration = 30.0
        self.usePointwise = True
        self.useCollision = False
        self.fixInitialState = True
        self.numberOfAddedKnots = 0
        self.numberOfInterpolatedCollisionChecks = 2
        self.collisionMinDistance = 0.03
        self.majorIterationsLimit = 500
        self.majorOptimalityTolerance = 1e-4
        self.majorFeasibilityTolerance = 1e-6
        self.rrtMaxEdgeLength = 0.05
        self.rrtGoalBias = 1.0
        self.rrtMaxNumVertices = 5000
        self.rrtNSmoothingPasses = 10;

        self.callbacks = callbacks.CallbackRegistry([self.STARTUP_COMPLETED])


    def _sendStartupCommands(self):

        if self.restarted:
            return

        commands = []
        commands.append('\n%-------- startup --------\n')
        commands.append('format long e')
        commands.append('addpath_control')
        commands.append("addpath([getenv('DRC_BASE'), '/software/ddapp/src/matlab'])")
        commands.append("robotURDF = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.robotURDF, ddapp.getDRCBaseDir()))
        commands.append("fixed_point_file = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.fixedPointFile, ddapp.getDRCBaseDir()))
        commands.append('runIKServer')
        commands.append('\n%------ startup end ------\n')
        return self.comm.sendCommandsAsync(commands)

    def _createMatlabClient(self):

        hostname = drcargs.args().matlab_host
        if hostname is not None:
            return matlab.MatlabSocketClient(host=hostname)
        else:
            return matlab.MatlabPipeClient()

    def startServerAsync(self):

        self.comm = matlab.MatlabCommunicator(self._createMatlabClient())
        self.comm.echoToStdOut = False
        self.comm.outputConsole = self.outputConsole

        taskQueue = AsyncTaskQueue()
        taskQueue.addTask(functools.partial(self.comm.sendCommandsAsync, ['\n']))
        taskQueue.addTask(self._checkServerRestarted)
        taskQueue.addTask(self._sendStartupCommands)
        taskQueue.addTask(self._checkServerStartup)
        taskQueue.addTask(self._notifyStartupCompleted)
        taskQueue.addTask(functools.partial(setattr, self.comm, 'echoToStdOut', True))

        self.taskQueue = taskQueue
        self.taskQueue.start()

    def connectStartupCompleted(self, func):
        return self.callbacks.connect(self.STARTUP_COMPLETED, func)

    def disconnectStartupCompleted(self, callbackId):
        self.callbacks.disconnect(callbackId)

    def _checkServerStartup(self):
        started = self.comm.getFloatArray("exist('ikServerStarted')")
        self.ready = len(started) and started[0] == 1

    def _checkServerRestarted(self):
        self._checkServerStartup()
        self.restarted = self.ready

    def _notifyStartupCompleted(self):
        self.callbacks.process(self.STARTUP_COMPLETED, self, self.ready)

    def interact(self):
        self.comm.interact()


    def fetchPoseFromServer(self, poseName):
        return self.comm.getFloatArray(poseName)


    def sendPoseToServer(self, pose, poseName):
{'fraction_non_alphanumeric': 0.10298832884442734, 'fraction_numerical': 0.00974733872002052, 'mean_word_length': 2.8603960396039603, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4516879', 'n_tokens_mistral': 2472, 'n_tokens_neox': 2341, 'n_words': 414}