Exemplo n.º 1
0
    def showRobotState(self, msg):

        jointMap = {}
        for name, position in zip(msg.joint_name, msg.joint_position):
            jointMap[name] = position

        jointPositions = []
        for name in robotstate.getRobotStateJointNames():
            jointPositions.append(jointMap[name])

        robotState = [0.0] * 7
        robotState += jointPositions

        self.model.setEstRobotState(robotState)
        self.view.render()
Exemplo n.º 2
0
def newAtlasCommandMessageAtZero():

    msg = lcmbotcore.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
Exemplo n.º 3
0
def newAtlasCommandMessageAtZero():

    msg = lcmbotcore.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
Exemplo n.º 4
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 = 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
Exemplo n.º 5
0
    for key, value in KpGains.iteritems():
        jointName = str(key)
        d = {'Kp': value}
        d['Kd'] = 2*dampingRatio*np.sqrt(value)
        gains[jointName] = d

    return gains

valkyrieGains = loadValkyrieGains()



# special for Valkyrie, note the off by one indexing of the constrained dofs
# not sure why it is off by one, something to do with how we do this message in lcm
# see QPLocomotionPlan.cpp for an example of the same shifting
valJointNames = robotstate.getRobotStateJointNames()
valJointIdx = dict()

for idx, singleJointName in enumerate(valJointNames):
    valJointIdx[str(singleJointName)] = idx

valConstrainedJoints = ['lowerNeckPitch', 'neckYaw', 'upperNeckPitch']
valConstrainedJointsIdx = []
for jointName in valConstrainedJoints:
    valConstrainedJointsIdx.append(1+valJointIdx[jointName])


def newAtlasCommandMessageAtZero():

    msg = lcmbotcore.atlas_command_t()
    msg.joint_names = [str(v) for v in robotstate.getRobotStateJointNames()]