Commit 9ce3cc47 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

[format] Fixed further errors in the code

parent f031aa25
...@@ -8,7 +8,7 @@ from pinocchio.robot_wrapper import RobotWrapper ...@@ -8,7 +8,7 @@ from pinocchio.robot_wrapper import RobotWrapper
def getModelPath(subpath): def getModelPath(subpath):
for path in ['..', '../..', '/opt/openrobots/share/example-robot-data']: for path in ['..', '../..', '/opt/openrobots/share/example-robot-data']:
if exists(join(path, subpath.strip('/'))): if exists(join(path, subpath.strip('/'))):
print "using %s as modelPath" % path print("using %s as modelPath" % path)
return path return path
raise IOError('%s not found' % (subpath)) raise IOError('%s not found' % (subpath))
...@@ -17,22 +17,9 @@ def readParamsFromSrdf(robot, SRDF_PATH, verbose): ...@@ -17,22 +17,9 @@ def readParamsFromSrdf(robot, SRDF_PATH, verbose):
rmodel = robot.model rmodel = robot.model
pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose) pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
rmodel.armature = \ rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
np.multiply(rmodel.rotorInertia.flat, pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
np.square(rmodel.rotorGearRatio.flat)) robot.q0.flat[:] = rmodel.referenceConfigurations["half_sitting"].copy()
try:
pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
robot.q0.flat[:] = \
rmodel.referenceConfigurations["half_sitting"].copy()
except:
print "loadReferenceConfigurations did not work. Please check your \
Pinocchio Version"
try:
pinocchio.getNeutralConfiguration(rmodel, SRDF_PATH, verbose)
robot.q0.flat[:] = rmodel.neutralConfiguration.copy()
except:
robot.q0.flat[:] = pinocchio.neutral(rmodel)
return return
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment