Python URDF.from_xml_string方法代码示例

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
    def __init__(self ,urdf=None):
        if urdf is None:
            print 'FROM PARAM SERVER'
            self._youbot = URDF.from_parameter_server(key='robot_description')
        else:
            print 'FROM STRING'
            self._youbot = URDF.from_xml_string(urdf)

        self._kdl_tree = kdl_tree_from_urdf_model(self._youbot)
        self._base_link = 'arm_link_0'
        print "ROOT : ",self._base_link
        self._tip_link = 'arm_link_5'
        print "self._tip_link : ", self._tip_link
        self._tip_frame = PyKDL.Frame()
        self._arm_chain = self._kdl_tree.getChain(self._base_link,
                                                  self._tip_link)

        # Baxter Interface Limb Instances
        #self._limb_interface = youbot_interface.Limb(limb)
        #self._joint_names = self._limb_interface.joint_names()
        self._joint_names = ['arm_joint_1', 'arm_joint_2', 'arm_joint_3', 'arm_joint_4', 'arm_joint_5']
        self._num_jnts = len(self._joint_names)

        # KDL Solvers
        self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain)
        self._fk_v_kdl = PyKDL.ChainFkSolverVel_recursive(self._arm_chain)
        self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain)
        self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_chain,
                                                   self._fk_p_kdl,
                                                   self._ik_v_kdl)
        self._jac_kdl = PyKDL.ChainJntToJacSolver(self._arm_chain)
        self._dyn_kdl = PyKDL.ChainDynParam(self._arm_chain,
                                            PyKDL.Vector.Zero())

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
def create_kdl_kin(base_link, end_link, urdf_filename=None):
    if urdf_filename is None:
        robot = URDF.from_parameter_server()
    else:
        f = open(urdf_filename, "r")
        robot = URDF.from_xml_string(f.read())
    return KDLKinematics(robot, base_link, end_link)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
 def __init__(self):
   super(RobotHeadWidget, self).__init__()
   self.lock = Lock()
   try:
     robot_model = URDF.from_xml_string(rospy.get_param("/robot_description"))
   except Exception, e:
     self.showError("Failed to load /robot_description: " + e.message)
     return

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
    def __init__(self, sim=True):

        f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
        robot = URDF.from_xml_string(f.read())  # parsed URDF

        # robot = URDF.from_parameter_server()
        self.kin = KDLKinematics(robot, 'base', 'mconn')

        # Selection of end-effector parameters for WidowX
        # x, y, z, yaw, pitch
        self.cart_from_matrix = lambda T: np.array([
            T[0, 3],
            T[1, 3],
            T[2, 3],
            math.atan2(T[1, 0], T[0, 0]),
            -math.asin(T[2, 0])])

        self.home = np.array([0.05, 0, 0.25, 0, 0])     # home end-effector pose

        self.q = np.array([0, 0, 0, 0, 0])              # joint angles
        self.dt = 0.05                                  # algorithm time step
        self.sim = sim                                  # true if virtual robot

        def publish(eventStop):

            # for arbotix
            pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
            pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
            pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
            pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
            pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)

            # for visualization
            jointPub = rospy.Publisher(
                "/joint_states", JointState, queue_size=5)
            jmsg = JointState()
            jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']

            while not rospy.is_shutdown() and not eventStop.is_set():

                jmsg.header.stamp = rospy.Time.now()
                jmsg.position = self.q
                if self.sim:
                    jointPub.publish(jmsg)

                pub1.publish(self.q[0])
                pub2.publish(self.q[1])
                pub3.publish(self.q[2])
                pub4.publish(self.q[3])
                pub5.publish(self.q[4])

                eventStop.wait(self.dt)

        rospy.init_node("robot")
        eventStop = threading.Event()
        threadJPub = threading.Thread(target=publish, args=(eventStop,))
        threadJPub.daemon = True
        threadJPub.start()  # Update joint angles in a background process

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
def main():
    parser = argparse.ArgumentParser(usage='Load an URDF file')
    parser.add_argument('file', type=argparse.FileType('r'), nargs='?',
                        default=None, help='File to load. Use - for stdin')
    parser.add_argument('-o', '--output', type=argparse.FileType('w'),
                        default=None, help='Dump file to XML')
    args = parser.parse_args()

    if args.file is None:
        print 'FROM PARAM SERVER'
        robot = URDF.from_parameter_server()
    else:
        print 'FROM STRING'
        robot = URDF.from_xml_string(args.file.read())

    print(robot)

    if args.output is not None:
        args.output.write(robot.to_xml_string())

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
 def from_ros_params(cls):
     '''
     Initialize class from URDF and /robot_paramters/ ros param set by upload.launch
     '''
     # Parse URDF of Sub for inertial information, etc
     urdf_string = rospy.get_param('/robot_description')
     urdf = URDF.from_xml_string(urdf_string)
     # Get root link of model
     root = urdf.link_map[urdf.get_root()]
     mass = root.inertial.mass
     rotational_inertia = np.array(root.inertial.inertia.to_matrix())
     drag_linear = np.array(rospy.get_param('/robot_parameters/drag/linear_coeffs'))
     drag_angular = np.array(rospy.get_param('/robot_parameters/drag/angular_coeffs'))
     drag_coeffs = np.hstack((drag_linear, drag_angular)).T
     volume = rospy.get_param('/robot_parameters/volume')
     height = rospy.get_param('/robot_parameters/height')
     water_density = rospy.get_param('/robot_parameters/fluid_density')
     air_density = rospy.get_param('/robot_parameters/air_density')
     G = rospy.get_param('/robot_parameters/G')
     return cls(mass, rotational_inertia, volume, drag_coeffs, height,
                water_density, air_density, G)

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]

