def _motion_regression_6d(self, pnts, qt, t):
        """
        Compute translational and rotational velocities and accelerations in
        the inertial frame at the target time t.

        [1] Sittel, Florian, Joerg Mueller, and Wolfram Burgard. Computing
            velocities and accelerations from a pose time sequence in
            three-dimensional space. Technical Report 272, University of
            Freiburg, Department of Computer Science, 2013.
        """

        lin_vel = np.zeros(3)
        lin_acc = np.zeros(3)

        q_d = np.zeros(4)
        q_dd = np.zeros(4)

        for i in range(3):
            v, a = self._motion_regression_1d(
                [(pnt['pos'][i], pnt['t']) for pnt in pnts], t)
            lin_vel[i] = v
            lin_acc[i] = a

        for i in range(4):
            v, a = self._motion_regression_1d(
                [(pnt['rot'][i], pnt['t']) for pnt in pnts], t)
            q_d[i] = v
            q_dd[i] = a

        # Keeping all velocities and accelerations in the inertial frame
        ang_vel = 2 * quaternion_multiply(q_d, quaternion_conjugate(qt))
        ang_acc = 2 * quaternion_multiply(q_dd, quaternion_conjugate(qt))

        return np.hstack((lin_vel, ang_vel[0:3])), np.hstack((lin_acc, ang_acc[0:3]))
Пример #2
0
 def generate_sigma_points(self, mean, covariance):
     sigmas = []
     sigmas.append(mean)
     if not check_symmetry(covariance):
         pdb.set_trace()
     temp = pos_sem_def_sqrt(covariance)
     if any(isnan(temp)):
         rospy.logerr("Sqrt matrix contained a NaN. Matrix was %s",
                 covariance)
     temp = temp * sqrt(self.n + self.kf_lambda)
     for i in range(0,self.n):
         #Must use columns in order to get the write thing out of the
         #Cholesky decomposition
         #(http://en.wikipedia.org/wiki/Cholesky_decomposition#Kalman_filters)
         column = temp[:,i].reshape((self.n,1))
         #Build the noise quaternion
         noise_vec = column[0:3,0]
         noise_quat = SF9DOF_UKF.build_noise_quat(noise_vec)
         original_quat = SF9DOF_UKF.recover_quat(mean)
         #Do the additive sample
         perturbed_quat = tf_math.quaternion_multiply(noise_quat,original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean + column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
         #Do the subtractive sample
         conj_noise_quat = tf_math.quaternion_conjugate(noise_quat)
         perturbed_quat = tf_math.quaternion_multiply(conj_noise_quat, original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean - column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
     return sigmas
Пример #3
0
 def generate_sigma_points(self, mean, covariance):
     sigmas = []
     sigmas.append(mean)
     temp = numpy.linalg.cholesky(covariance)
     temp = temp * sqrt(self.n + self.kf_lambda)
     for i in range(0,self.n):
         #Must use columns in order to get the write thing out of the
         #Cholesky decomposition
         #(http://en.wikipedia.org/wiki/Cholesky_decomposition#Kalman_filters)
         column = temp[:,i].reshape((self.n,1))
         #Build the noise quaternion
         noise_vec = column[0:3,0]
         noise_quat = SF9DOF_UKF.build_noise_quat(noise_vec)
         original_quat = SF9DOF_UKF.recover_quat(mean)
         #Do the additive sample
         perturbed_quat = tf_math.quaternion_multiply(noise_quat,original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean + column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
         #Do the subtractive sample
         conj_noise_quat = tf_math.quaternion_conjugate(noise_quat)
         perturbed_quat = tf_math.quaternion_multiply(conj_noise_quat, original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean - column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
     return sigmas
Пример #4
0
    def qv_mult(self):
        # rotate vector v1 by quaternion q1
        q1 = self.orientation_imu
        q2 = list(
            [self.gps_robot[0], self.gps_robot[1], self.gps_robot[2], 0.0])

        return quaternion_multiply(quaternion_multiply(q1, q2),
                                   quaternion_conjugate(q1))
Пример #5
0
def rotate(v, q):
    """
    Rotate vector v according to quaternion q
    """
    q2 = np.r_[v[0:3], 0.0]
    return transformations.quaternion_multiply(
        transformations.quaternion_multiply(q, q2),
        transformations.quaternion_conjugate(q))[0:3]
Пример #6
0
def quat_vec_mult(q1, v1):
    """Rotate vector v1 by quaternion q1, and return the resulting vector.
    From https://answers.ros.org/question/196149/how-to-rotate-vector-by-quaternion-in-python/
    """
    q2 = list(v1)
    q2.append(0.0)
    return quaternion_multiply(quaternion_multiply(q1, q2),
                               quaternion_conjugate(q1))[:3]
Пример #7
0
def rotate(v, q, inverse=False):
    """rotate vector v from world to body frame (with quaternion q)"""
    cq = quaternion_conjugate(q)
    if inverse:
        z = quaternion_multiply(q, v)
        return quaternion_multiply(z, cq)
    else:
        z = quaternion_multiply(v, q)
        return quaternion_multiply(cq, z)
Пример #8
0
    def odometry_callback(self, msg):
        """Handle updated measured velocity callback."""
        if not bool(self.config):
            return

        p = msg.pose.pose.position
        q = msg.pose.pose.orientation
        p = numpy.array([p.x, p.y, p.z])
        q = numpy.array([q.x, q.y, q.z, q.w])

        if not self.initialized:
            # If this is the first callback: Store and hold latest pose.
            self.pos_des  = p
            self.quat_des = q
            self.initialized = True

        # Compute control output:
        t = msg.header.stamp.to_sec()

        # Position error
        e_pos_world = self.pos_des - p
        e_pos_body = trans.quaternion_matrix(q).transpose()[0:3,0:3].dot(e_pos_world)

        # Error quaternion wrt body frame
        e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q), self.quat_des)

        if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
            # special case if we are far away from goal:
            # ignore desired heading, look towards goal position
            heading = math.atan2(e_pos_world[1],e_pos_world[0])
            quat_des = numpy.array([0, 0, math.sin(0.5*heading), math.cos(0.5*heading)])
            e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q), quat_des)

        # Error angles
        e_rot = numpy.array(trans.euler_from_quaternion(e_rot_quat))

        v_linear = self.pid_pos.regulate(e_pos_body, t)
        v_angular = self.pid_rot.regulate(e_rot, t)

        # Convert and publish vel. command:
        cmd_vel = geometry_msgs.Twist()
        cmd_vel.linear = geometry_msgs.Vector3(*v_linear)
        cmd_vel.angular = geometry_msgs.Vector3(*v_angular)
        self.pub_cmd_vel.publish(cmd_vel)
