Python代码创建URDF-动态建模

本文记录如何实现通过Python代码实现urdf的动态建模和加载，适用于可变尺寸的通用建模方法，抛弃传统的文件建模手段。

设计背景

需求：塔吊任意型号和尺寸的建模和仿真，并同步启动Moveit控制算法。

解决方案

工具

urdf_parser_py，

改进

urdf_parser_py并不完善，其具备joint、link、transmition等内容，但是缺少gazebo ros control控制插件的导入，改进的urdf_parser_py包放在GitHub - casiarobot/urdf_parser_py。

动态建塔吊的开源代码

        # if len(msg.msg.h1)
        if msg.h1 <1 or msg.msg.h2 <1  or msg.msg.h3 < 1 or msg.msg.l1 <1 or msg.msg.l2 < 1:
            msg.name = "robot"
            msg.name_space = "ns"
            msg.h1 = 120
            msg.h2 = 10
            msg.h3 = 20
            msg.l1 = 20
            msg.l2 = 150
            msg.box_w = 1
            msg.box_h = 1
            msg.density = 7000
            msg.car_weight = 200
            msg.hook_weight = 1
            msg.counter_weight = 1000
            msg.hook_limit.effort = 1000
            msg.hook_limit.velocity = 20
            msg.hook_limit.upper = 100
            msg.hook_limit.lower = 10

            msg.car_limit.effort = 1000
            msg.car_limit.velocity = 20
            msg.car_limit.upper = 100
            msg.car_limit.lower = 10

            msg.pivot_limit.effort = 1000
            msg.pivot_limit.velocity = 2
            msg.pivot_limit.upper = 3.14
            msg.pivot_limit.lower = -3.14
            msg.control_type = 0
        else:
            print(msg)

        weight_base = 900000
        weight_shoulder_base = 100
        weight_shoulder_top = 100
        weight_shoulder_right = 100 
        
        model = DeleteModelRequest()
        model.model_name = 'robot'
        self.model_del_srv.call(model)
        # rospy.sleep(2)
        os.system('rosnode kill controller_spawner ')
        os.system('rosnode kill gazebo_controller_spawner ')
        rospy.sleep(2)
        robot = urdf.Robot(name='crane_tower', version='1.0')

        base_link = urdf.Link(name='base_link')
        base_link.visual = urdf.Visual(geometry=urdf.Box([msg.box_w,msg.box_h,msg.h1]),origin=urdf.Pose(xyz=(0, 0, msg.h1/2),rpy=(0,0,0)), material=urdf.Material(name="orange"))
        base_link.collision = urdf.Collision(geometry=urdf.Box([msg.box_w,msg.box_h,msg.h1]),origin=urdf.Pose(xyz=(0, 0, msg.h1/2),rpy=(0,0,0)))
        base_link.inertial = urdf.Inertial(mass=weight_base, inertia=urdf.Inertia(23419752200000,0,0,2341975220000,0,100000000000))
        base_link.origin = urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0))
        robot.add_link(base_link)

        shoulder_link_base = urdf.Link(name='shoulder_link_base')
        shoulder_link_base.visual = urdf.Visual(geometry=urdf.Box([msg.box_w,msg.box_h,msg.h2]),origin=urdf.Pose(xyz=(0, 0, msg.h2/2),rpy=(0,0,0)),material=urdf.Material(name="orange"))
        shoulder_link_base.collision = urdf.Collision(geometry=urdf.Box([msg.box_w,msg.box_h,msg.h2]),origin=urdf.Pose(xyz=(0, 0, msg.h2/2),rpy=(0,0,0)))
        shoulder_link_base.inertial = urdf.Inertial(mass=weight_shoulder_base, inertia=urdf.Inertia(msg.density* (msg.box_w* msg.box_h * msg.h2)/12,0,0,msg.density* (msg.box_w* msg.box_h * msg.h2)/12,0,1))
        shoulder_link_base.origin = urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0))
        robot.add_link(shoulder_link_base)

        shoulder_link_top = urdf.Link(name='shoulder_link_top')
        shoulder_link_top.visual = urdf.Visual(geometry=urdf.Box([msg.box_w,msg.box_h,msg.h3]),origin=urdf.Pose(xyz=(0, 0, msg.h3/2),rpy=(0,0,0)),material=urdf.Material(name="orange"))
        shoulder_link_top.collision = urdf.Collision(geometry=urdf.Box([msg.box_w,msg.box_h,msg.h3]),origin=urdf.Pose(xyz=(0, 0, msg.h3/2),rpy=(0,0,0)))
        shoulder_link_top.inertial = urdf.Inertial(mass=weight_shoulder_top, inertia=urdf.Inertia(1,0,0,1,0,1))
        shoulder_link_top.origin = urdf.Pose(xyz=(0, 0, msg.h3/2),rpy=(0,0,0))
        robot.add_link(shoulder_link_top)

        shoulder_link_right = urdf.Link(name='shoulder_link_right')
        shoulder_link_right.visual = urdf.Visual(geometry=urdf.Box([msg.l2, msg.box_w,msg.box_h]),origin=urdf.Pose(xyz=(msg.l2/2 - msg.l1, 0, 0),rpy=(0,0,0)),material=urdf.Material(name="orange"))
        shoulder_link_right.collision = urdf.Collision(geometry=urdf.Box([msg.l2, msg.box_w,msg.box_h]),origin=urdf.Pose(xyz=(msg.l2/2 - msg.l1, 0, 0),rpy=(0,0,0)))
        shoulder_link_right.inertial = urdf.Inertial(weight_shoulder_right, inertia=urdf.Inertia(100,0,0,100,0,msg.density* (msg.box_w* msg.box_h * msg.l2) * msg.l2*msg.l2/3))
        shoulder_link_right.origin = urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0))
        robot.add_link(shoulder_link_right)

        shoulder_link_left = urdf.Link(name='shoulder_link_left')
        mesh = urdf.Mesh(filename=rospkg.RosPack().get_path('sim_api') + '/model/meshes/car_link.stl')
        shoulder_link_left.visual = urdf.Visual(geometry=mesh)
        shoulder_link_left.collision = urdf.Collision(geometry=mesh)
        shoulder_link_left.inertial = urdf.Inertial(msg.counter_weight, inertia=urdf.Inertia(1,0,0,1,0,msg.counter_weight*msg.l1*msg.l1))
        shoulder_link_left.origin = urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0))
        robot.add_link(shoulder_link_left)

        #fixed joint for shape group
        shoulder_joint1 = urdf.Joint(name="shoulder_joint_top")
        shoulder_joint1.parent = "shoulder_link_base"
        shoulder_joint1.child  = "shoulder_link_top"
        shoulder_joint1.type = "fixed"
        shoulder_joint1.origin = urdf.Pose(xyz=(0,0,0))
        robot.add_joint(shoulder_joint1)

        shoulder_joint2 = urdf.Joint(name="shoulder_joint_right")
        shoulder_joint2.parent = "shoulder_link_base"
        shoulder_joint2.child  = "shoulder_link_right"
        shoulder_joint2.type = "fixed"
        robot.add_joint(shoulder_joint2)

        shoulder_joint3 = urdf.Joint(name="shoulder_joint_left")
        shoulder_joint3.parent = "shoulder_link_right"
        shoulder_joint3.child  = "shoulder_link_left"
        shoulder_joint3.type = "fixed"
        shoulder_joint3.origin = urdf.Pose(xyz=(-msg.l1,0,0),rpy=(0,0,1.5707))
        robot.add_joint(shoulder_joint3)

        # control joint, pivot\car\hook

        joint_pivot = urdf.Joint(name="joint_pivot")
        joint_pivot.axis = (0, 0, 1)
        joint_pivot.type = "revolute"
        joint_pivot.parent = "base_link"
        joint_pivot.child = "shoulder_link_base"
        joint_pivot.dynamics = urdf.JointDynamics(1,0.5)
        joint_pivot.limit = urdf.JointLimit(msg.pivot_limit.effort,msg.pivot_limit.velocity, msg.pivot_limit.lower,msg.pivot_limit.upper)
        joint_pivot.origin = urdf.Pose(xyz=(0, 0, msg.h1 + msg.box_h))

        robot.add_joint(joint_pivot)


        #link car 
        car_link = urdf.Link(name='car_link')
        mesh = urdf.Mesh(filename=rospkg.RosPack().get_path('sim_api') + '/model/meshes/car_link.stl')
        car_link.visual = urdf.Visual(geometry=mesh)
        car_link.collision = urdf.Collision(geometry=mesh)
        # car_link.visual = urdf.Visual(geometry=urdf.Box([2,1,1]),origin=urdf.Pose(xyz=(1, 0.5, 0.5),rpy=(0,0,0)), material=urdf.Material(name="orange"))
        # car_link.collision = urdf.Collision(geometry=urdf.Box([2,1,1]),origin=urdf.Pose(xyz=(1, 0.5, 0.5),rpy=(0,0,0)),)
        car_link.inertial = urdf.Inertial(mass=msg.car_weight, inertia=urdf.Inertia(msg.car_weight*1,0,0,msg.car_weight*1,0,msg.car_weight*1),origin=urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0)))
        car_link.origin = urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0))
        robot.add_link(car_link)

        #joint car

        joint_car = urdf.Joint(name="joint_car")
        joint_car.axis = (-1, 0, 0)
        joint_car.type = "prismatic"
        joint_car.parent = "shoulder_link_right"
        joint_car.child = "car_link"
        joint_car.dynamics = urdf.JointDynamics(100,0.5)
        joint_car.limit = urdf.JointLimit(msg.car_limit.effort,msg.car_limit.velocity, msg.car_limit.lower, msg.car_limit.upper)
        joint_car.origin = urdf.Pose(xyz=(10, 0, -msg.box_w/2),rpy=(0,0,3.1415926))
        robot.add_joint(joint_car)


        # hook link
        hook_link = urdf.Link(name='hook_link')
        mesh = urdf.Mesh(filename=rospkg.RosPack().get_path('sim_api') + '/model/meshes/hook_link.stl')
        hook_link.visual = urdf.Visual(geometry=mesh,origin=urdf.Pose(xyz=(0,0,0),rpy=(3.1415926,0,0)))
        hook_link.collision = urdf.Collision(geometry=mesh,origin=urdf.Pose(xyz=(0,0,0),rpy=(3.1415926,0,0)))
        # hook_link.visual = urdf.Visual(geometry=urdf.Box([0.3,0.3,0.5]),material=urdf.Material(name="orange"),origin=urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0)))
        # hook_link.collision = urdf.Collision(geometry=urdf.Box([0.3,0.3,0.5]),origin=urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0)))
        hook_link.inertial = urdf.Inertial(mass=msg.hook_weight, inertia=urdf.Inertia(msg.hook_weight*0.25,0,0,msg.hook_weight*0.25,0,msg.hook_weight*0.25))
        hook_link.origin = urdf.Pose(xyz=(0, 0, 0),rpy=(0,0,0))
        robot.add_link(hook_link)

        # joint hook
        joint_hook = urdf.Joint(name="joint_hook")
        joint_hook.axis = (0, 0, 1)
        joint_hook.type = "prismatic"
        joint_hook.parent = "car_link"
        joint_hook.child = "hook_link"
        joint_hook.dynamics = urdf.JointDynamics(100,0.5)
        joint_hook.limit = urdf.JointLimit(msg.hook_limit.effort,msg.hook_limit.velocity, msg.hook_limit.lower, msg.h1)
        joint_hook.origin = urdf.Pose(xyz=(0, 0, 0),rpy=(3.1415926,0,0))
        robot.add_joint(joint_hook)

        # below for gazebo contollers configuration
        # pivot trans
        trans = urdf.Transmission(name='simple_trans_pivot')
        trans.type = 'transmission_interface/SimpleTransmission'
        joint = urdf.TransmissionJoint(name='joint_pivot')
        if msg.control_type == 0:
            joint.add_aggregate('hardwareInterface', 'VelocityJointInterface')
        elif msg.control_type == 1:
            joint.add_aggregate('hardwareInterface', 'PositionJointInterface')
        elif msg.control_type == 2:
            joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
        trans.add_aggregate('joint', joint)

        actuator = urdf.Actuator(name='pivot_motor')
        actuator.mechanicalReduction = 1
        trans.add_aggregate('actuator', actuator)
        robot.add_aggregate('transmission', trans)

        # car trans
        trans = urdf.Transmission(name='simple_trans_car')
        trans.type = 'transmission_interface/SimpleTransmission'
        joint = urdf.TransmissionJoint(name='joint_car')
        if msg.control_type == 0:
            joint.add_aggregate('hardwareInterface', 'VelocityJointInterface')
        elif msg.control_type == 1:
            joint.add_aggregate('hardwareInterface', 'PositionJointInterface')
        elif msg.control_type == 2:
            joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
        trans.add_aggregate('joint', joint)

        actuator = urdf.Actuator(name='car_motor')
        actuator.mechanicalReduction = 1
        trans.add_aggregate('actuator', actuator)
        robot.add_aggregate('transmission', trans)

        # trans hook
        trans = urdf.Transmission(name='simple_trans_hook')
        trans.type = 'transmission_interface/SimpleTransmission'
        joint = urdf.TransmissionJoint(name='joint_hook')
        if msg.control_type == 0:
            joint.add_aggregate('hardwareInterface', 'VelocityJointInterface')
        elif msg.control_type == 1:
            joint.add_aggregate('hardwareInterface', 'PositionJointInterface')
        elif msg.control_type == 2:
            joint.add_aggregate('hardwareInterface', 'EffortJointInterface')
        trans.add_aggregate('joint', joint)

        actuator = urdf.Actuator(name='hook_motor')
        actuator.mechanicalReduction = 1
        trans.add_aggregate('actuator', actuator)
        robot.add_aggregate('transmission', trans)

        gazebo = urdf.Gazebo(plugin=urdf.Plugin())
        robot.add_aggregate('gazebo', gazebo)

 
        robot_xml_string = robot.to_xml_string()
        print(robot_xml_string)

        
        initial_pose = Pose()
        initial_pose.position.x = msg.x
        initial_pose.position.y = msg.y
        initial_pose.position.z = msg.z
        initial_pose.orientation.x = tft.quaternion_from_euler(msg.rx, msg.ry, msg.rz)[0]
        initial_pose.orientation.y = tft.quaternion_from_euler(msg.rx, msg.ry, msg.rz)[1]
        initial_pose.orientation.z = tft.quaternion_from_euler(msg.rx, msg.ry, msg.rz)[2]
        initial_pose.orientation.w = tft.quaternion_from_euler(msg.rx, msg.ry, msg.rz)[3]

        

        # f = open(rospack.RosPack().get_path('sim_api') + '/urdf/robot.urdf',"w")
        # f.write(robot_xml_string)
        # f.close()

        # gz_ros.spawn_urdf_model_client(msg.name_space+"/"+msg.name, robot_xml_string,"robot",initial_pose,"","gazebo")
        gz_ros.spawn_urdf_model_client('robot', robot_xml_string,"robot",initial_pose,"","gazebo")

        # os.system("rosrun controller_manager spawner joint_state_controller")
        # os.system('rosrun controller_manager spawner arm_controller')
        os.system('roslaunch sim_api config.launch')