Esempio n. 1
0
def ros_pose_slerp(start, end, n):

    frame = None
    if isinstance(start, PoseStamped):
        frame = start.header.frame_id
        start = start.pose
        end = end.pose

    start_pos = point_to_array(start.position)
    end_pos = point_to_array(end.position)

    positions = np.linspace(start_pos, end_pos, n, endpoint=True)
    quats = [
        quaternion_slerp(quat_to_array(start.orientation),
                         quat_to_array(end.orientation), i)
        for i in np.linspace(0, 1, n, endpoint=True)
    ]

    poses = [
        Pose(Point(*pos), Quaternion(*quat))
        for pos, quat in zip(positions, quats)
    ]
    if frame is not None:
        header = Header()
        header.frame_id = frame
        poses_stamped = [PoseStamped(header, pose) for pose in poses]
        return poses_stamped
    return poses
Esempio n. 2
0
 def test_slerp2(self, q1, q2, t):
     r1 = w.compile_and_execute(w.quaternion_slerp, [q1, q2, t])
     r2 = quaternion_slerp(q1, q2, t)
     self.assertTrue(np.isclose(r1, r2, atol=1e-3).all()
                     or np.isclose(r1, -r2, atol=1e-3).all(),
                     msg='q1={} q2={} t={}\n{} != {}'.format(
                         q1, q2, t, r1, r2))
Esempio n. 3
0
def interpolatePoses(ts_out, ts_in, pose_in):
    """
    Map pose inputs from ts_in to ts_out
    Parameters:
        ts_out - Output time stamps
        ts_in - Input time stamps
        pose_in - Array of ts_in size containing poses-[position, quaterion]
                  size (7, N), Quat is stored as (x,y,z,w)
    Return: interpolated poses at ts_out
    """
    pos_out_list = []
    for i in range(3):
        pos_out_list.append(np.interp(ts_out, ts_in, pose_in[:,i]))
    position_out = np.vstack(pos_out_list)
    quat_out_list = []
    quat_in = pose_in[:,3:]
    # Find fraction for interpolating quaternion
    N = ts_in.size
    out = np.interp(ts_out, ts_in, np.arange(N))
    for out_i in out:
        id0 = int(np.floor(out_i))
        id1 = min(N - 1, id0 + 1)
        frac = out_i - id0
        quat_out = tf.quaternion_slerp(quat_in[id0], quat_in[id1], frac)
        quat_out_list.append(quat_out)
    quat_out_arr = np.vstack(quat_out_list).T
    return np.vstack((position_out, quat_out_arr))
def interpolate_pose_stamped(pose_stamped_a, pose_stamped_b, frac):
    apose = pose_stamped_a.pose
    bpose = pose_stamped_b.pose
    ap = np.matrix([apose.position.x, apose.position.y, apose.position.z])
    bp = np.matrix([bpose.position.x, bpose.position.y, bpose.position.z])
    ip = ap + (bp - ap) * frac

    aq = [
        apose.orientation.x, apose.orientation.y, apose.orientation.z,
        apose.orientation.w
    ]
    bq = [
        bpose.orientation.x, bpose.orientation.y, bpose.orientation.z,
        bpose.orientation.w
    ]
    iq = tr.quaternion_slerp(aq, bq, frac)

    ps = geo.PoseStamped()
    ps.header = pose_stamped_b.header
    ps.pose.position.x = ip[0, 0]
    ps.pose.position.y = ip[0, 1]
    ps.pose.position.z = ip[0, 2]
    ps.pose.orientation.x = iq[0]
    ps.pose.orientation.y = iq[1]
    ps.pose.orientation.z = iq[2]
    ps.pose.orientation.w = iq[3]
    return ps
Esempio n. 5
0
def despsi_fun(goal, t_gpsi, q0f, t_nowf):
    if t_nowf < t_gpsi:
        ratio = safe_div(t_nowf, t_gpsi)
        des = tft.quaternion_slerp(q0f, goal, ratio)
    else:
        des = goal
    return des
Esempio n. 6
0
    def build_trajectory(self, finish, start=None, arm=0, space=0.001, steps=None):  # WORKING, TESTED
        ##!!!!!!!!!!!!  MUST BALANCE SPACING WITH SEND RATE IN 'SEND_TRAJ' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
        if start is None:  # if given one pose, use current position as start
            start = self.curr_state[arm]

        dist = self.calc_dist(start, finish, arm=arm)  # Total distance to travel
        if steps is None:
            steps = int(math.ceil(dist / space))
        fracs = np.linspace(0, 1, steps)  # A list of fractional positions along course
        print "Steps: %s" % steps

        poses = [PoseStamped() for i in xrange(steps)]
        xs = np.linspace(start.pose.position.x, finish.pose.position.x, steps)
        ys = np.linspace(start.pose.position.y, finish.pose.position.y, steps)
        zs = np.linspace(start.pose.position.z, finish.pose.position.z, steps)

        qs = [start.pose.orientation.x, start.pose.orientation.y, start.pose.orientation.z, start.pose.orientation.w]
        qf = [
            finish.pose.orientation.x,
            finish.pose.orientation.y,
            finish.pose.orientation.z,
            finish.pose.orientation.w,
        ]

        for i, frac in enumerate(fracs):
            poses[i].header.stamp = rospy.Time.now()
            poses[i].header.frame_id = start.header.frame_id
            poses[i].pose.position = Point(xs[i], ys[i], zs[i])
            new_q = transformations.quaternion_slerp(qs, qf, frac)
            poses[i].pose.orientation = Quaternion(*new_q)
        # rospy.loginfo("Planning straight-line path, please wait")
        # self.wt_log_out.publish(data="Planning straight-line path, please wait")
        return poses
Esempio n. 7
0
def epsi_fun(goal, t_goal, q0, t_now, q_now):
    if t_now < t_goal:
        des = tft.quaternion_slerp(q0, goal, t_now / t_goal)
    else:
        des = goal
    qdc = tft.quaternion_multiply(des, [-q_now[0], -q_now[1], -q_now[2], q_now[3]])
    edc = tft.euler_from_quaternion(qdc)
    err = edc[2]
    return err
 def test_speed_up_diffable_slerp(self, q1, q2, t):
     q1 = np.array(q1)
     q2 = np.array(q2)
     r = speed_up_and_execute(spw.diffable_slerp, [q1, q2, t])
     r_ref = quaternion_slerp(q1, q2, t)
     try:
         np.testing.assert_almost_equal(r, r_ref, decimal=3)
     except:
         np.testing.assert_almost_equal(r, -r_ref, decimal=3)
Esempio n. 9
0
def averagePoses(pose1, pose2, t):
		"""Average two poses with weighting t to 1

		This computes the average of the kartesian positions using
		linear interpolation and of the rotation using SLERP"""

		factor= float(t)/(t+1)
		translation= tuple(map(lambda x,y: factor*x + (1-factor)*y, pose1[0], pose2[0]))
		rotation= quaternion_slerp(pose1[1], pose2[1], 1.0/(t+1))
		return (translation, rotation)
Esempio n. 10
0
 def update(self, translation_meas, rotation_meas, i):
     if (self.transformations[i] is None):
         self.transformations[i] = geometry_msgs.msg.Transform()
         self.transformations[i].translation = translation_meas
         self.transformations[i].rotation = rotation_meas
         return
     self.transformations[i].translation = self.transformations[
         i].translation * weight + translation_meas * (1 - weight)
     self.transformations[i].rotation = quaternion_slerp(
         self.transformations[i].rotation, rotation_meas, 1.0 - weight)
