def make_follow_joint_trajectory_goal(trajectory: JointTrajectory): goal = FollowJointTrajectoryGoal() goal.trajectory.header.stamp = rospy.Time.now() goal.trajectory = trajectory goal.goal_tolerance = [ make_joint_tolerance(0.02, n) for n in trajectory.joint_names ] goal.goal_time_tolerance = rospy.Duration(nsecs=500_000_000) return goal
def build_follow_trajectory(self, traj): # Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error) 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) return traj_goal
def receiverLoop(self): while True: try: input_msg = pickle.load(open("/dev/shm/right_arm_goal.p", "rb")) os.remove("/dev/shm/right_arm_goal.p") except IOError: print "file not there..." rospy.sleep(0.1) continue print input_msg goal = FollowJointTrajectoryGoal() rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" + str(input_msg.goal.goal_time_tolerance)) goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0) rospy.loginfo("input_msg.goal.goal_tolerance: \n" + str(input_msg.goal.goal_tolerance)) goal.goal_tolerance = input_msg.goal.goal_tolerance rospy.loginfo("input_msg.goal.path_tolerance: \n" + str(input_msg.goal.path_tolerance)) goal.path_tolerance = input_msg.goal.path_tolerance rospy.loginfo("input_msg.goal.trajectory.header: \n" + str(input_msg.goal.trajectory.header)) goal.trajectory.header = input_msg.goal.trajectory.header goal.trajectory.header.frame_id = '/base_link' goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names input_times = pickle.load( open("/dev/shm/right_arm_goal_times.p", "rb")) rospy.loginfo("input_times is:\n" + str(input_times)) for input_point, time in zip(input_msg.goal.trajectory.points, input_times): p = JointTrajectoryPoint() p.accelerations = input_point.accelerations p.positions = input_point.positions p.velocities = input_point.velocities # rospy.loginfo("???????????????????????????????????????????????????????????????") # rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start)) # rospy.loginfo("time is:\n" + str(time)) # rospy.loginfo("???????????????????????????????????????????????????????????????") p.time_from_start = time goal.trajectory.points.append(p) os.remove("/dev/shm/right_arm_goal_times.p") #print "Goal will be:" rospy.loginfo("!!!!!!!!!!!Goal:\n" + str(goal)) self.right_arm_as.send_goal(goal) rospy.loginfo("Waiting for result") self.right_arm_as.wait_for_result() rospy.loginfo("Goal done")
def receiverLoop(self): while True: try: input_msg = pickle.load( open( "/dev/shm/right_arm_goal.p", "rb" ) ) os.remove("/dev/shm/right_arm_goal.p") except IOError: print "file not there..." rospy.sleep(0.1) continue print input_msg goal = FollowJointTrajectoryGoal() rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" + str(input_msg.goal.goal_time_tolerance ) ) goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0) rospy.loginfo("input_msg.goal.goal_tolerance: \n" + str(input_msg.goal.goal_tolerance ) ) goal.goal_tolerance = input_msg.goal.goal_tolerance rospy.loginfo("input_msg.goal.path_tolerance: \n" + str(input_msg.goal.path_tolerance ) ) goal.path_tolerance = input_msg.goal.path_tolerance rospy.loginfo("input_msg.goal.trajectory.header: \n" + str(input_msg.goal.trajectory.header ) ) goal.trajectory.header = input_msg.goal.trajectory.header goal.trajectory.header.frame_id = '/base_link' goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names input_times = pickle.load( open( "/dev/shm/right_arm_goal_times.p", "rb" ) ) rospy.loginfo("input_times is:\n" + str(input_times)) for input_point, time in zip(input_msg.goal.trajectory.points, input_times): p = JointTrajectoryPoint() p.accelerations = input_point.accelerations p.positions = input_point.positions p.velocities = input_point.velocities # rospy.loginfo("???????????????????????????????????????????????????????????????") # rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start)) # rospy.loginfo("time is:\n" + str(time)) # rospy.loginfo("???????????????????????????????????????????????????????????????") p.time_from_start = time goal.trajectory.points.append(p) os.remove("/dev/shm/right_arm_goal_times.p") #print "Goal will be:" rospy.loginfo("!!!!!!!!!!!Goal:\n" +str(goal)) self.right_arm_as.send_goal(goal) rospy.loginfo("Waiting for result") self.right_arm_as.wait_for_result() rospy.loginfo("Goal done")
def receiverLoop(self): while True: print "self.conn.recv()" input_msg = self.conn.recv() print input_msg goal = FollowJointTrajectoryGoal() goal.goal_time_tolerance = input_msg.goal_time_tolerance goal.goal_tolerance = input_msg.goal_tolerance goal.path_tolerance = input_msg.path_tolerance goal.trajectory.header = input_msg.trajectory.header goal.trajectory.joint_names = input_msg.joint_names for input_point in input_msg.trajectory.points: p = JointTrajectoryPoint() p.accelerations = input_point.accelerations p.positions = input_point.positions p.velocities = input_point.velocities p.time_from_start = input_point.time_from_start goal.trajectory.points.append(p) print "Goal will be:" print goal #self.right_arm_as.send_goal(goal) rospy.loginfo("Waiting for result") self.right_arm_as.wait_for_result() rospy.loginfo("Goal done")
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))
pt3.name = 'joint_3' pt3.position = 10.1 pt3.velocity = 10.1 pt3.acceleration = 10.1 pt4 = JointTolerance() pt4.name = 'joint_4' pt4.position = 10.1 pt4.velocity = 10.1 pt4.acceleration = 10.1 pt5 = JointTolerance() pt5.name = 'joint_5' pt5.position = 10.1 pt5.velocity = 10.1 pt5.acceleration = 10.1 pt6 = JointTolerance() pt6.name = 'joint_6' pt6.position = 10.1 pt6.velocity = 10.1 pt6.acceleration = 0.1 goal.path_tolerance = [pt1, pt2, pt3, pt4, pt5, pt6] goal.goal_tolerance = [pt1, pt2, pt3, pt4, pt5, pt6] goal.goal_time_tolerance = rospy.Duration.from_sec(10) client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(100.0)) print(goal) rospy.spin()
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 on_enter(self, userdata): self.return_code = None self.current_action_topic = self.given_action_topic self.goal_names = None self.goal_values = None try: if self.current_action_topic is None: #Logger.logwarn(" %s - using the action topic to %s from %s for client!"%(self.name, userdata.trajectory_action_topic, self.current_action_topic) ) self.current_action_topic = userdata.trajectory_action_topic if self.action_client is None: self.action_client = ProxyActionClient( {self.current_action_topic: FollowJointTrajectoryAction}, self.wait_duration) Logger.loginfo('%s - created ProxyActionClient for %s!' % (self.name, self.current_action_topic)) if not self.action_client.is_available(self.current_action_topic): self.action_client.setupClient(self.current_action_topic, FollowJointTrajectoryAction, self.wait_duration) if not self.action_client.is_available( self.current_action_topic): self.status_text = '%s - no connection to %s available!' % ( self.name, self.current_action_topic) self.return_code = 'param_error' Logger.loginfo(self.status_text) return # Action Initialization action_goal = FollowJointTrajectoryGoal() if isinstance(userdata.joint_trajectory, RobotTrajectory): Logger.loginfo('%s - using RobotTrajectory ' % self.name) action_goal.trajectory = userdata.joint_trajectory.joint_trajectory elif isinstance(userdata.joint_trajectory, JointTrajectory): Logger.loginfo('%s - using JointTrajectory ' % self.name) action_goal.trajectory = userdata.joint_trajectory else: self.status_text = '%s - user data does not have valid trajectory message!\n Must be RobotTrajectory or JointTrajectory type!' % ( self.name) self.return_code = 'param_error' Logger.logerr(self.status_text) return action_goal.path_tolerance = userdata.joint_path_tolerances action_goal.goal_tolerance = userdata.joint_goal_tolerances action_goal.goal_time_tolerance = self.goal_time_tolerance # Validate that the trajectory and constraints are consistent num_joints = len(action_goal.trajectory.points[0].positions) num_path = len(action_goal.path_tolerance) num_goal = len(action_goal.goal_tolerance) if num_joints != num_path: self.status_text = '%s - Invalid number of path constraints %d vs. %d for %s' % ( self.name, num_path, num_joints, self.current_action_topic) self.return_code = 'param_error' if num_joints != num_goal: self.status_text = '%s - Invalid number of goal constraints %d vs. %d for %s' % ( self.name, num_path, num_joints, self.current_action_topic) self.return_code = 'param_error' for c, name in enumerate(action_goal.trajectory.joint_names): if action_goal.path_tolerance[c].name != name: self.status_text = '%s - %s name mismatch %s vs. %s for path constraints' % ( self.name, self.current_action_topic, action_goal.path_tolerance[c].name, name) self.return_code = 'param_error' if action_goal.goal_tolerance[c].name != name: self.status_text = '%s - %s name mismatch %s vs. %s for goal constraints' % ( self.name, self.current_action_topic, action_goal.goal_tolerance[c].name, name) self.return_code = 'param_error' if self.return_code is not None: # Something went wrong Logger.logerr(self.status_text) return # Check that our trajectory is still fresh elapsed = rospy.Time.now() - action_goal.trajectory.header.stamp if self.max_delay is not None and elapsed > self.max_delay: self.status_text = '%s - Stale trajectory - %f > %f - do not send action for %s' % ( self.name, elapsed.to_sec(), self.max_delay.to_sec(), self.current_action_topic) self.return_code = 'param_error' Logger.logerr(self.status_text) return elif elapsed.to_sec() > 2.0: Logger.logwarn( "%s - trajectory older than %f - send anyway!" % (self.name, elapsed.to_sec())) if (self.timeout_duration > rospy.Duration(0.0)): self.timeout_target = rospy.Time.now( ) + action_goal.trajectory.points[ -1].time_from_start + self.timeout_duration else: self.timeout_target = None # Good to go for it! action_goal.trajectory.header.stamp = rospy.Time( 0.0 ) # Start from the beginning of the trajectory with this node self.action_client.send_goal(self.current_action_topic, action_goal) self.goal_names = action_goal.trajectory.joint_names self.goal_values = action_goal.trajectory.points[-1].positions except Exception as e: self.status_text = '%s - Failed to send action for %s\n%s' % ( self.name, self.current_action_topic, str(e)) self.return_code = 'param_error' Logger.logerr(self.status_text) return