From 6ebb0f9923a1b10310ddd7874068ff0cb571184e Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 17 Jan 2013 17:49:38 +0100
Subject: [PATCH] Moved to python/tools.
---
python/dyn2openhrp.py | 115 -------------------------------
python/replay.py | 141 ---------------------------------------
python/replay_dot_pos.py | 121 ---------------------------------
3 files changed, 377 deletions(-)
delete mode 100644 python/dyn2openhrp.py
delete mode 100644 python/replay.py
delete mode 100644 python/replay_dot_pos.py
diff --git a/python/dyn2openhrp.py b/python/dyn2openhrp.py
deleted file mode 100644
index 4762d96..0000000
--- a/python/dyn2openhrp.py
+++ /dev/null
@@ -1,115 +0,0 @@
-# This is for HRP2-14 only. Define the variable q as the
-# current pose, and display it under the shape of a project OpenHRP.
-
-
-#import random
-#q=[random.random() for i in range(36)]
-#q=dyn.position.value
-q=[0, 0, 0.6487, 0, 0, 0, 0, 0, -0.453786, 0.872665, -0.418879, 0, 0, 0, -0.453786, 0.872665, -0.418879, 0, 0, 0, 0, 0, 0.261799, -0.174533, 0, -1.3, 0, -0.5, 0.174533, 0.261799, 0.174533, 0, -1.3, 0, -0.5, 0.174533 ]
-
-
-
-
-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
-import math
-
-# --- Conversion
-
-jointNames = ("RLEG_JOINT0",
-"RLEG_JOINT1",
-"RLEG_JOINT2",
-"RLEG_JOINT3",
-"RLEG_JOINT4",
-"RLEG_JOINT5",
-"LLEG_JOINT0",
-"LLEG_JOINT1",
-"LLEG_JOINT2",
-"LLEG_JOINT3",
-"LLEG_JOINT4",
-"LLEG_JOINT5",
-"CHEST_JOINT0",
-"CHEST_JOINT1",
-"HEAD_JOINT0",
-"HEAD_JOINT1",
-"RARM_JOINT0",
-"RARM_JOINT1",
-"RARM_JOINT2",
-"RARM_JOINT3",
-"RARM_JOINT4",
-"RARM_JOINT5",
-"RARM_JOINT6",
-"LARM_JOINT0",
-"LARM_JOINT1",
-"LARM_JOINT2",
-"LARM_JOINT3",
-"LARM_JOINT4",
-"LARM_JOINT5",
-"LARM_JOINT6")
-
-
-pattern = "<property name=\"%s.angle\" value=\"%f\"/>"
-
-for i in range(len(q)-6):
- print pattern % (jointNames[i],q[i+6])
-
-
-toUT=PoseRollPitchYawToPoseUTheta('toUT')
-#plug(dyn.position,toUT.sin)
-toUT.sin.value = tuple(q[0:6])
-toUT.sout.recompute(1)
-u=toUT.sout.value[3:6]
-theta=math.sqrt(sum(v*v for v in u))
-if theta>1e-4:
- (ux,uy,uz)=[v/theta for v in u]
-else:
- theta=0
- (ux,uy,uz)=[0,0,0]
-
-print "<property name=\"WAIST.translation\" value=\"%f %f %f\"/>" % (q[0],q[1],q[2])
-print "<property name=\"WAIST.angle\" value=\"0\"/>"
-print "<property name=\"WAIST.rotation\" value=\"%f %f %f %f\"/>" % (ux, uy, uz, theta)
-
-bushJointNames = ("RLEG_BUSH_ROLL","RLEG_BUSH_PITCH","RLEG_BUSH_Z",
-"LLEG_BUSH_ROLL","LLEG_BUSH_PITCH","LLEG_BUSH_Z")
-for i in range(len(bushJointNames)):
- print pattern % (bushJointNames[i],0)
-
-
-
-
-print " <property name=\"RHAND_JOINT4.angle\" value=\"0.0 \"/>"
-print " <property name=\"RHAND_JOINT1.angle\" value=\"0.0 \"/>"
-print " <property name=\"RHAND_JOINT3.angle\" value=\"0.0 \"/>"
-print " <property name=\"RHAND_JOINT0.angle\" value=\"0.0 \"/>"
-print " <property name=\"RHAND_JOINT2.angle\" value=\"0.0 \"/>"
-print " <property name=\"LHAND_JOINT2.angle\" value=\"0.0 \"/>"
-print " <property name=\"LHAND_JOINT1.angle\" value=\"0.0 \"/>"
-print " <property name=\"LHAND_JOINT4.angle\" value=\"0.0 \"/>"
-print " <property name=\"LHAND_JOINT0.angle\" value=\"0.0 \"/>"
-print " <property name=\"LHAND_JOINT3.angle\" value=\"0.0 \"/>"
-print ""
-print " <property name=\"setupDirectory\" value=\"/opt/grx3.0/HRP2LAAS/bin\"/>"
-print " <property name=\"imageProcessTime\" value=\"0.033\"/>"
-print " <property name=\"imageProcessor\" value=\"\"/>"
-print " <property name=\"controller\" value=\"HRP2LAASControllerFactory\"/>"
-print " <property name=\"isRobot\" value=\"true\"/>"
-print " <property name=\"controlTime\" value=\"0.0010\"/>"
-print " <property name=\"setupCommand\" value=\"HRP2Controller$(BIN_SFX)\"/>"
-
-
-
-#import robotviewer
-#viewer = robotviewer.client('XML-RPC')
-#viewer.updateElementConfig('hrp',[float(val) for val in q]+10*[0.0])
-
-
-
-posinit=""
-for v in q[6:]:
- posinit+=str(v*180/math.pi)+" "
-print "seq.sendMsg(\":joint-angles \"+\"" + posinit + "\"+\" 0 0 0 0 0 0 0 0 0 0 2.5\")"
diff --git a/python/replay.py b/python/replay.py
deleted file mode 100644
index 5892732..0000000
--- a/python/replay.py
+++ /dev/null
@@ -1,141 +0,0 @@
-'''
- This version does not use the free floating position for the 'base' of the robot,
- A thread is used to display, then, 'go', 'stop', 'next' are available
- it relies completely on the outputs of dyn_position
- Use it as:
- py replay.py dyn_position.dat
- and then: 'go'
-Author: O. Ramos - LAAS/CNRS
-'''
-
-from numpy import *
-from dynamic_graph.script_shortcuts import optionalparentheses
-import sys
-from optparse import OptionParser
-import time
-
-
-
-usage = "usage: py replay.py [options] args"
-parser = OptionParser(usage=usage)
-parser.add_option("-p", dest="prefix", help="prefix of the file to read, under the shape /path/prefix_ or prefix only (no / no _) to implicit /tmp/prefix_ .", default="dyn")
-parser.add_option("-N", dest="frequency", help="Nominal frequency of the robot play, %default Hz by default.", default='200', type=int)
-parser.add_option("-d", dest="delayTime", help="Delay between to images sent to robot viewer, default %default. Give auto to automatic estimation.", default="0")
-parser.add_option("-s", dest="server", help="Name of server (CORBA or XML-RPC). Default is %default", default="XML-RPC")
-parser.add_option("-r", dest="size", help="Robot size if file verification is needed.", default="-1", type=int)
-parser.add_option("-n", dest="iter", help="Number of iteration to play.", default="-1", type=int)
-parser.add_option("-z", dest="decimate", help="percentage of frame to use, should be <1", default="1", type=float)
-parser.add_option("-R", dest="robotName", help="Robot on which the simulation is replayed. Default is %default", default="hrp2")
-(options, args) = parser.parse_args()
-parser.print_help()
-print '\n -------------------------------------------------------------------------'
-
-
-prefix = options.prefix
-if len(prefix.split('/'))==1 and len(prefix.split('_'))==1:
- prefix='/tmp/'+prefix+'_'
-robotSize = options.size
-nbIter=options.iter
-decimate=options.decimate
-if( options.delayTime=="auto"): delayTime=0
-else: delayTime=float(options.delayTime)
-
-#------------------------------------------------------
-# Try to read the input file, if possible
-try:
- fileName = prefix+'position.dat'
- file_dyn_position = open(fileName,'r') # Open the file
-except:
- print "Non valid input file ",fileName
- sys.exit(0)
-
-
-#------------------------------------------------------
-# Initialize robotviewer, if possible
-try:
- import robotviewer
- clt = robotviewer.client(options.server)
-except:
- print "No robot viewer, sorry."
- sys.exit(0)
-
-
-#-----------------------------------------------------
-nbImage=0
-lastImage=0
-robotTime = 2
-hq=[]
-
-
-def inc():
-
- global robotTime, q, hq, robotSize, nbImage, lastImage, decimate
- while True:
- pos_angles = [float(val) for val in file_dyn_position.readline().split()[1:] ]
- if len(pos_angles)==0:
- print " -- File reading completed --"
- return False
- if robotSize==-1:
- robotSize=len(pos_angles)
- print "Robot size estimated to ",robotSize
- if len(pos_angles)==robotSize: break
-
- robotTime += 1
- if robotTime%1000==0: print "\nTime: " + str(robotTime)
-
- # Add 10 values for the hand DOFs for HRP2
- if options.robotName=="hrp2": pos_angles += 10*[0.0]
-
- # Plot only if the values are not 'NaN'
- nbImage+=decimate
- if floor(nbImage)>lastImage:
- lastImage=floor(nbImage)
- if reduce(lambda x,y: x and y,[isnan(q) for q in pos_angles]):
- print ' -- Not displaying: nan positions --'
- else:
- clt.updateElementConfig('hrp',pos_angles)
- q = pos_angles
- hq.append( q )
-
- return True
-
-def hlp_showNumber():
- global showNumber
- if showNumber: showNumber=False
- else: showNumber=True
-showNumber=False
-
-#-----------------------------------------------------
-@optionalparentheses
-def n():
- inc()
-def incIter(dummy,nb):
- for i in range(nb): n()
-setattr(n.__class__,'__add__',incIter)
-
-
-
-
-t0=time.time()
-iterNum=0
-while inc():
- if nbIter>0 and iterNum>nbIter: break
- iterNum+=1
- time.sleep( delayTime )
-
-t1=time.time()
-dt=t1-t0
-dtn=(iterNum+0.0)/options.frequency
-
-if options.delayTime=="auto":
- print "Duration: ",dt," s"
- print "Nominal time :",dtn
- if( abs(dt-dtn)/iterNum < 1e-3 ):
- print "Non pertinent delay"
- else:
- if dt>dtn:
- print "use decimate -z ",dtn/dt
- else:
- print "use delay time -d ",(dtn-dt)/iterNum
-
-
diff --git a/python/replay_dot_pos.py b/python/replay_dot_pos.py
deleted file mode 100644
index 7fb3f69..0000000
--- a/python/replay_dot_pos.py
+++ /dev/null
@@ -1,121 +0,0 @@
-'''
- This version does not use the free floating position for the 'base' of the robot,
- A thread is used to display, then, 'go', 'stop', 'next' are available
- it relies completely on the outputs of dyn_position
- Use it as:
- py replay.py dyn_position.dat
- and then: 'go'
-Author: O. Ramos - LAAS/CNRS
-'''
-
-#sys.path.append('/home/nmansard/src/hpp2/sot-dyninv/python')
-from numpy import *
-from dynamic_graph.script_shortcuts import optionalparentheses
-import sys
-
-
-#------------------------------------------------------
-# Try to read the input file, if possible
-try:
- #fileName = sys.argv[1] # Input File ([2] if using 'py', [1] if 'python')
- prefix='mocap/'
- fileName = prefix+'yoganmsd.pos'
- file_dyn_position = open(fileName,'r') # Open the file
-# fileffName = prefix+'ffposition.dat'
-# file_dyn_ffposition = open(fileffName,'r') # Open the file
-except:
- print "Please, specify the input file"
- sys.exit(0)
-
-
-#------------------------------------------------------
-# Initialize robotviewer, if possible
-try:
- import robotviewer
- clt = robotviewer.client('XML-RPC')
-except:
- print "No robot viewer, sorry."
- sys.exit(0)
-
-
-#-----------------------------------------------------
-# Main Loop
-robotTime = 1
-hq=[]
-def inc():
-
- global robotTime, q, hq
- while True:
- pos_angles = file_dyn_position.readline()
- if len(pos_angles)==0: break
- print float(pos_angles.split()[0])
- if int(round(float(pos_angles.split()[0])/0.005))==robotTime:
- print "OK ", int(float(pos_angles.split()[0])/0.005),robotTime
- pos_angles = pos_angles.split()[1:]
- pos_angles = 6*[0,] + [float(val) for val in pos_angles]
- break
- else:
- print "nonono ",int(round(float(pos_angles.split()[0])/0.005)),robotTime
- sys.exit(0)
-# while True:
-# pos_ffangles = file_dyn_ffposition.readline()
-# if len(pos_ffangles)==0: break
-# if float(pos_ffangles.split()[0])==robotTime:
-# pos_angles[0:6] = pos_ffangles.split()[1:]
-# break
-
- if len(pos_angles)==0:
- stop()
- print " -- File reading completed --"
- return
- else:
- robotTime += 1
-
- if robotTime%1000==0: print "\nTime: " + str(robotTime)
-
- # Add 10 values for the hand DOFs
- #pos_angles += 10*[0.0]
-
- # Plot only if the values are not 'NaN'
- plot=1
- for qi in pos_angles:
- if isnan(qi):
- plot=0
- print ' -- Not displaying: nan positions --'
- if plot==1:
- clt.updateElementConfig('hrp',pos_angles)
- q = pos_angles
- hq.append( q )
-
- global showNumber
- if showNumber:
- print 't = ', robotTime
- time.sleep(.005)
-
-
-def hlp_showNumber():
- global showNumber
- if showNumber: showNumber=False
- else: showNumber=True
-showNumber=False
-
-#-----------------------------------------------------
-# Thread
-
-from ThreadInterruptibleLoop import *
-@loopInThread
-def loop():
- inc()
-runner=loop()
-
-@optionalparentheses
-def go(): runner.play()
-@optionalparentheses
-def stop(): runner.pause()
-@optionalparentheses
-def next(): runner.once()
-@optionalparentheses
-def n(): hlp_showNumber()
-
-
-go()
--
GitLab