Пример #9
0
def _yaw_angle_between(qa, qb):
    yaw =  transformations.euler_from_quaternion(
        transformations.quaternion_multiply(
            qa, transformations.quaternion_conjugate(qb))
        )[2]
    if yaw > math.pi/2:
        yaw -= math.pi
    elif yaw < -math.pi/2:
        yaw += math.pi
    return yaw
Пример #10
0
def qv_mult(q1, v1):
    '''
    Rotates a vector by a unit quaternion
    @param q1 - the quaternion to rotate the vector by. Tuple (x, y, z, w)
    @param v1 - the vector to be rotated. Tuple (x, y, z)
    @return the rotated vector
    '''
    q2 = v1 + (0.0, )
    return quaternion_multiply(quaternion_multiply(q1, q2),
                               quaternion_conjugate(q1))[:3]
Пример #11
0
def trans_quat_to_transform((trans, quat)):
  if trans is None or quat is None:
    return None
  # I think Jesse is confused with what is actually stored in the file
  # because his quaternion_to_dcm seems to give the conjugate of the right quaternion
  m = transformations.quaternion_matrix(transformations.quaternion_conjugate(quat))
  m[0,3] = -trans[0]
  m[1,3] = -trans[1]
  m[2,3] = -trans[2]
  return m
Пример #12
0
def qv_mult(quaternion, vector):
    """
    Transforms a vector by a quaternion
    :param quaternion: Quaternion
    :type quaternion: list
    :param vector: vector
    :type vector: list
    :return: transformed vector
    :type: list
    """
    q = quaternion
    v = [vector[0], vector[1], vector[2], 0]
    return quaternion_multiply(quaternion_multiply(q, v), quaternion_conjugate(q))[:-1]
Пример #13
0
    def _motion_regression_6d(self, pnts, qt, t):
        """
        Compute translational and rotational velocities and accelerations in
        the inertial frame at the target time t.

        [1] Sittel, Florian, Joerg Mueller, and Wolfram Burgard. Computing
            velocities and accelerations from a pose time sequence in
            three-dimensional space. Technical Report 272, University of
            Freiburg, Department of Computer Science, 2013.
        """

        lin_vel = np.zeros(3)
        ang_vel = np.zeros(4)
        lin_acc = np.zeros(3)
        ang_acc = np.zeros(4)

        q_d = np.zeros(4)
        q_dd = np.zeros(4)

        for i in range(3):
            v, a = self._motion_regression_1d([(pnt['pos'][i], pnt['t'])
                                               for pnt in pnts], t)
            lin_vel[i] = v
            lin_acc[i] = a

        for i in range(4):
            v, a = self._motion_regression_1d([(pnt['rot'][i], pnt['t'])
                                               for pnt in pnts], t)
            q_d[i] = v
            q_dd[i] = a

        # Keeping all velocities and accelerations in the inertial frame
        ang_vel = 2 * quaternion_multiply(q_d, quaternion_conjugate(qt))
        ang_acc = 2 * quaternion_multiply(q_dd, quaternion_conjugate(qt))

        return np.hstack((lin_vel, ang_vel[0:3])), np.hstack(
            (lin_acc, ang_acc[0:3]))
Пример #14
0
    def listen_imu_quaternion(self, imu):
        """Doesn't work b/c desired yaw is too far from possible yaw"""
        current_orientation = np.array([imu.orientation.x,
                                        imu.orientation.y,
                                        imu.orientation.z,
                                        imu.orientation.w])
        # current_angular_speed = np.array([imu.angular_velocity.x,
        #                                   imu.angular_velocity.y,
        #                                   imu.angular_velocity.z])
        diff_orientation = transformations.quaternion_multiply(
            self.desired_orientation,
            # Unit quaternion => inverse = conjugate / norm = congugate
            transformations.quaternion_conjugate(current_orientation))

        assert np.allclose(
            transformations.quaternion_multiply(diff_orientation,
                                                current_orientation),
            self.desired_orientation)

        # diff_r, diff_p, diff_y = transformations.euler_from_quaternion(
        #     diff_orientation)
        # rospy.loginfo("Orientation error (quaternion): {}".format(diff_orientation))
        # rospy.loginfo(
        #     "Orientation error (deg): {}".format(
        #         np.rad2deg([diff_r, diff_p, diff_y]))
        # )
        # out = self.pitch_controller(diff_p)

        corrected_orientation = transformations.quaternion_multiply(
            quaternion_power(diff_orientation, 1.5),
            self.desired_orientation)
        rospy.loginfo(
            "Desired orientation (deg): {}".format(
                np.rad2deg(transformations.euler_from_quaternion(
                    self.desired_orientation))))
        rospy.loginfo(
            "Corrected orientation (deg): {}".format(
                np.rad2deg(transformations.euler_from_quaternion(
                    corrected_orientation))))

        rospy.loginfo("Desired position: {}".format(
            self.desired_position))

        setpoint_pose = Pose(self.desired_position, corrected_orientation)

        servo_angles = self.platform.ik(setpoint_pose)
        rospy.logdebug(
            "Servo angles (deg): {}".format(np.rad2deg(servo_angles)))
        self.publish_servo_angles(servo_angles)
