Пример #1
0
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)
Пример #2
0
# 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
Пример #3
0
__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']])
Пример #4
0
#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)
Пример #5
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

Пример #6
0
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)
Пример #7
0
__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']])
Пример #8
0
__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