def write_bag(bag_origin, bag_write: rosbag.Bag, t_start):
count = 0
for topic, msg, t in bag_origin.read_messages("/tf", t_start):
time_stamp = rospy.Time(0) + (t - t_start)
msg.transforms[0].header.stamp = time_stamp
bag_write.write(topic, msg, time_stamp)
count += 1
bag_write.close()
def process_bag(bag_in_fn, bag_out_fn, conf_file_fn):
bag_in = Bag(bag_in_fn)
bag_out = Bag(bag_out_fn, 'w')
include_rules, exclude_rules, time_rules = read_rules(conf_file_fn)
topic_rules = include_rules + exclude_rules
# Set the time UNIX time at the start of the bag file
for r in topic_rules:
r.set_begin_time(bag_in.get_start_time())
for r in time_rules:
r.set_begin_time(bag_in.get_start_time())
# Find start and end times that are actually required
t_start, t_end = get_begin_end(time_rules, bag_in)
# Find topics that are actually required
bag_topics = bag_in.get_type_and_topic_info().topics.keys()
topics = get_topics(topic_rules, bag_topics)
messages = bag_in.read_messages(topics=topics,
start_time=t_start,
end_time=t_end,
return_connection_header=True)
for topic, msg, t, conn_header in messages:
# Check default enforcement for this message
if FilterRule.is_tf_topic(topic):
default = FilterRule.DEFAULT_ENFORCEMENT_TF
else:
default = FilterRule.DEFAULT_ENFORCEMENT_TOPIC
# When default is to include, only check whether the exclusion
# rules are satisfied, and if all of them are ok, write it out
if default == FilterRule.INCLUDE:
# Check exclusions
ok = True
for r in exclude_rules:
if r.msg_match(topic, msg, t):
ok = False
# When default is to exclude, check if the message matches any
# of the inclusion rules and write it out if it does
else:
# Check inclusions
ok = False
for r in include_rules:
if r.msg_match(topic, msg, t):
ok = True
# Time rules
time_ok = True
for r in time_rules:
if not r.is_ok_with(topic, msg, t):
time_ok = False
# Write to file
if ok and time_ok:
bag_out.write(topic, msg, t, connection_header=conn_header)
bag_out.close()
def ipfs_upload(messages):
rospy.wait_for_service('/liability/infochan/ipfs/add_file')
upload = rospy.ServiceProxy('/liability/infochan/ipfs/add_file',
IpfsUploadFile)
with NamedTemporaryFile(delete=False) as tmpfile:
recorder = Bag(tmpfile.name, 'w')
for topic in messages:
recorder.write('/{}'.format(topic), messages[topic])
recorder.close()
res = upload(Filepath(tmpfile.name))
if not res.success:
raise Exception(res.error_msg)
return res.ipfs_address
def create_objective(period: str, stream_id: str, email: str) -> str:
ipfs = connect()
tempdir = gettempdir()
os.chdir(tempdir)
with NamedTemporaryFile(delete=False) as f:
bag = Bag(f.name, 'w')
bag.write('/period', String(period))
bag.write('/stream_id', String(stream_id))
bag.write('/email', String(email))
bag.close()
res = ipfs.add(f.name)
rospy.loginfo("Hash of the objective is {}".format(res['Hash']))
return res['Hash']
def process_bag(bag_in_fn, bag_out_fn, conf_file_fn):
bag_in = Bag(bag_in_fn)
bag_out = Bag(bag_out_fn, 'w')
topic_rules, tf_rules = RenameRule.parse(conf_file_fn)
messages = bag_in.read_messages(return_connection_header=True)
for topic, msg, t, conn_header in messages:
if topic == '/tf' or topic == '/tf_static':
new_topic = topic
new_msg = modify_tf(msg, tf_rules)
else:
new_topic = modify_topic(topic, topic_rules)
# Modify the frame_id in header if header exists
new_msg = modify_msg(msg, tf_rules)
bag_out.write(new_topic, new_msg, t, connection_header=conn_header)
bag_in.close()
bag_out.close()
def writepc2frombag2file(input_bagfile, pc2_topic, out_file_path,start_time=None, end_time=None, msg_count=None):
try:
output_file_name = get_output_file_name(input_bagfile) + '_xyz.txt'
if out_file_path.endswith('/'):
output_file = out_file_path + output_file_name
else:
output_file = out_file_path+'/' + output_file_name
output_file_fh = open(output_file,'w')
print('writing output_file at', output_file)
# check start and end time condition
if start_time is not None and end_time is not None:
assert (end_time - start_time) > 0 , "end_time should be higher than start time"
if msg_count is None:
use_msg_count = False
else:
use_msg_count = True
# msg_count is already static casted in arg parse, just checking again for sanity ad if function is re used individially
msg_count = int(msg_count)
assert msg_count > 0, "should have positive msg_count"
input_bag = Bag(input_bagfile,'r')
print('bag load success')
count = 0;
for topic, msg, ts in input_bag.read_messages(topics=[pc2_topic],start_time=start_time, end_time=end_time):
output_file_fh.write('msg_timestamp: %f\n' % msg.header.stamp.to_sec())
output_file_fh.write('%d\n' %msg.width)
for data_pts in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
output_file_fh.write('%f %f %f %d\n' % (data_pts[0], data_pts[1], data_pts[2], data_pts[3]))
count +=1
print(count,msg_count)
if use_msg_count and count >= msg_count:
break
input_bag.close()
output_file_fh.close()
except Exception as e_init:
print(traceback.format_exc(e_init))
def on_enter(self, userdata):
self._failed = False
try:
# Initialization
# userdata
self._bag_filename = userdata.bag_filename
self._trajectories_command = userdata.trajectories_command
# joint definitions
l_arm_range = range(16, 23)
r_arm_range = range(23, 30)
atlasJointNames = [
'back_bkz', 'back_bky', 'back_bkx', 'neck_ry', '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_shz',
'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_wry',
'l_arm_wrx', 'l_arm_wry2', 'r_arm_shz', 'r_arm_shx',
'r_arm_ely', 'r_arm_elx', 'r_arm_wry', 'r_arm_wrx',
'r_arm_wry2'
]
joint_position_cmd = [0] * len(l_arm_range)
# starting and stopping time of measurement data to be taken into account for cailbration
time_start = 0
time_end = 0
time_tf = 0
# gravity vector in world frame
g_world = np.resize(np.array([0, 0, -9.81]), (3, 1))
# take this number of data points for each pose.
max_data_points_per_pose = 100
# rotation of the force torque sensor in world frame (quaternions)
ft_rotation = [0, 0, 0, 0]
# loop over all ft sensors to calibrate
for chain in self._calibration_chain:
tf_chain = ['/pelvis', 'ltorso', 'mtorso', 'utorso']
if chain == 'left_arm':
joint_range = l_arm_range
tfname = 'l_hand'
tf_chain.extend([
'l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_ufarm',
'l_lfarm', 'l_hand'
])
elif chain == 'right_arm':
joint_range = r_arm_range
tf_chain.extend([
'r_clav', 'r_scap', 'r_uarm', 'r_larm', 'r_ufarm',
'r_lfarm', 'r_hand'
])
tfname = 'r_hand'
else:
Logger.logwarn(
'CalculateForceTorqueCalibration: Undefined chain %s',
chain)
# initialize transformations
tf_data = [
] # frame to frame transformation with numerical indexes from the tf_chain
tf_data_cum = [
] # world to frame transformation with numerical indexes from the tf_chain
for i in range(len(tf_chain)):
tf_data.append([0, 0, 0, 0])
tf_data_cum.append([0, 0, 0, 0])
# get number of poses from the commanded trajectories. 2 Points per Pose
number_of_poses = len(
self._trajectories_command[chain].points) / 2
# define information matrix for calibration
InfMat = np.zeros(
(6 * max_data_points_per_pose * number_of_poses, 10))
InfMat_i = 0 # Index in this Matrix
# definie measurement vector for calibration
MeasVec = np.zeros(
(6 * max_data_points_per_pose * number_of_poses, 1))
# commanded joint trajectory for current chain
# all commanded positions in this trajectory
current_traj_cmd = self._trajectories_command[chain]
# flag if the time interval was already set:
timesetflag = False
point_index_cmd = 0 # start with index 0
transformation_available = False
# read time series from bag file
# check in desired input trajectory, at which time a position is reached
# take the data in a defined time period after the commanded new position
bag_from_robot = Bag(os.path.expanduser(
self._bag_filename)) # open bag file
Logger.loginfo(
'CalculateForceTorqueCalibration: Calibrate %s from %s. Using %d different positions from trajectory command. Expecting 2 commanded positions per pose'
%
(chain, self._bag_filename, len(current_traj_cmd.points)))
for topic, msg, t in bag_from_robot.read_messages(topics=[
'/flor/controller/atlas_state', '/tf',
'/flor/controller/joint_command'
]):
# Check if all desired poses have been reached
if point_index_cmd > len(current_traj_cmd.points) - 1:
break
current_position_cmd = current_traj_cmd.points[
point_index_cmd].positions
####################################################
# Check tf message: remember last transformation from world to ft sensor
if topic == '/tf':
# loop over all transformations inside this tf message
for j in range(len(msg.transforms)):
data = msg.transforms[j]
tr = data.transform # current transformation
header = data.header
# check if frame matches on of the saved frames
for i in range(len(tf_chain)):
if data.child_frame_id == tf_chain[i]:
tf_data[i] = np.array([
tr.rotation.x, tr.rotation.y,
tr.rotation.z, tr.rotation.w
])
# calculate the newest transformation to the force torque sensor
tf_data_cum[0] = tf_data[0]
for i in range(1, len(tf_chain)):
tf_data_cum[
i] = q = tf.transformations.quaternion_multiply(
tf_data_cum[i - 1], tf_data[i])
if i == len(tf_chain) - 1:
time_tf = msg.transforms[
0].header.stamp.to_sec()
# nothing to do with tf message
# check if all transformations are available. (up to the last link)
if np.any(
tf_data_cum[-1]
): # real part of quaternion unequal to zero: data exists
transformation_available = True
continue
else:
time_msg = msg.header.stamp.to_sec()
####################################################
# check if timestamp is interesting. Then check the
if not (time_msg > time_start
and time_msg < time_end) and not timesetflag:
# the timestamp is not in evaluation interval. Look for reaching of the pose
# record data (see below)
if topic == '/flor/controller/joint_command':
# get the commanded position
for i in range(len(joint_range)):
osrf_ndx = joint_range[i]
joint_position_cmd[i] = msg.position[osrf_ndx]
# Check if position command matches the currently expected commanded position
pos_reached = True
for i in range(len(joint_range)):
if abs(joint_position_cmd[i] -
current_position_cmd[i]) > 0.001:
pos_reached = False
break
if pos_reached:
# end time of the values regarded for calibration data: take the latest time possible for this pose
time_end = msg.header.stamp.to_sec(
) + self._settlingtime
# starting time for calibration: Take 100ms
time_start = time_end - 0.1
data_points_for_this_pose = 0
timesetflag = True
# take the next point next time. Each pose consists of two trajectory points (one for reaching, one for settling).
# take the second one
point_index_cmd = point_index_cmd + 2
if not pos_reached:
# The commanded position has not been reached. Skip
continue
continue # continue here, because data aquisition is triggered by the time_start, time_end
if time_msg > time_end:
timesetflag = False # prepare for new evaluation interval
####################################################
# check if enough datapoints for this pose have been collected
if data_points_for_this_pose > max_data_points_per_pose:
# already enough data points for this pose
continue
####################################################
# Check if message is atlas_state
# IF this is the case, fill information matrix
if topic != '/flor/controller/atlas_state':
continue
# Extract measured force and torque
if chain == 'left_arm':
FT = [
msg.l_hand.force.x, msg.l_hand.force.y,
msg.l_hand.force.z, msg.l_hand.torque.x,
msg.l_hand.torque.y, msg.l_hand.torque.z
]
elif chain == 'right_arm':
FT = [
msg.r_hand.force.x, msg.r_hand.force.y,
msg.r_hand.force.z, msg.r_hand.torque.x,
msg.r_hand.torque.y, msg.r_hand.torque.z
]
# calculate gravitation vector
if not transformation_available:
Logger.logwarn(
'No tf messages available at time %1.4f.' %
time_msg)
continue
R = tf.transformations.quaternion_matrix(tf_data_cum[-1])
g = np.dot((R[0:3, 0:3]).transpose(), g_world)
gx = g[0]
gy = g[1]
gz = g[2]
# fill information matrix for this data point. Source: [1], equ. (7)
M = np.zeros((6, 10))
M[0, 0] = gx
M[1, 0] = gy
M[2, 0] = gz
M[3, 2] = gz
M[3, 3] = -gy
M[4, 1] = -gz
M[4, 3] = gx
M[5, 1] = gy
M[5, 2] = -gx
M[0, 4] = 1.0
M[1, 5] = 1.0
M[2, 6] = 1.0
M[3, 7] = 1.0
M[4, 8] = 1.0
M[5, 9] = 1.0
# fill big information matrix and vector (stack small information matrizes)
for i in range(6):
for j in range(10):
InfMat[InfMat_i * 6 + i, j] = M[i, j]
MeasVec[InfMat_i * 6 + i, 0] = FT[i]
InfMat_i = InfMat_i + 1 # increase index
data_points_for_this_pose = data_points_for_this_pose + 1
# shorten big information matrix
if InfMat_i < max_data_points_per_pose * number_of_poses:
InfMat_calc = InfMat[0:(6 * InfMat_i) - 1, :]
MeasVec_calc = MeasVec[0:(6 * InfMat_i - 1), :]
else:
InfMat_calc = InfMat
MeasVec_calc = MeasVec
# calculate calibration data
if chain in self._static_calibration_data.keys(
): # calculate calibration with given static parameters
# bring colums with first parameters on the other side of the equation
if len(self._static_calibration_data[chain]) != 4:
Logger.logwarn(
"CalculateForceTorqueCalibration: Given static calibration data for %s has length %d. Required 4 entries."
% (chain, len(k_fix)))
self._failed = True
return
# convert physical parameters to identification parameters (mass, 1st moment)
m = self._static_calibration_data[chain][0]
if m == 0:
mom_x = 0
mom_y = 0
mom_z = 0
elif m > 0:
mom_x = self._static_calibration_data[chain][1] / m
mom_y = self._static_calibration_data[chain][2] / m
mom_z = self._static_calibration_data[chain][3] / m
else:
Logger.logwarn(
"CalculateForceTorqueCalibration: Negative mass (%f) for calibration requested. Abort."
% mass)
self._failed = True
return
k_fix = np.resize(np.array([m, mom_x, mom_y, mom_z]),
(4, 1))
Logger.loginfo(
"CalculateForceTorqueCalibration:static calibration data for %s given: %s. Reduce equation system"
% (chain, str(self._static_calibration_data[chain])))
MeasVec_calc_corr = np.subtract(
np.array(MeasVec_calc),
np.dot(InfMat_calc[:, 0:4], k_fix))
InfMat_calc_corr = InfMat_calc[:, 4:
10] # only the last 6 colums which correspond to the sensor offsets
k = np.linalg.lstsq(
InfMat_calc_corr,
MeasVec_calc_corr)[0] # solve reduced equation system
k_calibration = self._static_calibration_data[chain]
k_calibration.extend(k)
else: # calculate normally with all parameters unknown
k = np.linalg.lstsq(InfMat_calc, MeasVec_calc)[0]
# convert to physical parameters (first moment -> mass)
if k[0] > 0:
k_calibration = [
k[0], k[1] / k[0], k[2] / k[0], k[3] / k[0], k[4],
k[5], k[6], k[7], k[8], k[9]
]
else:
Logger.loginfo(
"CalculateForceTorqueCalibration:Calibration brought negative mass %f"
% k[0])
k_calibration = [
0.0, 0.0, 0.0, 0.0, k[4], k[5], k[6], k[7], k[8],
k[9]
]
Logger.loginfo(
"CalculateForceTorqueCalibration:calibration data for %s" %
chain)
Logger.loginfo("CalculateForceTorqueCalibration:mass: %f" %
float(k_calibration[0]))
Logger.loginfo(
"CalculateForceTorqueCalibration:center of mass: %f %f %f"
% (k_calibration[1], k_calibration[2], k_calibration[3]))
Logger.loginfo(
"CalculateForceTorqueCalibration:F offset: %f %f %f" %
(k_calibration[4], k_calibration[5], k_calibration[6]))
Logger.loginfo(
"CalculateForceTorqueCalibration:M offset: %f %f %f" %
(k_calibration[7], k_calibration[8], k_calibration[9]))
self._ft_calib_data[chain] = k_calibration
bag_from_robot.close()
userdata.ft_calib_data = self._ft_calib_data
Logger.loginfo(
'CalculateForceTorqueCalibration:Calibration finished')
self._done = True
except Exception as e:
Logger.logwarn(
'CalculateForceTorqueCalibration:Unable to calculate calibration:\n%s'
% str(e))
self._failed = True
def on_enter(self, userdata):
self._failed = False
try:
# Initialization
# userdata
self._bag_filename = userdata.bag_filename
self._trajectories_command = userdata.trajectories_command
# joint definitions
l_arm_range = range(16,23);
r_arm_range = range(23,30);
atlasJointNames = [
'back_bkz', 'back_bky', 'back_bkx', 'neck_ry',
'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_shz', 'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_wry', 'l_arm_wrx', 'l_arm_wry2',
'r_arm_shz', 'r_arm_shx', 'r_arm_ely', 'r_arm_elx', 'r_arm_wry', 'r_arm_wrx', 'r_arm_wry2']
joint_position_cmd = [0]*len(l_arm_range)
# starting and stopping time of measurement data to be taken into account for cailbration
time_start = 0
time_end = 0
time_tf = 0
# gravity vector in world frame
g_world = np.resize(np.array([0,0,-9.81]),(3,1))
# take this number of data points for each pose.
max_data_points_per_pose = 100
# rotation of the force torque sensor in world frame (quaternions)
ft_rotation = [0,0,0,0]
# loop over all ft sensors to calibrate
for chain in self._calibration_chain:
tf_chain=['/pelvis', 'ltorso', 'mtorso', 'utorso']
if chain == 'left_arm':
joint_range = l_arm_range
tfname = 'l_hand'
tf_chain.extend(['l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_ufarm', 'l_lfarm', 'l_hand'])
elif chain == 'right_arm':
joint_range = r_arm_range
tf_chain.extend(['r_clav', 'r_scap', 'r_uarm', 'r_larm', 'r_ufarm', 'r_lfarm', 'r_hand'])
tfname = 'r_hand'
else:
Logger.logwarn('CalculateForceTorqueCalibration: Undefined chain %s', chain)
# initialize transformations
tf_data = [] # frame to frame transformation with numerical indexes from the tf_chain
tf_data_cum = [] # world to frame transformation with numerical indexes from the tf_chain
for i in range(len(tf_chain)):
tf_data.append([0,0,0,0])
tf_data_cum.append([0,0,0,0])
# get number of poses from the commanded trajectories. 2 Points per Pose
number_of_poses = len(self._trajectories_command[chain].points) / 2
# define information matrix for calibration
InfMat = np.zeros((6*max_data_points_per_pose*number_of_poses, 10))
InfMat_i = 0 # Index in this Matrix
# definie measurement vector for calibration
MeasVec = np.zeros((6*max_data_points_per_pose*number_of_poses, 1))
# commanded joint trajectory for current chain
# all commanded positions in this trajectory
current_traj_cmd = self._trajectories_command[chain]
# flag if the time interval was already set:
timesetflag = False
point_index_cmd = 0 # start with index 0
transformation_available = False
# read time series from bag file
# check in desired input trajectory, at which time a position is reached
# take the data in a defined time period after the commanded new position
bag_from_robot = Bag(os.path.expanduser(self._bag_filename)) # open bag file
Logger.loginfo('CalculateForceTorqueCalibration: Calibrate %s from %s. Using %d different positions from trajectory command. Expecting 2 commanded positions per pose' % (chain, self._bag_filename, len(current_traj_cmd.points)) )
for topic, msg, t in bag_from_robot.read_messages(topics=['/flor/controller/atlas_state', '/tf', '/flor/controller/joint_command']):
# Check if all desired poses have been reached
if point_index_cmd > len(current_traj_cmd.points)-1:
break
current_position_cmd = current_traj_cmd.points[point_index_cmd].positions
####################################################
# Check tf message: remember last transformation from world to ft sensor
if topic == '/tf':
# loop over all transformations inside this tf message
for j in range(len(msg.transforms)):
data = msg.transforms[j]
tr = data.transform # current transformation
header = data.header
# check if frame matches on of the saved frames
for i in range(len(tf_chain)):
if data.child_frame_id == tf_chain[i]:
tf_data[i] = np.array([tr.rotation.x, tr.rotation.y, tr.rotation.z, tr.rotation.w])
# calculate the newest transformation to the force torque sensor
tf_data_cum[0] = tf_data[0]
for i in range(1,len(tf_chain)):
tf_data_cum[i] = q = tf.transformations.quaternion_multiply(tf_data_cum[i-1], tf_data[i])
if i == len(tf_chain)-1:
time_tf = msg.transforms[0].header.stamp.to_sec()
# nothing to do with tf message
# check if all transformations are available. (up to the last link)
if np.any(tf_data_cum[-1]): # real part of quaternion unequal to zero: data exists
transformation_available = True
continue
else:
time_msg = msg.header.stamp.to_sec()
####################################################
# check if timestamp is interesting. Then check the
if not (time_msg > time_start and time_msg < time_end) and not timesetflag:
# the timestamp is not in evaluation interval. Look for reaching of the pose
# record data (see below)
if topic == '/flor/controller/joint_command':
# get the commanded position
for i in range(len(joint_range)):
osrf_ndx = joint_range[i]
joint_position_cmd[i] = msg.position[osrf_ndx]
# Check if position command matches the currently expected commanded position
pos_reached = True
for i in range(len(joint_range)):
if abs(joint_position_cmd[i]-current_position_cmd[i]) > 0.001:
pos_reached = False
break
if pos_reached:
# end time of the values regarded for calibration data: take the latest time possible for this pose
time_end = msg.header.stamp.to_sec() + self._settlingtime
# starting time for calibration: Take 100ms
time_start = time_end - 0.1
data_points_for_this_pose = 0
timesetflag = True
# take the next point next time. Each pose consists of two trajectory points (one for reaching, one for settling).
# take the second one
point_index_cmd = point_index_cmd + 2
if not pos_reached:
# The commanded position has not been reached. Skip
continue
continue # continue here, because data aquisition is triggered by the time_start, time_end
if time_msg > time_end:
timesetflag = False # prepare for new evaluation interval
####################################################
# check if enough datapoints for this pose have been collected
if data_points_for_this_pose > max_data_points_per_pose:
# already enough data points for this pose
continue
####################################################
# Check if message is atlas_state
# IF this is the case, fill information matrix
if topic != '/flor/controller/atlas_state':
continue
# Extract measured force and torque
if chain == 'left_arm':
FT = [msg.l_hand.force.x, msg.l_hand.force.y, msg.l_hand.force.z, msg.l_hand.torque.x, msg.l_hand.torque.y, msg.l_hand.torque.z ]
elif chain == 'right_arm':
FT = [msg.r_hand.force.x, msg.r_hand.force.y, msg.r_hand.force.z, msg.r_hand.torque.x, msg.r_hand.torque.y, msg.r_hand.torque.z ]
# calculate gravitation vector
if not transformation_available:
Logger.logwarn('No tf messages available at time %1.4f.' % time_msg)
continue
R = tf.transformations.quaternion_matrix(tf_data_cum[-1])
g = np.dot((R[0:3,0:3]).transpose(), g_world)
gx = g[0]
gy = g[1]
gz = g[2]
# fill information matrix for this data point. Source: [1], equ. (7)
M = np.zeros((6, 10))
M[0,0] = gx
M[1,0] = gy
M[2,0] = gz
M[3,2] = gz
M[3,3] = -gy
M[4,1] = -gz
M[4,3] = gx
M[5,1] = gy
M[5,2] = -gx
M[0,4] = 1.0
M[1,5] = 1.0
M[2,6] = 1.0
M[3,7] = 1.0
M[4,8] = 1.0
M[5,9] = 1.0
# fill big information matrix and vector (stack small information matrizes)
for i in range(6):
for j in range(10):
InfMat[InfMat_i*6+i,j] = M[i,j]
MeasVec[InfMat_i*6+i,0] = FT[i]
InfMat_i = InfMat_i + 1 # increase index
data_points_for_this_pose = data_points_for_this_pose + 1
# shorten big information matrix
if InfMat_i < max_data_points_per_pose*number_of_poses:
InfMat_calc = InfMat[0:(6*InfMat_i)-1,:]
MeasVec_calc = MeasVec[0:(6*InfMat_i-1),:]
else:
InfMat_calc = InfMat
MeasVec_calc = MeasVec
# calculate calibration data
if chain in self._static_calibration_data.keys(): # calculate calibration with given static parameters
# bring colums with first parameters on the other side of the equation
if len(self._static_calibration_data[chain]) != 4:
Logger.logwarn( "CalculateForceTorqueCalibration: Given static calibration data for %s has length %d. Required 4 entries." % (chain, len(k_fix)) )
self._failed = True
return
# convert physical parameters to identification parameters (mass, 1st moment)
m = self._static_calibration_data[chain][0]
if m == 0:
mom_x = 0
mom_y = 0
mom_z = 0
elif m > 0:
mom_x = self._static_calibration_data[chain][1]/m
mom_y = self._static_calibration_data[chain][2]/m
mom_z = self._static_calibration_data[chain][3]/m
else:
Logger.logwarn( "CalculateForceTorqueCalibration: Negative mass (%f) for calibration requested. Abort." % mass )
self._failed = True
return
k_fix = np.resize(np.array([m, mom_x, mom_y, mom_z]),(4,1))
Logger.loginfo( "CalculateForceTorqueCalibration:static calibration data for %s given: %s. Reduce equation system" % (chain, str(self._static_calibration_data[chain])) )
MeasVec_calc_corr = np.subtract(np.array(MeasVec_calc), np.dot(InfMat_calc[:,0:4], k_fix))
InfMat_calc_corr = InfMat_calc[:,4:10] # only the last 6 colums which correspond to the sensor offsets
k = np.linalg.lstsq(InfMat_calc_corr, MeasVec_calc_corr)[0] # solve reduced equation system
k_calibration = self._static_calibration_data[chain]
k_calibration.extend(k)
else: # calculate normally with all parameters unknown
k = np.linalg.lstsq(InfMat_calc, MeasVec_calc)[0]
# convert to physical parameters (first moment -> mass)
if k[0] > 0:
k_calibration = [k[0], k[1]/k[0], k[2]/k[0], k[3]/k[0], k[4], k[5], k[6], k[7], k[8], k[9]]
else:
Logger.loginfo("CalculateForceTorqueCalibration:Calibration brought negative mass %f" % k[0])
k_calibration = [0.0, 0.0, 0.0, 0.0, k[4], k[5], k[6], k[7], k[8], k[9]]
Logger.loginfo("CalculateForceTorqueCalibration:calibration data for %s" % chain)
Logger.loginfo("CalculateForceTorqueCalibration:mass: %f" % float(k_calibration[0]))
Logger.loginfo("CalculateForceTorqueCalibration:center of mass: %f %f %f" % (k_calibration[1], k_calibration[2], k_calibration[3]))
Logger.loginfo("CalculateForceTorqueCalibration:F offset: %f %f %f" % (k_calibration[4], k_calibration[5], k_calibration[6]))
Logger.loginfo("CalculateForceTorqueCalibration:M offset: %f %f %f" % (k_calibration[7], k_calibration[8], k_calibration[9]))
self._ft_calib_data[chain] = k_calibration
bag_from_robot.close()
userdata.ft_calib_data = self._ft_calib_data
Logger.loginfo('CalculateForceTorqueCalibration:Calibration finished')
self._done = True
except Exception as e:
Logger.logwarn('CalculateForceTorqueCalibration:Unable to calculate calibration:\n%s' % str(e))
self._failed = True
from rosbag import Bag
from std_msgs.msg import String
bag = Bag("objective.bag", "w")
bag.write("/liability", String("0xaaEc8f06cd803Daf8807C5aB10660b95d385a0dC"))
bag.write("/log_hash",
String("QmRTsJ4cT3Qax8bfAepTEne1rbFiPkgUApemtWCj9FhEgd"))
bag.close()
def create_ground_truth(path,
time_offset=0,
bagfile='.logs/*.bag',
topic='/StereoTUM/estimated_transform',
file=''):
from rosbag import Bag
from geometry_msgs.msg import TransformStamped
from glob import glob as findfiles
if not file: file = p.join(path, 'data', 'ground_truth.csv')
first = None
msgs = []
timefiles = findfiles(p.join(path, 'params', 'time.yaml'))
if not timefiles:
raise ValueError('Can\'t find %s/params/time.yaml' % path)
return
seq_stamps = (None, None)
with open(timefiles[0]) as stream:
time = yaml.load(stream)
seq_stamps = (time['time']['start'], time['time']['end'])
print('Using time offset: %fs' % time_offset)
a = p.join(path, bagfile)
b = findfiles(a)
if not b:
raise ValueError('Can\'t find the bag file %s' % a)
return
bag = Bag(b[0])
print('Found ground truth file at: %s' % b[0])
print('Will listen to topic %s' % topic)
for bagtopic, msg, t in bag.read_messages():
if bagtopic != topic: continue
now = t.to_sec() + time_offset
if now < seq_stamps[0]: continue # before recording started
if first is None: first = now
if now > seq_stamps[1]: continue # after recording finished
stamp = now - first
# Check if any message with the same timestamp has already been added...
if any(item.get('time', None) == stamp for item in msgs): continue
# If not, add the message to the list of ground truths
msgs.append({
'time': stamp,
'trans': msg.transform.translation,
'orien': msg.transform.rotation
})
bag.close()
print('Noticed %d messages, will now sort them by time...' % len(msgs))
msgs = sorted(msgs, key=lambda x: x['time'])
with open(file, 'w') as f:
print('Creating new file %s' % file)
f.write(
'Timestamp [s]\tTranslation X [m]\tTranslation Y [m]\tTranslation Z [m]\tOrientation W\tOrientation X\tOrientation Y\tOrientation Z\n'
)
for msg in msgs:
f.write('%.6f\t' % msg['time'])
f.write('%6.4f\t' % msg['trans'].x)
f.write('%6.4f\t' % msg['trans'].y)
f.write('%6.4f\t' % msg['trans'].z)
f.write('%6.4f\t' % msg['orien'].w)
f.write('%6.4f\t' % msg['orien'].x)
f.write('%6.4f\t' % msg['orien'].y)
f.write('%6.4f' % msg['orien'].z)
f.write('\n')
# remove last newline from file
f.seek(-1, os.SEEK_END)
f.truncate()
def parse_bag(bag_file_path):
bagfile = Bag(bag_file_path)
msgs_topics = [
"/drone/pose", "/gazebo/ground_truth", "/mavros/local_position/odom",
"/mavros/px4flow/ground_distance"
]
msgs_drone_pose = {
"y": [],
"z": [],
"x": [],
"roll": [],
"pitch": [],
"yaw": [],
"time": [],
"label": "EKF-SLAM"
}
msgs_ground_truth = {
"y": [],
"z": [],
"x": [],
"roll": [],
"pitch": [],
"yaw": [],
"time": [],
"label": "Ground Truth"
}
msgs_mavros_odom = {
"y": [],
"z": [],
"x": [],
"roll": [],
"pitch": [],
"yaw": [],
"time": [],
"label": "MAVROS"
}
msgs_mavros_laser = {"range": [], "time": [], "label": "Laser Range"}
for topic, message, time in bagfile.read_messages(msgs_topics):
if topic == msgs_topics[0]:
msgs_drone_pose["x"].append(message.pose.pose.position.x)
msgs_drone_pose["y"].append(message.pose.pose.position.y)
msgs_drone_pose["z"].append(message.pose.pose.position.z)
quat = message.pose.pose.orientation
euler = quat2euler([quat.x, quat.y, quat.z, quat.w])
msgs_drone_pose["roll"].append(euler[0])
msgs_drone_pose["pitch"].append(euler[1])
msgs_drone_pose["yaw"].append(euler[2])
msgs_drone_pose["time"].append(time.to_sec())
if topic == msgs_topics[1]:
msgs_ground_truth["x"].append(message.pose.pose.position.x)
msgs_ground_truth["y"].append(message.pose.pose.position.y)
msgs_ground_truth["z"].append(message.pose.pose.position.z)
quat = message.pose.pose.orientation
euler = quat2euler([quat.x, quat.y, quat.z, quat.w])
msgs_ground_truth["roll"].append(euler[0])
msgs_ground_truth["pitch"].append(euler[1])
msgs_ground_truth["yaw"].append(euler[2])
msgs_ground_truth["time"].append(time.to_sec())
if topic == msgs_topics[2]:
msgs_mavros_odom["x"].append(message.pose.pose.position.x - 3)
msgs_mavros_odom["y"].append(message.pose.pose.position.y)
msgs_mavros_odom["z"].append(message.pose.pose.position.z)
quat = message.pose.pose.orientation
euler = quat2euler([quat.x, quat.y, quat.z, quat.w])
msgs_mavros_odom["roll"].append(euler[0])
msgs_mavros_odom["pitch"].append(euler[1])
msgs_mavros_odom["yaw"].append(euler[2])
msgs_mavros_odom["time"].append(time.to_sec())
if topic == msgs_topics[3]:
msgs_mavros_laser["range"].append(message.range)
msgs_mavros_laser["time"].append(time.to_sec())
bagfile.close()
return {
"ground_truth": msgs_ground_truth,
"mavros_odom": msgs_mavros_odom,
"mavros_laser": msgs_mavros_laser,
"ekf2": msgs_drone_pose
}