Пример #15
0
 def measurement_update(current_state, dt, measurement):
     predicted_measurement = zeros(measurement.shape)
     orientation = SF9DOF_UKF.recover_quat(current_state)
     #Calculate predicted magnetometer readings
     h_vec = zeros((4,1))
     h_vec[0:3,0] = current_state[12:15,0]
     h_vec[3,0] = 0.
     temp = tf_math.quaternion_multiply(orientation, h_vec)
     result = tf_math.quaternion_multiply(temp, \
             tf_math.quaternion_conjugate(orientation))
     predicted_measurement[0:3,0] = result[0:3,0]
     #Calculate the predicted gyro readings
     temp_gyro = current_state[3:6,0] + current_state[9:12,0]
     predicted_measurement[3:6,0] = temp_gyro
     #Calculate the predicted accelerometer readings
     g_vec = zeros((4,1))
     g_vec[0:3,0] = current_state[15:18,0]
     g_vec[3,0] = 0.
     temp = tf_math.quaternion_multiply(orientation, g_vec)
     result = tf_math.quaternion_multiply(temp, \
             tf_math.quaternion_conjugate(orientation))
     temp_accel = current_state[6:9,0] + result[0:3,0]
     predicted_measurement[6:9,0] = temp_accel
     return predicted_measurement
Пример #16
0
def rightMultiplyPose(pose0, pose1):
    """
    Multiply pose0 with pose1 assuming pose0 is 7xn matrix and pose1 is 7
    vector.
    Returns: [7xn] pose matrix by multiplying pose0 x pose1
    """
    p0 = pose0[:3, :]
    q0 = pose0[3:, :]
    q0_c = tf.quaternion_conjugate(q0)
    p1 = pose1[:3]
    q1 = pose1[3:]
    p1_exp = np.hstack((p1, 0))
    p1_rot = tf.quaternion_multiply(q0, tf.quaternion_multiply(p1_exp, q0_c))
    q_out = tf.quaternion_multiply(q0, q1)
    p_out = p0 + p1_rot[:3, :]
    return np.vstack((p_out, q_out))
Пример #17
0
    def moveToMarker(self):  
        try:
            now=rospy.Time.now()
            self.listener.waitForTransform('/map', self.marker, now, rospy.Duration(10))
            trans, rot=self.listener.lookupTransform('/map', self.marker, now)  
            print "Have a TF frame"            
            self.move_base_client.wait_for_server()
            g = MoveBaseGoal()
            g.target_pose.header.frame_id = '/map'
            g.target_pose.header.stamp=rospy.Time.now()	
            #Add offset so that the PR2 sits right in front of the tag instead of running it over or getting the navigation stack stuck
            x=trans[0]-0.50                                  
            y=trans[1]
            z=trans[2]
            #Switch rotation around to be in the opposite direction        
            q1=np.array([rot[0], rot[1], rot[2], rot[3]])
            q1_con=tft.quaternion_conjugate(q1) #get the conjugate
            rot_y_axis=np.array([0.0*sin(pi/4), 1.0*sin(pi/4), 0.0*sin(pi/4), cos(pi/4)])
            q3=tft.quaternion_multiply(q1_con, q1)
            q4=tft.quaternion_multiply(rot_y_axis, q3)
            q_fin=tft.quaternion_multiply(q1, q4)

            g.target_pose.pose.position.x = x          
            g.target_pose.pose.position.y = y	        
            g.target_pose.pose.position.z = z
            g.target_pose.pose.orientation.x = q_fin[0]
            g.target_pose.pose.orientation.y = q_fin[1]
            g.target_pose.pose.orientation.z = q_fin[2]
            g.target_pose.pose.orientation.w = q_fin[3]
            
            self.move_base_client.send_goal(g)
            print "Made it this far, sent goal"
                     
            
            success_to_box=self.move_base_client.wait_for_result()
    
            if success_to_box:
                print "Movement to box successful"
                self.at_box=True
               
            else:
                print "Movement to box failed"
                self.at_box=False   
                

        except(tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception):
            print "TF Exception, try again"
