def compute_relative_imu_orientation(self): """name convention: parentFrame_childFrame_quat""" cone_imu_vec_quat = tfms.quaternion_from_euler(0, 0, math.pi, 'rxyz') earth_initImu_vec_quat = [ -0.71086415, 0.02551225, 0.00909408, 0.70280765 ] initImu_anchor_vec_quat = tfms.quaternion_from_euler( math.pi / 2, 0, math.pi, 'rxyz') earth_anchor_vec_quat = tfms.quaternion_multiply( earth_initImu_vec_quat, initImu_anchor_vec_quat) self._anchor_imu_vec_quat = tfms.quaternion_multiply( tfms.quaternion_inverse(earth_anchor_vec_quat), self._unit_imu_quaternion) anchor_cone = tfms.quaternion_multiply( self._anchor_imu_vec_quat, tfms.quaternion_inverse(cone_imu_vec_quat)) self._relative_body_quaternion = Quaternion(anchor_cone[0], anchor_cone[1], anchor_cone[2], anchor_cone[3])
def publish_relative_frames(self, data): tmp_dict = {} for segment in data.segments: if(segment.is_quaternion): tmp_dict[segment.id] = (segment.position,segment.quat) else: tmp_dict[segment.id] = (segment.position,segment.euler) for idx in tmp_dict.keys(): p_idx = xsens_client.get_segment_parent_id(idx) child_data = tmp_dict[idx] if(p_idx==-1): continue elif(p_idx==0): new_quat = tft.quaternion_multiply(child_data[1],tft.quaternion_inverse((1,1,1,1)))#if(segment.is_quaternion): TODO Handle Euler #self.sendTransform(child_data[0], self.sendTransform(child_data[0], child_data[1], #(1.0,0,0,0),#FIXME rospy.Time.now(), xsens_client.get_segment_name(idx), self.ref_frame) else: parent_data = tmp_dict[p_idx] parent_transform = tf.Transform(parent_data[1],parent_data[0]) child_transform = tf.Transform(child_data[1],child_data[0]) new_trans = parent_transform.inversetimes(child_transform) new_quat = tft.quaternion_multiply(child_data[1],tft.quaternion_inverse(parent_data[1])) tmp_pos = (child_data[0][0]-parent_data[0][0],child_data[0][1]-parent_data[0][1] ,child_data[0][2]-parent_data[0][2] ) #tmp_pos = (child_data[0][0] - parent_data[0][0],child_data[0][1] - parent_data[0][1],child_data[0][2] - parent_data[0][2]) #self.sendTransform(qv_mult(parent_data[1],tmp_pos), #self.sendTransform(tmp_pos, # new_quat, #child_data[1], self.sendTransform(new_trans.getOrigin(), new_trans.getRotation(),rospy.Time.now(),xsens_client.get_segment_name(idx),xsens_client.get_segment_name(p_idx))
def update_display(self): self.clear_plot() x = [1.0,0.0,0.0,0.0] x2 = [0.8,0.0,0.0,0.0] z = [0.8,0.0,0.1,0.0] q = self.normalize_tf_quaternion([self.val.x, self.val.y, self.val.z,self.val.w]) xr = tft.quaternion_multiply(tft.quaternion_multiply(q,x),tft.quaternion_inverse(q)) x2r = tft.quaternion_multiply(tft.quaternion_multiply(q,x2),tft.quaternion_inverse(q)) zr = tft.quaternion_multiply(tft.quaternion_multiply(q,z),tft.quaternion_inverse(q)) self.plot_3d_ax.plot([0.0,0.5], [0.0,0.0], [0.0,0.0], 'r-') self.plot_3d_ax.plot([0.0,0.0], [0.0,0.5], [0.0,0.0], 'g-') self.plot_3d_ax.plot([0.0,0.0], [0.0,0.0], [0.0,0.5], 'b-') self.plot_3d_ax.plot([0.0,xr[0]], [0.0,xr[1]], [0.0,xr[2]], 'k-', linewidth=2) self.plot_3d_ax.plot([zr[0],xr[0]], [zr[1],xr[1]], [zr[2],xr[2]], 'k-', linewidth=2) self.plot_3d_ax.plot([zr[0],x2r[0]], [zr[1],x2r[1]], [zr[2],x2r[2]], 'k-', linewidth=2) self.plot_3d_ax.set_aspect('equal', 'box') self.plot_3d_ax.set_xlim(-1.0, 1.0) self.plot_3d_ax.set_ylim(-1.0, 1.0) self.plot_3d_ax.set_zlim(-1.0, 1.0) self.plot_3d_canvas.draw() if not self.manual_mode: self.update_normalized_displays()
def increment_reference(self): ''' Steps the model reference (trajectory to track) by one self.timestep. ''' # Frame management quantities R_ref = trns.quaternion_matrix(self.q_ref)[:3, :3] y_ref = trns.euler_from_quaternion(self.q_ref)[2] # Convert body PD gains to world frame kp = R_ref.dot(self.kp_body).dot(R_ref.T) kd = R_ref.dot(self.kd_body).dot(R_ref.T) # Compute error components (desired - reference), using "smartyaw" p_err = self.p_des - self.p_ref v_err = -self.v_ref w_err = -self.w_ref if npl.norm(p_err) <= self.heading_threshold: q_err = trns.quaternion_multiply( self.q_des, trns.quaternion_inverse(self.q_ref)) else: q_direct = trns.quaternion_from_euler( 0, 0, np.angle(p_err[0] + (1j * p_err[1]))) q_err = trns.quaternion_multiply( q_direct, trns.quaternion_inverse(self.q_ref)) y_err = trns.euler_from_quaternion(q_err)[2] # Combine error components into error vectors err = np.concatenate((p_err, [y_err])) errdot = np.concatenate((v_err, [w_err])) wrench = (kp.dot(err)) + (kd.dot(errdot)) # Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts B = np.concatenate((R_ref.dot(self.thruster_directions.T), R_ref.dot(self.lever_arms.T))) B_3dof = np.concatenate((B[:2, :], [B[5, :]])) command = self.thruster_mapper(wrench, B_3dof) wrench_saturated = B.dot(command) # Use model drag to find drag force on virtual boat twist_body = R_ref.T.dot(np.concatenate((self.v_ref, [self.w_ref]))) D_body = np.zeros_like(twist_body) for i, v in enumerate(twist_body): if v >= 0: D_body[i] = self.D_body_positive[i] else: D_body[i] = self.D_body_negative[i] drag_ref = R_ref.dot(D_body * twist_body * abs(twist_body)) # Step forward the dynamics of the virtual boat self.a_ref = (wrench_saturated[:2] - drag_ref[:2]) / self.mass_ref self.aa_ref = (wrench_saturated[5] - drag_ref[2]) / self.inertia_ref self.p_ref = self.p_ref + (self.v_ref * self.timestep) self.q_ref = trns.quaternion_from_euler( 0, 0, y_ref + (self.w_ref * self.timestep)) self.v_ref = self.v_ref + (self.a_ref * self.timestep) self.w_ref = self.w_ref + (self.aa_ref * self.timestep)
def calc_offset_angle(current): """Calculates the offset angle from the x axis positiion""" x_axis = [1, 0, 0, 0] a = quaternion_multiply(x_axis, quaternion_inverse(current)) rotation = quaternion_multiply(quaternion_multiply(a, x_axis), quaternion_inverse(a)) angle = atan2(rotation[1], rotation[0]) return angle
def increment_reference(self): ''' Steps the model reference (trajectory to track) by one self.timestep. ''' # Frame management quantities R_ref = trns.quaternion_matrix(self.q_ref)[:3, :3] y_ref = trns.euler_from_quaternion(self.q_ref)[2] # Convert body PD gains to world frame kp = R_ref.dot(self.kp_body).dot(R_ref.T) kd = R_ref.dot(self.kd_body).dot(R_ref.T) # Compute error components (desired - reference), using "smartyaw" p_err = self.p_des - self.p_ref v_err = -self.v_ref w_err = -self.w_ref if npl.norm(p_err) <= self.heading_threshold: q_err = trns.quaternion_multiply(self.q_des, trns.quaternion_inverse(self.q_ref)) else: q_direct = trns.quaternion_from_euler(0, 0, np.angle(p_err[0] + (1j * p_err[1]))) q_err = trns.quaternion_multiply(q_direct, trns.quaternion_inverse(self.q_ref)) y_err = trns.euler_from_quaternion(q_err)[2] # Combine error components into error vectors err = np.concatenate((p_err, [y_err])) errdot = np.concatenate((v_err, [w_err])) wrench = (kp.dot(err)) + (kd.dot(errdot)) # Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts B = np.concatenate((R_ref.dot(self.thruster_directions.T), R_ref.dot(self.lever_arms.T))) B_3dof = np.concatenate((B[:2, :], [B[5, :]])) command = self.thruster_mapper(wrench, B_3dof) wrench_saturated = B.dot(command) # Use model drag to find drag force on virtual boat twist_body = R_ref.T.dot(np.concatenate((self.v_ref, [self.w_ref]))) D_body = np.zeros_like(twist_body) for i, v in enumerate(twist_body): if v >= 0: D_body[i] = self.D_body_positive[i] else: D_body[i] = self.D_body_negative[i] drag_ref = R_ref.dot(D_body * twist_body * abs(twist_body)) # Step forward the dynamics of the virtual boat self.a_ref = (wrench_saturated[:2] - drag_ref[:2]) / self.mass_ref self.aa_ref = (wrench_saturated[5] - drag_ref[2]) / self.inertia_ref self.p_ref = self.p_ref + (self.v_ref * self.timestep) self.q_ref = trns.quaternion_from_euler(0, 0, y_ref + (self.w_ref * self.timestep)) self.v_ref = self.v_ref + (self.a_ref * self.timestep) self.w_ref = self.w_ref + (self.aa_ref * self.timestep)
def _get_quaternion_error(self, q, target_q): """ Returns an angluar differnce between q and target_q in radians """ dq = trns.quaternion_multiply(np.array(target_q), trns.quaternion_inverse(np.array(q))) return 2 * np.arccos(dq[3])
def _generate_vel(self, s=None): if self._stamped_pose_only: return np.zeros(6) cur_s = (self._cur_s if s is None else s) last_s = cur_s - self.interpolator.s_step if last_s < 0 or cur_s > 1: return np.zeros(6) q_cur = self.interpolator.generate_quat(cur_s) q_last = self.interpolator.generate_quat(last_s) cur_pos = self.interpolator.generate_pos(cur_s) last_pos = self.interpolator.generate_pos(last_s) ######################################################## # Computing angular velocities ######################################################## # Quaternion difference to the last step in the inertial frame q_diff = quaternion_multiply(q_cur, quaternion_inverse(q_last)) # Angular velocity ang_vel = 2 * q_diff[0:3] / self._t_step vel = [(cur_pos[0] - last_pos[0]) / self._t_step, (cur_pos[1] - last_pos[1]) / self._t_step, (cur_pos[2] - last_pos[2]) / self._t_step, ang_vel[0], ang_vel[1], ang_vel[2]] return np.array(vel)
def deltaPose(currPose, desPose, posFrame=None, rotFrame=None): """ Returns pose0 - pose1 """ currPose, desPose = tfx.pose(currPose), tfx.pose(desPose) currPoseFrame, desPoseFrame = currPose.frame, desPose.frame currPos, desPos = currPose.position, desPose.position currRot, desRot = currPose.orientation, desPose.orientation if posFrame is not None and currPoseFrame is not None: tf_currPos_to_posFrame = tfx.lookupTransform(posFrame, currPoseFrame, wait=10) currPos = tf_currPos_to_posFrame * currPos tf_desPos_to_posFrame = tfx.lookupTransform(posFrame, desPoseFrame, wait=10) desPos = tf_desPos_to_posFrame * desPos if rotFrame is not None and currPoseFrame is not None: tf_currRot_to_rotFrame = tfx.lookupTransform(rotFrame, currPoseFrame, wait=10) currRot = tf_currRot_to_rotFrame * currRot tf_desRot_to_rotFrame = tfx.lookupTransform(rotFrame, desPoseFrame, wait=10) desRot = tf_desRot_to_rotFrame * desRot deltaPosition = desPos.array - currPos.array currQuat, desQuat = tfx.tb_angles(currRot).quaternion, tfx.tb_angles(desRot).quaternion deltaQuat = tft.quaternion_multiply(tft.quaternion_inverse(currQuat), desQuat) deltaPose = tfx.pose(deltaPosition, deltaQuat) return deltaPose
def update(self, dt, desired, current): (p, o), (p_dot, o_dot) = current (desired_p, desired_o), (desired_p_dot, desired_o_dot), (desired_p_dotdot, desired_o_dotdot) = desired world_from_body = transformations.quaternion_matrix(o)[:3, :3] x_dot = numpy.concatenate([ world_from_body.dot(p_dot), world_from_body.dot(o_dot), ]) # world_from_desiredbody = transformations.quaternion_matrix(desired_o)[:3, :3] desired_x_dot = numpy.concatenate([ world_from_body.dot(desired_p_dot), world_from_body.dot(desired_o_dot), ]) desired_x_dotdot = numpy.concatenate([ world_from_body.dot(desired_p_dotdot), world_from_body.dot(desired_o_dotdot), ]) error_position_world = numpy.concatenate([ desired_p - p, quat_to_rotvec(transformations.quaternion_multiply( desired_o, transformations.quaternion_inverse(o), )), ]) if self.config['two_d_mode']: error_position_world = error_position_world * [1, 1, 0, 0, 0, 1] world_from_body2 = numpy.zeros((6, 6)) world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body # Permitting lambda assignment b/c legacy body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T) # noqa error_velocity_world = (desired_x_dot + body_gain(numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot if self.config['two_d_mode']: error_velocity_world = error_velocity_world * [1, 1, 0, 0, 0, 1] pd_output = body_gain(numpy.diag(self.config['ks'])).dot(error_velocity_world) output = pd_output if self.config['use_rise']: rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \ body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world)) self._rise_term = self._rise_term + dt / 2 * (rise_term_int + self._rise_term_int_prev) self._rise_term_int_prev = rise_term_int output = output + self._rise_term else: # zero rise term so it doesn't wind up over time self._rise_term = numpy.zeros(6) self._rise_term_int_prev = numpy.zeros(6) output = output + body_gain(numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot) output = output + body_gain(numpy.diag(self.config['vel_feedforward'])).dot(desired_x_dot) # Permitting lambda assignment b/c legacy wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]), world_from_body.T.dot(output[3:6])) # noqa return wrench_from_vec(pd_output), wrench_from_vec(output)
def plot_command_matrix_in_matplotlib(command_matrix, control_dimensions, title): from mpl_toolkits.mplot3d import axes3d import matplotlib.pyplot as plt from tf.transformations import ( quaternion_inverse, quaternion_multiply, ) fig = plt.figure() ax = fig.gca(projection='3d') for i in range(command_matrix.shape[0]): qxyzw = command_matrix[i][qxyzw_idx] uvw = quaternion_multiply( qxyzw, quaternion_multiply( [1, 0, 0, 0], quaternion_inverse(qxyzw), )) x, y, z = command_matrix[i][pxyz_idx] u, v, w = uvw[:3] ax.quiver(x, y, z, u, v, w, length=0.01) ax.set_title(title) fig.show()
def update_attractor_rotvels(self): diff_quat = tfs.quaternion_multiply( self.goal_quats, tfs.quaternion_inverse(self.eef_quat)) diff_quat = diff_quat / np.linalg.norm(diff_quat) self.theta_to_goals = 2 * math.acos( diff_quat[3]) #0 to 2pi. only rotation in one direction. if self.theta_to_goals > math.pi: self.theta_to_goals -= 2 * math.pi self.theta_to_goals = abs(self.theta_to_goals) diff_quat = -diff_quat self.pfieldrot_array = (np.zeros((1, 3)))[0] norm_den = math.sqrt(1 - diff_quat[3] * diff_quat[3]) if norm_den < 0.001: self.pfieldrot_array[0] = diff_quat[0] self.pfieldrot_array[1] = diff_quat[1] self.pfieldrot_array[2] = diff_quat[2] else: self.pfieldrot_array[0] = diff_quat[0] / norm_den self.pfieldrot_array[1] = diff_quat[1] / norm_den self.pfieldrot_array[2] = diff_quat[2] / norm_den self.pfieldrot_array[:] = 0.375 * self.pfieldrot_array[:] #sclae the velocity # if self.stage == 1: # if abs(self.theta_to_goals) < 0.1: # self.stage = 2 # self.update_stage() # print "Got into stage 2" # self.pfieldrot_array = (np.zeros((1,3)))[0] # else: # if abs(self.theta_to_goals) < 0.12: # self.pfieldrot_array = (np.zeros((1,3)))[0] if abs(self.theta_to_goals) < 0.12: self.pfieldrot_array = (np.zeros((1, 3)))[0]
def compute_quaternion_euler_offset(original_quaternion, new_quaternion): offset_quaternion = transformations.quaternion_multiply( transformations.quaternion_inverse(new_quaternion), original_quaternion) yaw, pitch, roll = transformations.euler_from_quaternion(offset_quaternion, axes='szyx') return (abs(yaw) + abs(pitch) + abs(roll))
def inverse_transform(t): """ Return the inverse transformation of t :param t: A transform [[x, y, z], [x, y, z, w]] :return: t2 such as multiply_transform_(t, t2) = [[0, 0, 0], [0, 0, 0, 1]] """ return [quat_rotate(transformations.quaternion_inverse(t[1]), [-t[0][0], -t[0][1], -t[0][2]]), transformations.quaternion_inverse(t[1])]
def get_orientation_difference(self, desired_orientation): """ Finds difference between rigid body position and inputted position. Utilizes the following formula: difference = desired_orientation * inverse(current_orientation) where difference, desired_orientation, and current_orientation are quaternions, and inverse() denotes the quaternion inverse. Parameters ---------- desired_orientation : QuaternionArray Array of [x, y, z, w] of desired orientation Returns ------- difference: QuaternionArray Difference between rigid bodies orientation and inputted orientation """ inverted = quaternion_inverse(self.orientation) return quaternion_multiply(desired_orientation, inverted)
def get_velocity(self, prev_pose, cur_pose): velocity = TwistStamped() velocity.header.stamp = prev_pose.header.stamp velocity.header.frame_id = prev_pose.header.frame_id delta = (prev_pose.header.stamp - cur_pose.header.stamp).to_sec() velocity.twist.linear.x = (cur_pose.pose.pose.position.x - prev_pose.pose.pose.position.x) / delta velocity.twist.linear.y = (cur_pose.pose.pose.position.y - prev_pose.pose.pose.position.y) / delta velocity.twist.linear.z = (cur_pose.pose.pose.position.z - prev_pose.pose.pose.position.z) / delta #get angular velocity prev_pose_quat = numpy.array([ prev_pose.pose.pose.orientation.x, prev_pose.pose.pose.orientation.y, prev_pose.pose.pose.orientation.z, prev_pose.pose.pose.orientation.w ], dtype=numpy.float64) cur_pose_quat = numpy.array([ cur_pose.pose.pose.orientation.x, cur_pose.pose.pose.orientation.y, cur_pose.pose.pose.orientation.z, cur_pose.pose.pose.orientation.w ], dtype=numpy.float64) resudal_quat = cur_pose_quat - prev_pose_quat der_quat = numpy.float64(2.0) * resudal_quat / numpy.float64(delta) vel_quat = transformations.quaternion_multiply( transformations.quaternion_inverse(prev_pose_quat), der_quat) velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z = vel_quat.tolist( )[0:3] return velocity
def handle_odometry(self, odom): self.last_odom = self.curr_odom self.curr_odom = odom if self.last_odom: p_curr = np.array([ self.curr_odom.pose.pose.position.x, self.curr_odom.pose.pose.position.y, self.curr_odom.pose.pose.position.z ]) p_last = np.array([ self.last_odom.pose.pose.position.x, self.last_odom.pose.pose.position.y, self.last_odom.pose.pose.position.z ]) q_curr = np.array([ self.curr_odom.pose.pose.orientation.x, self.curr_odom.pose.pose.orientation.y, self.curr_odom.pose.pose.orientation.z, self.curr_odom.pose.pose.orientation.w ]) q_last = np.array([ self.last_odom.pose.pose.orientation.x, self.last_odom.pose.pose.orientation.y, self.last_odom.pose.pose.orientation.z, self.last_odom.pose.pose.orientation.w ]) rot_last = tr.quaternion_matrix(q_last)[0:3, 0:3] p_last_curr = rot_last.transpose().dot(p_curr - p_last) q_last_curr = tr.quaternion_multiply(tr.quaternion_inverse(q_last), q_curr) _, _, diff = tr.euler_from_quaternion(q_last_curr) self.dtheta = diff % (2 * np.pi) self.dx = (p_last_curr[0]) * 1000 self.dy = (p_last_curr[1]) * 1000
def handler(self, req): #define messages print("Message received") res = MoveToWaypointResponse() traj = Odometry() rate = rospy.Rate(10) while (self.boat_pos.size == 0): rate.sleep() pos = req.target_p.position ori = req.target_p.orientation #create boat trajectory traj.header.frame_id = "enu" traj.child_frame_id = "wamv/base_link" traj.pose.pose.position.x = pos.x traj.pose.pose.position.y = pos.y traj.pose.pose.orientation.x = ori.x traj.pose.pose.orientation.y = ori.y traj.pose.pose.orientation.z = ori.z traj.pose.pose.orientation.w = ori.w print("sending trajectory") self.pub.publish(traj) #go to waypoint #TODO: Account for orientation to determine when we are finished x_err = 100 y_err = 100 yaw_err = 100 start_force_end = False start_force_end_time = None self.orientation = np.array([ori.x, ori.y, ori.z, ori.w]) while ( (x_err > self.x_thresh) or (y_err > self.y_thresh) or (yaw_err > self.yaw_thresh) ): yaw_err = trns.euler_from_quaternion(trns.quaternion_multiply( self.boat_ori, trns.quaternion_inverse(self.orientation)))[2] x_err = abs(self.boat_pos[0] - pos.x) y_err = abs(self.boat_pos[1] - pos.y) if (x_err < 1) and (y_err < 1) and (yaw_err < 0.02) and not start_force_end: start_force_end = True start_force_end_time = rospy.get_rostime() if start_force_end and ((rospy.get_rostime() - start_force_end_time).to_sec() > 10): #Exit out of while loop after 10 seconds of being in same point break rate.sleep() print("Arrived") self.boat_pos = np.array([]) res.success = True return res
def __sub__(self, other): return numpy.concatenate([ self._p - other._p, _rotvec_from_quat(transformations.quaternion_multiply( self._q, transformations.quaternion_inverse(other._q), )), ])
def plot_command_matrix_in_matplotlib(command_matrix, control_dimensions, title): from mpl_toolkits.mplot3d import axes3d import matplotlib.pyplot as plt from tf.transformations import ( quaternion_inverse, quaternion_multiply, ) fig = plt.figure() ax = fig.gca(projection='3d') #ax.set_xlim(0,1) #ax.set_ylim(0,1) #ax.set_zlim(0,1) pxyz_idx = util.get_position_xyz_index(control_dimensions) qxyzw_idx = util.get_quaternion_xyzw_index(control_dimensions) for i in range(command_matrix.shape[0]): qxyzw = command_matrix[i][qxyzw_idx] uvw = quaternion_multiply( qxyzw, quaternion_multiply( [1,0,0,0], quaternion_inverse(qxyzw), ) ) uvw_ = quaternion_multiply( qxyzw, quaternion_multiply( [0,1,0,0], quaternion_inverse(qxyzw), ) ) uvw__ = quaternion_multiply( qxyzw, quaternion_multiply( [0,0,1,0], quaternion_inverse(qxyzw), ) ) x, y, z = command_matrix[i][pxyz_idx] u, v, w = uvw[:3] u_, v_, w_ = uvw_[:3] u__, v__, w__ = uvw__[:3] ax.quiver(x, y, z, u, v, w, length=0.003,color='red') ax.quiver(x, y, z, u_, v_, w_, length=0.005,color='green') ax.quiver(x, y, z, u__, v__, w__, length=0.002,color='blue') ax.set_title(title) fig.show()
def rotation_from(self, *args, **kwargs): """Get the rotation (as tb_angles) which, when applied to the input rotation by right-multiplication, produces this rotation""" other = tb_angles(*args, **kwargs) out = tb_angles( tft.quaternion_multiply(tft.quaternion_inverse(other.quaternion), self.quaternion)) return out
def publish_relative_frames2(self, data): tmp_dict = {} for segment in data.segments: if(segment.is_quaternion): tmp_dict[segment.id] = (segment.position,segment.quat,self.tr.fromTranslationRotation(segment.position,segment.quat)) else: tmp_dict[segment.id] = (segment.position,segment.euler) for idx in tmp_dict.keys(): p_idx = xsens_client.get_segment_parent_id(idx) child_data = tmp_dict[idx] if(p_idx==-1): continue elif(p_idx==0): helper = tft.quaternion_about_axis(1,(-1,0,0)) new_quat = tft.quaternion_multiply(tft.quaternion_inverse(helper),child_data[1])#if(segment.is_quaternion): TODO Handle Euler #self.sendTransform(child_data[0], self.sendTransform(child_data[0], child_data[1], #(1.0,0,0,0),#FIXME rospy.Time.now(), xsens_client.get_segment_name(idx), self.ref_frame) elif(p_idx>0): parent_data = tmp_dict[p_idx] new_quat = tft.quaternion_multiply(tft.quaternion_inverse(parent_data[1]),child_data[1]) tmp_trans = (parent_data[0][0] - child_data[0][0],parent_data[0][1] - child_data[0][1],parent_data[0][2] - child_data[0][2]) new_trans = qv_mult(parent_data[1],tmp_trans) self.sendTransform( new_trans, new_quat, rospy.Time.now(), xsens_client.get_segment_name(idx), xsens_client.get_segment_name(p_idx)) else: parent_data = tmp_dict[p_idx] this_data = np.multiply(tft.inverse_matrix(child_data[2]) , parent_data[2]) self.sendTransform( tft.translation_from_matrix(this_data), tft.quaternion_from_matrix(this_data), rospy.Time.now(), xsens_client.get_segment_name(idx), xsens_client.get_segment_name(p_idx))
def getRobotGridPosition(transMsg, gridInfo): pos = np.array([transMsg.transform.translation.x - gridInfo.origin.position.x, transMsg.transform.translation.y - gridInfo.origin.position.y, 0, 1]) quat = gridInfo.origin.orientation mat = tft.quaternion_matrix(tft.quaternion_inverse([quat.x, quat.y, quat.z, quat.w])) gridPos = (mat.dot(pos[np.newaxis].T).flatten()[:2]) / gridInfo.resolution roundedPos = np.round(gridPos) pos = roundedPos if np.allclose(gridPos, roundedPos) else np.floor(gridPos) return pos
def plot_command_matrix_in_matplotlib(command_matrix): from mpl_toolkits.mplot3d import axes3d import matplotlib.pyplot as plt from tf.transformations import ( quaternion_inverse, quaternion_multiply, ) fig = plt.figure() ax = fig.gca(projection='3d') ax.set_xlim(-1, 1) ax.set_ylim(-1, 1) ax.set_zlim(-1, 1) pxyz_idx = [0, 1, 2] qxyzw_idx = [3, 4, 5, 6] for i in range(command_matrix.shape[0]): qxyzw = command_matrix[i][qxyzw_idx] uvw = quaternion_multiply( qxyzw, quaternion_multiply( [1, 0, 0, 0], quaternion_inverse(qxyzw), )) uvw_ = quaternion_multiply( qxyzw, quaternion_multiply( [0, 1, 0, 0], quaternion_inverse(qxyzw), )) uvw__ = quaternion_multiply( qxyzw, quaternion_multiply( [0, 0, 1, 0], quaternion_inverse(qxyzw), )) x, y, z = command_matrix[i][pxyz_idx] u, v, w = uvw[:3] u_, v_, w_ = uvw_[:3] u__, v__, w__ = uvw__[:3] ax.quiver(x, y, z, u, v, w, length=0.01, color='red') ax.quiver(x, y, z, u_, v_, w_, length=0.01, color='green') ax.quiver(x, y, z, u__, v__, w__, length=0.01, color='blue') fig.show() plt.show()
def error(pose1,pose2): xyzerr = pose1[0:3,:] - pose2[0:3,:] rpy_list = [] for q1,q2 in zip(pose1[3:7,:].T,pose2[3:7,:].T): quaterr = tf.quaternion_multiply(tf.quaternion_inverse(q2),q1) eulererr = tf.euler_from_quaternion(quaterr) rpy_list.append(eulererr) rpy_list = np.array(rpy_list).T return np.vstack((xyzerr,rpy_list))
def compute_imu_pose(self): """This function should compute pose of IMU in world frame""" self._world_camera_position = [ -0.943419533992, -0.509536543674, 0.111264370752 ] # self._world_camera_quat = tfms.quaternion_from_euler(-math.pi/2,- math.pi/2, 0, 'rxyz') world_camera_init = tfms.quaternion_from_euler(-math.pi / 2, -math.pi / 2, 0, 'rxyz') self._world_camera_quat = tfms.quaternion_multiply( world_camera_init, tfms.quaternion_from_euler(math.radians(-28.5), 0, 0, 'rxyz')) camera_imu_position_cameraframe = [ self._object_pose_from_tag.position.x, self._object_pose_from_tag.position.y, self._object_pose_from_tag.position.z, 0 ] camera_imu_position = np.matmul( tfms.quaternion_matrix(self._world_camera_quat), camera_imu_position_cameraframe) world_imu_position = [ self._world_camera_position[0] + camera_imu_position[0], self._world_camera_position[1] + camera_imu_position[1], self._world_camera_position[2] + camera_imu_position[2] ] "Some additions here" earth_initIMU_quat = [ -0.71889202, -0.00903345, -0.02179016, 0.69472142 ] earth_world_quat = tfms.quaternion_multiply( earth_initIMU_quat, tfms.quaternion_from_euler(math.pi / 2, 0, math.pi / 2, 'rxyz')) # print(earth_world_quat) # earth_world_quat = [ 0.0078735, -0.01568597, 0.67480203, 0.73779006] #[-0.02563422, 0.02972348, 0.72386144, 0.68882801] -old one world_imu_quat = tfms.quaternion_multiply( tfms.quaternion_inverse(earth_world_quat), self._unit_imu_quaternion) self._world_imu_pose = Pose( Point(world_imu_position[0], world_imu_position[1], world_imu_position[2]), Quaternion(world_imu_quat[0], world_imu_quat[1], world_imu_quat[2], world_imu_quat[3])) self.compute_object_pose(world_imu_position, world_imu_quat)
def get_control_cmd(self, cur_value, reference, eular=False): if eular: reference = quaternion_from_euler(*reference) quaternion_diff = quaternion_multiply(reference, quaternion_inverse(cur_value)) euler_diff = np.array(euler_from_quaternion(quaternion_diff)) control_cmd = self.k_p * euler_diff return list(control_cmd)
def getDiff(self,futur=0,past=1): if len(self.TransList)-futur < 0 or len(self.TransList)-past < 0: ROS_ERROR("trying to see past the buffer size") return 0 else: euler = trans.euler_from_quaternion(self.rotList[-futur]*trans.quaternion_inverse(self.rotList[-past])) TransVect = (self.TransList[-futur][0]-self.TransList[-past][0], self.TransList[-futur][1]-self.TransList[-past][1], self.TransList[-futur][2]-self.TransList[-past][2]) return (TransVect,euler)
def on_imu_msg(self, msg): if self.initial_orientation is None: self.initial_orientation = msg.orientation return current_orientation = transformations.quaternion_multiply( quat_msg_to_array(msg.orientation), transformations.quaternion_inverse( quat_msg_to_array(self.initial_orientation))) data = transformations.euler_from_quaternion(current_orientation) self.orientation = Vector3(data[0], data[1], data[2])
def on_imu_msg(self, msg): if self.initial_orientation is None: self.initial_orientation = msg.orientation return current_orientation = transformations.quaternion_multiply( quat_msg_to_array(msg.orientation), transformations.quaternion_inverse(quat_msg_to_array(self.initial_orientation))) print(map(lambda v: degrees(v), transformations.euler_from_quaternion(current_orientation))) print(map(lambda v: degrees(v), transformations.euler_from_quaternion(quat_msg_to_array(msg.orientation)))) print("")
def testRotation(): rospy.init_node('ar_image_detection') imageDetector = ARImageDetector() listener = tf.TransformListener() tf_br = tf.TransformBroadcaster() while not rospy.is_shutdown(): if imageDetector.hasFoundGripper(Constants.Arm.Left): obj = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped() gripper = imageDetector.getGripperPose(Constants.Arm.Left) print('gripper') print(gripper) # obj_tb = tfx.tb_angles(obj.pose.orientation) gripper_tb = tfx.tb_angles(gripper.pose.orientation) print "gripper ori", gripper_tb obj_tb = tfx.tb_angles(imageDetector.normal) print "obj ori", obj_tb pt = gripper.pose.position ori = imageDetector.normal tf_br.sendTransform((pt.x, pt.y, pt.z), (ori.x, ori.y, ori.z, ori.w), gripper.header.stamp, '/des_pose', Constants.AR.Frames.Base) between = Util.angleBetweenQuaternions(ori, gripper_tb.msg) print('Angle between') print(between) quat = tft.quaternion_multiply(gripper_tb.quaternion, tft.quaternion_inverse(obj_tb.quaternion)) print 'new', tfx.tb_angles(quat) #rot = gripper_tb.rotation_to(ori) rot = gripper_tb.rotation_to(obj_tb) print('Rotation from gripper to obj') print(rot) deltaPoseTb = tfx.pose(Util.deltaPose(gripper, obj)).orientation print('deltaPose') print(deltaPoseTb) deltaPose = tfx.pose([0,0,0], deltaPoseTb).msg.PoseStamped() time = listener.getLatestCommonTime('0_link', 'tool_L') deltaPose.header.stamp = time deltaPose.header.frame_id = '0_link' deltaPose = listener.transformPose('tool_L', deltaPose) print "transformed", tfx.tb_angles(deltaPose.pose.orientation) endQuatMat = gripper_tb.matrix * rot.matrix print 'desOri', tfx.tb_angles(endQuatMat) rospy.sleep(1)
def update(self, now, accel, gyro, quat): self.accel = accel self.gyro = gyro w, x, y, z = quat # w, x, y, z = -w, y, x, z w, x, y, z = w, -y, x, z quat = w, x, y, z # quat = t.quaternion_multiply(t.quaternion_about_axis(math.pi, [0, 0, 1]), quat) self.quat_absolute = quat # create relative quat, based on our absolute quaternion and our parent's absolute quaternion self.quat_relative = (t.quaternion_multiply( self.quat_absolute, t.quaternion_inverse( self.parent.quat_absolute)) if self.parent else self.quat_absolute) self.quat_relative = t.quaternion_multiply( self.quat_relative, t.quaternion_about_axis(math.pi, [1, 0, 0])) # euler = t.euler_from_quaternion(quat_relative, axes='syzx') # quat_relative = t.quaternion_from_euler(euler[0], euler[1], -euler[2], axes='sxyz') # self.quat_relative = t.quaternion_multiply(quat_relative, t.quaternion_inverse(self.parent.quat_relative)) if self.lastUpdatedAt and self.relative_average_rate: dt = (now - self.lastUpdatedAt).to_sec() f = dt * self.relative_average_rate self.quat_average = self.quat_relative \ if self.quat_average is None \ else t.quaternion_slerp(self.quat_average, self.quat_relative, f) self.quat_relative_to_average = t.quaternion_multiply( self.quat_relative, t.quaternion_inverse(self.quat_average)) else: self.quat_average = self.quat_relative self.quat_relative_to_average = self.quat_relative self.lastUpdatedAt = now
def computeGraspToPickMatrix(self): pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion) translation = [pick_translation_distance, 0, 0] rotation_matrix[[0, 1, 2], 3] = translation pick_to_grasp_matrix = np.mat(rotation_matrix) grasp_to_pick_matrix = pick_to_grasp_matrix.getI() return grasp_to_pick_matrix
def computeGraspToPickMatrix(self): pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion) translation = [pick_translation_distance, 0, 0] rotation_matrix[[0,1,2],3] = translation pick_to_grasp_matrix = np.mat(rotation_matrix) grasp_to_pick_matrix = pick_to_grasp_matrix.getI() return grasp_to_pick_matrix
def reset(self, msg): #save old physics properties turn off gravity physics_properties = self.get_physics_properties() old_gravity = physics_properties.gravity.z physics_properties.gravity.z = 0 self.set_physics_properties(physics_properties.time_step, physics_properties.max_update_rate, physics_properties.gravity, physics_properties.ode_config) #teleport robot self.set_model_state(ModelState(self.model_name, Pose(Point(0, 0, 2), Quaternion()), Twist(), self.world_frame)) #set initial positions, wait for the joints to converge self.fieldCommandPub.publish(msg) robot_pose_sub = rospy.Subscriber("/gazebo/joint_states", JointState, self.processRobotPose) prev_joint_position = msg.position while not rospy.is_shutdown(): time.sleep(0.1) diff = np.mean([(math.fabs(msg.position[i] - self.joints[i]) + math.fabs(msg.position[i] - prev_joint_position[i])) for i in xrange(len(msg.setThisJoint)) if msg.setThisJoint[i]]) print "Diff: ", diff if diff < 0.025: break robot_pose_sub.unregister() #rotate the robot, so that feet will get on the ground newAtlasPose = self.get_model_state(self.model_name, self.world_frame).pose footOrientation = self.get_link_state(self.link_names[0], self.world_frame).link_state.pose.orientation a = [newAtlasPose.orientation.x, newAtlasPose.orientation.y, newAtlasPose.orientation.z, newAtlasPose.orientation.w] b = [footOrientation.x, footOrientation.y, footOrientation.z, footOrientation.w] res = t.quaternion_multiply(a, t.quaternion_inverse(b)) newAtlasPose.orientation = Quaternion(res[0], res[1], res[2], res[3]) self.set_model_state(ModelState(self.model_name, newAtlasPose, Twist(), self.world_frame)) #extract foot positions and offset them by foot_z_offset newAtlasPose = self.get_model_state(self.model_name, self.world_frame).pose footPosition = self.get_link_state(self.link_names[0], self.world_frame).link_state.pose.position newAtlasPose.position.z = newAtlasPose.position.z - footPosition.z + self.foot_z_offsets[0] + 0.01 #teleport the robot down self.set_model_state(ModelState(self.model_name, newAtlasPose, Twist(), self.world_frame)) #turn on gravity physics_properties.gravity.z = old_gravity self.set_physics_properties(physics_properties.time_step, physics_properties.max_update_rate, physics_properties.gravity, physics_properties.ode_config) self.resetDonePub.publish(Empty()) self.recordPerformance()
def pose_err(self, ps1, ps2): p1 = [ps1.pose.position.x, ps1.pose.position.y, ps1.pose.position.z] p2 = [ps2.pose.position.x, ps2.pose.position.y, ps2.pose.position.z] d_err = np.linalg.norm(np.subtract(p1,p2)) q1 = [ps1.pose.orientation.x, ps1.pose.orientation.y, ps1.pose.orientation.z, ps1.pose.orientation.w] q2 = [ps2.pose.orientation.x, ps2.pose.orientation.y, ps2.pose.orientation.z, ps2.pose.orientation.w] q1_inv = tft.quaternion_inverse(q1) q_err = tft.quaternion_multiply(q2, q1_inv) euler_angles = tft.euler_from_quaternion(q_err) theta_err = np.linalg.norm(euler_angles) return d_err, theta_err
def position_control(user_data, self): if self.inside_workspace(self.master_pos): self.command_pos = self.slave_synch_pos + self.master_pos / self.position_ratio # TODO: Rotations are driving me crazy! relative_rot = tr.quaternion_multiply(self.master_synch_rot, tr.quaternion_inverse(self.master_rot)) self.command_rot = tr.quaternion_multiply(self.slave_synch_rot, relative_rot) #~ self.command_rot = np.array(self.master_rot) return 'stay' else: self.command_pos = np.array(self.slave_pos) self.command_rot = np.array(self.slave_rot) self.vib_start_time = rospy.get_time() self.loginfo('State machine transitioning: POSITION_CONTROL:leave-->VIBRATORY_PHASE') return 'leave'
def test_triad(): q = transformations.random_quaternion() a = np.random.standard_normal(3) b = np.random.standard_normal(3) m = transformations.quaternion_matrix(q)[:3, :3] q_ = triad((m.dot(a), m.dot(b)), (a, b)) assert np.linalg.norm(quat_to_rotvec( transformations.quaternion_multiply( q, transformations.quaternion_inverse(q_), ) )) < 1e-6
def rate_control(user_data, self): if not self.inside_workspace(self.master_pos): # Send the force feedback to the master self.force_feedback = (self.k_rate * self.master_pos + self.b_rate * self.master_vel) * -1.0 # Send the rate command to the slave distance = sqrt(np.sum((self.master_pos - self.rate_pivot) ** 2)) / self.position_ratio self.command_pos += (self.rate_gain * distance * self.normalize_vector(self.master_pos)) / self.position_ratio relative_rot = tr.quaternion_multiply(self.master_synch_rot, tr.quaternion_inverse(self.master_rot)) self.command_rot = tr.quaternion_multiply(self.slave_synch_rot, relative_rot) return 'stay' else: self.command_pos = np.array(self.slave_pos) self.command_rot = np.array(self.slave_rot) self.force_feedback = np.zeros(3) self.loginfo('State machine transitioning: RATE_CONTROL:leave-->GO_TO_CENTER') return 'leave'
def convert_pose_inverse_transform(pose): """ Helper method to invert a transform (this is built into the tf C++ classes, but ommitted from Python) """ translation = np.zeros((4,1)) translation[0] = -pose.position.x translation[1] = -pose.position.y translation[2] = -pose.position.z translation[3] = 1.0 rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) rotation_inverse = quaternion_inverse(rotation) rot_mat = quaternion_matrix(rotation_inverse) transformed_translation = rot_mat.dot(translation) translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2]) rotation = rotation_inverse return (translation, rotation)
def cross_correlation_mat(self, est_mean, est_sigmas, meas_mean, meas_sigmas): cross_correlation_mat = zeros((est_mean.shape[0], meas_mean.shape[0])) est_diff = zeros(est_mean.shape) mean_quat = SF9DOF_UKF.recover_quat(est_mean) inverse_mean_quat = tf_math.quaternion_inverse(mean_quat.flatten()) for i in range(0,self.n*2+1): #Calculate the error for the quaternion sig_quat = SF9DOF_UKF.recover_quat(est_sigmas[i]) error_quat = tf_math.quaternion_multiply(sig_quat, \ inverse_mean_quat) est_diff[0:3,0] = error_quat[0:3,0] #Calculate the error for the rest of the state est_diff[3:,0] = est_sigmas[i][3:,0] - est_mean[3:,0] meas_diff = meas_sigmas[i] - meas_mean prod = dot(est_diff, meas_diff.T) term = self.weight_covariance[i] * prod cross_correlation_mat += term return cross_correlation_mat
def estimate_covariance(self, est_mean, transformed_sigmas): est_covariance = zeros(self.kalman_covariance.shape) diff = zeros(est_mean.shape) #Calculate the inverse mean quat mean_quat = SF9DOF_UKF.recover_quat(est_mean) inverse_mean_quat = tf_math.quaternion_inverse(mean_quat.flatten()) for i in range(0,self.n*2+1): #Calculate the error for the quaternion sig_quat = SF9DOF_UKF.recover_quat(transformed_sigmas[i]) error_quat = tf_math.quaternion_multiply(sig_quat, \ inverse_mean_quat) diff[0:3,0] = error_quat[0:3,0] #Calculate the error for the rest of the state diff[3:,0] = transformed_sigmas[i][3:,0] - est_mean[3:,0] prod = dot(diff, diff.T) term = self.weight_covariance[i] * prod est_covariance += term return est_covariance
def get_pose_error(self, goal_pose, curr_pose): dx = curr_pose.pose.position.x - goal_pose.pose.position.x dy = curr_pose.pose.position.y - goal_pose.pose.position.y dz = curr_pose.pose.position.z - goal_pose.pose.position.z pos_err = np.linalg.norm((dx, dy, dz)) goal_q = (goal_pose.pose.orientation.x, goal_pose.pose.orientation.y, goal_pose.pose.orientation.z, goal_pose.pose.orientation.w) curr_q = (curr_pose.pose.orientation.x, curr_pose.pose.orientation.y, curr_pose.pose.orientation.z, curr_pose.pose.orientation.w) #goal_q = curr_q * delta_q -> delta_q = curr_q^-1 * goal_q cq_inv = tft.quaternion_inverse(curr_q) dq = tft.quaternion_multiply(cq_inv, goal_q) #theta = 2*arctan2( ||X||, w) q_vec_norm = np.linalg.norm(dq[:3]) rot_err = 2*np.arctan2(q_vec_norm, dq[3]) return pos_err, rot_err
def euler_from_matrix(parent_quat, child_quat): q_relative = tr.quaternion_multiply(tr.quaternion_inverse(parent_quat), child_quat) R = tr.quaternion_matrix(q_relative) if not nearly_equal(abs(R[2][0]), 1.0): p1 = -math.asin(R[2][0]) p2 = math.pi - p1 r1 = math.atan2(R[2][1]/math.cos(p1), R[2][2]/math.cos(p1)) r2 = math.atan2(R[2][1]/math.cos(p2), R[2][2]/math.cos(p2)) y1 = math.atan2(R[1][0]/math.cos(p1), R[1][0]/math.cos(p1)) y2 = math.atan2(R[1][0]/math.cos(p2), R[1][0]/math.cos(p2)) r = r2 p = p2 y = y2 else: y = 0 if nearly_equal(R[2][0], -1.0): p = math.pi/2 r = y + math.atan2(R[0][1], R[0][2]) else: p = -math.pi/2 r = -y + math.atan2(-R[0][1], -R[0][2]) return [r,p,y]
def update_errors(self): if not self.odom_is_init: self._logger.warning('Odometry topic has not been update yet') return self._update_reference() # Calculate error in the BODY frame self._update_time_step() # Rotation matrix from INERTIAL to BODY frame rotItoB = self._vehicle_model.rotItoB rotBtoI = self._vehicle_model.rotBtoI if self._dt > 0: # Update position error with respect to the the BODY frame pos = self._vehicle_model.pos vel = self._vehicle_model.vel quat = self._vehicle_model.quat self._errors['pos'] = np.dot( rotItoB, self._reference['pos'] - pos) # Update orientation error self._errors['rot'] = quaternion_multiply( quaternion_inverse(quat), self._reference['rot']) # Velocity error with respect to the the BODY frame self._errors['vel'] = np.hstack(( np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3], np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6])) if self._error_pub.get_num_connections() > 0: stamp = rospy.Time.now() msg = TrajectoryPoint() msg.header.stamp = stamp msg.header.frame_id = self._local_planner.inertial_frame_id # Publish pose error msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos'])) msg.pose.orientation = Quaternion(*self._errors['rot']) # Publish velocity errors in INERTIAL frame msg.velocity.linear = Vector3(*np.dot(rotBtoI, self._errors['vel'][0:3])) msg.velocity.angular = Vector3(*np.dot(rotBtoI, self._errors['vel'][3:6])) self._error_pub.publish(msg)
def qerror(self, q1, q2): """ Returns rotation vector representing the orientation error from quaternion q1 to q2. """ # Quaternion from q1 to q2 qdiff = trns.quaternion_multiply(q2, trns.quaternion_inverse(q1)) # Renormalize for accuracy qdiff = trns.unit_vector(qdiff) # If "amount of rotation" is negative, flip quaternion if qdiff[3] < 0: qdiff = -qdiff # Extract axis if not np.allclose(qdiff[:3], 0): axis = trns.unit_vector(qdiff[:3]) else: axis = np.array([0, 0, 0]) # Extract angle angle = 2 * np.arccos(qdiff[3]) # Return rotvec return angle * axis
def get_euler_angles(parent_quat, child_quat): q_relative = tr.quaternion_multiply(tr.quaternion_inverse(parent_quat), child_quat) qw = q_relative[3] qx = q_relative[0] qy = q_relative[1] qz = q_relative[2] test = qx*qy + qz*qw if (test > 0.499): roll = 2.0 * math.atan2(qx,qw) pitch = math.pi/2 yaw = 0 elif (test < -0.499): roll = -2.0 * math.atan2(qx, qw) pitch = -math.pi/2 yaw = 0 else: sqx = qx**2; sqy = qy**2; sqz = qz**2; roll = math.atan2(2*qy*qw-2*qx*qz , 1 - 2*sqy - 2*sqz) pitch = math.asin(2*test) yaw = math.atan2(2*qx*qw-2*qy*qz , 1 - 2*sqx - 2*sqz) return [roll, pitch, yaw]
def get_command(self, msg): ''' Publishes the wrench for this instant. (Note: this is called get_command because it used to be used for getting the actual thruster values, but now it is only being used for getting the wrench which is then later mapped elsewhere). ''' if self.p_ref is None: return # C3 is killed # Compute timestep from interval between this message's stamp and last's if self.last_odom is None: self.last_odom = msg else: self.timestep = (msg.header.stamp - self.last_odom.header.stamp).to_sec() self.last_odom = msg # ROS read-in position = msg.pose.pose.position orientation = msg.pose.pose.orientation lin_vel_body = msg.twist.twist.linear ang_vel_body = msg.twist.twist.angular # ROS unpack self.position = np.array([position.x, position.y]) self.orientation = np.array([orientation.x, orientation.y, orientation.z, orientation.w]) # Frame management quantities R = np.eye(3) R[:2, :2] = trns.quaternion_matrix(self.orientation)[:2, :2] y = trns.euler_from_quaternion(self.orientation)[2] # More ROS unpacking, converting body frame twist to world frame lin_vel and ang_vel self.lin_vel = R.dot(np.array([lin_vel_body.x, lin_vel_body.y, lin_vel_body.z]))[:2] self.ang_vel = R.dot(np.array([ang_vel_body.x, ang_vel_body.y, ang_vel_body.z]))[2] # Convert body PD gains to world frame kp = R.dot(self.kp_body).dot(R.T) kd = R.dot(self.kd_body).dot(R.T) # Compute error components (reference - true) p_err = self.p_ref - self.position y_err = trns.euler_from_quaternion(trns.quaternion_multiply(self.q_ref, trns.quaternion_inverse(self.orientation)))[2] v_err = self.v_ref - self.lin_vel w_err = self.w_ref - self.ang_vel # Combine error components into error vectors err = np.concatenate((p_err, [y_err])) errdot = np.concatenate((v_err, [w_err])) # Compute "anticipation" feedforward based on the boat's inertia inertial_feedforward = np.concatenate((self.a_ref, [self.aa_ref])) * [self.mass_ref, self.mass_ref, self.inertia_ref] # Compute the "learning" matrix drag_regressor = np.array([[ self.lin_vel[0]*np.cos(y)**2 + self.lin_vel[1]*np.sin(y)*np.cos(y), self.lin_vel[0]/2 - (self.lin_vel[0]*np.cos(2*y))/2 - (self.lin_vel[1]*np.sin(2*y))/2, -self.ang_vel*np.sin(y), -self.ang_vel*np.cos(y), 0], [self.lin_vel[1]/2 - (self.lin_vel[1]*np.cos(2*y))/2 + (self.lin_vel[0]*np.sin(2*y))/2, self.lin_vel[1]*np.cos(y)**2 - self.lin_vel[0]*np.cos(y)*np.sin(y), self.ang_vel*np.cos(y), -self.ang_vel*np.sin(y), 0], [ 0, 0, self.lin_vel[1]*np.cos(y) - self.lin_vel[0]*np.sin(y), - self.lin_vel[0]*np.cos(y) - self.lin_vel[1]*np.sin(y), self.ang_vel]]) # wrench = PD + feedforward + I + adaptation if self.only_PD: wrench = (kp.dot(err)) + (kd.dot(errdot)) self.drag_effort = [0, 0, 0] self.dist_est = [0, 0, 0] else: self.drag_effort = np.clip(drag_regressor.dot(self.drag_est), -self.drag_limit, self.drag_limit) wrench = (kp.dot(err)) + (kd.dot(errdot)) + inertial_feedforward + self.dist_est + self.drag_effort # Update disturbance estimate, drag estimates if self.learn and (npl.norm(p_err) < self.learning_radius): self.dist_est = np.clip(self.dist_est + (self.ki * err * self.timestep), -self.dist_limit, self.dist_limit) self.drag_est = self.drag_est + (self.kg * (drag_regressor.T.dot(err + errdot)) * self.timestep) # Update model reference for the next call if not self.use_external_tgen: self.increment_reference() # convert wrench to body frame wrench_body = R.T.dot(wrench) # NOT NEEDED SINCE WE ARE USING A DIFFERENT NODE FOR ACTUAL THRUSTER MAPPING # # Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts # B = np.concatenate((R.dot(self.thruster_directions.T), R.dot(self.lever_arms.T))) # B_3dof = np.concatenate((B[:2, :], [B[5, :]])) # command = self.thruster_mapper(wrench, B_3dof) # Give wrench to ROS wrench_msg = WrenchStamped() wrench_msg.header.frame_id = "/base_link" wrench_msg.wrench.force.x = wrench_body[0] wrench_msg.wrench.force.y = wrench_body[1] wrench_msg.wrench.torque.z = wrench_body[2] self.wrench_pub.publish(wrench_msg) # Publish reference pose for examination self.pose_ref_pub.publish(PoseStamped( header=Header( frame_id='/enu', stamp=msg.header.stamp, ), pose=Pose( position=Point(self.p_ref[0], self.p_ref[1], 0), orientation=Quaternion(*self.q_ref), ), )) # Publish adaptation (Y*theta) for plotting adaptation_msg = WrenchStamped() adaptation_msg.header.frame_id = "/base_link" adaptation_msg.wrench.force.x = (self.dist_est + self.drag_effort)[0] adaptation_msg.wrench.force.y = (self.dist_est + self.drag_effort)[1] adaptation_msg.wrench.torque.z = (self.dist_est + self.drag_effort)[2] self.adaptation_pub.publish(adaptation_msg)
def inverse(self): return Transform( -self._q_mat.T.dot(self._p), transformations.quaternion_inverse(self._q), )
def _body_y_position(pos, traj_start): R_nav2body = transformations.quaternion_matrix( transformations.quaternion_inverse(traj_start.orientation))[:3, :3] return R_nav2body.dot(pos)[1]
def rotation_from(self,*args,**kwargs): """Get the rotation (as tb_angles) which, when applied to the input rotation by right-multiplication, produces this rotation""" other = tb_angles(*args,**kwargs) out = tb_angles(tft.quaternion_multiply(tft.quaternion_inverse(other.quaternion), self.quaternion)) return out
def fill_points(tfl): try: global user frame = "/openni_link" #"camera_link" allFramesString = tfl.getFrameStrings() onlyUsers = set([line for line in allFramesString if 'right_elbow_' in line]) n = len('right_elbow_') userIDs = [el[n:] for el in onlyUsers] user = '' if len(userIDs) > 0: mostRecentUID = userIDs[0] mostRecentTime = tfl.getLatestCommonTime(frame, 'right_elbow_' + mostRecentUID).to_sec() for uid in userIDs: compTime = tfl.getLatestCommonTime(frame, 'right_elbow_' + uid).to_sec() #rospy.loginfo("Diff time " + str(rospy.get_rostime().to_sec() - compTime)) if compTime >= mostRecentTime and rospy.get_rostime().to_sec() - compTime < 5: user = uid mostRecentTime = compTime global left_arm_origin global right_arm_origin global head_origin global head_point global left_arm_point global right_arm_point global left_foot global right_foot (to_left_elbow,_) = tfl.lookupTransform(frame,"/left_elbow_" + user, rospy.Time(0)) (to_right_elbow,_) = tfl.lookupTransform(frame,"/right_elbow_" + user, rospy.Time(0)) (to_left_hand,_) = tfl.lookupTransform(frame,"/left_hand_" + user, rospy.Time(0)) (to_right_hand,_) = tfl.lookupTransform(frame,"/right_hand_" + user, rospy.Time(0)) (right_foot,_) = tfl.lookupTransform(frame, "/right_foot_" + user, rospy.Time(0)) (left_foot,_) = tfl.lookupTransform(frame, "/left_foot_" + user, rospy.Time(0)) (to_head,head_rot) = tfl.lookupTransform(frame,"/head_" + user, rospy.Time(0)) left_arm_origin = to_left_hand left_arm_point = add_vec(to_left_hand, sub_vec(to_left_hand, to_left_elbow)) right_arm_origin = to_right_hand right_arm_point = add_vec(to_right_hand, sub_vec(to_right_hand, to_right_elbow)) head_origin = to_head head_temp = dot((0.0,0.0,-1.0,1.0), quaternion_matrix(quaternion_inverse(head_rot))) head_point = (head_temp[0] + to_head[0], head_temp[1] + to_head[1], head_temp[2] + to_head[2]) #visualization for testing (verify head vector) # marker = Marker() # marker.header.frame_id = "camera_link" # marker.header.stamp = rospy.Time(0) # marker.type = marker.POINTS # marker.action = marker.ADD # marker.scale.x = 0.2 # marker.scale.y = 0.2 # marker.scale.z = 0.2 # marker.color.a = 1.0 # p1 = Point(right_arm_origin[0],right_arm_origin[1],right_arm_origin[2]) # p2 = Point(right_arm_point[0],right_arm_point[1],right_arm_point[2]) # p3 = Point(left_arm_origin[0],left_arm_origin[1],left_arm_origin[2]) # p4 = Point(left_arm_point[0],left_arm_point[1],left_arm_point[2]) # p5 = Point(head_origin[0],head_origin[1],head_origin[2]) # p6 = Point(head_point[0],head_point[1],head_point[2]) # marker.points += [p1, p2, p3, p4, p5, p6] # pub.publish(marker) return True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): left_arm_point = None right_arm_origin = None left_arm_origin = None right_arm_point = None head_point = None head_origin = None left_foot = None right_foot = None return False
def odometry_callback(self, msg): """Handle odometry callback: The actual control loop.""" # Update local planner's vehicle position and orientation pos = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z] quat = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] self.local_planner.update_vehicle_pose(pos, quat) # Compute the desired position t = rospy.Time.now().to_sec() des = self.local_planner.interpolate(t) # Publish the reference ref_msg = TrajectoryPoint() ref_msg.header.stamp = rospy.Time.now() ref_msg.header.frame_id = self.local_planner.inertial_frame_id ref_msg.pose.position = geometry_msgs.Vector3(*des.p) ref_msg.pose.orientation = geometry_msgs.Quaternion(*des.q) ref_msg.velocity.linear = geometry_msgs.Vector3(*des.vel[0:3]) ref_msg.velocity.angular = geometry_msgs.Vector3(*des.vel[3::]) self.reference_pub.publish(ref_msg) p = self.vector_to_np(msg.pose.pose.position) forward_vel = self.vector_to_np(msg.twist.twist.linear) ref_vel = des.vel[0:3] q = self.quaternion_to_np(msg.pose.pose.orientation) rpy = trans.euler_from_quaternion(q, axes='sxyz') # Compute tracking errors wrt world frame: e_p = des.p - p abs_pos_error = np.linalg.norm(e_p) abs_vel_error = np.linalg.norm(ref_vel - forward_vel) # Generate error message error_msg = TrajectoryPoint() error_msg.header.stamp = rospy.Time.now() error_msg.header.frame_id = self.local_planner.inertial_frame_id error_msg.pose.position = geometry_msgs.Vector3(*e_p) error_msg.pose.orientation = geometry_msgs.Quaternion( *trans.quaternion_multiply(trans.quaternion_inverse(q), des.q)) error_msg.velocity.linear = geometry_msgs.Vector3(*(des.vel[0:3] - self.vector_to_np(msg.twist.twist.linear))) error_msg.velocity.angular = geometry_msgs.Vector3(*(des.vel[3::] - self.vector_to_np(msg.twist.twist.angular))) # Based on position tracking error: Compute desired orientation pitch_des = -math.atan2(e_p[2], np.linalg.norm(e_p[0:2])) # Limit desired pitch angle: pitch_des = max(-self.desired_pitch_limit, min(pitch_des, self.desired_pitch_limit)) yaw_des = math.atan2(e_p[1], e_p[0]) yaw_err = self.unwrap_angle(yaw_des - rpy[2]) # Limit yaw effort yaw_err = min(self.yaw_error_limit, max(-self.yaw_error_limit, yaw_err)) # Roll: P controller to keep roll == 0 roll_control = self.p_roll * rpy[0] # Pitch: P controller to reach desired pitch angle pitch_control = self.p_pitch * self.unwrap_angle(pitch_des - rpy[1]) + self.d_pitch * (des.vel[4] - msg.twist.twist.angular.y) # Yaw: P controller to reach desired yaw angle yaw_control = self.p_yaw * yaw_err + self.d_yaw * (des.vel[5] - msg.twist.twist.angular.z) # Limit thrust thrust = min(self.max_thrust, self.p_gain_thrust * np.linalg.norm(abs_pos_error) + self.d_gain_thrust * abs_vel_error) thrust = max(self.min_thrust, thrust) rpy = np.array([roll_control, pitch_control, yaw_control]) # In case the world_ned reference frame is used, the convert it back # to the ENU convention to generate the reference fin angles rtf = deepcopy(self.rpy_to_fins) if self.local_planner.inertial_frame_id == 'world_ned': rtf[:, 1] *= -1 rtf[:, 2] *= -1 # Transform orientation command into fin angle set points fins = rtf.dot(rpy) # Check for saturation max_angle = max(np.abs(fins)) if max_angle >= self.max_fin_angle: fins = fins * self.max_fin_angle / max_angle thrust_force = self.thruster.tam_column * thrust self.thruster.publish_command(thrust_force[0]) cmd = FloatStamped() for i in range(self.n_fins): cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = '%s/fin%d' % (self.namespace, i) cmd.data = min(fins[i], self.max_fin_angle) cmd.data = max(cmd.data, -self.max_fin_angle) self.pub_cmd[i].publish(cmd) self.error_pub.publish(error_msg)