Added dummy link as a parent of root link to support inertia
This commit is contained in:
parent
06cd9fafb0
commit
c407cd82b9
1 changed files with 22 additions and 2 deletions
|
@ -76,6 +76,13 @@ class Model(SpatialEntity):
|
||||||
def get_joint(self, joint_name):
|
def get_joint(self, joint_name):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
def get_root_link(self):
|
||||||
|
root_link = None
|
||||||
|
for link in self.links:
|
||||||
|
if not link.parent_joint:
|
||||||
|
root_link = link
|
||||||
|
return root_link
|
||||||
|
|
||||||
def build_tree(self):
|
def build_tree(self):
|
||||||
for joint in self.joints:
|
for joint in self.joints:
|
||||||
joint.parent_link = self.get_link(joint.parent)
|
joint.parent_link = self.get_link(joint.parent)
|
||||||
|
@ -113,9 +120,22 @@ class Model(SpatialEntity):
|
||||||
else:
|
else:
|
||||||
model.set('static', str(self.static).lower())
|
model.set('static', str(self.static).lower())
|
||||||
|
|
||||||
|
root_link = self.get_root_link()
|
||||||
|
if not root_link:
|
||||||
|
raise Exception("Couldn't find root link")
|
||||||
|
model.append(ET.Element('link', name=root_link.name+'_root'))
|
||||||
for link in self.links:
|
for link in self.links:
|
||||||
model.append(link.to_xml(fmt))
|
model.append(link.to_xml(fmt))
|
||||||
|
|
||||||
|
if fmt=='urdf':
|
||||||
|
root_joint = ET.Element('joint',
|
||||||
|
{"name": root_link.name+'_root',
|
||||||
|
"type": "fixed"})
|
||||||
|
root_joint.append(pose_to_xml(root_link.global_pose, fmt))
|
||||||
|
ET.SubElement(root_joint, 'parent', link= root_link.name+'_root')
|
||||||
|
ET.SubElement(root_joint, 'child', link= root_link.name)
|
||||||
|
model.append(root_joint)
|
||||||
|
|
||||||
for joint in self.joints:
|
for joint in self.joints:
|
||||||
model.append(joint.to_xml(fmt))
|
model.append(joint.to_xml(fmt))
|
||||||
|
|
||||||
|
@ -244,7 +264,7 @@ class Link(SpatialEntity):
|
||||||
self.urdf_pose = subtract_poses(self.global_pose,
|
self.urdf_pose = subtract_poses(self.global_pose,
|
||||||
self.parent_joint.global_pose)
|
self.parent_joint.global_pose)
|
||||||
else:
|
else:
|
||||||
self.urdf_pose = self.global_pose.copy()
|
self.urdf_pose = FreeCAD.Placement()
|
||||||
|
|
||||||
if fmt == 'sdf':
|
if fmt == 'sdf':
|
||||||
link.append(pose_to_xml(self.pose, fmt=fmt))
|
link.append(pose_to_xml(self.pose, fmt=fmt))
|
||||||
|
@ -325,7 +345,7 @@ class Joint(SpatialEntity):
|
||||||
self.urdf_pose = subtract_poses(self.global_pose,
|
self.urdf_pose = subtract_poses(self.global_pose,
|
||||||
self.parent_link.parent_joint.global_pose)
|
self.parent_link.parent_joint.global_pose)
|
||||||
else:
|
else:
|
||||||
self.urdf_pose = self.global_pose.copy()
|
self.urdf_pose = subtract_poses(self.global_pose, self.parent_link.global_pose)
|
||||||
|
|
||||||
joint = ET.Element('joint', {'name': self.name, 'type': self.type})
|
joint = ET.Element('joint', {'name': self.name, 'type': self.type})
|
||||||
pose = self.pose if fmt=='sdf' else self.urdf_pose
|
pose = self.pose if fmt=='sdf' else self.urdf_pose
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue