Commit b7518e7a authored by Francois Keith's avatar Francois Keith
Browse files

First draft for all python scripts.

Not completely using new macros.
parent 599846d2
# ______________________________________________________________________________
# ******************************************************************************
#
# ______________________________________________________________________________
# ******************************************************************************
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
import dynamic_graph.script_shortcuts
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
from dynamic_graph.sot.dyninv.meta_task_dyn_6d import MetaTaskDyn6d
from numpy import *
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('robot', device=RobotDynSimu('robot'))
plug(robot.device.state, robot.dynamic.position)
from dynamic_graph.ros import *
ros = Ros(robot)
# --- ROBOT SIMU ---------------------------------------------------------------
#robotName = 'romeo'
#robotDim=robotDimension[robotName]
#robot = RobotDynSimu("romeo")
#robot.resize(robotDim)
dt=5e-3
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
# --- shortcuts -------------------------------------------------
@optionalparentheses
def q():
if 'dyn' in globals(): print robot.dynamic.ffposition.__repr__()
print robot.state.__repr__()
@optionalparentheses
def qdot(): print robot.control.__repr__()
@optionalparentheses
def t(): print robot.state.time-1
@optionalparentheses
def iter(): print 'iter = ',robot.state.time
@optionalparentheses
def status(): print runner.isPlay
# --- DYN ----------------------------------------------------------------------
plug(robot.device.state, robot.dynamic.position)
plug(robot.device.velocity,robot.dynamic.velocity)
robot.dynamic.acceleration.value = robot.dimension*(0.,)
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
robot.device.control.unplug()
# --- Task Dyn -----------------------------------------
# Task right hand
task=MetaTaskDyn6d('rh',robot.dynamic,'task')
task.ref = ((0,0,-1,0.22),(0,1,0,-0.37),(1,0,0,.74),(0,0,0,1))
# Task LFoot: Move the right foot up.
taskLF=MetaTaskDyn6d('lf',robot.dynamic,'lf','left-ankle')
taskLF.ref = ((1,0,0,0.0),(0,1,0,+0.29),(0,0,1,.15),(0,0,0,1))
# --- TASK COM ------------------------------------------------------
robot.dynamic.setProperty('ComputeCoM','true')
featureCom = FeatureGeneric('featureCom')
featureComDes = FeatureGeneric('featureComDes')
plug(robot.dynamic.com,featureCom.errorIN)
plug(robot.dynamic.Jcom,featureCom.jacobianIN)
featureCom.setReference('featureComDes')
featureComDes.errorIN.value = (0.0478408688115,-0.0620357207995,0.684865189311)
taskCom = TaskDynPD('taskCom')
taskCom.add('featureCom')
plug(robot.dynamic.velocity,taskCom.qdot)
taskCom.dt.value = 1e-3
gCom = GainAdaptive('gCom')
plug(taskCom.error,gCom.error)
plug(gCom.gain,taskCom.controlGain)
# Work from .04 to .09 gCom.set 1050 45 125e3
# Good behavior gCom.set 1080 15 125e3
# Low gains gCom.set 400 1 39e3
# Current choice
gCom.set(1050,45,125e3)
# ---- CONTACT -----------------------------------------
# Left foot contact
contactLF = MetaTaskDyn6d('contact_lleg',robot.dynamic,'lf','left-ankle')
contactLF.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.feature.frame('desired')
# Right foot contact
contactRF = MetaTaskDyn6d('contact_rleg',robot.dynamic,'rf','right-ankle')
contactRF.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.feature.frame('desired')
# --- SOT Dyn OpSpaceH --------------------------------------
# SOT controller.
sot = SolverOpSpace('sot')
sot.setSize(robot.dimension-6)
#sot.damping.value = 2e-2
sot.breakFactor.value = 10
plug(robot.dynamic.inertiaReal,sot.matrixInertia)
plug(robot.dynamic.dynamicDrift,sot.dyndrift)
plug(robot.dynamic.velocity,sot.velocity)
plug(sot.control,robot.device.control)
# For the integration of q = int(int(qddot)).
plug(sot.acceleration,robot.device.acceleration)
# --- RUN ------------------------------------------------
# changes the posture of romeo while asserting that it keeps its balance
featureComDes.errorIN.value = (0.05, 0.1, 0.675)
sot.addContactFromTask(contactLF.task.name,'LF')
sot._LF_p.value = contactLF.support
sot.addContactFromTask(contactRF.task.name,'RF')
sot._RF_p.value = contactRF.support
sot.push('taskCom')
# go
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('robot')
plug(robot.device.state, robot.dynamic.position)
from dynamic_graph.ros import *
ros = Ros(robot)
# Create a solver.
# from dynamic_graph.sot.dynamics.solver import Solver
# solver = Solver(robot)
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# --- ROBOT SIMU ---------------------------------------------------------------
# robotName = 'romeo'
# robotDim=robotDimension[robotName]
# robot = RobotSimu("romeo")
# robot.resize(robotDim)
dt=5e-3
# robot.set( initialConfig[robotName] )
# addRobotViewer(robot,small=True,small_extra=24,verbose=False)
# --- ROMEO HANDS ---------------------------------------------------------------
# robot.gripper=0.0
# RobotClass = RobotSimu
# RobotClass.stateFullSizeOld = RobotClass.stateFullSize
# RobotClass.stateFullSize = lambda x: [float(v) for v in x.state.value+24*(x.gripper,)]
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
sot = SOT('sot')
sot.setNumberDofs(robot.dimension)
plug(sot.control,robot.device.control)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# robot.tasks['right-wrist'].add(taskRH.feature.name)
# --- STATIC COM (if not walking)
# taskCom = MetaTaskKineCom(robot.dynamic)
# robot.dynamic.com.recompute(0)
# taskCom.featureDes.errorIN.value = robot.dynamic.com.value
# taskCom.task.controlGain.value = 10
robot.createCenterOfMassFeatureAndTask(
'featureCom', 'featureComDef', 'comTask',
selec = '011',
gain = 10)
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact'+name] = contact
# --- RUN ----------------------------------------------------------------------
target=(0.5,-0.2,0.8)
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
# sot.push(taskCom.task.name)
# sot.push(robot.tasks['right-wrist'].name)
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push((robot.comTask.name))
sot.push(taskRH.task.name)
# go()
# ______________________________________________________________________________
# ******************************************************************************
# ______________________________________________________________________________
# ******************************************************************************
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
from dynamic_graph.sot.core import feature_vector3
from dynamic_graph.sot.dynamics import *
from dynamic_graph.sot.dyninv import *
import dynamic_graph.script_shortcuts
from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from dynamic_graph.tracer import Tracer
from numpy import *
from dynamic_graph.sot.core.utils.history import History
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('robot')
plug(robot.device.state, robot.dynamic.position)
from dynamic_graph.ros import *
ros = Ros(robot)
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
dt=5e-3
q0=list(robot.halfSitting)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
robot.device.increment(dt)
attime.run(robot.device.control.time)
history.record()
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
robot.dynamic.velocity.value = robot.dimension*(0.,)
robot.dynamic.acceleration.value = robot.dimension*(0.,)
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
robot.device.control.unplug()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
from dynamic_graph.sot.dyninv import SolverKine
def toList(sot):
return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
plug(sot.control,robot.device.control)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist')
handMgrip=eye(4); handMgrip[0:3,3] = (0.07,-0.02,0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',robot.dynamic,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
# --- STATIC COM (if not walking)
taskCom = MetaTaskKineCom(robot.dynamic)
# --- TASK AVOID
taskShoulder=MetaTaskKine6d('shoulder',robot.dynamic,'shoulder','LShoulderPitch')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
taskShoulder.task = TaskInequality('taskShoulderAvoid')
taskShoulder.task.add(taskShoulder.feature.name)
taskShoulder.task.referenceInf.value = (-10,) # Xmin, Ymin
taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
taskElbow=MetaTaskKine6d('elbow',robot.dynamic,'elbow','LElbowYaw')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
taskElbow.task = TaskInequality('taskElbowAvoid')
taskElbow.task.add(taskElbow.feature.name)
taskElbow.task.referenceInf.value = (-10,) # Xmin, Ymin
taskElbow.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskElbow.task.dt.value=dt
taskElbow.task.controlGain.value = 0.9
# --- TASK SUPPORT --------------------------------------------------
featureSupport = FeatureGeneric('featureSupport')
plug(robot.dynamic.com,featureSupport.errorIN)
plug(robot.dynamic.Jcom,featureSupport.jacobianIN)
taskSupport=TaskInequality('taskSupport')
taskSupport.add(featureSupport.name)
taskSupport.selec.value = '011'
taskSupport.referenceInf.value = (-0.08,-0.15,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax
taskSupport.dt.value=dt
# --- TASK SUPPORT SMALL --------------------------------------------
featureSupportSmall = FeatureGeneric('featureSupportSmall')
plug(robot.dynamic.com,featureSupportSmall.errorIN)
plug(robot.dynamic.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
# --- POSTURE ---
taskPosture = MetaTaskKinePosture(robot.dynamic)
# --- GAZE ---
taskGaze = MetaTaskVisualPoint('gaze',robot.dynamic,'head','gaze')
# Head to camera matrix transform
# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
# Camera LL
headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
headMcam = dot(headMcam,rotate('x',10*pi/180))
taskGaze.opmodif = matrixToTuple(headMcam)
# --- FOV ---
taskFoV = MetaTaskVisualPoint('FoV',robot.dynamic,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
taskFoV.task.add(taskFoV.feature.name)
[Xmax,Ymax]=[0.38,0.28]
taskFoV.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=dt
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
# --- Task Joint Limits -----------------------------------------
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
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 = dt
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.device.after.addSignal('tr.triger')
# tr.add('dyn2.com','com')
history = History(robot.dynamic,1)
# --- SHORTCUTS ----------------------------------------------------------------
qn = taskJL.normalizedPosition
@optionalparentheses
def pqn(details=True):
''' Display the normalized configuration vector. '''
qn.recompute(robot.state.time)
s = [ "{0:.1f}".format(v) for v in qn.value]
if details:
print("Rleg: "+" ".join(s[:6]))
print("Lleg: "+" ".join(s[6:12]))
print("Body: "+" ".join(s[12:16]))
print("Rarm: "+" ".join(s[16:23]))
print("Larm :"+" ".join(s[23:30]))
else:
print(" ".join(s[:30]))
def jlbound(t=None):
'''Display the velocity bound induced by the JL as a double-column matrix.'''
if t==None: t=robot.state.time
taskJL.task.recompute(t)
return matrix([ [float(x),float(y)] for x,y
in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
def p6d(R,t):
M=eye(4)
M[0:3,0:3]=R
M[0:3,3]=t
return M
def push(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName not in sot.toList():
sot.push(taskName)
if taskName!="taskposture" and "taskposture" in sot.toList():
sot.down("taskposture")
def pop(task):
if isinstance(task,str): taskName=task
elif "task" in task.__dict__: taskName=task.task.name
else: taskName=task.name
if taskName in sot.toList(): sot.rm(taskName)
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
# --- DISPLAY ------------------------------------------------------------------
RAD=pi/180
comproj = [0.1,-0.95,1.6]
#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
# robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
#robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
class BallPosition:
def __init__(self,xyz=(0,-1.1,0.9)):
self.ball = xyz
self.prec = 0
self.t = 0
self.duration = 0
self.f = 0
self.xyz= self.ball
def move(self,xyz,duration=50):
self.prec = self.ball
self.ball = xyz
self.t = 0
self.duration = duration
self.changeTargets()
if duration>0:
self.f = lambda : self.moveDisplay()
attime(ALWAYS,self.f)
else:
self.moveDisplay()
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
# robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def changeTargets(self):
gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
taskFoV.goto3D(self.ball)
b = BallPosition()
# tr.add(taskJL.name+".normalizedPosition","qn")
# tr.add('dyn.com','com')
# tr.add(taskRH.task.name+'.error','erh')
# tr.add(taskFoV.feature.name+'.error','fov')
# tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
# tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
# tr.add(taskSupport.name+".normalizedPosition",'comn')
# robot.device.after.addSignal(taskJL.name+".normalizedPosition")
# robot.device.after.addSignal(taskSupportSmall.name+".normalizedPosition")
# robot.device.after.addSignal(taskFoV.task.name+".normalizedPosition")
# robot.device.after.addSignal(taskFoV.feature.name+'.error')
# robot.device.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
robot.dynamic.com.recompute(0)
taskCom.featureDes.errorIN.value = robot.dynamic.com.value
taskCom.task.controlGain.value = 10
ball = BallPosition((0,-1.1,0.9))
push(taskRH)
ball.move((0.5,-0.2,1.0),0)
next()
next()
next()
def config(ref=0):
if ref==0:
print '''Only the task RH'''
elif ref==1:
print '''Task RH + foot constraint, balance is kept'''
sot.addContact(contactRF)
sot.addContact(contactLF)
elif ref==2:
print '''Task RH + foot constraint, balance is lost'''