pose['LAP']=-pi/8
pose['RAP']=-pi/8
pose['LKP']=pi/4
pose['RKP']=pi/4
pose['LHP']=-pi/8
pose['RHP']=-pi/8
pose['LSP']=-pi/8
pose['LEP']=-pi/4
trajectory.traj_append(traj,pose.to_waypt(1.0))
pose[:]=0.0
trajectory.traj_append(traj,pose.to_waypt(1.0))
planningutils.RetimeActiveDOFTrajectory(traj,robot,True)
print "Showing samples of knee pose at given times:"
for k in range(40):
data=traj.Sample(float(k)/10)
print data[ind('LKP')]
ctrl.SetPath(traj)
ctrl.SendCommand('start')
while not(ctrl.IsDone()):
oh.sleep(.1)
# Make any adjustments to initial pose here
handles = []
for k in links:
handles.append(oh.plot_body_com(k))
grips = makeGripTransforms(links)
griphandles = planning.plotTransforms(env, grips, array([0, 0, 1]))
# make a list of Link transformations
probs_cbirrt = RaveCreateProblem(env, "CBiRRT")
env.LoadProblem(probs_cbirrt, robot.GetName())
planning.setInitialPose(robot)
oh.sleep(1)
# Define manips used and goals
z1 = 0.05
theta = 0.5
LH = 0
RH = 8
POST = 8
RUNG0 = 16
RUNG = 2
LF = 0
RF = 1
# Post grips at shoulder height
rgrip1 = TSR(
grips[3 + RH], comps.Transform(None, [0.0, -0.015, 0]).tm, mat([0, 0, 0, 0, -z1, z1, 0, 0, 0, 0, -theta, theta]), 1
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'
import openhubo as oh
from numpy import pi
(env,options)=oh.setup('qtcoin',True)
env.SetDebugLevel(4)
#Note that the load function now directly parses the option structure
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
filename='gaintest.txt'
with robot:
ctrl.SendCommand('set radians ')
ctrl.SendCommand('set gains 150 0 .9 '.format(ind('RSP')))
ctrl.SendCommand('record_on {} '.format(filename))
#0.7.1 Syntax change: Note the new "Pose" class:
pose=oh.Pose(robot,ctrl)
pose['RSP']=.4
pose.send()
oh.sleep(3)
ctrl.SendCommand('record_off {0} {1} {1} '.format(filename,ind('RSP')))
rspdata=oh.ServoPlotter(filename)
#rspdata.plot(['RSP'])
print 'rsp = [ '+' '.join(['{}'.format(x) for x in rspdata.jointdata['RSP']])
#Make any adjustments to initial pose here
handles=[]
for k in links:
handles.append(oh.plot_body_com(k))
grips = makeGripTransforms(links)
griphandles=planning.plotTransforms(env,grips,array([0,0,1]))
# make a list of Link transformations
probs_cbirrt = RaveCreateProblem(env,'CBiRRT')
env.LoadProblem(probs_cbirrt,robot.GetName())
planning.setInitialPose(robot)
oh.sleep(1)
#Define manips used and goals
z1=.05
theta=0.5
LH=0
RH=8
POST=8
RUNG0=16
RUNG=2
LF=0
RF=1
#Post grips at shoulder height
rgrip1=TSR(grips[3+RH],comps.Transform(None,[.0,-.015,0]).tm,mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),1)
lgrip1=TSR(grips[3],comps.Transform(None,[.0,.015,0]).tm,mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),0)
from optparse import OptionParser
import time
import openravepy
import openhubo
from openhubo import kbhit
if __name__ == "__main__":
"""Modified version of contact display from openrave examples"""
(env,options)=openhubo.setup('qtcoin',True)
env.SetDebugLevel(2)
time.sleep(.25)
[robot,ctrl,ind,ref,recorder]=openhubo.load_scene(env,options)
stop=False
# Set the floor and other bodies to be slightly transparent to better visualize interpenetrations
for b in env.GetBodies():
if not b == robot:
openhubo.set_robot_color(b,trans=.3)
while not stop:
handles=openhubo.plot_contacts(robot)
#handles=openhubo.plot_dirs(robot)
openhubo.sleep(.05)
if kbhit.kbhit(True):
stop=True
pose['LAP'] = -pi / 8
pose['RAP'] = -pi / 8
pose['LKP'] = pi / 4
pose['RKP'] = pi / 4
pose['LHP'] = -pi / 8
pose['RHP'] = -pi / 8
pose['LSP'] = -pi / 8
pose['LEP'] = -pi / 4
trajectory.traj_append(traj, pose.to_waypt(1.0))
pose[:] = 0.0
trajectory.traj_append(traj, pose.to_waypt(1.0))
planningutils.RetimeActiveDOFTrajectory(traj, robot, True)
print "Showing samples of knee pose at given times:"
for k in range(40):
data = traj.Sample(float(k) / 10)
print data[ind('LKP')]
ctrl.SetPath(traj)
ctrl.SendCommand('start')
while not (ctrl.IsDone()):
oh.sleep(.1)
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'
import openhubo as oh
from numpy import pi
(env, options) = oh.setup('qtcoin', True)
env.SetDebugLevel(4)
#Note that the load function now directly parses the option structure
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
filename = 'gaintest.txt'
with robot:
ctrl.SendCommand('set radians ')
ctrl.SendCommand('set gains 150 0 .9 '.format(ind('RSP')))
ctrl.SendCommand('record_on {} '.format(filename))
#0.7.1 Syntax change: Note the new "Pose" class:
pose = oh.Pose(robot, ctrl)
pose['RSP'] = .4
pose.send()
oh.sleep(3)
ctrl.SendCommand('record_off {0} {1} {1} '.format(filename, ind('RSP')))
rspdata = oh.ServoPlotter(filename)
#rspdata.plot(['RSP'])
print 'rsp = [ ' + ' '.join(['{}'.format(x) for x in rspdata.jointdata['RSP']])
__license__ = 'Apache License, Version 2.0'
from optparse import OptionParser
import time
import openravepy
import openhubo
from openhubo import kbhit
if __name__ == "__main__":
"""Modified version of contact display from openrave examples"""
(env, options) = openhubo.setup('qtcoin', True)
env.SetDebugLevel(2)
time.sleep(.25)
[robot, ctrl, ind, ref, recorder] = openhubo.load_scene(env, options)
stop = False
# Set the floor and other bodies to be slightly transparent to better visualize interpenetrations
for b in env.GetBodies():
if not b == robot:
openhubo.set_robot_color(b, trans=.3)
while not stop:
handles = openhubo.plot_contacts(robot)
#handles=openhubo.plot_dirs(robot)
openhubo.sleep(.05)
if kbhit.kbhit(True):
stop = True
