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 bot_core.robot_state_t import robot_state_t from bot_core.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()
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 bot_core.robot_state_t import robot_state_t from bot_core.joint_state_t import joint_state_t ######################################################################################## def timestamp_now (): return int (time.time () * 1000000) global joint_state joint_state = joint_state_t() joint_state.joint_name = ['lf_haa_joint', 'lf_hfe_joint', 'lf_kfe_joint', 'rf_haa_joint', 'rf_hfe_joint', 'rf_kfe_joint', 'lh_haa_joint', 'lh_hfe_joint', 'lh_kfe_joint', 'rh_haa_joint', 'rh_hfe_joint', 'rh_kfe_joint', 'ptu_pan', 'ptu_tilt'] joint_state.num_joints = len(joint_state.joint_name) joint_state.joint_position = [0] * len(joint_state.joint_name) joint_state.joint_velocity = [0] * len(joint_state.joint_name) joint_state.joint_effort = [0] * len(joint_state.joint_name) received_joint_state = False skip_joint_angles = True if skip_joint_angles: received_joint_state = True def on_joint_state(channel, data): global joint_state, received_joint_state
from bot_core.pose_t import pose_t from bot_core.robot_state_t import robot_state_t from bot_core.joint_state_t import joint_state_t from bot_core.vector_3d_t import vector_3d_t from bot_core.position_3d_t import position_3d_t from bot_core.twist_t import twist_t from bot_core.quaternion_t import quaternion_t from bot_core.twist_t import twist_t from bot_core.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
from bot_core.vector_3d_t import vector_3d_t from bot_core.position_3d_t import position_3d_t from bot_core.twist_t import twist_t from bot_core.quaternion_t import quaternion_t from bot_core.twist_t import twist_t from bot_core.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()