Exemplo n.º 1
0
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
Exemplo n.º 3
0
def on_atlas_state(channel, data):
    m = atlas_state_t.decode(data)
    print "                                                  ", m.utime, " jnt"
Exemplo n.º 4
0
def on_atlas_state(channel, data):
  m = atlas_state_t.decode(data)
  print "1, ",m.utime
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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