Esempio n. 11
0
 def maintain_force_position(self, pose=None, mean=3, std=1):
     self.stop_maintain = False
     if pose is None:
         goal = self.aul.curr_pose()
     else:
         goal = pose
     goal_ort = [
         goal.pose.orientation.x, goal.pose.orientation.y,
         goal.pose.orientation.z, goal.pose.orientation.w
     ]
     error = PoseStamped()
     maintain_rate = rospy.Rate(250)
     while not (rospy.is_shutdown() or self.stop_maintain):
         current = self.aul.curr_pose()
         current_ort = [
             current.pose.orientation.x, current.pose.orientation.y,
             current.pose.orientation.z, current.pose.orientation.w
         ]
         error.pose.position.x = current.pose.position.x - goal.pose.position.x
         error.pose.position.y = current.pose.position.y - goal.pose.position.y
         error.pose.position.z = current.pose.position.z - goal.pose.position.z
         error_mag = math.sqrt(error.pose.position.x**2 +
                               error.pose.position.y**2 +
                               error.pose.position.z**2)
         #out = deepcopy(goal)
         out = PoseStamped()
         out.header.frame_id = goal.header.frame_id
         out.header.stamp = rospy.Time.now()
         out.pose.position = Point(goal.pose.position.x,
                                   goal.pose.position.y,
                                   goal.pose.position.z)
         self.test_pose.publish(out)
         if all(np.array(self.ft_mag_que) < mean -
                std) and error_mag > 0.005:
             #print 'Force Too LOW'
             out.pose.position.x += 0.990 * error.pose.position.x
             out.pose.position.y += 0.990 * error.pose.position.y
             out.pose.position.z += 0.990 * error.pose.position.z
             ori = transformations.quaternion_slerp(goal_ort, current_ort,
                                                    0.990)
             out.pose.orientation = Quaternion(*ori)
             self.aul.fast_move(out, 0.0038)
         elif all(np.array(self.ft_mag_que) > mean + std):
             #print 'Moving to avoid force'
             current.pose.position.x += self.ft.wrench.force.x / 9000
             current.pose.position.y += self.ft.wrench.force.y / 9000
             current.pose.position.z += self.ft.wrench.force.z / 9000
             self.aul.fast_move(current, 0.0038)
         else:
             pass
             #print "Force in desired range"
         mean = self.force_goal_mean
         std = self.force_goal_std
         #rospy.sleep(0.001)
         maintain_rate.sleep()
Esempio n. 12
0
 def maintain_force_position(self, pose=None, mean=3, std=1):
     self.stop_maintain = False
     if pose is None:
         goal = self.aul.curr_pose()
     else:
         goal = pose
     goal_ort = [
         goal.pose.orientation.x, goal.pose.orientation.y,
         goal.pose.orientation.z, goal.pose.orientation.w
     ]
     error = PoseStamped()
     maintain_rate = rospy.Rate(250)
     while not (rospy.is_shutdown() or self.stop_maintain):
         current = self.aul.curr_pose()
         current_ort = [
             current.pose.orientation.x, current.pose.orientation.y,
             current.pose.orientation.z, current.pose.orientation.w
         ]
         error.pose.position.x = current.pose.position.x - goal.pose.position.x
         error.pose.position.y = current.pose.position.y - goal.pose.position.y
         error.pose.position.z = current.pose.position.z - goal.pose.position.z
         error_mag = math.sqrt(error.pose.position.x**2 +
                               error.pose.position.y**2 +
                               error.pose.position.z**2)
         #out = deepcopy(goal)
         out = PoseStamped()
         out.header.frame_id = goal.header.frame_id
         out.header.stamp = rospy.Time.now()
         out.pose.position = Point(goal.pose.position.x,
                                   goal.pose.position.y,
                                   goal.pose.position.z)
         self.test_pose.publish(out)
         if all(np.array(self.ft_mag_que) < mean -
                std) and error_mag > 0.005:
             #print 'Force Too LOW'
             out.pose.position.x += 0.990 * error.pose.position.x
             out.pose.position.y += 0.990 * error.pose.position.y
             out.pose.position.z += 0.990 * error.pose.position.z
             ori = transformations.quaternion_slerp(goal_ort, current_ort,
                                                    0.990)
             out.pose.orientation = Quaternion(*ori)
             self.aul.fast_move(out, 0.0038)
         elif all(np.array(self.ft_mag_que) > mean + std):
             #print 'Moving to avoid force'
             current.pose.position.x += self.ft.wrench.force.x / 9000
             current.pose.position.y += self.ft.wrench.force.y / 9000
             current.pose.position.z += self.ft.wrench.force.z / 9000
             self.aul.fast_move(current, 0.0038)
         else:
             pass
             #print "Force in desired range"
         mean = self.force_goal_mean
         std = self.force_goal_std
         #rospy.sleep(0.001)
         maintain_rate.sleep()
Esempio n. 13
0
    def build_trajectory(self,
                         finish,
                         start=None,
                         ik_space=0.015,
                         duration=None,
                         tot_points=None,
                         jagged=False):
        if start == None:  # if given one pose, use current position as start, else, assume (start, finish)
            start = self.curr_pose()

        # Based upon distance to travel, determine the number of intermediate poses required
        # find linear increments of position.

        dist = self.calc_dist(start, finish)  #Total distance to travel
        ik_steps = math.ceil(dist / ik_space)
        print "IK Steps: %s" % ik_steps
        ik_fracs = np.linspace(
            0, 1, ik_steps
        )  #A list of fractional positions along course to evaluate ik

        x_gap = finish.pose.position.x - start.pose.position.x
        y_gap = finish.pose.position.y - start.pose.position.y
        z_gap = finish.pose.position.z - start.pose.position.z

        qs = [
            start.pose.orientation.x, start.pose.orientation.y,
            start.pose.orientation.z, start.pose.orientation.w
        ]
        qf = [
            finish.pose.orientation.x, finish.pose.orientation.y,
            finish.pose.orientation.z, finish.pose.orientation.w
        ]

        #For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
        poses = [PoseStamped() for i in range(len(ik_fracs))]
        for i, frac in enumerate(ik_fracs):
            poses[i].header.stamp = rospy.Time.now()
            poses[i].header.frame_id = start.header.frame_id
            poses[i].pose.position.x = start.pose.position.x + x_gap * frac
            poses[i].pose.position.y = start.pose.position.y + y_gap * frac
            poses[i].pose.position.z = start.pose.position.z + z_gap * frac
            new_q = transformations.quaternion_slerp(qs, qf, frac)
            poses[i].pose.orientation = Quaternion(*new_q)
        rospy.loginfo("Planning straight-line path, please wait")
        self.wt_log_out.publish(
            data="Planning straight-line path, please wait")

        if jagged:
            for i, p in enumerate(poses):
                if i % 2 == 0:
                    poses[i] = self.pose_frame_move(poses[i], 0, 0.007, 0)
                else:
                    poses[i] = self.pose_frame_move(poses[i], 0, -0.007, 0)
        #return poses
        return self.fill_ik_traj(poses, dist, duration, tot_points)
Esempio n. 14
0
def interpolate_cartesian(start_pose, end_pose, num_steps):
    diff_pos = end_pose[:3,3] - start_pose[:3,3]
    start_quat = tf_trans.quaternion_from_matrix(start_pose)
    end_quat = tf_trans.quaternion_from_matrix(end_pose)
    mat_list = []
    for fraction in np.linspace(0, 1, num_steps):
        cur_quat = tf_trans.quaternion_slerp(start_quat, end_quat, fraction)
        cur_mat = np.mat(tf_trans.quaternion_matrix(cur_quat))
        cur_mat[:3,3] = start_pose[:3,3] + fraction * diff_pos
        mat_list.append(cur_mat)
    return mat_list
Esempio n. 15
0
def interpolate_cartesian(start_pose, end_pose, num_steps):
    diff_pos = end_pose[:3, 3] - start_pose[:3, 3]
    start_quat = tf_trans.quaternion_from_matrix(start_pose)
    end_quat = tf_trans.quaternion_from_matrix(end_pose)
    mat_list = []
    for fraction in np.linspace(0, 1, num_steps):
        cur_quat = tf_trans.quaternion_slerp(start_quat, end_quat, fraction)
        cur_mat = np.mat(tf_trans.quaternion_matrix(cur_quat))
        cur_mat[:3, 3] = start_pose[:3, 3] + fraction * diff_pos
        mat_list.append(cur_mat)
    return mat_list
Esempio n. 16
0
def averagePoses(pose1, pose2, t):
    """Average two poses with weighting t to 1

		This computes the average of the kartesian positions using
		linear interpolation and of the rotation using SLERP"""

    factor = float(t) / (t + 1)
    translation = tuple(
        map(lambda x, y: factor * x + (1 - factor) * y, pose1[0], pose2[0]))
    rotation = quaternion_slerp(pose1[1], pose2[1], 1.0 / (t + 1))
    return (translation, rotation)
Esempio n. 17
0
def interpolate_imu_events(a, b, time):
    if a == None:
        return b.data.accel + b.data.gyro + b.data.quat
    if b == None:
        return a.data.accel + a.data.gyro + a.data.quat
    # if a and b are None it will crash
    factor = reverse_lerp(a.time.to_sec(), b.time.to_sec(), time.to_sec())

    aa = a.data.accel + a.data.gyro
    bb = b.data.accel + b.data.gyro

    return list(map(lambda x, y: lerp(x, y, factor), aa, bb)) + \
        list(t.quaternion_slerp(a.data.quat, b.data.quat, factor))
