Skip to content

Commit

Permalink
[Python 3] Format
Browse files Browse the repository at this point in the history
  • Loading branch information
@nim65s
nim65s committed Nov 5, 2019
1 parent 5821a3a commit 43892dd
Show file tree
Hide file tree
Showing 15 changed files with 432 additions and 360 deletions.
25 changes: 15 additions & 10 deletions src/dynamic_graph/sot/pyrene/prologue.py
View file
Original file line number Diff line number Diff line change
@@ -1,28 +1,33 @@
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST

print("Prologue Pyrene TALOS Robot")

from dynamic_graph.entity import PyEntityFactoryClass
from dynamic_graph.sot.pyrene.robot import Robot

# Create the device.
# This entity behaves exactly like robotsimu except:
# 1. it does not provide the increment command
# 2. it forwards the robot control to the sot-abstract
# controller.
def makeRobot ():
print("Prologue Pyrene TALOS Robot")


def makeRobot():
"""
Create the device.
This entity behaves exactly like robotsimu except:
1. it does not provide the increment command
2. it forwards the robot control to the sot-abstract
controller.
"""

DeviceTalos = PyEntityFactoryClass('DeviceTalos')

# Create the robot using the device.
robot = Robot(name = 'robot', device = DeviceTalos('PYRENE'))
robot = Robot(name='robot', device=DeviceTalos('PYRENE'))

robot.dynamic.com.recompute (0)
robot.dynamic.com.recompute(0)
_com = robot.dynamic.com.value
robot.device.zmp.value = (_com[0], _com[1], 0.)

return robot


####################################
# --- IMPORTANT --- #
# #
Expand Down
61 changes: 45 additions & 16 deletions src/dynamic_graph/sot/pyrene/robot.py
View file
Original file line number Diff line number Diff line change
@@ -1,29 +1,57 @@
# -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse, CNRS


from dynamic_graph.sot.talos import Talos
import numpy as np

class Robot (Talos):

class Robot(Talos):
"""
This class instantiates LAAS TALOS Robot
"""

halfSitting = (
0.0,
0.0,
1.018213,
0.00,
0.0,
0.0, # Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Left Leg
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
-0.001708, # Right Leg
0.0,
0.006761, # Chest
0.25847,
0.173046,
-0.0002,
-0.525366,
0.0,
-0.0,
0.1,
-0.005, # Left Arm
-0.25847,
-0.173046,
0.0002,
-0.525366,
0.0,
0.0,
0.1,
-0.005, # Right Arm
0.,
0. # Head
)

halfSitting = (0.0, 0.0, 1.018213, 0.00 , 0.0, 0.0, #Free flyer
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Right Leg
0.0 , 0.006761, #Chest
0.25847 , 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, -0.005, #Left Arm
-0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1,-0.005, #Right Arm
0., 0. #Head
)

def __init__(self, name,
device = None,
tracer = None):
Talos.__init__(self,name,self.halfSitting,device,tracer)
def __init__(self, name, device=None, tracer=None):
Talos.__init__(self, name, self.halfSitting, device, tracer)
"""
TODO:Confirm these values
# Define camera positions w.r.t gaze.
Expand Down Expand Up @@ -80,4 +108,5 @@ def __init__(self, name,
matrixToTuple(cameraTopRightPosition), "gaze"))
"""


__all__ = ["Robot"]
204 changes: 110 additions & 94 deletions src/dynamic_graph/sot/pyrene/seqplay.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,41 +2,45 @@
# Copyright 2012, CNRS-LAAS, Florent Lamiraux

import numpy as np
from math import sqrt
from dynamic_graph.sot.pyrene.robot import Robot
from dynamic_graph.sot.core import RPYToMatrix
from dynamic_graph.sot.tools.se3 import SE3, SO3, R3
from dynamic_graph.sot.pyrene.robot import Robot
from dynamic_graph.sot.tools.se3 import R3, SE3

robot = Robot ('seqplay')
rpy2matrix = RPYToMatrix ('rpy2matrix')
robot = Robot('seqplay')
rpy2matrix = RPYToMatrix('rpy2matrix')
m = 56.868
g = 9.81

pos = None
zmp = None
hip = None
def convert (filename) :


