def onIiwaStatus(self, channel, data):
msg = lcmt_iiwa_status.decode(data)
for data_idx in xrange(0, len(self.iiwa_joint_names)):
joint_name = self.iiwa_joint_names[data_idx]
if joint_name in self.joint_idx:
idx = self.joint_idx[joint_name]
self.joint_positions[idx] = msg.joint_position_measured[data_idx]
self.joint_velocities[idx] = msg.joint_velocity_estimated[data_idx] # TODO(gizatt) See which other fields are valid and add them here
self.joint_efforts[idx] = msg.joint_torque_measured[data_idx]
def onIiwaStatus(self, channel, data):
msg = lcmt_iiwa_status.decode(data)
for data_idx in xrange(0, len(self.iiwa_joint_names)):
joint_name = self.iiwa_joint_names[data_idx]
if joint_name in self.joint_idx:
idx = self.joint_idx[joint_name]
self.joint_positions[idx] = msg.joint_position_measured[data_idx]
self.joint_velocities[idx] = msg.joint_velocity_estimated[data_idx] # TODO(gizatt) See which other fields are valid and add them here
self.joint_efforts[idx] = msg.joint_torque_measured[data_idx]
# We *should* be using the sunrise cabinet time.
# But it's not always well synchronized with our time,
# (e.g. at time of writing, it was off by hours and was
# messing up the TF server), so for now I'm approximating with
# our local system time. -gizatt
self.publishROSJointStateMessage()
#%%
# log_path = os.path.join("/Users", "pangtao", "PycharmProjects",
# "lcmlog-2021-04-26.01")
log_path = os.path.join("/Users", "pangtao", "PycharmProjects",
"lcmlog-2021-05-15.02")
lcm_log = lcm.EventLog(log_path)
#%%
iiwa_status_list = []
iiwa_cmd_list = []
# extract time stamps and utimes of status and command messages from log file.
for event in lcm_log:
if event.channel == "IIWA_STATUS":
msg = lcmt_iiwa_status.decode(event.data)
iiwa_status_list.append((event.timestamp, msg.utime))
elif event.channel == "IIWA_COMMAND":
msg = lcmt_iiwa_command.decode(event.data)
iiwa_cmd_list.append((event.timestamp, msg.utime))
#%% match time stamps
# make sure iiwa_cmd_list[0].utime >= iiwa_status_list[0].utime
while iiwa_cmd_list[0][1] < iiwa_status_list[0][1]:
iiwa_cmd_list.pop(0)
# make sure iiwa_cmd_list[-1].utime == iiwa_status_list[-1].utime
i_start_status = 0
i_end_status = 0
for i, status_times in enumerate(iiwa_status_list):
if status_times[1] == iiwa_cmd_list[-1][1]:
def sub_callback(self, channel, data):
iiwa_status_msg = lcmt_iiwa_status.decode(data)
self.msg_lock.acquire()
self.iiwa_position_measured = iiwa_status_msg.joint_position_measured
self.msg_lock.release()
def HandleIiwaStatusMessage(channel, data):
msg = lcmt_iiwa_status.decode(data)
q0[:] = msg.joint_position_measured