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
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))
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
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
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
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)
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)
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)
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()
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)
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
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
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)
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))
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
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
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])
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)
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
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]
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)
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
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)
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
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
def update(self, now, accel, gyro, quat): self.accel = accel self.gyro = gyro w, x, y, z = quat # w, x, y, z = -w, y, x, z w, x, y, z = w, -y, x, z quat = w, x, y, z # quat = t.quaternion_multiply(t.quaternion_about_axis(math.pi, [0, 0, 1]), quat) self.quat_absolute = quat # create relative quat, based on our absolute quaternion and our parent's absolute quaternion self.quat_relative = (t.quaternion_multiply( self.quat_absolute, t.quaternion_inverse( self.parent.quat_absolute)) if self.parent else self.quat_absolute) self.quat_relative = t.quaternion_multiply( self.quat_relative, t.quaternion_about_axis(math.pi, [1, 0, 0])) # euler = t.euler_from_quaternion(quat_relative, axes='syzx') # quat_relative = t.quaternion_from_euler(euler[0], euler[1], -euler[2], axes='sxyz') # self.quat_relative = t.quaternion_multiply(quat_relative, t.quaternion_inverse(self.parent.quat_relative)) if self.lastUpdatedAt and self.relative_average_rate: dt = (now - self.lastUpdatedAt).to_sec() f = dt * self.relative_average_rate self.quat_average = self.quat_relative \ if self.quat_average is None \ else t.quaternion_slerp(self.quat_average, self.quat_relative, f) self.quat_relative_to_average = t.quaternion_multiply( self.quat_relative, t.quaternion_inverse(self.quat_average)) else: self.quat_average = self.quat_relative self.quat_relative_to_average = self.quat_relative self.lastUpdatedAt = now
def 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
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
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
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
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
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)
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
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
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))
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))
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]