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 = 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 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