Exemple #1
0
 def setUp(self):
     (self.env, options) = openhubo.setup()
     options.robotfile = filename
     options.physicsfile = None
     self.env.SetDebugLevel(1)
     [self.robot, ctrl, self.ind, ref,
      recorder] = openhubo.load_scene(self.env, options)
Exemple #2
0
def load_rlhuboplus(env, scenename=None, stop=False):
    """ Load the rlhuboplus model and a scene into openhubo.
    Returns a servocontroller and a reference robot to show desired
    movements vs. actual pose. The returned tuple contains the robots,
    controller, and a name-to-joint-index converter.
    """
    return _oh.load_scene(env, 'rlhuboplus.robot.xml', scenename, stop)
Exemple #3
0
 def setUp(self):
     (self.env,options)=openhubo.setup()
     self.env.SetDebugLevel(1)
     options.scenefile = 'forcetest.env.xml'
     options.robotfile = None
     options.physics='ode'
     self.robot,self.ctrl,self.ind,self.ghost,self.recorder=openhubo.load_scene(self.env,options)
Exemple #4
0
def load_rlhuboplus(env,scenename=None,stop=False):
    """ Load the rlhuboplus model and a scene into openhubo.
    Returns a servocontroller and a reference robot to show desired
    movements vs. actual pose. The returned tuple contains the robots,
    controller, and a name-to-joint-index converter.
    """
    return _oh.load_scene(env,'rlhuboplus.robot.xml',scenename,stop)
Exemple #5
0
 def setUp(self):
     (self.env, options) = openhubo.setup()
     self.env.SetDebugLevel(1)
     options.scenefile = 'forcetest.env.xml'
     options.robotfile = None
     options.physics = 'ode'
     self.robot, self.ctrl, self.ind, self.ghost, self.recorder = openhubo.load_scene(
         self.env, options)
Exemple #6
0
    def setUp(self):

        (env, options) = oh.setup()
        options.ghost = False
        options.physics = False
        options.stop = False
        (self.robot, self.ctrl, ind, ghost,
         recorder) = oh.load_scene(env, options)
        env.SetDebugLevel(1)
        self.env = env
Exemple #7
0
    def setUp(self):

        (env,options)=openhubo.setup()
        env.SetDebugLevel(1)
        #NOTE: Loads trajectory controller, which passes servo commands down
        options.physics=True
        options.scenefile=None
        [self.robot,self.controller,self.ind,__,__]=openhubo.load_scene(env,options)
        env.GetPhysicsEngine().SetGravity([0,0,0])
        self.env=env
        self.pose=zeros(self.robot.GetDOF())
        self.env.StartSimulation(0.0005)
Exemple #8
0
    if flag == 'nophysics':
        options.physics=False
        options.stop=False
        openhubo.TIMESTEP=ha.HUBO_LOOP_PERIOD
        print 'No Dynamics mode'
    else:
        options.physics=True
        options.stop=True

    #Enable simtime if required, or if force by physics
    options.simtime = (simtimeFlag == 'simtime') or options.physics

    # Detect Load robot and scene based on openhubo version
    if oh_version=='0.8.0':
        [robot,ctrl,ind,ghost,recorder]=openhubo.load_scene(env,options)
    elif oh_version=='0.7.0':
        [robot,ctrl,ind,ghost,recorder]=openhubo.load_scene(env,options.robotfile,options.scenefile,options.stop, options.physics, options.ghost)
    else:
        [robot,ctrl,ind,ghost,recorder]=openhubo.load(env,options.robotfile,options.scenefile,options.stop, options.physics, options.ghost)

    # Setup ACH channels to interface with hubo
    s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
    s.flush()
    state = ha.HUBO_STATE()

    ts = ach.Channel(ha.HUBO_CHAN_VIRTUAL_TO_SIM_NAME)
    ts.flush()
    sim = ha.HUBO_VIRTUAL()

    fs = ach.Channel(ha.HUBO_CHAN_VIRTUAL_FROM_SIM_NAME)
# boilerplate openhubo imports, avoid "import *" to allow pylint to check for
# undefined functions
import openhubo
from numpy import pi
from openravepy import planningutils

#example-specific imports
import openhubo.trajectory as tr

(env,options)=openhubo.setup()
env.SetDebugLevel(3)

timestep=openhubo.TIMESTEP

[robot,controller,ind,ref,recorder]=openhubo.load_scene(env,options)

pose0=openhubo.Pose(robot,controller)
pose0.update()
pose0.send()

