Commit 43892dd4 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python 3] Format

parent 5821a3ae
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST # Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
print("Prologue Pyrene TALOS Robot")
from dynamic_graph.entity import PyEntityFactoryClass from dynamic_graph.entity import PyEntityFactoryClass
from dynamic_graph.sot.pyrene.robot import Robot from dynamic_graph.sot.pyrene.robot import Robot
# Create the device. print("Prologue Pyrene TALOS Robot")
# 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 def makeRobot():
# controller. """
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') DeviceTalos = PyEntityFactoryClass('DeviceTalos')
# Create the robot using the device. # 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 _com = robot.dynamic.com.value
robot.device.zmp.value = (_com[0], _com[1], 0.) robot.device.zmp.value = (_com[0], _com[1], 0.)
return robot return robot
#################################### ####################################
# --- IMPORTANT --- # # --- IMPORTANT --- #
# # # #
......
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse, CNRS # Copyright 2016, Olivier Stasse, CNRS
from dynamic_graph.sot.talos import Talos from dynamic_graph.sot.talos import Talos
import numpy as np
class Robot (Talos):
class Robot(Talos):
""" """
This class instantiates LAAS TALOS Robot 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 def __init__(self, name, device=None, tracer=None):
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, #Left Leg Talos.__init__(self, name, self.halfSitting, device, tracer)
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)
""" """
TODO:Confirm these values TODO:Confirm these values
# Define camera positions w.r.t gaze. # Define camera positions w.r.t gaze.
...@@ -80,4 +108,5 @@ class Robot (Talos): ...@@ -80,4 +108,5 @@ class Robot (Talos):
matrixToTuple(cameraTopRightPosition), "gaze")) matrixToTuple(cameraTopRightPosition), "gaze"))
""" """
__all__ = ["Robot"] __all__ = ["Robot"]
...@@ -2,41 +2,45 @@ ...@@ -2,41 +2,45 @@
# Copyright 2012, CNRS-LAAS, Florent Lamiraux # Copyright 2012, CNRS-LAAS, Florent Lamiraux
import numpy as np 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.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') robot = Robot('seqplay')
rpy2matrix = RPYToMatrix ('rpy2matrix') rpy2matrix = RPYToMatrix('rpy2matrix')
m = 56.868 m = 56.868
g = 9.81 g = 9.81
pos = None pos = None
zmp = None zmp = None
hip = None hip = None
def convert (filename) :
def convert(filename):
""" """
Convert a seqplay file in OpenHRP format to sot-tool format Convert a seqplay file in OpenHRP format to sot-tool format
""" """
global pos, zmp, hip global pos, zmp, hip
openhrpPos = np.genfromtxt (filename + '.pos') openhrpPos = np.genfromtxt(filename + '.pos')
openhrpZmp = np.genfromtxt (filename + '.zmp') openhrpZmp = np.genfromtxt(filename + '.zmp')
nbConfig = len (openhrpPos) nbConfig = len(openhrpPos)
if len (openhrpZmp) != nbConfig : if len(openhrpZmp) != nbConfig:
raise RuntimeError (filename + ".pos and " + filename + raise RuntimeError(filename + ".pos and " + filename + ".zmp have different lengths.")
".zmp have different lengths.")
try: try:
openhrpHip = np.genfromtxt (filename + '.hip') openhrpHip = np.genfromtxt(filename + '.hip')
except IOError: except IOError:
hip = [] hip = []
for i in range (len (openhrpPos)): for i in range(len(openhrpPos)):
hip.append ((openhrpPos [i][0], 0, 0, 0,)) hip.append((
openhrpHip = np.array (hip) openhrpPos[i][0],
0,
if len (openhrpHip) != nbConfig : 0,
raise RuntimeError (filename + ".pos and " + filename + 0,
".hip have different lengths.") ))
openhrpHip = np.array(hip)
if len(openhrpHip) != nbConfig:
raise RuntimeError(filename + ".pos and " + filename + ".hip have different lengths.")
t = 1 t = 1
featurePos = [] featurePos = []
...@@ -49,110 +53,122 @@ def convert (filename) : ...@@ -49,110 +53,122 @@ def convert (filename) :
fixedFoot = None fixedFoot = None
fixedLeftFoot = None fixedLeftFoot = None
fixedRightFoot = None fixedRightFoot = None
for (pos, zmp, hip) in zip (openhrpPos, openhrpZmp, openhrpHip) : for (pos, zmp, hip) in zip(openhrpPos, openhrpZmp, openhrpHip):
translation = 3*(0.,) translation = 3 * (0., )
config = list (translation + tuple (hip [1:]) + tuple (pos [1:31])) config = list(translation + tuple(hip[1:]) + tuple(pos[1:31]))
robot.dynamic.position.value = tuple (config) robot.dynamic.position.value = tuple(config)
robot.dynamic.position.time = t robot.dynamic.position.time = t
robot.com.recompute (t) robot.com.recompute(t)
robot.leftAnkle.position.recompute (t) robot.leftAnkle.position.recompute(t)
robot.rightAnkle.position.recompute (t) robot.rightAnkle.position.recompute(t)
lf = SE3 (robot.leftAnkle.position.value) * R3 (0., 0., -0.107) lf = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
rf = SE3 (robot.rightAnkle.position.value) * R3 (0., 0., -0.107) rf = SE3(robot.rightAnkle.position.value) * R3(0., 0., -0.107)
# find support foot # find support foot
rpy2matrix.sin.value = tuple (hip [1:]) rpy2matrix.sin.value = tuple(hip[1:])
rpy2matrix.sout.recompute (t) rpy2matrix.sout.recompute(t)
waist = SE3 (rpy2matrix.sout.value, translation) waist = SE3(rpy2matrix.sout.value, translation)
zmpGlob = waist * R3 (tuple (zmp [1:])) zmpGlob = waist * R3(tuple(zmp[1:]))
# fr = m g * (zmpGlob - lf | rf - lf)/||rf - lf||^2 # fr = m g * (zmpGlob - lf | rf - lf)/||rf - lf||^2
# fl = (m g - fr) # 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 fl = m * g - fr
if (lf - zmpGlob) * (lf - zmpGlob) < (rf - zmpGlob) * (rf - zmpGlob) : if (lf - zmpGlob) * (lf - zmpGlob) < (rf - zmpGlob) * (rf - zmpGlob):
supportFoot = lf supportFoot = lf
fixedFoot = fixedLeftFoot fixedFoot = fixedLeftFoot
else : else:
supportFoot = rf supportFoot = rf
fixedFoot = fixedRightFoot fixedFoot = fixedRightFoot
t+=1 t += 1
# move support foot to previous value # move support foot to previous value
if fixedFoot is None: if fixedFoot is None:
config [2] -= supportFoot [2] config[2] -= supportFoot[2]
else: else:
config [0] += fixedFoot [0] - supportFoot [0] config[0] += fixedFoot[0] - supportFoot[0]
config [1] += fixedFoot [1] - supportFoot [1] config[1] += fixedFoot[1] - supportFoot[1]
config [2] += fixedFoot [2] - supportFoot [2] config[2] += fixedFoot[2] - supportFoot[2]
robot.dynamic.position.value = tuple (config) robot.dynamic.position.value = tuple(config)
robot.dynamic.position.time = t robot.dynamic.position.time = t
robot.com.recompute (t) robot.com.recompute(t)
robot.leftAnkle.position.recompute (t) robot.leftAnkle.position.recompute(t)
robot.rightAnkle.position.recompute (t) robot.rightAnkle.position.recompute(t)
featurePos.append (config) featurePos.append(config)
featureCom.append (robot.com.value) featureCom.append(robot.com.value)
featureLa.append (robot.leftAnkle.position.value) featureLa.append(robot.leftAnkle.position.value)
featureRa.append (robot.rightAnkle.position.value) featureRa.append(robot.rightAnkle.position.value)
forceLeftFoot.append ((0.,0.,fl,0.,0.,0.,)) forceLeftFoot.append((
forceRightFoot.append ((0.,0.,fr,0.,0.,0.,)) 0.,
0.,
fl,
0.,
0.,
0.,
))
forceRightFoot.append((
0.,
0.,
fr,
0.,
0.,
0.,
))
t += 1 t += 1
fixedLeftFoot = \ fixedLeftFoot = SE3(robot.leftAnkle.position.value) * R3(0., 0., -0.107)
SE3 (robot.leftAnkle.position.value) * R3 (0., 0., -0.107) fixedRightFoot = SE3(robot.rightAnkle.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')
filePos = open (filename + '.posture', 'w') fileRa = open(filename + '.ra', 'w')
fileLa = open (filename + '.la', 'w') fileCom = open(filename + '.com', 'w')
fileRa = open (filename + '.ra', 'w') fileFl = open(filename + '.fl', 'w')
fileCom = open (filename + '.com', 'w') fileFr = open(filename + '.fr', 'w')
fileFl = open (filename + '.fl', 'w')
fileFr = open (filename + '.fr', 'w')
dt = .005 dt = .005
for (pos, la, ra, com, for (pos, la, ra, com, force_lf, force_rf, i) in zip(featurePos, featureLa, featureRa, featureCom, forceLeftFoot,
force_lf, force_rf, i) in zip (featurePos, featureLa, featureRa, forceRightFoot, range(10000000)):
featureCom, forceLeftFoot, t = i * dt
forceRightFoot, xrange (10000000)) : filePos.write("{0}".format(t))
t = i*dt fileLa.write("{0}".format(t))
filePos.write ("{0}".format (t)) fileRa.write("{0}".format(t))
fileLa.write ("{0}".format (t)) fileCom.write("{0}".format(t))
fileRa.write ("{0}".format (t)) fileFl.write("{0}".format(t))
fileCom.write ("{0}".format (t)) fileFr.write("{0}".format(t))
fileFl.write ("{0}".format (t))
fileFr.write ("{0}".format (t))
for x in pos: for x in pos:
filePos.write ("\t{0}".format (x)) filePos.write("\t{0}".format(x))
for row in la: for row in la:
for x in row: for x in row:
fileLa.write ("\t{0}".format (x)) fileLa.write("\t{0}".format(x))
for row in ra: for row in ra:
for x in row: for x in row:
fileRa.write ("\t{0}".format (x)) fileRa.write("\t{0}".format(x))
for x in com: for x in com:
fileCom.write ("\t{0}".format (x)) fileCom.write("\t{0}".format(x))
for x in force_lf: for x in force_lf:
fileFl.write ("\t{0}".format (x)) fileFl.write("\t{0}".format(x))
for x in force_rf: for x in force_rf:
fileFr.write ("\t{0}".format (x)) fileFr.write("\t{0}".format(x))
filePos.write ("\n") filePos.write("\n")
fileLa.write ("\n") fileLa.write("\n")
fileRa.write ("\n") fileRa.write("\n")
fileCom.write ("\n") fileCom.write("\n")
fileFl.write ("\n") fileFl.write("\n")
fileFr.write ("\n") fileFr.write("\n")
filePos.close () filePos.close()
fileLa.close () fileLa.close()
fileRa.close () fileRa.close()
fileCom.close () fileCom.close()
fileFl.close () fileFl.close()
fileFr.close () fileFr.close()
if __name__ == '__main__': if __name__ == '__main__':
#convert ('/opt/grx3.0/HRP2LAAS/etc/walkfwd') pass
# convert('/opt/grx3.0/HRP2LAAS/etc/walkfwd')
# -*- coding: utf-8 -*- # -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse CNRS # Copyright 2016, Olivier Stasse CNRS
# flake8: noqa
from __future__ import print_function from __future__ import print_function
from dynamic_graph.sot.talos.talos import Talos from dynamic_graph.sot.talos.talos import Talos
...@@ -3,14 +3,13 @@ ...@@ -3,14 +3,13 @@
from __future__ import print_function from __future__ import print_function
import numpy as np import pinocchio as se3
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot
from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
from dynamic_graph import plug from dynamic_graph import plug
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Vector
from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as se3
from rospkg import RosPack
# Internal helper tool. # Internal helper tool.
def matrixToTuple(M): def matrixToTuple(M):
...@@ -20,19 +19,14 @@ def matrixToTuple(M): ...@@ -20,19 +19,14 @@ def matrixToTuple(M):
res.append(tuple(i)) res.append(tuple(i))
return tuple(res) return tuple(res)
class Talos(AbstractHumanoidRobot): class Talos(AbstractHumanoidRobot):
""" """
This class defines a Talos robot This class defines a Talos robot
""" """
forceSensorInLeftAnkle = ((1.,0.,0.,0.), forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
(0.,1.,0.,0.), forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
(0.,0.,1.,-0.107),
(0.,0.,0.,1.))
forceSensorInRightAnkle = ((1.,0.,0.,0.),
(0.,1.,0.,0.),
(0.,0.,1.,-0.107),
(0.,0.,0.,1.))
""" """
TODO: Confirm the position and existence of these sensors TODO: Confirm the position and existence of these sensors
accelerometerPosition = np.matrix (( accelerometerPosition = np.matrix ((
...@@ -50,36 +44,35 @@ class Talos(AbstractHumanoidRobot): ...@@ -50,36 +44,35 @@ class Talos(AbstractHumanoidRobot):
)) ))
""" """
def smallToFull(self, config): def smallToFull(self, config):
#Gripper position in full configuration: 27:34, and 41:48 # Gripper position in full configuration: 27:34, and 41:48
#Small configuration: 36 DOF # Small configuration: 36 DOF
#Full configuration: 50 DOF # Full configuration: 50 DOF
res = config[0:27] + 7*(0.,) + config[27:34]+ 7*(0.,)+config[34:] res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (0., ) + config[34:]
return res return res
def __init__(self, name, initialConfig, device = None, tracer = None): def __init__(self, name, initialConfig, device=None, tracer=None):
self.OperationalPointsMap = {'left-wrist' : 'arm_left_7_joint', self.OperationalPointsMap = {
'right-wrist' : 'arm_right_7_joint', 'left-wrist': 'arm_left_7_joint',
'left-ankle' : 'leg_left_6_joint', 'right-wrist': 'arm_right_7_joint',
'right-ankle' : 'leg_right_6_joint', 'left-ankle': 'leg_left_6_joint',
'gaze' : 'head_2_joint', 'right-ankle': 'leg_right_6_joint',
'waist' : 'root_joint', 'gaze': 'head_2_joint',
'chest' : 'torso_2_joint'} 'waist': 'root_joint',
'chest': 'torso_2_joint'
}
from rospkg import RosPack from rospkg import RosPack
rospack = RosPack() rospack = RosPack()
urdfPath = rospack.get_path('talos_data')+"/urdf/talos_reduced.urdf" urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data')+"/../"] urdfDir = [rospack.get_path('talos_data') + "/../"]
# Create a wrapper to access the dynamic model provided through an urdf file. # Create a wrapper to access the dynamic model provided through an urdf file.
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as se3
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper() pinocchioRobot = RobotWrapper()
pinocchioRobot.initFromURDF(urdfPath, urdfDir, se3.JointModelFreeFlyer()) pinocchioRobot.initFromURDF(urdfPath, urdfDir, se3.JointModelFreeFlyer())
self.pinocchioModel = pinocchioRobot.model self.pinocchioModel = pinocchioRobot.model
self.pinocchioData = pinocchioRobot.data self.pinocchioData = pinocchioRobot.data
AbstractHumanoidRobot.__init__ (self, name, tracer) AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist') self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest') self.OperationalPoints.append('chest')
...@@ -97,25 +90,20 @@ class Talos(AbstractHumanoidRobot): ...@@ -97,25 +90,20 @@ class Talos(AbstractHumanoidRobot):
# TODO For position limit, we remove the first value to get # TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not # a vector of the good size because SoT use euler angles and not
# quaternions... # quaternions...
self.device.setPositionBounds ( self.device.setPositionBounds(self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:],
self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:], self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:])
self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:]) self.device.setVelocityBounds((-self.pinocchioModel.velocityLimit).T.tolist()[0],
self.device.setVelocityBounds ( self.pinocchioModel.velocityLimit.T.tolist()[0])