本文记录如何实现通过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')