env.StartSimulation(timestep=timestep)

pose1=openhubo.Pose(robot,controller)

pose1['LAP']=-pi/8
pose1['RAP']=-pi/8

pose1['LKP']=pi/4
pose1['RKP']=pi/4
Exemple #10
0
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'

from openravepy import *
from numpy import *
import time
import openhubo
import matplotlib.pyplot as plt

if __name__=='__main__':

    (env,options)=openhubo.setup('qtcoin')
    env.SetDebugLevel(3)

    #Load environment and robot with default settings
    [robot,ctrl,ind,ref_robot,recorder]=openhubo.load_scene(env,options.robotfile,options.scenefile,True)

    j1=robot.GetJoint('LAR_dummy')
    j2=robot.GetJoint('RAR_dummy')
    env.Load('physics.xml')
    physics=env.GetPhysicsEngine()
    steps=4000
    timestep=openhubo.TIMESTEP

    #Initialize numpy arrays to store data efficiently
    LFz=zeros(steps)
    RFz=zeros(steps)
    LMx=zeros(steps)
    RMx=zeros(steps)
    LMy=zeros(steps)
    RMy=zeros(steps)
Exemple #11
0
    else:
        ljoints=[robot.GetJoint(n) for n in FINGER_NAMES if _re.search(r'left|^L',n) and robot.GetJoint(n) is not None]
        rjoints=[robot.GetJoint(n) for n in FINGER_NAMES if _re.search(r'right|^R',n) and robot.GetJoint(n) is not None]
        return (ljoints,rjoints)

def get_fingers_mask(joints,mask):
    return [j for j in joints if _re.search(mask,j.GetName())]

if __name__=='__main__':
    import openhubo as oh
    from openhubo import startup
    (env,options)=oh.setup()
    options.stop=False
    options.physics=False
    options.ghost=False

    [robot,ctrl,ind,__,__]=oh.load_scene(env,options)
    print oh_from_ha_names
    jointnames=[j.GetName() for j in robot.GetJoints()]

    print "Mapping from openhubo to hubo-ach"
    for n in jointnames:
        print n,ha_from_oh(n),get_huboname_from_name(n)

    print "Mapping from hubo-ach to openhubo"
    for n in ha_ind_name_map.keys():
        print n,oh_from_ha(n),get_name_from_huboname(n)

    print "Make a direct Index map from openhubo to hubo-ach"
    print ha_ind_from_oh_ind(robot)
Exemple #12
0
            for j in range(8):
                #create one grip above each rung
                h=0.07+.3*j
                grips.append(T * comps.Transform(None,[0,0,h]))
        elif re.search('rung',link.GetName()):
            grips.append(T * comps.Transform(None,[0,.12,0]))
            grips.append(T * comps.Transform(None,[0,-.12,0]))
    #backwards compatible
    return grips


(env,options)=oh.setup('qtcoin')
env.SetDebugLevel(3)

# Load the environment
[robot, ctrl, ind,ref,recorder]=oh.load_scene(env,options.robotfile,'ladderclimb.env.xml',True)
pose=oh.Pose(robot,ctrl)

print "Position the robot"
pause()
stairs=env.GetKinBody('ladder')
links=stairs.GetLinks()

#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]))
Exemple #13
0
import openhubo
import openhubo.planning as planning
import openhubo.comps as comps

from numpy import pi
from openhubo import pause
from openhubo.comps import TSR, TSRChain
from openravepy import RaveCreateProblem

(env, options) = openhubo.setup('qtcoin')
env.SetDebugLevel(3)

options.physics = True

[robot, ctrl, ind, ref, recorder] = openhubo.load_scene(env, options)

probs_cbirrt = RaveCreateProblem(env, 'CBiRRT')
env.LoadProblem(probs_cbirrt, robot.GetName())

first_pose = comps.Cbirrt(probs_cbirrt)
planning.setInitialPose(robot)

## Create an example goal pose (The result of all of these steps should be a
# goal pose vector)
pose = openhubo.Pose(robot, ctrl)
start_position = robot.GetTransform()

pose['RSP'] = -pi / 4
pose['LSP'] = -pi / 4
                CoPx=-torque[1]/force[2]
                CoPy=-torque[0]/force[2]
                #ASSUME that COM of dummy end-effector body is "contact center"
                localCoP=[CoPx,CoPy,0,1]

            cop=s.GetAttachingLink().GetTransform()*mat(array(localCoP)).T
            r=norm(force)/maxF
            h.append(env.plot3(cop[:-1].T,.01,[r,r,r/2.],True))