Esempio n. 18
0
def contKGpsi(goal, t_goal, zero, t_now, pos_now,
              gp):  # my controller function
    if t_now < t_goal:
        des = tft.quaternion_slerp(zero, goal, t_now / t_goal)
    else:
        des = goal
    zero_inv = tft.quaternion_inverse(pos_now)
    qdc = tft.quaternion_multiply(des, zero_inv)
    edc = tft.euler_from_quaternion(qdc)
    err = edc[2]
    f_nav = err * gp
    if abs(f_nav) > 1:
        f_nav = f_nav / abs(f_nav)
    return f_nav
Esempio n. 19
0
 def interpolate (pose1, pose2, ratio):
     if (pose1.timestamp > pose2.timestamp) :
         raise ValueError ("pose1 timestamp must be > pose2")
     td = (pose2.timestamp - pose1.timestamp)
     intpose = Pose(pose1.timestamp + ratio*td, 
         pose1.x + ratio*(pose2.x-pose1.x),
         pose1.y + ratio*(pose2.y-pose1.y),
         pose1.z + ratio*(pose2.z-pose1.z))
     q1 = pose1.quaternion()
     q2 = pose2.quaternion()
     qInt = trafo.quaternion_slerp(q1, q2, ratio)
     intpose.qx, intpose.qy, intpose.qz, intpose.qw = \
         qInt[0], qInt[1], qInt[2], qInt[3]
     return intpose
Esempio n. 20
0
 def interpolate (pose1, pose2, ratio):
     if (pose1.timestamp > pose2.timestamp) :
         raise ValueError ("pose1 timestamp must be > pose2")
     td = (pose2.timestamp - pose1.timestamp)
     intpose = Pose(pose1.timestamp + ratio*td, 
         pose1.x + ratio*(pose2.x-pose1.x),
         pose1.y + ratio*(pose2.y-pose1.y),
         pose1.z + ratio*(pose2.z-pose1.z))
     q1 = pose1.quaternion()
     q2 = pose2.quaternion()
     qInt = trafo.quaternion_slerp(q1, q2, ratio)
     intpose.qx, intpose.qy, intpose.qz, intpose.qw = \
         qInt[0], qInt[1], qInt[2], qInt[3]
     return intpose
Esempio n. 21
0
 def interpolateByTime (self, srcpose, tolerance=0.1):
     pmin, pmax = self.findNearPosesByTime (srcpose, tolerance)
     tRatio = (srcpose.timestamp - pmin.timestamp) / (pmax.timestamp - pmin.timestamp)
     
     # Interpolation of Position
     pvmin = pmin.coord()
     pvmax = pmax.coord()
     posInt = pvmin + tRatio * (pvmax - pvmin)
     
     # Interpolation of orientation
     qmin = pmin.quaternion()
     qmax = pmax.quaternion()
     qInt = trafo.quaternion_slerp(qmin, qmax, tRatio)
     return Pose(srcpose.timestamp, posInt[0], posInt[1], posInt[2], \
         qInt[0], qInt[1], qInt[2], qInt[3])
Esempio n. 22
0
 def interpolateByTime (self, srcpose, tolerance=0.1):
     pmin, pmax = self.findNearPosesByTime (srcpose, tolerance)
     tRatio = (srcpose.timestamp - pmin.timestamp) / (pmax.timestamp - pmin.timestamp)
     
     # Interpolation of Position
     pvmin = pmin.coord()
     pvmax = pmax.coord()
     posInt = pvmin + tRatio * (pvmax - pvmin)
     
     # Interpolation of orientation
     qmin = pmin.quaternion()
     qmax = pmax.quaternion()
     qInt = trafo.quaternion_slerp(qmin, qmax, tRatio)
     return Pose(srcpose.timestamp, posInt[0], posInt[1], posInt[2], \
         qInt[0], qInt[1], qInt[2], qInt[3])
Esempio n. 23
0
 def interpolate_ep(self, ep_a, ep_b, t_vals):
     pos_a, rot_a = ep_a
     pos_b, rot_b = ep_b
     num_samps = len(t_vals)
     pos_waypoints = np.array(pos_a) + np.array(np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals)
     pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T]
     rot_homo_a, rot_homo_b = np.eye(4), np.eye(4)
     rot_homo_a[:3,:3] = rot_a
     rot_homo_b[:3,:3] = rot_b
     quat_a = tf_trans.quaternion_from_matrix(rot_homo_a)
     quat_b = tf_trans.quaternion_from_matrix(rot_homo_b)
     rot_waypoints = []
     for t in t_vals:
         cur_quat = tf_trans.quaternion_slerp(quat_a, quat_b, t)
         rot_waypoints.append(np.mat(tf_trans.quaternion_matrix(cur_quat))[:3,:3])
     return zip(pos_waypoints, rot_waypoints)
Esempio n. 24
0
    def find_controlled_tool_pose(self, target_pose):
        # find current tool location
        t_B_c = self.get_transform("torso_lift_link", self.tool_frame)
        # find error in position
        err_pos = target_pose[:3, 3] - t_B_c[:3, 3]
        # find error in angle
        err_ang = util.quaternion_dist(target_pose, t_B_c)

        # find control values
        u_x = self.x_pid.update_state(err_pos[0, 0])
        u_y = self.y_pid.update_state(err_pos[1, 0])
        u_z = self.z_pid.update_state(err_pos[2, 0]) + self.grav_comp
        u_pos = np.mat([u_x, u_y, u_z]).T
        u_a = self.a_pid.update_state(err_ang)
        #       quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) -
        #                    np.array(tf_trans.quaternion_from_matrix(t_B_c)))
        #       u_qx = self.qx_pid.update_state(quat_diff[0])
        #       u_qy = self.qy_pid.update_state(quat_diff[1])
        #       u_qz = self.qz_pid.update_state(quat_diff[2])
        #       u_qw = self.qw_pid.update_state(quat_diff[3])
        #       u_quat = np.array([u_qx, u_qy, u_qz, u_qw])
        #       u_quat = u_quat / np.linalg.norm(u_quat)

        # find commanded frame of tool
        # rotation
        u_a = np.clip(u_a, 0, 1)
        ei_q_slerp = tf_trans.quaternion_slerp(
            tf_trans.quaternion_from_matrix(t_B_c),
            tf_trans.quaternion_from_matrix(target_pose), u_a)
        new_quat = ei_q_slerp
        #       new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c),
        #                                               u_quat)
        t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat))

        # position
        u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max)
        t_B_new[:3, 3] = t_B_c[:3, 3] + u_pos_clipped

        # publish informative topics
        self.goal_pub.publish(
            util.pose_mat_to_stamped_msg("torso_lift_link", target_pose))
        self.cur_pub.publish(
            util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c))
        self.cmd_pub.publish(
            util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new))

        return t_B_new, err_pos, err_ang
Esempio n. 25
0
def average_quaternions(qs, ws=None):
    """
  Averages the input quaternions using SLERP
  :param qs: list of input quaternions
  :param ws: weights
  :return: average quaternion
  """
    if ws is None:
        ws = np.ones(len(qs)) / len(qs)
    else:
        assert sum(ws) == 1

    for i in xrange(1, len(qs)):
        frac = ws[i] / (ws[i - 1] + ws[i])  # weight of qs[i]
        qs[i] = t.quaternion_slerp(qs[i - 1], qs[i], fraction=frac)
        ws[i] = 1 - sum(ws[i + 1:])

    return qs[-1]
Esempio n. 26
0
 def interpolate_ep(self, ep_a, ep_b, t_vals):
     pos_a, rot_a = ep_a
     pos_b, rot_b = ep_b
     num_samps = len(t_vals)
     pos_waypoints = np.array(pos_a) + np.array(
         np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals)
     pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T]
     rot_homo_a, rot_homo_b = np.eye(4), np.eye(4)
     rot_homo_a[:3, :3] = rot_a
     rot_homo_b[:3, :3] = rot_b
     quat_a = tf_trans.quaternion_from_matrix(rot_homo_a)
     quat_b = tf_trans.quaternion_from_matrix(rot_homo_b)
     rot_waypoints = []
     for t in t_vals:
         cur_quat = tf_trans.quaternion_slerp(quat_a, quat_b, t)
         rot_waypoints.append(
             np.mat(tf_trans.quaternion_matrix(cur_quat))[:3, :3])
     return zip(pos_waypoints, rot_waypoints)
Esempio n. 27
0
    def find_controlled_tool_pose(self, target_pose):
        # find current tool location
        t_B_c = self.get_transform("torso_lift_link", self.tool_frame)
        # find error in position
        err_pos = target_pose[:3,3] - t_B_c[:3,3]
        # find error in angle
        err_ang = util.quaternion_dist(target_pose, t_B_c)

        # find control values
        u_x = self.x_pid.update_state(err_pos[0,0])
        u_y = self.y_pid.update_state(err_pos[1,0])
        u_z = self.z_pid.update_state(err_pos[2,0]) + self.grav_comp
        u_pos = np.mat([u_x, u_y, u_z]).T
        u_a = self.a_pid.update_state(err_ang)
