mirror of
https://github.com/PotentiaRobotics/Fusion2PyBullet-Redux.git
synced 2025-04-09 23:00:18 -04:00
178 lines
6.5 KiB
Python
178 lines
6.5 KiB
Python
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Sun May 12 20:46:26 2019
|
|
|
|
@author: syuntoku
|
|
"""
|
|
|
|
import adsk, os, re
|
|
from xml.etree.ElementTree import Element, SubElement
|
|
from . import Link, Joint
|
|
from ..utils import utils
|
|
|
|
def write_link_urdf(joints_dict, repo, links_xyz_dict, file_name, inertial_dict):
|
|
"""
|
|
Write links information into urdf "repo/file_name"
|
|
|
|
|
|
Parameters
|
|
----------
|
|
joints_dict: dict
|
|
information of the each joint
|
|
repo: str
|
|
the name of the repository to save the xml file
|
|
links_xyz_dict: vacant dict
|
|
xyz information of the each link
|
|
file_name: str
|
|
urdf full path
|
|
inertial_dict:
|
|
information of the each inertial
|
|
|
|
Note
|
|
----------
|
|
In this function, links_xyz_dict is set for write_joint_tran_urdf.
|
|
The origin of the coordinate of center_of_mass is the coordinate of the link
|
|
"""
|
|
with open(file_name, mode='a') as f:
|
|
# for base_link
|
|
center_of_mass = inertial_dict['base_link']['center_of_mass']
|
|
link = Link.Link(name='base_link', xyz=[0,0,0],
|
|
center_of_mass=center_of_mass, repo=repo,
|
|
mass=inertial_dict['base_link']['mass'],
|
|
inertia_tensor=inertial_dict['base_link']['inertia'])
|
|
links_xyz_dict[link.name] = link.xyz
|
|
link.make_link_xml()
|
|
f.write(link.link_xml)
|
|
f.write('\n')
|
|
|
|
# others
|
|
for joint in joints_dict:
|
|
num_child = 0
|
|
for joint_search in joints_dict:
|
|
if joints_dict[joint]['child'] == joints_dict[joint_search]['child']:
|
|
num_child += 1
|
|
|
|
if num_child > 1:
|
|
app = adsk.core.Application.get()
|
|
ui = app.userInterface
|
|
ui.messageBox("Component %s with more than one child connection.\
|
|
\nThis mostly happens when you connect several subcomponents to different parents.\
|
|
\nBe aware to threat nested componets as a singel component!"
|
|
% (joints_dict[joint]['child']), "Error!")
|
|
quit()
|
|
else:
|
|
name = re.sub('[ :()]', '_', joints_dict[joint]['child'])
|
|
center_of_mass = \
|
|
[ i-j for i, j in zip(inertial_dict[name]['center_of_mass'], joints_dict[joint]['xyz'])]
|
|
link = Link.Link(name=name, xyz=joints_dict[joint]['xyz'],\
|
|
center_of_mass=center_of_mass,\
|
|
repo=repo, mass=inertial_dict[name]['mass'],\
|
|
inertia_tensor=inertial_dict[name]['inertia'])
|
|
links_xyz_dict[link.name] = link.xyz
|
|
link.make_link_xml()
|
|
f.write(link.link_xml)
|
|
f.write('\n')
|
|
|
|
|
|
def write_joint_tran_urdf(joints_dict, repo, links_xyz_dict, file_name):
|
|
"""
|
|
Write joints and transmission information into urdf "repo/file_name"
|
|
|
|
|
|
Parameters
|
|
----------
|
|
joints_dict: dict
|
|
information of the each joint
|
|
repo: str
|
|
the name of the repository to save the xml file
|
|
links_xyz_dict: dict
|
|
xyz information of the each link
|
|
file_name: str
|
|
urdf full path
|
|
"""
|
|
|
|
with open(file_name, mode='a') as f:
|
|
for j in joints_dict:
|
|
parent = joints_dict[j]['parent']
|
|
child = joints_dict[j]['child']
|
|
joint_type = joints_dict[j]['type']
|
|
upper_limit = joints_dict[j]['upper_limit']
|
|
lower_limit = joints_dict[j]['lower_limit']
|
|
try:
|
|
xyz = [round(p-c, 6) for p, c in \
|
|
zip(links_xyz_dict[parent], links_xyz_dict[child])] # xyz = parent - child
|
|
except KeyError as ke:
|
|
app = adsk.core.Application.get()
|
|
ui = app.userInterface
|
|
ui.messageBox("Joint %s: There seems to be an error with the connection between\n\n%s\nand\n%s\n\nCheck \
|
|
whether the connections\nparent=component2=%s\nchild=component1=%s\nare correct or if you need \
|
|
to swap component1<=>component2"
|
|
% (j, parent, child, parent, child), "Error!")
|
|
quit()
|
|
|
|
joint = Joint.Joint(name=j, joint_type = joint_type, xyz=xyz, \
|
|
axis=joints_dict[j]['axis'], parent=parent, child=child, \
|
|
upper_limit=upper_limit, lower_limit=lower_limit)
|
|
joint.make_joint_xml()
|
|
joint.make_transmission_xml()
|
|
f.write(joint.joint_xml)
|
|
if joint_type != 'fixed':
|
|
f.write(joint.tran_xml)
|
|
f.write('\n')
|
|
|
|
|
|
def write_urdf(joints_dict, links_xyz_dict, inertial_dict, package_name, save_dir, robot_name):
|
|
file_name = save_dir + '/' + robot_name + '.urdf' # the name of urdf file
|
|
repo = 'meshes/' # Pybullet only need relative paths
|
|
with open(file_name, mode='w') as f:
|
|
f.write('<?xml version="1.0" ?>\n')
|
|
f.write('<robot name="{}">\n'.format(robot_name))
|
|
f.write('\n')
|
|
f.write('<material name="silver">\n')
|
|
f.write(' <color rgba="0.700 0.700 0.700 1.000"/>\n')
|
|
f.write('</material>\n')
|
|
f.write('\n')
|
|
|
|
write_link_urdf(joints_dict, repo, links_xyz_dict, file_name, inertial_dict)
|
|
write_joint_tran_urdf(joints_dict, repo, links_xyz_dict, file_name)
|
|
write_endtag(file_name)
|
|
|
|
def write_endtag(file_name):
|
|
"""
|
|
Write the </robot> tag at the end of the urdf
|
|
Parameters
|
|
----------
|
|
file_name: str
|
|
urdf full path
|
|
"""
|
|
with open(file_name, mode='a') as f:
|
|
f.write('</robot>\n')
|
|
|
|
def write_hello_pybullet(robot_name, save_dir):
|
|
robot_urdf = robot_name + '.urdf' ## basename of robot.urdf
|
|
file_name = save_dir + '/' + 'hello_bullet.py'
|
|
hello_pybullet = """import pybullet as p
|
|
import time
|
|
import pybullet_data
|
|
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
|
|
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
|
|
p.setGravity(0,0,-10)
|
|
planeId = p.loadURDF("plane.urdf")
|
|
cubeStartPos = [0,0,0]
|
|
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
|
|
robotId = p.loadURDF("TEMPLATE.urdf",cubeStartPos, cubeStartOrientation,
|
|
# useMaximalCoordinates=1, ## New feature in Pybullet
|
|
flags=p.URDF_USE_INERTIA_FROM_FILE)
|
|
for i in range (10000):
|
|
p.stepSimulation()
|
|
time.sleep(1./240.)
|
|
cubePos, cubeOrn = p.getBasePositionAndOrientation(robotId)
|
|
print(cubePos,cubeOrn)
|
|
p.disconnect()
|
|
"""
|
|
hello_pybullet = hello_pybullet.replace('TEMPLATE.urdf', robot_urdf).replace('\t', ' ')
|
|
with open(file_name, mode='w') as f:
|
|
f.write(hello_pybullet)
|
|
f.write('\n')
|
|
|