Skip to content
Snippets Groups Projects
Commit 7a60a88e authored by Nicolas Mansard's avatar Nicolas Mansard
Browse files

Link with robot-viewer through threads.

parent 7b1c0728
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 44 additions and 125 deletions
python/dyndebug.py
+ 44
− 125
View file @ 7a60a88e
......@@ -4,7 +4,9 @@ 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 MetaTask6d import MetaTask6d
# --- Dynamic parameters ---
hrp2_14_pkgdatarootdir = "/home/nmansard/compil/devgiri/hpp2/share/hrp2_14"
......@@ -15,26 +17,57 @@ specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
robotDim=36
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
robot = RobotSimu("robot")
robot = RobotDynSimu("robot")
robot.resize(robotDim)
#robot.set( (0,0,0,0,0,0, -0.0091163033083694940822,-0.091409963793829082657,-0.47197874328281280709,0.84038019261713603481,-0.47023279905304871118,0.089662407859063653071,0.0095078180225693850747,0.091110286804129178573,-0.4694503518480188653,0.83530799538565336793,-0.46768619090392338222,-0.093802946636709280681,-8.7564658296357018321e-05,0.0032643187737393364843,-7.8338082008638037324e-06,0.00019474257801330629915,0.25837025731361307201,-0.17509910214200960499,-6.1173425555032122825e-05,-0.52495354876847799552,3.1825099712999219606e-06,-0.00025760004742291479777,-3.4121048192112107608e-06,0.25836727162795458668,0.17432209754621175168,-8.8902395499734237117e-05,-0.52498369084585783106,-3.4610294148795152394e-07,-0.00026540143977199129972,1.004984814529256132e-06) )
robot.set( ( 0.0274106623863, 0.143843868989, 0.646921914726, 0.00221064938462, 0.101393756965, 1.36729741242e-05, -0.00911630330837, -0.0914099637938, -0.471978743283, 0.840380192617, -0.470232799053, 0.0896624078591, 0.00950781802257, 0.0911102868041, -0.469450351848, 0.835307995386, -0.467686190904, -0.0938029466367, -8.75646582964e-05, 0.00326431877374, -7.83380820086e-06, 0.000194742578013, 0.258370257314, -0.175099102142, -6.1173425555e-05, -0.524953548768, 3.1825099713e-06, -0.000257600047423, -3.41210481921e-06, 0.258367271628, 0.174322097546, -8.89023954997e-05, -0.524983690846, -3.46102941488e-07, -0.000265401439772, 1.00498481453e-06 ) )
robot_wrapper=robot
robot = PseudoRobotDynamic("dynint")
robot.replace(robot_wrapper.name,True)
dt=1e-3
robot.dt.value = dt
#robot.boundNewCommand("increment")
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
try:
import robotviewer
def stateFullSize(robot):
simulation_angles = [float(val) for val in robot.state.value]
simulation_angles += 10*[0.0]
return simulation_angles
RobotDynSimu.stateFullSize = stateFullSize
robot.viewer = robotviewer.client('XML-RPC')
robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
def incrementView( robot,dt ):
robot.incrementNoView(dt)
robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
RobotDynSimu.incrementNoView = RobotDynSimu.increment
RobotDynSimu.increment = incrementView
except:
print "No robot viewer, sorry."
robot.viewer = None
# --- MAIN LOOP ------------------------------------------
def inc():
robot.increment(dt)
tr.triger.recompute( robot.control.time )
from ThreadInterruptibleLoop import *
@loopInThread
def loop():
inc()
runner=loop()
@optionalparentheses
def go(): runner.play()
@optionalparentheses
def stop(): runner.pause()
@optionalparentheses
def next(): runner.once()
# --- DYN ----------------------------------------------------------------------
# --- DYN ----------------------------------------------------------------------
......@@ -47,7 +80,6 @@ dyn.parse()
dyn.inertiaRotor.value = (0,0,0,0,0,0,1.01e-4,6.96e-4,1.34e-4,1.34e-4,6.96e-4,6.96e-4,1.01e-4,6.96e-4,1.34e-4,1.34e-4,6.96e-4,6.96e-4,6.96e-4,6.96e-4,1.10e-4,1.10e-4,6.96e-4,6.60e-4,1.00e-4,6.60e-4,1.10e-4,1.00e-4,1.00e-4,6.96e-4,6.60e-4,1.00e-4,6.60e-4,1.10e-4,1.00e-4,1.00e-4)
dyn.gearRatio.value = (0,0,0,0,0,0,384.0,240.0,180.0,200.0,180.0,100.0,384.0,240.0,180.0,200.0,180.0,100.0,207.69,381.54,100.0,100.0,219.23,231.25,266.67,250.0,145.45,350.0,200.0,219.23,231.25,266.67,250.0,145.45,350.0,200.0)
plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
dyn.acceleration.value = robotDim*(0.,)
......@@ -61,48 +93,6 @@ dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
# --- FOOT on the ground
# The integrator is not working properly. So added some trick
# to integrate properly, based on the knowledge that one foot
# stay still on the ground ... bbbaaaaaaaaadddd! (but working)
dyn2 = Dynamic('dyn2')
dyn2.setFiles(modelDir, modelName,specificitiesPath,jointRankPath)
dyn2.parse()
plug(robot.state,dyn2.position)
dyn2.velocity.value = robotDim*(0.,)
dyn2.acceleration.value = robotDim*(0.,)
dyn2.ffposition.value = 6*(0,)
# TODO: was J0 instead of Jn in previous version
dyn2.createOpPoint('contact','right-ankle')
waistMsole = OpPointModifier('waistMsole')
plug(dyn2.contact,waistMsole.positionIN)
wMs=((1,0,0,0),(0,1,0,0),(0,0,1,-0.105),(0,0,0,1))
waistMsole.setTransformation( wMs )
flex=AngleEstimator('flex')
flex.setFromSensor(False)
plug(dyn2.contact,flex.contactEmbeddedPosition)
plug(waistMsole.position,flex.contactEmbeddedPosition)
plug(flex.waistWorldPoseRPY,dyn.ffposition)
flex.contactWorldPosition.value = ((1,0,0,0),(0,1,0,0),(0,0,1,0),(0,0,0,1))
# --- FF VELOCITY ----------
dyn3=Dynamic('dyn3')
dyn3.setFiles(modelDir, modelName,specificitiesPath,jointRankPath)
dyn3.parse()
plug(robot.state,dyn3.position)
plug(flex.waistWorldPoseRPY,dyn3.ffposition)
dyn3.velocity.value = robotDim*(0.,)
dyn3.acceleration.value = robotDim*(0.,)
dyn3.createPosition('contact','right-ankle')
dyn3.createJacobian('Jcontact','right-ankle')
plug(dyn3.Jcontact,flex.jacobian)
plug(robot.velocity,flex.qdot)
plug(flex.qdotOUT,dyn.velocity)
# --- CONTACT MODIFICATIONS --- ---------------------------------------------
# Put the robot on the ground
# These computations are done only for debug, to be use by the programmer
......@@ -138,68 +128,6 @@ robot.setRoot(((0.994864055284,0.000210089058006,0.101219896104,0.0274106623863)
# --- Task Dyn -----------------------------------------
class MetaTask6d(object):
name=''
opPoint=''
dyn=0
derivator=0
task=0
feature=0
featureDes=0
def opPointExist(self,opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J'+opPoint, self.dyn.signals())
return len(sigsP)==1 & len(sigsJ)==1
def defineDynEntities(self,dyn):
self.dyn=dyn
def createOpPoint(self,opPoint,opPointRef = 'right-wrist'):
self.opPoint=opPoint
if self.opPointExist(opPoint): return
self.dyn.createOpPoint(opPoint,opPointRef)
def createFeatures(self):
self.feature = FeaturePoint6d('feature'+self.name)
self.featureDes = FeaturePoint6d('feature'+self.name+'_ref')
self.feature.selec.value = '111111'
self.feature.frame('current')
def createTask(self):
self.task = TaskDynPD('task'+self.name)
self.task.dt.value = 1e-3
def createGain(self):
self.gain = GainAdaptive('gain'+self.name)
self.gain.set(1050,45,125e3)
def plugEverything(self):
self.feature.sdes.value = self.featureDes.name
plug(self.dyn.signal(self.opPoint),self.feature.signal('position'))
plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq'))
self.task.add(self.feature.name)
plug(self.dyn.velocity,self.task.qdot)
plug(self.task.error,self.gain.error)
plug(self.gain.gain,self.task.controlGain)
def __init__(self,name,dyn,opPoint,opPointRef='right-wrist'):
self.name=name
self.defineDynEntities(dyn)
self.createOpPoint(opPoint,opPointRef)
self.createFeatures()
self.createTask()
self.createGain()
self.plugEverything()
@property
def ref(self):
return self.featureDes.position.value
@ref.setter
def ref(self,m):
self.featureDes.position.value = m
# Task right hand
task=MetaTask6d('rh',dyn,'task')
task.ref = ((0,0,-1,0.22),(0,1,0,-0.37),(1,0,0,.74),(0,0,0,1))
......@@ -303,17 +231,8 @@ tr.add('dyn.ffposition','ffposition')
tr.add('sot.control','')
# --- RUN ------------------------------------------------
def inc():
robot_wrapper.increment(dt)
tr.triger.recompute( robot_wrapper.control.time )
featureComDes.errorIN.value = (0.06,0,0.8)
sot.push('taskCom')
#for i in range(2100):
# inc()
#tr.dump()
#go
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment