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