#       quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) -
#                    np.array(tf_trans.quaternion_from_matrix(t_B_c)))
#       u_qx = self.qx_pid.update_state(quat_diff[0])
#       u_qy = self.qy_pid.update_state(quat_diff[1])
#       u_qz = self.qz_pid.update_state(quat_diff[2])
#       u_qw = self.qw_pid.update_state(quat_diff[3])
#       u_quat = np.array([u_qx, u_qy, u_qz, u_qw])
#       u_quat = u_quat / np.linalg.norm(u_quat)

        # find commanded frame of tool
        # rotation
        u_a = np.clip(u_a, 0, 1)
        ei_q_slerp = tf_trans.quaternion_slerp(tf_trans.quaternion_from_matrix(t_B_c),
                                               tf_trans.quaternion_from_matrix(target_pose),
                                               u_a)
        new_quat = ei_q_slerp
#       new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c),
#                                               u_quat)
        t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat))

        # position
        u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max)
        t_B_new[:3,3] = t_B_c[:3,3] + u_pos_clipped 

        # publish informative topics
        self.goal_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", target_pose))
        self.cur_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c))
        self.cmd_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new))
        
        return t_B_new, err_pos, err_ang
Esempio n. 28
0
    def build_trajectory(self, finish, start=None, ik_space = 0.015, duration = None, tot_points=None, jagged=False):
        if start == None: # if given one pose, use current position as start, else, assume (start, finish)
            start = self.curr_pose()
        
        # Based upon distance to travel, determine the number of intermediate poses required
        # find linear increments of position.

        dist = self.calc_dist(start,finish)     #Total distance to travel
        ik_steps = math.ceil(dist/ik_space)     
        print "IK Steps: %s" %ik_steps
        ik_fracs = np.linspace(0, 1, ik_steps)   #A list of fractional positions along course to evaluate ik

        x_gap = finish.pose.position.x - start.pose.position.x
        y_gap = finish.pose.position.y - start.pose.position.y
        z_gap = finish.pose.position.z - start.pose.position.z

        qs = [start.pose.orientation.x, start.pose.orientation.y,
              start.pose.orientation.z, start.pose.orientation.w] 
        qf = [finish.pose.orientation.x, finish.pose.orientation.y,
              finish.pose.orientation.z, finish.pose.orientation.w] 

        #For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
        poses = [PoseStamped() for i in range(len(ik_fracs))]
        for i,frac in enumerate(ik_fracs):
            poses[i].header.stamp = rospy.Time.now()
            poses[i].header.frame_id = start.header.frame_id
            poses[i].pose.position.x = start.pose.position.x + x_gap*frac
            poses[i].pose.position.y = start.pose.position.y + y_gap*frac
            poses[i].pose.position.z = start.pose.position.z + z_gap*frac
            new_q = transformations.quaternion_slerp(qs,qf,frac)
            poses[i].pose.orientation = Quaternion(*new_q)
        rospy.loginfo("Planning straight-line path, please wait")
        self.wt_log_out.publish(data="Planning straight-line path, please wait")
      
        if jagged:
            for i,p in enumerate(poses):
                if i%2 == 0:
                    poses[i] = self.pose_frame_move(poses[i], 0, 0.007,0)
                else:
                    poses[i] = self.pose_frame_move(poses[i], 0, -0.007,0)
        #return poses
        return self.fill_ik_traj(poses, dist, duration, tot_points)
Esempio n. 29
0
    def build_trajectory(self,
                         finish,
                         start=None,
                         arm=0,
                         space=0.001,
                         steps=None):  #WORKING, TESTED
        ##!!!!!!!!!!!!  MUST BALANCE SPACING WITH SEND RATE IN 'SEND_TRAJ' FOR CONTROL OF VELOCITY !!!!!!!!!!!!
        if start is None:  # if given one pose, use current position as start
            start = self.curr_state[arm]

        dist = self.calc_dist(start, finish,
                              arm=arm)  #Total distance to travel
        if steps is None:
            steps = int(math.ceil(dist / space))
        fracs = np.linspace(
            0, 1, steps)  #A list of fractional positions along course
        print "Steps: %s" % steps

        poses = [PoseStamped() for i in xrange(steps)]
        xs = np.linspace(start.pose.position.x, finish.pose.position.x, steps)
        ys = np.linspace(start.pose.position.y, finish.pose.position.y, steps)
        zs = np.linspace(start.pose.position.z, finish.pose.position.z, steps)

        qs = [
            start.pose.orientation.x, start.pose.orientation.y,
            start.pose.orientation.z, start.pose.orientation.w
        ]
        qf = [
            finish.pose.orientation.x, finish.pose.orientation.y,
            finish.pose.orientation.z, finish.pose.orientation.w
        ]

        for i, frac in enumerate(fracs):
            poses[i].header.stamp = rospy.Time.now()
            poses[i].header.frame_id = start.header.frame_id
            poses[i].pose.position = Point(xs[i], ys[i], zs[i])
            new_q = transformations.quaternion_slerp(qs, qf, frac)
            poses[i].pose.orientation = Quaternion(*new_q)
        #rospy.loginfo("Planning straight-line path, please wait")
        #self.wt_log_out.publish(data="Planning straight-line path, please wait")
        return poses
Esempio n. 30
0
    def turn_in_place(self, dist_vec):

        cmd_vel = Twist()
        cmd_vel.angular.z = self.angular_speed

        # Publish the Twist message and sleep 1 cycle
        self.cmd_vel_pub.publish(cmd_vel)

        trans, quat = self.get_odom()
        quat0 = [
            self.goal.pose.orientation.w, self.goal.pose.orientation.x,
            self.goal.pose.orientation.y, self.goal.pose.orientation.z
        ]

        dist_vec.z = tftr.euler_from_quaternion(
            tftr.quaternion_slerp(quat0, quat,
                                  fraction=1))[2]  # take only the Yaw value

        r = rospy.Rate(20)
        r.sleep()
        return dist_vec
def interpolate_pose_stamped(pose_stamped_a, pose_stamped_b, frac):
    apose = pose_stamped_a.pose
    bpose = pose_stamped_b.pose
    ap = np.matrix([apose.position.x, apose.position.y, apose.position.z])
    bp = np.matrix([bpose.position.x, bpose.position.y, bpose.position.z])
    ip = ap + (bp - ap) * frac

    aq = [apose.orientation.x, apose.orientation.y, apose.orientation.z, apose.orientation.w]
    bq = [bpose.orientation.x, bpose.orientation.y, bpose.orientation.z, bpose.orientation.w]
    iq = tr.quaternion_slerp(aq, bq, frac)

    ps = geo.PoseStamped()
    ps.header = pose_stamped_b.header
    ps.pose.position.x = ip[0,0]
    ps.pose.position.y = ip[0,1]
    ps.pose.position.z = ip[0,2]
    ps.pose.orientation.x = iq[0]
    ps.pose.orientation.y = iq[1]
    ps.pose.orientation.z = iq[2]
    ps.pose.orientation.w = iq[3]
    return ps
Esempio n. 32
0
    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
Esempio n. 33
0
    def calc_orient_error(self, eef_orient, goal_orient, thresh, limit_p):
        # Error threshold
        precision_o = 0.17  # 10 deg tolerance
        z_threshold = 0.36  # 20 deg tolerance
        reached_orientation = False
        p = self.prop_gain_orient

        eef_inv = qo.q_inv(eef_orient)
        rot_vector = qo.quat_to_axisangle(qo.q_mult(eef_inv, goal_orient))
        rot_error = sqrt(
            pow(rot_vector[0], 2) + pow(rot_vector[1], 2) +
            pow(rot_vector[2], 2))

        if (thresh - rot_error) >= 0:
            scaling = 1
        else:
            scaling = thresh / rot_error

        slerp = quaternion_slerp(eef_orient, goal_orient, scaling)
        quat_error = qo.q_mult(slerp, eef_inv)
        error_orient = euler_from_quaternion(quat_error)
        # error_orient = qo.quaternion_error(slerp, eef_orient)

        # Stopping conditions
        x_y_error = sqrt(pow(error_orient[0], 2) + pow(error_orient[1], 2))
        if x_y_error < precision_o and abs(error_orient[2]) < z_threshold:
            reached_orientation = True
            '''if max(error_orient) > max(self.old_error):
                error_orient = [0.0, 0.0, 0.0]
        self.old_error = error_orient'''

        error_w_gain = [
            error_orient[0] * p, error_orient[1] * p, error_orient[2] * p
        ]

        return error_w_gain, reached_orientation
