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