def preview_traj(self): #Assume that robot is in initial position now T0=self.base.GetTransform() s=self.robot.CreateRobotStateSaver() for k in xrange(self.srcdata.shape[0]): pose_map={key:self.srcdata[k,v] for (key,v) in self.joint_map.items()} pose_array=array([pose_map[dof] if pose_map.has_key(dof) else 0.0 for dof in xrange(self.robot.GetDOF())]) values=pose_array*self.joint_signs+self.joint_offsets self.robot.SetDOFValues(values) T=self.get_transform(T0,self.srcdata[k,0:6]) self.robot.SetTransform(T) _time.sleep(self.timestep/2.) if kbhit.kbhit(): kbhit.getch() _oh.pause() s.Restore()
import time import openhubo from openravepy import RaveCreateController # Get the global environment for simulation if __name__ == "__main__": (env, options) = openhubo.setup("qtcoin") env.SetDebugLevel(4) [robot, ctrl, ind, ref, recorder] = openhubo.load_scene(env, None, "pendulum.env.xml", True) robot.SetController(RaveCreateController(env, "servocontroller")) spring = robot.GetController() with env: spring.SendCommand("springdamper 0 ") spring.SendCommand("setgains 10 0 1 ") spring.SendCommand("record_on") spring.SetDesired([1]) robot.SetDOFValues([1]) # time.sleep(0.1) # env.StartSimulation(openhubo.TIMESTEP) time.sleep(0.1) openhubo.pause() for k in range(20000): env.StepSimulation(openhubo.TIMESTEP) spring.SendCommand("record_off pendulum.txt ") p = openhubo.ServoPlotter("pendulum.txt", ["j0"])
#the goal. activedofs = first_pose.ActivateManipsByIndex(robot, [0, 1]) activedofs.append(ind('LHP')) activedofs.append(ind('RHP')) #Set the goal pose as for x in activedofs: first_pose.jointgoals.append(pose[x]) #Add in hip pitches robot.SetActiveDOFs(activedofs) first_pose.filename = 'firstpose.traj' print first_pose.Serialize() first_pose.run() pause() env.StartSimulation(openhubo.TIMESTEP) planning.RunTrajectoryFromFile(robot, first_pose, False) ## Now, reset to initial conditions and activate whole body env.StopSimulation() second_pose = comps.Cbirrt(probs_cbirrt) planning.setInitialPose(robot) robot.SetTransform(start_position) ## This time, use the whole body (or at least, all 4 manipulators) activedofs = second_pose.ActivateManipsByIndex(robot, [0, 1, 2, 3]) TSR_left = TSR(robot.GetLink('leftFoot').GetTransform()) TSR_left.manipindex = 2
waypt0.append(t0) waypt1.append(t1) waypt2=copy(waypt0) waypt2[-1]=t1; traj.Insert(0,waypt0) traj.Insert(1,waypt1) traj.Insert(2,waypt2) traj.Insert(3,waypt1) traj.Insert(4,waypt2) planningutils.RetimeActiveDOFTrajectory(traj,robot,True) #Prove that the retiming actually works #for k in range(40): #data=traj.Sample(float(k)/10) #print data[ind('LKP')] controller.SendCommand('SetRecord 1 timedata.txt') time.sleep(1) controller.SetPath(traj) #Everything should be in order... env.StartSimulation(timestep,True) while not(controller.IsDone()): time.sleep(timestep*1.0) controller.SendCommand('SetRecord 0') time.sleep(1) analyzeTime('timedata.txt') openhubo.pause() env.Destroy()
(env,options)=oh.setup('qtcoin',True) env.SetDebugLevel(4) #Note that the load function now directly parses the option structure if options.physics is None: options.physics=True [robot,ctrl,ind,ref,recorder]=oh.load_scene(env,options) env.StartSimulation(oh.TIMESTEP) #Change the pose to lift the elbows and send ctrl.SendCommand('set radians ') #0.7.1 Syntax change: Note the new "Pose" class: pose=oh.Pose(robot,ctrl) pose['REP']=-pi/2 pose['LEP']=-pi/2 pose.send() oh.pause(2) #Hack to get hand if robot.GetName() == 'rlhuboplus' or robot.GetName() == 'huboplus': ctrl.SendCommand('directtorque '+' '.join(['{}'.format(x) for x in range(42,57)])) for i in range(42,57): pose[i]=pi/2 pose.send() oh.pause(2) pose[42:57]=0 pose.send()
N = numpy.ceil(ha.HUBO_LOOP_PERIOD/openhubo.TIMESTEP) T = 1/N*ha.HUBO_LOOP_PERIOD # print 'openhubo.TIMESTEP = ',openhubo.TIMESTEP, ' : N = ', N, ' : T = ', T for x in range(0,int(N)): env.StepSimulation(openhubo.TIMESTEP) # this is in seconds sim.time = sim.time + openhubo.TIMESTEP if('physics' == flag ): s.put(state) pose = sim2state(robot,state) fs.put(sim) else: env.StepSimulation(openhubo.TIMESTEP) # this is in seconds # put the current state time.sleep(0.001) # sleep to allow for keyboard input # end here openhubo.pause(2) #Hack to get hand if robot.GetName() == 'rlhuboplus' or robot.GetName() == 'huboplus': ctrl.SendCommand('openloop '+' '.join(['{}'.format(x) for x in range(42,57)])) for i in range(42,57): pose[i]=pi/2 ctrl.SetDesired(pose) openhubo.pause(2) pose[42:57]=0 ctrl.SetDesired(pose)
#-- Set the robot controller and start the simulation with env: env.StopSimulation() env.Load(file_env) robot = env.GetRobots()[0] #robot.SetController(RaveCreateController(env,'trajectorycontroller')) collisionChecker = RaveCreateCollisionChecker(env,'bullet') env.SetCollisionChecker(collisionChecker) print "Position the robot" #pause() stairs=env.GetKinBody('ladder') links=stairs.GetLinks() ind=openhubo.makeNameToIndexConverter(robot) #Make any adjustments to initial pose here pause() handles=[] for k in links: handles.append(plotBodyCOM(env,k)) #pause() grips = makeGripTransforms(links) griphandles=plotTransforms(env,grips,array([0,0,1])) #pause() # make a list of Link transformations probs_cbirrt = RaveCreateProblem(env,'CBiRRT') env.LoadProblem(probs_cbirrt,'hubo')
(env, options) = oh.setup('qtcoin', True) env.SetDebugLevel(4) #Note that the load function now directly parses the option structure if options.physics is None: options.physics = True [robot, ctrl, ind, ref, recorder] = oh.load_scene(env, options) env.StartSimulation(oh.TIMESTEP) #Change the pose to lift the elbows and send ctrl.SendCommand('set radians ') #0.7.1 Syntax change: Note the new "Pose" class: pose = oh.Pose(robot, ctrl) pose['REP'] = -pi / 2 pose['LEP'] = -pi / 2 pose.send() oh.pause(2) #Hack to get hand if robot.GetName() == 'rlhuboplus' or robot.GetName() == 'huboplus': ctrl.SendCommand('directtorque ' + ' '.join(['{}'.format(x) for x in range(42, 57)])) for i in range(42, 57): pose[i] = pi / 2 pose.send() oh.pause(2) pose[42:57] = 0 pose.send()