Example #1
0
def on_atlas_state(channel, data):
  m = atlas_state_t.decode(data)
  o = joint_state_t()
  o.utime = m.utime
  o.num_joints = m.num_joints
  o.joint_name = atlas_joint_names
  o.joint_position = m.joint_position
  o.joint_velocity = m.joint_velocity
  o.joint_effort = m.joint_effort
  lc.publish("CORE_ROBOT_STATE", o.encode()) 

  ft = m.force_torque
  lf = six_axis_force_torque_t()
  lf.utime = m.utime
  lf.force = [0, 0, ft.l_foot_force_z]
  lf.moment = [ft.l_foot_torque_x, ft.l_foot_torque_y, 0]
  rf = six_axis_force_torque_t()
  rf.utime = m.utime
  rf.force = [0, 0, ft.r_foot_force_z]
  rf.moment = [ft.r_foot_torque_x, ft.r_foot_torque_y, 0]
  lh = six_axis_force_torque_t()
  lh.utime = m.utime
  lh.force = ft.l_hand_force
  lh.moment = ft.l_hand_torque
  rh = six_axis_force_torque_t()
  rh.utime = m.utime
  rh.force = ft.r_hand_force
  rh.moment = ft.r_hand_torque

  msg_out = six_axis_force_torque_array_t()
  msg_out.utime = m.utime
  msg_out.sensors = [lf, rf, lh, rh]
  msg_out.names =['l_foot', 'r_foot', 'l_hand', 'r_hand']
  msg_out.num_sensors = len(msg_out.sensors)
  lc.publish("FORCE_TORQUE", msg_out.encode())
Example #2
0
def on_atlas_state(channel, data):
    m = atlas_state_t.decode(data)
    o = joint_state_t()
    o.utime = m.utime
    o.num_joints = m.num_joints
    o.joint_name = atlas_joint_names
    o.joint_position = m.joint_position
    o.joint_velocity = m.joint_velocity
    o.joint_effort = m.joint_effort
    lc.publish("CORE_ROBOT_STATE", o.encode())

    ft = m.force_torque
    lf = six_axis_force_torque_t()
    lf.utime = m.utime
    lf.force = [0, 0, ft.l_foot_force_z]
    lf.moment = [ft.l_foot_torque_x, ft.l_foot_torque_y, 0]
    rf = six_axis_force_torque_t()
    rf.utime = m.utime
    rf.force = [0, 0, ft.r_foot_force_z]
    rf.moment = [ft.r_foot_torque_x, ft.r_foot_torque_y, 0]
    lh = six_axis_force_torque_t()
    lh.utime = m.utime
    lh.force = ft.l_hand_force
    lh.moment = ft.l_hand_torque
    rh = six_axis_force_torque_t()
    rh.utime = m.utime
    rh.force = ft.r_hand_force
    rh.moment = ft.r_hand_torque

    msg_out = six_axis_force_torque_array_t()
    msg_out.utime = m.utime
    msg_out.sensors = [lf, rf, lh, rh]
    msg_out.names = ['l_foot', 'r_foot', 'l_hand', 'r_hand']
    msg_out.num_sensors = len(msg_out.sensors)
    lc.publish("FORCE_TORQUE", msg_out.encode())
Example #3
0
def on_atlas_state(channel, data):
  global atlas_state
  atlas_state = atlas_state_t.decode(data)
Example #4
0
def on_atlas_state(channel, data):
  m = atlas_state_t.decode(data)
  if (mode is 0):
    print "                                                  ",m.utime, " jnt"
  else:
    print "0,0,",m.utime
Example #5
0
def on_atlas_state(channel, data):
    m = atlas_state_t.decode(data)
    if (mode is 0):
        print "                                                  ", m.utime, " jnt"
    else:
        print "0,0,", m.utime
Example #6
0
def on_atlas_state(channel, data):
    global atlas_state
    atlas_state = atlas_state_t.decode(data)