def on_pose_body(channel, data): global atlas_state, joint_names if (atlas_state.num_joints==0): return m = pose_t.decode(data) o = robot_state_t() o.utime = m.utime o.num_joints = atlas_state.num_joints #o.joint_name = ["" for x in range(o.num_joints)] o.joint_name = joint_name_list o.joint_position = atlas_state.joint_position o.joint_velocity = atlas_state.joint_velocity o.joint_effort = atlas_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 on_pose_body(channel, data): global atlas_state, joint_names if (atlas_state.num_joints == 0): return m = pose_t.decode(data) o = robot_state_t() o.utime = m.utime o.num_joints = atlas_state.num_joints #o.joint_name = ["" for x in range(o.num_joints)] o.joint_name = joint_name_list o.joint_position = atlas_state.joint_position o.joint_velocity = atlas_state.joint_velocity o.joint_effort = atlas_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 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 = 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", o.encode())