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()
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()
