diff --git a/src/dynamic_graph/sot/pyrene/prologue.py b/src/dynamic_graph/sot/pyrene/prologue.py index df663b0..5b4c304 100644 --- a/src/dynamic_graph/sot/pyrene/prologue.py +++ b/src/dynamic_graph/sot/pyrene/prologue.py @@ -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 --- # # # diff --git a/src/dynamic_graph/sot/pyrene/robot.py b/src/dynamic_graph/sot/pyrene/robot.py index 48aaae7..5d3189b 100644 --- a/src/dynamic_graph/sot/pyrene/robot.py +++ b/src/dynamic_graph/sot/pyrene/robot.py @@ -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. @@ -80,4 +108,5 @@ def __init__(self, name, matrixToTuple(cameraTopRightPosition), "gaze")) """ + __all__ = ["Robot"] diff --git a/src/dynamic_graph/sot/pyrene/seqplay.py b/src/dynamic_graph/sot/pyrene/seqplay.py index 6322f02..eccccb4 100644 --- a/src/dynamic_graph/sot/pyrene/seqplay.py +++ b/src/dynamic_graph/sot/pyrene/seqplay.py @@ -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') diff --git a/src/dynamic_graph/sot/talos/__init__.py b/src/dynamic_graph/sot/talos/__init__.py index 9fbf179..52a3319 100644 --- a/src/dynamic_graph/sot/talos/__init__.py +++ b/src/dynamic_graph/sot/talos/__init__.py @@ -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 diff --git a/src/dynamic_graph/sot/talos/talos.py b/src/dynamic_graph/sot/talos/talos.py index ebe0e4c..50f8ba5 100644 --- a/src/dynamic_graph/sot/talos/talos.py +++ b/src/dynamic_graph/sot/talos/talos.py @@ -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 @@ def __init__(self, name, initialConfig, device = None, tracer = None): # 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 @@ def __init__(self, name, initialConfig, device = None, tracer = None): 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] diff --git a/tests/appli-test-simple-seq-play.py b/tests/appli-test-simple-seq-play.py index 2cab49b..b9ac657 100644 --- a/tests/appli-test-simple-seq-play.py +++ b/tests/appli-test-simple-seq-play.py @@ -1,56 +1,52 @@ -#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) jacobian_posture = hstack([first_6, other_dof]) -taskPosture.feature.jacobianIN.value = matrixToTuple( jacobian_posture ) +taskPosture.feature.jacobianIN.value = matrixToTuple(jacobian_posture) taskPosture.feature.setReference(taskPosture.featureDes.name) taskPosture.add(taskPosture.feature.name) - # Create the sequence player aSimpleSeqPlay = SimpleSeqPlay('aSimpleSeqPlay') -aSimpleSeqPlay.load("/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz") +aSimpleSeqPlay.load( + "/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz") aSimpleSeqPlay.setTimeToStart(10.0) -# Connects the sequence player to the posture task -from dynamic_graph.sot.core import Selec_of_vector -from dynamic_graph import plug plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN) # Connects the dynamics to the current feature of the posture task getPostureValue = Selec_of_vector("current_posture") -getPostureValue.selec(6,robotDim) +getPostureValue.selec(6, robotDim) plug(robot.dynamic.position, getPostureValue.sin) plug(getPostureValue.sout, taskPosture.feature.errorIN) plug(getPostureValue.sout, aSimpleSeqPlay.currentPosture) # Set the gain of the posture task -setGain(taskPosture.gain,(4.9,0.9,0.01,0.9)) +setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9)) plug(taskPosture.gain.gain, taskPosture.controlGain) plug(taskPosture.error, taskPosture.gain.error) -# Create the solver -from dynamic_graph.sot.core import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) -plug(sot.control,robot.device.control) +plug(sot.control, robot.device.control) taskPosture.featureDes.errorIN.recompute(0) diff --git a/tests/appli.py b/tests/appli.py index 9460c80..f9bd54d 100644 --- a/tests/appli.py +++ b/tests/appli.py @@ -1,9 +1,14 @@ -from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd +# flake8: noqa +from dynamic_graph import plug +from dynamic_graph.ros import RosPublish +from dynamic_graph.sot.core import SOT from dynamic_graph.sot.core.matrix_util import matrixToTuple +from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd from numpy import eye -taskRH = MetaTaskKine6d('rh',robot.dynamic,'rh',robot.OperationalPointsMap['right-wrist']) -handMgrip = eye(4); handMgrip[0:3,3] = (0.1,0,0) +taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) +handMgrip = eye(4) +handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') # --- STATIC COM (if not walking) @@ -12,34 +17,28 @@ taskCom.featureDes.errorIN.value = robot.dynamic.com.value taskCom.task.controlGain.value = 10 - # --- CONTACTS #define contactLF and contactRF -contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle']) +contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) contactLF.feature.frame('desired') contactLF.gain.setConstant(10) contactLF.keep() locals()['contactLF'] = contactLF -contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle']) +contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) contactRF.feature.frame('desired') contactRF.gain.setConstant(10) contactRF.keep() locals()['contactRF'] = contactRF - -from dynamic_graph import plug -from dynamic_graph.sot.core import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) -plug(sot.control,robot.device.control) +plug(sot.control, robot.device.control) -from dynamic_graph.ros import RosPublish -ros_publish_state = RosPublish ("ros_publish_state") -ros_publish_state.add ("vector", "state", "/sot_control/state") -from dynamic_graph import plug -plug (robot.device.state, ros_publish_state.state) -robot.device.after.addDownsampledSignal ("ros_publish_state.trigger", 100) +ros_publish_state = RosPublish("ros_publish_state") +ros_publish_state.add("vector", "state", "/sot_control/state") +plug(robot.device.state, ros_publish_state.state) +robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) #target = (0.5,-0.2,1.0) diff --git a/tests/appli_basic.py b/tests/appli_basic.py index 4b71354..9011ec2 100644 --- a/tests/appli_basic.py +++ b/tests/appli_basic.py @@ -1,10 +1,12 @@ - -from dynamic_graph.ros import RosPublish -ros_publish_state = RosPublish ("ros_publish_state") -ros_publish_state.add ("vector", "state", "/sot_hpp/state") +# flake8: noqa from dynamic_graph import plug -plug (robot.device.state, ros_publish_state.state) -robot.device.after.addDownsampledSignal ("ros_publish_state.trigger", 100) +from dynamic_graph.ros import RosPublish + +ros_publish_state = RosPublish("ros_publish_state") +ros_publish_state.add("vector", "state", "/sot_hpp/state") +plug(robot.device.state, ros_publish_state.state) +robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) -v=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) -robot.device.control.value=v +v = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) +robot.device.control.value = v diff --git a/tests/publish-robot-state.py b/tests/publish-robot-state.py index 8a405f0..ecc278e 100755 --- a/tests/publish-robot-state.py +++ b/tests/publish-robot-state.py @@ -1,22 +1,28 @@ #!/usr/bin/python -import sys +# flake8: noqa import rospy - -from std_srvs.srv import * from dynamic_graph_bridge.srv import * from dynamic_graph_bridge_msgs.srv import * +from std_srvs.srv import * + +try: + # Python 2 + input = raw_input +except NameError: + pass -def launchScript(code,title,description = ""): - raw_input(title+': '+description) + +def launchScript(code, title, description=""): + input(title + ': ' + description) rospy.loginfo(title) rospy.loginfo(code) for line in code: if line != '' and line[0] != '#': - print line + print(line) answer = runCommandClient(str(line)) rospy.logdebug(answer) - print answer - rospy.loginfo("...done with "+title) + print(answer) + rospy.loginfo("...done with " + title) # Waiting for services @@ -32,13 +38,11 @@ def launchScript(code,title,description = ""): runCommandClient = rospy.ServiceProxy('run_command', RunCommand) runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty) - initCode = open( "servooff.py", "r").read().split("\n") - - rospy.loginfo("Stack of Tasks launched") + initCode = open("servooff.py", "r").read().split("\n") + rospy.loginfo("Stack of Tasks launched") - launchScript(initCode,'Publishing robotState_ros') + launchScript(initCode, 'Publishing robotState_ros') -except rospy.ServiceException, e: +except rospy.ServiceException as e: rospy.logerr("Service call failed: %s" % e) - diff --git a/tests/run_motion_python_node.py b/tests/run_motion_python_node.py index c6c1f40..e551269 100755 --- a/tests/run_motion_python_node.py +++ b/tests/run_motion_python_node.py @@ -21,9 +21,11 @@ # System imports import sys import time + # ROS imports import rospy -from actionlib import SimpleActionClient, GoalStatus +from actionlib import GoalStatus, SimpleActionClient + from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal @@ -69,6 +71,7 @@ def wait_for_valid_time(timeout): def get_status_string(status_code): return GoalStatus.to_string(status_code) + if __name__ == '__main__': rospy.init_node('run_motion_python') if len(sys.argv) < 2: diff --git a/tests/servooff.py b/tests/servooff.py index e04f530..ba19692 100644 --- a/tests/servooff.py +++ b/tests/servooff.py @@ -1,7 +1,8 @@ +# flake8: noqa from dynamic_graph import plug from dynamic_graph.ros import RosPublish -robot.ros=RosPublish('rosPublish') -robot.ros.add('vector','robotState_ros','robotState') -plug(robot.device.robotState,robot.ros.signal('robotState_ros')) -robot.device.after.addDownsampledSignal('rosPublish.trigger',1); +robot.ros = RosPublish('rosPublish') +robot.ros.add('vector', 'robotState_ros', 'robotState') +plug(robot.device.robotState, robot.ros.signal('robotState_ros')) +robot.device.after.addDownsampledSignal('rosPublish.trigger', 1) diff --git a/tests/test-simple-seq-play.py b/tests/test-simple-seq-play.py index fba6728..e60e456 100755 --- a/tests/test-simple-seq-play.py +++ b/tests/test-simple-seq-play.py @@ -1,13 +1,21 @@ #!/usr/bin/python +# flake8: noqa import sys -import rospy -from std_srvs.srv import * +import rospy from dynamic_graph_bridge.srv import * from dynamic_graph_bridge_msgs.srv import * +from std_srvs.srv import * + +try: + # Python 2 + input = raw_input +except NameError: + pass + -def launchScript(code,title,description = ""): - raw_input(title+': '+description) +def launchScript(code, title, description=""): + input(title + ': ' + description) rospy.loginfo(title) rospy.loginfo(code) for line in code: @@ -16,7 +24,7 @@ def launchScript(code,title,description = ""): answer = runCommandClient(str(line)) rospy.logdebug(answer) print answer - rospy.loginfo("...done with "+title) + rospy.loginfo("...done with " + title) # Waiting for services @@ -32,21 +40,19 @@ def launchScript(code,title,description = ""): runCommandClient = rospy.ServiceProxy('run_command', RunCommand) runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty) - initCode = open( "appli-test-simple-seq-play.py", "r").read().split("\n") - - rospy.loginfo("Stack of Tasks launched") + initCode = open("appli-test-simple-seq-play.py", "r").read().split("\n") + rospy.loginfo("Stack of Tasks launched") - launchScript(initCode,'initialize SoT') - raw_input("Wait before starting the dynamic graph") + launchScript(initCode, 'initialize SoT') + input("Wait before starting the dynamic graph") runCommandStartDynamicGraph() - raw_input("Wait before pushing the posture task in SoT") + input("Wait before pushing the posture task in SoT") runCommandClient("sot.push(taskPosture.name)") runCommandClient("robot.device.control.recompute(0)") - raw_input("Wait before starting the seqplay") + input("Wait before starting the seqplay") runCommandClient("aSimpleSeqPlay.start()") except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e) - diff --git a/tests/test.py b/tests/test.py index 770ca33..3a7759a 100755 --- a/tests/test.py +++ b/tests/test.py @@ -1,22 +1,28 @@ #!/usr/bin/python -import sys +# flake8: noqa import rospy - -from std_srvs.srv import * from dynamic_graph_bridge.srv import * from dynamic_graph_bridge_msgs.srv import * +from std_srvs.srv import * + +try: + # Python 2 + input = raw_input +except NameError: + pass + -def launchScript(code,title,description = ""): - raw_input(title+': '+description) +def launchScript(code, title, description=""): + input(title + ': ' + description) rospy.loginfo(title) rospy.loginfo(code) for line in code: if line != '' and line[0] != '#': - print line + print(line) answer = runCommandClient(str(line)) rospy.logdebug(answer) - print answer - rospy.loginfo("...done with "+title) + print(answer) + rospy.loginfo("...done with " + title) # Waiting for services @@ -32,8 +38,8 @@ def launchScript(code,title,description = ""): runCommandClient = rospy.ServiceProxy('run_command', RunCommand) runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty) - initCode = open( "appli.py", "r").read().split("\n") - + initCode = open("appli.py", "r").read().split("\n") + rospy.loginfo("Stack of Tasks launched") # runCommandClient("from dynamic_graph import plug") @@ -42,14 +48,13 @@ def launchScript(code,title,description = ""): # runCommandClient("sot.setSize(robot.dynamic.getDimension())") # runCommandClient("plug(sot.control,robot.device.control)") - launchScript(initCode,'initialize SoT') - raw_input("Wait before starting the dynamic graph") + launchScript(initCode, 'initialize SoT') + input("Wait before starting the dynamic graph") runCommandStartDynamicGraph() - raw_input("Wait before moving the hand") + input("Wait before moving the hand") runCommandClient("target = (0.5,-0.2,1.0)") runCommandClient("gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))") runCommandClient("sot.push(taskRH.task.name)") -except rospy.ServiceException, e: +except rospy.ServiceException as e: rospy.logerr("Service call failed: %s" % e) - diff --git a/tests/test_basic.py b/tests/test_basic.py index 3e34c6d..60d2665 100755 --- a/tests/test_basic.py +++ b/tests/test_basic.py @@ -1,22 +1,28 @@ #!/usr/bin/python -import sys +# flake8: noqa import rospy - -from std_srvs.srv import * from dynamic_graph_bridge.srv import * from dynamic_graph_bridge_msgs.srv import * +from std_srvs.srv import * + +try: + # Python 2 + input = raw_input +except NameError: + pass + -def launchScript(code,title,description = ""): - raw_input(title+': '+description) +def launchScript(code, title, description=""): + input(title + ': ' + description) rospy.loginfo(title) rospy.loginfo(code) for line in code: if line != '' and line[0] != '#': - print line + print(line) answer = runCommandClient(str(line)) rospy.logdebug(answer) - print answer - rospy.loginfo("...done with "+title) + print(answer) + rospy.loginfo("...done with " + title) # Waiting for services @@ -32,8 +38,8 @@ def launchScript(code,title,description = ""): runCommandClient = rospy.ServiceProxy('run_command', RunCommand) runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty) - initCode = open( "appli_basic.py", "r").read().split("\n") - + initCode = open("appli_basic.py", "r").read().split("\n") + rospy.loginfo("Stack of Tasks launched") # runCommandClient("from dynamic_graph import plug") @@ -42,10 +48,9 @@ def launchScript(code,title,description = ""): # runCommandClient("sot.setSize(robot.dynamic.getDimension())") # runCommandClient("plug(sot.control,robot.device.control)") - launchScript(initCode,'initialize SoT') - raw_input("Wait before starting the dynamic graph") + launchScript(initCode, 'initialize SoT') + input("Wait before starting the dynamic graph") runCommandStartDynamicGraph() -except rospy.ServiceException, e: +except rospy.ServiceException as e: rospy.logerr("Service call failed: %s" % e) - diff --git a/tests/test_seqplay.py b/tests/test_seqplay.py index fb1d3ae..c20c462 100644 --- a/tests/test_seqplay.py +++ b/tests/test_seqplay.py @@ -6,133 +6,144 @@ # ______________________________________________________________________________ # ****************************************************************************** +import pinocchio as se3 +from dynamic_graph import plug +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.core.meta_tasks import setGain +from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio +from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot +from dynamic_graph.sot.tools import SimpleSeqPlay +from numpy import hstack, identity, zeros +from pinocchio.robot_wrapper import RobotWrapper -#----------------------------------------------------------------------------- -#SET THE PATH TO THE URDF AND MESHES -#Define robotName, urdfpath and initialConfig +# ----------------------------------------------------------------------------- +# SET THE PATH TO THE URDF AND MESHES +# Define robotName, urdfpath and initialConfig -#Make sure talos_description is in the ROS_PACKAGE_PATH -#from rospkg import RosPack -#rospack = RosPack() -#urdfPath = rospack.get_path('talos_description')+"/robots/talos_full_collision.urdf" -#urdfDir = [rospack.get_path('talos_description')+"/../"] +# Make sure talos_description is in the ROS_PACKAGE_PATH +# from rospkg import RosPack +# rospack = RosPack() +# urdfPath = rospack.get_path('talos_description')+"/robots/talos_full_collision.urdf" +# urdfDir = [rospack.get_path('talos_description')+"/../"] -URDFPATH = "~/git/pyrene/talos-data"+"/robots/talos_reduced.urdf" -URDFDIR = ["~/git/pyrene/talos-data"+"/../"] +URDFPATH = "~/git/pyrene/talos-data" + "/robots/talos_reduced.urdf" +URDFDIR = ["~/git/pyrene/talos-data" + "/../"] MOTION_SEQUENCE = "~/git/pyrene/pyrene-motions/grabHandrail15/stairs_15cm_handrail_grab_actuated" DISPLAY = True - dt = 1e-3 robotName = 'TALOS' -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'} - -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.1, #Left Arm - -0.25847 , -0.173046, 0.0002 , -0.525366, 0.0, 0.0, 0.1, 0.1, #Right Arm - 0., 0. #Head - ) - - -#----------------------------------------------------------------------------- -#---- ROBOT SPECIFICATIONS---------------------------------------------------- -#----------------------------------------------------------------------------- - - -#----------------------------------------------------------------------------- -#---- DYN -------------------------------------------------------------------- -#----------------------------------------------------------------------------- -from pinocchio.robot_wrapper import RobotWrapper -import pinocchio as se3 -from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio +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' +} + +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.1, # Left Arm + -0.25847, + -0.173046, + 0.0002, + -0.525366, + 0.0, + 0.0, + 0.1, + 0.1, # Right Arm + 0., + 0. # Head +) + +# ----------------------------------------------------------------------------- +# ---- ROBOT SPECIFICATIONS---------------------------------------------------- +# ----------------------------------------------------------------------------- + pinocchioRobot = RobotWrapper(URDFPATH, URDFDIR, se3.JointModelFreeFlyer()) + pinocchioRobot.initDisplay(loadModel=DISPLAY) if DISPLAY: pinocchioRobot.display(fromSotToPinocchio(halfSitting)) +robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data, halfSitting, OperationalPointsMap) -from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot -robot = HumanoidRobot(robotName, pinocchioRobot.model, - pinocchioRobot.data, halfSitting, OperationalPointsMap) - - -# ------------------------------------------------------------------------------ -# ---- Kinematic Stack of Tasks (SoT) ----------------------------------------- -# ------------------------------------------------------------------------------ -from dynamic_graph import plug -from dynamic_graph.sot.core import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) -plug(sot.control,robot.device.control) - - - - - -# DEFINE POSTURE TASK -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 +plug(sot.control, robot.device.control) 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) jacobian_posture = hstack([first_6, other_dof]) -taskPosture.feature.jacobianIN.value = matrixToTuple( jacobian_posture ) +taskPosture.feature.jacobianIN.value = matrixToTuple(jacobian_posture) taskPosture.feature.setReference(taskPosture.featureDes.name) taskPosture.add(taskPosture.feature.name) - -#DEFINE SEQUENCE PLAYER -from dynamic_graph.sot.tools import SimpleSeqPlay seqplay = SimpleSeqPlay("seq_play") seqplay.load(MOTION_SEQUENCE) - - -#MAKE CONNECTIONS -from dynamic_graph.sot.core import Selec_of_vector - plug(seqplay.posture, taskPosture.featureDes.errorIN) getPostureValue = Selec_of_vector("current_posture") -getPostureValue.selec(6,robotDim) +getPostureValue.selec(6, robotDim) plug(robot.dynamic.position, getPostureValue.sin) plug(getPostureValue.sout, taskPosture.feature.errorIN) plug(getPostureValue.sout, seqplay.currentPosture) -setGain(taskPosture.gain,(4.9,0.9,0.01,0.9)) +setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9)) plug(taskPosture.gain.gain, taskPosture.controlGain) plug(taskPosture.error, taskPosture.gain.error) -#START SEQUENCE PLAYER +# START SEQUENCE PLAYER seqplay.start() taskPosture.featureDes.errorIN.recompute(0) -#PUSH TO SOLVER +# PUSH TO SOLVER sot.push(taskPosture.name) -#------------------------------------------------------------------------------- -#----- MAIN LOOP --------------------------------------------------------------- -#------------------------------------------------------------------------------- +# ------------------------------------------------------------------------------- +# ----- MAIN LOOP --------------------------------------------------------------- +# ------------------------------------------------------------------------------- + def runner(n): - for i in xrange(n): + for i in range(n): robot.device.increment(dt) pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value)) + runner(3000)