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())
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()
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