<commit_before>from PythonQt import QtCore, QtGui, QtUiTools
import ddapp.applogic as app
import ddapp.objectmodel as om
from ddapp.timercallback import TimerCallback
from ddapp import robotstate
from ddapp import visualization as vis
from ddapp import transformUtils
from ddapp import ikplanner
from ddapp import footstepsdriver
from ddapp import vtkAll as vtk
from ddapp import drcargs
from ddapp import affordanceurdf
import ddapp.applogic as app

import math
import numpy as np


def addWidgetsToDict(widgets, d):

    for widget in widgets:
        if widget.objectName:
            d[str(widget.objectName)] = widget
        addWidgetsToDict(widget.children(), d)


class WidgetDict(object):

    def __init__(self, widgets):
        addWidgetsToDict(widgets, self.__dict__)


def clearLayout(w):
    children = w.findChildren(QtGui.QWidget)
    for child in children:
        child.delete()


class ConstraintItem(om.ObjectModelItem):

    def __init__(self, constraint):

        linkStr = '(%s)' % constraint.linkName if hasattr(constraint, 'linkName') else ''
        name = '%s %s' % (type(constraint).__name__, linkStr)

        om.ObjectModelItem.__init__(self, name)
        self.constraint = constraint

        for propertyName, propertyValue in constraint:

            if isinstance(propertyValue, np.ndarray):
                propertyValue = propertyValue.tolist()

            if isinstance(propertyValue, vtk.vtkTransform):
                propertyValue = list(propertyValue.GetPosition()) + list(propertyValue.GetOrientation())

            self.addProperty(propertyName, propertyValue, attributes=om.PropertyAttributes(decimals=3, minimum=-100, maximum=100))

    def _onPropertyChanged(self, propertySet, propertyName):
        om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)
        self.constraint.__setattr__(propertyName, propertySet.getProperty(propertyName))


class EndEffectorTeleopPanel(object):

    def __init__(self, panel):
        self.panel = panel
        self.ui = panel.ui
        self.ui.eeTeleopButton.connect('clicked()', self.teleopButtonClicked)
        self.ui.planButton.connect('clicked()', self.planClicked)
        self.ui.updateIkButton.connect('clicked()', self.onUpdateIkClicked)
        self.ui.baseCombo.connect('currentIndexChanged(const QString&)', self.baseComboChanged)
        self.ui.backCombo.connect('currentIndexChanged(const QString&)', self.backComboChanged)
        self.ui.lhandCombo.connect('currentIndexChanged(const QString&)', self.lhandComboChanged)
        self.ui.rhandCombo.connect('currentIndexChanged(const QString&)', self.rhandComboChanged)
        self.ui.lfootCombo.connect('currentIndexChanged(const QString&)', self.lfootComboChanged)
        self.ui.rfootCombo.connect('currentIndexChanged(const QString&)', self.rfootComboChanged)

        self.constraintSet = None

        #self.ui.interactiveCheckbox.visible = False
        #self.ui.updateIkButton.visible = False


    def setComboText(self, combo, text):
        index = combo.findText(text)
        assert index >= 0
        combo.setCurrentIndex(index)

    def getComboText(self, combo):
        return str(combo.currentText)

    def getBaseConstraint(self):
        return self.getComboText(self.ui.baseCombo)

    def setBaseConstraint(self, value):
        return self.setComboText(self.ui.baseCombo, value)

    def getBackConstraint(self):
        return self.getComboText(self.ui.backCombo)

    def setBackConstraint(self, value):
        return self.setComboText(self.ui.backCombo, value)

    def getLHandConstraint(self):
        return self.getComboText(self.ui.lhandCombo)

    def setLHandConstraint(self, value):
        return self.setComboText(self.ui.lhandCombo, value)

    def getRHandConstraint(self):
        return self.getComboText(self.ui.rhandCombo)

    def setRHandConstraint(self, value):
        return self.setComboText(self.ui.rhandCombo, value)

    def getLFootConstraint(self):
        return self.getComboText(self.ui.lfootCombo)

    def setLFootConstraint(self, value):
        return self.setComboText(self.ui.lfootCombo, value)

    def getRFootConstraint(self):
        return self.getComboText(self.ui.rfootCombo)

    def setRFootConstraint(self, value):
        return self.setComboText(self.ui.rfootCombo, value)

    def baseComboChanged(self):
        self.updateConstraints()

    def backComboChanged(self):
        self.updateConstraints()

    def lhandComboChanged(self):
        self.updateConstraints()

    def rhandComboChanged(self):
        self.updateConstraints()

    def lfootComboChanged(self):
        self.updateConstraints()

    def rfootComboChanged(self):
        self.updateConstraints()

    def onGoalFrameModified(self, frame):
        if self.constraintSet and self.ui.interactiveCheckbox.checked:
            self.updateIk()

    def onUpdateIkClicked(self):
        self.updateIk()

    def updateIk(self):
        endPose, info = self.constraintSet.runIk()
        self.panel.showPose(self.constraintSet.endPose)
        app.displaySnoptInfo(info)


    def updateCollisionEnvironment(self):
        affs = self.panel.affordanceManager.getCollisionAffordances()
        if not affs:
            self.panel.ikPlanner.ikServer.clearEnvironment()
        else:
            urdfStr = affordanceurdf.urdfStringFromAffordances(affs)
            self.panel.ikPlanner.ikServer.setEnvironment(urdfStr)

    def planClicked(self):
        if not self.ui.eeTeleopButton.checked:
            return
        self.updateCollisionEnvironment()
        self.generatePlan()

    def generatePlan(self):

        self.updateConstraints()
        if not self.ui.interactiveCheckbox.checked:
            self.updateIk()

        # todo- need an option here
        goalMode = ikplanner.getIkOptions().getProperty('Goal planning mode')
        if goalMode == 1 or ikplanner.getIkOptions().getProperty('Use collision'):
            plan = self.constraintSet.runIkTraj()
        else:
            plan = self.constraintSet.planEndPoseGoal(feetOnGround = False)

        self.panel.showPlan(plan)

    def teleopButtonClicked(self):
        if self.ui.eeTeleopButton.checked:
            self.activate()
        else:
            self.deactivate()

    def activate(self):<commit_msg>Revert 'feetOnGround' to True in generatePlan<commit_after>from PythonQt import QtCore, QtGui, QtUiTools