Esempio n. 34
0
def get_center_of_tongs(p1,p2):
    p1 = deepcopy(p1)
    p2 = deepcopy(p2)
    tongs_center = PoseStamped()
    tongs_center.pose.position.x = (p1.pose.position.x + p2.pose.position.x) / 2
    tongs_center.pose.position.y = (p1.pose.position.y + p2.pose.position.y) / 2
    tongs_center.pose.position.z = (p1.pose.position.z + p2.pose.position.z) / 2


    # quaternion averaging:
    # ref: https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
    w1 = p1.pose.orientation.w
    x1 = p1.pose.orientation.x
    y1 = p1.pose.orientation.y
    z1 = p1.pose.orientation.z

    w2 = p2.pose.orientation.w
    x2 = p2.pose.orientation.x
    y2 = p2.pose.orientation.y
    z2 = p2.pose.orientation.z

    q1 = np.array([x1,y1,z1,w1])
    q2 = np.array([x2, y2, z2, w2])
    q1_rot = tf.quaternion_multiply(q1,tf.quaternion_about_axis(np.pi,(1,0,0)))


    q_avg = tf.quaternion_slerp(q1_rot,q2,0.5)


    tongs_center.pose.orientation.x = q_avg[0]
    tongs_center.pose.orientation.y = q_avg[1]
    tongs_center.pose.orientation.z = q_avg[2]
    tongs_center.pose.orientation.w = q_avg[3]

    tongs_center.pose.orientation.x = q1_rot[0]
    tongs_center.pose.orientation.y = q1_rot[1]
    tongs_center.pose.orientation.z = q1_rot[2]
    tongs_center.pose.orientation.w = q1_rot[3]

    ## Distance correction:

    trans1 = tf.translation_matrix((0,0,-a1))
    trans2 = tf.translation_matrix((0,0,-a2))

    rot1 = tf.quaternion_matrix(q1)
    rot2 = tf.quaternion_matrix(q2)

    pos1_trans = tf.translation_matrix((p1.pose.position.x,p1.pose.position.y,p1.pose.position.z))
    pos2_trans = tf.translation_matrix((p2.pose.position.x,p2.pose.position.y,p2.pose.position.z))

    tongs_side_1 = tf.concatenate_matrices(pos1_trans,rot1,trans1)
    tongs_side_2 = tf.concatenate_matrices(pos2_trans,rot2,trans2)
    # tongs_side_1 = tf.concatenate_matrices(trans1,trans1)
    # tongs_side_2 = trans2



    tongs_side_1_trans = tf.translation_from_matrix(tongs_side_1)
    tongs_side_1_quart = tf.quaternion_from_matrix(tongs_side_1)

    tongs_side_2_trans = tf.translation_from_matrix(tongs_side_2)
    tongs_side_2_quart = tf.quaternion_from_matrix(tongs_side_2)

    pose_tongs_side_1 = p1
    pose_tongs_side_2 = p2

    pose_tongs_side_1.pose.position.x = tongs_side_1_trans[0]
    pose_tongs_side_1.pose.position.y = tongs_side_1_trans[1]
    pose_tongs_side_1.pose.position.z = tongs_side_1_trans[2]

    pose_tongs_side_1.pose.orientation.x = tongs_side_1_quart[0]
    pose_tongs_side_1.pose.orientation.y = tongs_side_1_quart[1]
    pose_tongs_side_1.pose.orientation.z = tongs_side_1_quart[2]
    pose_tongs_side_1.pose.orientation.w = tongs_side_1_quart[3]


    pose_tongs_side_2.pose.position.x = tongs_side_2_trans[0]
    pose_tongs_side_2.pose.position.y = tongs_side_2_trans[1]
    pose_tongs_side_2.pose.position.z = tongs_side_2_trans[2]

    pose_tongs_side_2.pose.orientation.x = tongs_side_2_quart[0]
    pose_tongs_side_2.pose.orientation.y = tongs_side_2_quart[1]
    pose_tongs_side_2.pose.orientation.z = tongs_side_2_quart[2]
    pose_tongs_side_2.pose.orientation.w = tongs_side_2_quart[3]

    wid = ((p1.pose.position.x - p2.pose.position.x) ** 2 +
           (p1.pose.position.y - p2.pose.position.y) ** 2 +
           (p1.pose.position.z - p2.pose.position.z) ** 2) ** 0.5

    return tongs_center,wid,pose_tongs_side_1,pose_tongs_side_2
Esempio n. 35
0
    def build_trajectory(self,
                         finish,
                         start=None,
                         ik_space=0.005,
                         duration=None,
                         tot_points=None):
        if start == None:  # if given one pose, use current position as start, else, assume (start, finish)
            start = self.curr_pose()

        #TODO: USE TF TO BASE DISTANCE ON TOOL_FRAME MOVEMENT DISTANCE, NOT WRIST_ROLL_LINK

        # Based upon distance to travel, determine the number of intermediate steps required
        # find linear increments of position.

        dist = pu.calc_dist(start, finish)  #Total distance to travel
        ik_steps = int(round(dist / ik_space) + 1.)
        print "IK Steps: %s" % ik_steps
        if tot_points is None:
            tot_points = 1500 * dist
        if duration is None:
            duration = dist * 120
        ik_fracs = np.linspace(
            0, 1, ik_steps
        )  #A list of fractional positions along course to evaluate ik
        ang_fracs = np.linspace(0, 1, tot_points)

        x_gap = finish.pose.position.x - start.pose.position.x
        y_gap = finish.pose.position.y - start.pose.position.y
        z_gap = finish.pose.position.z - start.pose.position.z

        qs = [
            start.pose.orientation.x, start.pose.orientation.y,
            start.pose.orientation.z, start.pose.orientation.w
        ]
        qf = [
            finish.pose.orientation.x, finish.pose.orientation.y,
            finish.pose.orientation.z, finish.pose.orientation.w
        ]

        #For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
        steps = [PoseStamped() for i in range(len(ik_fracs))]
        for i, frac in enumerate(ik_fracs):
            steps[i].header.stamp = rospy.Time.now()
            steps[i].header.frame_id = start.header.frame_id
            steps[i].pose.position.x = start.pose.position.x + x_gap * frac
            steps[i].pose.position.y = start.pose.position.y + y_gap * frac
            steps[i].pose.position.z = start.pose.position.z + z_gap * frac
            new_q = transformations.quaternion_slerp(qs, qf, frac)
            steps[i].pose.orientation.x = new_q[0]
            steps[i].pose.orientation.y = new_q[1]
            steps[i].pose.orientation.z = new_q[2]
            steps[i].pose.orientation.w = new_q[3]
        rospy.loginfo("Planning straight-line path, please wait")
        self.log_out.publish(data="Planning straight-line path, please wait")

        #For each pose in trajectory, find ik angles
        #Find initial ik for seeding
        req = self.form_ik_request(steps[0])
        ik_goal = self.ik_pose_proxy(req)
        seed = ik_goal.solution.joint_state.position

        ik_points = [[0] * 7 for i in range(len(ik_fracs))]
        for i, p in enumerate(steps):
            request = self.form_ik_request(p)
            request.ik_request.ik_seed_state.joint_state.position = seed
            ik_goal = self.ik_pose_proxy(request)
            ik_points[i] = ik_goal.solution.joint_state.position
            seed = ik_goal.solution.joint_state.position  # seed the next ik w/previous points results
        ik_points = np.array(ik_points)
        # Linearly interpolate angles 10 times between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path.  Used to maintain large number of trajectory points without running IK on every one.
        angle_points = np.zeros((7, tot_points))
        for i in xrange(7):
            angle_points[i, :] = np.interp(ang_fracs, ik_fracs, ik_points[:,
                                                                          i])

        #Build Joint Trajectory from angle positions
        traj = JointTrajectory()
        traj.header.frame_id = steps[0].header.frame_id
        traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
        #print 'angle_points', len(angle_points[0]), angle_points
        for i in range(len(angle_points[0])):
            traj.points.append(JointTrajectoryPoint())
            traj.points[i].positions = angle_points[:, i]
            traj.points[i].velocities = [0] * 7
            traj.points[i].time_from_start = rospy.Duration(ang_fracs[i] *
                                                            duration)
        return traj
