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