if __name__=='__main__':

    (env,options)=openhubo.setup('qtcoin')
    env.SetDebugLevel(3)
    options.physics=True

    [robot,controller,ind,ref,recorder]=openhubo.load_scene(env,options)

    with env:
        for s in robot.GetAttachedSensors():
            s.GetSensor().Configure(Sensor.ConfigureCommand.PowerOff)
            s.GetSensor().SendCommand('histlen 10')
            s.GetSensor().Configure(Sensor.ConfigureCommand.PowerOn)
        controller.SendCommand('set radians ')

    #Use .0005 timestep for non-realtime simulation with ODE to reduce jitter.
    env.StartSimulation(openhubo.TIMESTEP)
    time.sleep(1)
    env.StopSimulation()

    pose=openhubo.Pose(robot,controller)
    pose['REP']=-pi/4
Exemple #15
0
def load_simplefloor(env):
    """ Load up and configure the simpleFloor environment for hacking with
    physics. Sets some useful defaults.
    """
    return _oh.load_scene(env,None,'simpleFloor.env.xml',True)
Exemple #16
0
            for j in range(8):
                # create one grip above each rung
                h = 0.07 + 0.3 * j
                grips.append(T * comps.Transform(None, [0, 0, h]))
        elif re.search("rung", link.GetName()):
            grips.append(T * comps.Transform(None, [0, 0.12, 0]))
            grips.append(T * comps.Transform(None, [0, -0.12, 0]))
    # backwards compatible
    return grips


(env, options) = oh.setup("qtcoin")
env.SetDebugLevel(3)

# Load the environment
[robot, ctrl, ind, ref, recorder] = oh.load_scene(env, options.robotfile, "ladderclimb.env.xml", True)
pose = oh.Pose(robot, ctrl)

print "Position the robot"
pause()
stairs = env.GetKinBody("ladder")
links = stairs.GetLinks()

# 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]))
Exemple #17
0
 def setUp(self):
     (self.env,options)=openhubo.setup()
     options.robotfile=filename
     options.physicsfile=None
     self.env.SetDebugLevel(1)
     [self.robot,ctrl,self.ind,ref,recorder]=openhubo.load_scene(self.env,options)
Exemple #18
0
from __future__ import with_statement  # for python 2.5
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'

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(.1)
    openhubo.pause()

    for k in range(20000):
#//
#// You should have received a copy of the GNU Lesser General Public License
#// along with this program.  If not, see <http://www.gnu.org/licenses/>.

from __future__ import with_statement  # for python 2.5
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'

from openhubo import trajectory
import openhubo
import openravepy as rave
import time

if __name__ == '__main__':
    (env, options) = openhubo.setup('qtcoin')
    [robot, ind, __, __, __] = openhubo.load_scene(env, options)

    #Read in trajectory from matlab source
    traj = trajectory.read_text_traj('trajectories/ingress_test.traj.txt',
                                     robot, 0.01, 1.0)
    config = traj.GetConfigurationSpecification()

    Getvals = trajectory.makeJointValueExtractor(robot, traj, config)
    Gettrans = trajectory.makeTransformExtractor(robot, traj, config)

    probs_cbirrt = rave.RaveCreateProblem(env, 'CBiRRT')
    env.LoadProblem(probs_cbirrt, 'rlhuboplus')
    i = 0
    t0 = time.time()
    for i in range(traj.GetNumWaypoints()):
        robot.SetTransformWithDOFValues(Gettrans(i), Getvals(i))
#//
#// You should have received a copy of the GNU Lesser General Public License
#// along with this program.  If not, see <http://www.gnu.org/licenses/>.

from __future__ import with_statement # for python 2.5
__author__ = 'Robert Ellenberg'
__license__ = 'GPLv3 license'

from openhubo import trajectory
import openhubo
import openravepy as rave
import time

if __name__=='__main__':
    (env,options)=openhubo.setup('qtcoin')
    [robot,ind,__,__,__]=openhubo.load_scene(env,options)

    #Read in trajectory from matlab source
    traj=trajectory.read_text_traj('trajectories/ingress_test.traj.txt',robot,0.01,1.0)
    config=traj.GetConfigurationSpecification()

    Getvals=trajectory.makeJointValueExtractor(robot,traj,config)
    Gettrans=trajectory.makeTransformExtractor(robot,traj,config)

    probs_cbirrt = rave.RaveCreateProblem(env,'CBiRRT')
    env.LoadProblem(probs_cbirrt,'rlhuboplus')
    i=0
    t0=time.time()
    for i in range(traj.GetNumWaypoints()):
        robot.SetTransformWithDOFValues(Gettrans(i),Getvals(i))
        #print openhubo.find_com(robot)
