Commit 49fb9628 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Style] fix flake8, isort & yapf

parent d59165f4
Pipeline #3845 passed with stage
in 43 seconds
[flake8]
max-line-length = 119
exclude = cmake
[yapf]
column_limit = 119
[isort]
line_length = 119
skip = cmake
......@@ -2,24 +2,19 @@
# Copyright 2013, Florent Lamiraux, Francesco Morsillo CNRS
from numpy import eye
from dynamic_graph.sot.core.feature_position import FeaturePosition
from dynamic_graph.sot.core import FeatureGeneric, FeatureJointLimits, Task, \
JointLimitator
from dynamic_graph.sot.dyninv import SolverKine, TaskDynLimits, TaskDynInequality
from dynamic_graph.sot.dyninv.meta_tasks_dyn import gotoNd, MetaTaskDynCom, \
MetaTaskDynPosture
from dynamic_graph.sot.dyninv.meta_task_dyn_6d import MetaTaskDyn6d
from dynamic_graph import plug
from numpy import eye
from dynamic_graph.sot.core import FeatureGeneric
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.dyninv import SolverKine, TaskDynInequality, TaskDynLimits
from dynamic_graph.sot.dyninv.meta_task_dyn_6d import MetaTaskDyn6d
from dynamic_graph.sot.dyninv.meta_tasks_dyn import MetaTaskDynCom, MetaTaskDynPosture
class Solver:
class Solver:
def __init__(self, robot):
self.robot = robot
"""
# Make sure control does not exceed joint limits.
self.jointLimitator = JointLimitator('joint_limitator')
......@@ -32,13 +27,12 @@ class Solver:
self.sot = SolverKine('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Set second order computation
self.robot.device.setSecondOrderIntegration()
self.sot.setSecondOrderKinematics()
plug(self.robot.device.velocity,self.robot.dynamic.velocity)
plug(self.robot.dynamic.velocity,self.sot.velocity)
plug(self.robot.device.velocity, self.robot.dynamic.velocity)
plug(self.robot.dynamic.velocity, self.sot.velocity)
"""
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
......@@ -48,154 +42,151 @@ class Solver:
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
"""
# Plug the solver control into the robot.
plug(self.sot.control, robot.device.control)
def push (self, task):
def push(self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push (task.name)
if task.name!="taskposture" and "taskposture" in self.toList():
self.sot.push(task.name)
if task.name != "taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm (self, task):
def rm(self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm (task.name)
self.sot.rm(task.name)
def pop (self):
def pop(self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop ()
self.sot.pop()
def __str__ (self):
return self.sot.display ()
def __str__(self):
return self.sot.display()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1],self.sot.dispStack().split('|')[1:])
return map(lambda x: x[1:-1], self.sot.dispStack().split('|')[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear ()
self.sot.clear()
def setTaskLim(taskLim,robot):
def setTaskLim(taskLim, robot):
"""
Sets the parameters for teh 'task-limits'
"""
# Angular position and velocity limits
plug(robot.dynamic.position,taskLim.position)
plug(robot.dynamic.velocity,taskLim.velocity)
plug(robot.dynamic.position, taskLim.position)
plug(robot.dynamic.velocity, taskLim.velocity)
taskLim.dt.value = robot.timeStep
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskLim.referencePosInf.value = robot.dynamic.lowerJl.value
taskLim.referencePosSup.value = robot.dynamic.upperJl.value
#dqup = (0, 0, 0, 0, 0, 0, 200, 220, 250, 230, 290, 520, 200, 220, 250, 230, 290, 520, 250, 140, 390, 390, 240, 140, 240, 130, 270, 180, 330, 240, 140, 240, 130, 270, 180, 330)
dqup = (1000,)*robot.dimension ########################
taskLim.referenceVelInf.value = tuple([-val*3.14/180 for val in dqup])
taskLim.referenceVelSup.value = tuple([ val*3.14/180 for val in dqup])
# dqup = (0, 0, 0, 0, 0, 0, 200, 220, 250, 230, 290, 520, 200, 220, 250, 230, 290, 520, 250, 140, 390, 390, 240,
# 140, 240, 130, 270, 180, 330, 240, 140, 240, 130, 270, 180, 330)
dqup = (1000, ) * robot.dimension
taskLim.referenceVelInf.value = tuple([-val * 3.14 / 180 for val in dqup])
taskLim.referenceVelSup.value = tuple([val * 3.14 / 180 for val in dqup])
taskLim.controlGain.value = 0.3
def setContacts(contactLF,contactRF):
def setContacts(contactLF, contactRF):
"""
Sets the parameters for teh contacts
"""
# Left foot
contactLF.featureDes.velocity.value=(0,0,0,0,0,0)
contactLF.featureDes.velocity.value = (0, 0, 0, 0, 0, 0)
contactLF.feature.frame('desired')
contactLF.name = "LF"
# Right foot
contactRF.featureDes.velocity.value=(0,0,0,0,0,0)
contactRF.featureDes.velocity.value = (0, 0, 0, 0, 0, 0)
contactRF.feature.frame('desired')
contactRF.name = "RF"
contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105))
contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-0.105,-0.105,-0.105))
contactRF.support = ((0.11, -0.08, -0.08, 0.11), (-0.045, -0.045, 0.07, 0.07), (-0.105, -0.105, -0.105, -0.105))
contactLF.support = ((0.11, -0.08, -0.08, 0.11), (-0.07, -0.07, 0.045, 0.045), (-0.105, -0.105, -0.105, -0.105))
# Imposed errordot = 0
contactLF.feature.errordot.value=(0,0,0,0,0,0)
contactRF.feature.errordot.value=(0,0,0,0,0,0)
contactLF.feature.errordot.value = (0, 0, 0, 0, 0, 0)
contactRF.feature.errordot.value = (0, 0, 0, 0, 0, 0)
def createTasks(robot):
# MetaTasks dictonary
robot.mTasks = dict()
robot.tasksIne = dict()
# Foot contacts
robot.contactLF = MetaTaskDyn6d('contactLF',robot.dynamic,'lf','left-ankle')
robot.contactRF = MetaTaskDyn6d('contactRF',robot.dynamic,'rf','right-ankle')
setContacts(robot.contactLF,robot.contactRF)
robot.contactLF = MetaTaskDyn6d('contactLF', robot.dynamic, 'lf', 'left-ankle')
robot.contactRF = MetaTaskDyn6d('contactRF', robot.dynamic, 'rf', 'right-ankle')
setContacts(robot.contactLF, robot.contactRF)
# MetaTasksDyn6d for other operational points
robot.mTasks['waist'] = MetaTaskDyn6d('waist', robot.dynamic, 'waist', 'waist')
robot.mTasks['chest'] = MetaTaskDyn6d('chest', robot.dynamic, 'chest', 'chest')
robot.mTasks['rh'] = MetaTaskDyn6d('rh', robot.dynamic, 'rh', 'right-wrist')
robot.mTasks['lh'] = MetaTaskDyn6d('lh', robot.dynamic, 'lh', 'left-wrist')
for taskName in robot.mTasks:
robot.mTasks[taskName].feature.frame('desired')
robot.mTasks[taskName].gain.setConstant(10)
robot.mTasks[taskName].task.dt.value = robot.timeStep
robot.mTasks[taskName].featureDes.velocity.value=(0,0,0,0,0,0)
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.14)
robot.mTasks[taskName].featureDes.velocity.value = (0, 0, 0, 0, 0, 0)
handMgrip = eye(4)
handMgrip[0:3, 3] = (0, 0, -0.14)
robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip)
robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip)
# CoM Task
robot.mTasks['com'] = MetaTaskDynCom(robot.dynamic,robot.timeStep)
robot.mTasks['com'] = MetaTaskDynCom(robot.dynamic, robot.timeStep)
robot.dynamic.com.recompute(0)
robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value
robot.mTasks['com'].task.controlGain.value = 10
robot.mTasks['com'].feature.selec.value = '011'
# Posture Task
robot.mTasks['posture'] = MetaTaskDynPosture(robot.dynamic,robot.timeStep)
robot.mTasks['posture'] = MetaTaskDynPosture(robot.dynamic, robot.timeStep)
robot.mTasks['posture'].ref = robot.halfSitting
robot.mTasks['posture'].gain.setConstant(5)
robot.mTasks['posture'].gain.setConstant(5)
## TASK INEQUALITY
# TASK INEQUALITY
# Task Height
featureHeight = FeatureGeneric('featureHeight')
plug(robot.dynamic.com,featureHeight.errorIN)
plug(robot.dynamic.Jcom,featureHeight.jacobianIN)
robot.tasksIne['taskHeight']=TaskDynInequality('taskHeight')
plug(robot.dynamic.velocity,robot.tasksIne['taskHeight'].qdot)
plug(robot.dynamic.com, featureHeight.errorIN)
plug(robot.dynamic.Jcom, featureHeight.jacobianIN)
robot.tasksIne['taskHeight'] = TaskDynInequality('taskHeight')
plug(robot.dynamic.velocity, robot.tasksIne['taskHeight'].qdot)
robot.tasksIne['taskHeight'].add(featureHeight.name)
robot.tasksIne['taskHeight'].selec.value = '100'
robot.tasksIne['taskHeight'].referenceInf.value = (0.,0.,0.) # Xmin, Ymin
robot.tasksIne['taskHeight'].referenceSup.value = (0.,0.,0.80771) # Xmax, Ymax
robot.tasksIne['taskHeight'].dt.value=robot.timeStep
robot.tasksIne['taskHeight'].referenceInf.value = (0., 0., 0.) # Xmin, Ymin
robot.tasksIne['taskHeight'].referenceSup.value = (0., 0., 0.80771) # Xmax, Ymax
robot.tasksIne['taskHeight'].dt.value = robot.timeStep
def createBalanceAndPosture(robot,solver):
def createBalanceAndPosture(robot, solver):
solver.clear()
# Task Limits
robot.taskLim = TaskDynLimits('taskLim')
setTaskLim(robot.taskLim,robot)
setTaskLim(robot.taskLim, robot)
# --- push tasks --- #
solver.sot.addContact(robot.contactLF)
......@@ -205,16 +196,14 @@ def createBalanceAndPosture(robot,solver):
solver.push(robot.mTasks['posture'].task)
def initialize (robot):
def initialize(robot):
# --- create solver --- #
solver = Solver (robot)
solver = Solver(robot)
# --- create tasks --- #
createTasks(robot)
createBalanceAndPosture(robot,solver)
createBalanceAndPosture(robot, solver)
return solver
......@@ -2,23 +2,18 @@
# Copyright 2013, Florent Lamiraux, Francesco Morsillo CNRS
from numpy import eye
from dynamic_graph.sot.core.feature_position import FeaturePosition
from dynamic_graph.sot.core import FeatureGeneric, FeatureJointLimits, Task, JointLimitator
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom
from dynamic_graph import plug
from dynamic_graph.sot.core import FeatureGeneric, JointLimitator
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom
from dynamic_graph.sot.dyninv import SolverKine, TaskInequality, TaskJointLimits
from dynamic_graph.sot.dyninv import SolverKine
from dynamic_graph.sot.dyninv import TaskJointLimits, TaskInequality
from dynamic_graph import plug
from numpy import eye
from dynamic_graph.sot.core.matrix_util import matrixToTuple
class Solver:
def __init__(self, robot):
self.robot = robot
......@@ -32,7 +27,6 @@ class Solver:
self.sot = SolverKine('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
......@@ -43,91 +37,84 @@ class Solver:
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
def push (self, task):
def push(self, task):
"""
Proxy method to push a task (not a MetaTask) in the sot
"""
self.sot.push (task.name)
if task.name!="taskposture" and "taskposture" in self.toList():
self.sot.push(task.name)
if task.name != "taskposture" and "taskposture" in self.toList():
self.sot.down("taskposture")
def rm (self, task):
def rm(self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.rm (task.name)
self.sot.rm(task.name)
def pop (self):
def pop(self):
"""
Proxy method to remove the last (usually posture) task from the sot
"""
self.sot.pop ()
self.sot.pop()
def __str__ (self):
return self.sot.display ()
def __str__(self):
return self.sot.display()
def toList(self):
"""
Creates the list of the tasks in the sot
"""
return map(lambda x: x[1:-1],self.sot.dispStack().split('|')[1:])
return map(lambda x: x[1:-1], self.sot.dispStack().split('|')[1:])
def clear(self):
"""
Proxy method to remove all tasks from the sot
"""
self.sot.clear ()
self.sot.clear()
def setTaskLim(taskJL,robot):
def setTaskLim(taskJL, robot):
"""
Sets the parameters for the 'joint-limits'
"""
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
plug(robot.dynamic.position,taskJL.position)
plug(robot.dynamic.position, taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = robot.timeStep
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
taskJL.selec.value = toFlags(range(6, 22) + range(22, 28) + range(29, 35))
def createTasks(robot):
# MetaTasks dictonary
robot.mTasks = dict()
robot.tasksIne = dict()
# Foot contacts
robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',
robot.OperationalPointsMap['left-ankle'])
robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(10)
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',
robot.OperationalPointsMap['right-ankle'])
robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(10)
# MetaTasksKine6d for other operational points
robot.mTasks['waist'] = MetaTaskKine6d('waist', robot.dynamic, 'waist',
robot.OperationalPointsMap['waist'])
robot.mTasks['chest'] = MetaTaskKine6d('chest', robot.dynamic, 'chest',
robot.OperationalPointsMap['chest'])
robot.mTasks['rh'] = MetaTaskKine6d('rh', robot.dynamic, 'rh',
robot.OperationalPointsMap['right-wrist'])
robot.mTasks['lh'] = MetaTaskKine6d('lh', robot.dynamic, 'lh',
robot.OperationalPointsMap['left-wrist'])
robot.mTasks['waist'] = MetaTaskKine6d('waist', robot.dynamic, 'waist', robot.OperationalPointsMap['waist'])
robot.mTasks['chest'] = MetaTaskKine6d('chest', robot.dynamic, 'chest', robot.OperationalPointsMap['chest'])
robot.mTasks['rh'] = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist'])
robot.mTasks['lh'] = MetaTaskKine6d('lh', robot.dynamic, 'lh', robot.OperationalPointsMap['left-wrist'])
for taskName in robot.mTasks:
robot.mTasks[taskName].feature.frame('desired')
robot.mTasks[taskName].gain.setConstant(10)
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.14)
handMgrip = eye(4)
handMgrip[0:3, 3] = (0, 0, -0.14)
robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip)
robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip)
# CoM Task
robot.mTasks['com'] = MetaTaskKineCom(robot.dynamic)
......@@ -139,50 +126,46 @@ def createTasks(robot):
# Posture Task
robot.mTasks['posture'] = MetaTaskKinePosture(robot.dynamic)
robot.mTasks['posture'].ref = robot.halfSitting
robot.mTasks['posture'].gain.setConstant(5)
## TASK INEQUALITY
robot.mTasks['posture'].gain.setConstant(5)
# TASK INEQUALITY
# Task Height
featureHeight = FeatureGeneric('featureHeight')
plug(robot.dynamic.com,featureHeight.errorIN)
plug(robot.dynamic.Jcom,featureHeight.jacobianIN)
robot.tasksIne['taskHeight']=TaskInequality('taskHeight')
plug(robot.dynamic.com, featureHeight.errorIN)
plug(robot.dynamic.Jcom, featureHeight.jacobianIN)
robot.tasksIne['taskHeight'] = TaskInequality('taskHeight')
robot.tasksIne['taskHeight'].add(featureHeight.name)
robot.tasksIne['taskHeight'].selec.value = '100'
robot.tasksIne['taskHeight'].referenceInf.value = (0.,0.,0.) # Xmin, Ymin
robot.tasksIne['taskHeight'].referenceSup.value = (0.,0.,0.80771) # Xmax, Ymax
robot.tasksIne['taskHeight'].dt.value=robot.timeStep
robot.tasksIne['taskHeight'].referenceInf.value = (0., 0., 0.) # Xmin, Ymin
robot.tasksIne['taskHeight'].referenceSup.value = (0., 0., 0.80771) # Xmax, Ymax
robot.tasksIne['taskHeight'].dt.value = robot.timeStep
def createBalance(robot,solver):
def createBalance(robot, solver):
solver.clear()
# Task Limits
robot.taskLim = TaskJointLimits('taskLim')
setTaskLim(robot.taskLim,robot)
setTaskLim(robot.taskLim, robot)
# --- push tasks --- #
solver.sot.addContact(robot.contactLF)
solver.sot.addContact(robot.contactRF)
solver.push(robot.taskLim)
solver.push(robot.mTasks['com'].task)
#solver.push(robot.mTasks['posture'].task)
# solver.push(robot.mTasks['posture'].task)
def initialize (robot):
def initialize(robot):
# --- create solver --- #
solver = Solver (robot)
solver = Solver(robot)
# --- create tasks --- #
createTasks(robot)
createBalance(robot,solver)
createBalance(robot, solver)
return solver
......@@ -4,16 +4,15 @@
import warnings
from numpy import identity
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT, FeatureGeneric, GainAdaptive, JointLimitator, Task
from dynamic_graph.sot.core.feature_position import FeaturePosition
from dynamic_graph.sot.core import FeatureGeneric, FeatureJointLimits, Task, \
JointLimitator, SOT
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from dynamic_graph.sot.core import GainAdaptive
from numpy import identity
class Solver:
class Solver:
def __init__(self, robot, solverType=SOT):
self.robot = robot
......@@ -28,7 +27,6 @@ class Solver:
self.sot.signal('damping').value = 1e-6
self.sot.setSize(self.robot.dimension)
# Plug the solver control into the filter.
plug(self.sot.control, self.jointLimitator.controlIN)
......@@ -38,22 +36,23 @@ class Solver:
if robot.device:
plug(self.jointLimitator.control, robot.device.control)
def push (self, task):
def push(self, task):
"""
Proxy method to push a task in the sot
"""
self.sot.push (task.name)
self.sot.push(task.name)
def remove (self, task):
def remove(self, task):
"""
Proxy method to remove a task from the sot
"""
self.sot.remove (task.name)
self.sot.remove(task.name)
def __str__(self):
return self.sot.display()
def __str__ (self):
return self.sot.display ()
def initializeSignals (application, robot):
def initializeSignals(application, robot):
"""
For portability, make some signals accessible as attributes.
"""
......@@ -63,14 +62,11 @@ def initializeSignals (application, robot):
application.comSelec = application.featureCom.selec
application.comdot = application.featureComDes.errordotIN
def createCenterOfMassFeatureAndTask(robot,
featureName, featureDesName,
taskName,
selec = '111',
ingain = 1.):
def createCenterOfMassFeatureAndTask(robot, featureName, featureDesName, taskName, selec='111', ingain=1.):
robot.dynamic.com.recompute(0)
robot.dynamic.Jcom.recompute(0)