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