#.........这里部分代码省略.........
        temp_bg_color = self.barrett_red_dark.darker()
        temp_fill_color = self.barrett_orange
        temp_alarm_color = self.barrett_red

        for w in jp_widgets + jn_widgets:
            w.setAlarmLevel(0.80)
            w.setFillColor(joint_fill_color)
            w.setAlarmColor(joint_alarm_color)
            p = w.palette()
            p.setColor(tp.backgroundRole(), joint_bg_color)
            w.setPalette(p)

        for w in tp_widgets + tn_widgets:
            w.setAlarmLevel(0.66)
            w.setFillColor(torque_fill_color)
            w.setAlarmColor(torque_alarm_color)
            p = w.palette()
            p.setColor(tp.backgroundRole(), torque_bg_color)
            w.setPalette(p)

        self._widget.hand_temp.setAlarmLevel(0.66)
        self._widget.hand_temp.setFillColor(temp_fill_color)
        self._widget.hand_temp.setAlarmColor(temp_alarm_color)
        p = self._widget.hand_temp.palette()
        p.setColor(self._widget.hand_temp.backgroundRole(), temp_bg_color)
        self._widget.hand_temp.setPalette(p)
        self._widget.hand_temp.setOrientation(Qt.Horizontal, QwtThermo.RightScale)

        self._widget.jf_0.setStyleSheet("QLabel { background-color : rgb(%d,%d,%d); color : rgb(%d,%d,%d); }" % (
            joint_bg_color.red(), joint_bg_color.green(), joint_bg_color.blue(), joint_fill_color.red(), joint_fill_color.green(), joint_fill_color.blue()))

        self.urdf = rospy.get_param('robot_description')
        self.robot = URDF()
        self.robot = self.robot.from_xml_string(self.urdf)

        self.pos_norm = [0] * 7
        self.torque_norm = [0] * 7

        self._joint_sub = rospy.Subscriber(
                'joint_states',
                sensor_msgs.msg.JointState,
                self._joint_state_cb)

        self._status_sub = rospy.Subscriber(
                'status',
                oro_barrett_msgs.msg.BarrettStatus,
                self._status_cb)

        self._hand_status_sub = rospy.Subscriber(
                'hand/status',
                oro_barrett_msgs.msg.BHandStatus,
                self._hand_status_cb)

        self._update_status(-1)
        self.safety_mode = -1
        self.run_mode = 0
        self.hand_run_mode = 0
        self.hand_min_temperature = 40.0
        self.hand_max_temperature = 65.0
        self.hand_temperature = 0.0

        self.update_timer = QTimer(self)
        self.update_timer.setInterval(50)
        self.update_timer.timeout.connect(self._update_widget_values)
        self.update_timer.start()

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
        text.fg_color.b = color[2] / 255.0
        text.fg_color.a = 1.0
        text.bg_color.a = 0.0
        text.text = "%dC -- (%s)"  % (int(max_temparature), max_temparature_name)
        g_text_publisher.publish(text)

def jointStatesCallback(msg):
    global g_effort_publishers, robot_model
    values = msg.effort
    names = msg.name
    allocateEffortPublishers(names)
    for val, name in zip(values, names):
        # lookup effort limit
        candidate_joints = [j for j in robot_model.joints if j.name == name]
        if candidate_joints:
            if candidate_joints[0].limit:
                limit = candidate_joints[0].limit.effort
            else:
                limit = 0
            if limit != 0:
                g_effort_publishers[name].publish(Float32(abs(val/limit)))

if __name__ == "__main__":
    rospy.init_node("motor_state_temperature_decomposer")
    robot_model = URDF.from_xml_string(rospy.get_param("/robot_description"))
    g_text_publisher = rospy.Publisher("max_temparature_text", OverlayText)
    s = rospy.Subscriber("/motor_states", MotorStates, motorStatesCallback, queue_size=1)
    s_joint_states = rospy.Subscriber("/joint_states", JointState, jointStatesCallback, queue_size=1)
    #s = rospy.Subscriber("joint_states", MotorStates, motorStatesCallback)
    rospy.spin()

# 需要导入模块: from urdf_parser_py.urdf import URDF [as 别名]
# 或者: from urdf_parser_py.urdf.URDF import from_xml_string [as 别名]
#!/usr/bin/env python

import sys
import argparse

from urdf_parser_py.urdf import URDF

parser = argparse.ArgumentParser(usage='Load an URDF file')
parser.add_argument('file', type=argparse.FileType('r'), nargs='?', default=None, help='File to load. Use - for stdin')
parser.add_argument('-o', '--output', type=argparse.FileType('w'), default=None, help='Dump file to XML')
args = parser.parse_args()

if args.file is None:
    print 'VERIFYING FROM PARAM SERVER'
    robot = URDF.from_parameter_server()
else:
    print 'VERIFYING FROM STRING'
    robot = URDF.from_xml_string(args.file.read())

print(robot)
print "ALL IS WELL! URDF PASSED PARSING!"

if args.output is not None:
    args.output.write(robot.to_xml_string())

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