Exemple #21
0
    if flag == 'nophysics':
        options.physics = False
        options.stop = False
        openhubo.TIMESTEP = ha.HUBO_LOOP_PERIOD
        print 'No Dynamics mode'
    else:
        options.physics = True
        options.stop = True

    #Enable simtime if required, or if force by physics
    options.simtime = (simtimeFlag == 'simtime') or options.physics

    # Detect Load robot and scene based on openhubo version
    if oh_version == '0.8.0':
        [robot, ctrl, ind, ghost, recorder] = openhubo.load_scene(env, options)
    elif oh_version == '0.7.0':
        [robot, ctrl, ind, ghost,
         recorder] = openhubo.load_scene(env, options.robotfile,
                                         options.scenefile, options.stop,
                                         options.physics, options.ghost)
    else:
        [robot, ctrl, ind, ghost,
         recorder] = openhubo.load(env, options.robotfile, options.scenefile,
                                   options.stop, options.physics,
                                   options.ghost)

    # Setup ACH channels to interface with hubo
    s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
    s.flush()
    state = ha.HUBO_STATE()
#// You should have received a copy of the GNU Lesser General Public License
#// along with this program.  If not, see <http://www.gnu.org/licenses/>.

__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
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)]))
Exemple #23
0
        instring = f.read()
    inlist = instring.split("\n")
    indata = [float(x) for x in inlist[:-2]]
    plt.plot(diff(indata))
    plt.show()
    return indata


""" Simple test script to run some of the functions above. """
if __name__ == "__main__":

    (env, options) = openhubo.setup("qtcoin")
    env.SetDebugLevel(1)

    timestep = 0.01

    [robot, ctrl, ind, __, recorder] = openhubo.load_scene(env, options)

    # Override default controller with ach-read controller
    ctrl = RaveCreateController(env, "achreadcontroller")
    robot.SetController(ctrl)
    ref_ghost = openhubo.load_ghost(env, options.robotfile, "ref_", [0.7, 1, 0.7])
    ctrl.SendCommand("SetRefRobot " + ref_ghost.GetName())

    # Uncomment this section if you want to visualize the commands separately
    # cmd_ghost=openhubo.load_ghost(env,options.robotfile,"cmd_",[.9,.7,.9])
    # ctrl.SendCommand('SetCmdRobot '+cmd_ghost.GetName())

    print "Starting Simulation..."
    env.StartSimulation(timestep, True)
Exemple #24
0
def is_finger(name):
    if _re.search(r'^rightKnuckle|^[LR]F[0-5][0-3]?$|^leftKnuckle', name):
        return True
    else:
        return False


if __name__ == '__main__':
    import openhubo as oh
    from openhubo import startup
    (env, options) = oh.setup()
    options.stop = False
    options.physics = False
    options.ghost = False

    [robot, ctrl, ind, __, __] = oh.load_scene(env, options)
    print oh_from_ha_names
    jointnames = [j.GetName() for j in robot.GetJoints()]

    print "Mapping from openhubo to hubo-ach"
    for n in jointnames:
        print n, ha_from_oh(n), get_huboname_from_name(n)

    print "Mapping from hubo-ach to openhubo"
    for n in ha_ind_name_map.keys():
        print n, oh_from_ha(n), get_name_from_huboname(n)

    print "Make a direct Index map from openhubo to hubo-ach"
    print ha_ind_from_oh_ind(robot)
Exemple #25
0
def load_simplefloor(env):
    """ Load up and configure the simpleFloor environment for hacking with
    physics. Sets some useful defaults.
    """
    return _oh.load_scene(env, None, 'simpleFloor.env.xml', True)
Exemple #26
0
from __future__ import with_statement  # for python 2.5

__author__ = "Robert Ellenberg"
__license__ = "GPLv3 license"

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):
Exemple #27
0
 def setUp(self):
     (self.env,options)=openhubo.setup()
     self.env.SetDebugLevel(1)
     [self.robot,ctrl,self.ind,ref,recorder]=openhubo.load_scene(self.env,options)
     self.manip=self.robot.GetManipulator('rightArm')
     self.fingers=self.manip.GetChildJoints()