From c1df40b3db1a5cc8f919a1800acc831eadad4216 Mon Sep 17 00:00:00 2001 From: Dawit Abate Date: Thu, 4 Jul 2019 15:17:14 +0300 Subject: [PATCH] Added urdf origin calculations --- src/freecad_to_gazebo/model.py | 83 ++++++++++++++++++++++++++++++---- 1 file changed, 74 insertions(+), 9 deletions(-) diff --git a/src/freecad_to_gazebo/model.py b/src/freecad_to_gazebo/model.py index da30089..0cfe6aa 100644 --- a/src/freecad_to_gazebo/model.py +++ b/src/freecad_to_gazebo/model.py @@ -1,8 +1,15 @@ -from .conversions import * +from freecad_to_gazebo.conversions import * import FreeCAD from xml.etree import ElementTree as ET +from xml.dom.minidom import parseString +def add_poses(p1, p2): + return FreeCAD.Placement(p1.toMatrix() + p2.toMatrix()) + +def subtract_poses(p1, p2): + return FreeCAD.Placement(p1.toMatrix() + p2.toMatrix().inverse()) + def pose_to_xml(pose, fmt='sdf'): '''Converts a pose/freecad placement/ to xml element with tag "pose" for sdf and "origin" for urdf''' @@ -22,7 +29,7 @@ def pose_to_xml(pose, fmt='sdf'): def pose_xyz(pose): '''Returns the xyz/Base portion of a pose as string''' - xyz = pose.Base + xyz = pose.Base if hasattr(pose, 'Base') else pose return ' '.join([flt2str(i) for i in xyz]) @@ -31,6 +38,7 @@ class SpatialEntity(object): def __init__(self, **kwargs): self.name = kwargs.get('name', '') self.pose = kwargs.get('pose', FreeCAD.Placement()) + self.global_pose = kwargs.get('global_pose', FreeCAD.Placement()) self.urdf_pose = kwargs.get('urdf_pose', FreeCAD.Placement()) self.formats = ['sdf', 'urdf'] @@ -60,13 +68,44 @@ class Model(SpatialEntity): self.joints.append(kwargs.get('joint', Joint())) self.joints.extend(kwargs.get('joints', [])) + def get_link(self, link_name): + for link in self.links: + if link_name == link.name: + return link + + def get_joint(self, joint_name): + pass + + def build_tree(self): + for joint in self.joints: + joint.parent_link = self.get_link(joint.parent) + if not joint.parent_link: + raise Exception('Parent not found for joint %s' % joint.name) + joint.parent_link.child_joints.append(joint) + + joint.child_link = self.get_link(joint.child) + if not joint.child_link: + raise Exception('Child not found for joint %s' % joint.name) + joint.child_link.parent_joint = joint + + def calculate_global_poses(self): + for link in self.links: + link.global_pose = add_poses(self.pose, link.pose) + for joint in self.joints: + joint.global_pose = add_poses(joint.child_link.global_pose, joint.pose) + def to_xml(self, fmt='sdf'): '''returns xml element of a model/robot''' super(Model, self).to_xml(fmt) + + self.build_tree() + self.calculate_global_poses() + tag = 'robot' if fmt=='urdf' else 'model' model = ET.Element(tag, name=self.name) if fmt == 'sdf': + model.append(pose_to_xml(self.pose)) static = ET.SubElement(model, 'static') static.text = str(self.static).lower() self_collide = ET.SubElement(model, 'self_collide') @@ -87,6 +126,10 @@ class Model(SpatialEntity): return model + def to_xml_string(self, fmt='sdf', header=True): + dom = parseString(ET.tostring(self.to_xml(fmt))) + return dom.toprettyxml(indent=' '*2) + class Inertia(object): '''A clss representing an inertia element''' @@ -117,7 +160,7 @@ class Inertia(object): class Inertial(SpatialEntity): '''A class representing an inertial element''' def __init__(self, **kwargs): - super(Inertial, self).__init__() + super(Inertial, self).__init__(**kwargs) self.mass = kwargs.get('mass', 0) self.inertia = kwargs.get('inertia', Inertia()) @@ -131,7 +174,7 @@ class Inertial(SpatialEntity): if fmt == 'sdf': mass.text = flt2str(self.mass) else: - mass.set('vaue', flt2str(self.mass)) + mass.set('value', flt2str(self.mass)) inertial.append(self.inertia.to_xml(fmt=fmt)) return inertial @@ -177,10 +220,13 @@ class Link(SpatialEntity): '''A class representing a link element''' def __init__(self, **kwargs): super(Link, self).__init__(**kwargs) - self.inertial = Inertial() + self.inertial = kwargs.get('inertial', Inertial()) self.visuals = [] self.collisions = [] + self.parent_joint = None + self.child_joints = [] + if 'visual' in kwargs: self.visuals.append(kwargs.get('visual', Visual())) self.visuals.extend(kwargs.get('visuals', [])) @@ -194,14 +240,23 @@ class Link(SpatialEntity): super(Link, self).to_xml(fmt) link = ET.Element('link', name=self.name) + if self.parent_joint: + self.urdf_pose = subtract_poses(self.global_pose, + self.parent_joint.global_pose) + else: + self.urdf_pose = self.global_pose.copy() + if fmt == 'sdf': link.append(pose_to_xml(self.pose, fmt=fmt)) + self.inertial.urdf_pose = add_poses(self.inertial.pose, self.urdf_pose) link.append(self.inertial.to_xml(fmt=fmt)) for visual in self.visuals: + visual.urdf_pose = self.urdf_pose.copy() link.append(visual.to_xml(fmt=fmt)) for collision in self.collisions: + collision.urdf_pose = self.urdf_pose.copy() link.append(collision.to_xml(fmt=fmt)) return link @@ -238,13 +293,13 @@ class Axis(SpatialEntity): use_parent_frame = ET.SubElement(axis, 'use_parent_model_frame') use_parent_frame.text = str(self.use_parent_frame).lower() else: - axis.set('xyz', pose_xyz(self.urdf_pose)) + axis.set('xyz', pose_xyz(self.pose)) axis.set('use_parent_model_frame', str(self.use_parent_frame).lower()) limit = ET.Element('limit') limit.set('lower', flt2str(deg2rad(self.lower_limit))) limit.set('upper', flt2str(deg2rad(self.upper_limit))) - limit.set('effort', flt2str(deg2rad(self.effort_limit))) - limit.set('velocity', flt2str(deg2rad(self.velocity_limit))) + limit.set('effort', flt2str(self.effort_limit)) + limit.set('velocity', flt2str(self.velocity_limit)) return [axis, limit] return axis @@ -259,9 +314,19 @@ class Joint(SpatialEntity): self.type = kwargs.get('type', '') self.axis = kwargs.get('axis', Axis()) + self.parent_link = None + self.child_link = None + def to_xml(self, fmt='sdf'): '''returns a joint xml element''' super(Joint, self).to_xml(fmt) + + if self.parent_link.parent_joint: + self.urdf_pose = subtract_poses(self.global_pose, + self.parent_link.parent_joint.global_pose) + else: + self.urdf_pose = self.global_pose.copy() + joint = ET.Element('joint', {'name': self.name, 'type': self.type}) pose = self.pose if fmt=='sdf' else self.urdf_pose joint.append(pose_to_xml(pose, fmt=fmt)) @@ -274,7 +339,7 @@ class Joint(SpatialEntity): joint.append(self.axis.to_xml(fmt=fmt)) else: parent.set('link', self.parent) - child.set('Link', self.child) + child.set('link', self.child) joint.extend(self.axis.to_xml(fmt=fmt)) return joint