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 displayTrajFromRobotTraj(self, robot_traj): """Given a robot trajectory return a displaytrajectory with it""" dt = DisplayTrajectory() dt.trajectory.append(robot_traj) dt.trajectory_start.joint_state.name = robot_traj.joint_trajectory.joint_names dt.trajectory_start.joint_state.position = robot_traj.joint_trajectory.points[0].positions 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 display_planned_path(self, path): """Sends the trajectory to apropriate topic to be displayed. Arguments: path : {RobotTrajectory message} Trajectory to be displayed """ self.pub = rospy.Publisher(TOPIC_DISPLAY_PLANNED_PATH, DisplayTrajectory) dsp = DisplayTrajectory() dsp.trajectory = [path] self.pub.publish(dsp)
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 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 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 callback(self, anymsg, topic_name): if self.pause_button.isChecked(): return # In case of control_msgs/FollowJointTrajectoryActionGoal # set trajectory_msgs/JointTrajectory to 'msg' # Convert AnyMsg to trajectory_msgs/JointTrajectory msg_class = self.topic_name_class_map[topic_name] if msg_class == JointTrajectory: msg = JointTrajectory().deserialize(anymsg._buff) elif msg_class == FollowJointTrajectoryActionGoal: msg = FollowJointTrajectoryActionGoal().deserialize( anymsg._buff).goal.trajectory elif msg_class == DisplayTrajectory: if DisplayTrajectory().deserialize( anymsg._buff).trajectory.__len__() > 0: msg = DisplayTrajectory().deserialize( anymsg._buff).trajectory.pop().joint_trajectory else: rospy.logwarn( "Received planned trajectory has no waypoints in it. Nothing to plot!" ) return else: rospy.logerr('Wrong message type %s' % msg_class) return self.time = np.array([0.0] * len(msg.points)) (self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {}) for joint_name in msg.joint_names: self.dis[joint_name] = np.array([0.0] * len(msg.points)) self.vel[joint_name] = np.array([0.0] * len(msg.points)) self.acc[joint_name] = np.array([0.0] * len(msg.points)) self.eff[joint_name] = np.array([0.0] * len(msg.points)) for i in range(len(msg.points)): point = msg.points[i] self.time[i] = point.time_from_start.to_sec() for j in range(len(msg.joint_names)): joint_name = msg.joint_names[j] if point.positions: self.dis[joint_name][i] = point.positions[j] if point.velocities: self.vel[joint_name][i] = point.velocities[j] if point.accelerations: self.acc[joint_name][i] = point.accelerations[j] if point.effort: self.eff[joint_name][i] = point.effort[j] if self.joint_names != msg.joint_names: self.joint_names = msg.joint_names self.refresh_tree() self.joint_names = msg.joint_names self.plot_graph()
def viz_joint_traj(self, j_traj, base_link='base_link'): robot_tr = RobotTrajectory() j_traj.header.frame_id = base_link robot_tr.joint_trajectory = j_traj disp_tr = DisplayTrajectory() disp_tr.trajectory.append(robot_tr) disp_tr.model_id = self.robot_name i = 0 while (not rospy.is_shutdown() and i < 10): i += 1 self.pub_tr.publish(disp_tr) self.rate.sleep()
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 publish_traj(self): display_trajectory = DisplayTrajectory() if self.trajectory[0]: display_trajectory.trajectory.append(self.trajectory[1]) self._display_trajectory_publisher.publish(display_trajectory) else: rospy.logwarn("No trajectory to view")
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 display(self, trajectory): """ Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin :param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message """ # Publish the DisplayTrajectory (only for trajectory preview in RViz) # includes a convert of float durations in rospy.Duration() def js_to_rt(js): rt = RobotTrajectory() rt.joint_trajectory.joint_names = js.name rt.joint_trajectory.points.append( JointTrajectoryPoint(positions=js.position)) return rt dt = DisplayTrajectory() if isinstance(trajectory, RobotTrajectory): dt.trajectory.append(trajectory) elif isinstance(trajectory, JointTrajectory): rt = RobotTrajectory() rt.joint_trajectory = trajectory dt.trajectory.append(rt) elif isinstance(trajectory, RobotState): dt.trajectory.append(js_to_rt(trajectory.joint_state)) elif isinstance(trajectory, JointState): dt.trajectory.append(js_to_rt(trajectory)) else: raise NotImplementedError( "ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}" .format(str(type(trajectory)))) self._display_traj.publish(dt)
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 display(self, trajectory): if isinstance(trajectory, RobotTrajectory): trajectory = trajectory.joint_trajectory if not isinstance(trajectory, JointTrajectory): rospy.logerr("robot.display() only accepts joint trajectories") return dt = DisplayTrajectory() rt = RobotTrajectory(joint_trajectory=trajectory) dt.trajectory.append(rt) self._display_pub.publish(dt)
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 send_traj(self, u_arr): # Formulate joint trajectory message: jt_msg = JointTrajectory() jt_msg.joint_names = self.joint_names for i in range(len(u_arr)): u = u_arr[i] jt_pt = JointTrajectoryPoint() jt_pt.positions = u jt_msg.points.append(jt_pt) robot_tr = RobotTrajectory() robot_tr.joint_trajectory = jt_msg disp_tr = DisplayTrajectory() disp_tr.trajectory.append(robot_tr) disp_tr.model_id = self.robot_name self.pub_tr.publish(disp_tr) i = 0 while (not rospy.is_shutdown() and i < 10): i += 1 self.rate.sleep()
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 create_display_traj(self, joint_traj): self.display_traj = DisplayTrajectory() self.display_traj.model_id = 'lbr4' robot_traj = RobotTrajectory() robot_traj.joint_trajectory = joint_traj self.display_traj.trajectory.append(robot_traj) self.display_traj.trajectory_start.\ joint_state.header.frame_id = '/world' self.display_traj.trajectory_start.joint_state.name = \ self.kdl_kin.get_joint_names() self.display_traj.trajectory_start.joint_state.position = \ joint_traj.points[0].positions self.display_traj.trajectory_start.joint_state.velocity = \ joint_traj.points[0].velocities
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 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 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 single_shot(path, obstacles): exp = DiffCoplusBaxterExperiments() print('Adding box') rospy.sleep(2) box_names = [] ## Commented out because assuming obstacles already exist in scene # for i, obs in enumerate(obstacles): # box_name = 'box_{}'.format(i) # box_names.append(box_name) # box_pose = geometry_msgs.msg.PoseStamped() # box_pose.header.frame_id = exp.planning_frame # # box_pose.pose.orientation.w = 1.0 # box_pose.pose.position.x = obs[1][0] # box_pose.pose.position.y = obs[1][1] # box_pose.pose.position.z = obs[1][2] # exp.scene.add_box(box_name, box_pose, size=obs[2]) # wait_for_state_update(exp.scene, box_name, box_is_known=True) pub = exp.display_trajectory_publisher joint_traj = JointTrajectory() for q in path: traj_point = JointTrajectoryPoint() traj_point.positions = q.numpy().tolist() joint_traj.points.append(traj_point) joint_traj.joint_names = [ 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2' ] robot_traj = RobotTrajectory() robot_traj.joint_trajectory = joint_traj disp_traj = DisplayTrajectory() disp_traj.trajectory.append(robot_traj) disp_traj.trajectory_start.joint_state.position = path[0].numpy() disp_traj.trajectory_start.joint_state.name = joint_traj.joint_names pub.publish(disp_traj) return box_names, exp
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 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()
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)