def on_pose_body(channel, data): if (skip_joint_angles is False): if (received_joint_state is False): return m = pose_t.decode(data) o = robot_state_t() o.utime = m.utime o.num_joints = joint_state.num_joints o.joint_name = joint_state.joint_name o.joint_position = joint_state.joint_position o.joint_velocity = joint_state.joint_velocity o.joint_effort = joint_state.joint_effort o.pose.translation.x =m.pos[0]; o.pose.translation.y =m.pos[1]; o.pose.translation.z =m.pos[2]; o.pose.rotation.w = m.orientation[0]; o.pose.rotation.x = m.orientation[1]; o.pose.rotation.y = m.orientation[2]; o.pose.rotation.z = m.orientation[3]; lc.publish("EST_ROBOT_STATE",o.encode())
def send_state(): o = robot_state_t() o.utime = timestamp_now() o.num_joints = len(joint_name_list) o.joint_name = joint_name_list o.joint_position = random.rand(o.num_joints) - 0.25 o.joint_velocity = [0] * o.num_joints o.joint_effort = [0] * o.num_joints o.pose.translation.x = 0 o.pose.translation.y = 0 o.pose.translation.z = 1 o.pose.rotation.w = 1 o.pose.rotation.x = 0 o.pose.rotation.y = 0 o.pose.rotation.z = 0 lc.publish("EST_ROBOT_STATE_ALT", o.encode())
def getRobotStateMsg(): msg = robot_state_t() msg.utime = timestamp_now () ft = force_torque_t() msg.pose = getPoseMsg() msg.twist = getTwistMsg() msg.force_torque = ft msg.num_joints = 28 msg.joint_position = [0]*28 msg.joint_velocity = [0]*28 msg.joint_effort = [0]*28 msg.joint_name = ['back_bkz', 'back_bky', 'back_bkx', 'neck_ay', 'l_leg_hpz', 'l_leg_hpx',\ 'l_leg_hpy', 'l_leg_kny', 'l_leg_aky', 'l_leg_akx', 'r_leg_hpz', 'r_leg_hpx',\ 'r_leg_hpy', 'r_leg_kny', 'r_leg_aky', 'r_leg_akx', 'l_arm_usy', 'l_arm_shx',\ 'l_arm_ely', 'l_arm_elx', 'l_arm_uwy', 'l_arm_mwx', 'r_arm_usy', 'r_arm_shx',\ 'r_arm_ely', 'r_arm_elx', 'r_arm_uwy', 'r_arm_mwx'] return msg
def send_state(): o = robot_state_t() o.utime = timestamp_now () o.num_joints = len(joint_name_list) o.joint_name = joint_name_list o.joint_position = random.rand(o.num_joints) -0.25 o.joint_velocity = [0]*o.num_joints o.joint_effort = [0]*o.num_joints o.pose.translation.x =0; o.pose.translation.y =0; o.pose.translation.z =1; o.pose.rotation.w = 1; o.pose.rotation.x = 0 o.pose.rotation.y = 0 o.pose.rotation.z = 0 lc.publish("EST_ROBOT_STATE_ALT",o.encode())
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 o.num_joints = core_robot_state.num_joints o.joint_name = core_robot_state.joint_name o.joint_position = core_robot_state.joint_position o.joint_velocity = core_robot_state.joint_velocity o.joint_effort = core_robot_state.joint_effort nrot = quaternion_t() nvec = vector_3d_t() p = position_3d_t() p.rotation = nrot p.translation = nvec o.pose = p t = twist_t() t.linear_velocity = nvec t.angular_velocity = nvec o.twist = t o.pose.translation.x =m.pos[0]; o.pose.translation.y =m.pos[1]; o.pose.translation.z =m.pos[2]; o.pose.rotation.w = m.orientation[0]; o.pose.rotation.x = m.orientation[1]; o.pose.rotation.y = m.orientation[2]; o.pose.rotation.z = m.orientation[3]; ft = force_torque_t() o.force_torque = ft lc.publish("EST_ROBOT_STATE",o.encode())
def send_state(): o = robot_state_t() o.utime = timestamp_now() o.num_joints = len(joint_name_list) #o.joint_name = ["" for x in range(o.num_joints)] o.joint_name = joint_name_list o.joint_position = random.rand(o.num_joints) - 0.5 #o.joint_position = [0.0, 0.0, -1.3962634015945001, 0.0, 0.0, -1.3962634015945001, 0.0, -2.220446049250313e-16, 1.3962634015945001, 0.0, -2.220446049250313e-16, 1.3962634015945001, 0.0, 1.1102230246251565e-16, 0,0] o.joint_velocity = [0] * o.num_joints o.joint_effort = [0] * o.num_joints o.pose.translation.x = 0 o.pose.translation.y = 0 o.pose.translation.z = 2.3 o.pose.rotation.w = 1 o.pose.rotation.x = 0 o.pose.rotation.y = 0 o.pose.rotation.z = 0 lc.publish("EST_ROBOT_STATE", o.encode())
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 o.num_joints = core_robot_state.num_joints o.joint_name = core_robot_state.joint_name o.joint_position = core_robot_state.joint_position o.joint_velocity = core_robot_state.joint_velocity o.joint_effort = core_robot_state.joint_effort nrot = quaternion_t() nvec = vector_3d_t() p = position_3d_t() p.rotation = nrot p.translation = nvec o.pose = p t = twist_t() t.linear_velocity = nvec t.angular_velocity = nvec o.twist = t o.pose.translation.x = m.pos[0] o.pose.translation.y = m.pos[1] o.pose.translation.z = m.pos[2] o.pose.rotation.w = m.orientation[0] o.pose.rotation.x = m.orientation[1] o.pose.rotation.y = m.orientation[2] o.pose.rotation.z = m.orientation[3] ft = force_torque_t() o.force_torque = ft lc.publish("EST_ROBOT_STATE", o.encode())
def on_core_robot_state(channel, data): core_robot_state = joint_state_t.decode(data) #m = pose_t.decode(data) o = robot_state_t() o.utime = core_robot_state.utime o.num_joints = core_robot_state.num_joints o.joint_name = core_robot_state.joint_name o.joint_position = core_robot_state.joint_position o.joint_velocity = core_robot_state.joint_velocity o.joint_effort = core_robot_state.joint_effort nrot = quaternion_t() nvec = vector_3d_t() p = position_3d_t() p.rotation = nrot p.translation = nvec o.pose = p t = twist_t() t.linear_velocity = nvec t.angular_velocity = nvec o.twist = t o.pose.translation.x = 0 o.pose.translation.y = 0 o.pose.translation.z = 0 o.pose.rotation.w = 1 o.pose.rotation.x = 0 o.pose.rotation.y = 0 o.pose.rotation.z = 0 ft = force_torque_t() o.force_torque = ft lc.publish("EST_ROBOT_STATE", o.encode())
def on_pose_body(channel, data): if (joint_state.num_joints==0): return m = pose_t.decode(data) o = robot_state_t() o.utime = m.utime o.num_joints = joint_state.num_joints #o.joint_name = ["" for x in range(o.num_joints)] o.joint_name = joint_state.joint_name o.joint_position = joint_state.joint_position o.joint_velocity = joint_state.joint_velocity o.joint_effort = joint_state.joint_effort o.pose.translation.x =m.pos[0]; o.pose.translation.y =m.pos[1]; o.pose.translation.z =m.pos[2]; o.pose.rotation.w = m.orientation[0]; o.pose.rotation.x = m.orientation[1]; o.pose.rotation.y = m.orientation[2]; o.pose.rotation.z = m.orientation[3]; lc.publish("EST_ROBOT_STATE",o.encode())
"leftShoulderRoll", "leftShoulderYaw", "leftElbowPitch", "leftForearmYaw", "leftWristRoll", "leftWristPitch", "leftHipYaw", "leftHipRoll", "leftHipPitch", "leftKneePitch", "leftAnklePitch", "leftAnkleRoll", "rightHipYaw", "rightHipRoll", "rightHipPitch", "rightKneePitch", "rightAnklePitch", "rightAnkleRoll" ] elif robot_name == "multisense": print "Multisense" joint_names = ["hokuyo_joint"] elif robot_name == "husky": print "Husky" joint_names = ["hokuyo_joint"] lc = lcm.LCM() print "Sending EST_ROBOT_STATE for ", robot_name msg = robot_state_t() msg.utime = 0 msg.joint_name = joint_names msg.num_joints = len(msg.joint_name) msg.joint_position = [0] * msg.num_joints msg.joint_velocity = [0] * msg.num_joints msg.joint_effort = [0] * msg.num_joints msg2 = pose_t() msg2.utime = msg.utime if mode == "static": print "Send a single pose..." msg.pose.translation.x = 0 msg.pose.translation.y = 0 msg.pose.translation.z = 1.2