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())
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())
def on_atlas_state(channel, data): global atlas_state atlas_state = atlas_state_t.decode(data)
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
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
def on_atlas_state(channel, data):
global atlas_state
atlas_state = atlas_state_t.decode(data)