Browse Source

added comments in the code

pull/7/head
Charlie Kemp 4 years ago
parent
commit
ccb5fb9ab0
1 changed files with 48 additions and 16 deletions
  1. +48
    -16
      stretch_calibration/nodes/process_head_calibration_data

+ 48
- 16
stretch_calibration/nodes/process_head_calibration_data View File

@ -453,8 +453,10 @@ class HeadCalibrator:
def update_urdf(self, parameters):
# This updates self.new_urdf via the camera's kinematic chain.
# Updates self.new_urdf using the provided parameter
# vector.
# Unpack the parameter vector into relevant variables.
i = 0
if self.calibrate_controller_offsets:
@ -509,7 +511,8 @@ class HeadCalibrator:
i += 3
arm_offset_rotvec = parameters[i:i+3]
i += 3
# Update the URDF.
self.camera_pan_joint.origin.xyz = self.original_pan_assembly_xyz + pan_assembly_offset_xyz
pan_assembly_r = Rotation.from_rotvec(pan_assembly_offset_rotvec) * self.original_pan_assembly_r
self.camera_pan_joint.origin.rpy = pan_assembly_r.as_euler('xyz')
@ -534,7 +537,8 @@ class HeadCalibrator:
def calculate_normalization(self):
# Finds the number of observations that will be used for each error measure
# Finds the number of observations that will be used for each
# type of error measurement.
for e in self.error_measures:
e.reset_observation_count()
for i, s in enumerate(self.data):
@ -589,6 +593,9 @@ class HeadCalibrator:
def visualize_fit(self, parameters_to_fit):
# Visualize the fit of the provided parameter vector. Also
# calculate the fit error, display it, and display an error if
# it is too high.
self.visualize = True
self.infinite_duration_visualization = True
fit_error = self.calculate_error(parameters_to_fit)
@ -628,15 +635,22 @@ class HeadCalibrator:
def calculate_error(self, parameters_to_fit, output_error_terms=False):
error_terms = self.initial_error_terms.copy()
# Calculates the fit error for the provide parameter
# vector. This can output the total error by itself or both
# the total error and the individual unweighted error terms
# depending on the output_error_terms argument.
# Initialize the error terms.
error_terms = self.initial_error_terms.copy()
# Find the error based solely on the parameter vector values.
error_terms['parameter_error_total'] += self.parameter_error_term(parameters_to_fit)['total']
# updates the URDF and fit method variables with the current
# parameters
# Updates the URDF and related method variables using the
# current parameter vector.
self.update_urdf(parameters_to_fit)
# Initialize marker array for visualization.
marker_array = MarkerArray()
marker_time = rospy.Time.now()
marker_id = 0
@ -647,9 +661,11 @@ class HeadCalibrator:
# backlash compensation for arm retraction.
self.target_markers = self.generate_target_visualizations()
marker_array.markers.extend(self.target_markers)
# Iterate through all the observations (samples) in the data.
for i, s in enumerate(self.data):
# Only use selected samples.
if self.use_this_sample(i, s):
# Do not alter the original data.
@ -696,20 +712,33 @@ class HeadCalibrator:
for e in self.error_measures:
if e.detected:
if e.location == 'wrist':
# fit_z is only relevant to the wrist,
# which could deflect downward.
delta_error, ros_markers, marker_id = e.error(self.camera_transform, fit_z, fit_orientation, marker_id, self.visualize)
else:
delta_error, ros_markers, marker_id = e.error(self.camera_transform, True, fit_orientation, marker_id, self.visualize)
# Typically there will be far fewer
# observations made with the head tilted
# upwards. This weight can help compensate for
# this imbalance.
if backlash_state['joint_head_tilt_looking_up']:
error_multiplier = self.error_weights['joint_head_tilt_looking_up_error_multiplier']
tilt_error_multiplier = self.error_weights['joint_head_tilt_looking_up_error_multiplier']
else:
error_multiplier = 1.0
tilt_error_multiplier = 1.0
# Add the errors to the relevant error
# terms. For example, there can be both a
# position and an orientation error.
for d in delta_error:
error_term_name = e.name + '_' + d
error_terms[error_term_name] += (error_multiplier * delta_error[d])
error_terms[error_term_name] += (tilt_error_multiplier * delta_error[d])
# Collects markers to visualize.
marker_array.markers.extend(ros_markers)
# Calculate the total error by summing the weighted error
# terms.
total_error = 0.0
for k in error_terms:
total_error += self.error_weights[k] * error_terms[k]
@ -722,9 +751,12 @@ class HeadCalibrator:
m.lifetime = rospy.Duration()
self.visualization_markers_pub.publish(marker_array)
# Returns the total error and the individual, unweighted error
# terms.
if output_error_terms:
return total_error, error_terms
# Only returns the total error.
return total_error

Loading…
Cancel
Save