def convert(filename):
"""
Convert a seqplay file in OpenHRP format to sot-tool format
"""
global pos, zmp, hip
openhrpPos = np.genfromtxt (filename + '.pos')
openhrpZmp = np.genfromtxt (filename + '.zmp')
nbConfig = len (openhrpPos)
if len (openhrpZmp) != nbConfig :
raise RuntimeError (filename + ".pos and " + filename +
".zmp have different lengths.")
openhrpPos = np.genfromtxt(filename + '.pos')
openhrpZmp = np.genfromtxt(filename + '.zmp')
nbConfig = len(openhrpPos)
if len(openhrpZmp) != nbConfig:
raise RuntimeError(filename + ".pos and " + filename + ".zmp have different lengths.")
try:
openhrpHip = np.genfromtxt (filename + '.hip')
openhrpHip = np.genfromtxt(filename + '.hip')
except IOError:
hip = []
for i in range (len (openhrpPos)):
hip.append ((openhrpPos [i][0], 0, 0, 0,))
openhrpHip = np.array (hip)

if len (openhrpHip) != nbConfig :
raise RuntimeError (filename + ".pos and " + filename +
".hip have different lengths.")
for i in range(len(openhrpPos)):
hip.append((
openhrpPos[i][0],
0,
0,
0,
))
openhrpHip = np.array(hip)

if len(openhrpHip) != nbConfig:
raise RuntimeError(filename + ".pos and " + filename + ".hip have different lengths.")

t = 1
featurePos = []
Expand All @@ -49,110 +53,122 @@ def convert (filename) :
fixedFoot = None
fixedLeftFoot = None
fixedRightFoot = None
for (pos, zmp, hip) in zip (openhrpPos, openhrpZmp, openhrpHip) :
translation = 3*(0.,)
config = list (translation + tuple (hip [1:]) + tuple (pos [1:31]))
robot.dynamic.position.value = tuple (config)
for (pos, zmp, hip) in zip(openhrpPos, openhrpZmp, openhrpHip):
translation = 3 * (0., )
config = list(translation + tuple(hip[1:]) + tuple(pos[1:31]))
robot.dynamic.position.value = tuple(config)
robot.dynamic.position.time = t
robot.com.recompute (t)
robot.leftAnkle.position.recompute (t)
robot.rightAnkle.position.recompute (t)
lf = SE3 (robot.leftAnkle.position.value) * R3 (0., 0., -0.107)
rf = SE3 (robot.rightAnkle.position.value) * R3 (0., 0., -0.107)
robot.com.recompute(t)
robot.leftAnkle.position.recompute(t)
robot.rightAnkle.position.recompute(t)
lf = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
rf = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)
# find support foot
rpy2matrix.sin.value = tuple (hip [1:])
rpy2matrix.sout.recompute (t)
waist = SE3 (rpy2matrix.sout.value, translation)
zmpGlob = waist * R3 (tuple (zmp [1:]))
rpy2matrix.sin.value = tuple(hip[1:])
rpy2matrix.sout.recompute(t)
waist = SE3(rpy2matrix.sout.value, translation)
zmpGlob = waist * R3(tuple(zmp[1:]))
# fr = m g * (zmpGlob - lf | rf - lf)/||rf - lf||^2
# fl = (m g - fr)
fr = m * g * ((zmpGlob - lf)*(rf - lf)/((rf - lf)*(rf - lf)))
fr = m * g * ((zmpGlob - lf) * (rf - lf) / ((rf - lf) * (rf - lf)))
fl = m * g - fr
if (lf - zmpGlob) * (lf - zmpGlob) < (rf - zmpGlob) * (rf - zmpGlob) :
if (lf - zmpGlob) * (lf - zmpGlob) < (rf - zmpGlob) * (rf - zmpGlob):
supportFoot = lf
fixedFoot = fixedLeftFoot
else :
else:
supportFoot = rf
fixedFoot = fixedRightFoot
t+=1
t += 1
# move support foot to previous value
if fixedFoot is None:
config [2] -= supportFoot [2]
config[2] -= supportFoot[2]
else:
config [0] += fixedFoot [0] - supportFoot [0]
config [1] += fixedFoot [1] - supportFoot [1]
config [2] += fixedFoot [2] - supportFoot [2]
config[0] += fixedFoot[0] - supportFoot[0]
config[1] += fixedFoot[1] - supportFoot[1]
config[2] += fixedFoot[2] - supportFoot[2]

