Python URDF.get_chain方法代码示例

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import get_chain [as 别名]
class CalibrationSetupHelper:
    def __init__(self, robot_description, base_link, arm_root_link, arm_tip_link, head_root_link, head_tip_link, arm_controller, head_controller, head_camera_frame, head_camera_joint, camera_namespace):
        self.robot_description = robot_description
        self.base_link = base_link
        self.arm_root_link = arm_root_link
        self.arm_tip_link = arm_tip_link
        self.head_root_link = head_root_link
        self.head_tip_link = head_tip_link
        self.arm_controller = arm_controller
        self.head_controller = head_controller
        self.head_camera_frame = head_camera_frame
        self.head_camera_joint = head_camera_joint
        self.camera_namespace = camera_namespace

        self.robot = URDF().parse(self.robot_description)
        self.robot_name = self.robot.name.lower()

        if not self.base_link:
            self.base_link = self.robot.get_root()
        if self.robot.link_map.has_key(self.base_link) == False:
            self.error_tip_link(self.base_link, '--base-link')
        if self.robot.link_map.has_key(self.arm_root_link) == False:
            self.error_tip_link(self.arm_root_link, '--arm-root-link')
        if self.robot.link_map.has_key(self.head_root_link) == False:
            self.error_tip_link(self.head_root_link, '--head-root-link')
        if self.robot.link_map.has_key(self.arm_tip_link) == False:
            self.error_tip_link(self.arm_tip_link, '--arm-tip-link')
        if self.robot.link_map.has_key(self.head_tip_link) == False:
            self.error_tip_link(self.head_tip_link, '--head-tip-link')
        if self.robot.link_map.has_key(self.head_camera_frame) == False:
            self.error_tip_link(self.head_camera_frame, '--head-camera-frame')
        if self.robot.joint_map.has_key(self.head_camera_joint) == False:
            self.error_joint(self.head_camera_joint, '--head-camera-joint')

        all_chain = []
        for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
            all_chain.append(self.robot.get_chain(base_link, end_link)[1:])
        for c1,c2 in itertools.product(*all_chain):
            if c1 == c2 :
                rospy.logerr('arm/head chain share same joint ({}), this will cause failure'.format(c1))
                sys.exit()

        for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
            joint_list = filter(lambda x: self.robot.joint_map.has_key(x) and self.robot.joint_map[x].type != 'fixed', [c for c in self.robot.get_chain(base_link, end_link)])
            exec('self.{0}_joint_list = {1}'.format(limb, joint_list)) in locals()

        rospy.loginfo('using following joint for {} chain'.format('arm'))
        rospy.loginfo('... {}'.format(self.arm_joint_list))
        rospy.loginfo('using following joint for {} chain'.format('head'))
        rospy.loginfo('... {}'.format(self.head_joint_list))

        # create robot_calibration directory
        self.dirname_top = self.robot_name+'_calibration'
        self.dirname_capture = self.robot_name+'_calibration/capture_data'
        self.dirname_estimate = self.robot_name+'_calibration/estimate_params'
        self.dirname_results = self.robot_name+'_calibration/view_results'

        try:
            os.makedirs(self.dirname_top)
            os.makedirs(self.dirname_capture)
            os.makedirs(self.dirname_estimate)
            os.makedirs(self.dirname_results)
        except Exception as e:
            rospy.logfatal(e)
            #sys.exit(-1)

        # setup capture_data
        self.write_all_pipelines(args.use_kinect)       # all_pipelines.launch
        self.write_all_viewers(args.use_kinect)         # all_viewers.launch
        self.write_capture_data()                       # capture_data.launch
        self.write_capture_exec()                       # capture_exec.launch
        self.write_hardware_config()                    # hardware_config
        self.write_make_samples()                       # make_samples.py
        # setup estimate_params
        self.write_estimation_config()                  # estimation_config.launch
        self.write_calibrate_sh()                       # calibrate_<robot>.sh
        self.write_estimate_params_config(args.use_kinect)             # free_arms.yaml  free_cameras.yaml  free_cb_locations.yaml  free_torso.yaml  system.yaml
        # setup view_results
        self.write_view_results()                       # scatter_config.yaml  view_scatter.sh


    def error_tip_link(self, link, option):
        rospy.logfatal('could not find valid link name ... {}'.format(link))
        rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.links)))
        f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
        print >>f, self.robot_description
        f.close()
        rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
        sys.exit(-1)

    def error_joint(self, joint, option):
        rospy.logfatal('could not find valid joint name ... {}'.format(joint))
        rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.joints)))
        f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
        print >>f, self.robot_description
        f.close()
        rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
        sys.exit(-1)

    def write_all_pipelines(self, use_kinect):