import ddapp.applogic as app
import ddapp.objectmodel as om
from ddapp.timercallback import TimerCallback
from ddapp import robotstate
from ddapp import visualization as vis
from ddapp import transformUtils
from ddapp import ikplanner
from ddapp import footstepsdriver
from ddapp import vtkAll as vtk
from ddapp import drcargs
from ddapp import affordanceurdf
import ddapp.applogic as app

import math
import numpy as np


def addWidgetsToDict(widgets, d):

    for widget in widgets:
        if widget.objectName:
            d[str(widget.objectName)] = widget
        addWidgetsToDict(widget.children(), d)


class WidgetDict(object):

    def __init__(self, widgets):
        addWidgetsToDict(widgets, self.__dict__)


def clearLayout(w):
    children = w.findChildren(QtGui.QWidget)
    for child in children:
        child.delete()


class ConstraintItem(om.ObjectModelItem):

    def __init__(self, constraint):

        linkStr = '(%s)' % constraint.linkName if hasattr(constraint, 'linkName') else ''
        name = '%s %s' % (type(constraint).__name__, linkStr)

        om.ObjectModelItem.__init__(self, name)
        self.constraint = constraint

        for propertyName, propertyValue in constraint:

            if isinstance(propertyValue, np.ndarray):
                propertyValue = propertyValue.tolist()

            if isinstance(propertyValue, vtk.vtkTransform):
                propertyValue = list(propertyValue.GetPosition()) + list(propertyValue.GetOrientation())

            self.addProperty(propertyName, propertyValue, attributes=om.PropertyAttributes(decimals=3, minimum=-100, maximum=100))

    def _onPropertyChanged(self, propertySet, propertyName):
        om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)
        self.constraint.__setattr__(propertyName, propertySet.getProperty(propertyName))


