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)
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)
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)
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)
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)
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
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)
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
__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)
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)
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]))
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
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)
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]))
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)
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)
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)]))
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)
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)
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)
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):
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()