def plan(self): dt = DisplayTrajectory() dt.trajectory_start = self.robot.get_current_state() plan = self.group.plan() dt.trajectory = [plan] self.dt_pub.publish(dt) return plan
def visualize_movement(start, path): display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', DisplayTrajectory, queue_size=100) # for visualizing the robot movement; sleep(0.5) display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = convertStateToRobotState(start) trajectory = RobotTrajectory() points = [] joint_trajectory = JointTrajectory() joint_trajectory.joint_names = get_joint_names('right') for state, _ in path: point = JointTrajectoryPoint() point.positions = convertStateToRobotState(state).joint_state.position points.append(point) joint_trajectory.points = points trajectory.joint_trajectory = joint_trajectory # print("The joint trajectory is: %s" % trajectory) display_trajectory.trajectory.append(trajectory) display_trajectory_publisher.publish(display_trajectory)
def displayTrajFromPlan(self, plan, joint_names, initial_state): """Given a plan (GetDMPPlanResponse), joint_names list and an initial_state as RobotState Create a displayTraj message""" dt = DisplayTrajectory() dt.trajectory.append( self.robotTrajectoryFromPlan(plan, joint_names) ) dt.trajectory_start = self.robotStateFromJoints(joint_names, initial_state) dt.model_id = "reem" return dt
def visualize_plan(self, plan): # Untested """ Visualize the plan in RViz """ display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = plan.points[0] display_trajectory.trajectory.extend(plan.points) self.display_planned_path_publisher.publish(display_trajectory)
def get_display_trajectory(self, *joint_trajectories): display_trajectory = DisplayTrajectory() display_trajectory.model_id = 'pr2' for joint_trajectory in joint_trajectories: robot_trajectory = RobotTrajectory() robot_trajectory.joint_trajectory = joint_trajectory # robot_trajectory.multi_dof_joint_trajectory = ... display_trajectory.trajectory.append(robot_trajectory) display_trajectory.trajectory_start = self.get_robot_state() return display_trajectory
def publish_trajectory(self, plan): # type: (RobotTrajectory) -> None """ Publish trajectory so that we can visualize in Rviz :param plan: :return: None """ display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.get_robot_state() display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory)
def visualize_path(self, path): ''' path will be a list of grid cells ''' current_state = self.robot.get_current_state() display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = current_state path_config = [self._grid_to_continuous(cell) for cell in path] display_trajectory.trajectory.append(self.get_robot_trajectory_msg( path_config)) self.display_trajectory_publisher.publish(display_trajectory)
def _visualize_plan(self, data_store): # Pull out the plan from the data full_plan = [x.plan for x in data_store[self.PLAN_OBJ_KEY]] plan = self._merge_plans(full_plan) display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.arm_planner.robot.get_current_state( ) display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory) return plan
def moveArmCartesian(self, config): step = 0.01 attempts = 1 waypoints = [] pose_goal = self.group.get_current_pose().pose waypoints.append(pose_goal) # current pose pose_goal.orientation = geometry_msgs.msg.Quaternion( *tf_conversions.transformations.quaternion_from_euler( 0., -math.pi / 2, 0.)) pose_goal.position.x = config.x pose_goal.position.y = config.y pose_goal.position.z = config.z waypoints.append(copy.deepcopy(pose_goal)) # goal pose print "- Planning Move..." # create cartesian plan based on current and goal pose # compute_cartesian_path: Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. # Configurations are computed for every eef_step meters; # The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resulting path. # The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. # avoid_collision = True (plan, fraction) = self.group.compute_cartesian_path(waypoints, step, 0.0) rospy.sleep(0.5) print "%% success:", fraction while (fraction < 0.8) and ( attempts <= 4 ): # force a somewhat successful plan by adding more intermediary points attempts += 1 step = step / 2 print "%% success:", fraction (plan, fraction) = self.group.compute_cartesian_path( waypoints, step, 0.0) rospy.sleep(0.5) # check if the move was successful if fraction > 0.8: print "Success.-" display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state( ) display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory) rospy.sleep(1) print "- Executing..." self.group.execute(plan, wait=True) rospy.sleep(1) self.reachable = True else: print "Fail.-" self.reachable = False
def execute(self, traj, wait=True, display=True): """ Execute the trajectory on the robot arm """ if traj: rospy.loginfo("Executing trajectory") if display: # Display the planned trajectory display_traj = DisplayTrajectory() display_traj.trajectory_start = self.robot.get_current_state() display_traj.trajectory.append(traj) self.display_trajectory_pub.publish(display_traj) # Execute the trajectory self.arm_group.execute(traj, wait=wait)
def plan(samplerIndex, start, goal, ss, space, display_trajectory_publisher): si = ss.getSpaceInformation() if samplerIndex == 1: # use obstacle-based sampling space.setStateSamplerAllocator( ob.StateSamplerAllocator(allocSelfCollisionFreeStateSampler)) ss.setStartAndGoalStates(start, goal) # create a planner for the defined space planner = og.FMT(si) # change this to FMT; if samplerIndex == 1: planner.setVAEFMT( 1) # This flag is for turning on sampling with the VAE generator; # set parameter; planner.setExtendedFMT( False) # do not extend if the planner does not terminate; planner.setNumSamples(100) planner.setNearestK(False) planner.setCacheCC(True) planner.setHeuristics(True) # planner.setNearestK(1) # Disable K nearest neighbor implementation; ss.setPlanner(planner) start_time = time.time() solved = ss.solve(40.0) elapsed_time = time.time() - start_time if solved: print("Found solution after %s seconds:" % elapsed_time) # print the path to screen path = ss.getSolutionPath() # ("The solution is: %s" % path) # Visualization display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = convertStateToRobotState(start) trajectory = convertPlanToTrajectory(path) display_trajectory.trajectory.append(trajectory) display_trajectory_publisher.publish(display_trajectory) print("Visualizing trajectory...") sleep(0.5) else: print("No solution found after %s seconds: " % elapsed_time) return elapsed_time, float((int(solved)))
def handle_show_request(self, req): display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() #build moveit msg for display joint_multi_traj_msg = MultiDOFJointTrajectory() robot_traj_msg = RobotTrajectory() robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg display_trajectory.trajectory.append(robot_traj_msg) rospy.loginfo("PlanningService::handle_show_request() -- showing trajectory %s", req.JOINT_TRAJECTORY) self.display_trajectory_publisher.publish(display_trajectory); return True
def plan_trajectory_in_cspace(self, goal, visualization=True): ## Then, we will get the current set of joint values for the group self.group.set_joint_value_target(goal) plan = self.group.plan() if visualization: print "============ Visualizing trajectory_plan" display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state( ) display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory) print "============ Waiting while plan is visualized (again)..." rospy.sleep(5) return plan
def execute(self): rospy.loginfo("Start Task") # Get latest task plan plan = self.task_q[0] for points in plan.trajectory.joint_trajectory.points: tfs = points.time_from_start.to_sec() tfs /= self.exe_speed_rate points.time_from_start = rospy.Duration(tfs) scaled_vel = [] scaled_acc = [] for vel in points.velocities: scaled_vel.append(vel * self.exe_speed_rate) points.velocities = tuple(scaled_vel) for acc in points.accelerations: scaled_acc.append(acc * self.exe_speed_rate * self.exe_speed_rate) points.accelerations = tuple(scaled_acc) # Display the Trajectory start_state = JointState() start_state.header = Header() start_state.header.stamp = rospy.Time.now() start_state.name = plan.trajectory.joint_trajectory.joint_names[:] start_state.position = plan.trajectory.joint_trajectory.points[ -1].positions[:] moveit_start_state = RobotState() moveit_start_state.joint_state = start_state pub_display_msg = DisplayTrajectory() pub_display_msg.model_id = "vs087" pub_display_msg.trajectory.append(plan.trajectory) pub_display_msg.trajectory_start = moveit_start_state self.display_hp_pub.publish(pub_display_msg) # Send Action and Wait result goal = ExecutePlanAction().action_goal.goal goal.planning_time = plan.planning_time goal.start_state = plan.start_state # goal.start_state.joint_state.header.stamp = rospy.Time.now() goal.trajectory = plan.trajectory # rospy.logwarn(goal) self.client.send_goal(goal) self.client.wait_for_result() # Update the task queue self.task_q.pop(0) rospy.loginfo("End Task")
def visualize_trajectory(self, blocking=True): if not self.display: return if not self.single_arm: self.adjust_traj_length() if self.rviz_pub.get_num_connections() < 1: rospy.logerr("Rviz topic not subscribed") return msg = DisplayTrajectory() msg.trajectory_start = self.robot_model.get_current_state() traj = RobotTrajectory() goal = JointTrajectory() goal.joint_names = self.left.JOINT_NAMES[:] if not self.single_arm: goal.joint_names += self.right.JOINT_NAMES[:] # The sim is very slow if the number of points is too large steps = 1 if len(self.left._goal.trajectory.points) < 100 else 10 for idx in range(0, len(self.left._goal.trajectory.points), steps): comb_point = JointTrajectoryPoint() lpt = self.left._goal.trajectory.points[idx] comb_point.positions = lpt.positions[:] if not self.single_arm: comb_point.positions += self.right._goal.trajectory.points[idx].positions[:] comb_point.time_from_start = lpt.time_from_start goal.points.append(comb_point) traj.joint_trajectory = goal msg.trajectory.append(traj) duration = goal.points[-1].time_from_start.to_sec() self.rviz_pub.publish(msg) if blocking: rospy.loginfo("Waiting for trajectory animation {} seconds".format(duration)) rospy.sleep(duration)
def visualize_path(robot_trajectory, position, planned_path_pub, goal_pos_pub): disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_trajectory) disp_traj.trajectory_start = RobotState() planned_path_pub.publish(disp_traj) marker = Marker() marker.header.frame_id = "base" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 1.0 marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] print 'Goal position {0}'.format(position) goal_pos_pub.publish(marker)
def execute(self): # Get latest task plan plan = self.task_q[0].trajectory for points in plan.joint_trajectory.points: tfs = points.time_from_start.to_sec() tfs /= self.exe_speed_rate points.time_from_start = rospy.Duration(tfs) self.grasp_[0] = self.task_q[0].grasp # Display the Trajectory start_state = JointState() start_state.header = Header() start_state.header.stamp = rospy.Time.now() start_state.name = plan.joint_trajectory.joint_names[:] start_state.position = plan.joint_trajectory.points[-1].positions[:] moveit_start_state = RobotState() moveit_start_state.joint_state = start_state pub_display_msg = DisplayTrajectory() pub_display_msg.model_id = "sia5" pub_display_msg.trajectory.append(plan) pub_display_msg.trajectory_start = moveit_start_state self.display_hp_pub.publish(pub_display_msg) # Send Action and Wait result goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory) rospy.loginfo("Start Task") self.client.send_goal(goal) self.client.wait_for_result() # Grasping if self.grasp_[0] != self.grasp_[1]: self.executeGrasp(self.grasp_[0]) self.grasp_[1] = self.grasp_[0] # Update the task queue self.task_q.pop(0) rospy.loginfo("End Task")
def display_trajectory(self, plan): print(plan) traj = plan.trajectory joints = traj.joint_names print(traj) init_joints = JointState() init_joints.name = joints init_joints.position = traj.points[0].positions init_joints.velocity = traj.points[0].velocities init_joints.effort = traj.points[0].effort joint_traj = RobotTrajectory() joint_traj.joint_trajectory = plan.trajectory init_state = RobotState() init_state.joint_state = init_joints display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = init_state display_trajectory.trajectory.append(joint_traj) # Publish self.display_trajectory_publisher.publish(display_trajectory) rospy.sleep(3)
def moveArm(self, config): print "- Planning Move..." pose_goal = self.group.get_current_pose().pose pose_goal.orientation = geometry_msgs.msg.Quaternion( *tf_conversions.transformations.quaternion_from_euler( 0., -math.pi / 2, 0.)) pose_goal.position.x = config.x pose_goal.position.y = config.y pose_goal.position.z = config.z self.group.set_pose_target(pose_goal) plan = self.group.plan() rospy.sleep(0.5) display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.robot.get_current_state() display_trajectory.trajectory.append(plan) self.display_trajectory_publisher.publish(display_trajectory) rospy.sleep(1) print "- Executing..." self.group.go(wait=True) rospy.sleep(1)
def main(): """ python scripts/main.py --help <------This prints out all the help messages and describes what each parameter is. NOTE: If running with the --moveit flag, it makes no sense to also choose your controller to be workspace, since moveit cannot accept workspace trajectories. This script simply ignores the controller selection if you specify both --moveit and --controller_name workspace, so if you want to run with moveit simply leave --controller_name as default. You can also change the rate, timeout if you want """ parser = argparse.ArgumentParser() parser.add_argument( '-task', '-t', type=str, default='line', help='Options: line, circle, arbitrary. Default: line') parser.add_argument( '-goal_point', '-g', type=float, default=[0, 0, 0], nargs=3, help= 'Specify a goal point for line and circle trajectories. Ex: -g 0 0 0') parser.add_argument( '-controller_name', '-c', type=str, default='jointspace', help='Options: workspace, jointspace, or torque. Default: jointspace') parser.add_argument('-arm', '-a', type=str, default='left', help='Options: left, right. Default: left') parser.add_argument('-rate', type=int, default=200, help=""" (Hz) This specifies how many loop iterations should occur per second. It is important to use a rate and not a regular while loop because you want the loop to refresh at a constant rate, otherwise you would have to tune your PD parameters if the loop runs slower / faster. Default: 200""") parser.add_argument( '-timeout', type=int, default=None, help= """after how many seconds should the controller terminate if it hasn\'t already. Default: None""") parser.add_argument( '-num_way', type=int, default=300, help= 'How many waypoints for the :obj:`moveit_msgs.msg.RobotTrajectory`. Default: 300' ) parser.add_argument( '--moveit', action='store_true', help= """If you set this flag, moveit will take the path you plan and execute it on the real robot""") parser.add_argument('--log', action='store_true', help='plots controller performance') args = parser.parse_args() if args.moveit: # We can't give moveit a workspace trajectory, so we make it # impossible. args.controller_name = 'jointspace' rospy.init_node('moveit_node') # this is used for sending commands (velocity, torque, etc) to the robot limb = baxter_interface.Limb(args.arm) # Choose an inverse kinematics solver. If you leave ik_solver as None, then the default # KDL solver will be used. Otherwise, you can uncomment the next line which sets ik_solver. # Then, the TRAC-IK inverse kinematics solver will be used. If you have trouble with inconsistency # or poor solutions with one method, feel free to try the other one. ik_solver = None # ik_solver = IK("base", args.arm + "_gripper") # A KDL instance for Baxter. This can be used for forward and inverse kinematics, # computing jacobians etc. kin = baxter_kinematics(args.arm) # This is a wrapper around MoveIt! for you to use. planner = PathPlanner('{}_arm'.format(args.arm)) # Get an appropriate RobotTrajectory for the task (circular, linear, or arbitrary MoveIt) # If the controller is a workspace controller, this should return a trajectory where the # positions and velocities are workspace configurations and velocities. If the controller # is a jointspace or torque controller, it should return a trajectory where the positions # and velocities are the positions and velocities of each joint. robot_trajectory = get_trajectory(limb, kin, ik_solver, planner, args) if args.controller_name == "workspace": config = robot_trajectory.joint_trajectory.points[0].positions pose = create_pose_stamped_from_pos_quat( [config[0], config[1], config[2]], # XYZ location [config[3], config[4], config[5], config[6] ], # XYZW quaterion orientation 'base') success, plan, time_taken, error_code = planner.plan_to_pose(pose) planner.execute_plan(plan) else: start = robot_trajectory.joint_trajectory.points[0].positions while not rospy.is_shutdown(): try: # ONLY FOR EMERGENCIES!!! DON'T UNCOMMENT THE NEXT LINE UNLESS YOU KNOW WHAT YOU'RE DOING!!! # limb.move_to_joint_positions(joint_array_to_dict(start, limb), timeout=7.0, threshold=0.0001) success, plan, time_taken, error_code = planner.plan_to_joint_pos( start) planner.execute_plan(plan) break except moveit_commander.exception.MoveItCommanderException as e: print(e) print("Failed planning, retrying...") if args.moveit: # PROJECT 1 PART A # by publishing the trajectory to the move_group/display_planned_path topic, you should # be able to view it in RViz. You will have to click the "loop animation" setting in # the planned path section of MoveIt! in the menu on the left side of the screen. pub = rospy.Publisher('move_group/display_planned_path', DisplayTrajectory, queue_size=10) disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_trajectory) disp_traj.trajectory_start = RobotState() pub.publish(disp_traj) try: input('Press <Enter> to execute the trajectory using MOVEIT') except KeyboardInterrupt: sys.exit() # uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this. # the lines above will display the trajectory in RViz if args.log: node = roslaunch.core.Node("proj1_pkg", "moveit_plot.py", args="{} {}".format( args.arm, args.rate)) launch = roslaunch.scriptapi.ROSLaunch() launch.start() process = launch.launch(node) rospy.wait_for_service('start_logging') start_service = rospy.ServiceProxy('start_logging', TriggerLogging) request = TriggerLoggingRequest(path=robot_trajectory) start_service(robot_trajectory) planner.execute_plan(robot_trajectory) if args.log: r = rospy.Rate(1) while process.is_alive(): r.sleep() else: # PROJECT 1 PART B controller = get_controller(args.controller_name, limb, kin) try: input( 'Press <Enter> to execute the trajectory using YOUR OWN controller' ) except KeyboardInterrupt: sys.exit() # execute the path using your own controller. done = controller.execute_path(robot_trajectory, rate=args.rate, timeout=args.timeout, log=args.log) if not done: print('Failed to move to position') sys.exit(0)
def visualize_plan(self, plan): display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = plan.points[0] display_trajectory.trajectory.extend(plan.points) self.display_planned_path_publisher.publish(display_trajectory)
def _plan(self, event): if self._done: return self._arm.remember_joint_values("HOME") base_pose = convert.pose2matrix(self._arm.get_current_pose()) home_counter = 0 error_counter = 0 previous_state = copy.deepcopy(self._robot.get_current_state()) while True: if error_counter > self._max_errors: self.send_error("PLANNING", [], "Failed too often to plan to next pose") self._arm.set_start_state(previous_state) if not self._done or home_counter > 5: random_translation = transform.random_translation(1.0) random_translation[0,3] *= self._max_translation[0] random_translation[1,3] *= self._max_translation[1] random_translation[2,3] *= self._max_translation[2] random_rotation = transform.random_rotation(self._max_angle) new_pose = convert.matrix2pose(random_translation.dot(base_pose).dot(random_rotation)) self._arm.set_pose_target(new_pose) home_counter = 0 else: self._arm.set_named_target("HOME") home_counter += 1 print("Planning home: {}".format(home_counter)) # do the planning plan = self._arm.plan() # replan if trajectory constraints not satified points = len(plan.joint_trajectory.points) if points <= 0: error_counter += 1 continue if points > 15: error_counter += 1 print("Complex trajectory with {} points".format(points)) continue error_counter = 0 # scale trajectory speed self._scale_trajectory(plan, self._speed) # display plan trajectoryMsg = DisplayTrajectoryMsg() trajectoryMsg.trajectory_start = previous_state trajectoryMsg.trajectory = [plan] self._pubMove.publish(trajectoryMsg) # update robot state previous_state = copy.deepcopy(self._robot.get_current_state()) previous_state.joint_state.position = list(previous_state.joint_state.position) for i, joint in enumerate(plan.joint_trajectory.joint_names): idx = previous_state.joint_state.name.index(joint) previous_state.joint_state.position[idx] = plan.joint_trajectory.points[-1].positions[i] # add plan self._queue.put(plan) # stop after plan home found if self._done and home_counter > 0: break self._arm.forget_joint_values("HOME")
def __init__(self): #关于baxter无法通过moveit获取当前姿态的错误 https://github.com/ros-planning/moveit/issues/1187 #joint_state_topic = ['joint_states:=/robot/joint_states'] #初始化moveit的 API接口 moveit_commander.roscpp_initialize(sys.argv) #初始化ros节点, rospy.init_node('panda_grasp', anonymous=True) #构建一个tf发布器 self.tf_broadcaster = tf.TransformBroadcaster() self.grasp_config = GraspConfig() #创建一个TF监听器 self.tf_listener = tf.TransformListener() #一直等待接收到桌面标签和机器人base坐标系之间的变换(需要提前进行手眼标定) get_transform = False while not get_transform: try: #尝试查看机器人基座base与桌面标签之间的转换 trans, rot = self.tf_listener.lookupTransform( '/panda_link0', '/ar_marker_6', rospy.Time(0)) euler = tf.transformations.euler_from_quaternion(rot) self.base2marker = tf.transformations.compose_matrix( translate=trans, angles=euler) #查看gripper到link8之间的变换 trans, rot = self.tf_listener.lookupTransform( '/panda_EE', '/panda_link8', rospy.Time(0)) euler = tf.transformations.euler_from_quaternion(rot) self.gripper2link8 = tf.transformations.compose_matrix( translate=trans, angles=euler) #查看base到panda_link8的变换,此时就是查询gripper的初始姿态 trans, rot = self.tf_listener.lookupTransform( '/panda_link0', '/panda_link8', rospy.Time(0)) euler = tf.transformations.euler_from_quaternion(rot) self.base2Initial_link8 = tf.transformations.compose_matrix( translate=trans, angles=euler) get_transform = True rospy.loginfo("got transform complete") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("got transform failed") rospy.sleep(0.5) continue # 初始化场景对象 scene = moveit_commander.PlanningSceneInterface() rospy.sleep(2) # 创建机械臂规划组对象 self.selected_arm = moveit_commander.MoveGroupCommander('panda_arm') #创建机械手规划对象 hand_group = moveit_commander.MoveGroupCommander('hand') display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', DisplayTrajectory, queue_size=20) # 获取左臂末端执行器名称 self.end_effector_link = self.selected_arm.get_end_effector_link() print("i am here") print(self.end_effector_link) # 创建机械臂父坐标系名称字符 reference_frame = 'panda_link0' #self.tf_listener = tf.TransformListener() # 设置父坐标系名称 #self.selected_arm.set_pose_reference_frame(reference_frame) # 允许机械臂进行重规划 #self.selected_arm.allow_replanning(True) # 允许机械臂末位姿的错误余量 self.selected_arm.set_goal_position_tolerance(0.01) self.selected_arm.set_goal_orientation_tolerance(0.05) #不允许规划失败重规划,规划时间只允许5秒钟,否则很浪费时间 self.selected_arm.allow_replanning(False) self.selected_arm.set_planning_time(5) #清除之前遗留的物体 scene.remove_world_object('table') #设置桌面高度 table_ground = 0.6 #桌面尺寸 x y z table_size = [0.6, 1.2, 0.01] # 将table加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = 'panda_link0' table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = 0.025 table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) #设置初始姿态 #Home_positions = [-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50]#移动到工作位置,使用正运动学 # Set the arm's goal configuration to the be the joint positions #self.selected_arm.set_joint_value_target(Home_positions) # Plan and execute the motion #self.selected_arm.go() #rospy.sleep(5) # 设置home姿态 Home_positions = [0.04, -0.70, 0.18, -2.80, 0.19, 2.13, 0.92] #移动到工作位置,使用正运动学 #self.selected_arm.remember_joint_values('resting', joint_positions)#存储当前状态为初始状态 #self.start_state =self.selected_arm.get_current_pose() # Set the arm's goal configuration to the be the joint positions self.selected_arm.set_joint_value_target(Home_positions) # Plan and execute the motion,运动到Home位置 self.selected_arm.go() self.selected_arm.stop() joint_goal = hand_group.get_current_joint_values() joint_goal[0] = 0.04 joint_goal[1] = 0.04 hand_group.go(joint_goal, wait=True) hand_group.stop() ######################开始等待接收夹爪姿态######################### print("Waiting for gripper pose!") #等待gripper_pose这个话题的发布(目标抓取姿态,该姿态将会进行抓取) #rospy.wait_for_message('/detect_grasps/clustered_grasps', GraspConfigList) #创建消息订阅器,订阅“gripper_pose”话题,这个gripper_pose,是以桌面标签为参考系的 #接收到之后,调用回调函数,有两件事要做 # 1.将gripper_pose # 计算后撤距离 self.r_flag = False rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, self.Callback, queue_size=1) #能不能,先检查并读取 桌面标签和机器人底座之间的转换关系?(这只是其中一条路吧) #利用TF直接,读取grasp_pose在base坐标系下面的姿态 #调用pose处理函数,计算预抓取姿态, #rospy.sleep(5) print("Start to Recieve Grasp Config!") ##################################################################### while not rospy.is_shutdown(): if self.r_flag: #目标姿态设定为预抓取位置 #self.pre_grasp_pose self.r_flag = False #尝试利用TF直接,读取grasp_pose在base坐标系下面的姿态 #计算预抓取姿态 else: rospy.sleep(0.5) continue #以当前姿态作为规划起始点 self.selected_arm.set_start_state_to_current_state() # 对末端执行器姿态设定目标姿态 #self.selected_arm.set_pose_target(target_pose, 'left_gripper') # 规划轨迹 #traj = self.selected_arm.plan(target_pose.pose) # 执行轨迹,运行到预抓取位置 #self.selected_arm.execute(traj) print(self.end_effector_link) print('Moving to pre_grasp_pose') #self.selected_arm.pick("test",self.grasp_config,plan_only = True) #traj=self.selected_arm.plan(self.pre_grasp_pose) #self.selected_arm.set_pose_target(self.pre_grasp_pose,end_effector_link="panda_EE") #traj=self.selected_arm.plan() #continue #success=self.selected_arm.execute(traj) #print(target_pose.pose) #设置规划 #self.selected_arm.set_planning_time(5) success = self.selected_arm.go(self.pre_grasp_pose, wait=True) self.selected_arm.stop() self.selected_arm.clear_pose_targets() if not success: print('Failed to move to pre_grasp_pose!') continue print('Move to pre_grasp_pose succeed') #等待机械臂稳定 rospy.sleep(1) #再设置当前姿态为起始姿态 self.selected_arm.set_start_state_to_current_state() # waypoints = [] wpose = self.selected_arm.get_current_pose().pose #print("#####wpose.position") #print(wpose.position) #print("#####self.grasp_pose2") #print(self.grasp_pose.position) wpose.position.x = self.grasp_pose.position.x wpose.position.y = self.grasp_pose.position.y wpose.position.z = self.grasp_pose.position.z waypoints.append(copy.deepcopy(wpose)) #wpose = self.selected_arm.get_current_pose().pose #wpose.position.z -= scale * 0.1 #规划从当前位姿,保持姿态,转移到目标夹爪姿态的路径 (plan, fraction) = self.selected_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold ##显示轨迹 display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.selected_arm.get_current_state( ) display_trajectory.trajectory.append(plan) # Publish display_trajectory_publisher.publish(display_trajectory) #执行,并等待这个轨迹执行成功 new_plan = self.scale_trajectory_speed(plan, 0.3) self.selected_arm.execute(new_plan, wait=True) #self.selected_arm.shift_pose_target(2,0.05,"panda_link8") #self.selected_arm.go() #执行抓取 rospy.sleep(2) print("Grasping") joint_goal = hand_group.get_current_joint_values() joint_goal[0] = 0.015 joint_goal[1] = 0.015 #plan=hand_group.plan(joint_goal) #new_plan=self.scale_trajectory_speed(plan,0.3) hand_group.go(joint_goal, wait=True) hand_group.stop() ####################抓取完后撤#################### waypoints = [] wpose = self.selected_arm.get_current_pose().pose wpose.position.x = self.pre_grasp_pose.position.x wpose.position.y = self.pre_grasp_pose.position.y wpose.position.z = self.pre_grasp_pose.position.z waypoints.append(copy.deepcopy(wpose)) #规划从当前位姿,保持姿态,转移到目标夹爪姿态的路径 (plan, fraction) = self.selected_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold #执行,并等待后撤成功 new_plan = self.scale_trajectory_speed(plan, 0.6) self.selected_arm.execute(new_plan, wait=True) """ display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.selected_arm.get_current_state() display_trajectory.trajectory.append(plan) # Publish display_trajectory_publisher.publish(display_trajectory) """ ######################暂时设置直接回到Home############################ #self.selected_arm.remember_joint_values('resting', joint_positions)#存储当前状态为初始状态 #self.start_state =self.selected_arm.get_current_pose(self.end_effector_link) # Set the arm's goal configuration to the be the joint positions self.selected_arm.set_joint_value_target(Home_positions) # Plan and execute the motion,运动到Home位置 self.selected_arm.go() self.selected_arm.stop() joint_goal = hand_group.get_current_joint_values() joint_goal[0] = 0.04 joint_goal[1] = 0.04 hand_group.go(joint_goal, wait=True) hand_group.stop() print("Grasp done") rospy.sleep(5) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def poseCallback(Pose_Array): print type(Pose_Array) posearray=Pose_Array.posearray print (len(posearray)) i=80 for pose in posearray[80:]: i=i+1 data=pose.pose #print (type(data)) print (data) try: dataCopy1 = copy.deepcopy(data) (r, p, y) = tf.transformations.euler_from_quaternion([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]) tMatrix = tf.transformations.euler_matrix(r, p, y) x_offset=-0.10 data.pose.position.x=data.pose.position.x+(tMatrix[0,0]*x_offset) data.pose.position.y=data.pose.position.y + (tMatrix[1,0]*x_offset) data.pose.position.z=data.pose.position.z + (tMatrix[2,0]*x_offset) newRotMatrix = tf.transformations.euler_matrix(0, 1.57, 0) transformedMatrix = numpy.dot(tMatrix, newRotMatrix) dataCopy1.pose.position.x=dataCopy1.pose.position.x - (transformedMatrix[0,2]*x_offset) dataCopy1.pose.position.y=dataCopy1.pose.position.y - (transformedMatrix[1,2]*x_offset) dataCopy1.pose.position.z=dataCopy1.pose.position.z - (transformedMatrix[2,2]*x_offset) dis = (dataCopy1.pose.position.x - data.pose.position.x) * (dataCopy1.pose.position.x - data.pose.position.x) dis = dis + (dataCopy1.pose.position.y - data.pose.position.y) * (dataCopy1.pose.position.y - data.pose.position.y) dis = dis + (dataCopy1.pose.position.z - data.pose.position.z) * (dataCopy1.pose.position.z - data.pose.position.z) if (dis < 0.008): pass else: newRotMatrix = tf.transformations.euler_matrix(0, -1.57, 0) transformedMatrix = numpy.dot(tMatrix, newRotMatrix) q = tf.transformations.quaternion_from_matrix(transformedMatrix) data.pose.orientation.x = q[0] data.pose.orientation.y = q[1] data.pose.orientation.z = q[2] data.pose.orientation.w = q[3] pub1.publish(data) #rospy.sleep(5) arm.set_joint_value_target(data, False) arm.set_planner_id("RRTConnectkConfigDefault") print "======= Waiting while setting joint value target" #rospy.sleep(5) #modifed from here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! # find who throw motion planer failures try: plan1 = arm.plan() except Exception, e: print "Exception thrown while path planning for grasp " + str(i) print traceback.format_exc() continue print "============ Waiting while RVIZ displays plan1..." #rospy.sleep(5) a = raw_input("Shall i execute grasp" + str(i) + "? Say y/n ") if a == "y": print "executing" arm.execute(plan1) ######################### ## Wait for Plan1 to finish ######################### while(not IsMotionFinished(True)): print "Plan1 is being executed!" print "Plan1 has finished!!" #b = raw_input("Shall i plan ik path" + str(i) + "? Say y/n ") #if b == 'y' : rospy.sleep(1) print "============ Computing cartesian path..." waypoints = [] waypoints.append(data.pose) data.header.stamp = rospy.Time(0) dataHandLink = tl.transformPose("Hand_Link", data) dataHandLink.pose.position.z= dataHandLink.pose.position.z + x_offset dataHandLink.header.stamp = rospy.Time(0) wpose = tl.transformPose(arm.get_pose_reference_frame(), dataHandLink).pose waypoints.append(copy.deepcopy(wpose)) print wpose (plan2, fraction) = arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0, False) # jump_threshold print fraction print "============ Visualizing plan1 and plan2" display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory.trajectory.append(plan2) display_trajectory_publisher.publish(display_trajectory); #c = raw_input("Shall i execute ik path" + str(i) + "? Say y/n ") #if c == 'y' : #print "executing" arm.execute(plan2) ######################### ## Wait for Plan2 to finish ######################### while(not IsMotionFinished(True)): print "Plan2 is being executed!" print "Plan2 has finished!!" #d=raw_input("Shall I close the gripper? Say y/n ") #if d=='y': # print "closing the gripper!" r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): strr = "close" pub_gripper.publish(strr) r.sleep() break if a == "n": print "not executing" continue except Exception, err: print "NO IK solution for grasp " + str(i) print traceback.format_exc() continue
def _disp_trj(self): disp_trj = DisplayTrajectory() disp_trj.trajectory_start = self._robot.get_current_state() disp_trj.trajectory.append(self._arm_plan) self._pub_traj.publish(disp_trj)
# Display the Trajectory start_state = JointState() start_state.header = Header() start_state.header.stamp = rospy.Time.now() start_state.name = plan.joint_trajectory.joint_names[:] start_state.position = plan.joint_trajectory.points[-1].positions[:] moveit_start_state = RobotState() <<<<<<< HEAD moveit_start_state.joint_state = start_state ======= moveit_start_state.joint_state = start_state >>>>>>> d95c7577c838d4cc104407dcb9fe5ba08262d4e9 pub_display_msg = DisplayTrajectory() pub_display_msg.model_id = "sia5" pub_display_msg.trajectory.append(plan) pub_display_msg.trajectory_start = moveit_start_state self.display_hp_pub.publish(pub_display_msg) # Send Action and Wait result goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory) rospy.loginfo("Start Task") self.client.send_goal(goal) self.client.wait_for_result() # Grasping if self.grasp_[0] != self.grasp_[1]: self.executeGrasp(self.grasp_[0]) self.grasp_[1] = self.grasp_[0] # Update the task queue self.task_q.pop(0)
# pose_goal.y = 0.1 # pose_goal.z = 0.2 # # random_pose = group.get_random_pose() # mp_req_2 = create_basic_mp_position_request( # planning_frame, eef_link, pose_goal, "LIN") motion_sequence_request = MotionSequenceRequest() item_1 = MotionSequenceItem() item_1.blend_radius = 0 item_1.req = mp_req_1 item_2 = MotionSequenceItem() item_2.blend_radius = 0 item_2.req = mp_req_2 motion_sequence_request = MotionSequenceRequest() motion_sequence_request.items = [item_1, item_2] rospy.wait_for_service('plan_sequence_path') try: service_call = rospy.ServiceProxy('plan_sequence_path', GetMotionSequence) result = service_call(motion_sequence_request) display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory = result.response.planned_trajectories # Publish display_trajectory_publisher.publish(display_trajectory) except rospy.ServiceException as e: print ("Service call failed: %s" % e)
def publish_plan_B(self, plan): display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self.robot_B.get_current_state() display_trajectory.trajectory.append(plan) self.display_trajectory_publisher_B.publish(display_trajectory)
def visualize_plan_rviz(self): display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = self._robot.get_current_state() display_trajectory.trajectory.append(self.plan) self._display_trajectory_publisher.publish(display_trajectory) rospy.sleep(5)
def main(): """ Examples of how to run me: python scripts/main.py --help <------This prints out all the help messages and describes what each parameter is python scripts/main.py -t 1 -ar 1 -c workspace -a left --log python scripts/main.py -t 2 -ar 2 -c velocity -a left --log python scripts/main.py -t 3 -ar 3 -c torque -a right --log python scripts/main.py -t 1 -ar 4 5 --path_only --log NOTE: If running with the --moveit flag, it makes no sense to also choose your controller to be workspace, since moveit cannot accept workspace trajectories. This script simply ignores the controller selection if you specify both --moveit and --controller_name workspace, so if you want to run with moveit simply leave --controller_name as default. You can also change the rate, timeout if you want """ parser = argparse.ArgumentParser() parser.add_argument('-task', '-t', type=str, default='line', help='Options: line, circle, square. Default: line') parser.add_argument('-ar_marker', '-ar', nargs='+', help='Which AR marker to use. Default: 1') parser.add_argument( '-controller_name', '-c', type=str, default='jointspace', help='Options: workspace, jointspace, or torque. Default: jointspace') parser.add_argument('-arm', '-a', type=str, default='left', help='Options: left, right. Default: left') parser.add_argument('-rate', type=int, default=200, help=""" This specifies how many ms between loops. It is important to use a rate and not a regular while loop because you want the loop to refresh at a constant rate, otherwise you would have to tune your PD parameters if the loop runs slower / faster. Default: 200""") parser.add_argument( '-timeout', type=int, default=None, help= """after how many seconds should the controller terminate if it hasn\'t already. Default: None""") parser.add_argument( '-num_way', type=int, default=300, help= 'How many waypoints for the :obj:`moveit_msgs.msg.RobotTrajectory`. Default: 300' ) parser.add_argument( '--moveit', action='store_true', help= """If you set this flag, moveit will take the path you plan and execute it on the real robot""") parser.add_argument('--log', action='store_true', help='plots controller performance') args = parser.parse_args() rospy.init_node('moveit_node') # this is used for sending commands (velocity, torque, etc) to the robot limb = baxter_interface.Limb(args.arm) # this is used to get the dynamics (inertia matrix, manipulator jacobian, etc) from the robot # in the current position, UNLESS you specify other joint angles. see the source code # https://github.com/valmik/baxter_pykdl/blob/master/src/baxter_pykdl/baxter_pykdl.py # for info on how to use each method kin = baxter_kinematics(args.arm) # ADD COMMENT EHRE tag_pos = [lookup_tag(marker) for marker in args.ar_marker] # Get an appropriate RobotTrajectory for the task (circular, linear, or square) # If the controller is a workspace controller, this should return a trajectory where the # positions and velocities are workspace positions and velocities. If the controller # is a jointspace or torque controller, it should return a trajectory where the positions # and velocities are the positions and velocities of each joint. robot_trajectory = get_trajectory(args.task, limb, kin, tag_pos, args.num_way, args.controller_name) # This is a wrapper around MoveIt! for you to use. We use MoveIt! to go to the start position # of the trajectory planner = PathPlanner('{}_arm'.format(args.arm)) if args.controller_name == "workspace": pose = create_pose_stamped_from_pos_quat( robot_trajectory.joint_trajectory.points[0].positions, [0, 1, 0, 0], 'base') plan = planner.plan_to_pose(pose) else: plan = planner.plan_to_joint_pos( robot_trajectory.joint_trajectory.points[0].positions) planner.execute_plan(plan) if args.moveit: # LAB 1 PART A # by publishing the trajectory to the move_group/display_planned_path topic, you should # be able to view it in RViz. You will have to click the "loop animation" setting in # the planned path section of MoveIt! in the menu on the left side of the screen. pub = rospy.Publisher('move_group/display_planned_path', DisplayTrajectory, queue_size=10) disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_trajectory) # disp_traj.trajectory_start = planner._group.get_current_joint_values() disp_traj.trajectory_start = RobotState() pub.publish(disp_traj) try: raw_input('Press <Enter> to execute the trajectory using MOVEIT') except KeyboardInterrupt: sys.exit() # uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this. # the lines above will display the trajectory in RViz planner.execute_plan(robot_trajectory) else: # LAB 1 PART B controller = get_controller(args.controller_name, limb, kin) try: raw_input( 'Press <Enter> to execute the trajectory using YOUR OWN controller' ) except KeyboardInterrupt: sys.exit() # execute the path using your own controller. done = controller.execute_path(robot_trajectory, rate=args.rate, timeout=args.timeout, log=args.log) if not done: print 'Failed to move to position' sys.exit(0)
def main(): # Robot Node initialization rospy.init_node('robot_init', anonymous=True) # Visible -> on/off in the RVIZ environment visible_object = False rospy.set_param('object_visible', visible_object) # Moveit Initialization group = moveit_commander.MoveGroupCommander('manipulator') scene = moveit_commander.PlanningSceneInterface() robot = moveit_commander.RobotCommander() # Reset robot positions (go to home position) rospy.wait_for_service('/reset_robot') reset_robot = rospy.ServiceProxy('/reset_robot', Trigger) reset_robot.call() rospy.sleep(0.5) # Initialize the current position of the robot w_pose_initial = rospy.wait_for_message('/current_tcp_pose', geometry_msgs.msg.PoseStamped, timeout=None) position = [ w_pose_initial.pose.position.x, w_pose_initial.pose.position.y, w_pose_initial.pose.position.z ] orientation = [ w_pose_initial.pose.orientation.w, w_pose_initial.pose.orientation.x, w_pose_initial.pose.orientation.y, w_pose_initial.pose.orientation.z ] # Set the parameters of the Object (display -> environment) rospy.set_param('object_pos', position) # Setting parameters for planning vel_scaling_f = 1.0 acc_scaling_f = 1.0 group.set_max_velocity_scaling_factor(vel_scaling_f) group.set_max_acceleration_scaling_factor(acc_scaling_f) # Planner -> OMPL (Default) or BiTRRTkConfigDefault group.set_planner_id('OMPL') # Joint, Cartesian_1, Cartesian_2 or None mode = 'Cartesian_1' if mode == 'Joint': # Generate Joint Trajectory plan = joint_trajectory(group, [1.57, -1.57, 1.57, -1.57, -1.57, 0.0]) elif mode == 'Cartesian_1': # Generate Cartesian Trajectory (1) plan = cartesian_trajectory_1(group, [ position[0], position[1] - 0.1, position[2], orientation[0], orientation[1], orientation[2], orientation[3] ]) elif mode == 'Cartesian_2': # Generate Cartesian Trajectory (2) plan = cartesian_trajectory_2(group, [ position[0], position[1], position[2], orientation[0], orientation[1], orientation[2], orientation[3] ], [0.05, 0.25, 0.30], vel_scaling_f, acc_scaling_f, 0.01, True) if mode != 'None': rospy.loginfo('Intermediate points on the robot trajectory: %f' % len(plan.joint_trajectory.points)) # Show trajectory display_trajectory = DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) # Execute the trajectory group.execute(plan, wait=True) rospy.sleep(0.5) # Reset group.stop() group.clear_path_constraints() group.clear_pose_targets()
plan = planner.plan_to_joint_pos( robot_trajectory.joint_trajectory.points[0].positions) planner.execute_plan(plan) if args.moveit: # LAB 1 PART A # by publishing the trajectory to the move_group/display_planned_path topic, you should # be able to view it in RViz. You will have to click the "loop animation" setting in # the planned path section of MoveIt! in the menu on the left side of the screen. pub = rospy.Publisher('move_group/display_planned_path', DisplayTrajectory, queue_size=10) disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_trajectory) # disp_traj.trajectory_start = planner._group.get_current_joint_values() disp_traj.trajectory_start = RobotState() pub.publish(disp_traj) try: raw_input('Press <Enter> to execute the trajectory using MOVEIT') except KeyboardInterrupt: sys.exit() # uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this. # the lines above will display the trajectory in RViz #print('ROBOT TRAJECTORY') #path_list = [] #print(robot_trajectory.joint_trajectory.points[0].positions) #for p in robot_trajectory.joint_trajectory.points: #print(p.positions) #path_list.append(p.positions) #path_list = np.array(path_list)