Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Revert "accuracy improvement" #132

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Revert "accuracy improvement" #132

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 24 additions & 24 deletions onshape_to_robot/robot_description.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ def rotationMatrixToEulerAngles(R):


def origin(matrix):
urdf = '<origin xyz="%.20g %.20g %.20g" rpy="%.20g %.20g %.20g" />'
urdf = '<origin xyz="%g %g %g" rpy="%g %g %g" />'
x = matrix[0, 3]
y = matrix[1, 3]
z = matrix[2, 3]
Expand All @@ -42,7 +42,7 @@ def origin(matrix):


def pose(matrix, frame=''):
sdf = '<pose>%.20g %.20g %.20g %.20g %.20g %.20g</pose>'
sdf = '<pose>%g %g %g %g %g %g</pose>'
x = matrix[0, 3]
y = matrix[1, 3]
z = matrix[2, 3]
Expand Down Expand Up @@ -231,10 +231,10 @@ def endLink(self):
self.addSTL(np.identity(4), filename, color, self._link_name, node)

self.append('<inertial>')
self.append('<origin xyz="%.20g %.20g %.20g" rpy="0 0 0"/>' %
self.append('<origin xyz="%g %g %g" rpy="0 0 0"/>' %
(com[0], com[1], com[2]))
self.append('<mass value="%.20g" />' % mass)
self.append('<inertia ixx="%.20g" ixy="%.20g" ixz="%.20g" iyy="%.20g" iyz="%.20g" izz="%.20g" />' %
self.append('<mass value="%g" />' % mass)
self.append('<inertia ixx="%g" ixy="%g" ixz="%g" iyy="%g" iyz="%g" izz="%g" />' %
(inertia[0, 0], inertia[0, 1], inertia[0, 2], inertia[1, 1], inertia[1, 2], inertia[2, 2]))
self.append('</inertial>')

Expand Down Expand Up @@ -275,7 +275,7 @@ def addSTL(self, matrix, stl, color, name, node='visual'):
self.append('</geometry>')
if node == 'visual':
self.append(f'<material name="{material_name}">')
self.append('<color rgba="%.20g %.20g %.20g 1.0"/>' %
self.append('<color rgba="%g %g %g 1.0"/>' %
(color[0], color[1], color[2]))
self.append('</material>')
self.append('</'+node+'>')
Expand Down Expand Up @@ -312,19 +312,19 @@ def addPart(self, matrix, stl, mass, com, inertia, color, shapes=None, name=''):
self.append(origin(matrix*shape['transform']))
self.append('<geometry>')
if shape['type'] == 'cube':
self.append('<box size="%.20g %.20g %.20g" />' %
self.append('<box size="%g %g %g" />' %
tuple(shape['parameters']))
if shape['type'] == 'cylinder':
self.append(
'<cylinder length="%.20g" radius="%.20g" />' % tuple(shape['parameters']))
'<cylinder length="%g" radius="%g" />' % tuple(shape['parameters']))
if shape['type'] == 'sphere':
self.append('<sphere radius="%.20g" />' %
self.append('<sphere radius="%g" />' %
shape['parameters'])
self.append('</geometry>')

if entry == 'visual':
self.append('<material name="'+name+'_material">')
self.append('<color rgba="%.20g %.20g %.20g 1.0"/>' %
self.append('<color rgba="%g %g %g 1.0"/>' %
(color[0], color[1], color[2]))
self.append('</material>')
self.append('</'+entry+'>')
Expand All @@ -336,11 +336,11 @@ def addJoint(self, jointType, linkFrom, linkTo, transform, name, jointLimits, zA
self.append(origin(transform))
self.append('<parent link="'+linkFrom+'" />')
self.append('<child link="'+linkTo+'" />')
self.append('<axis xyz="%.20g %.20g %.20g"/>' % tuple(zAxis))
self.append('<axis xyz="%g %g %g"/>' % tuple(zAxis))
lowerUpperLimits = ''
if jointLimits is not None:
lowerUpperLimits = 'lower="%.20g" upper="%.20g"' % jointLimits
self.append('<limit effort="%.20g" velocity="%.20g" %s/>' %
lowerUpperLimits = 'lower="%g" upper="%g"' % jointLimits
self.append('<limit effort="%g" velocity="%g" %s/>' %
(self.jointMaxEffortFor(name), self.jointMaxVelocityFor(name), lowerUpperLimits))
self.append('<joint_properties friction="0.0"/>')
self.append('</joint>')
Expand Down Expand Up @@ -409,9 +409,9 @@ def endLink(self):

self.append('<inertial>')
self.append('<pose frame="'+self._link_name +
'_frame">%.20g %.20g %.20g 0 0 0</pose>' % (com[0], com[1], com[2]))
self.append('<mass>%.20g</mass>' % mass)
self.append('<inertia><ixx>%.20g</ixx><ixy>%.20g</ixy><ixz>%.20g</ixz><iyy>%.20g</iyy><iyz>%.20g</iyz><izz>%.20g</izz></inertia>' %
'_frame">%g %g %g 0 0 0</pose>' % (com[0], com[1], com[2]))
self.append('<mass>%g</mass>' % mass)
self.append('<inertia><ixx>%g</ixx><ixy>%g</ixy><ixz>%g</ixz><iyy>%g</iyy><iyz>%g</iyz><izz>%g</izz></inertia>' %
(inertia[0, 0], inertia[0, 1], inertia[0, 2], inertia[1, 1], inertia[1, 2], inertia[2, 2]))
self.append('</inertial>')

Expand Down Expand Up @@ -440,8 +440,8 @@ def addFrame(self, name, matrix):

def material(self, color):
m = '<material>'
m += '<ambient>%.20g %.20g %.20g 1</ambient>' % (color[0], color[1], color[2])
m += '<diffuse>%.20g %.20g %.20g 1</diffuse>' % (color[0], color[1], color[2])
m += '<ambient>%g %g %g 1</ambient>' % (color[0], color[1], color[2])
m += '<diffuse>%g %g %g 1</diffuse>' % (color[0], color[1], color[2])
m += '<specular>0.1 0.1 0.1 1</specular>'
m += '<emissive>0 0 0 0</emissive>'
m += '</material>'
Expand Down Expand Up @@ -497,14 +497,14 @@ def addPart(self, matrix, stl, mass, com, inertia, color, shapes=None, name=''):
self.append(pose(matrix*shape['transform']))
self.append('<geometry>')
if shape['type'] == 'cube':
self.append('<box><size>%.20g %.20g %.20g</size></box>' %
self.append('<box><size>%g %g %g</size></box>' %
tuple(shape['parameters']))
if shape['type'] == 'cylinder':
self.append(
'<cylinder><length>%.20g</length><radius>%.20g</radius></cylinder>' % tuple(shape['parameters']))
'<cylinder><length>%g</length><radius>%g</radius></cylinder>' % tuple(shape['parameters']))
if shape['type'] == 'sphere':
self.append(
'<sphere><radius>%.20g</radius></sphere>' % shape['parameters'])
'<sphere><radius>%g</radius></sphere>' % shape['parameters'])
self.append('</geometry>')

if entry == 'visual':
Expand All @@ -519,11 +519,11 @@ def addJoint(self, jointType, linkFrom, linkTo, transform, name, jointLimits, zA
self.append('<parent>'+linkFrom+'</parent>')
self.append('<child>'+linkTo+'</child>')
self.append('<axis>')
self.append('<xyz>%.20g %.20g %.20g</xyz>' % tuple(zAxis))
self.append('<xyz>%g %g %g</xyz>' % tuple(zAxis))
lowerUpperLimits = ''
if jointLimits is not None:
lowerUpperLimits = '<lower>%.20g</lower><upper>%.20g</upper>' % jointLimits
self.append('<limit><effort>%.20g</effort><velocity>%.20g</velocity>%s</limit>' %
lowerUpperLimits = '<lower>%g</lower><upper>%g</upper>' % jointLimits
self.append('<limit><effort>%g</effort><velocity>%g</velocity>%s</limit>' %
(self.jointMaxEffortFor(name), self.jointMaxVelocityFor(name), lowerUpperLimits))
self.append('</axis>')
self.append('</joint>')
Expand Down