Skip to content

GitLab

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

[Python 3] Format

parent 5821a3ae
Hide whitespace changes
Inline Side-by-side
src/dynamic_graph/sot/pyrene/prologue.py
View file @ 43892dd4
# -*- 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 --- #
# #
......
src/dynamic_graph/sot/pyrene/robot.py
View file @ 43892dd4
# -*- 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.
......@@ -80,4 +108,5 @@ class Robot (Talos):
matrixToTuple(cameraTopRightPosition), "gaze"))
"""
__all__ = ["Robot"]
src/dynamic_graph/sot/pyrene/seqplay.py
View file @ 43892dd4
......@@ -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 = []
......@@ -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')
src/dynamic_graph/sot/talos/__init__.py
View file @ 43892dd4
# -*- coding: utf-8 -*-
# Copyright 2016, Olivier Stasse CNRS
# flake8: noqa
from __future__ import print_function
from dynamic_graph.sot.talos.talos import Talos
src/dynamic_graph/sot/talos/talos.py
View file @ 43892dd4
......@@ -3,14 +3,13 @@
from __future__ import print_function
import numpy as np
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot
from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
import pinocchio as se3
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.
def matrixToTuple(M):
......@@ -20,19 +19,14 @@ def matrixToTuple(M):
res.append(tuple(i))
return tuple(res)
class Talos(AbstractHumanoidRobot):
"""
This class defines a Talos robot
"""
forceSensorInLeftAnkle = ((1.,0.,0.,0.),
(0.,1.,0.,0.),
(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.))
forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (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
accelerometerPosition = np.matrix ((
......@@ -50,36 +44,35 @@ class Talos(AbstractHumanoidRobot):
))
"""
def smallToFull(self, config):
#Gripper position in full configuration: 27:34, and 41:48
#Small configuration: 36 DOF
#Full configuration: 50 DOF
res = config[0:27] + 7*(0.,) + config[27:34]+ 7*(0.,)+config[34:]
# Gripper position in full configuration: 27:34, and 41:48
# Small configuration: 36 DOF
# Full configuration: 50 DOF
res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (0., ) + config[34:]
return res
def __init__(self, name, initialConfig, device = None, tracer = None):
self.OperationalPointsMap = {'left-wrist' : 'arm_left_7_joint',
'right-wrist' : 'arm_right_7_joint',
'left-ankle' : 'leg_left_6_joint',
'right-ankle' : 'leg_right_6_joint',
'gaze' : 'head_2_joint',
'waist' : 'root_joint',
'chest' : 'torso_2_joint'}
def __init__(self, name, initialConfig, device=None, tracer=None):
self.OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint',
'left-ankle': 'leg_left_6_joint',
'right-ankle': 'leg_right_6_joint',
'gaze': 'head_2_joint',
'waist': 'root_joint',
'chest': 'torso_2_joint'
}
from rospkg import RosPack
rospack = RosPack()
urdfPath = rospack.get_path('talos_data')+"/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data')+"/../"]
urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data') + "/../"]
# 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.initFromURDF(urdfPath, urdfDir, se3.JointModelFreeFlyer())
self.pinocchioModel = pinocchioRobot.model
self.pinocchioData = pinocchioRobot.data
AbstractHumanoidRobot.__init__ (self, name, tracer)
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
......@@ -97,25 +90,20 @@ class Talos(AbstractHumanoidRobot):
# TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not
# quaternions...
self.device.setPositionBounds (
self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:],
self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:])
self.device.setVelocityBounds (
(-self.pinocchioModel.velocityLimit).T.tolist()[0],
self.pinocchioModel.velocityLimit .T.tolist()[0])
self.device.setTorqueBounds (
(-self.pinocchioModel.effortLimit).T.tolist()[0],
self.pinocchioModel.effortLimit .T.tolist()[0])
self.device.setPositionBounds(self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:],
self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:])
self.device.setVelocityBounds((-self.pinocchioModel.velocityLimit).T.tolist()[0],
self.pinocchioModel.velocityLimit.T.tolist()[0])
self.device.setTorqueBounds((-self.pinocchioModel.effortLimit).T.tolist()[0],
self.pinocchioModel.effortLimit.T.tolist()[0])
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
self.AdditionalFrames.append(
("leftFootForceSensor",
self.forceSensorInLeftAnkle, self.OperationalPointsMap["left-ankle"]))
("leftFootForceSensor", self.forceSensorInLeftAnkle, self.OperationalPointsMap["left-ankle"]))
self.AdditionalFrames.append(
("rightFootForceSensor",
self.forceSensorInRightAnkle, self.OperationalPointsMap["right-ankle"]))
("rightFootForceSensor", self.forceSensorInRightAnkle, self.OperationalPointsMap["right-ankle"]))
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
......@@ -128,21 +116,21 @@ class Talos(AbstractHumanoidRobot):
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension*(0.,)
self.dynamic.velocity.value = self.dimension * (0., )
# Initialize acceleration derivator if chosen
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout,
self.accelerationDerivator.sin)
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension*(0.,)
self.dynamic.acceleration.value = self.dimension * (0., )
# Create operational points based on operational points map (if provided)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
__all__ = [Talos]
tests/appli-test-simple-seq-play.py
View file @ 43892dd4
#from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
# flake8: noqa
# from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph import plug
# Create the solver
# Connects the sequence player to the posture task
from dynamic_graph.sot.core import SOT, FeatureGeneric, GainAdaptive, Selec_of_vector, Task
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.tools import SimpleSeqPlay
from numpy import eye
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity, hstack, zeros
from dynamic_graph.sot.tools import SimpleSeqPlay
from numpy import eye, hstack, identity, zeros
# Create the posture task
task_name = "posture_task"
taskPosture = Task(task_name)
taskPosture.dyn = robot.dynamic
taskPosture.feature = FeatureGeneric('feature_'+task_name)
taskPosture.featureDes = FeatureGeneric('feature_des_'+task_name)
taskPosture.gain = GainAdaptive("gain_"+task_name)
taskPosture.feature = FeatureGeneric('feature_' + task_name)
taskPosture.featureDes = FeatureGeneric('feature_des_' + task_name)
taskPosture.gain = GainAdaptive("gain_" + task_name)
robotDim = robot.dynamic.getDimension()
first_6 = zeros((32,6))
other_dof = identity(robotDim-6)
first_6 = zeros((32, 6))
other_dof = identity(robotDim - 6)