def main(): rospy.init_node('test', anonymous=True) raw_data = np.load('record_demo.npy') # ipdb.set_trace() filtered_data = gaussian_filter1d(raw_data.T, sigma=5).T # filtered_data = util.filter_static_points(filtered_data,0.03) marker_pub = rospy.Publisher("/iiwa/visualization_marker", Marker, queue_size=100) rospy.sleep(0.5) rgba_tuple = [ random.uniform(0, 1), random.uniform(0, 1), random.uniform(0.5, 1), 1 ] for idx, point in enumerate(filtered_data): # ipdb.set_trace() now_pose = Pose() now_pose.position.x = point[0] now_pose.position.y = point[1] now_pose.position.z = point[2] util.send_traj_point_marker(marker_pub=marker_pub, pose=now_pose, id=idx, rgba_tuple=rgba_tuple)
def main(): rospy.init_node('test', anonymous=True) # raw_data = np.load('record_demo.npy') raw_data = np.load('./demonstrations/record_demo_1_.npy') # ipdb.set_trace() filtered_data = gaussian_filter1d(raw_data.T, sigma=5).T # filtered_data = util.filter_static_points(filtered_data,0.03) marker_pub = rospy.Publisher("/iiwa/visualization_marker", Marker, queue_size=100) rospy.sleep(0.5) rgba_tuple = [ random.uniform(0, 1), random.uniform(0, 1), random.uniform(0.5, 1), 1 ] now_pose = Pose() # buffer pose #calculate mean point mean_point = [0.0, 0.0, 0.0] #x,y,z for idx, point in enumerate(filtered_data): mean_point[0] += point[0] mean_point[1] += point[1] mean_point[2] += point[2] for i in range(len(mean_point)): mean_point[i] = mean_point[i] / len(filtered_data) #show meanpoint rviz with marker now_pose.position.x = mean_point[0] now_pose.position.y = mean_point[1] now_pose.position.z = mean_point[2] util.send_traj_point_marker(marker_pub=marker_pub, pose=now_pose, id=1000, rgba_tuple=[0.1, 0.1, 0.1, 1]) # calculate offset traj on zero center point filtered_data_zero = filtered_data for idx, point in enumerate(filtered_data): filtered_data_zero[idx][:3] = point[:3] - mean_point #display offset traj on zero center point for idx, point in enumerate(filtered_data_zero): # ipdb.set_trace() now_pose.position.x = point[0] #+mean_point[0] now_pose.position.y = point[1] #+mean_point[1] now_pose.position.z = point[2] #+mean_point[2] util.send_traj_point_marker(marker_pub=marker_pub, pose=now_pose, id=idx, rgba_tuple=rgba_tuple)
def main(): # moveit ros moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cartesian_move_test', anonymous=True) robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander('arm') scene = moveit_commander.PlanningSceneInterface() planning_frame = group.get_planning_frame() print "============ Reference frame: %s" % planning_frame eef_link = group.get_end_effector_link() print "============ End effector: %s" % eef_link group_names = robot.get_group_names() print "============ Robot Groups:", robot.get_group_names() print "============ Printing robot state" print robot.get_current_state() print "" print(group.get_current_pose()) joint_goal = group.get_current_joint_values() joint_goal = [0, 0, 0, 0, 0, 0, 0] group.go(joint_goal, wait=True) group.stop() raw_data = np.load( '/home/hayashi/worksp/tomato_ws/src/tomato_ws/handtracking/src/handtracking/hand_pub.npy') print(raw_data, raw_data.shape) filtered_data = gaussian_filter1d(raw_data.T, sigma=15).T filtered_data = util.filter_static_points(filtered_data) marker_pub = rospy.Publisher( "/visualization_marker", Marker, queue_size=100) rospy.sleep(0.5) rgba_tuple = [random.uniform(0.5, 1), random.uniform( 0.5, 1), random.uniform(0.5, 1), 1] display('publish visualization marker') print(filtered_data, raw_data.shape) # convert to world waypoints = [] for idx, point in enumerate(filtered_data): # ipdb.set_trace() now_pose = Pose() now_pose.position.x = point[0] now_pose.position.y = point[1] now_pose.position.z = point[2] pos = now_pose.position ori = now_pose.orientation mat = np.dot(translation_matrix((pos.x, pos.y, pos.z)), quaternion_matrix((ori.x, ori.y, ori.z, ori.w))) transform_mat = np.dot(translation_matrix( (0.077, 0.035, 0.188)), quaternion_matrix((-0.500, 0.500, -0.500, 0.500))) new_mat = np.dot(transform_mat, mat) new_trans = translation_from_matrix(new_mat) new_quat = quaternion_from_matrix(new_mat) new_pose = Pose() new_pose.position.x = new_trans[0] new_pose.position.y = new_trans[1] new_pose.position.z = new_trans[2] new_pose.orientation.x = 2.6944e-5 new_pose.orientation.y = 0.70713 new_pose.orientation.z = 2.5173e-5 new_pose.orientation.w = 0.70708 util.send_traj_point_marker( marker_pub=marker_pub, frameid="/world", pose=new_pose, id=idx+300, rgba_tuple=rgba_tuple) waypoints.append(copy.deepcopy(new_pose)) group.set_max_velocity_scaling_factor(0.01) for i in range (3): (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.005, # eef_step 0.0) # jump_threshold # with open ('plan_handreplay.txt', 'w') as f: f.write (str(plan)) group.execute(plan, wait=True) print group.get_current_pose() group.stop() group.clear_pose_targets() # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
dmp = DMPs_discrete(n_dmps=filtered_data.shape[1], n_bfs=200) dmp.imitate_path(y_des=filtered_data.T) # dmp.y0[2] = dmp.y0[2] # dmp.goal[2] = dmp.goal[2] - 0.1 # dmp.goal[1] = dmp.goal[1]+ 0.1 y_track_d, dy_track_d, ddy_track_d = dmp.rollout() marker_pub = rospy.Publisher("/visualization_marker", Marker, queue_size=100) rospy.sleep(0.5) rgba_tuple = [random.uniform(0, 1), random.uniform(0, 1), random.uniform(0.5, 1), 1] for idx, point in enumerate(y_track_d): now_pose = Pose() now_pose.position.x = point[0] now_pose.position.y = point[1] now_pose.position.z = point[2] util.send_traj_point_marker(marker_pub=marker_pub, pose=now_pose, id=idx+400, rgba_tuple=rgba_tuple) # moveit moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander('manipulator') reference_frame = 'world' group.set_pose_reference_frame(reference_frame) plan = util.ik_cartesain_path(group, y_track_d) util.execute_plan(group,plan) # print "demonstration" # print raw_data[-1] # print "DMP" # print group.get_current_pose().pose
def main(): rospy.init_node('test', anonymous=True) # raw_data = np.load('record_demo.npy') # raw_data = np.load('./demonstrations/record_demo_1_.npy') raw_data = np.load( '/home/hayashi/worksp/tomato_ws/src/tomato_ws/handtracking/src/handtracking/hand_pub.npy' ) print(raw_data, raw_data.shape) # ipdb.set_trace() filtered_data = gaussian_filter1d(raw_data.T, sigma=15).T # filtered_data = raw_data filtered_data = util.filter_static_points(filtered_data) marker_pub = rospy.Publisher("/visualization_marker", Marker, queue_size=100) rospy.sleep(0.5) rgba_tuple = [ random.uniform(0.5, 1), random.uniform(0.5, 1), random.uniform(0.5, 1), 1 ] display('publish visualization marker') print(filtered_data, raw_data.shape) #convert to world for idx, point in enumerate(filtered_data): # ipdb.set_trace() now_pose = Pose() now_pose.position.x = point[0] now_pose.position.y = point[1] now_pose.position.z = point[2] now_pose.orientation.x = point[3] now_pose.orientation.y = point[4] now_pose.orientation.z = point[5] now_pose.orientation.w = point[6] # util.send_traj_point_marker(marker_pub=marker_pub,frameid="/camera_color_optical_frame", pose=now_pose, id=idx, rgba_tuple=rgba_tuple) pos = now_pose.position ori = now_pose.orientation mat = np.dot(translation_matrix((pos.x, pos.y, pos.z)), quaternion_matrix((ori.x, ori.y, ori.z, ori.w))) transform_mat = np.dot( translation_matrix((0.077, 0.035, 0.188)), quaternion_matrix((-0.500, 0.500, -0.500, 0.500))) new_mat = np.dot(transform_mat, mat) new_trans = translation_from_matrix(new_mat) new_quat = quaternion_from_matrix(new_mat) new_pose = Pose() new_pose.position.x = new_trans[0] new_pose.position.y = new_trans[1] new_pose.position.z = new_trans[2] new_pose.orientation.x = new_quat[0] new_pose.orientation.y = new_quat[1] new_pose.orientation.z = new_quat[2] new_pose.orientation.w = new_quat[3] util.send_traj_point_marker(marker_pub=marker_pub, frameid="/world", pose=new_pose, id=idx + 300, rgba_tuple=rgba_tuple)