Esempio n. 36
0
def holoSolve():

    global holoTransformList
    global inverseholoTransformList
    global markerTransformList
    global holoTransList
    global holoRotList
    global markerTransList
    global markerRotList
    global worldTransform
    global offsetTransform
    global buffersFilled
    global roughCalibrate
    global preCalibration
    global i
    global listFull
    global speechPub
    global wTQuatOut
    global wTPosOut
    global listener

    #ros related variables
    rospy.init_node('optimiseHolo', anonymous=True)
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(100)
    rospy.Subscriber("reCalibrate", String, reCalCB)
    rospy.Subscriber("cameraPosArr", PoseArray, poseArrCB)
    rospy.Subscriber("grip", Pose, gripCB)
    rospy.Subscriber("pointClicked", Int32, clickedCB)
    speechPub = rospy.Publisher('speech', String, queue_size=10)

    #State holding variables
    preCalibration = True
    roughCalibrate = True
    listFull = False
    holoSaved = np.zeros((3))
    i = 0
    lastUp = 0
    sinceLast = 0

    #check if we have a saved callibration
    rospack = rospkg.RosPack()
    packPath = rospack.get_path('hololens_vis')
    try:
        offsetTransform = np.load(packPath + '/calibrate/saved.npy')
        print 'using saved offsetTransform'
        wTQuatOut = np.array([0, 0, 0, 1])
        wTPosOut = np.array([0, 0, 0])
        worldTransform = TR.compose_matrix(
            translate=wTPosOut, angles=TR.euler_from_quaternion(wTQuatOut))
        preCalibration = False
        roughCalibrate = False
        updateOnFirst = True
    except (IOError):
        preCalibration = True
        updateOnFirst = False
        roughCalibrate = True

    while not rospy.is_shutdown():
        try:
            #if preCalibration:
            #print 'precal'
            #else:
            #print 'notCal'
            #get holoLens reported position
            timeMC = listener.getLatestCommonTime('/holoLens', '/holoMC')
            (holoTrans,
             holoRot) = listener.lookupTransform('/mocha_world', '/holoLens',
                                                 timeMC)

            # only when we are doing the precal and rough cal do we need the transforms to be adequately separated.
            if buffersFilled < 1 and preCalibration:
                if LA.norm(np.asarray(holoSaved) -
                           np.asarray(holoTrans)) < 0.005:
                    raise NameError('Too close')
            else:
                if LA.norm(np.asarray(holoSaved) -
                           np.asarray(holoTrans)) < 0.005:
                    raise NameError('Too close')
            holoSaved = holoTrans

            #this is the collection of one sample of the marker position as reported by the motion capture.
            (markerTrans,
             markerRot) = listener.lookupTransform('/mocha_world', '/holoMC',
                                                   timeMC)

            #Produce stack of matricies for use in the optimisation
            holoTranslations[i, :] = holoTrans
            markerTranslations[i, :] = markerTrans
            holoTransformList[i, :, :] = TR.compose_matrix(
                translate=holoTrans, angles=TR.euler_from_quaternion(holoRot))
            inverseholoTransformList[i, :, :] = TR.inverse_matrix(
                TR.compose_matrix(translate=holoTrans,
                                  angles=TR.euler_from_quaternion(holoRot)))
            markerTransformList[i, :, :] = TR.compose_matrix(
                translate=markerTrans,
                angles=TR.euler_from_quaternion(markerRot))
            mostRecentholoInv = inverseholoTransformList[i, :, :]
            mostRecentMarker = markerTransformList[i, :, :]

            #Track where we are in the circular buffers, indicate when the lists are first full.
            i = i + 1
            sinceLast = sinceLast + 1
            i = i % targetLength
            if i == 0:
                listFull = True
                buffersFilled = buffersFilled + 1
                #print 'listfull'

            if updateOnFirst:
                print 'setting agressivly'
                worldTransform = np.dot(mostRecentMarker, offsetTransform)
                worldTransform = np.dot(worldTransform, mostRecentholoInv)
                wTQuatOut = TR.quaternion_from_matrix(worldTransform)
                wTPosOut = TR.translation_from_matrix(worldTransform)

                worldTransform = TR.compose_matrix(
                    translate=wTPosOut,
                    angles=TR.euler_from_quaternion(wTQuatOut))
                updateOnFirst = False

        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException, tf.Exception, NameError):
            continue

        if preCalibration:
            print i
            # are we ready to perform the preCal?
            if listFull:
                print 'svd calibration'
                #make an initial guess at world trans and Rot using SVD matching using the position vectors only.
                (worldRot,
                 worldTrans) = rigid_transform_3D(holoTranslations,
                                                  markerTranslations)
                worldRot4 = np.identity(4)
                for i in range(3):
                    worldRot4[(0, 1, 2), i] = worldRot[:, i]
                worldTransform = TR.compose_matrix(
                    translate=worldTrans,
                    angles=TR.euler_from_matrix(worldRot4))

                #make an initial guess at the marker offset, uses only first value in the buffers.
                findOffset()

                preCalibration = False

        #Here we have calculated the precalibration and will now calculate the error between the transforms using a solver.
        else:
            #full 12 degrees of freedom as marker offset is still not known
            if buffersFilled == 1 and roughCalibrate:
                print '12DoF calibration'
                roughCalibrate = False
                # initial value for solver. 12 values representing the worldTrans, worldRot, markerOffset, markerRot
                Xinit = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])

                # The main solving function. # should return near 0 in functional steady state.
                result = least_squares(costFun,
                                       Xinit,
                                       method='lm',
                                       jac='3-point',
                                       verbose=0)
                print result.x
                #update our guesses with the Delta that we optimisted.
                updateStoredTransforms(result.x)
                print 'offsetTransform'
                print offsetTransform
                speechPub.publish('Finished Calibration')
                np.save(packPath + '/calibrate/newest.npy', offsetTransform)
                wTQuatOut = TR.quaternion_from_matrix(worldTransform)
                wTPosOut = TR.translation_from_matrix(worldTransform)
            #Only 6 degrees of freedom as only the world offset might drift.
            else:
                # transmit the one shot calculated world transform assuming offset Transform remains correct.
                if lastUp != i and sinceLast > 1:
                    sinceLast = 0
                    lastUp = i
                    #Xinit = np.array([0,0,0,0,0,0])

                    # The main solving function. # should return near 0 in functional steady state.
                    #result  = least_squares(costFun6,Xinit, method = 'lm', jac = '3-point',verbose = 0)
                    #print result.x
                    #update our guesses with the Delta that we optimisted.
                    #updateStoredTransforms6(result.x)
                    worldTransform = np.dot(mostRecentMarker, offsetTransform)
                    worldTransform = np.dot(worldTransform, mostRecentholoInv)
                    ##make an IIR filter for the transform.
                    wTQuatIn = TR.quaternion_from_matrix(worldTransform)
                    wTQuatOut = TR.quaternion_slerp(wTQuatOut, wTQuatIn, 0.01)
                    wTPosIn = TR.translation_from_matrix(worldTransform)
                    wTPosOut = 0.99 * wTPosOut + 0.01 * wTPosIn
                    #wTQuatOut = wTQuatIn;
                    #wtPosOut = wTPosOut;

                    #wTQuatOut = wTQuatOut / LA.norm(wTQuatOut)
                    worldTransform = TR.compose_matrix(
                        translate=wTPosOut,
                        angles=TR.euler_from_quaternion(wTQuatOut))

            #send transforms for visualisation.
            br.sendTransform(TR.translation_from_matrix(worldTransform),
                             TR.quaternion_from_matrix(worldTransform), timeMC,
                             'hlInMC', 'mocha_world')
            br.sendTransform(TR.translation_from_matrix(offsetTransform),
                             TR.quaternion_from_matrix(offsetTransform),
                             rospy.Time.now(), 'OCmark', 'holoMC')
            br.sendTransform(
                holoTrans,
                holoRot,
                #TR.quaternion_inverse(holoRot2),
                timeMC,
                'OCholo',
                'hlInMC')
        try:
            rate.sleep()
        except:
            continue
