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
home_dir =os.getenv("HOME")
#print home_dir
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/site-packages")
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/dist-packages")
#sys.path.append(home_dir + "/otherprojects/pronto-distro/build/lib/python2.7/site-packages")
#sys.path.append(home_dir + "/otherprojects/pronto-distro/build/lib/python2.7/dist-packages")

from bot_core.pose_t import pose_t
from pronto.robot_state_t import robot_state_t
from pronto.joint_state_t import joint_state_t
########################################################################################
def timestamp_now (): return int (time.time () * 1000000)

global joint_state

joint_state = joint_state_t()




def on_joint_state(channel, data):
  global joint_state
  joint_state = joint_state_t.decode(data)

def on_pose_body(channel, data):
  if (joint_state.num_joints==0):
    return

  m = pose_t.decode(data)

  o = robot_state_t()
Example #4
0
from bot_core.pose_t import pose_t
from pronto.robot_state_t import robot_state_t
from pronto.joint_state_t import joint_state_t
from pronto.vector_3d_t import vector_3d_t
from pronto.position_3d_t import position_3d_t
from pronto.twist_t import twist_t
from pronto.quaternion_t import quaternion_t
from pronto.twist_t import twist_t
from pronto.force_torque_t import force_torque_t

########################################################################################
def timestamp_now (): return int (time.time () * 1000000)

global core_robot_state

core_robot_state = joint_state_t()


def on_core_robot_state(channel, data):
  global core_robot_state
  core_robot_state = joint_state_t.decode(data)

def on_pose_body(channel, data):
  global core_robot_state, joint_names
  if (core_robot_state.num_joints==0):
    return

  m = pose_t.decode(data)

  o = robot_state_t()
  o.utime = m.utime