def setupEnv():
env = Environment()
#env.SetViewer('qtcoin')
#time.sleep(.5)
env.SetDebugLevel(DebugLevel.Info)
timestep = 0.0005
with env:
env.StopSimulation()
env.Load('simpleFloor.env.xml')
collisionChecker = RaveCreateCollisionChecker(env, 'ode')
env.SetCollisionChecker(collisionChecker)
robot = env.GetRobots()[0]
#Create a "shortcut" function to translate joint names to indices
ind = openhubo.makeNameToIndexConverter(robot)
#initialize the servo controller
controller = RaveCreateController(env, 'servocontroller')
robot.SetController(controller)
#Set an initial pose before the simulation starts
controller.SendCommand('setgains 50 0 8')
pose = array(zeros(robot.GetDOF()))
pose[ind('RSR')] = -pi / 8
pose[ind('LSR')] = pi / 8
#Set initial pose to avoid thumb collisions
robot.SetDOFValues(pose)
controller.SetDesired(pose)
return shortcuts(env, robot, controller, pose)
def read_youngbum_traj(filename, robot, dt=.01, scale=1.0):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign convention)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
traj = RaveCreateTrajectory(robot.GetEnv(), '')
config = robot.GetConfigurationSpecification()
config.AddDeltaTimeGroup()
traj.Init(config)
ind = openhubo.makeNameToIndexConverter(robot)
#Affine DOF are not controlled, so fill with zeros
affinedof = zeros(7)
f = open(filename, 'r')
#Read in header row to find joint names
header = f.readline().rstrip()
print header.split(' ')
indices = [ind(s) for s in header.split(' ')]
#Read in sign row
signlist = f.readline().rstrip().split(' ')
signs = []
print signlist
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with zeros if not used)
offsetlist = f.readline().rstrip().split(' ')
print offsetlist
offsets = [float(x) for x in offsetlist]
k = 0
while True:
string = f.readline().rstrip()
if len(string) == 0:
break
jointvals = [float(x) for x in string.split(' ')]
data = zeros(robot.GetDOF())
for i in range(len(jointvals)):
data[indices[i]] = (jointvals[i] +
offsets[i]) * pi / 180.0 * signs[i] * scale
waypt = list(data)
waypt.extend(affinedof)
waypt.append(dt)
traj.Insert(k, waypt)
k = k + 1
planningutils.RetimeActiveDOFTrajectory(traj, robot, True)
return traj
def setupEnv():
env = Environment()
#env.SetViewer('qtcoin')
#time.sleep(.5)
env.SetDebugLevel(DebugLevel.Info)
timestep=0.0005
with env:
env.StopSimulation()
env.Load('simpleFloor.env.xml')
collisionChecker = RaveCreateCollisionChecker(env,'ode')
env.SetCollisionChecker(collisionChecker)
robot = env.GetRobots()[0]
#Create a "shortcut" function to translate joint names to indices
ind = openhubo.makeNameToIndexConverter(robot)
#initialize the servo controller
controller=RaveCreateController(env,'servocontroller')
robot.SetController(controller)
#Set an initial pose before the simulation starts
controller.SendCommand('setgains 50 0 8')
pose=array(zeros(robot.GetDOF()))
pose[ind('RSR')]=-pi/8
pose[ind('LSR')]=pi/8
#Set initial pose to avoid thumb collisions
robot.SetDOFValues(pose)
controller.SetDesired(pose)
return shortcuts(env,robot,controller,pose)
def read_youngbum_traj(filename,robot,dt=.01,scale=1.0):
""" Read in trajectory data stored in Youngbum's format (100Hz data):
HPY LHY LHR ... RWP (3-letter names)
+ - + ... + (sign of joint about equivalent global axis + / -)
0.0 5.0 2.0 ... -2.0 (Offset of joint from openHubo "zero" in YOUR sign convention)
(data by row, single space separated)
"""
#TODO: handle multiple spaces
#Setup trajectory and source file
traj=RaveCreateTrajectory(robot.GetEnv(),'')
config=robot.GetConfigurationSpecification()
config.AddDeltaTimeGroup()
traj.Init(config)
ind=openhubo.makeNameToIndexConverter(robot)
#Affine DOF are not controlled, so fill with zeros
affinedof=zeros(7)
f=open(filename,'r')
#Read in header row to find joint names
header=f.readline().rstrip()
print header.split(' ')
indices=[ind(s) for s in header.split(' ')]
#Read in sign row
signlist=f.readline().rstrip().split(' ')
signs=[]
print signlist
for s in signlist:
if s == '+':
signs.append(1)
else:
signs.append(-1)
#Read in offset row (fill with zeros if not used)
offsetlist=f.readline().rstrip().split(' ')
print offsetlist
offsets=[float(x) for x in offsetlist]
k=0
while True:
string=f.readline().rstrip()
if len(string)==0:
break
jointvals=[float(x) for x in string.split(' ')]
data=zeros(robot.GetDOF())
for i in range(len(jointvals)):
data[indices[i]]=(jointvals[i]+offsets[i])*pi/180.0*signs[i]*scale
waypt=list(data)
waypt.extend(affinedof)
waypt.append(dt)
traj.Insert(k,waypt)
k=k+1
planningutils.RetimeActiveDOFTrajectory(traj,robot,True)
return traj
def setUp(self):
shortcuts=servo.setupEnv();
self.env=shortcuts.env
self.robot=shortcuts.robot
self.controller=shortcuts.controller
self.pose=shortcuts.pose
self.ind=openhubo.makeNameToIndexConverter(self.robot)
self.controller.SendCommand('setgains 50 0 8')
self.env.StartSimulation(0.0005)
#bullet = RaveCreateCollisionChecker(env,'bullet')
ode = RaveCreateCollisionChecker(env, 'ode')
env.SetCollisionChecker(ode)
#basemanipulation interface
#basemanip = interfaces.BaseManipulation(robot)
#create problem instances
prob_cbirrt = RaveCreateProblem(env, 'CBiRRT')
env.LoadProblem(prob_cbirrt, 'huboplus')
#prob_manip = RaveCreateProblem(env,'Manipulation')
#env.LoadProblem(prob_manip,'huboplus')
#Set an initial pose before the simulation starts
ind = openhubo.makeNameToIndexConverter(robot)
robot.SetDOFValues([pi / 8, -pi / 8], [ind('LSR'), ind('RSR')])
controller.SendCommand('set gains 50 1 5 .9998 .1')
#Use the new SetDesired command to set a whole pose at once.
pose = array(zeros(60))
#Manually align the goal pose and the initial pose so the thumbs clear
pose[ind('RSR')] = -pi / 2
pose[ind('LSR')] = pi / 2
controller.SetDesired(pose)
env.StartSimulation(timestep=timestep)
recorder = RaveCreateModule(env, 'viewerrecorder')
env.AddModule(recorder, '')
filename = 'hoseexp1_execute.mpg'
env = Environment()
env.SetViewer('qtcoin')
env.SetDebugLevel(4)
timestep=0.01
with env:
env.StopSimulation()
env.Load(file_env)
collisionChecker = RaveCreateCollisionChecker(env,'ode')
env.SetCollisionChecker(collisionChecker)
#Clear physics for kinematic trajectory playback
env.SetPhysicsEngine(RaveCreatePhysicsEngine(env,'GenericPhysicsEngine'))
robot = env.GetRobots()[0]
#Create a "shortcut" function to translate joint names to indices
ind = openhubo.makeNameToIndexConverter(robot)
#initialize the servo controller
controller=RaveCreateController(env,'achcontroller')
robot.SetController(controller)
controller.SendCommand("SetCheckCollisions false")
#Set an initial pose before the simulation starts
robot.SetDOFValues([pi/8,-pi/8],[ind('LSR'),ind('RSR')])
time.sleep(1)
#Use the new SetDesired command to set a whole pose at once.
pose=array(zeros(robot.GetDOF()))
#Manually align the goal pose and the initial pose so the thumbs clear
pose[ind('RSR')]=-pi/8