Пример #18
0
def main():
  parser = argparse.ArgumentParser(description='Calibrate intrinsic parameters.')
  parser.add_argument('-d', '--dock_cam', dest='dock_cam', action='store_true', help='Calibrate dock camera.')
  parser.add_argument('-v', '--verbose',  dest='verbose', action='store_true', help='Verbose mode.')
  parser.add_argument('robot', help='The name of the robot to configure (i.e., put p4d to edit p4d.config).')
  parser.add_argument('intrinsics_bag', help='The bag file with intrinsics calibration data.')
  parser.add_argument('extrinsics_bag', help='The bag file with extrinsics calibration data.')
  args = parser.parse_args()

  SCRIPT_DIR = os.path.dirname(os.path.realpath(__file__))
  target_yaml = SCRIPT_DIR + '/data/granite_april_tag.yaml'
  imu_yaml = SCRIPT_DIR + '/data/epson_g362p_imu.yaml'
  config_file = SCRIPT_DIR + '/../../astrobee/config/robots/' + args.robot + '.config'

  print 'Calibrating camera intrinsics...'
  intrinsics_yaml = intrinsics_calibrate.calibrate_camera(args.intrinsics_bag, target_yaml,
                          dock_cam=args.dock_cam, verbose=args.verbose, model='pinhole-equi')
  if intrinsics_yaml == None:
    print >> sys.stderr, 'Failed to calibrate intrinsics.'
    return

  print 'Calibrating camera extrinsics...'
  extrinsics_yaml = calibrate_extrinsics(args.extrinsics_bag, target_yaml,
                                         intrinsics_yaml, imu_yaml, verbose=args.verbose)
  if extrinsics_yaml == None:
    print >> sys.stderr, 'Failed to calibrate extrinsics.'
    return

  imu_to_camera = read_yaml_extrinsics(extrinsics_yaml)
  if imu_to_camera is None:
    print >> sys.stderr, 'Failed to read extrinsics from yaml file.'
    return
  
  body_to_imu = trans_quat_to_transform(lua_read_transform(config_file, 'imu_transform'))
  if body_to_imu is None:
    print >> sys.stderr, 'Failed to read imu transform.'
    return
  imu_to_body = np.linalg.inv(body_to_imu)
  body_to_camera = np.linalg.inv(imu_to_body.dot(np.linalg.inv(imu_to_camera)))
  q = transformations.quaternion_conjugate(transformations.quaternion_from_matrix(body_to_camera))
  t = imu_to_body.dot(np.linalg.inv(imu_to_camera)[0:4,3])
  print 'Translation: ', t
  print 'Rotation quaternion: ', q

  if lua_replace_transform(config_file, 'dock_cam_transform' if args.dock_cam else 'nav_cam_transform', (t, q)):
    print >> sys.stderr, 'Failed to update config file.'
    return
Пример #19
0
def leftMultiplyPose(pose0, pose1):
    """
    Multiply pose0 with pose1 assuming pose1 is 7xn matrix. pose0 is 7
    vector.
    Returns: [7xn] pose matrix by multiplying pose0 on the left
    """
    p0 = pose0[:3]
    q0 = pose0[3:]
    q0_c = tf.quaternion_conjugate(q0)
    p1 = pose1[:3, :]
    q1 = pose1[3:,:]
    N = p1.shape[1]
    #print pose1.shape
    p1_exp = np.vstack((p1, np.zeros(N)))
    p1_rot = tf.quaternion_multiply(q0, tf.quaternion_multiply(p1_exp, q0_c))
    q_out = tf.quaternion_multiply(q0, q1)
    p_out = np.expand_dims(p0, axis=1) + p1_rot[:3, :]
    return np.vstack((p_out, q_out))
Пример #20
0
def qv_mult(q1, v1):
    """
    Transforms a vector by a quaternion
    :param q1: Quaternion
    :type: Quaternion
    :param v1: vector
    :type: Point
    :return: transformed vector
    :type: Point
    """
    q = q1
    v = v1
    if type(q1) is Quaternion:
        q = (q1.x, q1.y, q1.z, q1.w)
    if type(v1) is Point:
        v = (v1.x, v1.y, v1.z, 0)

    r = quaternion_multiply(quaternion_multiply(q, v), quaternion_conjugate(q))
    return Point(r[0], r[1], r[2])
Пример #21
0
def qv_mult(q1, v1):
    """
    Transforms a vector by a quaternion
    :param q1: Quaternion
    :type: Quaternion
    :param v1: vector
    :type: Point
    :return: transformed vector
    :type: Point
    """
    q = q1
    v = v1
    if type(q1) is Quaternion:
        q = (q1.x, q1.y, q1.z, q1.w)
    if type(v1) is Point:
        v = (v1.x, v1.y, v1.z, 0)

    r = quaternion_multiply(quaternion_multiply(q, v), quaternion_conjugate(q))
    return Point(r[0], r[1], r[2])
    def get_data(self, t):
        try:
            sys.stdout = open(os.devnull, 'w')  # supress stdout
            (trans, rot) = self.t.lookupTransform(self.root_frame,
                                                  self.measured_frame,
                                                  rospy.Time(0))
        except tf.LookupException as e:
            sys.stdout = sys.__stdout__  # restore stdout
            #print "Exception in metric '%s' %s %s"%(self.name, type(e), e)
            return None
        except tf.ExtrapolationException as e:
            sys.stdout = sys.__stdout__  # restore stdout
            #print "Exception in metric '%s' %s %s"%(self.name, type(e), e)
            return None
        except tf.ConnectivityException as e:
            sys.stdout = sys.__stdout__  # restore stdout
            #print "Exception in metric '%s' %s %s"%(self.name, type(e), e)
            return None
        except Exception as e:
            sys.stdout = sys.__stdout__  # restore stdout
            print("general exeption in metric '%s':" % self.name, type(e), e)
            return None
        sys.stdout = sys.__stdout__  # restore stdout

        if self.time_old == None:
            self.trans_old = trans
            self.rot_old = rot
            self.time_old = t
            return None

        diff_q = transformations.quaternion_multiply(
            rot, transformations.quaternion_conjugate(self.rot_old))
        diff_e = transformations.euler_from_quaternion(diff_q)
        path_increment = sum([axis**2 for axis in diff_e])**0.5

        self.trans_old = trans
        self.rot_old = rot
        self.time_old = t

        return path_increment