#.........这里部分代码省略.........

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import get_chain [as 别名]
class Hybrid():
    def __init__(self):
        rospy.init_node('hybrid_node')
        self.freq = 100
        self.rate = rospy.Rate(self.freq)  # 100 hz

        # pub
        self.pub_test = rospy.Publisher('/hybrid/test', String)

        # sub
        self.sub_jr3 = rospy.Subscriber('/jr3/wrench', WrenchStamped, self.cb_jr3)
        self.sub_joy = rospy.Subscriber('/spacenav/joy', Joy, self.cb_joy)
        self.sub_enable = rospy.Subscriber('/hybrid/enable', Bool, self.cb_enable)
        self.sub_cmd = rospy.Subscriber('/hybrid/cmd', String, self.cb_cmd)

        # tf 
        self.ler = tf.TransformListener()  # listener
        self.ber = tf.TransformBroadcaster() # broadcaster

        # datas
        self.enabled = False
        self.cmdfrm = Frame()
        self.wrench = Wrench()

        self.cmdtwist = Twist()
        self.urdf = rospy.get_param('/wam/robot_description')
        print self.urdf
        
        self.robot = URDF()
        self.robot = self.robot.from_xml_string(self.urdf)
        self.chain = self.robot.get_chain('base_link',
                                          'cutter_tip_link')

        print self.chain
        pass

    def cb_jr3(self, msg):
        self.wrench.force = Vector(msg.wrench.force.x,
                                   msg.wrench.force.y,
                                   msg.wrench.force.z)
        self.wrench.torque = Vector(msg.wrench.torque.x,
                                    msg.wrench.torque.y,
                                    msg.wrench.torque.z)
        pass

    def cb_joy(self, msg):
        # might want to rotate this twist 
        # self.cmdtwist.vel = Vector(msg.axes[0], msg.axes[1], msg.axes[2])
        # self.cmdtwist.rot = Vector(msg.axes[3], msg.axes[4], msg.axes[5])

        # align twist to be same as base link
        self.cmdtwist.vel = Vector(msg.axes[2], msg.axes[1], -msg.axes[0])
        self.cmdtwist.rot = Vector(msg.axes[5], msg.axes[4], -msg.axes[3])
        # print self.cmdtwist
        pass

    def cb_enable(self, msg):
        self.enabled = msg.data
        if msg.data:
            # sync cmd pos/rot
            rospy.loginfo("enable hybrid force control")
            try:
                (trans, rot) = \
                        self.ler.lookupTransform('wam/base_link',
                                                 'wam/cutter_tip_link',
                                                 rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                print "tf read error"
            
            self.cmdfrm.p = Vector(trans[0], trans[1], trans[2])
            self.cmdfrm.M = Rotation.Quaternion(rot[0], rot[1], rot[2], rot[3])
            
            print self.cmdfrm
        pass

    def cb_cmd(self, msg):

        pass
        
    def ctrl_hybrid(self):
        # massage cmd frm with force controller
        # z: force control on z axis
        cmdforce = -4  # -4N on z axis
        errforce = cmdforce - self.wrench.force.z()
        print self.wrench.force
        pass

    def run(self):
        while not rospy.is_shutdown():
            msg = String()
            msg.data = 'yeah'
            self.pub_test.publish(msg)

            # update states

            # option 1: direct
            
            # option 2: hybrid

            if self.enabled:
#.........这里部分代码省略.........