<commit_before>from director import robotsystem
from director.consoleapp import ConsoleApp
from director import robotstate
from director import planplayback
from director import lcmUtils
from director import drcargs
from director import roboturdf
from director import simpletimer
from director.timercallback import TimerCallback
from director.fieldcontainer import FieldContainer
from PythonQt import QtCore, QtGui, QtUiTools
import drc as lcmdrc
import numpy as np
import math
import argparse

from director.utime import getUtime
from lcmtypes.drake import lcmt_qp_controller_input, lcmt_whole_body_data

import scipy.interpolate



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 newAtlasCommandMessageAtZero():

    msg = lcmdrc.atlas_command_t()
    msg.joint_names = [str(v) for v in robotstate.getRobotStateJointNames()]
    msg.num_joints = len(msg.joint_names)
    zeros = np.zeros(msg.num_joints)
    msg.k_q_p = getDefaultUserModePositionGains()
    msg.k_q_i = zeros.tolist()
    msg.k_qd_p = zeros.tolist()
    msg.k_f_p = zeros.tolist()
    msg.ff_qd = zeros.tolist()
    msg.ff_qd_d = zeros.tolist()
    msg.ff_f_d = zeros.tolist()
    msg.ff_const = zeros.tolist()
    msg.effort = zeros.tolist()
    msg.velocity = zeros.tolist()
    msg.position = zeros.tolist()
    msg.desired_controller_period_ms = 3
    msg.k_effort = '0'*msg.num_joints
    return msg

def getDefaultUserModePositionGains():
    return [10.0, 70.0, 70.0, 1000.0, 60.0, 78.0, 50.0, 140.0, 2000.0, 2000.0, 60.0, 78.0, 50.0, 140.0, 2000.0, 2000.0, 4.0, 4.0, 4.0, 4.0, 20.0, 20.0, 20.0, 4.0, 4.0, 4.0, 4.0, 20.0, 20.0, 20.0]


def drakePoseToAtlasCommand(drakePose):
    jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap()
    robotState = np.zeros(len(jointIndexMap))
    for jointIdx, drakeIdx in jointIndexMap.iteritems():
        robotState[jointIdx] = drakePose[drakeIdx]
    position = robotState.tolist()
    msg = newAtlasCommandMessageAtZero()
    msg.position = position
    return msg

def atlasCommandToDrakePose(msg):
    jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap()
    drakePose = np.zeros(len(robotstate.getDrakePoseJointNames()))
    for jointIdx, drakeIdx in jointIndexMap.iteritems():
        drakePose[drakeIdx] = msg.position[jointIdx]
    return drakePose.tolist()

def drakePoseToQPInput(pose, atlasVersion=5):<commit_msg>add valkyrie gains to joint streaming app<commit_after>from director import robotsystem
from director.consoleapp import ConsoleApp
from director import robotstate
from director import planplayback
from director import lcmUtils
from director import drcargs
from director import roboturdf
from director import simpletimer
from director.timercallback import TimerCallback
from director.fieldcontainer import FieldContainer
from PythonQt import QtCore, QtGui, QtUiTools
import drc as lcmdrc
import numpy as np
import math
import argparse

from director.utime import getUtime
from lcmtypes.drake import lcmt_qp_controller_input, lcmt_whole_body_data

import scipy.interpolate



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__)

val_gains = {
  'torsoYaw': (100.0, 1.0),
  'torsoPitch': (800.0, 1.0),
  'torsoRoll': (800.0, 1.0),
  'lowerNeckPitch': (30.0, 1.0),
  'neckYaw': (30.0, 1.0),
  'upperNeckPitch': (30.0, 1.0),
  'rightShoulderPitch': (100.0, 1.0),
  'rightShoulderRoll': (100.0, 1.0),
  'rightShoulderYaw': (50.0, 1.0),
  'rightElbowPitch': (50.0, 1.0),
  'rightForearmYaw': (30.0, 1.0),
  'rightWristRoll': (30.0, 1.0),
  'rightWristPitch': (30.0, 1.0),
  'leftShoulderPitch': (100.0, 1.0),
  'leftShoulderRoll': ( 100.0, 1.0),
  'leftShoulderYaw': (50.0, 1.0),
  'leftElbowPitch': (30.0, 1.0),
  'leftForearmYaw': (30.0, 1.0),
  'leftWristRoll': (30.0, 1.0),
  'leftWristPitch': (30.0, 1.0),
  'rightHipYaw': (50.0, 1.0),
  'rightHipRoll': (100.0, 1.0),
  'rightHipPitch': ( 200.0, 1.0),
  'rightKneePitch': (50.0, 1.0),
  'rightAnklePitch': ( 30.0, 1.0),
  'rightAnkleRoll': ( 30.0, 1.0),
  'leftHipYaw': (50.0, 1.0),
  'leftHipRoll': (100.0, 1.0),
  'leftHipPitch': ( 200.0, 1.0),
  'leftKneePitch': (50.0, 1.0),
  'leftAnklePitch': (30.0, 1.0),
  'leftAnkleRoll': (30.0, 1.0)
}


def newAtlasCommandMessageAtZero():

    msg = lcmdrc.atlas_command_t()
    msg.joint_names = [str(v) for v in robotstate.getRobotStateJointNames()]
    msg.num_joints = len(msg.joint_names)
    zeros = np.zeros(msg.num_joints)
    msg.k_q_p = [val_gains[name][0] for name in msg.joint_names]
    msg.k_q_i = zeros.tolist()
    msg.k_qd_p = [val_gains[name][1] for name in msg.joint_names]
    msg.k_f_p = zeros.tolist()
    msg.ff_qd = zeros.tolist()
    msg.ff_qd_d = zeros.tolist()
    msg.ff_f_d = zeros.tolist()
    msg.ff_const = zeros.tolist()
    msg.effort = zeros.tolist()
    msg.velocity = zeros.tolist()
    msg.position = zeros.tolist()
    msg.desired_controller_period_ms = 3
    msg.k_effort = '0'*msg.num_joints
    return msg


def getAtlasUserModePositionGains():
    return [10.0, 70.0, 70.0, 1000.0, 60.0, 78.0, 50.0, 140.0, 2000.0, 2000.0, 60.0, 78.0, 50.0, 140.0, 2000.0, 2000.0, 4.0, 4.0, 4.0, 4.0, 20.0, 20.0, 20.0, 4.0, 4.0, 4.0, 4.0, 20.0, 20.0, 20.0]


def drakePoseToAtlasCommand(drakePose):
    jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap()
    robotState = np.zeros(len(jointIndexMap))
    for jointIdx, drakeIdx in jointIndexMap.iteritems():
        robotState[jointIdx] = drakePose[drakeIdx]
    position = robotState.tolist()
    msg = newAtlasCommandMessageAtZero()
    msg.position = position
    return msg

def atlasCommandToDrakePose(msg):
    jointIndexMap = robotstate.getRobotStateToDrakePoseJointMap()
    drakePose = np.zeros(len(robotstate.getDrakePoseJointNames()))
    for jointIdx, drakeIdx in jointIndexMap.iteritems():
        drakePose[drakeIdx] = msg.position[jointIdx]
    return drakePose.tolist()

def drakePoseToQPInput(pose, atlasVersion=5):
{'fraction_non_alphanumeric': 0.12894612847494896, 'fraction_numerical': 0.05764096120621957, 'mean_word_length': 4.026045777426993, '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': '5199879', 'n_tokens_mistral': 2705, 'n_tokens_neox': 2452, 'n_words': 516}