def recoverXfromT(self, T):
'''
This function will extract the x, y, z, azimuth, elevation in the corrected
coordinate frame (x = insertion) to match the ascension setup
also makes the azimuth term more stable for most of the workspace
'''
x, y, z = T[:3, 3]
alpha, beta, gamma = trig.getAnglesZYZ(T[:3, :3])
return np.array([x, y, z, alpha, beta, gamma])
def get_base_angles(curve, tip_angles):
# finds the deterministic base angles to fit the curve parameters with the measured tip angles
# R_tip_ground = R_base_ground * R_tip_base
# R_base_ground = R_tip_ground * R_tip_base^T
angles_to_base = robot.getPositionFromCurve(curve)[
3:] # extracting expected alpha, beta, gamma from curve
R_tip_base = trig.R_zyz(angles_to_base)
R_tip_ground = trig.R_zyz(tip_angles)
R_base_ground = R_tip_ground.dot(R_tip_base.T)
base_angles = trig.getAnglesZYZ(R_base_ground)
return base_angles
def base_pose_update(robot):
# finds the deterministic base angles to fit the curve parameters with the measured tip angles
# R_tip_ground = R_base_ground * R_tip_base
# R_base_ground = R_tip_ground * R_tip_base^T
# elevation = -pi/2 + beta
if not robot.model_estimation.lower() == 'abc':
return
curve = robot_model.get_leader_curvature(robot.q_desired[4:8] -
robot.q_initial[4:8])
angles_to_base = robot_model.getPositionFromCurve(curve)[
3:] # extracting expected alpha, beta, gamma from curve
R_tip_base = trig.R_zyz(angles_to_base)
tip_angles = trig.aer_to_abc(
robot.x_sensed[3:]) # grabbing azimuth, elevation, roll
tip_angles[2] -= robot.x_initial[
5] * np.pi / 180 # to line up with the taped sensor, which is also zeroed
R_tip_ground = trig.R_zyz(tip_angles)
R_base_ground = R_tip_ground.dot(R_tip_base.T)
base_angles = trig.getAnglesZYZ(R_base_ground)
filter_make_R(robot, base_angles)
def get_sigmas_h(sigmas_f, sigma_num, dx_model, angle_vec, use_curve):
# transform sigma points into measurement space
# see what you'd get for each state
n_dx = len(dx_model)
n_ab = len(angle_vec)
n_curve = 1 if use_curve else 0
sigmas_dx = np.zeros((sigma_num, n_dx))
sigmas_ab = np.zeros((sigma_num, n_ab))
sigmas_curve = np.zeros((sigma_num, n_curve))
for i in range(sigma_num):
R_env_guess = trig.R_zyz(sigmas_f[i, :3])
if n_dx: # differential xyz
sigmas_dx[i, :] = R_env_guess.dot(dx_model[:3])
if n_ab: # instantaneous angles
R_model = trig.R_zyz(angle_vec[i, :])
sigmas_ab[i, :] = trig.getAnglesZYZ(R_env_guess.dot(
R_model))[:n_ab] # excludes roll if not included
if use_curve: # displacment based curvature
sigmas_curve[i, :] = sigmas_f[i, 3]
return np.hstack((sigmas_dx, sigmas_ab, sigmas_curve))
def updatePosition(self):
'''
Finds the tip position and orientation in the ground frame.
Trasforms robot frame by the enviroment-caused rotation matrices
dx is rotated, otherwise it rotates the entire model about the origin
R_env takes robot frame and brings it to ground frame
'''
# self.x_robot_frame_previous = self.x_robot_frame.copy()
self.x_ground_frame_previous = self.x_ground_frame.copy()
self.x_robot_frame, T = self.updateRobotFramePosition(
self.q, self.use_lung_task)
if self.use_lung_task:
self.R_env, self.R_angles = self.lung_env.getEnvRotation(
self.q.copy())
self.x_ground_frame = self.R_env.dot(self.x_robot_frame[:3])
dx = self.x_ground_frame - self.x_ground_frame_previous
dx += self.R_env[:, 2] * (
self.q[8] - self.q_ins_previous
) # insertion not accounted for in updateRobotFramePosition
self.x[:3] += dx
self.x[3:] = trig.getAnglesZYZ(self.R_env.dot(T[:3, :3]))
def getPointsAlongBody(self, q, num=10):
'''
This function was added 3/7/2016, J. Sganga, to enable plotting of the cathter body after a simulation
is run. It mimics update position, except the arc length, s, is allowed to take num equally spaced lengths.
The function will return a 2*num x 6 array (x,y,z,a,b,c of each point for leader and sheath)
Not plotting straight sections, yet
'''
q = self.scaleGearRatios(q)
sheath_points = np.zeros((num, self.x_num))
arc_length = np.linspace(0, self.length, num)
for i, al in enumerate(arc_length):
curve = self.getCurvature(q[0:4] * al / self.length, al)
x = self.getPositionFromCurve(curve, q[8])
T = self.getTfromX(x)
# T[:3,:] = trig.R_reorient.dot(T[:3,:])
x = self.recoverXfromT(T)
sheath_points[i, :] = x
body_points = sheath_points.copy()
if q[9] > 0:
leader_points = np.zeros((num, self.x_num))
arc = min(q[9], self.length)
arc_length = np.linspace(0, arc, num)
for i, al in enumerate(arc_length):
curve_leader = self.getCurvature(
q[4:8] * al / arc, al, leader_flag=True
) # don't want to assume it keeps articulating
x_leader = self.getPositionFromCurve(
curve_leader, max(0, q[9] - self.length))
T_leader = self.getTfromX(x_leader)
T_tip = np.eye(4)
T_tip[:3, 3] = [0, 0, 10] # 1cm rigid extension to pose sensor
T_leader = T.dot(T_leader.dot(T_tip)) # last T from sheath
x = self.recoverXfromT(T_leader)
x[:3] = self.R_env.dot(x[:3])
x[3:] = trig.getAnglesZYZ(self.R_env.dot(T_leader[:3, :3]))
leader_points[i, :] = x
body_points = np.vstack((sheath_points, leader_points))
return body_points