def on_atlas_state(channel, data): global state m = atlas_state_t.decode(data) alpha = 0.05 if (state.l_foot_force_z_prev is None): # do nothing m.force_torque.l_foot_force_z = m.force_torque.l_foot_force_z m.force_torque.r_foot_force_z = m.force_torque.r_foot_force_z else: m.force_torque.l_foot_force_z = alpha*m.force_torque.l_foot_force_z + (1-alpha)*state.l_foot_force_z_prev m.force_torque.r_foot_force_z = alpha*m.force_torque.r_foot_force_z + (1-alpha)*state.r_foot_force_z_prev lc.publish("ATLAS_STATE", m.encode() ) state.l_foot_force_z_prev = m.force_torque.l_foot_force_z state.r_foot_force_z_prev = m.force_torque.r_foot_force_z
def on_atlas_state(channel, data): global state m = atlas_state_t.decode(data) alpha = 0.05 if (state.l_foot_force_z_prev is None): # do nothing m.force_torque.l_foot_force_z = m.force_torque.l_foot_force_z m.force_torque.r_foot_force_z = m.force_torque.r_foot_force_z else: m.force_torque.l_foot_force_z = alpha * m.force_torque.l_foot_force_z + ( 1 - alpha) * state.l_foot_force_z_prev m.force_torque.r_foot_force_z = alpha * m.force_torque.r_foot_force_z + ( 1 - alpha) * state.r_foot_force_z_prev lc.publish("ATLAS_STATE", m.encode()) state.l_foot_force_z_prev = m.force_torque.l_foot_force_z state.r_foot_force_z_prev = m.force_torque.r_foot_force_z
def on_atlas_state(channel, data): m = atlas_state_t.decode(data) print " ", m.utime, " jnt"
def on_atlas_state(channel, data): m = atlas_state_t.decode(data) print "1, ",m.utime
def on_atlas_state(channel, data): global state m = atlas_state_t.decode(data) state.pot_angle = m.joint_position[18] state.pot_utime = m.utime