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

Fix for pr2_calibration package to work #55

Closed
wants to merge 2 commits into from
Closed

Fix for pr2_calibration package to work #55

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
eca4ed7
support DockTypedFactory, copied from 0915e8d
k-okada Oct 24, 2014
174c1d6
add PR2 transmission, this is similar to 484a34d, but confirmed on re…
k-okada Oct 24, 2014
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
158 changes: 150 additions & 8 deletions urdf_parser_py/src/urdf_parser_py/urdf.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -329,22 +329,164 @@ def __init__(self, name=None, visual=None, inertial=None, collision=None, origin
])


class Actuator(xmlr.Object):
def __init__(self, name = None, hardwareInterface = None, mechanicalReduction = 1):
self.name = name
self.hardwareInterface = None
self.mechanicalReduction = None

xmlr.reflect(Actuator, tag = 'actuator', params = [
name_attribute,
xmlr.Element('hardwareInterface', str),
xmlr.Element('mechanicalReduction', float, required = False)
])

class Transmission(xmlr.Object):
def __init__(self, name = None, joint = None, actuator = None, type = None, mechanicalReduction = 1):
def __init__(self, name = None, joint = None, actuator = None):
self.name = name
self.joint = joint
self.actuator = actuator

xmlr.reflect(Transmission, tag = 'new_transmission', params = [
name_attribute,
xmlr.Attribute('type', str),
xmlr.Element('joint', 'element_name'),
xmlr.Element('actuator', Actuator),
])

class PR2Compensator(xmlr.Object):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We shold not put pr2 specific things in the urdfdom

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We shold not put pr2 specific things in the urdfdom

I agree with you, however PR2 requires several "extension" tags and old urdf parser could handle them because on groovy pr2 calibration worked.

What is the best way to solve this issue?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure what the issue is, but this is inconsistent with the C++ parser. There is no code specific to the pr2 there. The python shold be an equivalent feature set. Could we maybe have this pr2-specific parsing done in the calibration package itself?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The problem is when we load pr2 urdf which contains pr2-specific tag and output to file, undefined tags are disappeared. Does anyone knows how to fix this?

def __init__(self, k_belt=4000.0, kd_motor=10.0, lambda_combined=0.0, lambda_joint=60.0, lambda_motor=60.0, mass_motor=0.05):
self.k_belt = k_belt
self.kd_motor = kd_motor
self.lambda_combined = lambda_combined
self.lambda_joint = lambda_joint
self.lambda_motor = lambda_motor
self.mass_motor = mass_motor

xmlr.reflect(PR2Compensator, tag = 'compensator', params = [
xmlr.Attribute('k_belt', float),
xmlr.Attribute('kd_motor', float),
xmlr.Attribute('lambda_combined', float),
xmlr.Attribute('lambda_joint', float),
xmlr.Attribute('lambda_motor', float),
xmlr.Attribute('mass_motor', float)
])

class PR2SimulatedActuatedJoint(xmlr.Object):
def __init__(self, name = None, simulated_reduction = 3000, passive_actuated_joint = None):
self.name = name
self.simulated_reduction = simulated_reduction
self.passive_actuated_joint = passive_actuated_joint

xmlr.reflect(PR2SimulatedActuatedJoint, tag = 'simulated_actuated_joint', params = [
name_attribute,
xmlr.Attribute('simulated_reduction', float),
xmlr.Attribute('passive_actuated_joint', str, False)
])

class PR2Transmission(xmlr.Object):
def __init__(self, name = None, joint = None, actuator = None, mechanicalReduction = None, compensator = None, simulated_actuated_joint = None):
self.name = name
self.type = type
self.joint = joint
self.actuator = actuator
self.mechanicalReduction = mechanicalReduction
self.compensator = compensator
self.simulated_actuated_joint = simulated_actuated_joint