Пример #23
0
    def odometry_callback(self, msg):
        """Handle updated measured velocity callback."""
        if not bool(self.config):
            return

        p = msg.pose.pose.position
        q = msg.pose.pose.orientation
        p = numpy.array([p.x, p.y, p.z])
        q = numpy.array([q.x, q.y, q.z, q.w])

        if not self.initialized:
            # If this is the first callback: Store and hold latest pose.
            self.pos_des = p
            self.quat_des = q
            self.initialized = True

        # Compute control output:
        t = msg.header.stamp.to_sec()

        # Position error wrt. body frame
        e_pos = trans.quaternion_matrix(q).transpose()[0:3,
                                                       0:3].dot(self.pos_des -
                                                                p)

        # Error quaternion wrt body frame
        e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q),
                                               self.quat_des)

        # Error angles
        e_rot = numpy.array(trans.euler_from_quaternion(e_rot_quat))

        v_linear = self.pid_pos.regulate(e_pos, t)
        v_angular = self.pid_rot.regulate(e_rot, t)

        # Convert and publish vel. command:
        cmd_vel = geometry_msgs.Twist()
        cmd_vel.linear = geometry_msgs.Vector3(*v_linear)
        cmd_vel.angular = geometry_msgs.Vector3(*v_angular)
        self.pub_cmd_vel.publish(cmd_vel)
Пример #24
0
def calcRelativeCoordinate(
    pose, point
):  # (geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
    worldCarR_q = np.array([
        pose.orientation.x, pose.orientation.y, pose.orientation.z,
        pose.orientation.w
    ])
    worldCarT_v = np.array(
        [pose.position.x, pose.position.y, pose.position.z, 1.0])

    carWorldR_m = ttf.quaternion_matrix(ttf.quaternion_conjugate(worldCarR_q))
    carWorldT_v = -np.dot(carWorldR_m, worldCarT_v)
    carWorld_m = carWorldR_m
    carWorld_m[:3, 3] = carWorldT_v[:3]
    point_world = np.array([point.x, point.y, point.z, 1.0])
    point_car = np.dot(carWorld_m, point_world)
    p = geometry_msgs.msg.Point()
    p.x, p.y, p.z, _ = point_car
    # print(t)
    # print(p)
    # print(tf2_geometry_msgs.do_transform_point(p, t))
    # assert False ," hh"
    return p
Пример #25
0
 def test_quaternion_diff(self, q1, q2):
     q3 = quaternion_multiply(quaternion_conjugate(q1), q2)
     q4 = w.compile_and_execute(w.quaternion_diff, [q1, q2])
     self.assertTrue(np.isclose(q3, q4).all() or np.isclose(q3, -q4).all(), msg='{} != {}'.format(q1, q4))
Пример #26
0
 def vectorRotateQuaternion(self, q, v):
     '''return  qvq^-1.  q(x, y, z, w) '''
     #t = tf.transformations
     qua_mul = t.quaternion_multiply
     q_c = t.quaternion_conjugate(q)
     return qua_mul(qua_mul(q, v), q_c)
Пример #27
0
 def test_quaternion_conjugate(self, q):
     r1 = w.compile_and_execute(w.quaternion_conjugate, [q])
     r2 = quaternion_conjugate(q)
     self.assertTrue(np.isclose(r1, r2).all() or np.isclose(r1, -r2).all(), msg='{} != {}'.format(r1, r2))
Пример #28
0
def quatRotatePoint(q, p, o=Vec(0, 0, 0)):
    """returns point p rotated around quaternion q with the origin o (default (0,0,0)"""
    return quatMult(quatMult(q, numpy.append(p - o, (0, ))),
                    quaternion_conjugate(q))[:3] + o
