Baxter机器人末端轨迹还原（笛卡尔规划）

￡神魔★判官ぃ 2022-06-17 10:08 205阅读 0赞

调用命令：

rosrun baxter_moveit_config endpoint_trajectory_playback.py -f filename joint_states:=/robot/joint_states #此处filename为存储的末端轨迹数据的文件名

#!/usr/bin/env python
    import rospy,sys
    import argparse
    import moveit_commander
    import numpy as np
    from geometry_msgs.msg import PoseStamped,Pose
    from moveit_commander import MoveGroupCommander
    import time
    from copy import deepcopy
    from os import path
    class MoveItDemo:
        def __init__ (self):
            moveit_commander.roscpp_initialize(sys.argv)
            self.left_arm  = moveit_commander.MoveGroupCommander( 'left_arm')
            self.right_arm = moveit_commander.MoveGroupCommander('right_arm')
            self.right_eef = self.right_arm.get_end_effector_link()
            self.left_eef = self.left_arm.get_end_effector_link()
            self.right_arm.allow_replanning(True)
            self.left_arm.allow_replanning(True)
            self.right_arm.set_goal_position_tolerance(0.5)
            self.right_arm.set_goal_orientation_tolerance(0.5)
            self.left_arm.set_goal_position_tolerance(0.1)
            self.left_arm.set_goal_orientation_tolerance(0.1)
            self.left_arm.set_planning_time(5)
            self.right_arm.set_planning_time(5)
        def execute(self,filename):
            with open(filename, 'r') as f:
                lines = f.readlines()
            waypoints1 = []
            waypoints2 = []
            waypoints_l_start = []
            waypoints_r_start = []
            i = 1
            for idx, values in enumerate(lines[1:]):
                print "Line:\n", i
                i = i + 1
                if i == len(lines):
                    break
                values = values.rstrip().split(',')
                values = [float(x) for x in values]
                print values
                if idx == 0:
                    print idx
                    start_pose1 = self.left_arm.get_current_pose(self.left_eef).pose
                    wpose1 = deepcopy(start_pose1)
                    wpose1.position.x = values[1]
                    wpose1.position.y= values[2]
                    wpose1.position.z = values[3]
                    wpose1.orientation.x = values[7]
                    wpose1.orientation.y = values[8]
                    wpose1.orientation.z = values[9]
                    wpose1.orientation.w = values[10]
    
                    start_pose2 = self.right_arm.get_current_pose(self.right_eef).pose
                    wpose2 = deepcopy(start_pose2)
                    wpose2.position.x = values[4]
                    wpose2.position.y = values[5]
                    wpose2.position.z = values[6]
                    wpose2.orientation.x = values[11]
                    wpose2.orientation.y = values[12]
                    wpose2.orientation.z = values[13]
                    wpose2.orientation.w = values[14]
    
                    waypoints_l_start.append(deepcopy(start_pose1))
                    waypoints_l_start.append(deepcopy(wpose1))
    
                    waypoints_r_start.append(deepcopy(start_pose2))
                    waypoints_r_start.append(deepcopy(wpose2))
    
                wpose1.position.x = values[1]
                wpose1.position.y = values[2]
                wpose1.position.z = values[3]
                wpose1.orientation.x = values[7]
                wpose1.orientation.y = values[8]
                wpose1.orientation.z = values[9]
                wpose1.orientation.w = values[10]
                waypoints1.append(deepcopy(wpose1))
                wpose2.position.x = values[4]
                wpose2.position.y = values[5]
                wpose2.position.z = values[6]
                wpose2.orientation.x = values[11]
                wpose2.orientation.y = values[12]
                wpose2.orientation.z = values[13]
                wpose2.orientation.w = values[14]
                waypoints2.append(deepcopy(wpose2))
            print "To start pose\n"
            (cartesian_plan_l,fraction) = self.left_arm.compute_cartesian_path(waypoints_l_start,0.01, 0.0, True)
            self.left_arm.execute(cartesian_plan_l)
            (cartesian_plan_r,fraction) = self.right_arm.compute_cartesian_path(waypoints_r_start,0.01, 0.0, True)
            self.right_arm.execute(cartesian_plan_r)
            rospy.sleep(3)
            print "start playback\n"
            (cartesian_plan_l,fraction) = self.left_arm.compute_cartesian_path(waypoints1,0.01, 0.0, True)
            self.left_arm.execute(cartesian_plan_l)
            (cartesian_plan_r,fraction) = self.right_arm.compute_cartesian_path(waypoints2,0.01, 0.0, True)
            self.right_arm.execute(cartesian_plan_r)
            rospy.sleep(3)
            print("Done.")
            moveit_commander.roscpp_shutdown()
            moveit_commander.os._exit(0)    
    def main():
        print("Initializing node... ")
        rospy.init_node('moveit_demo')
        rospy.sleep(1)
        epilog = """
    Related examples:
        joint_recorder.py; joint_position_file_playback.py.
        """
        arg_fmt = argparse.RawDescriptionHelpFormatter
        parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                        description=main.__doc__,
                                        epilog=epilog)
        parser.add_argument(
        '-f', '--file', metavar='PATH', required=True,
        help='path to input file'
        )
        args = parser.parse_args(rospy.myargv()[1:])
        moveit = MoveItDemo()
        moveit.execute(path.expanduser(args.file))
    if __name__ == "__main__":
        main()