<commit_before>#import ddapp
from ddapp import cameraview
from ddapp import transformUtils
from ddapp import visualization as vis
from ddapp import objectmodel as om
from ddapp import ik
from ddapp.ikplanner import ConstraintSet
from ddapp import polarisplatformplanner
from ddapp import robotstate
from ddapp import segmentation
from ddapp import sitstandplanner
from ddapp.timercallback import TimerCallback
from ddapp import visualization as vis
from ddapp import planplayback
from ddapp import lcmUtils

import os
import functools
import numpy as np
import scipy.io
import vtkAll as vtk
from ddapp.tasks.taskuserpanel import TaskUserPanel
import ddapp.tasks.robottasks as rt
from ddapp import filterUtils
from ddapp import ioUtils


class PolarisModel(object):

    def __init__(self):
        pose = transformUtils.poseFromTransform(vtk.vtkTransform())
        desc = dict(classname='MeshAffordanceItem', Name='polaris',
                    Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
        self.affordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
        self.aprilTagFrame = vis.updateFrame(vtk.vtkTransform(), 'grab bar april tag',
                                             visible=True, scale=0.2, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.99548239, 0.04156693, 0.35259928]),
                                             np.array([ 0.18827199, 0.84761397, 0.41552535,
                                                       0.27100351]))
        self.leftFootEgressStartFrame  = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.93707546,  0.07409333,  0.32871604]),
                                             np.array([ 0.22455191,  0.71396247,  0.60983921,
                                                       0.26063418]))
        self.leftFootEgressInsideFrame  = vis.updateFrame(t, 'left foot inside', scale=0.2,visible=True, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.89783714,  0.23503719,  0.29039189]),
                                             np.array([ 0.2331762 ,  0.69031269,  0.6311807,
                                                       0.2659101]))
        self.leftFootEgressMidFrame  = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.88436275,  0.50939115,  0.31281047]),
                                             np.array([ 0.22600245,  0.69177731,  0.63305905,
                                                       0.26382435]))
        self.leftFootEgressOutsideFrame  = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.affordance)
        self.frameSync = vis.FrameSync()
        self.frameSync.addFrame(self.aprilTagFrame)
        self.frameSync.addFrame(self.affordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressInsideFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)

        self.timerCallback = TimerCallback()
        self.timerCallback.targetFps = 5
        self.timerCallback.callback = self.updateAprilTagFrame
        self.timerCallback.start()

    def updateAprilTagFrame(self):
        t = vtk.vtkTransform()
        cameraview.imageManager.queue.getTransform('april_tag_car_beam', 'local', t)
        self.aprilTagFrame.copyFrame(t)


class EgressPlanner(object):

    def __init__(self, robotSystem):

        self.liftHeight = 0.1
        self.legLiftAngle = 8

        self.robotSystem = robotSystem
        self.polaris = None

    def spawnPolaris(self):
        self.polaris = PolarisModel()

    def createLeftFootPoseConstraint(self, targetFrame, tspan=[-np.inf,np.inf]):
        positionConstraint, orientationConstraint = self.robotSystem.ikPlanner.createPositionOrientationConstraint('l_foot', targetFrame, vtk.vtkTransform())
        positionConstraint.tspan = tspan
        orientationConstraint.tspan = tspan
        return positionConstraint, orientationConstraint

    def createAllButLeftLegPostureConstraint(self, poseName):
        joints = robotstate.matchJoints('^(?!l_leg)')
        return self.robotSystem.ikPlanner.createPostureConstraint(poseName, joints)

<commit_msg>Add mising attibutes to EgressPlanner<commit_after>#import ddapp
from ddapp import cameraview
from ddapp import transformUtils
from ddapp import visualization as vis
from ddapp import objectmodel as om
from ddapp import ik
from ddapp.ikplanner import ConstraintSet
from ddapp import polarisplatformplanner
from ddapp import robotstate
from ddapp import segmentation
from ddapp import sitstandplanner
from ddapp.timercallback import TimerCallback
from ddapp import visualization as vis
from ddapp import planplayback
from ddapp import lcmUtils

import os
import functools
import numpy as np
import scipy.io
import vtkAll as vtk
from ddapp.tasks.taskuserpanel import TaskUserPanel
import ddapp.tasks.robottasks as rt
from ddapp import filterUtils
from ddapp import ioUtils


class PolarisModel(object):

    def __init__(self):
        pose = transformUtils.poseFromTransform(vtk.vtkTransform())
        desc = dict(classname='MeshAffordanceItem', Name='polaris',
                    Filename='software/models/polaris/polaris_cropped.vtp', pose=pose)
        self.affordance = segmentation.affordanceManager.newAffordanceFromDescription(desc)
        self.aprilTagFrame = vis.updateFrame(vtk.vtkTransform(), 'grab bar april tag',
                                             visible=True, scale=0.2, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.99548239, 0.04156693, 0.35259928]),
                                             np.array([ 0.18827199, 0.84761397, 0.41552535,
                                                       0.27100351]))
        self.leftFootEgressStartFrame  = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.93707546,  0.07409333,  0.32871604]),
                                             np.array([ 0.22455191,  0.71396247,  0.60983921,
                                                       0.26063418]))
        self.leftFootEgressInsideFrame  = vis.updateFrame(t, 'left foot inside', scale=0.2,visible=True, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.89783714,  0.23503719,  0.29039189]),
                                             np.array([ 0.2331762 ,  0.69031269,  0.6311807,
                                                       0.2659101]))
        self.leftFootEgressMidFrame  = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.affordance)

        t = transformUtils.transformFromPose(np.array([-0.88436275,  0.50939115,  0.31281047]),
                                             np.array([ 0.22600245,  0.69177731,  0.63305905,
                                                       0.26382435]))
        self.leftFootEgressOutsideFrame  = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.affordance)
        self.frameSync = vis.FrameSync()
        self.frameSync.addFrame(self.aprilTagFrame)
        self.frameSync.addFrame(self.affordance.getChildFrame(), ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressInsideFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True)
        self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True)

        self.timerCallback = TimerCallback()
        self.timerCallback.targetFps = 5
        self.timerCallback.callback = self.updateAprilTagFrame
        self.timerCallback.start()

    def updateAprilTagFrame(self):
        t = vtk.vtkTransform()
        cameraview.imageManager.queue.getTransform('april_tag_car_beam', 'local', t)
        self.aprilTagFrame.copyFrame(t)


class EgressPlanner(object):

    def __init__(self, robotSystem):

        self.pelvisLiftX = 0.0
        self.pelvisLiftZ = 0.05
        self.legLiftAngle = 8

        self.robotSystem = robotSystem
        self.polaris = None

    def spawnPolaris(self):
        self.polaris = PolarisModel()

    def createLeftFootPoseConstraint(self, targetFrame, tspan=[-np.inf,np.inf]):
        positionConstraint, orientationConstraint = self.robotSystem.ikPlanner.createPositionOrientationConstraint('l_foot', targetFrame, vtk.vtkTransform())
        positionConstraint.tspan = tspan
        orientationConstraint.tspan = tspan
        return positionConstraint, orientationConstraint

    def createAllButLeftLegPostureConstraint(self, poseName):
        joints = robotstate.matchJoints('^(?!l_leg)')
        return self.robotSystem.ikPlanner.createPostureConstraint(poseName, joints)


{'fraction_non_alphanumeric': 0.08813008130081301, 'fraction_numerical': 0.05734417344173442, 'mean_word_length': 2.7201612903225807, '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': 36, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1059875', 'n_tokens_mistral': 3020, 'n_tokens_neox': 2629, 'n_words': 504}