Пример #29
0
def main():

    SCRIPT_DIR = os.path.dirname(os.path.realpath(__file__))
    default_calibration_target = SCRIPT_DIR + "/config/granite_april_tag.yaml"

    parser = argparse.ArgumentParser(
        description="Calibrate extrinsics parameters.")
    parser.add_argument(
        "robot",
        help=
        "The name of the robot to configure (i.e., put p4d to edit p4d.config).",
    )
    parser.add_argument("intrinsics_yaml",
                        help="The yaml file with intrinsics calibration data.")
    parser.add_argument("extrinsics_bag",
                        help="The bag file with extrinsics calibration data.")
    parser.add_argument(
        "-d",
        "--dock_cam",
        dest="dock_cam",
        action="store_true",
        help="Calibrate dock camera.",
    )
    parser.add_argument(
        "-f",
        "--from",
        dest="from_time",
        help="Use the bag data starting at this time, in seconds.",
    )
    parser.add_argument(
        "-t",
        "--to",
        dest="to_time",
        help="Use the bag data until this time, in seconds.",
    )
    parser.add_argument(
        "--timeoffset-padding",
        dest="padding",
        help=
        "Maximum range in which the timeoffset may change during estimation, in seconds. See readme.md for more info. (default: 0.01)",
    )
    parser.add_argument(
        "--calibration_target",
        dest="calibration_target",
        help=
        "Use this yaml file to desribe the calibration target, overriding the default: "
        + default_calibration_target,
    )
    parser.add_argument("-v",
                        "--verbose",
                        dest="verbose",
                        action="store_true",
                        help="Verbose mode.")
    args = parser.parse_args()

    if args.calibration_target is None:
        args.calibration_target = default_calibration_target

    imu_yaml = SCRIPT_DIR + "/config/imu.yaml"
    config_file = SCRIPT_DIR + "/../../astrobee/config/robots/" + args.robot + ".config"

    # Sanity check
    has_dock = has_dock_topic(args.intrinsics_yaml)
    if has_dock and (not args.dock_cam):
        print(
            "File " + args.intrinsics_yaml + " + has a dock topic, " +
            " yet the --dock-cam flag was not used.",
            file=sys.stderr,
        )
        return

    print("Calibrating camera extrinsics...")
    extrinsics_yaml = calibrate_extrinsics(
        args.extrinsics_bag,
        args.calibration_target,
        args.intrinsics_yaml,
        imu_yaml,
        args.from_time,
        args.to_time,
        args.padding,
        verbose=args.verbose,
    )

    if extrinsics_yaml == None:
        print("Failed to calibrate extrinsics.", file=sys.stderr)
        return

    two_cams = has_two_cameras(extrinsics_yaml)
    if two_cams:
        imu_to_camera = read_yaml_extrinsics(extrinsics_yaml, ["cam0", "cam1"])
    else:
        imu_to_camera = read_yaml_extrinsics(extrinsics_yaml, ["cam0"])

    if not imu_to_camera:
        print("Failed to read extrinsics from yaml file.", file=sys.stderr)
        return

    body_to_imu = trans_quat_to_transform(
        lua_read_transform(config_file, "imu_transform"))
    if body_to_imu is None:
        print("Failed to read imu transform.", file=sys.stderr)
        return

    imu_to_body = np.linalg.inv(body_to_imu)
    body_to_camera = np.linalg.inv(
        imu_to_body.dot(np.linalg.inv(imu_to_camera[0])))

    q = transformations.quaternion_conjugate(
        transformations.quaternion_from_matrix(body_to_camera))
    t = imu_to_body.dot(np.linalg.inv(imu_to_camera[0])[0:4, 3])
    print("Cam0:")
    print("Translation: ", t)
    print("Rotation quaternion: ", q)

    if two_cams:
        # Modify the transform for the second camera, that is the depth one
        body_to_depth_camera = np.linalg.inv(
            imu_to_body.dot(np.linalg.inv(imu_to_camera[1])))
        q_depth = transformations.quaternion_conjugate(
            transformations.quaternion_from_matrix(body_to_depth_camera))
        t_depth = imu_to_body.dot(np.linalg.inv(imu_to_camera[1])[0:4, 3])
        print("Cam1:")
        print("Translation: ", t_depth)
        print("Rotation quaternion: ", q_depth)
        if lua_replace_transform(
                config_file,
                "perch_cam_transform"
                if args.dock_cam else "haz_cam_transform",
            (t_depth, q_depth),
        ):
            print(
                "Failed to update config file with depth camera parameters.",
                file=sys.stderr,
            )
            return

    has_depth = has_depth_topic(args.intrinsics_yaml)
    if has_depth and (not two_cams):
        # Only the depth camera is present, so the first transform corresponds to the depth camera
        if lua_replace_transform(
                config_file,
                "perch_cam_transform"
                if args.dock_cam else "haz_cam_transform",
            (t, q),
        ):
            print(
                "Failed to update config file with depth camera parameters",
                file=sys.stderr,
            )
            return
    else:
        # Either both cameras are present, or only the HD cam, so the first transform
        # corresponds to the HD camera
        if lua_replace_transform(
                config_file,
                "dock_cam_transform" if args.dock_cam else "nav_cam_transform",
            (t, q),
        ):
            print(
                "Failed to update config file with HD camera parameters",
                file=sys.stderr,
            )
            return
Пример #30
0
def quaternion_rotate(quaternion, vector):  # type: (np.ndarray, np.ndarray) -> np.ndarray
    q = quaternion
    v = np.concatenate((vector, [0]))
    qc = transformations.quaternion_conjugate(quaternion)
    return transformations.quaternion_multiply(transformations.quaternion_multiply(q, v), qc)
Пример #31
0
def qv_mult(q1, v1):
    q2 = np.append(v1, 0.0)
    return tfs.quaternion_multiply(tfs.quaternion_multiply(q1, q2),
                                   tfs.quaternion_conjugate(q1))[:3]
Пример #32
0
 def inverse(self):
     q_congugate = quaternion_conjugate(self.q)
     return CompactTransform(-rotationMatrix(q_congugate) * self.p,
                             q_congugate)
Пример #33
0
import numpy as np

vel = np.genfromtxt('/home/harish/RRC/ICRA_2019/baseline_2/aaaaa/New_method/x_y_z_translations/baseline_2_velocities_single_1.txt', delimiter=' ')
pose = np.genfromtxt('/home/harish/RRC/ICRA_2019/baseline_2/aaaaa/New_method/x_y_z_translations/baseline_2_exp_indi.txt',delimiter=' ')
pose1 = np.zeros_like(pose)
pose1[0,:] = pose[0,:]

for i in range(pose.shape[0]-1):
    ti = pose1[i,:3]
    qi = pose1[i,3:]

    #vt = np.array([vel[i,0]*(-0.1),vel[i,1]*(-0.0988),vel[i,2]*(0.01)])
    #wt = np.array([vel[i,3]*1,vel[i,4]*(-1),vel[i,5]*2])

    vt = np.array([vel[i,0],vel[i,1],vel[i,2]])
    wt = np.array([vel[i,3],vel[i,4],vel[i,5]])


    td = vt
    qd = tft.quaternion_from_euler(wt[0],wt[1],wt[2],'sxyz')

    qi1 = tft.quaternion_multiply(qd, qi) #quaternion_multiply(q_rot, q_orig)

    #rotating vector around quat equivivalent of ri*Td(1:3,4)+ti
    tdh = np.hstack([td,0])
    ti1 = tft.quaternion_multiply(tft.quaternion_multiply(qi, tdh), tft.quaternion_conjugate(qi))[:3] + ti

    pose1[i+1,:] = np.hstack([ti1,qi1])

np.savetxt('/home/yvsharish/working/aaaaa_2/baseline_2_after_change.txt', pose1,delimiter=' ')
Пример #34
0
def main():

    SCRIPT_DIR = os.path.dirname(os.path.realpath(__file__))
    default_calibration_target = SCRIPT_DIR + '/config/granite_april_tag.yaml'

    parser = argparse.ArgumentParser(
        description='Calibrate extrinsics parameters.')
    parser.add_argument(
        'robot',
        help=
        'The name of the robot to configure (i.e., put p4d to edit p4d.config).'
    )
    parser.add_argument('intrinsics_yaml',
                        help='The yaml file with intrinsics calibration data.')
    parser.add_argument('extrinsics_bag',
                        help='The bag file with extrinsics calibration data.')
    parser.add_argument('-d',
                        '--dock_cam',
                        dest='dock_cam',
                        action='store_true',
                        help='Calibrate dock camera.')
    parser.add_argument(
        '-f',
        '--from',
        dest='from_time',
        help='Use the bag data starting at this time, in seconds.')
    parser.add_argument('-t',
                        '--to',
                        dest='to_time',
                        help='Use the bag data until this time, in seconds.')
    parser.add_argument(
        '--timeoffset-padding',
        dest='padding',
        help=
        'Maximum range in which the timeoffset may change during estimation, in seconds. See readme.md for more info. (default: 0.01)'
    )
    parser.add_argument(
        '--calibration_target',
        dest='calibration_target',
        help=
        'Use this yaml file to desribe the calibration target, overriding the default: '
        + default_calibration_target)
    parser.add_argument('-v',
                        '--verbose',
                        dest='verbose',
                        action='store_true',
                        help='Verbose mode.')
    args = parser.parse_args()

    if args.calibration_target is None:
        args.calibration_target = default_calibration_target

    imu_yaml = SCRIPT_DIR + '/config/imu.yaml'
    config_file = SCRIPT_DIR + '/../../astrobee/config/robots/' + args.robot + '.config'

    # Sanity check
    has_dock = has_dock_topic(args.intrinsics_yaml)
    if has_dock and (not args.dock_cam):
        print >> sys.stderr, 'File ' + args.intrinsics_yaml + ' + has a dock topic, ' + \
              ' yet the --dock-cam flag was not used.'
        return

    print 'Calibrating camera extrinsics...'
    extrinsics_yaml = calibrate_extrinsics(args.extrinsics_bag,
                                           args.calibration_target,
                                           args.intrinsics_yaml,
                                           imu_yaml,
                                           args.from_time,
                                           args.to_time,
                                           args.padding,
                                           verbose=args.verbose)

    if extrinsics_yaml == None:
        print >> sys.stderr, 'Failed to calibrate extrinsics.'
        return

    two_cams = has_two_cameras(extrinsics_yaml)
    if two_cams:
        imu_to_camera = read_yaml_extrinsics(extrinsics_yaml, ['cam0', 'cam1'])
    else:
        imu_to_camera = read_yaml_extrinsics(extrinsics_yaml, ['cam0'])

    if not imu_to_camera:
        print >> sys.stderr, 'Failed to read extrinsics from yaml file.'
        return

    body_to_imu = trans_quat_to_transform(
        lua_read_transform(config_file, 'imu_transform'))
    if body_to_imu is None:
        print >> sys.stderr, 'Failed to read imu transform.'
        return

    imu_to_body = np.linalg.inv(body_to_imu)
    body_to_camera = np.linalg.inv(
        imu_to_body.dot(np.linalg.inv(imu_to_camera[0])))

    q = transformations.quaternion_conjugate(
        transformations.quaternion_from_matrix(body_to_camera))
    t = imu_to_body.dot(np.linalg.inv(imu_to_camera[0])[0:4, 3])
    print 'Cam0:'
    print 'Translation: ', t
    print 'Rotation quaternion: ', q

    if two_cams:
        # Modify the transform for the second camera, that is the depth one
        body_to_depth_camera = np.linalg.inv(
            imu_to_body.dot(np.linalg.inv(imu_to_camera[1])))
        q_depth = transformations.quaternion_conjugate(
            transformations.quaternion_from_matrix(body_to_depth_camera))
        t_depth = imu_to_body.dot(np.linalg.inv(imu_to_camera[1])[0:4, 3])
        print 'Cam1:'
        print 'Translation: ', t_depth
        print 'Rotation quaternion: ', q_depth
        if lua_replace_transform(
                config_file, 'perch_cam_transform'
                if args.dock_cam else 'haz_cam_transform', (t_depth, q_depth)):
            print >> sys.stderr, 'Failed to update config file with depth camera parameters.'
            return

    has_depth = has_depth_topic(args.intrinsics_yaml)
    if has_depth and (not two_cams):
        # Only the depth camera is present, so the first transform corresponds to the depth camera
        if lua_replace_transform(
                config_file, 'perch_cam_transform'
                if args.dock_cam else 'haz_cam_transform', (t, q)):
            print >> sys.stderr, 'Failed to update config file with depth camera parameters'
            return
    else:
        # Either both cameras are present, or only the HD cam, so the first transform
        # corresponds to the HD camera
        if lua_replace_transform(
                config_file,
                'dock_cam_transform' if args.dock_cam else 'nav_cam_transform',
            (t, q)):
            print >> sys.stderr, 'Failed to update config file with HD camera parameters'
            return