class EndEffectorTeleopPanel(object):

    def __init__(self, panel):
        self.panel = panel
        self.ui = panel.ui
        self.ui.eeTeleopButton.connect('clicked()', self.teleopButtonClicked)
        self.ui.planButton.connect('clicked()', self.planClicked)
        self.ui.updateIkButton.connect('clicked()', self.onUpdateIkClicked)
        self.ui.baseCombo.connect('currentIndexChanged(const QString&)', self.baseComboChanged)
        self.ui.backCombo.connect('currentIndexChanged(const QString&)', self.backComboChanged)
        self.ui.lhandCombo.connect('currentIndexChanged(const QString&)', self.lhandComboChanged)
        self.ui.rhandCombo.connect('currentIndexChanged(const QString&)', self.rhandComboChanged)
        self.ui.lfootCombo.connect('currentIndexChanged(const QString&)', self.lfootComboChanged)
        self.ui.rfootCombo.connect('currentIndexChanged(const QString&)', self.rfootComboChanged)

        self.constraintSet = None

        #self.ui.interactiveCheckbox.visible = False
        #self.ui.updateIkButton.visible = False


    def setComboText(self, combo, text):
        index = combo.findText(text)
        assert index >= 0
        combo.setCurrentIndex(index)

    def getComboText(self, combo):
        return str(combo.currentText)

    def getBaseConstraint(self):
        return self.getComboText(self.ui.baseCombo)

    def setBaseConstraint(self, value):
        return self.setComboText(self.ui.baseCombo, value)

    def getBackConstraint(self):
        return self.getComboText(self.ui.backCombo)

    def setBackConstraint(self, value):
        return self.setComboText(self.ui.backCombo, value)

    def getLHandConstraint(self):
        return self.getComboText(self.ui.lhandCombo)

    def setLHandConstraint(self, value):
        return self.setComboText(self.ui.lhandCombo, value)

    def getRHandConstraint(self):
        return self.getComboText(self.ui.rhandCombo)

    def setRHandConstraint(self, value):
        return self.setComboText(self.ui.rhandCombo, value)

    def getLFootConstraint(self):
        return self.getComboText(self.ui.lfootCombo)

    def setLFootConstraint(self, value):
        return self.setComboText(self.ui.lfootCombo, value)

    def getRFootConstraint(self):
        return self.getComboText(self.ui.rfootCombo)

    def setRFootConstraint(self, value):
        return self.setComboText(self.ui.rfootCombo, value)

    def baseComboChanged(self):
        self.updateConstraints()

    def backComboChanged(self):
        self.updateConstraints()

    def lhandComboChanged(self):
        self.updateConstraints()

    def rhandComboChanged(self):
        self.updateConstraints()

    def lfootComboChanged(self):
        self.updateConstraints()

    def rfootComboChanged(self):
        self.updateConstraints()

    def onGoalFrameModified(self, frame):
        if self.constraintSet and self.ui.interactiveCheckbox.checked:
            self.updateIk()

    def onUpdateIkClicked(self):
        self.updateIk()

    def updateIk(self):
        endPose, info = self.constraintSet.runIk()
        self.panel.showPose(self.constraintSet.endPose)
        app.displaySnoptInfo(info)


    def updateCollisionEnvironment(self):
        affs = self.panel.affordanceManager.getCollisionAffordances()
        if not affs:
            self.panel.ikPlanner.ikServer.clearEnvironment()
        else:
            urdfStr = affordanceurdf.urdfStringFromAffordances(affs)
            self.panel.ikPlanner.ikServer.setEnvironment(urdfStr)

    def planClicked(self):
        if not self.ui.eeTeleopButton.checked:
            return
        self.updateCollisionEnvironment()
        self.generatePlan()

    def generatePlan(self):

        self.updateConstraints()
        if not self.ui.interactiveCheckbox.checked:
            self.updateIk()

        # todo- need an option here
        goalMode = ikplanner.getIkOptions().getProperty('Goal planning mode')
        if goalMode == 1 or ikplanner.getIkOptions().getProperty('Use collision'):
            plan = self.constraintSet.runIkTraj()
        else:
            plan = self.constraintSet.planEndPoseGoal()

        self.panel.showPlan(plan)

    def teleopButtonClicked(self):
        if self.ui.eeTeleopButton.checked:
            self.activate()
        else:
            self.deactivate()

    def activate(self):
{'fraction_non_alphanumeric': 0.0940123554569935, 'fraction_numerical': 0.0014256296530967844, 'mean_word_length': 3.2515151515151515, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 5, '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': '6337091', 'n_tokens_mistral': 3719, 'n_tokens_neox': 3620, 'n_words': 707}