xmlr.reflect(Transmission, tag = 'transmission', params = [
xmlr.reflect(PR2Transmission, tag = 'pr2_transmission', params = [
name_attribute,
xmlr.Attribute('type', str, False),
xmlr.Element('joint', 'element_name', False),
xmlr.Element('actuator', 'element_name', False),
xmlr.Element('mechanicalReduction', float, False)
xmlr.Attribute('type', str),
xmlr.Element('joint', 'element_name'),
xmlr.Element('actuator', 'element_name'),
xmlr.Element('mechanicalReduction', float),
xmlr.Element('compensator', PR2Compensator, False),
xmlr.Element('simulated_actuated_joint', PR2SimulatedActuatedJoint, False)
])

class PR2Actuator(xmlr.Object):
def __init__(self, name = None, mechanicalReduction = 1):
self.name = name
self.mechanicalReduction = mechanicalReduction

xmlr.reflect(PR2Actuator, tag = 'pr2_actuator', params = [
name_attribute,
xmlr.Attribute('mechanicalReduction', float)
])

class PR2WristTransmission(xmlr.Object):
def __init__(self, name = None, type = None, rightActuator = None, leftActuator = None, flexJoint = None, rollJoint = None):
self.name = name
self.type = type
self.rightActuator = rightActuator
self.leftActuator = leftActuator
self.flexJoint = flexJoint
self.rollJoint = rollJoint

xmlr.reflect(PR2WristTransmission, tag = 'pr2_wrist_transmission', params = [
name_attribute,
xmlr.Attribute('type', str),
xmlr.Element('rightActuator', PR2Actuator),
xmlr.Element('leftActuator', PR2Actuator),
xmlr.Element('flexJoint', PR2Actuator),
xmlr.Element('rollJoint', PR2Actuator)
])


class PR2GapJoint(xmlr.Object):
def __init__(self, L0=0.0, a=0.0, b=0.0, gear_ratio=40.0, h=0.0, mechanical_reduction=1.0, name=None, phi0=0.5, r=0.0, screw_reduction=0.0, t0=0.0, theta0=0.0):
self.L0 = L0
self.a = a
self.b = b
self.gear_ratio = gear_ratio
self.h = h
self.mechanical_reduction = mechanical_reduction
self.name = name
self.phi0 = phi0
self.r = r
self.screw_reduction = screw_reduction
self.t0 = t0
self.theta0 = theta0

xmlr.reflect(PR2GapJoint, tag = 'pr2_gap_joint', params = [
xmlr.Attribute('L0', float),
xmlr.Attribute('a', float),
xmlr.Attribute('b', float),
xmlr.Attribute('gear_ratio', float),
xmlr.Attribute('h', float),
xmlr.Attribute('mechanical_reduction', float),
xmlr.Attribute('name', str),
xmlr.Attribute('phi0', float),
xmlr.Attribute('r', float),
xmlr.Attribute('screw_reduction', float),
xmlr.Attribute('t0', float),
xmlr.Attribute('theta0', float)
])

class PR2GripperTransmission(xmlr.Object):
def __init__(self, name = None, type = None, actuator = None, gap_joint = None, use_simulated_gripper_joint = None, passive_joint = [], simulated_actuated_joint = None):
self.aggregate_init()
self.name = name
self.type = type
self.actuator = actuator
self.gap_joint = gap_joint
self.use_simulated_gripper_joint = use_simulated_gripper_joint
self.passive_joint = passive_joint
self.simulated_actuated_joint = simulated_actuated_joint

xmlr.reflect(PR2GripperTransmission, tag = 'pr2_gripper_transmission', params = [
name_attribute,
xmlr.Attribute('type', str),
xmlr.Element('actuator', 'element_name'),
xmlr.Element('gap_joint', PR2GapJoint),
xmlr.Element('use_simulated_gripper_joint', 'element_name'),
xmlr.AggregateElement('passive_joint', 'element_name', 'passive_joint'),
xmlr.Element('simulated_actuated_joint', PR2SimulatedActuatedJoint),
])

#xmlr.add_type('transmission', xmlr.DuckTypedFactory('transmission', [Transmission, PR2WristTransmission, PR2GripperTransmission]))
xmlr.add_type('transmission', xmlr.DuckTypedFactory('transmission', [Transmission, PR2Transmission, PR2WristTransmission, PR2GripperTransmission]))


class Robot(xmlr.Object):
def __init__(self, name = None):
self.aggregate_init()
Expand Down Expand Up @@ -426,7 +568,7 @@ def from_parameter_server(cls, key = 'robot_description'):
xmlr.AggregateElement('link', Link),
xmlr.AggregateElement('joint', Joint),
xmlr.AggregateElement('gazebo', xmlr.RawType()),
xmlr.AggregateElement('transmission', Transmission),
xmlr.AggregateElement('transmission', 'transmission'),
xmlr.AggregateElement('material', Material)
])

Expand Down
22 changes: 22 additions & 0 deletions urdf_parser_py/src/urdf_parser_py/xml_reflection/core.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -194,6 +194,28 @@ def write_xml(self, node, obj):
obj.write_xml(node)


class DuckTypedFactory(ValueType):
def __init__(self, name, typeOrder):
self.name = name
assert len(typeOrder) > 0
self.type_order = typeOrder

def from_xml(self, node):
error_set = []
for value_type in self.type_order:
try:
return value_type.from_xml(node)
except Exception as e:
error_set.append((value_type, e))
# Should have returned, we encountered errors
out = "Could not perform duck-typed parsing."
for (value_type, e) in error_set:
out += "\nValue Type: {}\nException: {}\n".format(value_type, e)
raise Exception(out)

def write_xml(self, node, obj):
obj.write_xml(node)

class Param(object):
""" Mirroring Gazebo's SDF api

Expand Down