Python FollowJointTrajectoryGoal.path_tolerance方法代码示例

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import path_tolerance [as 别名]
 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

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import path_tolerance [as 别名]
    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")

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import path_tolerance [as 别名]
 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")

# 需要导入模块: from control_msgs.msg import FollowJointTrajectoryGoal [as 别名]
# 或者: from control_msgs.msg.FollowJointTrajectoryGoal import path_tolerance [as 别名]
    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
#.........这里部分代码省略.........