robot.dynamic.position.value = tuple (config)
robot.dynamic.position.value = tuple(config)
robot.dynamic.position.time = t
robot.com.recompute (t)
robot.leftAnkle.position.recompute (t)
robot.rightAnkle.position.recompute (t)
featurePos.append (config)
featureCom.append (robot.com.value)
featureLa.append (robot.leftAnkle.position.value)
featureRa.append (robot.rightAnkle.position.value)
forceLeftFoot.append ((0.,0.,fl,0.,0.,0.,))
forceRightFoot.append ((0.,0.,fr,0.,0.,0.,))
robot.com.recompute(t)
robot.leftAnkle.position.recompute(t)
robot.rightAnkle.position.recompute(t)
featurePos.append(config)
featureCom.append(robot.com.value)
featureLa.append(robot.leftAnkle.position.value)
featureRa.append(robot.rightAnkle.position.value)
forceLeftFoot.append((
0.,
0.,
fl,
0.,
0.,
0.,
))
forceRightFoot.append((
0.,
0.,
fr,
0.,
0.,
0.,
))
t += 1
fixedLeftFoot = \
SE3 (robot.leftAnkle.position.value) * R3 (0., 0., -0.107)
fixedRightFoot = \
SE3 (robot.rightAnkle.position.value) * R3 (0., 0., -0.107)

filePos = open (filename + '.posture', 'w')
fileLa = open (filename + '.la', 'w')
fileRa = open (filename + '.ra', 'w')
fileCom = open (filename + '.com', 'w')
fileFl = open (filename + '.fl', 'w')
fileFr = open (filename + '.fr', 'w')
fixedLeftFoot = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
fixedRightFoot = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)

filePos = open(filename + '.posture', 'w')
fileLa = open(filename + '.la', 'w')
fileRa = open(filename + '.ra', 'w')
fileCom = open(filename + '.com', 'w')
fileFl = open(filename + '.fl', 'w')
fileFr = open(filename + '.fr', 'w')

dt = .005
for (pos, la, ra, com,
force_lf, force_rf, i) in zip (featurePos, featureLa, featureRa,
featureCom, forceLeftFoot,
forceRightFoot, xrange (10000000)) :
t = i*dt
filePos.write ("{0}".format (t))
fileLa.write ("{0}".format (t))
fileRa.write ("{0}".format (t))
fileCom.write ("{0}".format (t))
fileFl.write ("{0}".format (t))
fileFr.write ("{0}".format (t))
for (pos, la, ra, com, force_lf, force_rf, i) in zip(featurePos, featureLa, featureRa, featureCom, forceLeftFoot,
forceRightFoot, range(10000000)):
t = i * dt
filePos.write("{0}".format(t))
fileLa.write("{0}".format(t))
fileRa.write("{0}".format(t))
fileCom.write("{0}".format(t))
fileFl.write("{0}".format(t))
fileFr.write("{0}".format(t))

for x in pos:
filePos.write ("\t{0}".format (x))
filePos.write("\t{0}".format(x))

for row in la:
for x in row:
fileLa.write ("\t{0}".format (x))
fileLa.write("\t{0}".format(x))

for row in ra:
for x in row:
fileRa.write ("\t{0}".format (x))
fileRa.write("\t{0}".format(x))

for x in com:
fileCom.write ("\t{0}".format (x))
fileCom.write("\t{0}".format(x))

for x in force_lf:
fileFl.write ("\t{0}".format (x))
fileFl.write("\t{0}".format(x))

for x in force_rf:
fileFr.write ("\t{0}".format (x))

filePos.write ("\n")
fileLa.write ("\n")
fileRa.write ("\n")
fileCom.write ("\n")
fileFl.write ("\n")
fileFr.write ("\n")

filePos.close ()
fileLa.close ()
fileRa.close ()
fileCom.close ()
fileFl.close ()
fileFr.close ()
fileFr.write("\t{0}".format(x))

filePos.write("\n")
fileLa.write("\n")
fileRa.write("\n")
fileCom.write("\n")
fileFl.write("\n")
fileFr.write("\n")

filePos.close()
fileLa.close()
fileRa.close()
fileCom.close()
fileFl.close()
fileFr.close()


if __name__ == '__main__':
#convert ('/opt/grx3.0/HRP2LAAS/etc/walkfwd')
pass
# convert('/opt/grx3.0/HRP2LAAS/etc/walkfwd')
2 changes: 2 additions & 0 deletions src/dynamic_graph/sot/talos/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
# -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse CNRS
# flake8: noqa

from __future__ import print_function

from dynamic_graph.sot.talos.talos import Talos
Loading

0 comments on commit 43892dd

Please sign in to comment.