Пример #35
0
def qv_mult(q1, v1):
    # https: // answers.ros.org / question / 196149 / how - to - rotate - vector - by - quaternion - in -python /
    v1 = unit_vector(v1)
    q2 = list(v1) + [0.0]
    return quaternion_multiply(quaternion_multiply(q1, q2),
                               quaternion_conjugate(q1))[:3]
Пример #36
0
def main():
    parser = argparse.ArgumentParser(
        description='Calibrate extrinsics parameters.')
    parser.add_argument('-d',
                        '--dock_cam',
                        dest='dock_cam',
                        action='store_true',
                        help='Calibrate dock camera.')
    parser.add_argument('-v',
                        '--verbose',
                        dest='verbose',
                        action='store_true',
                        help='Verbose mode.')
    parser.add_argument(
        'robot',
        help=
        'The name of the robot to configure (i.e., put p4d to edit p4d.config).'
    )
    parser.add_argument('intrinsics_yaml',
                        help='The bag file with intrinsics calibration data.')
    parser.add_argument('extrinsics_bag',
                        help='The bag file with extrinsics calibration data.')
    args = parser.parse_args()

    SCRIPT_DIR = os.path.dirname(os.path.realpath(__file__))

    target_yaml = SCRIPT_DIR + '/data/granite_april_tag.yaml'
    imu_yaml = SCRIPT_DIR + '/data/imu.yaml'
    config_file = SCRIPT_DIR + '/../../astrobee/config/robots/' + args.robot + '.config'

    print 'Calibrating camera extrinsics...'
    extrinsics_yaml = calibrate_extrinsics(args.extrinsics_bag,
                                           target_yaml,
                                           intrinsics_yaml,
                                           imu_yaml,
                                           verbose=args.verbose)

    if extrinsics_yaml == None:
        print >> sys.stderr, 'Failed to calibrate extrinsics.'
        return

    depth = has_depth_cam(extrinsics_yaml)

    if depth:
        imu_to_camera = read_yaml_extrinsics(extrinsics_yaml, ['cam0', 'cam1'])
    else:
        imu_to_camera = read_yaml_extrinsics(extrinsics_yaml, ['cam0'])

    if not imu_to_camera:
        print >> sys.stderr, 'Failed to read extrinsics from yaml file.'
        return

    body_to_imu = trans_quat_to_transform(
        lua_read_transform(config_file, 'imu_transform'))
    if body_to_imu is None:
        print >> sys.stderr, 'Failed to read imu transform.'
        return

    imu_to_body = np.linalg.inv(body_to_imu)
    body_to_camera = np.linalg.inv(
        imu_to_body.dot(np.linalg.inv(imu_to_camera[0])))

    q = transformations.quaternion_conjugate(
        transformations.quaternion_from_matrix(body_to_camera))
    t = imu_to_body.dot(np.linalg.inv(imu_to_camera[0])[0:4, 3])
    print 'Cam0:'
    print 'Translation: ', t
    print 'Rotation quaternion: ', q

    #if args.depth_cam:
    if depth:
        body_to_depth_camera = np.linalg.inv(
            imu_to_body.dot(np.linalg.inv(imu_to_camera[1])))
        q_depth = transformations.quaternion_conjugate(
            transformations.quaternion_from_matrix(body_to_depth_camera))
        t_depth = imu_to_body.dot(np.linalg.inv(imu_to_camera[1])[0:4, 3])
        print 'Cam1:'
        print 'Translation: ', t_depth
        print 'Rotation quaternion: ', q_depth
        if lua_replace_transform(
                config_file, 'perch_cam_transform'
                if args.dock_cam else 'haz_cam_transform', (t_depth, q_depth)):
            print >> sys.stderr, 'Failed to update config file with depth camera parameters.'
            return

    if lua_replace_transform(
            config_file,
            'dock_cam_transform' if args.dock_cam else 'nav_cam_transform',
        (t, q)):
        print >> sys.stderr, 'Failed to update config file with cam0 parameters'
        return
Пример #37
0
 def __sub__(self, other):
     return CompactTransform(
         self.p - other.p,
         quaternion_multiply(self.q, quaternion_conjugate(other.q)))
Пример #38
0
def qv_mult(q, v):
    v = v + (0.0,)
    q_conj = transformations.quaternion_conjugate(q)
    return transformations.quaternion_multiply(transformations.quaternion_multiply(q,v), q_conj)[:-1]
Пример #39
0
def qinv(q):
    """ Quaternion Inverse (Conjugate, Unit) """
    # no normalization, assume unit
    #va, ra = q[:3], q[-1]
    #return np.concatenate([va, [-ra]], axis=-1)
    return tx.quaternion_conjugate(q)
Пример #40
0
def qv_mult(q1, v1):
    q2 = (0.0,) + v1
    q3 = tft.quaternion_multiply(q1, q2)
    return tft.quaternion_multiply(q3, tft.quaternion_conjugate(q1))[1:]