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()
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
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
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
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()]