Esempio n. 37
0
    def build_trajectory(self, finish, start=None, ik_space = 0.005, duration = None, tot_points=None):
        if start == None: # if given one pose, use current position as start, else, assume (start, finish)
            start = self.curr_pose()
       
        #TODO: USE TF TO BASE DISTANCE ON TOOL_FRAME MOVEMENT DISTANCE, NOT WRIST_ROLL_LINK

        # Based upon distance to travel, determine the number of intermediate steps required
        # find linear increments of position.

        dist = self.calc_dist(start,finish)     #Total distance to travel
        ik_steps = math.ceil(dist/ik_space)     
        print "IK Steps: %s" %ik_steps
        if tot_points is None:
           tot_points = 1500*dist
        if duration is None:
            duration = dist*120
        ik_fracs = np.linspace(0, 1, ik_steps)   #A list of fractional positions along course to evaluate ik
        ang_fracs = np.linspace(0,1, tot_points)  

        x_gap = finish.pose.position.x - start.pose.position.x
        y_gap = finish.pose.position.y - start.pose.position.y
        z_gap = finish.pose.position.z - start.pose.position.z

        qs = [start.pose.orientation.x, start.pose.orientation.y,
              start.pose.orientation.z, start.pose.orientation.w] 
        qf = [finish.pose.orientation.x, finish.pose.orientation.y,
              finish.pose.orientation.z, finish.pose.orientation.w] 

        #For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
        steps = [PoseStamped() for i in range(len(ik_fracs))]
        for i,frac in enumerate(ik_fracs):
            steps[i].header.stamp = rospy.Time.now()
            steps[i].header.frame_id = start.header.frame_id
            steps[i].pose.position.x = start.pose.position.x + x_gap*frac
            steps[i].pose.position.y = start.pose.position.y + y_gap*frac
            steps[i].pose.position.z = start.pose.position.z + z_gap*frac
            new_q = transformations.quaternion_slerp(qs,qf,frac)
            steps[i].pose.orientation.x = new_q[0]
            steps[i].pose.orientation.y = new_q[1]
            steps[i].pose.orientation.z = new_q[2]
            steps[i].pose.orientation.w = new_q[3]
        rospy.loginfo("Planning straight-line path, please wait")
        self.wt_log_out.publish(data="Planning straight-line path, please wait")
       
        #For each pose in trajectory, find ik angles
        #Find initial ik for seeding
        req = self.form_ik_request(steps[0])
        ik_goal = self.ik_pose_proxy(req)
        seed = ik_goal.solution.joint_state.position

        ik_points = [[0]*7 for i in range(len(ik_fracs))]
        for i, p in enumerate(steps):
            request = self.form_ik_request(p)
            request.ik_request.ik_seed_state.joint_state.position = seed
            ik_goal = self.ik_pose_proxy(request)
            ik_points[i] = ik_goal.solution.joint_state.position
            seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
        ik_points = np.array(ik_points)
        # Linearly interpolate angles 10 times between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path.  Used to maintain large number of trajectory points without running IK on every one.    
        angle_points = np.zeros((7, tot_points))
        for i in xrange(7):
            angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])

        #Build Joint Trajectory from angle positions
        traj = JointTrajectory()
        traj.header.frame_id = steps[0].header.frame_id
        traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
        #print 'angle_points', len(angle_points[0]), angle_points
        for i in range(len(angle_points[0])):
            traj.points.append(JointTrajectoryPoint())
            traj.points[i].positions = angle_points[:,i]
            traj.points[i].velocities = [0]*7
            traj.points[i].time_from_start = rospy.Duration(ang_fracs[i]*duration)
        return traj
Esempio n. 38
0
    def execute_cb(self, goal):
        self.goal = goal
        self._result.reached = False
        # How fast will we update the robot's movement?
        dist_vec = Vector3()
        trans, quat = self.get_odom()
        dist_vec.x = self.goal.pose.position.x - trans[0]
        dist_vec.y = self.goal.pose.position.y - trans[1]
        # the goal contains a quat already turned, so this with end up with the error between current and goal
        # dist_vec.z will be the euler angle theta of the error to goal
        quat0 = [
            self.goal.pose.orientation.w, self.goal.pose.orientation.x,
            self.goal.pose.orientation.y, self.goal.pose.orientation.z
        ]
        dist_vec.z = tftr.euler_from_quaternion(
            tftr.quaternion_slerp(quat0, quat, fraction=1))[2]

        print tftr.euler_from_quaternion(quat0)
        print tftr.euler_from_quaternion(
            tftr.quaternion_slerp(quat0, quat, fraction=1))
        print('goal z', dist_vec.z)
        distance = sqrt(dist_vec.x**2 + dist_vec.y**2)

        while (distance -
               self.linear_tolerance) > 0.0 and not rospy.is_shutdown():
            dist_vec = self.go_straight(dist_vec)
            distance = sqrt(dist_vec.x**2 + dist_vec.y**2)
            self._feedback.vector = dist_vec
            self._as.publish_feedback(self._feedback)
            # rospy.loginfo('%s: Moving, distance to goal %f ' % (self._action_name, distance))

        # Stop the robot before the rotation
        cmd_vel = Twist()
        self.cmd_vel_pub.publish(cmd_vel)
        rospy.sleep(3)

        # Now start turning
        trans, quat = self.get_odom()
        quat0 = [
            self.goal.pose.orientation.w, self.goal.pose.orientation.x,
            self.goal.pose.orientation.y, self.goal.pose.orientation.z
        ]

        # update the orientation again to make sure you have the latest value after the straight action
        dist_vec.z = tftr.euler_from_quaternion(
            tftr.quaternion_slerp(quat0, quat, fraction=1))[2]

        while (dist_vec.z -
               self.angular_tolerance) > 0.0 and not rospy.is_shutdown():
            dist_vec = self.turn_in_place(dist_vec)
            self._feedback.vector = dist_vec
            self._as.publish_feedback(self._feedback)
            rospy.loginfo('%s: Truning to goal %f' %
                          (self._action_name, self._feedback.x,
                           self._feedback.y, self._feedback.z))

        # Goal should be reached after the operations but this is not helpful
        self._result.reached = True
        trans, quat = self.get_odom()
        pose = Pose()
        pose.position = Point(*trans)
        pose.orientation = Quaternion(*quat)

        self._result.pose = pose
        rospy.loginfo('%s: Succeeded' % self._action_name)
        self._as.set_succeeded(self._result)
Esempio n. 39
0
    def build_trajectory(self, finish, start=None, ik_space=0.10,
                        duration=None, tot_points=None):
        if start == None: # if given one pose, use current position as start, else, assume (start, finish)
            start = self.curr_pose()
            print "start fro curr_pos"
            print start
       
        #TODO: USE TF TO BASE DISTANCE ON TOOL_FRAME MOVEMENT DISTANCE, NOT WRIST_ROLL_LINK

        # Based upon distance to travel, determine the number of intermediate steps required
        # find linear increments of position.

        print "finish pose"
        print finish
        dist = self.calc_dist(start, finish)     #Total distance to travel
        ik_steps = int(round(dist/ik_space)+1.)   
        rospy.loginfo("IK Steps: %s  Distance: %s " %(ik_steps, dist))
        if tot_points is None:
           tot_points = 1500*dist
        if duration is None:
            duration = dist*120
       
        ik_fracs = np.linspace(0, 1, ik_steps) #A list of fractional positions along course to evaluate ik
        tot_points = ik_steps  # ARD
        ang_fracs = np.linspace(0,1, tot_points)  

        seed = None
        x_gap = finish.pose.position.x - start.pose.position.x
        y_gap = finish.pose.position.y - start.pose.position.y
        z_gap = finish.pose.position.z - start.pose.position.z
        print "Arm gaps: x ", x_gap, " y ", y_gap, " z ", z_gap

        qs = [start.pose.orientation.x, start.pose.orientation.y,
              start.pose.orientation.z, start.pose.orientation.w] 
        qf = [finish.pose.orientation.x, finish.pose.orientation.y,
              finish.pose.orientation.z, finish.pose.orientation.w] 

        #For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
        steps = [PoseStamped() for i in range(len(ik_fracs))]
        for i,frac in enumerate(ik_fracs):
            steps[i].header.stamp = rospy.Time.now()
            steps[i].header.frame_id = start.header.frame_id
            steps[i].pose.position.x = start.pose.position.x + x_gap*frac
            steps[i].pose.position.y = start.pose.position.y + y_gap*frac
            # steps[i].pose.position.z = start.pose.position.z + z_gap*frac 
            # ARD: Chess hack for torso height.  Really should fix time on tf?
            steps[i].pose.position.z = start.pose.position.z + z_gap*frac - .3
            # steps[i].pose.position.z = start.pose.position.z + z_gap*frac
            new_q = transformations.quaternion_slerp(qs,qf,frac)
            steps[i].pose.orientation.x = new_q[0]
            steps[i].pose.orientation.y = new_q[1]
            steps[i].pose.orientation.z = new_q[2]
            steps[i].pose.orientation.w = new_q[3]
        rospy.loginfo("Planning straight-line path, please wait")
        self.log_out.publish(data="Planning straight-line path, please wait")
        rospy.loginfo("frameid %s %s" % (start.header.frame_id, finish.header.frame_id))
       
        #Find initial ik for seeding
        req = self.form_constraint_aware_ik_request(steps[0])
        ik_goal = self.ik_constraint_aware_pose_proxy(req)
        print "IK goal"
        print req
        # print req.pose_stamped.pose.position
        # print req.ik_seed_state.name
        # print req.ik_seed_state.position
        for i, name in enumerate(ik_goal.solution.joint_state.name):
           rospy.loginfo("traj joints %s" % name)
           if name == 'left_upper_arm_hinge_joint' or name == 'right_upper_arm_hinge_joint':
             hinge_index = i
           elif name == 'left_shoulder_tilt_joint' or name == 'right_shoulder_tilt_joint':
             shoulder_tilt_index = i

        if len(ik_goal.solution.joint_state.position) == 0:
          print "initial seeding failed"
          seed = None
        #ik_points = list([[0]*7 for i in range(len(ik_fracs))])
        ik_points = list()
        ik_fracs2 = list()
        for i, p in enumerate(steps):
            if (i == 0):
              continue
            #request = self.form_ik_request(p)
            request = self.form_constraint_aware_ik_request(p)
            if seed != None:
              request.ik_request.ik_seed_state.joint_state.position = seed
              request.ik_request.ik_seed_state.joint_state.name = ik_goal.solution.joint_state.name
            ik_goal = self.ik_constraint_aware_pose_proxy(request)
            print "IK goal %d" %(i)
            print request
            # print request.pose_stamped.pose.position
            # print request.ik_seed_state.name
            # print request.ik_seed_state.position
            if len(ik_goal.solution.joint_state.position) != 0:
              seed = list(ik_goal.solution.joint_state.position) # seed the next ik w/previous points results
              seed[hinge_index] = -1*seed[shoulder_tilt_index]
              ik_points.append(ik_goal.solution.joint_state.position)
              ik_fracs2.append(ik_fracs[i])
              print "IK solution for %d" % i
            else:
              print "No solution for %d" % i

        rospy.loginfo("linear interp")
        ik_fracs = ik_fracs2
        if len(ik_points) == 0:
          return None

        ik_points = np.array(ik_points)
        # Linearly interpolate angles 10 times between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path.  Used to maintain large number of trajectory points without running IK on every one.    
        angle_points = np.zeros((7, tot_points))
        for i in xrange(7):
            angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])

        #Build Joint Trajectory from angle positions
        traj = JointTrajectory()
        traj.header.frame_id = steps[0].header.frame_id
        traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
        for name in traj.joint_names:
          rospy.loginfo("traj joints %s" % name)
        #print 'angle_points', len(angle_points[0]), angle_points
        for i in range(len(angle_points[0])):
            traj.points.append(JointTrajectoryPoint())
            traj.points[i].positions = angle_points[:,i]
            traj.points[i].velocities = [0]*7
            traj.points[i].time_from_start = rospy.Duration(
                                                ang_fracs[i]*duration)
        return traj
Esempio n. 40
0
def despsi_fun(goal, t_goal, q0, t_nowf):
    if t_nowf < t_goal:
        des = tft.quaternion_slerp(q0, goal, t_nowf / t_goal)
    else:
        des = goal
    return des
Esempio n. 41
0
 def test_slerp2(self, q1, q2, t):
     r1 = np.array([float(x.evalf(real=True)) for x in spw.diffable_slerp(spw.Matrix(q1), spw.Matrix(q2), t)])
     r2 = quaternion_slerp(q1, q2, t)
     self.assertTrue(np.isclose(r1, r2, atol=1e-3).all() or
                     np.isclose(r1, -r2, atol=1e-3).all(),
                     msg='q1={} q2={} t={}\n{} != {}'.format(q1, q2, t, r1, r2))
Esempio n. 42
0
    def wipe(self, start, finish):
            dist = int(round(200*self.calc_dist(start, finish))) # 1 point per cm of separation
            print "Steps: %s" %dist
            x_step = finish.pose.position.x - start.pose.position.x
            y_step = finish.pose.position.y - start.pose.position.y
            z_step = finish.pose.position.z - start.pose.position.z
            #print "Increments: %s,%s,%s" %(x_step, y_step, z_step)

            qs = [start.pose.orientation.x, start.pose.orientation.y, start.pose.orientation.z, start.pose.orientation.w] 
            qf = [finish.pose.orientation.x, finish.pose.orientation.y, finish.pose.orientation.z, finish.pose.orientation.w] 
            steps = []
            #print "Start: %s" %start
            #print "Finish: %s" %finish
            for i in range(dist):
                frac = float(i)/float(dist)
                steps.append(PoseStamped())
                steps[i].header.stamp = rospy.Time.now()
                steps[i].header.frame_id = start.header.frame_id
                steps[i].pose.position.x = start.pose.position.x + x_step*frac
                steps[i].pose.position.y = start.pose.position.y + y_step*frac
                steps[i].pose.position.z = start.pose.position.z + z_step*frac
                new_q = transformations.quaternion_slerp(qs,qf,frac)
                steps[i].pose.orientation.x = new_q[0]
                steps[i].pose.orientation.y = new_q[1]
                steps[i].pose.orientation.z = new_q[2]
                steps[i].pose.orientation.w = new_q[3]
            steps.append(finish)
            #print "Steps:"
            #print steps
            #raw_input("Press Enter to continue")
            rospy.loginfo("Planning straight-line path, please wait")
            self.wt_log_out.publish(data="Planning straight-line path, please wait")
           
            rospy.loginfo("Initiating wipe action")
            self.blind_move(finish)
            self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
            rospy.loginfo("At beginning of path")
            pts = []
            
            for i, p in enumerate(steps):
                frac = float(i)/float(len(steps))
                request = self.form_ik_request(p)
                if not i == 0:
                    request.ik_request.ik_seed_state.joint_state.position = seed
                ik_goal = self.ik_pose_proxy(request)
                pts.append(ik_goal.solution.joint_state.position)
                seed = pts[i]

                
            points = []    
            for i in range(len(pts)-1):
                angs1 = pts[i]
                angs2 = pts[i+1]
                increm = np.subtract(angs2, angs1) 
                for j in range(10):
                    points.append(np.add(angs1, np.multiply(0.1*j, increm)))
            
            #points = np.unwrap(points,1)
            p1= points
            traj = JointTrajectory()
            traj.header.frame_id = steps[0].header.frame_id
            traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
            times = []
            for i in range(len(points)):
                frac = float(i)/float(len(points))
                traj.points.append(JointTrajectoryPoint())
                traj.points[i].positions = points[i]
                traj.points[i].velocities = [0]*7
                times.append(rospy.Duration(frac*dist*0.2))

            traj_goal = FollowJointTrajectoryGoal()
            traj_goal.trajectory = traj
            tolerance = JointTolerance()
            tolerance.position = 0.05
            tolerance.velocity = 0.1
            traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
            traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
            traj_goal.goal_time_tolerance = rospy.Duration(3)

            #print "Steps: %s" %steps
            count = 0

            while count < 6:
                traj_goal.trajectory.points.reverse()
                for i in range(len(times)):
                    traj_goal.trajectory.points[i].time_from_start = times[i]
                if count == 0:
                    print traj_goal.trajectory.points
                    raw_input("Review Trajectory Goal")
                traj_goal.trajectory.header.stamp = rospy.Time.now()
                self.r_arm_follow_traj_client.send_goal(traj_goal)
                self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
                rospy.sleep(0.5)
                count += 1
            
            
            #traj_goal = JointTrajectoryGoal()
            #traj_goal.trajectory = traj
            #print "Steps: %s" %steps
            #count = 0
#
            #while count < 6:
                #traj_goal.trajectory.points.reverse()
                #for i in range(len(times)):
                    #traj_goal.trajectory.points[i].time_from_start = times[i]
                #print traj_goal
                #raw_input("Review Trajectory Goal")
                ##print "Traj goal start:"
                ##print traj_goal.trajectory.points[0].positions
                ##print "Traj goal end:"
                ##print traj_goal.trajectory.points[-1].positions
                #traj_goal.trajectory.header.stamp = rospy.Time.now()
                #self.r_arm_traj_client.send_goal(traj_goal)
                #self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
                #rospy.sleep(0.5)
                #count += 1
            rospy.loginfo("Done Wiping")
            self.wt_log_out.publish(data="Done Wiping")
            self.linear_move(Float32(-0.15))
Esempio n. 43
0
def updateAngular(quat1, var1, quat2, var2):
    newMean = quaternion_slerp(quat1, quat2, var1/(var1+var2))
    newVar = 1.0 / (1.0/var1 + 1.0/var2)
    return [newMean, newVar]